From 024c4051ba207d5ad1b68753848bbf80599b3877 Mon Sep 17 00:00:00 2001 From: patacongo Date: Sun, 11 Sep 2011 14:55:31 +0000 Subject: Correct error in clock_gettime(); fix wait for STOP in I2C stm32 driver git-svn-id: svn://svn.code.sf.net/p/nuttx/code/trunk@3948 42af7a65-404d-4744-a932-0658087f49c3 --- nuttx/ChangeLog | 4 + nuttx/arch/arm/src/stm32/stm32_i2c.c | 80 ++++++++++++++----- nuttx/configs/stm3210e-eval/README.txt | 27 +++++++ nuttx/drivers/sensors/lm75.c | 142 ++++++++++++++++++++++++--------- nuttx/include/nuttx/sensors/lm75.h | 6 +- nuttx/sched/clock_gettime.c | 2 +- 6 files changed, 201 insertions(+), 60 deletions(-) diff --git a/nuttx/ChangeLog b/nuttx/ChangeLog index 5728cecca..57eaf7a99 100644 --- a/nuttx/ChangeLog +++ b/nuttx/ChangeLog @@ -2070,3 +2070,7 @@ temperature sensor driver. * configs/stm3210e-eval/src/up_lm75.c: Add support for the LM-75 on the STMicro STM3210E-EVAL board. + * sched/clock_gettime.c: Correct an error in the tv_nsec calculation + that happens only config CONFIG_RTC is enabled. + * arch/arm/src/stm32/stm32_i2c.c: Correct some bugs related to waiting + to the I2C STOP condition to be cleared. diff --git a/nuttx/arch/arm/src/stm32/stm32_i2c.c b/nuttx/arch/arm/src/stm32/stm32_i2c.c index 15f22e602..dd551bf3b 100644 --- a/nuttx/arch/arm/src/stm32/stm32_i2c.c +++ b/nuttx/arch/arm/src/stm32/stm32_i2c.c @@ -429,12 +429,25 @@ static inline void stm32_i2c_sendstart(FAR struct stm32_i2c_priv_s *priv) static inline void stm32_i2c_clrstart(FAR struct stm32_i2c_priv_s *priv) { - /* "This [START] bit is set and cleared by software and cleared by hardware + /* "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." + * + * "The [STOP] bit is set and cleared by software, cleared by hardware + * when a Stop condition is detected, set by hardware when a timeout + * error is detected. + * + * "This [START] bit is set and cleared by software and cleared by hardware * when start is sent or PE=0." The bit must be cleared by software if the * START is never sent. + * + * "This [PEC] bit is set and cleared by software, and cleared by hardware + * when PEC is transferred or by a START or Stop condition or when PE=0." */ - stm32_i2c_modifyreg(priv, STM32_I2C_CR1_OFFSET, I2C_CR1_START, 0); + stm32_i2c_modifyreg(priv, STM32_I2C_CR1_OFFSET, + I2C_CR1_START|I2C_CR1_STOP|I2C_CR1_PEC, 0); } static inline void stm32_i2c_sendstop(FAR struct stm32_i2c_priv_s *priv) @@ -871,6 +884,7 @@ int stm32_i2c_process(FAR struct i2c_dev_s *dev, FAR struct i2c_msg_s *msgs, int struct stm32_i2c_inst_s *inst = (struct stm32_i2c_inst_s *)dev; uint32_t status = 0; uint32_t ahbenr; + uint16_t regval; int status_errno = 0; ASSERT(count); @@ -879,15 +893,43 @@ int stm32_i2c_process(FAR struct i2c_dev_s *dev, FAR struct i2c_msg_s *msgs, int ahbenr = stm32_i2c_disablefsmc(inst->priv); - /* Wait as stop might still be in progress - * - * \todo GET RID OF THIS PERFORMANCE LOSS and for() loop + /* 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 + * detected, set by hardware when a timeout error is detected." */ - - for (; stm32_i2c_getreg(inst->priv, STM32_I2C_CR1_OFFSET) & I2C_CR1_STOP; ) + + for (;;) { - up_waste(); + /* Check for STOP condition */ + + regval = stm32_i2c_getreg(inst->priv, STM32_I2C_CR1_OFFSET); + if ((regval & I2C_CR1_STOP) == 0) + { + break; + } + + /* Check for timeout error */ + + regval = stm32_i2c_getreg(inst->priv, STM32_I2C_SR1_OFFSET); + if ((regval & I2C_SR1_TIMEOUT) != 0) + { + break; + } } + + /* Clear any pending error interrupts */ + + stm32_i2c_putreg(inst->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(inst->priv); /* Old transfers are done */ @@ -898,10 +940,6 @@ int stm32_i2c_process(FAR struct i2c_dev_s *dev, FAR struct i2c_msg_s *msgs, int stm32_i2c_setclock(inst->priv, inst->frequency); - /* Clear any pending error interrupts */ - - stm32_i2c_putreg(inst->priv, STM32_I2C_SR1_OFFSET, 0); - /* Trigger start condition, then the process moves into the ISR. I2C * interrupts will be enabled within stm32_i2c_waitisr(). */ @@ -917,10 +955,10 @@ int stm32_i2c_process(FAR struct i2c_dev_s *dev, FAR struct i2c_msg_s *msgs, int status = stm32_i2c_getstatus(inst->priv); status_errno = ETIMEDOUT; - /* " 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." + /* "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." */ stm32_i2c_clrstart(inst->priv); @@ -936,6 +974,8 @@ int stm32_i2c_process(FAR struct i2c_dev_s *dev, FAR struct i2c_msg_s *msgs, int if ((status & I2C_SR1_ERRORMASK) != 0) { + /* I2C_SR1_ERRORMASK is the 'OR' of the following individual bits: */ + if (status & I2C_SR1_BERR) { /* Bus Error */ @@ -975,7 +1015,7 @@ int stm32_i2c_process(FAR struct i2c_dev_s *dev, FAR struct i2c_msg_s *msgs, int /* This is not an error and should never happen since SMBus is not enabled */ - else if (status & I2C_SR1_SMBALERT) + else /* if (status & I2C_SR1_SMBALERT) */ { /* SMBus alert is an optional signal with an interrupt line for devices * that want to trade their ability to master for a pin. @@ -987,9 +1027,11 @@ int stm32_i2c_process(FAR struct i2c_dev_s *dev, FAR struct i2c_msg_s *msgs, int /* This is not an error, but should not happen. The BUSY signal can hang, * however, if there are unhealthy devices on the bus that need to be reset. + * NOTE: We will only see this buy indication if stm32_i2c_sem_waitisr() + * fails above; Otherwise it is cleared. */ - else if (status & (I2C_SR2_BUSY << 16)) + else if ((status & (I2C_SR2_BUSY << 16)) != 0) { /* I2C Bus is for some reason busy */ @@ -1016,7 +1058,7 @@ int stm32_i2c_write(FAR struct i2c_dev_s *dev, const uint8_t *buffer, int buflen .buffer = (uint8_t *)buffer, .length = buflen }; - + return stm32_i2c_process(dev, &msgv, 1); } diff --git a/nuttx/configs/stm3210e-eval/README.txt b/nuttx/configs/stm3210e-eval/README.txt index 772020e96..b63f7c5f2 100755 --- a/nuttx/configs/stm3210e-eval/README.txt +++ b/nuttx/configs/stm3210e-eval/README.txt @@ -13,6 +13,7 @@ Contents - NuttX buildroot Toolchain - DFU - LEDs + - Temperature Sensor - STM3210E-EVAL-specific Configuration Options - Configurations @@ -237,6 +238,32 @@ They are encoded as follows: on a small proportion of the time. *** LED2 may also flicker normally if signals are processed. +Temperature Sensor +================== + +Support for the on-board LM-75 temperature sensor is available. This supported +has been verified, but has not been included in any of the available the +configurations. To set up the temperature sensor, add the following to the +NuttX configuration file + + CONFIG_I2C=y + CONFIG_I2C_LM75=y + +Then you can implement logic like the following to use the temperature sensor: + + #include + #include + + ret = stm32_lm75initialize("/dev/temp"); /* Register the temperature sensor */ + fd = open("/dev/temp", O_RDONLY); /* Open the temperature sensor device */ + ret = ioctl(fd, SNIOC_FAHRENHEIT, 0); /* Select Fahrenheit */ + bytesread = read(fd, buffer, 8*sizeof(b16_t)); /* Read temperature samples */ + +More complex temperature sensor operations are also available. See the IOCTAL +commands enumerated in include/nuttx/sensors/lm75.h. Also read the descriptions +of the stm32_lm75initialize() and stm32_lm75attach() interfaces in the +arch/board/board.h file (sames as configs/stm3210e-eval/include/board.h). + STM3210E-EVAL-specific Configuration Options ============================================ diff --git a/nuttx/drivers/sensors/lm75.c b/nuttx/drivers/sensors/lm75.c index 600f9aab3..397cb9582 100644 --- a/nuttx/drivers/sensors/lm75.c +++ b/nuttx/drivers/sensors/lm75.c @@ -43,6 +43,7 @@ #include #include #include +#include #include #include @@ -57,7 +58,19 @@ /* Centigrade to Fahrenheit conversion: F = 9*C/5 + 32 */ #define B16_9DIV5 (9 * 65536 / 5) -#define B16_32 (32 * 65536); +#define B16_32 (32 * 65536) + +/* Debug for this file only */ + +#ifdef CONFIG_DEBUG_LM75 +# define lm75dbg dbg +#else +# ifdef CONFIG_CPP_HAVE_VARARGS +# define lm75dbg(x...) +# else +# define lm75dbg (void) +# endif +#endif /**************************************************************************** * Private @@ -75,14 +88,12 @@ struct lm75_dev_s ****************************************************************************/ /* I2C Helpers */ -static int lm75_readb8(FAR struct lm75_dev_s *priv, uint8_t regaddr, - FAR b8_t *regvalue); -#if 0 // Not used -static int lm75_writeb8(FAR struct lm75_dev_s *priv, uint8_t regaddr, - b8_t regval); -#endif -static int lm75_readtemp(FAR struct lm75_dev_s *priv, b16_t *temp); -static int lm75_readconf(FAR struct lm75_dev_s *priv, uint8_t *conf); +static int lm75_readb16(FAR struct lm75_dev_s *priv, uint8_t regaddr, + FAR b16_t *regvalue); +static int lm75_writeb16(FAR struct lm75_dev_s *priv, uint8_t regaddr, + b16_t regval); +static int lm75_readtemp(FAR struct lm75_dev_s *priv, FAR b16_t *temp); +static int lm75_readconf(FAR struct lm75_dev_s *priv, FAR uint8_t *conf); static int lm75_writeconf(FAR struct lm75_dev_s *priv, uint8_t conf); /* Character driver methods */ @@ -114,15 +125,15 @@ static const struct file_operations lm75_fops = * Private Functions ****************************************************************************/ /**************************************************************************** - * Name: lm75_readb8 + * Name: lm75_readb16 * * Description: * Read a 16-bit register (LM75_TEMP_REG, LM75_THYS_REG, or LM75_TOS_REG) * ****************************************************************************/ -static int lm75_readb8(FAR struct lm75_dev_s *priv, uint8_t regaddr, - FAR b8_t *regvalue) +static int lm75_readb16(FAR struct lm75_dev_s *priv, uint8_t regaddr, + FAR b16_t *regvalue) { uint8_t buffer[2]; int ret; @@ -133,6 +144,7 @@ static int lm75_readb8(FAR struct lm75_dev_s *priv, uint8_t regaddr, ret = I2C_WRITE(priv->i2c, ®addr, 1); if (ret < 0) { + lm75dbg("I2C_WRITE failed: %d\n", ret); return ret; } @@ -141,43 +153,49 @@ static int lm75_readb8(FAR struct lm75_dev_s *priv, uint8_t regaddr, ret = I2C_READ(priv->i2c, buffer, 2); if (ret < 0) { + lm75dbg("I2C_READ failed: %d\n", ret); return ret; } - /* Data format is: TTTTTTTT Txxxxxxx where TTTTTTTTT is a nine-bit - * temperature value with LSB = 0.5 degrees centigrade. So the raw - * data is b8_t + /* Data format is: TTTTTTTT Txxxxxxx where TTTTTTTTT is a nine-bit, + * signed temperature value with LSB = 0.5 degrees centigrade. So the + * raw data is b8_t */ - *regvalue = (b8_t)buffer[0] << 8 | (b8_t)buffer[1]; + *regvalue = b8tob16((b8_t)buffer[0] << 8 | (b8_t)buffer[1]); + lm75dbg("addr: %02x value: %08x ret: %d\n", regaddr, *regvalue, ret); return OK; } /**************************************************************************** - * Name: lm75_writeb8 + * Name: lm75_writeb16 * * Description: * Write to a 16-bit register (LM75_TEMP_REG, LM75_THYS_REG, or LM75_TOS_REG) * ****************************************************************************/ -#if 0 // Not used -static int lm75_writeb8(FAR struct lm75_dev_s *priv, uint8_t regaddr, - b8_t regval) + +static int lm75_writeb16(FAR struct lm75_dev_s *priv, uint8_t regaddr, + b16_t regval) { uint8_t buffer[3]; + b8_t regb8; + + lm75dbg("addr: %02x value: %08x\n", regaddr, regval); /* Set up a 3 byte message to send */ buffer[0] = regaddr; - buffer[1] = regval >> 8; - buffer[2] = regval; + + regb8 = b16tob8(regval); + buffer[1] = (uint8_t)(regb8 >> 8); + buffer[2] = (uint8_t)regb8; /* Write the register address followed by the data (no RESTART) */ I2C_SETADDRESS(priv->i2c, priv->addr, 7); return I2C_WRITE(priv->i2c, buffer, 3); } -#endif /**************************************************************************** * Name: lm75_readtemp @@ -187,26 +205,20 @@ static int lm75_writeb8(FAR struct lm75_dev_s *priv, uint8_t regaddr, * ****************************************************************************/ -static int lm75_readtemp(FAR struct lm75_dev_s *priv, b16_t *temp) +static int lm75_readtemp(FAR struct lm75_dev_s *priv, FAR b16_t *temp) { - b8_t temp8; b16_t temp16; int ret; - /* Read the raw temperature data. Data format is: TTTTTTTT Txxxxxxx where - * TTTTTTTTT is a nine-bit temperature value with LSB = 0.5 degrees centigrade. - * So the raw data is b8_t. - */ - - ret = lm75_readb8(priv, LM75_TEMP_REG, &temp8); + /* Read the raw temperature data (b16_t) */ + + ret = lm75_readb16(priv, LM75_TEMP_REG, &temp16); if (ret < 0) { + lm75dbg("lm75_readb16 failed: %d\n", ret); return ret; } - - /* Convert to b16_t */ - - temp16 = b8tob16(temp8); + lm75dbg("Centigrade: %08x\n", temp16); /* Was fahrenheit requested? */ @@ -215,6 +227,7 @@ static int lm75_readtemp(FAR struct lm75_dev_s *priv, b16_t *temp) /* Centigrade to Fahrenheit conversion: F = 9*C/5 + 32 */ temp16 = b16mulb16(temp16, B16_9DIV5) + B16_32; + lm75dbg("Fahrenheit: %08x\n", temp16); } *temp = temp16; @@ -229,7 +242,7 @@ static int lm75_readtemp(FAR struct lm75_dev_s *priv, b16_t *temp) * ****************************************************************************/ -static int lm75_readconf(FAR struct lm75_dev_s *priv, uint8_t *conf) +static int lm75_readconf(FAR struct lm75_dev_s *priv, FAR uint8_t *conf) { uint8_t buffer; int ret; @@ -242,12 +255,14 @@ static int lm75_readconf(FAR struct lm75_dev_s *priv, uint8_t *conf) ret = I2C_WRITE(priv->i2c, &buffer, 1); if (ret < 0) { + lm75dbg("I2C_WRITE failed: %d\n", ret); return ret; } /* Restart and read 8-bits from the register */ ret = I2C_READ(priv->i2c, conf, 1); + lm75dbg("conf: %02x ret: %d\n", *conf, ret); return ret; } @@ -263,6 +278,8 @@ static int lm75_writeconf(FAR struct lm75_dev_s *priv, uint8_t conf) { uint8_t buffer[2]; + lm75dbg("conf: %02x\n", conf); + /* Set up a 2 byte message to send */ buffer[0] = LM75_CONF_REG; @@ -310,16 +327,19 @@ static ssize_t lm75_read(FAR struct file *filep, FAR char *buffer, size_t buflen FAR struct lm75_dev_s *priv = inode->i_private; FAR b16_t *ptr; ssize_t nsamples; + int i; int ret; /* How many samples were requested to get? */ - nsamples = buflen/sizeof(b16_t); + nsamples = buflen / sizeof(b16_t); ptr = (FAR b16_t *)buffer; + lm75dbg("buflen: %d nsamples: %d\n", buflen, nsamples); + /* Get the requested number of samples */ - for (; nsamples > 0; nsamples--) + for (i = 0; i < nsamples; i++) { b16_t temp; @@ -328,6 +348,7 @@ static ssize_t lm75_read(FAR struct file *filep, FAR char *buffer, size_t buflen ret = lm75_readtemp(priv, &temp); if (ret < 0) { + lm75dbg("lm75_readtemp failed: %d\n",ret); return (ssize_t)ret; } @@ -359,7 +380,7 @@ static int lm75_ioctl(FAR struct file *filep, int cmd, unsigned long arg) FAR struct lm75_dev_s *priv = inode->i_private; int ret = OK; - switch (arg) + switch (cmd) { /* Read from the configuration register. Arg: uint8_t* pointer */ @@ -367,6 +388,7 @@ static int lm75_ioctl(FAR struct file *filep, int cmd, unsigned long arg) { FAR uint8_t *ptr = (FAR uint8_t *)((uintptr_t)arg); ret = lm75_readconf(priv, ptr); + lm75dbg("conf: %02x ret: %d\n", *ptr, ret); } break; @@ -374,6 +396,7 @@ static int lm75_ioctl(FAR struct file *filep, int cmd, unsigned long arg) case SNIOC_WRITECONF: ret = lm75_writeconf(priv, (uint8_t)arg); + lm75dbg("conf: %02x ret: %d\n", *(uint8_t*)arg, ret); break; /* Shutdown the LM75, Arg: None */ @@ -386,6 +409,7 @@ static int lm75_ioctl(FAR struct file *filep, int cmd, unsigned long arg) { ret = lm75_writeconf(priv, conf | LM75_CONF_SHUTDOWN); } + lm75dbg("conf: %02x ret: %d\n", conf | LM75_CONF_SHUTDOWN, ret); } break; @@ -399,6 +423,7 @@ static int lm75_ioctl(FAR struct file *filep, int cmd, unsigned long arg) { ret = lm75_writeconf(priv, conf & ~LM75_CONF_SHUTDOWN); } + lm75dbg("conf: %02x ret: %d\n", conf & ~LM75_CONF_SHUTDOWN, ret); } break; @@ -406,15 +431,52 @@ static int lm75_ioctl(FAR struct file *filep, int cmd, unsigned long arg) case SNIOC_FAHRENHEIT: priv->fahrenheit = true; + lm75dbg("Fahrenheit\n"); break; /* Report Samples in Centigrade */ case SNIOC_CENTIGRADE: priv->fahrenheit = false; + lm75dbg("Centigrade\n"); + break; + + /* Read THYS temperature register. Arg: b16_t* pointer */ + + case SNIOC_READTHYS: + { + FAR b16_t *ptr = (FAR b16_t *)((uintptr_t)arg); + ret = lm75_readb16(priv, LM75_THYS_REG, ptr); + lm75dbg("THYS: %08x ret: %d\n", *ptr, ret); + } + break; + + /* Write THYS temperature register. Arg: b16_t value */ + + case SNIOC_WRITETHYS: + ret = lm75_writeb16(priv, LM75_THYS_REG, (b16_t)arg); + lm75dbg("THYS: %08x ret: %d\n", (b16_t)arg, ret); + break; + + /* Read TOS (Over-temp Shutdown Threshold) Register. Arg: b16_t* pointer */ + + case SNIOC_READTOS: + { + FAR b16_t *ptr = (FAR b16_t *)((uintptr_t)arg); + ret = lm75_readb16(priv, LM75_TOS_REG, ptr); + lm75dbg("TOS: %08x ret: %d\n", *ptr, ret); + } + break; + + /* Write TOS (Over-temp Shutdown Threshold) Register. Arg: b16_t value */ + + case SNIOC_WRITRETOS: + ret = lm75_writeb16(priv, LM75_TOS_REG, (b16_t)arg); + lm75dbg("TOS: %08x ret: %d\n", (b16_t)arg, ret); break; default: + lm75dbg("Unrecognized cmd: %d\n", cmd); ret = -ENOTTY; break; } @@ -454,6 +516,7 @@ int lm75_register(FAR const char *devpath, FAR struct i2c_dev_s *i2c, uint8_t ad priv = (FAR struct lm75_dev_s *)malloc(sizeof(struct lm75_dev_s)); if (!priv) { + lm75dbg("Failed to allocate instance\n"); return -ENOMEM; } @@ -466,6 +529,7 @@ int lm75_register(FAR const char *devpath, FAR struct i2c_dev_s *i2c, uint8_t ad ret = register_driver(devpath, &lm75_fops, 0555, priv); if (ret < 0) { + lm75dbg("Failed to register driver: %d\n", ret); free(priv); } return ret; diff --git a/nuttx/include/nuttx/sensors/lm75.h b/nuttx/include/nuttx/sensors/lm75.h index 54100088e..0d27a586b 100644 --- a/nuttx/include/nuttx/sensors/lm75.h +++ b/nuttx/include/nuttx/sensors/lm75.h @@ -61,6 +61,10 @@ #define SNIOC_POWERUP _SNIOC(0x0004) /* Arg: None */ #define SNIOC_FAHRENHEIT _SNIOC(0x0005) /* Arg: None */ #define SNIOC_CENTIGRADE _SNIOC(0x0006) /* Arg: None */ +#define SNIOC_READTHYS _SNIOC(0x0007) /* Arg: b16_t* pointer */ +#define SNIOC_WRITETHYS _SNIOC(0x0008) /* Arg: b16_t value */ +#define SNIOC_READTOS _SNIOC(0x0009) /* Arg: b16_t* pointer */ +#define SNIOC_WRITRETOS _SNIOC(0x000a) /* Arg: b16_t value */ /* LM-75 Register Definitions ***********************************************/ /* LM-75 Registers addresses */ @@ -68,7 +72,7 @@ #define LM75_TEMP_REG 0x00 /* Temperature Register */ #define LM75_CONF_REG 0x01 /* Configuration Register */ #define LM75_THYS_REG 0x02 /* Temperature Register */ -#define LM75_TOS_REG 0x03 /* Over-temp Shutdown threshold Register */ +#define LM75_TOS_REG 0x03 /* Over-temp Shutdown Threshold Register */ /* Configuration Register Bit Definitions */ diff --git a/nuttx/sched/clock_gettime.c b/nuttx/sched/clock_gettime.c index 5f975f52b..3fdfe9602 100644 --- a/nuttx/sched/clock_gettime.c +++ b/nuttx/sched/clock_gettime.c @@ -148,7 +148,7 @@ int clock_gettime(clockid_t clock_id, struct timespec *tp) if (g_rtc_enabled) { tp->tv_sec = up_rtc_gettime(); - tp->tv_nsec = (up_rtc_getclock() & (RTC_CLOCKS_PER_SEC-1) ) * (1000000000/TICK_PER_SEC); + tp->tv_nsec = (up_rtc_getclock() & (RTC_CLOCKS_PER_SEC-1) ) * (1000000000/RTC_CLOCKS_PER_SEC); } else #endif -- cgit v1.2.3