From 8c5b57d449bae1b9f5b3b501356e486439f93ae0 Mon Sep 17 00:00:00 2001 From: patacongo Date: Tue, 27 Nov 2012 15:09:12 +0000 Subject: configs/z8f64200100kit/ostest at same level as ez80 configurations; nuttx/arch/arm/src/lpc17xx/lpc17_i2c.cuninitialization fix git-svn-id: http://svn.code.sf.net/p/nuttx/code/trunk@5392 42af7a65-404d-4744-a932-0658087f49c3 --- nuttx/arch/arm/src/lpc17xx/lpc17_i2c.c | 541 ++++++++++++++++++--------------- 1 file changed, 300 insertions(+), 241 deletions(-) (limited to 'nuttx/arch') diff --git a/nuttx/arch/arm/src/lpc17xx/lpc17_i2c.c b/nuttx/arch/arm/src/lpc17xx/lpc17_i2c.c index 48d6fefce..935dbfa0c 100644 --- a/nuttx/arch/arm/src/lpc17xx/lpc17_i2c.c +++ b/nuttx/arch/arm/src/lpc17xx/lpc17_i2c.c @@ -67,7 +67,6 @@ #include "up_internal.h" #include "os_internal.h" - #include "lpc17_internal.h" #include "lpc17_syscon.h" #include "lpc17_pinconn.h" @@ -79,12 +78,15 @@ #define GPIO_I2C1_SCL GPIO_I2C1_SCL_1 #define GPIO_I2C1_SDA GPIO_I2C1_SDA_1 #endif + #ifndef CONFIG_I2C0_FREQ #define CONFIG_I2C0_FREQ 100000 #endif + #ifndef CONFIG_I2C1_FREQ #define CONFIG_I2C1_FREQ 100000 #endif + #ifndef CONFIG_I2C2_FREQ #define CONFIG_I2C2_FREQ 100000 #endif @@ -93,44 +95,46 @@ * Definitions *******************************************************************************/ -/**************************************************************************** +/******************************************************************************* * Pre-processor Definitions - ****************************************************************************/ + *******************************************************************************/ #define I2C_TIMEOUT ((20 * CLK_TCK) / 1000) /* 20 mS */ -/**************************************************************************** +/******************************************************************************* * Private Data - ****************************************************************************/ + *******************************************************************************/ + struct lpc17_i2cdev_s { - struct i2c_dev_s dev; /* Generic I2C device */ - struct i2c_msg_s msg; /* a single message for legacy read/write */ - unsigned int base; /* Base address of registers */ - uint16_t irqid; /* IRQ for this device */ - - sem_t mutex; /* Only one thread can access at a time */ - sem_t wait; /* Place to wait for state machine completion */ - volatile uint8_t state; /* State of state machine */ - WDOG_ID timeout; /* watchdog to timeout when bus hung */ - - uint16_t wrcnt; /* number of bytes sent to tx fifo */ - uint16_t rdcnt; /* number of bytes read from rx fifo */ + struct i2c_dev_s dev; /* Generic I2C device */ + struct i2c_msg_s msg; /* a single message for legacy read/write */ + unsigned int base; /* Base address of registers */ + uint16_t irqid; /* IRQ for this device */ + + sem_t mutex; /* Only one thread can access at a time */ + sem_t wait; /* Place to wait for state machine completion */ + volatile uint8_t state; /* State of state machine */ + WDOG_ID timeout; /* watchdog to timeout when bus hung */ + + uint16_t wrcnt; /* number of bytes sent to tx fifo */ + uint16_t rdcnt; /* number of bytes read from rx fifo */ }; static struct lpc17_i2cdev_s i2cdevices[3]; -/**************************************************************************** +/******************************************************************************* * Private Functions - ****************************************************************************/ -static int i2c_start (struct lpc17_i2cdev_s *priv); -static void i2c_stop (struct lpc17_i2cdev_s *priv); -static int i2c_interrupt (int irq, FAR void *context); -static void i2c_timeout (int argc, uint32_t arg, ...); + *******************************************************************************/ + +static int i2c_start(struct lpc17_i2cdev_s *priv); +static void i2c_stop(struct lpc17_i2cdev_s *priv); +static int i2c_interrupt(int irq, FAR void *context); +static void i2c_timeout(int argc, uint32_t arg, ...); -/**************************************************************************** +/******************************************************************************* * I2C device operations - ****************************************************************************/ + *******************************************************************************/ static uint32_t i2c_setfrequency(FAR struct i2c_dev_s *dev, uint32_t frequency); static int i2c_setaddress(FAR struct i2c_dev_s *dev, int addr, int nbits); @@ -140,12 +144,12 @@ static int i2c_transfer(FAR struct i2c_dev_s *dev, FAR struct i2c_msg_s *ms struct i2c_ops_s lpc17_i2c_ops = { - .setfrequency = i2c_setfrequency, - .setaddress = i2c_setaddress, - .write = i2c_write, - .read = i2c_read, + .setfrequency = i2c_setfrequency, + .setaddress = i2c_setaddress, + .write = i2c_write, + .read = i2c_read, #ifdef CONFIG_I2C_TRANSFER - .transfer = i2c_transfer + .transfer = i2c_transfer #endif }; @@ -159,23 +163,26 @@ struct i2c_ops_s lpc17_i2c_ops = static uint32_t i2c_setfrequency(FAR struct i2c_dev_s *dev, uint32_t frequency) { - struct lpc17_i2cdev_s *priv = (struct lpc17_i2cdev_s *) dev; + struct lpc17_i2cdev_s *priv = (struct lpc17_i2cdev_s *) dev; - if (frequency > 100000) + if (frequency > 100000) { - /* asymetric per 400Khz I2C spec */ - putreg32 ( LPC17_CCLK / (83 + 47) * 47 / frequency, priv->base + LPC17_I2C_SCLH_OFFSET); - putreg32 ( LPC17_CCLK / (83 + 47) * 83 / frequency, priv->base + LPC17_I2C_SCLL_OFFSET); + /* asymetric per 400Khz I2C spec */ + + putreg32(LPC17_CCLK / (83 + 47) * 47 / frequency, priv->base + LPC17_I2C_SCLH_OFFSET); + putreg32(LPC17_CCLK / (83 + 47) * 83 / frequency, priv->base + LPC17_I2C_SCLL_OFFSET); } - else + else { - /* 50/50 mark space ratio */ - putreg32 (LPC17_CCLK / 100 * 50 / frequency, priv->base + LPC17_I2C_SCLH_OFFSET); - putreg32 (LPC17_CCLK / 100 * 50 / frequency, priv->base + LPC17_I2C_SCLL_OFFSET); + /* 50/50 mark space ratio */ + + putreg32(LPC17_CCLK / 100 * 50 / frequency, priv->base + LPC17_I2C_SCLH_OFFSET); + putreg32(LPC17_CCLK / 100 * 50 / frequency, priv->base + LPC17_I2C_SCLL_OFFSET); } - /* FIXME: This function should return the actual selected frequency */ - return frequency; + /* FIXME: This function should return the actual selected frequency */ + + return frequency; } /******************************************************************************* @@ -185,17 +192,18 @@ static uint32_t i2c_setfrequency(FAR struct i2c_dev_s *dev, uint32_t frequency) * Set the I2C slave address for a subsequent read/write * *******************************************************************************/ + static int i2c_setaddress(FAR struct i2c_dev_s *dev, int addr, int nbits) { - struct lpc17_i2cdev_s *priv = (struct lpc17_i2cdev_s *) dev; + struct lpc17_i2cdev_s *priv = (struct lpc17_i2cdev_s *) dev; - DEBUGASSERT(dev != NULL); - DEBUGASSERT(nbits == 7 ); + DEBUGASSERT(dev != NULL); + DEBUGASSERT(nbits == 7 ); - priv->msg.addr = addr<<1; - priv->msg.flags = 0 ; + priv->msg.addr = addr<<1; + priv->msg.flags = 0 ; - return OK; + return OK; } /******************************************************************************* @@ -206,22 +214,23 @@ static int i2c_setaddress(FAR struct i2c_dev_s *dev, int addr, int nbits) * frequency and slave address. * *******************************************************************************/ + static int i2c_write(FAR struct i2c_dev_s *dev, const uint8_t *buffer, int buflen) { - struct lpc17_i2cdev_s *priv = (struct lpc17_i2cdev_s *) dev; - int ret; + struct lpc17_i2cdev_s *priv = (struct lpc17_i2cdev_s *) dev; + int ret; - DEBUGASSERT (dev != NULL); + DEBUGASSERT(dev != NULL); - priv->wrcnt=0; - priv->rdcnt=0; - priv->msg.addr &= ~0x01; - priv->msg.buffer = (uint8_t*)buffer; - priv->msg.length = buflen; - - ret = i2c_start (priv); + priv->wrcnt = 0; + priv->rdcnt = 0; + priv->msg.addr &= ~0x01; + priv->msg.buffer = (uint8_t*)buffer; + priv->msg.length = buflen; + + ret = i2c_start(priv); - return ret >0 ? OK : -ETIMEDOUT; + return ret > 0 ? OK : -ETIMEDOUT; } /******************************************************************************* @@ -232,22 +241,23 @@ static int i2c_write(FAR struct i2c_dev_s *dev, const uint8_t *buffer, int bufle * frequency and slave address. * *******************************************************************************/ + static int i2c_read(FAR struct i2c_dev_s *dev, uint8_t *buffer, int buflen) { - struct lpc17_i2cdev_s *priv = (struct lpc17_i2cdev_s *) dev; - int ret; + struct lpc17_i2cdev_s *priv = (struct lpc17_i2cdev_s *) dev; + int ret; - DEBUGASSERT (dev != NULL); + DEBUGASSERT(dev != NULL); - priv->wrcnt=0; - priv->rdcnt=0; - priv->msg.addr |= 0x01; - priv->msg.buffer = buffer; - priv->msg.length = buflen; + priv->wrcnt = 0; + priv->rdcnt = 0; + priv->msg.addr |= 0x01; + priv->msg.buffer = buffer; + priv->msg.length = buflen; - ret = i2c_start (priv); + ret = i2c_start(priv); - return ret >0 ? OK : -ETIMEDOUT; + return ret > 0 ? OK : -ETIMEDOUT; } /******************************************************************************* @@ -257,24 +267,31 @@ static int i2c_read(FAR struct i2c_dev_s *dev, uint8_t *buffer, int buflen) * Perform a I2C transfer start * *******************************************************************************/ -static int i2c_start (struct lpc17_i2cdev_s *priv) + +static int i2c_start(struct lpc17_i2cdev_s *priv) { - int ret=-1; - sem_wait (&priv->mutex); - - putreg32(I2C_CONCLR_STAC|I2C_CONCLR_SIC,priv->base+LPC17_I2C_CONCLR_OFFSET); - putreg32(I2C_CONSET_STA,priv->base+LPC17_I2C_CONSET_OFFSET); - - wd_start (priv->timeout, I2C_TIMEOUT, i2c_timeout, 1, (uint32_t)priv); - sem_wait(&priv->wait); - wd_cancel (priv->timeout); - sem_post (&priv->mutex); - - if( priv-> state == 0x18 || priv->state == 0x28) - ret=priv->wrcnt; - else if( priv-> state == 0x50 || priv->state == 0x58) - ret=priv->rdcnt; - return ret; + int ret = -1; + + sem_wait(&priv->mutex); + + putreg32(I2C_CONCLR_STAC|I2C_CONCLR_SIC, priv->base+LPC17_I2C_CONCLR_OFFSET); + putreg32(I2C_CONSET_STA, priv->base+LPC17_I2C_CONSET_OFFSET); + + wd_start(priv->timeout, I2C_TIMEOUT, i2c_timeout, 1, (uint32_t)priv); + sem_wait(&priv->wait); + wd_cancel(priv->timeout); + sem_post(&priv->mutex); + + if (priv-> state == 0x18 || priv->state == 0x28) + { + ret = priv->wrcnt; + } + else if (priv-> state == 0x50 || priv->state == 0x58) + { + ret = priv->rdcnt; + } + + return ret; } /******************************************************************************* @@ -284,11 +301,15 @@ static int i2c_start (struct lpc17_i2cdev_s *priv) * Perform a I2C transfer stop * *******************************************************************************/ -static void i2c_stop (struct lpc17_i2cdev_s *priv) + +static void i2c_stop(struct lpc17_i2cdev_s *priv) { - if(priv->state!=0x38) - putreg32(I2C_CONSET_STO|I2C_CONSET_AA,priv->base+LPC17_I2C_CONSET_OFFSET); - sem_post (&priv->wait); + if (priv->state != 0x38) + { + putreg32(I2C_CONSET_STO|I2C_CONSET_AA,priv->base+LPC17_I2C_CONSET_OFFSET); + } + + sem_post(&priv->wait); } /******************************************************************************* @@ -299,14 +320,14 @@ static void i2c_stop (struct lpc17_i2cdev_s *priv) * *******************************************************************************/ -static void i2c_timeout (int argc, uint32_t arg, ...) +static void i2c_timeout(int argc, uint32_t arg, ...) { - struct lpc17_i2cdev_s *priv = (struct lpc17_i2cdev_s *) arg; + struct lpc17_i2cdev_s *priv = (struct lpc17_i2cdev_s *) arg; - irqstate_t flags = irqsave(); - priv->state = 0xff; - sem_post (&priv->wait); - irqrestore (flags); + irqstate_t flags = irqsave(); + priv->state = 0xff; + sem_post(&priv->wait); + irqrestore(flags); } /******************************************************************************* @@ -317,90 +338,109 @@ static void i2c_timeout (int argc, uint32_t arg, ...) * *******************************************************************************/ -static int i2c_interrupt (int irq, FAR void *context) +static int i2c_interrupt(int irq, FAR void *context) { - struct lpc17_i2cdev_s *priv; + struct lpc17_i2cdev_s *priv; + uint32_t state; + #ifdef CONFIG_LPC17_I2C0 - if (irq == LPC17_IRQ_I2C0) + if (irq == LPC17_IRQ_I2C0) { - priv=&i2cdevices[0]; + priv=&i2cdevices[0]; } - else + else #endif #ifdef CONFIG_LPC17_I2C1 - if (irq == LPC17_IRQ_I2C1) + if (irq == LPC17_IRQ_I2C1) { - priv=&i2cdevices[1]; + priv=&i2cdevices[1]; } - else + else #endif #ifdef CONFIG_LPC17_I2C2 - if (irq == LPC17_IRQ_I2C2) + if (irq == LPC17_IRQ_I2C2) { - priv=&i2cdevices[2]; + priv=&i2cdevices[2]; } else #endif { PANIC(OSERR_INTERNAL); } -/* - * refrence UM10360 19.10.5 - */ - uint32_t state = getreg32(priv->base+LPC17_I2C_STAT_OFFSET); - putreg32(I2C_CONCLR_SIC,priv->base+LPC17_I2C_CONCLR_OFFSET); - priv->state=state; - state &=0xf8; - switch (state) - { - case 0x00: //Bus Error - case 0x20: - case 0x30: - case 0x38: - case 0x48: - i2c_stop(priv); - break; - case 0x08: //START - case 0x10: //Repeat START - putreg32(priv->msg.addr,priv->base+LPC17_I2C_DAT_OFFSET); - putreg32(I2C_CONCLR_STAC,priv->base+LPC17_I2C_CONCLR_OFFSET); - break; - case 0x18: - priv->wrcnt=0; - putreg32(priv->msg.buffer[0],priv->base+LPC17_I2C_DAT_OFFSET); - break; - case 0x28: - priv->wrcnt++; - if(priv->wrcntmsg.length) - putreg32(priv->msg.buffer[priv->wrcnt],priv->base+LPC17_I2C_DAT_OFFSET); - else - i2c_stop(priv); - break; - case 0x40: - priv->rdcnt=-1; - putreg32(I2C_CONSET_AA,priv->base+LPC17_I2C_CONSET_OFFSET); - break; - case 0x50: - priv->rdcnt++; - if(priv->rdcntmsg.length) - priv->msg.buffer[priv->rdcnt]=getreg32(priv->base+LPC17_I2C_BUFR_OFFSET); - if(priv->rdcnt>=priv->msg.length-1) - putreg32(I2C_CONCLR_AAC|I2C_CONCLR_SIC,priv->base+LPC17_I2C_CONCLR_OFFSET); - break; - case 0x58: - i2c_stop(priv); - break; - default: - i2c_stop(priv); - break; - } - return OK; + +/* Reference UM10360 19.10.5 */ + + state = getreg32(priv->base+LPC17_I2C_STAT_OFFSET); + putreg32(I2C_CONCLR_SIC, priv->base+LPC17_I2C_CONCLR_OFFSET); + priv->state = state; + state &= 0xf8; + + switch (state) + { + case 0x00: // Bus Error + case 0x20: + case 0x30: + case 0x38: + case 0x48: + i2c_stop(priv); + break; + + case 0x08: // START + case 0x10: // Repeat START + putreg32(priv->msg.addr, priv->base+LPC17_I2C_DAT_OFFSET); + putreg32(I2C_CONCLR_STAC, priv->base+LPC17_I2C_CONCLR_OFFSET); + break; + + case 0x18: + priv->wrcnt = 0; + putreg32(priv->msg.buffer[0], priv->base+LPC17_I2C_DAT_OFFSET); + break; + + case 0x28: + priv->wrcnt++; + if (priv->wrcntmsg.length) + { + putreg32(priv->msg.buffer[priv->wrcnt],priv->base+LPC17_I2C_DAT_OFFSET); + } + else + { + i2c_stop(priv); + } + break; + + case 0x40: + priv->rdcnt = -1; + putreg32(I2C_CONSET_AA, priv->base+LPC17_I2C_CONSET_OFFSET); + break; + + case 0x50: + priv->rdcnt++; + if (priv->rdcnt < priv->msg.length) + { + priv->msg.buffer[priv->rdcnt] = getreg32(priv->base+LPC17_I2C_BUFR_OFFSET); + } + + if (priv->rdcnt>=priv->msg.length-1) + { + putreg32(I2C_CONCLR_AAC|I2C_CONCLR_SIC, priv->base+LPC17_I2C_CONCLR_OFFSET); + } + break; + + case 0x58: + i2c_stop(priv); + break; + + default: + i2c_stop(priv); + break; + } + + return OK; } -/**************************************************************************** +/******************************************************************************* * Public Functions - ****************************************************************************/ - + *******************************************************************************/ /******************************************************************************* * Name: up_i2cinitialize @@ -412,116 +452,117 @@ static int i2c_interrupt (int irq, FAR void *context) struct i2c_dev_s *up_i2cinitialize(int port) { - struct lpc17_i2cdev_s *priv; + struct lpc17_i2cdev_s *priv; + irqstate_t flags; + uint32_t regval; - if (port>2) + if (port > 2) { - dbg("lpc I2C Only support 0,1,2\n"); - return NULL; + dbg("lpc I2C Only support 0,1,2\n"); + return NULL; } - irqstate_t flags; - uint32_t regval; - - flags = irqsave(); + flags = irqsave(); - priv= &i2cdevices[port]; + priv= &i2cdevices[port]; #ifdef CONFIG_LPC17_I2C0 - if (port==0) + if (port == 0) { - priv= (FAR struct lpc17_i2cdev_s *)&i2cdevices[0]; - priv->base = LPC17_I2C0_BASE; - priv->irqid = LPC17_IRQ_I2C0; - - regval = getreg32(LPC17_SYSCON_PCONP); - regval |= SYSCON_PCONP_PCI2C0; - putreg32(regval, LPC17_SYSCON_PCONP); - - regval = getreg32(LPC17_SYSCON_PCLKSEL0); - regval &= ~SYSCON_PCLKSEL0_I2C0_MASK; - regval |= (SYSCON_PCLKSEL_CCLK << SYSCON_PCLKSEL0_I2C0_SHIFT); - putreg32(regval, LPC17_SYSCON_PCLKSEL0); + priv= (FAR struct lpc17_i2cdev_s *)&i2cdevices[0]; + priv->base = LPC17_I2C0_BASE; + priv->irqid = LPC17_IRQ_I2C0; + + regval = getreg32(LPC17_SYSCON_PCONP); + regval |= SYSCON_PCONP_PCI2C0; + putreg32(regval, LPC17_SYSCON_PCONP); + + regval = getreg32(LPC17_SYSCON_PCLKSEL0); + regval &= ~SYSCON_PCLKSEL0_I2C0_MASK; + regval |= (SYSCON_PCLKSEL_CCLK << SYSCON_PCLKSEL0_I2C0_SHIFT); + putreg32(regval, LPC17_SYSCON_PCLKSEL0); - lpc17_configgpio(GPIO_I2C0_SCL); - lpc17_configgpio(GPIO_I2C0_SDA); + lpc17_configgpio(GPIO_I2C0_SCL); + lpc17_configgpio(GPIO_I2C0_SDA); - putreg32 (LPC17_CCLK/CONFIG_I2C0_FREQ/2, priv->base + LPC17_I2C_SCLH_OFFSET); - putreg32 (LPC17_CCLK/CONFIG_I2C0_FREQ/2, priv->base + LPC17_I2C_SCLL_OFFSET); - + putreg32(LPC17_CCLK/CONFIG_I2C0_FREQ/2, priv->base + LPC17_I2C_SCLH_OFFSET); + putreg32(LPC17_CCLK/CONFIG_I2C0_FREQ/2, priv->base + LPC17_I2C_SCLL_OFFSET); } - else + else #endif #ifdef CONFIG_LPC17_I2C1 - if(port==1) + if (port == 1) { - priv= (FAR struct lpc17_i2cdev_s *)&i2cdevices[1]; - priv->base = LPC17_I2C1_BASE; - priv->irqid = LPC17_IRQ_I2C1; + priv= (FAR struct lpc17_i2cdev_s *)&i2cdevices[1]; + priv->base = LPC17_I2C1_BASE; + priv->irqid = LPC17_IRQ_I2C1; - regval = getreg32(LPC17_SYSCON_PCONP); - regval |= SYSCON_PCONP_PCI2C1; - putreg32(regval, LPC17_SYSCON_PCONP); + regval = getreg32(LPC17_SYSCON_PCONP); + regval |= SYSCON_PCONP_PCI2C1; + putreg32(regval, LPC17_SYSCON_PCONP); - regval = getreg32(LPC17_SYSCON_PCLKSEL1); - regval &= ~SYSCON_PCLKSEL1_I2C1_MASK; - regval |= (SYSCON_PCLKSEL_CCLK << SYSCON_PCLKSEL1_I2C1_SHIFT); - putreg32(regval, LPC17_SYSCON_PCLKSEL1); + regval = getreg32(LPC17_SYSCON_PCLKSEL1); + regval &= ~SYSCON_PCLKSEL1_I2C1_MASK; + regval |= (SYSCON_PCLKSEL_CCLK << SYSCON_PCLKSEL1_I2C1_SHIFT); + putreg32(regval, LPC17_SYSCON_PCLKSEL1); - lpc17_configgpio(GPIO_I2C1_SCL); - lpc17_configgpio(GPIO_I2C1_SDA); + lpc17_configgpio(GPIO_I2C1_SCL); + lpc17_configgpio(GPIO_I2C1_SDA); - putreg32 (LPC17_CCLK/CONFIG_I2C1_FREQ/2, priv->base + LPC17_I2C_SCLH_OFFSET); - putreg32 (LPC17_CCLK/CONFIG_I2C1_FREQ/2, priv->base + LPC17_I2C_SCLL_OFFSET); + putreg32(LPC17_CCLK/CONFIG_I2C1_FREQ/2, priv->base + LPC17_I2C_SCLH_OFFSET); + putreg32(LPC17_CCLK/CONFIG_I2C1_FREQ/2, priv->base + LPC17_I2C_SCLL_OFFSET); } - else + else #endif #ifdef CONFIG_LPC17_I2C2 - if(port==2) + if (port == 2) { - priv= (FAR struct lpc17_i2cdev_s *)&i2cdevices[2]; - priv->base = LPC17_I2C2_BASE; - priv->irqid = LPC17_IRQ_I2C2; - - regval = getreg32(LPC17_SYSCON_PCONP); - regval |= SYSCON_PCONP_PCI2C2; - putreg32(regval, LPC17_SYSCON_PCONP); - - regval = getreg32(LPC17_SYSCON_PCLKSEL1); - regval &= ~SYSCON_PCLKSEL1_I2C2_MASK; - regval |= (SYSCON_PCLKSEL_CCLK << SYSCON_PCLKSEL1_I2C2_SHIFT); - putreg32(regval, LPC17_SYSCON_PCLKSEL1); + priv= (FAR struct lpc17_i2cdev_s *)&i2cdevices[2]; + priv->base = LPC17_I2C2_BASE; + priv->irqid = LPC17_IRQ_I2C2; + + regval = getreg32(LPC17_SYSCON_PCONP); + regval |= SYSCON_PCONP_PCI2C2; + putreg32(regval, LPC17_SYSCON_PCONP); + + regval = getreg32(LPC17_SYSCON_PCLKSEL1); + regval &= ~SYSCON_PCLKSEL1_I2C2_MASK; + regval |= (SYSCON_PCLKSEL_CCLK << SYSCON_PCLKSEL1_I2C2_SHIFT); + putreg32(regval, LPC17_SYSCON_PCLKSEL1); - lpc17_configgpio(GPIO_I2C2_SCL); - lpc17_configgpio(GPIO_I2C2_SDA); + lpc17_configgpio(GPIO_I2C2_SCL); + lpc17_configgpio(GPIO_I2C2_SDA); - putreg32 (LPC17_CCLK/CONFIG_I2C2_FREQ/2, priv->base + LPC17_I2C_SCLH_OFFSET); - putreg32 (LPC17_CCLK/CONFIG_I2C2_FREQ/2, priv->base + LPC17_I2C_SCLL_OFFSET); + putreg32(LPC17_CCLK/CONFIG_I2C2_FREQ/2, priv->base + LPC17_I2C_SCLH_OFFSET); + putreg32(LPC17_CCLK/CONFIG_I2C2_FREQ/2, priv->base + LPC17_I2C_SCLL_OFFSET); } - else + else #endif { - return NULL; + return NULL; } - putreg32(I2C_CONSET_I2EN,priv->base+LPC17_I2C_CONSET_OFFSET); - sem_init (&priv->mutex, 0, 1); - sem_init (&priv->wait, 0, 0); + putreg32(I2C_CONSET_I2EN, priv->base+LPC17_I2C_CONSET_OFFSET); - /* Allocate a watchdog timer */ - priv->timeout = wd_create(); + sem_init(&priv->mutex, 0, 1); + sem_init(&priv->wait, 0, 0); - DEBUGASSERT(priv->timeout != 0); + /* Allocate a watchdog timer */ - /* Attach Interrupt Handler */ - irq_attach (priv->irqid, i2c_interrupt); + priv->timeout = wd_create(); + DEBUGASSERT(priv->timeout != 0); - /* Enable Interrupt Handler */ - up_enable_irq(priv->irqid); + /* Attach Interrupt Handler */ - /* Install our operations */ - priv->dev.ops = &lpc17_i2c_ops; + irq_attach(priv->irqid, i2c_interrupt); - return &priv->dev; + /* Enable Interrupt Handler */ + + up_enable_irq(priv->irqid); + + /* Install our operations */ + + priv->dev.ops = &lpc17_i2c_ops; + return &priv->dev; } /******************************************************************************* @@ -534,12 +575,30 @@ struct i2c_dev_s *up_i2cinitialize(int port) int up_i2cuninitialize(FAR struct i2c_dev_s * dev) { - struct lpc17_i2cdev_s *priv = (struct lpc17_i2cdev_s *) dev; - - putreg32(I2C_CONCLRT_I2ENC,priv->base+LPC17_I2C_CONCLR_OFFSET); - up_disable_irq(priv->irqid); - irq_detach (priv->irqid); - return OK; + struct lpc17_i2cdev_s *priv = (struct lpc17_i2cdev_s *) dev; + + /* Disable I2C */ + + putreg32(I2C_CONCLRT_I2ENC, priv->base+LPC17_I2C_CONCLR_OFFSET); + + /* Reset data structures */ + + sem_destroy(&priv->mutex); + sem_destroy(&priv->wait); + + /* Free the watchdog timer */ + + wd_delete(priv->timeout); + priv->timeout = NULL; + + /* Disable interrupts */ + + up_disable_irq(priv->irqid); + + /* Detach Interrupt Handler */ + + irq_detach(priv->irqid); + return OK; } #endif -- cgit v1.2.3