1 files changed, 75 insertions, 0 deletions
diff --git a/c/src/lib/libbsp/arm/stm32f4/startup/io.c b/c/src/lib/libbsp/arm/stm32f4/startup/io.c
new file mode 100644
index 0000000000..1bba73cd37
--- /dev/null
+++ b/c/src/lib/libbsp/arm/stm32f4/startup/io.c
@@ -0,0 +1,75 @@
+/*
+ * Copyright (c) 2012 Sebastian Huber.  All rights reserved.
+ *
+ *  embedded brains GmbH
+ *  Obere Lagerstr. 30
+ *  82178 Puchheim
+ *  Germany
+ *  <rtems@embedded-brains.de>
+ *
+ * The license and distribution terms for this file may be
+ * found in the file LICENSE in this distribution or at
+ * http://www.rtems.com/license/LICENSE.
+ */
+
+#include <bsp/io.h>
+
+#include <rtems.h>
+
+static void clear_and_set(
+  volatile uint32_t *reg,
+  unsigned index,
+  unsigned width,
+  uint32_t set
+)
+{
+  uint32_t one = 1;
+  uint32_t mask = (one << width) - one;
+  unsigned shift = width * index;
+  uint32_t val = *reg;
+
+  val &= ~(mask << shift);
+  val |= set << shift;
+
+  *reg = val;
+}
+
+void stm32f4_gpio_set_config(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;
+  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);
+  rtems_interrupt_enable(level);
+}
+
+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;
+
+  gpio->bsrr = one << (index + offset);
+}
+
+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;
+}