1 files changed, 61 insertions, 2 deletions

diff --git a/c/src/lib/libbsp/arm/stm32f4/console/usart.c b/c/src/lib/libbsp/arm/stm32f4/console/usart.c
index 539423faa2..f18dd7124f 100644
--- a/c/src/lib/libbsp/arm/stm32f4/console/usart.c
+++ b/c/src/lib/libbsp/arm/stm32f4/console/usart.c
@@ -15,6 +15,8 @@
 #include <libchip/sersupp.h>
 
 #include <bsp.h>
+#include <bsp/io.h>
+#include <bsp/rcc.h>
 #include <bsp/irq.h>
 #include <bsp/usart.h>
 #include <bsp/stm32f4.h>
@@ -31,6 +33,20 @@ static rtems_vector_number usart_get_irq_number(const console_tbl *ct)
 }
 #endif
 
+static const stm32f4_rcc_index usart_rcc_index [] = {
+  STM32F4_RCC_USART1,
+  STM32F4_RCC_USART2,
+  STM32F4_RCC_USART3,
+  STM32F4_RCC_UART4,
+  STM32F4_RCC_UART5,
+  STM32F4_RCC_USART6
+};
+
+static stm32f4_rcc_index usart_get_rcc_index(const console_tbl *ct)
+{
+  return usart_rcc_index [ct->ulCtrlPort2];
+}
+
 static const uint8_t usart_pclk_index [] = { 1, 0, 0, 0, 0, 1 };
 
 static const uint32_t usart_pclk_by_index [] = {
@@ -102,15 +118,58 @@ static uint32_t usart_get_bbr(
     | STM32F4_USART_BBR_DIV_FRACTION(div_fraction);
 }
 
+#define USART_CFG(port, idx, altfunc) \
+  { \
+    .pin = STM32F4_GPIO_PIN(port, idx), \
+    .mode = STM32F4_GPIO_MODE_AF, \
+    .otype = STM32F4_GPIO_OTYPE_PUSH_PULL, \
+    .ospeed = STM32F4_GPIO_OSPEED_2_MHZ, \
+    .pupd = STM32F4_GPIO_PULL_UP, \
+    .af = altfunc \
+  }
+
+static const stm32f4_gpio_config usart_gpio_config [] [2] = {
+  {
+    USART_CFG(0, 9, STM32F4_GPIO_AF_USART1),
+    USART_CFG(0, 10, STM32F4_GPIO_AF_USART1)
+  }, {
+    USART_CFG(0, 2, STM32F4_GPIO_AF_USART2),
+    USART_CFG(0, 3, STM32F4_GPIO_AF_USART2)
+  }, {
+    USART_CFG(3, 8, STM32F4_GPIO_AF_USART3),
+    USART_CFG(3, 9, STM32F4_GPIO_AF_USART3)
+  }, {
+    USART_CFG(0, 1, STM32F4_GPIO_AF_UART4),
+    USART_CFG(0, 2, STM32F4_GPIO_AF_UART4)
+  }, {
+    USART_CFG(2, 11, STM32F4_GPIO_AF_UART5),
+    USART_CFG(2, 12, STM32F4_GPIO_AF_UART5)
+  }, {
+    USART_CFG(2, 6, STM32F4_GPIO_AF_USART6),
+    USART_CFG(2, 7, STM32F4_GPIO_AF_USART6)
+  }
+};
+
+static void usart_set_gpio_config(const console_tbl *ct)
+{
+  const stm32f4_gpio_config *config = usart_gpio_config [ct->ulCtrlPort2];
+
+  stm32f4_rcc_set_gpio_clock(config [0].pin, true);
+  stm32f4_gpio_set_config(&config [0]);
+  stm32f4_rcc_set_gpio_clock(config [1].pin, true);
+  stm32f4_gpio_set_config(&config [1]);
+}
+
 static void usart_initialize(int minor)
 {
   const console_tbl *ct = Console_Port_Tbl [minor];
   volatile stm32f4_usart *usart = usart_get_regs(ct);
   uint32_t pclk = usart_get_pclk(ct);
   uint32_t baud = usart_get_baud(ct);
-  volatile stm32f4_rcc *rcc = &STM32F4_RCC;
+  stm32f4_rcc_index rcc_index = usart_get_rcc_index(ct);
 
-  rcc->apb2enr |= STM32F4_RCC_APB2ENR_USART1_EN;
+  stm32f4_rcc_set_clock(rcc_index, true);
+  usart_set_gpio_config(ct);
 
   usart->cr1 = 0;
   usart->cr2 = 0;