diff --git a/apps/drivers/device/i2c.cpp b/apps/drivers/device/i2c.cpp index 8489db4bb0..e403ce42b5 100644 --- a/apps/drivers/device/i2c.cpp +++ b/apps/drivers/device/i2c.cpp @@ -53,6 +53,7 @@ I2C::I2C(const char *name, CDev(name, devname, irq), // public // protected + _retries(0), // private _bus(bus), _address(address), @@ -117,33 +118,44 @@ I2C::transfer(uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len struct i2c_msg_s msgv[2]; unsigned msgs; int ret; + unsigned tries; -// debug("transfer out %p/%u in %p/%u", send, send_len, recv, recv_len); + do { + // debug("transfer out %p/%u in %p/%u", send, send_len, recv, recv_len); - msgs = 0; + msgs = 0; - if (send_len > 0) { - msgv[msgs].addr = _address; - msgv[msgs].flags = 0; - msgv[msgs].buffer = send; - msgv[msgs].length = send_len; - msgs++; - } + if (send_len > 0) { + msgv[msgs].addr = _address; + msgv[msgs].flags = 0; + msgv[msgs].buffer = send; + msgv[msgs].length = send_len; + msgs++; + } - if (recv_len > 0) { - msgv[msgs].addr = _address; - msgv[msgs].flags = I2C_M_READ; - msgv[msgs].buffer = recv; - msgv[msgs].length = recv_len; - msgs++; - } + if (recv_len > 0) { + msgv[msgs].addr = _address; + msgv[msgs].flags = I2C_M_READ; + msgv[msgs].buffer = recv; + msgv[msgs].length = recv_len; + msgs++; + } + + if (msgs == 0) + return -EINVAL; - if (msgs == 0) - return -EINVAL; + ret = I2C_TRANSFER(_dev, &msgv[0], msgs); - ret = I2C_TRANSFER(_dev, &msgv[0], msgs); + if (ret == OK) + break; + + // reset the I2C bus to unwedge on error + up_i2creset(_dev); + + } while (tries++ < _retries); return ret; + } } // namespace device \ No newline at end of file diff --git a/apps/drivers/device/i2c.h b/apps/drivers/device/i2c.h index d84f7bd09b..7c5a14d6bf 100644 --- a/apps/drivers/device/i2c.h +++ b/apps/drivers/device/i2c.h @@ -52,6 +52,12 @@ class __EXPORT I2C : public CDev { protected: + /** + * The number of times a read or write operation will be retried on + * error. + */ + unsigned _retries; + /** * @ Constructor * diff --git a/nuttx/arch/arm/src/stm32/stm32_i2c.c b/nuttx/arch/arm/src/stm32/stm32_i2c.c index d02115a157..cca7a24aaa 100644 --- a/nuttx/arch/arm/src/stm32/stm32_i2c.c +++ b/nuttx/arch/arm/src/stm32/stm32_i2c.c @@ -196,11 +196,29 @@ struct stm32_trace_s uint32_t time; /* First of event or first status */ }; +/* I2C Device hardware configuration */ + +struct stm32_i2c_config_s +{ + uint32_t base; /* I2C base address */ +#ifndef CONFIG_I2C_POLLED + int ( *isr)(int, void *); /* Interrupt handler */ +#endif + uint32_t clk_bit; /* Clock enable bit */ + uint32_t reset_bit; /* Reset bit */ + uint32_t scl_pin; /* GPIO configuration for SCL as SCL */ + uint32_t scl_gpio; /* GPIO configuration for SCL as a GPIO */ + uint32_t sda_pin; /* GPIO configuration for SDA as SDA */ + uint32_t sda_gpio; /* GPIO configuration for SDA as a GPIO */ + uint32_t ev_irq; /* Event IRQ */ + uint32_t er_irq; /* Error IRQ */ +}; + /* I2C Device Private Data */ struct stm32_i2c_priv_s { - uint32_t base; /* I2C base address */ + const struct stm32_i2c_config_s *config; /* Port configuration */ int refs; /* Referernce count */ sem_t sem_excl; /* Mutual exclusion semaphore */ #ifndef CONFIG_I2C_POLLED @@ -311,9 +329,32 @@ static int stm32_i2c_transfer(FAR struct i2c_dev_s *dev, FAR struct i2c_msg_s *m ************************************************************************************/ #ifdef CONFIG_STM32_I2C1 -struct stm32_i2c_priv_s stm32_i2c1_priv = +# ifndef GPIO_I2C1_SCL_GPIO +# define GPIO_I2C1_SCL_GPIO 0 +# endif +# ifndef GPIO_I2C1_SDA_GPIO +# define GPIO_I2C1_SDA_GPIO 0 +# endif + +static const struct stm32_i2c_config_s stm32_i2c1_config = { .base = STM32_I2C1_BASE, +#ifndef CONFIG_I2C_POLLED + .isr = stm32_i2c1_isr, +#endif + .clk_bit = RCC_APB1ENR_I2C1EN, + .reset_bit = RCC_APB1RSTR_I2C1RST, + .scl_pin = GPIO_I2C1_SCL, + .scl_gpio = GPIO_I2C1_SCL_GPIO, + .sda_pin = GPIO_I2C1_SDA, + .sda_gpio = GPIO_I2C1_SDA_GPIO, + .ev_irq = STM32_IRQ_I2C1EV, + .er_irq = STM32_IRQ_I2C1ER +}; + +struct stm32_i2c_priv_s stm32_i2c1_priv = +{ + .config = &stm32_i2c1_config, .refs = 0, .intstate = INTSTATE_IDLE, .msgc = 0, @@ -326,9 +367,32 @@ struct stm32_i2c_priv_s stm32_i2c1_priv = #endif #ifdef CONFIG_STM32_I2C2 -struct stm32_i2c_priv_s stm32_i2c2_priv = +# ifndef GPIO_I2C2_SCL_GPIO +# define GPIO_I2C2_SCL_GPIO 0 +# endif +# ifndef GPIO_I2C2_SDA_GPIO +# define GPIO_I2C2_SDA_GPIO 0 +# endif + +static const struct stm32_i2c_config_s stm32_i2c2_config = { .base = STM32_I2C2_BASE, +#ifndef CONFIG_I2C_POLLED + .isr = stm32_i2c2_isr, +#endif + .clk_bit = RCC_APB1ENR_I2C2EN, + .reset_bit = RCC_APB1RSTR_I2C2RST, + .scl_pin = GPIO_I2C2_SCL, + .scl_gpio = GPIO_I2C2_SCL_GPIO, + .sda_pin = GPIO_I2C2_SDA, + .sda_gpio = GPIO_I2C2_SDA_GPIO, + .ev_irq = STM32_IRQ_I2C2EV, + .er_irq = STM32_IRQ_I2C2ER +}; + +struct stm32_i2c_priv_s stm32_i2c2_priv = +{ + .config = &stm32_i2c2_config, .refs = 0, .intstate = INTSTATE_IDLE, .msgc = 0, @@ -341,9 +405,32 @@ struct stm32_i2c_priv_s stm32_i2c2_priv = #endif #ifdef CONFIG_STM32_I2C3 -struct stm32_i2c_priv_s stm32_i2c3_priv = +# ifndef GPIO_I2C3_SCL_GPIO +# define GPIO_I2C3_SCL_GPIO 0 +# endif +# ifndef GPIO_I2C3_SDA_GPIO +# define GPIO_I2C3_SDA_GPIO 0 +# endif + +static const struct stm32_i2c_config_s stm32_i2c3_config = { .base = STM32_I2C3_BASE, +#ifndef CONFIG_I2C_POLLED + .isr = stm32_i2c3_isr, +#endif + .clk_bit = RCC_APB1ENR_I2C3EN, + .reset_bit = RCC_APB1RSTR_I2C3RST, + .scl_pin = GPIO_I2C3_SCL, + .scl_gpio = GPIO_I2C3_SCL_GPIO, + .sda_pin = GPIO_I2C3_SDA, + .sda_gpio = GPIO_I2C3_SDA_GPIO, + .ev_irq = STM32_IRQ_I2C3EV, + .er_irq = STM32_IRQ_I2C3ER +}; + +struct stm32_i2c_priv_s stm32_i2c3_priv = +{ + .config = &stm32_i2c3_config, .refs = 0, .intstate = INTSTATE_IDLE, .msgc = 0, @@ -355,7 +442,6 @@ struct stm32_i2c_priv_s stm32_i2c3_priv = }; #endif - /* Device Structures, Instantiation */ struct i2c_ops_s stm32_i2c_ops = @@ -391,7 +477,7 @@ struct i2c_ops_s stm32_i2c_ops = static inline uint16_t stm32_i2c_getreg(FAR struct stm32_i2c_priv_s *priv, uint8_t offset) { - return getreg16(priv->base + offset); + return getreg16(priv->config->base + offset); } /************************************************************************************ @@ -405,7 +491,7 @@ static inline uint16_t stm32_i2c_getreg(FAR struct stm32_i2c_priv_s *priv, static inline void stm32_i2c_putreg(FAR struct stm32_i2c_priv_s *priv, uint8_t offset, uint16_t value) { - putreg16(value, priv->base + offset); + putreg16(value, priv->config->base + offset); } /************************************************************************************ @@ -420,7 +506,7 @@ static inline void stm32_i2c_modifyreg(FAR struct stm32_i2c_priv_s *priv, uint8_t offset, uint16_t clearbits, uint16_t setbits) { - modifyreg16(priv->base + offset, clearbits, setbits); + modifyreg16(priv->config->base + offset, clearbits, setbits); } /************************************************************************************ @@ -986,7 +1072,7 @@ static inline uint32_t stm32_i2c_disablefsmc(FAR struct stm32_i2c_priv_s *priv) /* Is this I2C1 */ #ifdef CONFIG_STM32_I2C2 - if (priv->base == STM32_I2C1_BASE) + if (priv->config->base == STM32_I2C1_BASE) #endif { /* Disable FSMC unconditionally */ @@ -1310,114 +1396,35 @@ static int stm32_i2c_init(FAR struct stm32_i2c_priv_s *priv) { /* Power-up and configure GPIOs */ - switch (priv->base) - { -#ifdef CONFIG_STM32_I2C1 - case STM32_I2C1_BASE: - - /* Enable power and reset the peripheral */ - - modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_I2C1EN); + /* Enable power and reset the peripheral */ - modifyreg32(STM32_RCC_APB1RSTR, 0, RCC_APB1RSTR_I2C1RST); - modifyreg32(STM32_RCC_APB1RSTR, RCC_APB1RSTR_I2C1RST, 0); + modifyreg32(STM32_RCC_APB1ENR, 0, priv->config->clk_bit); - /* Configure pins */ - - if (stm32_configgpio(GPIO_I2C1_SCL) < 0) - { - return ERROR; - } - - if (stm32_configgpio(GPIO_I2C1_SDA) < 0) - { - stm32_unconfiggpio(GPIO_I2C1_SCL); - return ERROR; - } + modifyreg32(STM32_RCC_APB1RSTR, 0, priv->config->reset_bit); + modifyreg32(STM32_RCC_APB1RSTR, priv->config->reset_bit, 0); - /* Attach ISRs */ - -#ifndef CONFIG_I2C_POLLED - irq_attach(STM32_IRQ_I2C1EV, stm32_i2c1_isr); - irq_attach(STM32_IRQ_I2C1ER, stm32_i2c1_isr); - up_enable_irq(STM32_IRQ_I2C1EV); - up_enable_irq(STM32_IRQ_I2C1ER); -#endif - break; -#endif + /* Configure pins */ -#ifdef CONFIG_STM32_I2C2 - case STM32_I2C2_BASE: - - /* Enable power and reset the peripheral */ - - modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_I2C2EN); - - modifyreg32(STM32_RCC_APB1RSTR, 0, RCC_APB1RSTR_I2C2RST); - modifyreg32(STM32_RCC_APB1RSTR, RCC_APB1RSTR_I2C2RST, 0); - - /* Configure pins */ - - if (stm32_configgpio(GPIO_I2C2_SCL) < 0) - { - return ERROR; - } - - if (stm32_configgpio(GPIO_I2C2_SDA) < 0) - { - stm32_unconfiggpio(GPIO_I2C2_SCL); - return ERROR; - } - - /* Attach ISRs */ - -#ifndef CONFIG_I2C_POLLED - irq_attach(STM32_IRQ_I2C2EV, stm32_i2c2_isr); - irq_attach(STM32_IRQ_I2C2ER, stm32_i2c2_isr); - up_enable_irq(STM32_IRQ_I2C2EV); - up_enable_irq(STM32_IRQ_I2C2ER); -#endif - break; -#endif - -#ifdef CONFIG_STM32_I2C3 - case STM32_I2C3_BASE: - - /* Enable power and reset the peripheral */ - - modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_I2C3EN); - - modifyreg32(STM32_RCC_APB1RSTR, 0, RCC_APB1RSTR_I2C3RST); - modifyreg32(STM32_RCC_APB1RSTR, RCC_APB1RSTR_I2C3RST, 0); - - /* Configure pins */ - - if (stm32_configgpio(GPIO_I2C3_SCL) < 0) - { - return ERROR; - } - - if (stm32_configgpio(GPIO_I2C3_SDA) < 0) - { - stm32_unconfiggpio(GPIO_I2C3_SCL); - return ERROR; - } + if (stm32_configgpio(priv->config->scl_pin) < 0) + { + return ERROR; + } - /* Attach ISRs */ + if (stm32_configgpio(priv->config->sda_pin) < 0) + { + stm32_unconfiggpio(priv->config->scl_pin); + return ERROR; + } + /* Attach ISRs */ + #ifndef CONFIG_I2C_POLLED - irq_attach(STM32_IRQ_I2C3EV, stm32_i2c3_isr); - irq_attach(STM32_IRQ_I2C3ER, stm32_i2c3_isr); - up_enable_irq(STM32_IRQ_I2C3EV); - up_enable_irq(STM32_IRQ_I2C3ER); -#endif - break; + irq_attach(priv->config->ev_irq, priv->config->isr); + irq_attach(priv->config->er_irq, priv->config->isr); + up_enable_irq(priv->config->ev_irq); + up_enable_irq(priv->config->er_irq); #endif - default: - return ERROR; - } - /* Set peripheral frequency, where it must be at least 2 MHz for 100 kHz * or 4 MHz for 400 kHz. This also disables all I2C interrupts. */ @@ -1445,56 +1452,16 @@ static int stm32_i2c_deinit(FAR struct stm32_i2c_priv_s *priv) stm32_i2c_putreg(priv, STM32_I2C_CR1_OFFSET, 0); - switch (priv->base) - { -#ifdef CONFIG_STM32_I2C1 - case STM32_I2C1_BASE: - stm32_unconfiggpio(GPIO_I2C1_SCL); - stm32_unconfiggpio(GPIO_I2C1_SDA); - -#ifndef CONFIG_I2C_POLLED - up_disable_irq(STM32_IRQ_I2C1EV); - up_disable_irq(STM32_IRQ_I2C1ER); - irq_detach(STM32_IRQ_I2C1EV); - irq_detach(STM32_IRQ_I2C1ER); -#endif - modifyreg32(STM32_RCC_APB1ENR, RCC_APB1ENR_I2C1EN, 0); - break; -#endif - -#ifdef CONFIG_STM32_I2C2 - case STM32_I2C2_BASE: - stm32_unconfiggpio(GPIO_I2C2_SCL); - stm32_unconfiggpio(GPIO_I2C2_SDA); - -#ifndef CONFIG_I2C_POLLED - up_disable_irq(STM32_IRQ_I2C2EV); - up_disable_irq(STM32_IRQ_I2C2ER); - irq_detach(STM32_IRQ_I2C2EV); - irq_detach(STM32_IRQ_I2C2ER); -#endif - modifyreg32(STM32_RCC_APB1ENR, RCC_APB1ENR_I2C2EN, 0); - break; -#endif - -#ifdef CONFIG_STM32_I2C3 - case STM32_I2C3_BASE: - stm32_unconfiggpio(GPIO_I2C3_SCL); - stm32_unconfiggpio(GPIO_I2C3_SDA); + stm32_unconfiggpio(priv->config->scl_pin); + stm32_unconfiggpio(priv->config->sda_pin); #ifndef CONFIG_I2C_POLLED - up_disable_irq(STM32_IRQ_I2C3EV); - up_disable_irq(STM32_IRQ_I2C3ER); - irq_detach(STM32_IRQ_I2C3EV); - irq_detach(STM32_IRQ_I2C3ER); + up_disable_irq(priv->config->ev_irq); + up_disable_irq(priv->config->er_irq); + irq_detach(priv->config->ev_irq); + irq_detach(priv->config->er_irq); #endif - modifyreg32(STM32_RCC_APB1ENR, RCC_APB1ENR_I2C3EN, 0); - break; -#endif - - default: - return ERROR; - } + modifyreg32(STM32_RCC_APB1ENR, priv->config->clk_bit, 0); return OK; } @@ -1979,159 +1946,119 @@ int up_i2cuninitialize(FAR struct i2c_dev_s * dev) int up_i2creset(FAR struct i2c_dev_s * dev) { - // ASSERT(dev); - - // stm32_i2c_sem_wait(dev); /* ensure that address or flags don't change meanwhile */ - - // struct stm32_i2c_inst_s *inst = (struct stm32_i2c_inst_s *)dev; - // FAR struct stm32_i2c_priv_s *priv = inst->priv; - - // /* Clear any pending error interrupts */ - - // stm32_i2c_putreg(priv, STM32_I2C_SR1_OFFSET, 0); - - // /* "Note: When the STOP, START or PEC bit is set, the software must - // * not perform any write access to I2C_CR1 before this bit is - // * cleared by hardware. Otherwise there is a risk of setting a - // * second STOP, START or PEC request." However, if the bits are - // * not cleared by hardware, then we will have to do that from hardware. - // */ - - // stm32_i2c_clrstart(priv); - - // /* Reset peripheral */ - // uint16_t cr1 = stm32_i2c_getreg(priv, STM32_I2C_CR1_OFFSET); - // /* Set reset flag */ - // cr1 &= I2C_CR1_SWRST; - // stm32_i2c_putreg(priv, STM32_I2C_CR1_OFFSET, cr1); - // /* Clear CR1 */ - // stm32_i2c_putreg(priv, STM32_I2C_CR1_OFFSET, 0); - - // switch (priv->base) - // { - // case STM32_I2C1_BASE: - // modifyreg32(STM32_RCC_APB1RSTR, 0, RCC_APB1RSTR_I2C1RST); - // modifyreg32(STM32_RCC_APB1RSTR, RCC_APB1RSTR_I2C1RST, 0); - // break; - // case STM32_I2C2_BASE: - // modifyreg32(STM32_RCC_APB1RSTR, 0, RCC_APB1RSTR_I2C2RST); - // modifyreg32(STM32_RCC_APB1RSTR, RCC_APB1RSTR_I2C2RST, 0); - // break; - // case STM32_I2C3_BASE: - // modifyreg32(STM32_RCC_APB1RSTR, 0, RCC_APB1RSTR_I2C3RST); - // modifyreg32(STM32_RCC_APB1RSTR, RCC_APB1RSTR_I2C3RST, 0); - // break; - // } - - - - // stm32_i2c_putreg(priv, STM32_I2C_CR2_OFFSET, (STM32_PCLK1_FREQUENCY / 1000000)); - // stm32_i2c_setclock(priv, inst->frequency); - // inst->flags = 0; - - // /* Enable I2C */ - - // stm32_i2c_putreg(priv, STM32_I2C_CR1_OFFSET, I2C_CR1_PE); + struct stm32_i2c_priv_s * priv; + unsigned clock_count; + unsigned stretch_count; + int ret = ERROR; + irqstate_t state; + ASSERT(dev); + /* Get I2C private structure */ + priv = ((struct stm32_i2c_inst_s *)dev)->priv; + /* Our caller must own a ref */ + ASSERT(priv->refs > 0); + /* Lock out other clients */ + stm32_i2c_sem_wait(dev); + /* De-init the port */ + stm32_i2c_deinit(priv); + /* If possible, use GPIO configuration to un-wedge the bus */ + if ((priv->config->scl_gpio != 0) && (priv->config->sda_gpio != 0)) + { + stm32_configgpio(priv->config->scl_gpio); + stm32_configgpio(priv->config->sda_gpio); + /* + * Clock the bus until any slaves currently driving it let it go. + */ + clock_count = 0; + while (!stm32_gpioread(priv->config->sda_gpio)) + { + /* Give up if we have tried too hard */ - int irqs; - - ASSERT(dev); - - /* Decrement refs and check for underflow */ - - if (((struct stm32_i2c_inst_s *)dev)->priv->refs == 0) - { - return ERROR; - } + if (clock_count++ > 1000) + { + goto out; + } - irqs = irqsave(); + /* + * Sniff to make sure that clock stretching has finished. + * + * If the bus never relaxes, the reset has failed. + */ - irqrestore(irqs); + stretch_count = 0; + while (!stm32_gpioread(priv->config->scl_gpio)) + { - /* Disable power and other HW resource (GPIO's) */ + /* Give up if we have tried too hard */ - //stm32_i2c_deinit( ((struct stm32_i2c_inst_s *)dev)->priv ); + if (stretch_count++ > 1000) + { + goto out; + } - /* Release unused resources */ + up_udelay(10); - //stm32_i2c_sem_destroy( (struct i2c_dev_s *)dev ); - - struct stm32_i2c_priv_s * priv = NULL; /* private data of device with multiple instances */ - struct stm32_i2c_inst_s * inst = NULL; /* device, single instance */ + } -#if STM32_PCLK1_FREQUENCY < 4000000 -# warning STM32_I2C_INIT: Peripheral clock must be at least 4 MHz to support 400 kHz operation. -#endif + /* Drive SCL low */ -#if STM32_PCLK1_FREQUENCY < 2000000 -# warning STM32_I2C_INIT: Peripheral clock must be at least 2 MHz to support 100 kHz operation. - return NULL; -#endif - - /* Get I2C private structure */ + stm32_gpiowrite(priv->config->scl_gpio, 0); + up_udelay(10); - priv = ((struct stm32_i2c_inst_s *)dev)->priv; - -// switch (port) -// { -// #ifdef CONFIG_STM32_I2C1 -// case 1: -// priv = (struct stm32_i2c_priv_s *)&stm32_i2c1_priv; -// break; -// #endif -// #ifdef CONFIG_STM32_I2C2 -// case 2: -// priv = (struct stm32_i2c_priv_s *)&stm32_i2c2_priv; -// break; -// #endif -// #ifdef CONFIG_STM32_I2C3 -// case 3: -// priv = (struct stm32_i2c_priv_s *)&stm32_i2c3_priv; -// break; -// #endif -// default: -// return NULL; -// } + /* Drive SCL high again */ - /* Allocate instance */ - inst = (struct stm32_i2c_inst_s *)dev; + stm32_gpiowrite(priv->config->scl_gpio, 1); + up_udelay(10); - /* Initialize instance */ + } - inst->ops = &stm32_i2c_ops; - inst->priv = priv; - inst->frequency = 100000; - inst->address = 0; - inst->flags = 0; + /* + * Generate a start followed by a stop to reset slave + * state machines. + */ + + stm32_gpiowrite(priv->config->sda_gpio, 0); + up_udelay(10); + stm32_gpiowrite(priv->config->scl_gpio, 0); + up_udelay(10); + stm32_gpiowrite(priv->config->scl_gpio, 1); + up_udelay(10); + stm32_gpiowrite(priv->config->sda_gpio, 1); + up_udelay(10); + + /* + * Revert the GPIO configuration. + */ + stm32_unconfiggpio(priv->config->sda_gpio); + stm32_unconfiggpio(priv->config->scl_gpio); - /* Init private data for the first time, increment refs count, - * power-up hardware and configure GPIOs. - */ - - irqs = irqsave(); - - if ((volatile int)priv->refs++ == 0) - { - stm32_i2c_sem_init( (struct i2c_dev_s *)inst ); - stm32_i2c_init( priv ); } - - irqrestore(irqs); - return OK; + + /* Re-init the port */ + + stm32_i2c_init(priv); + ret = OK; + +out: + + /* release the port for re-use by other clients */ + + stm32_i2c_sem_post(dev); + + return ret; } -#endif /* defined(CONFIG_STM32_I2C1) && defined(CONFIG_STM32_I2C2) */ +#endif /* defined(CONFIG_STM32_I2C1) || defined(CONFIG_STM32_I2C2) || defined(CONFIG_STM32_I2C3) */ diff --git a/nuttx/configs/px4fmu/include/board.h b/nuttx/configs/px4fmu/include/board.h index 29ad52c61c..60a295f8d8 100755 --- a/nuttx/configs/px4fmu/include/board.h +++ b/nuttx/configs/px4fmu/include/board.h @@ -265,15 +265,25 @@ /* * I2C + * + * The optional _GPIO configurations allow the I2C driver to manually + * reset the bus to clear stuck slaves. They match the pin configuration, + * but are normally-high GPIOs. */ -#define GPIO_I2C1_SCL GPIO_I2C1_SCL_2 -#define GPIO_I2C1_SDA GPIO_I2C1_SDA_2 - -#define GPIO_I2C2_SCL GPIO_I2C2_SCL_1 -#define GPIO_I2C2_SDA GPIO_I2C2_SDA_1 - -#define GPIO_I2C3_SCL GPIO_I2C3_SCL_1 -#define GPIO_I2C3_SDA GPIO_I2C3_SDA_1 +#define GPIO_I2C1_SCL GPIO_I2C1_SCL_2 +#define GPIO_I2C1_SDA GPIO_I2C1_SDA_2 +#define GPIO_I2C1_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN8) +#define GPIO_I2C1_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN9) + +#define GPIO_I2C2_SCL GPIO_I2C2_SCL_1 +#define GPIO_I2C2_SDA GPIO_I2C2_SDA_1 +#define GPIO_I2C2_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN10) +#define GPIO_I2C2_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN11) + +#define GPIO_I2C3_SCL GPIO_I2C3_SCL_1 +#define GPIO_I2C3_SDA GPIO_I2C3_SDA_1 +#define GPIO_I2C3_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN8) +#define GPIO_I2C3_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN9) /* * I2C busses diff --git a/nuttx/configs/px4fmu/src/drv_hmc5833l.c b/nuttx/configs/px4fmu/src/drv_hmc5833l.c index df6e26d4b6..d51d654f7b 100644 --- a/nuttx/configs/px4fmu/src/drv_hmc5833l.c +++ b/nuttx/configs/px4fmu/src/drv_hmc5833l.c @@ -323,13 +323,20 @@ hmc5883l_ioctl(struct file *filp, int cmd, unsigned long arg) return result; } +extern int up_i2creset(FAR struct i2c_dev_s * dev); + int hmc5883l_reset() { int ret; +#if 1 + ret = up_i2creset(hmc5883l_dev.i2c); + printf("HMC5883: BUS RESET %s\n", ret ? "FAIL" : "OK"); +#else printf("[hmc5883l drv] Resettet I2C2 BUS\n"); up_i2cuninitialize(hmc5883l_dev.i2c); hmc5883l_dev.i2c = up_i2cinitialize(2); I2C_SETFREQUENCY(hmc5883l_dev.i2c, 400000); +#endif return ret; } diff --git a/nuttx/include/nuttx/i2c.h b/nuttx/include/nuttx/i2c.h index b2238b1cf0..ef3d9a3883 100644 --- a/nuttx/include/nuttx/i2c.h +++ b/nuttx/include/nuttx/i2c.h @@ -325,6 +325,23 @@ EXTERN FAR struct i2c_dev_s *up_i2cinitialize(int port); EXTERN int up_i2cuninitialize(FAR struct i2c_dev_s * dev); +/**************************************************************************** + * Name: up_i2creset + * + * Description: + * Reset the port and the associated I2C bus. Useful when the bus or an + * attached slave has become wedged or unresponsive. + * + * Input Parameter: + * Device structure as returned by the up_i2cinitalize() + * + * Returned Value: + * OK on success, ERROR if the bus cannot be unwedged. + * + ****************************************************************************/ + +EXTERN int up_i2creset(FAR struct i2c_dev_s * dev); + #undef EXTERN #if defined(__cplusplus) }