1 files changed, 33 insertions, 37 deletions

diff --git a/os/drivers/pinctrl/stm32f769_pinctrl.c b/os/drivers/pinctrl/stm32f769_pinctrl.c
index ce45ec17..98b975f6 100644
--- a/os/drivers/pinctrl/stm32f769_pinctrl.c
+++ b/os/drivers/pinctrl/stm32f769_pinctrl.c
@@ -1,7 +1,12 @@
+#include <arch/sys_io.h>
+#include <arch/aarch32/armv7e_m/stm32f7/stm32f769xx.h>
 #include <drivers/clock/stm32f769_clocks.h>
 #include <drivers/clock/stm32f769_clock_control.h>
 #include <drivers/gpio/stm32f769_gpio.h>
 
+#include <stddef.h>
+#include "stm32f769_pinctrl.h"
+
 static const GPIO_TypeDef *stm32f769_port_addrs[] = {
   GPIOA,
   GPIOB,
@@ -30,65 +35,56 @@ static const uint16_t stm32f769_port_clkids[] = {
   STM32F769_CLOCK_GPIOK,
 };
 
-static GPIO_TypeDef *stm32f769_get_port(unsigned int port_idx) {
+GPIO_TypeDef *stm32f769_get_port(unsigned int port_idx) {
   GPIO_TypeDef *p = stm32f769_port_addrs[port_idx];
   return p;
 }
 
-static uint16_t stm32f769_get_port_clkid(unsigned int port_idx) {
+uint16_t stm32f769_get_port_clkid(unsigned int port_idx) {
   return stm32f769_port_clkids[port_idx];
 }
 
 void stm32f769_pinctrl_configure_pin(uint32_t pin)
 {
-	uint8_t port_idx;
-	uint32_t port;
-	uint32_t pin_num;
-	uint32_t af;
-	uint32_t dir;
-	uint16_t clkid;
-	//uint16_t strength;
-
-	port_idx = STM32F769_PORT_GET(pin);
+  uint8_t port_idx;
+  uint32_t port;
+  uint32_t pin_num;
+  uint32_t af;
+  uint16_t clkid;
+  //uint16_t strength;
+
+  port_idx = STM32F769_PORT_GET(pin);
   GPIO_TypeDef *p = stm32f769_get_port(port_idx);
 
   clkid = stm32f769_get_port_clkid(port_idx);
 
-	pin_num = STM32F769_PIN_GET(pin);
-	af = STM32F769_AF_GET(pin);
-	dir = STM32F769_DIR_GET(pin);
-
-	clock_control_mik32_on(clkid);
-
-	uint32_t pupd;
-	uint32_t ds;
-	uint32_t cfg;
-	uint32_t dirin;
-	uint32_t dirout;
+  pin_num = STM32F769_PIN_GET(pin);
+  af = STM32F769_AF_GET(pin);
 
-	pupd = MIK32_PAD_PUPD(port);
-	cfg = MIK32_PAD_CFG(port);
-	ds = MIK32_PAD_DS(port);
-	dirin = MIK32_GPIO_DIRIN(port);
-	dirout = MIK32_GPIO_DIROUT(port);
+  stm32f769_clock_control_on(clkid);
 
-	if (af != STM32F769_ANALOG) {
+  if (af != STM32F769_ANALOG) {
     stm32f769_gpio_configure_af(NULL, pin, pin);
-	} else {
-		// set mode ANALOG
+    if (pin_num < 8) {
+      MODIFY_REG(p->AFR[0], (0xf << (pin_num * 4)), (af << (pin_num * 4)));
+    } else {
+      MODIFY_REG(p->AFR[1], (0xf << ((pin_num - 8) * 4)), (af << ((pin_num - 8) * 4)));
+    }
+  } else {
+    // set mode ANALOG
     stm32f769_gpio_configure_analog(NULL, pin, pin);
-	}
+  }
 }
 
 /* int pinctrl_configure_pins(const pinctrl_soc_pin_t *pins, uint8_t pin_cnt, */
-/* 			   uintptr_t reg) */
+/*         uintptr_t reg) */
 /* { */
-/* 	ARG_UNUSED(reg); */
+/*  ARG_UNUSED(reg); */
 
-/* 	for (uint8_t i = 0U; i < pin_cnt; i++) { */
-/* 		pinctrl_configure_pin(pins[i]); */
-/* 	} */
+/*  for (uint8_t i = 0U; i < pin_cnt; i++) { */
+/*    pinctrl_configure_pin(pins[i]); */
+/*  } */
 
-/* 	return 0; */
+/*  return 0; */
 /* } */