diff --git a/cpu/kinetis_common/include/periph_cpu.h b/cpu/kinetis_common/include/periph_cpu.h index d9d529bf3161e..0ce3a6ffcf537 100644 --- a/cpu/kinetis_common/include/periph_cpu.h +++ b/cpu/kinetis_common/include/periph_cpu.h @@ -21,7 +21,7 @@ #include -#include "periph/dev_enums.h" +#include "cpu.h" #ifdef __cplusplus extern "C" { @@ -40,20 +40,10 @@ typedef uint16_t gpio_t; */ #define GPIO_UNDEF (0xffff) -/** - * @brief Definition of pin and port positions in the gpio_t type - * @{ - */ -#define GPIO_PORT_SHIFT (8) -#define GPIO_PORT_MASK (0xff << (GPIO_PORT_SHIFT)) -#define GPIO_PIN_SHIFT (0) -#define GPIO_PIN_MASK (0xff << (GPIO_PIN_SHIFT)) -/** @} */ - /** * @brief Define a CPU specific GPIO pin generator macro */ -#define GPIO_PIN(port, pin) ((port << GPIO_PORT_SHIFT) | pin) +#define GPIO_PIN(x, y) (((x + 1) << 12) | (x << 6) | y) /** * @brief Length of the CPU_ID in octets @@ -66,9 +56,9 @@ typedef uint16_t gpio_t; */ #define HAVE_GPIO_PP_T typedef enum { - GPIO_NOPULL = 4, /**< do not use internal pull resistors */ - GPIO_PULLUP = 9, /**< enable internal pull-up resistor */ - GPIO_PULLDOWN = 8 /**< enable internal pull-down resistor */ + GPIO_NOPULL = 0x0, /**< do not use internal pull resistors */ + GPIO_PULLUP = 0x3, /**< enable internal pull-up resistor */ + GPIO_PULLDOWN = 0x2 /**< enable internal pull-down resistor */ } gpio_pp_t; /** @} */ @@ -78,16 +68,16 @@ typedef enum { */ #define HAVE_GPIO_FLANK_T typedef enum { - GPIO_LOGIC_ZERO = 0x8, /**< interrupt on logic zero */ - GPIO_RISING = 0x9, /**< emit interrupt on rising flank */ - GPIO_FALLING = 0xa, /**< emit interrupt on falling flank */ - GPIO_BOTH = 0xb, /**< emit interrupt on both flanks */ - GPIO_LOGIC_ONE = 0xc, /**< interrupt on logic one */ + GPIO_RISING = PORT_PCR_IRQC(0x9), /**< emit interrupt on rising flank */ + GPIO_FALLING = PORT_PCR_IRQC(0xa), /**< emit interrupt on falling flank */ + GPIO_BOTH = PORT_PCR_IRQC(0xb), /**< emit interrupt on both flanks */ } gpio_flank_t; /** @} */ /** * @brief Available ports on the Kinetis family + * + * @todo This is not equal for all members of the Kinetis family, right? */ enum { PORT_A = 0, /**< port A */ @@ -97,9 +87,22 @@ enum { PORT_E = 4, /**< port E */ PORT_F = 5, /**< port F */ PORT_G = 6, /**< port G */ - PORT_NUMOF }; +/** + * @brief Definition of possible alternate function modes for the pin mux + */ +typedef enum { + GPIO_AF_ANALOG = PORT_PCR_MUX(0), + GPIO_AF_GPIO = PORT_PCR_MUX(1), + GPIO_AF2 = PORT_PCR_MUX(2), + GPIO_AF3 = PORT_PCR_MUX(3), + GPIO_AF4 = PORT_PCR_MUX(4), + GPIO_AF5 = PORT_PCR_MUX(5), + GPIO_AF6 = PORT_PCR_MUX(6), + GPIO_AF7 = PORT_PCR_MUX(7) +} gpio_af_t; + #ifdef __cplusplus } #endif diff --git a/cpu/kinetis_common/periph/gpio.c b/cpu/kinetis_common/periph/gpio.c index e612d7c482db2..eba29f3cb0368 100644 --- a/cpu/kinetis_common/periph/gpio.c +++ b/cpu/kinetis_common/periph/gpio.c @@ -24,384 +24,228 @@ * @} */ -#include "cpu.h" #include "sched.h" #include "thread.h" -#include "utlist.h" -#include "mutex.h" +#include "cpu.h" #include "periph/gpio.h" -#include "periph_conf.h" -#define ENABLE_DEBUG (0) -#include "debug.h" +#define PORT_SHIFT (12) +#define GPIO_SHIFT (6) -#ifndef PIN_MUX_FUNCTION_ANALOG -#define PIN_MUX_FUNCTION_ANALOG 0 -#endif +#define PORT_ADDR_MASK (0x00007000) +#define GPIO_ADDR_MASK (0x000001c0) -#ifndef PIN_MUX_FUNCTION_GPIO -#define PIN_MUX_FUNCTION_GPIO 1 -#endif +#define PORT_ADDR_BASE (PORTA_BASE & ~(PORT_ADDR_MASK)) +#define GPIO_ADDR_BASE (GPIOA_BASE & ~(GPIO_ADDR_MASK)) + +#define GPIO_PORTS_NUMOF (7) + +/** + * @brief We allow for 7 (4-bit - 1) concurrent external interrupts to be set + */ +#define CTX_NUMOF (7U) + +typedef struct { + gpio_cb_t cb; + void *arg; + uint32_t state; +} isr_ctx_t; /** - * @brief Linked list entry for interrupt configurations. + * @brief Allocation of memory for 7 independent interrupt slots */ -typedef struct gpio_int_config_entry { - struct gpio_int_config_entry* next; /**< pointer to next entry */ - gpio_cb_t cb; /**< callback called from GPIO interrupt */ - void *arg; /**< argument passed to the callback */ - uint32_t irqc; /**< remember interrupt configuration between disable/enable */ - uint8_t pin; /**< pin number within the port */ -} gpio_int_config_entry_t; - -/* Linked list of interrupt handlers for each port. - * Using a linked list saves memory when less than 80% of all GPIO pins on the - * CPU are configured for interrupts, which is true for (almost) all real world - * applications. */ -static gpio_int_config_entry_t* gpio_interrupts[PORT_NUMOF]; - -static mutex_t int_config_lock = MUTEX_INIT; - -/* Maximum number of simultaneously enabled GPIO interrupts. Each pool entry - * uses 20 bytes of RAM. +static isr_ctx_t isr_ctx[CTX_NUMOF] = {{0}}; + +/** + * @brief Allocation of 4 bit per pin to map a pin to an interrupt context */ -#ifndef GPIO_INT_POOL_SIZE -#define GPIO_INT_POOL_SIZE 16 +static uint32_t isr_map[] = { + 0xffffffff, 0xffffffff, 0xffffffff, 0xffffffff, + 0xffffffff, 0xffffffff, 0xffffffff, 0xffffffff, + 0xffffffff, 0xffffffff, 0xffffffff, 0xffffffff, + 0xffffffff, 0xffffffff, 0xffffffff, 0xffffffff, +#if GPIO_PORTS_NUMOF > 4 + 0xffffffff, 0xffffffff, 0xffffffff, 0xffffffff, +#endif +#if GPIO_PORTS_NUMOF > 5 + 0xffffffff, 0xffffffff, 0xffffffff, 0xffffffff, +#endif +#if GPIO_PORTS_NUMOF > 6 + 0xffffffff, 0xffffffff, 0xffffffff, 0xffffffff, +#endif +#if GPIO_PORTS_NUMOF > 7 + 0xffffffff, 0xffffffff, 0xffffffff, 0xffffffff #endif +}; -/* Pool of linked list entries which can be used by any port configuration. - * Rationale: Avoid dynamic memory inside low level periph drivers. */ -gpio_int_config_entry_t config_pool[GPIO_INT_POOL_SIZE]; -static PORT_Type * const _port_ptrs[] = PORT_BASE_PTRS; -static GPIO_Type * const _gpio_ptrs[] = GPIO_BASE_PTRS; +static inline PORT_Type *port(gpio_t pin) +{ + return (PORT_Type *)(PORT_ADDR_BASE | (pin & PORT_ADDR_MASK)); +} -static inline uint32_t _port_num(gpio_t dev) { - return (uint32_t)((dev & GPIO_PORT_MASK) >> GPIO_PORT_SHIFT); +static inline GPIO_Type *gpio(gpio_t pin) +{ + return (GPIO_Type *)(GPIO_ADDR_BASE | (pin & GPIO_ADDR_MASK)); } -static inline uint8_t _pin_num(gpio_t dev) { - return (uint8_t)((dev & GPIO_PIN_MASK) >> GPIO_PIN_SHIFT); +static inline int port_num(gpio_t pin) +{ + return (int)((pin >> GPIO_SHIFT) & 0x7); } -static inline PORT_Type *_port(gpio_t dev) { - return _port_ptrs[_port_num(dev)]; +static inline int pin_num(gpio_t pin) +{ + return (int)(pin & 0x3f); } -static inline GPIO_Type *_gpio(gpio_t dev) { - return _gpio_ptrs[_port_num(dev)]; +static inline void clk_en(gpio_t pin) +{ + BITBAND_REG32(SIM->SCGC5, SIM_SCGC5_PORTA_SHIFT + port_num(pin)) = 1; } -static void _clear_interrupt_config(gpio_t dev) { - gpio_int_config_entry_t* entry = NULL; - uint8_t pin_number = _pin_num(dev); - mutex_lock(&int_config_lock); - /* Search for the given pin in the port's interrupt configuration */ - LL_SEARCH_SCALAR(gpio_interrupts[_port_num(dev)], entry, pin, pin_number); - if (entry != NULL) { - LL_DELETE(gpio_interrupts[_port_num(dev)], entry); - /* pin == 0 means the entry is available */ - entry->pin = 0; - } - mutex_unlock(&int_config_lock); +/** + * @brief Get context for a specific pin + */ +static inline int get_ctx(int port, int pin) +{ + return (isr_map[(port * 4) + (pin >> 3)] >> ((pin & 0x7) * 4)) & 0xf; } -static gpio_int_config_entry_t* _allocate_interrupt_config(uint8_t port) { - gpio_int_config_entry_t* ret = NULL; - - mutex_lock(&int_config_lock); - for (uint8_t i = 0; i < GPIO_INT_POOL_SIZE; ++i) { - if (config_pool[i].pin == 0) { - /* temporarily set pin to something non-zero until the proper pin - * number is set by the init code */ - config_pool[i].pin = 200; - ret = &config_pool[i]; - LL_PREPEND(gpio_interrupts[port], ret); - break; +/** + * @brief Find a free spot in the array containing the interrupt contexts + */ +static int get_free_ctx(void) +{ + for (int i = 0; i < CTX_NUMOF; i++) { + if (isr_ctx[i].cb == NULL) { + return i; } } - mutex_unlock(&int_config_lock); - - return ret; + return -1; } -int gpio_init(gpio_t dev, gpio_dir_t dir, gpio_pp_t pushpull) +/** + * @brief Write an entry to the context map array + */ +static void write_map(int port, int pin, int ctx) { - switch (_port_num(dev)) { -#ifdef PORTA_BASE - case PORT_A: - PORTA_CLOCK_GATE = 1; - break; -#endif - -#ifdef PORTB_BASE - case PORT_B: - PORTB_CLOCK_GATE = 1; - break; -#endif - -#ifdef PORTC_BASE - case PORT_C: - PORTC_CLOCK_GATE = 1; - break; -#endif - -#ifdef PORTD_BASE - case PORT_D: - PORTD_CLOCK_GATE = 1; - break; -#endif - -#ifdef PORTE_BASE - case PORT_E: - PORTE_CLOCK_GATE = 1; - break; -#endif - -#ifdef PORTF_BASE - case PORT_F: - PORTF_CLOCK_GATE = 1; - break; -#endif - -#ifdef PORTG_BASE - case PORT_G: - PORTG_CLOCK_GATE = 1; - break; -#endif - - default: - return -1; - } - - uint8_t pin = _pin_num(dev); - PORT_Type *port = _port(dev); - GPIO_Type *gpio = _gpio(dev); - - _clear_interrupt_config(dev); - - /* Reset all pin control settings for the pin */ - /* Switch to analog input function while fiddling with the settings, to be safe. */ - port->PCR[pin] = PORT_PCR_MUX(PIN_MUX_FUNCTION_ANALOG); - - /* set to push-pull configuration */ - switch (pushpull) { - case GPIO_PULLUP: - port->PCR[pin] |= PORT_PCR_PE_MASK | PORT_PCR_PS_MASK; /* Pull enable, pull up */ - break; - - case GPIO_PULLDOWN: - port->PCR[pin] |= PORT_PCR_PE_MASK; /* Pull enable, !pull up */ - break; - - default: - break; - } + isr_map[(port * 4) + (pin >> 3)] &= ~(0xf << ((pin & 0x7) * 4)); + isr_map[(port * 4) + (pin >> 3)] |= (ctx << ((pin & 0x7) * 4)); +} +int gpio_init(gpio_t pin, gpio_dir_t dir, gpio_pp_t pullup) +{ + /* enable port clock */ + clk_en(pin); + /* configure the port, set to analog mode while doing this to be safe */ + port(pin)->PCR[pin_num(pin)] = GPIO_AF_ANALOG; + /* set pin direction */ + gpio(pin)->PDDR &= ~(1 << pin_num(pin)); + gpio(pin)->PDDR |= (dir << pin_num(pin)); if (dir == GPIO_DIR_OUT) { - BITBAND_REG32(gpio->PDDR, pin) = 1; /* set pin to output mode */ - gpio->PCOR = GPIO_PCOR_PTCO(1 << pin); /* set output to low */ - } - else { - BITBAND_REG32(gpio->PDDR, pin) = 0; /* set pin to input mode */ + gpio(pin)->PCOR = (1 << pin_num(pin)); } - - /* Select GPIO function for the pin */ - port->PCR[pin] |= PORT_PCR_MUX(PIN_MUX_FUNCTION_GPIO); - + /* enable GPIO function */ + port(pin)->PCR[pin_num(pin)] = (GPIO_AF_GPIO | pullup); return 0; } -int gpio_init_int(gpio_t dev, gpio_pp_t pushpull, gpio_flank_t flank, gpio_cb_t cb, void *arg) +int gpio_init_int(gpio_t pin, gpio_pp_t pullup, gpio_flank_t flank, gpio_cb_t cb, void *arg) { - int res; - IRQn_Type irqn; - - res = gpio_init(dev, GPIO_DIR_IN, pushpull); - - if (res < 0) { - return res; - } - - switch (_port_num(dev)) { -#ifdef PORTA_BASE - case PORT_A: - irqn = PORTA_IRQn; - break; -#endif - -#ifdef PORTB_BASE - case PORT_B: - irqn = PORTB_IRQn; - break; -#endif - -#ifdef PORTC_BASE - case PORT_C: - irqn = PORTC_IRQn; - break; -#endif - -#ifdef PORTD_BASE - case PORT_D: - irqn = PORTD_IRQn; - break; -#endif - -#ifdef PORTE_BASE - case PORT_E: - irqn = PORTE_IRQn; - break; -#endif - -#ifdef PORTF_BASE - case PORT_F: - irqn = PORTF_IRQn; - break; -#endif - -#ifdef PORTG_BASE - case PORT_G: - irqn = PORTG_IRQn; - break; -#endif - - default: - return -1; - } - - uint32_t irqc; - /* configure the active edges */ - switch (flank) { - case GPIO_RISING: - irqc = PORT_PCR_IRQC(PIN_INTERRUPT_RISING); - break; - - case GPIO_FALLING: - irqc = PORT_PCR_IRQC(PIN_INTERRUPT_FALLING); - break; - - case GPIO_BOTH: - irqc = PORT_PCR_IRQC(PIN_INTERRUPT_EDGE); - break; - - default: - /* Unknown setting */ - return -1; + if (gpio_init(pin, GPIO_DIR_IN, pullup) < 0) { + return -1; } - gpio_int_config_entry_t* config = _allocate_interrupt_config(_port_num(dev)); - if (config == NULL) { - /* No free interrupt config entries */ + /* try go grab a free spot in the context array */ + int ctx_num = get_free_ctx(); + if (ctx_num < 0) { return -1; } - /* Enable port interrupts in the NVIC */ - NVIC_SetPriority(irqn, GPIO_IRQ_PRIO); - NVIC_EnableIRQ(irqn); - - uint8_t pin = _pin_num(dev); - PORT_Type *port = _port(dev); - - config->cb = cb; - config->arg = arg; - config->irqc = irqc; - /* Allow the callback to be found by the IRQ handler by setting the proper - * pin number */ - config->pin = pin; - - port->PCR[pin] &= ~(PORT_PCR_IRQC_MASK); /* Disable interrupt */ - BITBAND_REG32(port->PCR[pin], PORT_PCR_ISF_SHIFT) = 1; /* Clear interrupt flag */ - port->PCR[pin] |= config->irqc; /* Enable interrupt */ - + /* save interrupt context */ + isr_ctx[ctx_num].cb = cb; + isr_ctx[ctx_num].arg = arg; + isr_ctx[ctx_num].state = flank; + write_map(port_num(pin), pin_num(pin), ctx_num); + + /* clear interrupt flags */ + port(pin)->ISFR &= ~(1 << pin_num(pin)); + /* enable global port interrupts in the NVIC */ + NVIC_EnableIRQ(PORTA_IRQn + port_num(pin)); + /* finally, enable the interrupt for the select pin */ + port(pin)->PCR[pin_num(pin)] |= flank; return 0; } -void gpio_irq_enable(gpio_t dev) +void gpio_init_af(gpio_t pin, gpio_af_t af) { - /* Restore saved state */ - PORT_Type *port = _port(dev); - gpio_int_config_entry_t* entry = NULL; - uint8_t pin_number = _pin_num(dev); - mutex_lock(&int_config_lock); - /* Search for the given pin in the port's interrupt configuration */ - LL_SEARCH_SCALAR(gpio_interrupts[_port_num(dev)], entry, pin, pin_number); - mutex_unlock(&int_config_lock); - if (entry == NULL) { - /* Pin has not been configured for interrupts */ - return; - } - uint32_t irqc = entry->irqc; - port->PCR[pin_number] &= ~(PORT_PCR_IRQC_MASK); - port->PCR[pin_number] |= irqc; + /* enable clock */ + clk_en(pin); + /* set alternate function mode */ + port(pin)->PCR[pin_num(pin)] = af; } -void gpio_irq_disable(gpio_t dev) +void gpio_irq_enable(gpio_t pin) { - /* Save irqc state before disabling to allow enabling with the same trigger - * settings later. */ - PORT_Type *port = _port(dev); - uint8_t pin_number = _pin_num(dev); - uint32_t irqc = PORT_PCR_IRQC_MASK & port->PCR[pin_number]; - gpio_int_config_entry_t* entry = NULL; - mutex_lock(&int_config_lock); - /* Search for the given pin in the port's interrupt configuration */ - LL_SEARCH_SCALAR(gpio_interrupts[_port_num(dev)], entry, pin, pin_number); - if (entry == NULL) { - /* Pin has not been configured for interrupts */ - mutex_unlock(&int_config_lock); - return; - } - entry->irqc = irqc; - mutex_unlock(&int_config_lock); - port->PCR[pin_number] &= ~(PORT_PCR_IRQC_MASK); + int ctx = get_ctx(port_num(pin), pin_num(pin)); + port(pin)->PCR[pin_num(pin)] |= isr_ctx[ctx].state; } -int gpio_read(gpio_t dev) +void gpio_irq_disable(gpio_t pin) { - return ((_gpio(dev)->PDIR & GPIO_PDIR_PDI(1 << _pin_num(dev))) ? 1 : 0); + int ctx = get_ctx(port_num(pin), pin_num(pin)); + isr_ctx[ctx].state = port(pin)->PCR[pin_num(pin)] & PORT_PCR_IRQC_MASK; + port(pin)->PCR[pin_num(pin)] &= ~(PORT_PCR_IRQC_MASK); } -void gpio_set(gpio_t dev) +int gpio_read(gpio_t pin) { - _gpio(dev)->PSOR = (1 << _pin_num(dev)); + if (gpio(pin)->PDDR & (1 << pin_num(pin))) { + return (gpio(pin)->PDOR & (1 << pin_num(pin))) ? 1 : 0; + } + else { + return (gpio(pin)->PDIR & (1 << pin_num(pin))) ? 1 : 0; + } +} + +void gpio_set(gpio_t pin) +{ + gpio(pin)->PSOR = (1 << pin_num(pin)); } -void gpio_clear(gpio_t dev) +void gpio_clear(gpio_t pin) { - _gpio(dev)->PCOR = (1 << _pin_num(dev)); + gpio(pin)->PCOR = (1 << pin_num(pin)); } -void gpio_toggle(gpio_t dev) +void gpio_toggle(gpio_t pin) { - _gpio(dev)->PTOR = (1 << _pin_num(dev)); + gpio(pin)->PTOR = (1 << pin_num(pin)); } -void gpio_write(gpio_t dev, int value) +void gpio_write(gpio_t pin, int value) { if (value) { - _gpio(dev)->PSOR = (1 << _pin_num(dev)); + gpio(pin)->PSOR = (1 << pin_num(pin)); } else { - _gpio(dev)->PCOR = (1 << _pin_num(dev)); + gpio(pin)->PCOR = (1 << pin_num(pin)); } } -static inline void irq_handler(uint8_t port_num) +static inline void irq_handler(PORT_Type *port, int port_num) { - gpio_int_config_entry_t *entry; - PORT_Type *port = _port_ptrs[port_num]; - uint32_t isf = port->ISFR; /* Interrupt status flags */ - LL_FOREACH(gpio_interrupts[port_num], entry) { - if (isf & (1 << entry->pin)) { - if (entry->cb != NULL) { - entry->cb(entry->arg); - } + /* take interrupt flags only from pins which interrupt is enabled */ + uint32_t status = port->ISFR; + + for (int i = 0; i < 32; i++) { + if ((status & (1 << i)) && (port->PCR[i] & PORT_PCR_IRQC_MASK)) { + port->PCR[i] |= PORT_PCR_ISF_MASK; + int ctx = get_ctx(port_num, i); + isr_ctx[ctx].cb(isr_ctx[ctx].arg); } } - /* Clear interrupt flags */ - port->ISFR = isf; - if (sched_context_switch_request) { thread_yield(); } @@ -410,48 +254,48 @@ static inline void irq_handler(uint8_t port_num) #ifdef PORTA_BASE void isr_porta(void) { - irq_handler(PORT_A); + irq_handler(PORTA, 0); } #endif /* PORTA_BASE */ #ifdef PORTB_BASE void isr_portb(void) { - irq_handler(PORT_B); + irq_handler(PORTB, 1); } #endif /* ISR_PORT_B */ #ifdef PORTC_BASE void isr_portc(void) { - irq_handler(PORT_C); + irq_handler(PORTC, 2); } #endif /* ISR_PORT_C */ #ifdef PORTD_BASE void isr_portd(void) { - irq_handler(PORT_D); + irq_handler(PORTD, 3); } #endif /* ISR_PORT_D */ #ifdef PORTE_BASE void isr_porte(void) { - irq_handler(PORT_E); + irq_handler(PORTE, 4); } #endif /* ISR_PORT_E */ #ifdef PORTF_BASE void isr_portf(void) { - irq_handler(PORT_F); + irq_handler(PORTF, 5); } #endif /* ISR_PORT_F */ #ifdef PORTG_BASE void isr_portg(void) { - irq_handler(PORT_G); + irq_handler(PORTG, 6); } #endif /* ISR_PORT_G */