diff --git a/cpu/cc2538/cc2538-gpio.c b/cpu/cc2538/cc2538-gpio.c deleted file mode 100644 index 86bc8dbf7..000000000 --- a/cpu/cc2538/cc2538-gpio.c +++ /dev/null @@ -1,38 +0,0 @@ -/* - * Copyright (C) 2015 Loci Controls Inc. - * - * This file is subject to the terms and conditions of the GNU Lesser - * General Public License v2.1. See the file LICENSE in the top level - * directory for more details. - */ - -/** - * @addtogroup cpu_cc2538 - * @{ - * - * @file cc2538-gpio.c - * @brief cc2538 GPIO controller driver implementation - * - * @author Ian Martin - * @{ - */ - -#include "cc2538_gpio.h" - -void cc2538_gpio_port_init(cc2538_gpio_t* gpio_port) { - /* Disable power-up interrupts */ - gpio_port->PI_IEN = 0; - - /* Disable normal interrupts */ - gpio_port->IE = 0; - - /* Clear any pending interrupts */ - gpio_port->IC = 0xff; -} - -void cc2538_gpio_init(void) { - cc2538_gpio_port_init(GPIO_A); - cc2538_gpio_port_init(GPIO_B); - cc2538_gpio_port_init(GPIO_C); - cc2538_gpio_port_init(GPIO_D); -} diff --git a/cpu/cc2538/cpu.c b/cpu/cc2538/cpu.c index 417e14892..68c7b39e9 100644 --- a/cpu/cc2538/cpu.c +++ b/cpu/cc2538/cpu.c @@ -47,8 +47,6 @@ void cpu_init(void) SYS_CTRL->I_MAP = 1; /* initialize the clock system */ cpu_clock_init(); - /* initialize the GPIO controller */ - cc2538_gpio_init(); } /** diff --git a/cpu/cc2538/include/cc2538_gpio.h b/cpu/cc2538/include/cc2538_gpio.h index f9197ad9e..d58938d6a 100644 --- a/cpu/cc2538/include/cc2538_gpio.h +++ b/cpu/cc2538/include/cc2538_gpio.h @@ -122,6 +122,11 @@ enum { */ #define gpio_dir_input(gpio_num) ( GPIO_NUM_TO_DEV(gpio_num)->DIR &= ~GPIO_PIN_MASK(GPIO_BIT_NUM(gpio_num)) ) +/** + * @brief Read the value of the given pin + * + * @param[in] gpio_num GPIO number (0-31) + */ #define cc2538_gpio_read(gpio_num) ( (GPIO_NUM_TO_DEV(gpio_num)->DATA >> GPIO_BIT_NUM(gpio_num)) & 1 ) /** @@ -218,14 +223,8 @@ typedef struct { * @brief IOC port component registers */ typedef struct { - cc2538_reg_t PA_SEL[8]; /**< Port A SEL register */ - cc2538_reg_t PB_SEL[8]; /**< Port B SEL register */ - cc2538_reg_t PC_SEL[8]; /**< Port C SEL register */ - cc2538_reg_t PD_SEL[8]; /**< Port D SEL register */ - cc2538_reg_t PA_OVER[8]; /**< Port A OVER register */ - cc2538_reg_t PB_OVER[8]; /**< Port B OVER register */ - cc2538_reg_t PC_OVER[8]; /**< Port C OVER register */ - cc2538_reg_t PD_OVER[8]; /**< Port D OVER register */ + cc2538_reg_t SEL[32]; /**< Port A to D SEL registers */ + cc2538_reg_t OVER[32]; /**< Port A OVER register */ } cc2538_ioc_t; /** @@ -283,19 +282,6 @@ typedef struct { #define IOC ((cc2538_ioc_t *)0x400d4000) /**< IOC instance */ /** @} */ -/** - * @brief Initialize a GPIO port. - * @details Initializes the port to a default state, disables interrupts and - * clears any pending interrupts. - * @param[in] gpio_port A pointer to the port's instance, e.g. "GPIO_A". - */ -void cc2538_gpio_port_init(cc2538_gpio_t* gpio_port); - -/** - * @brief Initialize all four GPIO ports. - */ -void cc2538_gpio_init(void); - #ifdef __cplusplus } /* end extern "C" */ #endif diff --git a/cpu/cc2538/include/cc2538_gptimer.h b/cpu/cc2538/include/cc2538_gptimer.h index 34e7bbe97..a9ce19453 100644 --- a/cpu/cc2538/include/cc2538_gptimer.h +++ b/cpu/cc2538/include/cc2538_gptimer.h @@ -117,6 +117,9 @@ typedef struct { cc2538_reg_t SYNC; /**< GPTIMER Synchronize */ cc2538_reg_t RESERVED2; /**< Reserved word */ + /** + * @brief Interrupt mask control + */ union { cc2538_reg_t IMR; /**< GPTIMER Interrupt Mask */ struct { diff --git a/cpu/cc2538/include/periph_cpu.h b/cpu/cc2538/include/periph_cpu.h index 81fc4b2ef..1b859fb91 100644 --- a/cpu/cc2538/include/periph_cpu.h +++ b/cpu/cc2538/include/periph_cpu.h @@ -1,5 +1,5 @@ /* - * Copyright (C) 2015 Freie Universität Berlin + * Copyright (C) 2015-2016 Freie Universität Berlin * * This file is subject to the terms and conditions of the GNU Lesser * General Public License v2.1. See the file LICENSE in the top level @@ -40,14 +40,15 @@ extern "C" { */ #define HAVE_GPIO_T typedef uint32_t gpio_t; - -#define GPIO_PIN(port_num, bit_num) GPIO_PXX_TO_NUM(port_num, bit_num) /** @} */ /** - * @brief Definition of a fitting UNDEF value + * @brief Define a custom GPIO_PIN macro + * + * For the CC2538, we use OR the gpio ports base register address with the + * actual pin number. */ -#define GPIO_UNDEF (0xffffffff) +#define GPIO_PIN(port, pin) (gpio_t)(((uint32_t)GPIO_A + (port << 12)) | pin) /** * @brief declare needed generic SPI functions @@ -57,6 +58,24 @@ typedef uint32_t gpio_t; #define PERIPH_SPI_NEEDS_TRANSFER_REGS /** @} */ +/** + * @brief Override the default GPIO mode settings + * @{ + */ +#define HAVE_GPIO_MODE_T +typedef enum { + GPIO_IN = ((uint8_t)0x00), /**< input, no pull */ + GPIO_IN_PD = ((uint8_t)IOC_OVERRIDE_PDE), /**< input, pull-down */ + GPIO_IN_PU = ((uint8_t)IOC_OVERRIDE_PUE), /**< input, pull-up */ + GPIO_OUT = ((uint8_t)IOC_OVERRIDE_OE), /**< output */ + GPIO_OD = (0xff), /**< not supported */ + GPIO_OD_PU = (0xff) /**< not supported */ +} gpio_mode_t; +/** @} */ + +/** + * @brief I2C configuration options + */ typedef struct { gpio_t scl_pin; /**< pin used for SCL */ gpio_t sda_pin; /**< pin used for SDA */ @@ -64,8 +83,8 @@ typedef struct { /** * @brief SPI configuration data structure + * @{ */ -#define HAVE_PERIPH_SPI_CONF_T typedef struct { cc2538_ssi_t *dev; /**< SSI device */ gpio_t mosi_pin; /**< pin used for MOSI */ @@ -73,6 +92,7 @@ typedef struct { gpio_t sck_pin; /**< pin used for SCK */ gpio_t cs_pin; /**< pin used for CS */ } periph_spi_conf_t; +/** @} */ /** * @brief Timer configuration data diff --git a/cpu/cc2538/periph/gpio.c b/cpu/cc2538/periph/gpio.c index 595addae0..f5e71413c 100644 --- a/cpu/cc2538/periph/gpio.c +++ b/cpu/cc2538/periph/gpio.c @@ -1,19 +1,21 @@ /* * Copyright (C) 2014 Loci Controls Inc. + * 2016 Freie Universität Berlin * - * This file is subject to the terms and conditions of the GNU Lesser General - * Public License v2.1. See the file LICENSE in the top level directory for more - * details. + * This file is subject to the terms and conditions of the GNU Lesser + * General Public License v2.1. See the file LICENSE in the top level + * directory for more details. */ /** - * @ingroup driver_periph + * @ingroup cpu_cc2538 * @{ * * @file * @brief Low-level GPIO driver implementation * * @author Ian Martin + * @author Hauke Petersen * * @} */ @@ -22,584 +24,187 @@ #include "cpu.h" #include "periph/gpio.h" -#include "periph_conf.h" -/** - * @brief Generate a bit mask in which only the specified bit is high. - * - * @param[in] n Number of the bit to set high in the mask. - * - * @return A bit mask in which bit n is high. -*/ -#define BIT(n) ( 1 << (n) ) +#define GPIO_MASK (0xfffff000) +#define PORTNUM_MASK (0x00007000) +#define PORTNUM_SHIFT (12U) +#define PIN_MASK (0x00000007) +#define MODE_NOTSUP (0xff) -/** - * @brief Checks a bit in enable_lut to determine if a GPIO is enabled - * - * @param[in] dev RIOT GPIO device number - * - * @return 0 or 1 indicating if the specified GPIO is enabled -*/ -#define gpio_enabled(dev) ( (enable_lut >> (dev)) & 1 ) - -static gpio_isr_ctx_t gpio_config[GPIO_NUMOF]; - -const uint32_t enable_lut = 0 -#if GPIO_0_EN - | BIT(0) -#endif -#if GPIO_1_EN - | BIT(1) -#endif -#if GPIO_2_EN - | BIT(2) -#endif -#if GPIO_3_EN - | BIT(3) -#endif -#if GPIO_4_EN - | BIT(4) -#endif -#if GPIO_5_EN - | BIT(5) -#endif -#if GPIO_6_EN - | BIT(6) -#endif -#if GPIO_7_EN - | BIT(7) -#endif -#if GPIO_8_EN - | BIT(8) -#endif -#if GPIO_9_EN - | BIT(9) -#endif -#if GPIO_10_EN - | BIT(10) -#endif -#if GPIO_11_EN - | BIT(11) -#endif -#if GPIO_12_EN - | BIT(12) -#endif -#if GPIO_13_EN - | BIT(13) -#endif -#if GPIO_14_EN - | BIT(14) -#endif -#if GPIO_15_EN - | BIT(15) -#endif -#if GPIO_16_EN - | BIT(16) -#endif -#if GPIO_17_EN - | BIT(17) -#endif -#if GPIO_18_EN - | BIT(18) -#endif -#if GPIO_19_EN - | BIT(19) -#endif -#if GPIO_20_EN - | BIT(20) -#endif -#if GPIO_21_EN - | BIT(21) -#endif -#if GPIO_22_EN - | BIT(22) -#endif -#if GPIO_23_EN - | BIT(23) -#endif -#if GPIO_24_EN - | BIT(24) -#endif -#if GPIO_25_EN - | BIT(25) -#endif -#if GPIO_26_EN - | BIT(26) -#endif -#if GPIO_27_EN - | BIT(27) -#endif -#if GPIO_28_EN - | BIT(28) -#endif -#if GPIO_29_EN - | BIT(29) -#endif -#if GPIO_30_EN - | BIT(30) -#endif -#if GPIO_31_EN - | BIT(31) -#endif -; - -const unsigned int pin_lut[] = { -#if GPIO_0_EN - [ 0] = GPIO_0_PIN, -#endif -#if GPIO_1_EN - [ 1] = GPIO_1_PIN, -#endif -#if GPIO_2_EN - [ 2] = GPIO_2_PIN, -#endif -#if GPIO_3_EN - [ 3] = GPIO_3_PIN, -#endif -#if GPIO_4_EN - [ 4] = GPIO_4_PIN, -#endif -#if GPIO_5_EN - [ 5] = GPIO_5_PIN, -#endif -#if GPIO_6_EN - [ 6] = GPIO_6_PIN, -#endif -#if GPIO_7_EN - [ 7] = GPIO_7_PIN, -#endif -#if GPIO_8_EN - [ 8] = GPIO_8_PIN, -#endif -#if GPIO_9_EN - [ 9] = GPIO_9_PIN, -#endif -#if GPIO_10_EN - [10] = GPIO_10_PIN, -#endif -#if GPIO_11_EN - [11] = GPIO_11_PIN, -#endif -#if GPIO_12_EN - [12] = GPIO_12_PIN, -#endif -#if GPIO_13_EN - [13] = GPIO_13_PIN, -#endif -#if GPIO_14_EN - [14] = GPIO_14_PIN, -#endif -#if GPIO_15_EN - [15] = GPIO_15_PIN, -#endif -#if GPIO_16_EN - [16] = GPIO_16_PIN, -#endif -#if GPIO_17_EN - [17] = GPIO_17_PIN, -#endif -#if GPIO_18_EN - [18] = GPIO_18_PIN, -#endif -#if GPIO_19_EN - [19] = GPIO_19_PIN, -#endif -#if GPIO_20_EN - [20] = GPIO_20_PIN, -#endif -#if GPIO_21_EN - [21] = GPIO_21_PIN, -#endif -#if GPIO_22_EN - [22] = GPIO_22_PIN, -#endif -#if GPIO_23_EN - [23] = GPIO_23_PIN, -#endif -#if GPIO_24_EN - [24] = GPIO_24_PIN, -#endif -#if GPIO_25_EN - [25] = GPIO_25_PIN, -#endif -#if GPIO_26_EN - [26] = GPIO_26_PIN, -#endif -#if GPIO_27_EN - [27] = GPIO_27_PIN, -#endif -#if GPIO_28_EN - [28] = GPIO_28_PIN, -#endif -#if GPIO_29_EN - [29] = GPIO_29_PIN, -#endif -#if GPIO_30_EN - [30] = GPIO_30_PIN, -#endif -#if GPIO_31_EN - [31] = GPIO_31_PIN, -#endif -}; - -static const uint8_t reverse_pin_lut[] = { -#if GPIO_0_EN - [GPIO_0_PIN] = 0, -#endif -#if GPIO_1_EN - [GPIO_1_PIN] = 1, -#endif -#if GPIO_2_EN - [GPIO_2_PIN] = 2, -#endif -#if GPIO_3_EN - [GPIO_3_PIN] = 3, -#endif -#if GPIO_4_EN - [GPIO_4_PIN] = 4, -#endif -#if GPIO_5_EN - [GPIO_5_PIN] = 5, -#endif -#if GPIO_6_EN - [GPIO_6_PIN] = 6, -#endif -#if GPIO_7_EN - [GPIO_7_PIN] = 7, -#endif -#if GPIO_8_EN - [GPIO_8_PIN] = 8, -#endif -#if GPIO_9_EN - [GPIO_9_PIN] = 9, -#endif -#if GPIO_10_EN - [GPIO_10_PIN] = 10, -#endif -#if GPIO_11_EN - [GPIO_11_PIN] = 11, -#endif -#if GPIO_12_EN - [GPIO_12_PIN] = 12, -#endif -#if GPIO_13_EN - [GPIO_13_PIN] = 13, -#endif -#if GPIO_14_EN - [GPIO_14_PIN] = 14, -#endif -#if GPIO_15_EN - [GPIO_15_PIN] = 15, -#endif -#if GPIO_16_EN - [GPIO_16_PIN] = 16, -#endif -#if GPIO_17_EN - [GPIO_17_PIN] = 17, -#endif -#if GPIO_18_EN - [GPIO_18_PIN] = 18, -#endif -#if GPIO_19_EN - [GPIO_19_PIN] = 19, -#endif -#if GPIO_20_EN - [GPIO_20_PIN] = 20, -#endif -#if GPIO_21_EN - [GPIO_21_PIN] = 21, -#endif -#if GPIO_22_EN - [GPIO_22_PIN] = 22, -#endif -#if GPIO_23_EN - [GPIO_23_PIN] = 23, -#endif -#if GPIO_24_EN - [GPIO_24_PIN] = 24, -#endif -#if GPIO_25_EN - [GPIO_25_PIN] = 25, -#endif -#if GPIO_26_EN - [GPIO_26_PIN] = 26, -#endif -#if GPIO_27_EN - [GPIO_27_PIN] = 27, -#endif -#if GPIO_28_EN - [GPIO_28_PIN] = 28, -#endif -#if GPIO_29_EN - [GPIO_29_PIN] = 29, -#endif -#if GPIO_30_EN - [GPIO_30_PIN] = 30, -#endif -#if GPIO_31_EN - [GPIO_31_PIN] = 31, -#endif -}; - -int gpio_init(gpio_t dev, gpio_mode_t mode) +static inline cc2538_gpio_t *gpio(gpio_t pin) +{ + return (cc2538_gpio_t *)(pin & GPIO_MASK); +} + +static inline int port_num(gpio_t pin) +{ + return (int)((pin & PORTNUM_MASK) >> PORTNUM_SHIFT) - 1; +} + +static inline int pin_num(gpio_t pin) +{ + return (int)(pin & PIN_MASK); +} + +static inline uint32_t pin_mask(gpio_t pin) { - int pin; + return (1 << (pin & PIN_MASK)); +} + +static inline int pp_num(gpio_t pin) +{ + return (port_num(pin) * 8) + pin_num(pin); +} + +static gpio_isr_ctx_t isr_ctx[4][8]; - if (!gpio_enabled(dev)) { +int gpio_init(gpio_t pin, gpio_mode_t mode) +{ + /* check if mode is valid */ + if (mode == MODE_NOTSUP) { return -1; } - pin = pin_lut[dev]; - gpio_software_control(pin); - switch (mode) { - case GPIO_IN: - gpio_dir_input(pin); - /* configure the pin's pull resistor state */ - IOC_PXX_OVER[pin] = (IOC_OVERRIDE_DIS); - break; - case GPIO_IN_PD: - gpio_dir_input(pin); - /* configure the pin's pull resistor state */ - IOC_PXX_OVER[pin] = (IOC_OVERRIDE_PDE); - break; - case GPIO_IN_PU: - gpio_dir_input(pin); - /* configure the pin's pull resistor state */ - IOC_PXX_OVER[pin] = (IOC_OVERRIDE_PUE); - break; - case GPIO_OUT: - gpio_dir_output(pin); - /* configure the pin's pull resistor state */ - IOC_PXX_OVER[pin] = (IOC_OVERRIDE_OE); - break; - default: - return -1; + /* disable any alternate function and any eventual interrupts */ + gpio(pin)->IE &= ~pin_mask(pin); + gpio(pin)->AFSEL &= ~pin_mask(pin); + /* configure pull configuration */ + IOC->OVER[pp_num(pin)] = mode; + + /* set pin direction */ + if (mode == IOC_OVERRIDE_OE) { + gpio(pin)->DIR |= pin_mask(pin); + } + else { + gpio(pin)->DIR &= ~pin_mask(pin); } + /* clear pin */ + gpio(pin)->DATA &= ~pin_mask(pin); return 0; } -int gpio_init_int(gpio_t dev, gpio_mode_t mode, gpio_flank_t flank, +int gpio_init_int(gpio_t pin, gpio_mode_t mode, gpio_flank_t flank, gpio_cb_t cb, void *arg) { - int res, pin, irq_num; - uint32_t mask; - cc2538_gpio_t* instance; - - /* Note: gpio_init() also checks if the gpio is enabled. */ - res = gpio_init(dev, mode); - if (res < 0) { - return res; + if (gpio_init(pin, mode) != 0) { + return -1; } - /* Store the callback information for later: */ - gpio_config[dev].cb = cb; - gpio_config[dev].arg = arg; - - pin = pin_lut[dev]; - mask = GPIO_PIN_MASK(GPIO_BIT_NUM(pin)); - - instance = GPIO_NUM_TO_DEV(pin); + /* store the callback information for later: */ + isr_ctx[port_num(pin)][pin_num(pin)].cb = cb; + isr_ctx[port_num(pin)][pin_num(pin)].arg = arg; - /* Enable power-up interrupts for this GPIO port: */ - SYS_CTRL_IWE |= BIT(GPIO_NUM_TO_PORT_NUM(pin)); + /* enable power-up interrupts for this GPIO port: */ + SYS_CTRL->IWE |= port_num(pin); + /* configure the active flank(s) */ + gpio(pin)->IS &= ~pin_mask(pin); switch(flank) { case GPIO_FALLING: - instance->IBE &= ~mask; /**< Not both edges */ - instance->IEV &= ~mask; /**< Falling edge */ - instance->P_EDGE_CTRL |= BIT(pin); /**< Falling edge power-up interrupt */ + gpio(pin)->IBE &= ~pin_mask(pin); + gpio(pin)->IEV &= ~pin_mask(pin); + gpio(pin)->P_EDGE_CTRL |= (1 << pp_num(pin)); break; - case GPIO_RISING: - instance->IBE &= ~mask; /**< Not both edges */ - instance->IEV |= mask; /**< Rising edge */ - instance->P_EDGE_CTRL &= ~BIT(pin); /**< Rising edge power-up interrupt */ + gpio(pin)->IBE &= ~pin_mask(pin); + gpio(pin)->IEV |= pin_mask(pin); + gpio(pin)->P_EDGE_CTRL &= (1 << pp_num(pin)); break; - case GPIO_BOTH: - instance->IBE = mask; /**< Both edges */ + gpio(pin)->IBE |= pin_mask(pin); break; + default: + return -1; } - instance->IS &= ~mask; /**< Edge triggered (as opposed to level-triggered) */ - instance->IC |= mask; /**< Clear any preexisting interrupt state */ - instance->PI_IEN |= BIT(pin); /**< Enable power-up interrupts for this pin */ - - /* Set interrupt priority for the whole GPIO port: */ - irq_num = GPIO_PORT_A_IRQn + GPIO_NUM_TO_PORT_NUM(pin); - NVIC_SetPriority(irq_num, GPIO_IRQ_PRIO); - - /* Enable interrupts for the whole GPIO port: */ - NVIC_EnableIRQ(irq_num); - - /* Enable interrupts for the specific pin: */ - instance->IE |= mask; + /* reset interrupt status */ + gpio(pin)->IC = pin_mask(pin); + gpio(pin)->PI_IEN |= (1 << pp_num(pin)); + /* enable global interrupt for the selected GPIO port */ + NVIC_EnableIRQ(GPIO_PORT_A_IRQn + port_num(pin)); + /* unmask pin interrupt */ + gpio(pin)->IE |= pin_mask(pin); return 0; } -void gpio_irq_enable(gpio_t dev) +void gpio_irq_enable(gpio_t pin) { - if (gpio_enabled(dev)) { - int pin = pin_lut[dev]; - GPIO_NUM_TO_DEV(pin)->IE |= GPIO_PIN_MASK(GPIO_BIT_NUM(pin)); - } + gpio(pin)->IE |= pin_mask(pin); } -void gpio_irq_disable(gpio_t dev) +void gpio_irq_disable(gpio_t pin) { - if (gpio_enabled(dev)) { - int pin = pin_lut[dev]; - GPIO_NUM_TO_DEV(pin)->IE &= ~GPIO_PIN_MASK(GPIO_BIT_NUM(pin)); - } + gpio(pin)->IE &= ~pin_mask(pin); } -int gpio_read(gpio_t dev) +int gpio_read(gpio_t pin) { - int pin; - - if (!gpio_enabled(dev)) { - return -1; - } - - pin = pin_lut[dev]; - return (GPIO_NUM_TO_DEV(pin)->DATA >> GPIO_BIT_NUM(pin)) & 1; + return (int)(gpio(pin)->DATA & pin_mask(pin)); } -void gpio_set(gpio_t dev) +void gpio_set(gpio_t pin) { - int pin; - - if (!gpio_enabled(dev)) { - return; - } - - pin = pin_lut[dev]; - GPIO_NUM_TO_DEV(pin)->DATA |= GPIO_PIN_MASK(GPIO_BIT_NUM(pin)); + gpio(pin)->DATA |= pin_mask(pin); } -void gpio_clear(gpio_t dev) +void gpio_clear(gpio_t pin) { - int pin; - - if (!gpio_enabled(dev)) { - return; - } - - pin = pin_lut[dev]; - GPIO_NUM_TO_DEV(pin)->DATA &= ~GPIO_PIN_MASK(GPIO_BIT_NUM(pin)); + gpio(pin)->DATA &= ~pin_mask(pin); } -void gpio_toggle(gpio_t dev) +void gpio_toggle(gpio_t pin) { - if (gpio_read(dev)) { - gpio_clear(dev); - } - else { - gpio_set(dev); - } + gpio(pin)->DATA ^= pin_mask(pin); } -void gpio_write(gpio_t dev, int value) +void gpio_write(gpio_t pin, int value) { if (value) { - gpio_set(dev); + gpio(pin)->DATA |= pin_mask(pin); } else { - gpio_clear(dev); + gpio(pin)->DATA &= ~pin_mask(pin); } } -/** @brief Interrupt service routine for Port A */ -void isr_gpioa(void) +static inline void handle_isr(cc2538_gpio_t *gpio, int port_num) { - int mis, bit; - gpio_isr_ctx_t* state; - - /* Latch and clear the interrupt status early on: */ - mis = GPIO_A->MIS; - GPIO_A->IC = 0x000000ff; - GPIO_A->IRQ_DETECT_ACK = 0x000000ff; - - for (bit = 0; bit < GPIO_BITS_PER_PORT; bit++) { - if (mis & 1) { - state = gpio_config + reverse_pin_lut[GPIO_PXX_TO_NUM(PORT_A, bit)]; - state->cb(state->arg); - } + uint32_t state = gpio->MIS; + gpio->IC = 0x000000ff; + gpio->IRQ_DETECT_ACK = 0x000000ff; - mis >>= 1; + for (int i = 0; i < GPIO_BITS_PER_PORT; i++) { + if (state & (1 << i)) { + isr_ctx[port_num][i].cb(isr_ctx[port_num][i].arg); + } } cortexm_isr_end(); } +/** @brief Interrupt service routine for Port A */ +void isr_gpioa(void) +{ + handle_isr(GPIO_A, 0); +} + /** @brief Interrupt service routine for Port B */ void isr_gpiob(void) { - int mis, bit; - gpio_isr_ctx_t* state; - - /* Latch and clear the interrupt status early on: */ - mis = GPIO_B->MIS; - GPIO_B->IC = 0x000000ff; - GPIO_B->IRQ_DETECT_ACK = 0x0000ff00; - - for (bit = 0; bit < GPIO_BITS_PER_PORT; bit++) { - if (mis & 1) { - state = gpio_config + reverse_pin_lut[GPIO_PXX_TO_NUM(PORT_B, bit)]; - state->cb(state->arg); - } - - mis >>= 1; - } - - cortexm_isr_end(); + handle_isr(GPIO_B, 1); } /** @brief Interrupt service routine for Port C */ void isr_gpioc(void) { - int mis, bit; - gpio_isr_ctx_t* state; - - /* Latch and clear the interrupt status early on: */ - mis = GPIO_C->MIS; - GPIO_C->IC = 0x000000ff; - GPIO_C->IRQ_DETECT_ACK = 0x00ff0000; - - for (bit = 0; bit < GPIO_BITS_PER_PORT; bit++) { - if (mis & 1) { - state = gpio_config + reverse_pin_lut[GPIO_PXX_TO_NUM(PORT_C, bit)]; - state->cb(state->arg); - } - - mis >>= 1; - } - - cortexm_isr_end(); + handle_isr(GPIO_C, 2); } /** @brief Interrupt service routine for Port D */ void isr_gpiod(void) { - int mis, bit; - gpio_isr_ctx_t* state; - - /* Latch and clear the interrupt status early on: */ - mis = GPIO_D->MIS; - GPIO_D->IC = 0x000000ff; - GPIO_D->IRQ_DETECT_ACK = 0xff000000; - - for (bit = 0; bit < GPIO_BITS_PER_PORT; bit++) { - if (mis & 1) { - state = gpio_config + reverse_pin_lut[GPIO_PXX_TO_NUM(PORT_D, bit)]; - state->cb(state->arg); - } - - mis >>= 1; - } - - cortexm_isr_end(); + handle_isr(GPIO_D, 3); }