From 07114f0a322613a1c3f2838df54916fdac4d8691 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 15 Jan 2013 23:17:12 +0100 Subject: Windows build fix attempt --- nuttx/configs/px4fmu/common/Make.defs | 4 ++-- nuttx/configs/px4io/common/Make.defs | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/nuttx/configs/px4fmu/common/Make.defs b/nuttx/configs/px4fmu/common/Make.defs index 52c00cd5a..756286ccb 100644 --- a/nuttx/configs/px4fmu/common/Make.defs +++ b/nuttx/configs/px4fmu/common/Make.defs @@ -79,7 +79,7 @@ LDSCRIPT = ld.script ifeq ($(WINTOOL),y) # Windows-native toolchains - DIRLINK = $(TOPDIR)/tools/winlink.sh + DIRLINK = $(TOPDIR)/tools/copydir.sh DIRUNLINK = $(TOPDIR)/tools/unlink.sh MKDEP = $(TOPDIR)/tools/mknulldeps.sh ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" @@ -88,7 +88,7 @@ ifeq ($(WINTOOL),y) else ifeq ($(PX4_WINTOOL),y) # Windows-native toolchains (MSYS) - DIRLINK = $(TOPDIR)/tools/winlink.sh + DIRLINK = $(TOPDIR)/tools/copydir.sh DIRUNLINK = $(TOPDIR)/tools/unlink.sh MKDEP = $(TOPDIR)/tools/mknulldeps.sh ARCHINCLUDES = -I. -isystem $(TOPDIR)/include diff --git a/nuttx/configs/px4io/common/Make.defs b/nuttx/configs/px4io/common/Make.defs index d6d52e3d2..4958f7092 100644 --- a/nuttx/configs/px4io/common/Make.defs +++ b/nuttx/configs/px4io/common/Make.defs @@ -72,7 +72,7 @@ LDSCRIPT = ld.script ifeq ($(WINTOOL),y) # Windows-native toolchains - DIRLINK = $(TOPDIR)/tools/winlink.sh + DIRLINK = $(TOPDIR)/tools/copydir.sh DIRUNLINK = $(TOPDIR)/tools/unlink.sh MKDEP = $(TOPDIR)/tools/mknulldeps.sh ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" @@ -81,7 +81,7 @@ ifeq ($(WINTOOL),y) else ifeq ($(PX4_WINTOOL),y) # Windows-native toolchains (MSYS) - DIRLINK = $(TOPDIR)/tools/winlink.sh + DIRLINK = $(TOPDIR)/tools/copydir.sh DIRUNLINK = $(TOPDIR)/tools/unlink.sh MKDEP = $(TOPDIR)/tools/mknulldeps.sh ARCHINCLUDES = -I. -isystem $(TOPDIR)/include -- cgit v1.2.3 From bc35bb23dd8cb035c080f8ef8b4cd7a30d5184c2 Mon Sep 17 00:00:00 2001 From: px4dev Date: Fri, 18 Jan 2013 00:43:57 -0800 Subject: HOTFIX: disable interrupt-driven I2C mode, configure pessimistic I2C timeout, correct handling of the NAK generation for I2C master reads. This looks like it addresses the recent I2C lockup issue, unfortunately it also increases CPU consumption by ~5% for the I2C sensor bus. --- apps/systemcmds/eeprom/24xxxx_mtd.c | 27 +++++++++++++++++++++++++++ apps/systemcmds/eeprom/eeprom.c | 13 +++++++++++++ nuttx/arch/arm/src/stm32/stm32_i2c.c | 10 ++++++---- nuttx/configs/px4fmu/nsh/defconfig | 18 ++++++++++-------- 4 files changed, 56 insertions(+), 12 deletions(-) diff --git a/apps/systemcmds/eeprom/24xxxx_mtd.c b/apps/systemcmds/eeprom/24xxxx_mtd.c index 781b01065..e34be44e3 100644 --- a/apps/systemcmds/eeprom/24xxxx_mtd.c +++ b/apps/systemcmds/eeprom/24xxxx_mtd.c @@ -163,6 +163,8 @@ static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks, FAR const uint8_t *buf); static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg); +void at24c_test(void); + /************************************************************************************ * Private Data ************************************************************************************/ @@ -218,6 +220,31 @@ static int at24c_erase(FAR struct mtd_dev_s *dev, off_t startblock, size_t nbloc return (int)nblocks; } +/************************************************************************************ + * Name: at24c_test + ************************************************************************************/ + +void at24c_test(void) +{ + uint8_t buf[CONFIG_AT24XX_MTD_BLOCKSIZE]; + unsigned count = 0; + unsigned errors = 0; + + for (count = 0; count < 10000; count++) { + ssize_t result = at24c_bread(&g_at24c.mtd, 0, 1, buf); + if (result == ERROR) { + if (errors++ > 2) { + vdbg("too many errors\n"); + return; + } + } else if (result != 1) { + vdbg("unexpected %u\n", result); + } + if ((count % 100) == 0) + vdbg("test %u errors %u\n", count, errors); + } +} + /************************************************************************************ * Name: at24c_bread ************************************************************************************/ diff --git a/apps/systemcmds/eeprom/eeprom.c b/apps/systemcmds/eeprom/eeprom.c index b4257cda9..49da51358 100644 --- a/apps/systemcmds/eeprom/eeprom.c +++ b/apps/systemcmds/eeprom/eeprom.c @@ -73,6 +73,7 @@ static void eeprom_erase(void); static void eeprom_ioctl(unsigned operation); static void eeprom_save(const char *name); static void eeprom_load(const char *name); +static void eeprom_test(void); static bool attached = false; static bool started = false; @@ -93,6 +94,9 @@ int eeprom_main(int argc, char *argv[]) if (!strcmp(argv[1], "erase")) eeprom_erase(); + if (!strcmp(argv[1], "test")) + eeprom_test(); + if (0) { /* these actually require a file on the filesystem... */ if (!strcmp(argv[1], "reformat")) @@ -250,3 +254,12 @@ eeprom_load(const char *name) exit(0); } + +extern void at24c_test(void); + +static void +eeprom_test(void) +{ + at24c_test(); + exit(0); +} diff --git a/nuttx/arch/arm/src/stm32/stm32_i2c.c b/nuttx/arch/arm/src/stm32/stm32_i2c.c index fd682cd5d..10e6cac65 100644 --- a/nuttx/arch/arm/src/stm32/stm32_i2c.c +++ b/nuttx/arch/arm/src/stm32/stm32_i2c.c @@ -1245,11 +1245,11 @@ static int stm32_i2c_isr(struct stm32_i2c_priv_s *priv) /* Disable acknowledge when last byte is to be received */ + priv->dcnt--; if (priv->dcnt == 1) { stm32_i2c_modifyreg(priv, STM32_I2C_CR1_OFFSET, I2C_CR1_ACK, 0); } - priv->dcnt--; #ifdef CONFIG_I2C_POLLED irqrestore(state); @@ -1985,7 +1985,6 @@ int up_i2creset(FAR struct i2c_dev_s * dev) uint32_t scl_gpio; uint32_t sda_gpio; int ret = ERROR; - irqstate_t state; ASSERT(dev); @@ -2010,6 +2009,9 @@ int up_i2creset(FAR struct i2c_dev_s * dev) scl_gpio = MKI2C_OUTPUT(priv->config->scl_pin); sda_gpio = MKI2C_OUTPUT(priv->config->sda_pin); + /* Let SDA go high */ + stm32_gpiowrite(sda_gpio, 1); + /* Clock the bus until any slaves currently driving it let it go. */ clock_count = 0; @@ -2017,7 +2019,7 @@ int up_i2creset(FAR struct i2c_dev_s * dev) { /* Give up if we have tried too hard */ - if (clock_count++ > 1000) + if (clock_count++ > 10) { goto out; } @@ -2032,7 +2034,7 @@ int up_i2creset(FAR struct i2c_dev_s * dev) { /* Give up if we have tried too hard */ - if (stretch_count++ > 1000) + if (stretch_count++ > 10) { goto out; } diff --git a/nuttx/configs/px4fmu/nsh/defconfig b/nuttx/configs/px4fmu/nsh/defconfig index 8a76f0e05..7bb4d1003 100755 --- a/nuttx/configs/px4fmu/nsh/defconfig +++ b/nuttx/configs/px4fmu/nsh/defconfig @@ -352,17 +352,19 @@ CONFIG_CAN2_BAUD=700000 # I2C configuration # CONFIG_I2C=y -#CONFIG_I2C_POLLED=y +CONFIG_I2C_POLLED=y CONFIG_I2C_TRANSFER=y CONFIG_I2C_TRACE=n CONFIG_I2C_RESET=y -# Allow 180 us per byte, a wide margin for the 400 KHz clock we're using -# e.g. 9.6 ms for an EEPROM page write, 0.9 ms for a MAG update -CONFIG_STM32_I2CTIMEOUS_PER_BYTE=200 -# Constant overhead for generating I2C start / stop conditions -CONFIG_STM32_I2CTIMEOUS_START_STOP=700 -# XXX this is bad and we want it gone -CONFIG_I2C_WRITEREAD=y + +# Dynamic timeout +#CONFIG_STM32_I2C_DYNTIMEO=y +#CONFIG_STM32_I2C_DYNTIMEO_STARTSTOP=500 +#CONFIG_STM32_I2C_DYNTIMEO_USECPERBYTE=200 + +# Fixed per-transaction timeout +CONFIG_STM32_I2CTIMEOSEC=0 +CONFIG_STM32_I2CTIMEOMS=10 # # General build options -- cgit v1.2.3 From c15093bb5591b24c590fc6f8278f26d5807ac882 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jan 2013 12:52:25 +0100 Subject: Bringing back old I2C code, just temporary workaround --- nuttx/arch/arm/src/stm32/stm32_i2c.c | 391 +++++++++++++++++------------------ 1 file changed, 188 insertions(+), 203 deletions(-) diff --git a/nuttx/arch/arm/src/stm32/stm32_i2c.c b/nuttx/arch/arm/src/stm32/stm32_i2c.c index 10e6cac65..a4b10b55c 100644 --- a/nuttx/arch/arm/src/stm32/stm32_i2c.c +++ b/nuttx/arch/arm/src/stm32/stm32_i2c.c @@ -107,23 +107,17 @@ #if !defined(CONFIG_STM32_I2CTIMEOSEC) && !defined(CONFIG_STM32_I2CTIMEOMS) # define CONFIG_STM32_I2CTIMEOSEC 0 -# define CONFIG_STM32_I2CTIMEOMS 500 /* Default is 500 milliseconds */ +# define CONFIG_STM32_I2CTIMEOMS 500 /* Default is 500 milliseconds */ #elif !defined(CONFIG_STM32_I2CTIMEOSEC) # define CONFIG_STM32_I2CTIMEOSEC 0 /* User provided milliseconds */ #elif !defined(CONFIG_STM32_I2CTIMEOMS) -# define CONFIG_STM32_I2CTIMEOMS 0 /* User provided seconds */ +# define CONFIG_STM32_I2CTIMEOMS 0 /* User provided seconds */ #endif /* Interrupt wait time timeout in system timer ticks */ -#ifndef CONFIG_STM32_I2CTIMEOTICKS -# define CONFIG_STM32_I2CTIMEOTICKS \ - (SEC2TICK(CONFIG_STM32_I2CTIMEOSEC) + MSEC2TICK(CONFIG_STM32_I2CTIMEOMS)) -#endif - -#ifndef CONFIG_STM32_I2C_DYNTIMEO_STARTSTOP -# define CONFIG_STM32_I2C_DYNTIMEO_STARTSTOP TICK2USEC(CONFIG_STM32_I2CTIMEOTICKS) -#endif +#define CONFIG_STM32_I2CTIMEOTICKS \ + (SEC2TICK(CONFIG_STM32_I2CTIMEOSEC) + MSEC2TICK(CONFIG_STM32_I2CTIMEOMS)) /* On the STM32F103ZE, there is an internal conflict between I2C1 and FSMC. In that * case, it is necessary to disable FSMC before each I2C1 access and re-enable FSMC @@ -135,18 +129,6 @@ # define I2C1_FSMC_CONFLICT #endif -/* Macros to convert a I2C pin to a GPIO output */ - -#if defined(CONFIG_STM32_STM32F10XX) -# define I2C_OUTPUT (GPIO_OUTPUT | GPIO_OUTPUT_SET | GPIO_CNF_OUTOD | \ - GPIO_MODE_50MHz) -#elif defined(CONFIG_STM32_STM32F20XX) || defined(CONFIG_STM32_STM32F40XX) -# define I2C_OUTPUT (GPIO_OUTPUT | GPIO_FLOAT | GPIO_OPENDRAIN |\ - GPIO_SPEED_50MHz | GPIO_OUTPUT_SET) -#endif - -#define MKI2C_OUTPUT(p) (((p) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | I2C_OUTPUT) - /* Debug ****************************************************************************/ /* CONFIG_DEBUG_I2C + CONFIG_DEBUG enables general I2C debug output. */ @@ -218,16 +200,18 @@ struct stm32_trace_s struct stm32_i2c_config_s { - uint32_t base; /* I2C base address */ - 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 sda_pin; /* GPIO configuration for SDA as SDA */ + uint32_t base; /* I2C base address */ #ifndef CONFIG_I2C_POLLED - int (*isr)(int, void *); /* Interrupt handler */ - uint32_t ev_irq; /* Event IRQ */ - uint32_t er_irq; /* Error IRQ */ + 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 */ @@ -235,31 +219,31 @@ struct stm32_i2c_config_s struct stm32_i2c_priv_s { const struct stm32_i2c_config_s *config; /* Port configuration */ - int refs; /* Referernce count */ - sem_t sem_excl; /* Mutual exclusion semaphore */ + int refs; /* Referernce count */ + sem_t sem_excl; /* Mutual exclusion semaphore */ #ifndef CONFIG_I2C_POLLED - sem_t sem_isr; /* Interrupt wait semaphore */ + sem_t sem_isr; /* Interrupt wait semaphore */ #endif - volatile uint8_t intstate; /* Interrupt handshake (see enum stm32_intstate_e) */ + volatile uint8_t intstate; /* Interrupt handshake (see enum stm32_intstate_e) */ - uint8_t msgc; /* Message count */ - struct i2c_msg_s *msgv; /* Message list */ - uint8_t *ptr; /* Current message buffer */ - int dcnt; /* Current message length */ - uint16_t flags; /* Current message flags */ + uint8_t msgc; /* Message count */ + struct i2c_msg_s *msgv; /* Message list */ + uint8_t *ptr; /* Current message buffer */ + int dcnt; /* Current message length */ + uint16_t flags; /* Current message flags */ /* I2C trace support */ #ifdef CONFIG_I2C_TRACE - int tndx; /* Trace array index */ - uint32_t start_time; /* Time when the trace was started */ + int tndx; /* Trace array index */ + uint32_t start_time; /* Time when the trace was started */ /* The actual trace data */ struct stm32_trace_s trace[CONFIG_I2C_NTRACE]; #endif - uint32_t status; /* End of transfer SR2|SR1 status */ + uint32_t status; /* End of transfer SR2|SR1 status */ }; /* I2C Device, Instance */ @@ -286,11 +270,8 @@ static inline void stm32_i2c_modifyreg(FAR struct stm32_i2c_priv_s *priv, uint8_t offset, uint16_t clearbits, uint16_t setbits); static inline void stm32_i2c_sem_wait(FAR struct i2c_dev_s *dev); -#ifdef CONFIG_STM32_I2C_DYNTIMEO -static useconds_t stm32_i2c_tousecs(int msgc, FAR struct i2c_msg_s *msgs); -#endif /* CONFIG_STM32_I2C_DYNTIMEO */ -static inline int stm32_i2c_sem_waitdone(FAR struct stm32_i2c_priv_s *priv); -static inline void stm32_i2c_sem_waitstop(FAR struct stm32_i2c_priv_s *priv); +static inline int stm32_i2c_sem_waitdone(FAR struct stm32_i2c_priv_s *priv, int timeout_us); +static inline void stm32_i2c_sem_waitstop(FAR struct stm32_i2c_priv_s *priv, int timeout_us); static inline void stm32_i2c_sem_post(FAR struct i2c_dev_s *dev); static inline void stm32_i2c_sem_init(FAR struct i2c_dev_s *dev); static inline void stm32_i2c_sem_destroy(FAR struct i2c_dev_s *dev); @@ -300,7 +281,7 @@ static void stm32_i2c_tracenew(FAR struct stm32_i2c_priv_s *priv, uint32_t statu static void stm32_i2c_traceevent(FAR struct stm32_i2c_priv_s *priv, enum stm32_trace_e event, uint32_t parm); static void stm32_i2c_tracedump(FAR struct stm32_i2c_priv_s *priv); -#endif /* CONFIG_I2C_TRACE */ +#endif static void stm32_i2c_setclock(FAR struct stm32_i2c_priv_s *priv, uint32_t frequency); static inline void stm32_i2c_sendstart(FAR struct stm32_i2c_priv_s *priv); @@ -310,7 +291,7 @@ static inline uint32_t stm32_i2c_getstatus(FAR struct stm32_i2c_priv_s *priv); #ifdef I2C1_FSMC_CONFLICT static inline uint32_t stm32_i2c_disablefsmc(FAR struct stm32_i2c_priv_s *priv); static inline void stm32_i2c_enablefsmc(uint32_t ahbenr); -#endif /* I2C1_FSMC_CONFLICT */ +#endif static int stm32_i2c_isr(struct stm32_i2c_priv_s * priv); #ifndef CONFIG_I2C_POLLED #ifdef CONFIG_STM32_I2C1 @@ -348,18 +329,27 @@ static int stm32_i2c_transfer(FAR struct i2c_dev_s *dev, FAR struct i2c_msg_s *m ************************************************************************************/ #ifdef CONFIG_STM32_I2C1 +# 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, -#ifndef CONFIG_I2C_POLLED - .isr = stm32_i2c1_isr, + .sda_gpio = GPIO_I2C1_SDA_GPIO, .ev_irq = STM32_IRQ_I2C1EV, .er_irq = STM32_IRQ_I2C1ER -#endif }; struct stm32_i2c_priv_s stm32_i2c1_priv = @@ -377,18 +367,27 @@ struct stm32_i2c_priv_s stm32_i2c1_priv = #endif #ifdef CONFIG_STM32_I2C2 +# 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, -#ifndef CONFIG_I2C_POLLED - .isr = stm32_i2c2_isr, + .sda_gpio = GPIO_I2C2_SDA_GPIO, .ev_irq = STM32_IRQ_I2C2EV, .er_irq = STM32_IRQ_I2C2ER -#endif }; struct stm32_i2c_priv_s stm32_i2c2_priv = @@ -406,18 +405,27 @@ struct stm32_i2c_priv_s stm32_i2c2_priv = #endif #ifdef CONFIG_STM32_I2C3 +# 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, -#ifndef CONFIG_I2C_POLLED - .isr = stm32_i2c3_isr, + .sda_gpio = GPIO_I2C3_SDA_GPIO, .ev_irq = STM32_IRQ_I2C3EV, .er_irq = STM32_IRQ_I2C3ER -#endif }; struct stm32_i2c_priv_s stm32_i2c3_priv = @@ -517,35 +525,6 @@ static inline void stm32_i2c_sem_wait(FAR struct i2c_dev_s *dev) } } -/************************************************************************************ - * Name: stm32_i2c_tousecs - * - * Description: - * Return a micro-second delay based on the number of bytes left to be processed. - * - ************************************************************************************/ - -#ifdef CONFIG_STM32_I2C_DYNTIMEO -static useconds_t stm32_i2c_tousecs(int msgc, FAR struct i2c_msg_s *msgs) -{ - size_t bytecount = 0; - int i; - - /* Count the number of bytes left to process */ - - for (i = 0; i < msgc; i++) - { - bytecount += msgs[i].length; - } - - /* Then return a number of microseconds based on a user provided scaling - * factor. - */ - - return (useconds_t)(CONFIG_STM32_I2C_DYNTIMEO_USECPERBYTE * bytecount); -} -#endif - /************************************************************************************ * Name: stm32_i2c_sem_waitdone * @@ -555,7 +534,7 @@ static useconds_t stm32_i2c_tousecs(int msgc, FAR struct i2c_msg_s *msgs) ************************************************************************************/ #ifndef CONFIG_I2C_POLLED -static inline int stm32_i2c_sem_waitdone(FAR struct stm32_i2c_priv_s *priv) +static inline int stm32_i2c_sem_waitdone(FAR struct stm32_i2c_priv_s *priv, int timeout_us) { struct timespec abstime; irqstate_t flags; @@ -587,24 +566,31 @@ static inline int stm32_i2c_sem_waitdone(FAR struct stm32_i2c_priv_s *priv) #if CONFIG_STM32_I2CTIMEOSEC > 0 abstime.tv_sec += CONFIG_STM32_I2CTIMEOSEC; #endif +#if CONFIG_STM32_I2CTIMEOUS_PER_BYTE > 0 - /* Add a value proportional to the number of bytes in the transfer */ - -#ifdef CONFIG_STM32_I2C_DYNTIMEO - abstime.tv_nsec += 1000 * stm32_i2c_tousecs(priv->msgc, priv->msgv); - if (abstime.tv_nsec > 1000 * 1000 * 1000) + /* Count the number of bytes left to process */ + int i; + int bytecount = 0; + for (i = 0; i < priv->msgc; i++) { - abstime.tv_sec++; - abstime.tv_nsec -= 1000 * 1000 * 1000; + bytecount += priv->msgv[i].length; } -#elif CONFIG_STM32_I2CTIMEOMS > 0 - abstime.tv_nsec += CONFIG_STM32_I2CTIMEOMS * 1000 * 1000; + abstime.tv_nsec += (CONFIG_STM32_I2CTIMEOUS_PER_BYTE * bytecount) * 1000; if (abstime.tv_nsec > 1000 * 1000 * 1000) { abstime.tv_sec++; abstime.tv_nsec -= 1000 * 1000 * 1000; } +#else + #if CONFIG_STM32_I2CTIMEOMS > 0 + abstime.tv_nsec += CONFIG_STM32_I2CTIMEOMS * 1000 * 1000; + if (abstime.tv_nsec > 1000 * 1000 * 1000) + { + abstime.tv_sec++; + abstime.tv_nsec -= 1000 * 1000 * 1000; + } + #endif #endif /* Wait until either the transfer is complete or the timeout expires */ @@ -638,21 +624,12 @@ static inline int stm32_i2c_sem_waitdone(FAR struct stm32_i2c_priv_s *priv) return ret; } #else -static inline int stm32_i2c_sem_waitdone(FAR struct stm32_i2c_priv_s *priv) +static inline int stm32_i2c_sem_waitdone(FAR struct stm32_i2c_priv_s *priv, int timeout_us) { - uint32_t timeout; uint32_t start; uint32_t elapsed; int ret; - /* Get the timeout value */ - -#ifdef CONFIG_STM32_I2C_DYNTIMEO - timeout = USEC2TICK(stm32_i2c_tousecs(priv->msgc, priv->msgv)); -#else - timeout = CONFIG_STM32_I2CTIMEOTICKS; -#endif - /* Signal the interrupt handler that we are waiting. NOTE: Interrupts * are currently disabled but will be temporarily re-enabled below when * sem_timedwait() sleeps. @@ -675,11 +652,10 @@ static inline int stm32_i2c_sem_waitdone(FAR struct stm32_i2c_priv_s *priv) } /* Loop until the transfer is complete. */ - - while (priv->intstate != INTSTATE_DONE && elapsed < timeout); + while (priv->intstate != INTSTATE_DONE && elapsed < USEC2TICK(timeout_us)); i2cvdbg("intstate: %d elapsed: %d threshold: %d status: %08x\n", - priv->intstate, elapsed, timeout, priv->status); + priv->intstate, elapsed, USEC2TICK(timeout_us), priv->status); /* Set the interrupt state back to IDLE */ @@ -697,22 +673,13 @@ static inline int stm32_i2c_sem_waitdone(FAR struct stm32_i2c_priv_s *priv) * ************************************************************************************/ -static inline void stm32_i2c_sem_waitstop(FAR struct stm32_i2c_priv_s *priv) +static inline void stm32_i2c_sem_waitstop(FAR struct stm32_i2c_priv_s *priv, int timeout_us) { uint32_t start; uint32_t elapsed; - uint32_t timeout; uint32_t cr1; uint32_t sr1; - /* Select a timeout */ - -#ifdef CONFIG_STM32_I2C_DYNTIMEO - timeout = USEC2TICK(CONFIG_STM32_I2C_DYNTIMEO_STARTSTOP); -#else - timeout = CONFIG_STM32_I2CTIMEOTICKS; -#endif - /* Wait as stop might still be in progress; but stop might also * be set because of a timeout error: "The [STOP] bit is set and * cleared by software, cleared by hardware when a Stop condition is @@ -745,7 +712,7 @@ static inline void stm32_i2c_sem_waitstop(FAR struct stm32_i2c_priv_s *priv) /* Loop until the stop is complete or a timeout occurs. */ - while (elapsed < timeout); + while (elapsed < USEC2TICK(timeout_us)); /* If we get here then a timeout occurred with the STOP condition * still pending. @@ -965,7 +932,7 @@ static void stm32_i2c_setclock(FAR struct stm32_i2c_priv_s *priv, uint32_t frequ { /* Fast mode speed calculation with Tlow/Thigh = 16/9 */ -#ifdef CONFIG_STM32_I2C_DUTY16_9 +#ifdef CONFIG_I2C_DUTY16_9 speed = (uint16_t)(STM32_PCLK1_FREQUENCY / (frequency * 25)); /* Set DUTY and fast speed bits */ @@ -1104,7 +1071,7 @@ static inline uint32_t stm32_i2c_disablefsmc(FAR struct stm32_i2c_priv_s *priv) /* Is this I2C1 */ -#if defined(CONFIG_STM32_I2C2) || defined(CONFIG_STM32_I2C3) +#ifdef CONFIG_STM32_I2C2 if (priv->config->base == STM32_I2C1_BASE) #endif { @@ -1231,29 +1198,26 @@ static int stm32_i2c_isr(struct stm32_i2c_priv_s *priv) { stm32_i2c_traceevent(priv, I2CEVENT_RCVBYTE, priv->dcnt); - /* No interrupts or context switches may occur in the following - * sequence. Otherwise, additional bytes may be sent by the - * device. - */ - #ifdef CONFIG_I2C_POLLED irqstate_t state = irqsave(); #endif + /* Receive a byte */ *priv->ptr++ = stm32_i2c_getreg(priv, STM32_I2C_DR_OFFSET); /* Disable acknowledge when last byte is to be received */ - priv->dcnt--; if (priv->dcnt == 1) { stm32_i2c_modifyreg(priv, STM32_I2C_CR1_OFFSET, I2C_CR1_ACK, 0); } + priv->dcnt--; #ifdef CONFIG_I2C_POLLED irqrestore(state); #endif + } } @@ -1444,6 +1408,7 @@ static int stm32_i2c_init(FAR struct stm32_i2c_priv_s *priv) /* Enable power and reset the peripheral */ modifyreg32(STM32_RCC_APB1ENR, 0, priv->config->clk_bit); + modifyreg32(STM32_RCC_APB1RSTR, 0, priv->config->reset_bit); modifyreg32(STM32_RCC_APB1RSTR, priv->config->reset_bit, 0); @@ -1463,10 +1428,10 @@ static int stm32_i2c_init(FAR struct stm32_i2c_priv_s *priv) /* Attach ISRs */ #ifndef CONFIG_I2C_POLLED - 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); + 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 /* Set peripheral frequency, where it must be at least 2 MHz for 100 kHz @@ -1496,23 +1461,17 @@ static int stm32_i2c_deinit(FAR struct stm32_i2c_priv_s *priv) stm32_i2c_putreg(priv, STM32_I2C_CR1_OFFSET, 0); - /* Unconfigure GPIO pins */ - stm32_unconfiggpio(priv->config->scl_pin); stm32_unconfiggpio(priv->config->sda_pin); - /* Disable and detach interrupts */ - #ifndef CONFIG_I2C_POLLED 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 - - /* Disable clocking */ - modifyreg32(STM32_RCC_APB1ENR, priv->config->clk_bit, 0); + return OK; } @@ -1574,14 +1533,14 @@ static int stm32_i2c_process(FAR struct i2c_dev_s *dev, FAR struct i2c_msg_s *ms struct stm32_i2c_inst_s *inst = (struct stm32_i2c_inst_s *)dev; FAR struct stm32_i2c_priv_s *priv = inst->priv; uint32_t status = 0; - uint32_t ahbenr; + //uint32_t ahbenr; int errval = 0; ASSERT(count); /* Disable FSMC that shares a pin with I2C1 (LBAR) */ - ahbenr = stm32_i2c_disablefsmc(priv); + (void)stm32_i2c_disablefsmc(priv); /* Wait for any STOP in progress. NOTE: If we have to disable the FSMC * then we cannot do this at the top of the loop, unfortunately. The STOP @@ -1589,7 +1548,11 @@ static int stm32_i2c_process(FAR struct i2c_dev_s *dev, FAR struct i2c_msg_s *ms */ #ifndef I2C1_FSMC_CONFLICT - stm32_i2c_sem_waitstop(priv); + #if CONFIG_STM32_I2CTIMEOUS_START_STOP > 0 + stm32_i2c_sem_waitstop(priv, CONFIG_STM32_I2CTIMEOUS_START_STOP); + #else + stm32_i2c_sem_waitstop(priv, CONFIG_STM32_I2CTIMEOMS + CONFIG_STM32_I2CTIMEOSEC * 1000000); + #endif #endif /* Clear any pending error interrupts */ @@ -1610,6 +1573,22 @@ static int stm32_i2c_process(FAR struct i2c_dev_s *dev, FAR struct i2c_msg_s *ms priv->msgv = msgs; priv->msgc = count; + /* Calculate timeout values */ + int timeout_us = 0; + #if CONFIG_STM32_I2CTIMEOUS_PER_BYTE > 0 + /* Count the number of bytes left to process */ + int i; + int bytecount = 10; + for (i = 0; i < count; i++) + { + bytecount += msgs[i].length; + } + timeout_us = CONFIG_STM32_I2CTIMEOUS_PER_BYTE * bytecount; + //i2cvdbg("i2c wait: %d\n", timeout_us); + #else + timeout_us = CONFIG_STM32_I2CTIMEOMS + CONFIG_STM32_I2CTIMEOSEC * 1000000; + #endif + /* Reset I2C trace logic */ stm32_i2c_tracereset(priv); @@ -1629,7 +1608,7 @@ static int stm32_i2c_process(FAR struct i2c_dev_s *dev, FAR struct i2c_msg_s *ms * the BUSY flag. */ - if (stm32_i2c_sem_waitdone(priv) < 0) + if (stm32_i2c_sem_waitdone(priv, timeout_us) < 0) { status = stm32_i2c_getstatus(priv); errval = ETIMEDOUT; @@ -1644,9 +1623,7 @@ static int stm32_i2c_process(FAR struct i2c_dev_s *dev, FAR struct i2c_msg_s *ms */ stm32_i2c_clrstart(priv); - - /* Clear busy flag in case of timeout */ - + // XXX also clear busy flag in case of timeout status = priv->status & 0xffff; } else @@ -1976,15 +1953,13 @@ int up_i2cuninitialize(FAR struct i2c_dev_s * dev) * ************************************************************************************/ -#ifdef CONFIG_I2C_RESET int up_i2creset(FAR struct i2c_dev_s * dev) { struct stm32_i2c_priv_s * priv; - unsigned int clock_count; - unsigned int stretch_count; - uint32_t scl_gpio; - uint32_t sda_gpio; + unsigned clock_count; + unsigned stretch_count; int ret = ERROR; + irqstate_t state; ASSERT(dev); @@ -2004,72 +1979,82 @@ int up_i2creset(FAR struct i2c_dev_s * dev) stm32_i2c_deinit(priv); - /* Use GPIO configuration to un-wedge the bus */ - - scl_gpio = MKI2C_OUTPUT(priv->config->scl_pin); - sda_gpio = MKI2C_OUTPUT(priv->config->sda_pin); - - /* Let SDA go high */ - stm32_gpiowrite(sda_gpio, 1); - - /* Clock the bus until any slaves currently driving it let it go. */ + /* If possible, use GPIO configuration to un-wedge the bus */ - clock_count = 0; - while (!stm32_gpioread(sda_gpio)) + if ((priv->config->scl_gpio != 0) && (priv->config->sda_gpio != 0)) { - /* Give up if we have tried too hard */ + stm32_configgpio(priv->config->scl_gpio); + stm32_configgpio(priv->config->sda_gpio); - if (clock_count++ > 10) - { - goto out; - } - - /* Sniff to make sure that clock stretching has finished. - * - * If the bus never relaxes, the reset has failed. + /* + * Clock the bus until any slaves currently driving it let it go. */ - stretch_count = 0; - while (!stm32_gpioread(scl_gpio)) - { + clock_count = 0; + while (!stm32_gpioread(priv->config->sda_gpio)) + { + /* Give up if we have tried too hard */ - if (stretch_count++ > 10) + if (clock_count++ > 1000) { goto out; } - up_udelay(10); - } + /* + * Sniff to make sure that clock stretching has finished. + * + * If the bus never relaxes, the reset has failed. + */ - /* Drive SCL low */ + stretch_count = 0; + while (!stm32_gpioread(priv->config->scl_gpio)) + { - stm32_gpiowrite(scl_gpio, 0); - up_udelay(10); + /* Give up if we have tried too hard */ - /* Drive SCL high again */ + if (stretch_count++ > 1000) + { + goto out; + } - stm32_gpiowrite(scl_gpio, 1); - up_udelay(10); - } + up_udelay(10); - /* Generate a start followed by a stop to reset slave - * state machines. - */ + } + + /* Drive SCL low */ + + stm32_gpiowrite(priv->config->scl_gpio, 0); + up_udelay(10); - stm32_gpiowrite(sda_gpio, 0); - up_udelay(10); - stm32_gpiowrite(scl_gpio, 0); - up_udelay(10); - stm32_gpiowrite(scl_gpio, 1); - up_udelay(10); - stm32_gpiowrite(sda_gpio, 1); - up_udelay(10); + /* Drive SCL high again */ - /* Revert the GPIO configuration. */ - - stm32_unconfiggpio(sda_gpio); - stm32_unconfiggpio(scl_gpio); + stm32_gpiowrite(priv->config->scl_gpio, 1); + up_udelay(10); + + } + + /* + * 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); + + } /* Re-init the port */ @@ -2078,11 +2063,11 @@ int up_i2creset(FAR struct i2c_dev_s * dev) out: - /* Release the port for re-use by other clients */ + /* release the port for re-use by other clients */ stm32_i2c_sem_post(dev); + return ret; } -#endif /* CONFIG_I2C_RESET */ -#endif /* CONFIG_STM32_I2C1 || CONFIG_STM32_I2C2 || CONFIG_STM32_I2C3 */ +#endif /* defined(CONFIG_STM32_I2C1) || defined(CONFIG_STM32_I2C2) || defined(CONFIG_STM32_I2C3) */ -- cgit v1.2.3 From a10ff0fe1cd7525fdcf9fe9b11ff9e42ea0e2926 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Jan 2013 14:52:33 +0100 Subject: First round of testing successful - back to I2C code from Dec / Nov 2012 --- nuttx/configs/px4fmu/nsh/defconfig | 32 ++++++++++++++++++++++++++++++-- 1 file changed, 30 insertions(+), 2 deletions(-) diff --git a/nuttx/configs/px4fmu/nsh/defconfig b/nuttx/configs/px4fmu/nsh/defconfig index 7bb4d1003..0b27b552e 100755 --- a/nuttx/configs/px4fmu/nsh/defconfig +++ b/nuttx/configs/px4fmu/nsh/defconfig @@ -348,6 +348,16 @@ CONFIG_CAN_LOOPBACK=n CONFIG_CAN1_BAUD=700000 CONFIG_CAN2_BAUD=700000 + +# XXX remove after integration testing +# Allow 180 us per byte, a wide margin for the 400 KHz clock we're using +# e.g. 9.6 ms for an EEPROM page write, 0.9 ms for a MAG update +CONFIG_STM32_I2CTIMEOUS_PER_BYTE=200 +# Constant overhead for generating I2C start / stop conditions +CONFIG_STM32_I2CTIMEOUS_START_STOP=700 +# XXX this is bad and we want it gone +CONFIG_I2C_WRITEREAD=y + # # I2C configuration # @@ -357,14 +367,32 @@ CONFIG_I2C_TRANSFER=y CONFIG_I2C_TRACE=n CONFIG_I2C_RESET=y + + +# XXX re-enable after integration testing + +# +# I2C configuration +# +#CONFIG_I2C=y +#CONFIG_I2C_POLLED=y +#CONFIG_I2C_TRANSFER=y +#CONFIG_I2C_TRACE=n +#CONFIG_I2C_RESET=y + # Dynamic timeout #CONFIG_STM32_I2C_DYNTIMEO=y #CONFIG_STM32_I2C_DYNTIMEO_STARTSTOP=500 #CONFIG_STM32_I2C_DYNTIMEO_USECPERBYTE=200 # Fixed per-transaction timeout -CONFIG_STM32_I2CTIMEOSEC=0 -CONFIG_STM32_I2CTIMEOMS=10 +#CONFIG_STM32_I2CTIMEOSEC=0 +#CONFIG_STM32_I2CTIMEOMS=10 + + + + + # # General build options -- cgit v1.2.3