1 files changed, 47 insertions, 16 deletions

diff --git a/c/src/lib/libbsp/arm/stm32f4/startup/io.c b/c/src/lib/libbsp/arm/stm32f4/startup/io.c
index 1bba73cd37..5ecbabe394 100644
--- a/c/src/lib/libbsp/arm/stm32f4/startup/io.c
+++ b/c/src/lib/libbsp/arm/stm32f4/startup/io.c
@@ -13,9 +13,20 @@
  */
 
 #include <bsp/io.h>
+#include <bsp/rcc.h>
 
 #include <rtems.h>
 
+RTEMS_STATIC_ASSERT(sizeof(stm32f4_gpio_config) == 4, size_of_config);
+
+void stm32f4_gpio_set_clock(int pin, bool set)
+{
+  int port = STM32F4_GPIO_PORT_OF_PIN(pin);
+  stm32f4_rcc_index index = STM32F4_RCC_GPIOA + port;
+
+  stm32f4_rcc_set_clock(index, set);
+}
+
 static void clear_and_set(
   volatile uint32_t *reg,
   unsigned index,
@@ -23,8 +34,7 @@ static void clear_and_set(
   uint32_t set
 )
 {
-  uint32_t one = 1;
-  uint32_t mask = (one << width) - one;
+  uint32_t mask = (1U << width) - 1U;
   unsigned shift = width * index;
   uint32_t val = *reg;
 
@@ -34,34 +44,56 @@ static void clear_and_set(
   *reg = val;
 }
 
-void stm32f4_gpio_set_config(const stm32f4_gpio_config *config)
+static void set_config(unsigned pin, const stm32f4_gpio_config *config)
 {
-  unsigned pin = config->pin;
   unsigned port = STM32F4_GPIO_PORT_OF_PIN(pin);
   volatile stm32f4_gpio *gpio = STM32F4_GPIO(port);
   unsigned index = STM32F4_GPIO_INDEX_OF_PIN(pin);
-  unsigned af_reg = index >> 8;
-  unsigned af_index = index & 0x3;
+  unsigned af_reg = index >> 3;
+  unsigned af_index = index & 0x7;
+  int set_or_clear_offset = config->fields.output ? 0 : 16;
   rtems_interrupt_level level;
 
   rtems_interrupt_disable(level);
-  clear_and_set(&gpio->moder, index, 2, config->mode);
-  clear_and_set(&gpio->afr [af_reg], af_index, 4, config->af);
-  clear_and_set(&gpio->pupdr, index, 2, config->pupd);
-  clear_and_set(&gpio->otyper, index, 1, config->otype);
-  clear_and_set(&gpio->ospeedr, index, 2, config->ospeed);
+  gpio->bsrr = 1U << (index + set_or_clear_offset);
+  clear_and_set(&gpio->pupdr, index, 2, config->fields.pupd);
+  clear_and_set(&gpio->otyper, index, 1, config->fields.otype);
+  clear_and_set(&gpio->ospeedr, index, 2, config->fields.ospeed);
+  clear_and_set(&gpio->afr [af_reg], af_index, 4, config->fields.af);
+  clear_and_set(&gpio->moder, index, 2, config->fields.mode);
   rtems_interrupt_enable(level);
 }
 
+void stm32f4_gpio_set_config(const stm32f4_gpio_config *config)
+{
+  int current = config->fields.pin_first;
+  int last = config->fields.pin_last;
+
+  while (current <= last) {
+    stm32f4_gpio_set_clock(current, true);
+    set_config(current, config);
+    ++current;
+  }
+}
+
+void stm32f4_gpio_set_config_array(const stm32f4_gpio_config *configs)
+{
+  stm32f4_gpio_config terminal = STM32F4_GPIO_CONFIG_TERMINAL;
+
+  while (configs->value != terminal.value) {
+    stm32f4_gpio_set_config(configs);
+    ++configs;
+  }
+}
+
 void stm32f4_gpio_set_output(int pin, bool set)
 {
   int port = STM32F4_GPIO_PORT_OF_PIN(pin);
   volatile stm32f4_gpio *gpio = STM32F4_GPIO(port);
   int index = STM32F4_GPIO_INDEX_OF_PIN(pin);
-  int offset = set ? 0 : 16;
-  uint32_t one = 1;
+  int set_or_clear_offset = set ? 0 : 16;
 
-  gpio->bsrr = one << (index + offset);
+  gpio->bsrr = 1U << (index + set_or_clear_offset);
 }
 
 bool stm32f4_gpio_get_input(int pin)
@@ -69,7 +101,6 @@ bool stm32f4_gpio_get_input(int pin)
   int port = STM32F4_GPIO_PORT_OF_PIN(pin);
   volatile stm32f4_gpio *gpio = STM32F4_GPIO(port);
   int index = STM32F4_GPIO_INDEX_OF_PIN(pin);
-  uint32_t one = 1;
 
-  return (gpio->idr & (one << index)) != 0;
+  return (gpio->idr & (1U << index)) != 0;
 }