summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--nuttx/ChangeLog13
-rw-r--r--nuttx/Documentation/NuttX.html15
-rw-r--r--nuttx/TODO13
-rwxr-xr-xnuttx/arch/arm/src/lpc313x/Make.defs9
-rwxr-xr-xnuttx/arch/arm/src/lpc313x/lpc313x_adc.h2
-rwxr-xr-xnuttx/arch/arm/src/lpc313x/lpc313x_evntrtr.h8
-rw-r--r--nuttx/arch/arm/src/lpc313x/lpc313x_i2c.c605
-rwxr-xr-xnuttx/arch/arm/src/lpc313x/lpc313x_ioconfig.h6
-rw-r--r--nuttx/arch/arm/src/lpc313x/lpc313x_spi.c781
-rwxr-xr-xnuttx/arch/arm/src/lpc313x/lpc313x_spi.h2
-rwxr-xr-xnuttx/arch/arm/src/lpc313x/lpc313x_usbdev.c113
11 files changed, 1482 insertions, 85 deletions
diff --git a/nuttx/ChangeLog b/nuttx/ChangeLog
index 8ed09d3ef..7ee696e9c 100644
--- a/nuttx/ChangeLog
+++ b/nuttx/ChangeLog
@@ -1129,3 +1129,16 @@
* drivers/lcd/p14201.c - Driver for RiT P14201 series 128x96 4-bit OLED.
* configs/lm3s6965-ek/nx - NX graphics configuration for the LM3S6965
Ethernet Evaluation Kit.
+ * graphics/ - Numerous fixes to get the P14201 4-bpp greyscale display
+ working (there may still be some minor issues .. see the TODO list).
+ * arch/arm/include/lpc17xx and arch/arm/src/lpc17xxx - Began port for
+ NXP LPC1768
+ * drivers/mtd/m25px.c - Add support for M25P1 flash part (See NOTE)
+ * include/nuttx/i2c.h - Extended I2C interface definition to handle
+ multiple transfers (See NOTE).
+ * include/nuttx/usbdev.h - Corrected an important macro definition
+ needed to correctly handle USB null packet transfers (See NOTE).
+ * arch/arm/src/lpc313x - New drivers: I2C and SPI. Plus several
+ important LPC313x USB bug fixes (See NOTE).
+
+ NOTE: Contributed by David Hewson.
diff --git a/nuttx/Documentation/NuttX.html b/nuttx/Documentation/NuttX.html
index 05b31b4b1..702eeaf0c 100644
--- a/nuttx/Documentation/NuttX.html
+++ b/nuttx/Documentation/NuttX.html
@@ -8,7 +8,7 @@
<tr align="center" bgcolor="#e4e4e4">
<td>
<h1><big><font color="#3c34ec"><i>NuttX RTOS</i></font></big></h1>
- <p>Last Updated: May 12, 2010</p>
+ <p>Last Updated: May 26, 2010</p>
</td>
</tr>
</table>
@@ -1752,6 +1752,19 @@ nuttx-5.6 2010-xx-xx Gregory Nutt &lt;spudmonkey@racsa.co.cr&gt;
* drivers/lcd/p14201.c - Driver for RiT P14201 series 128x96 4-bit OLED.
* configs/lm3s6965-ek/nx - NX graphics configuration for the LM3S6965
Ethernet Evaluation Kit.
+ * graphics/ - Numerous fixes to get the P14201 4-bpp greyscale display
+ working (there may still be some minor issues .. see the TODO list).
+ * arch/arm/include/lpc17xx and arch/arm/src/lpc17xxx - Began port for
+ NXP LPC1768
+ * drivers/mtd/m25px.c - Add support for M25P1 flash part (See NOTE)
+ * include/nuttx/i2c.h - Extended I2C interface definition to handle
+ multiple transfers (See NOTE).
+ * include/nuttx/usbdev.h - Corrected an important macro definition
+ needed to correctly handle USB null packet transfers (See NOTE).
+ * arch/arm/src/lpc313x - New drivers: I2C and SPI. Plus several
+ important LPC313x USB bug fixes (See NOTE).
+
+ NOTE: Contributed by David Hewson.
pascal-2.1 2010-xx-xx Gregory Nutt &lt;spudmonkey@racsa.co.cr&gt;
diff --git a/nuttx/TODO b/nuttx/TODO
index 0458f4ddc..4fbfeee94 100644
--- a/nuttx/TODO
+++ b/nuttx/TODO
@@ -1,4 +1,4 @@
-NuttX TODO List (Last updated May 16, 2010)
+NuttX TODO List (Last updated May 19, 2010)
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
(5) Task/Scheduler (sched/)
@@ -12,7 +12,7 @@ NuttX TODO List (Last updated May 16, 2010)
(1) USB (drivers/usbdev)
(5) Libraries (lib/)
(12) File system/Generic drivers (fs/, drivers/)
- (2) Graphics subystem (graphics/)
+ (3) Graphics subystem (graphics/)
(1) Pascal add-on (pcode/)
(1) Documentation (Documentation/)
(6) Build system / Toolchains
@@ -430,6 +430,15 @@ o Graphics subystem (graphics/)
Status: Open
Priority: Medium
+ Description: The examples/nx test using lcd/p14201.c and the configs/lm3s6965-ek
+ configuration shows two single pixel-wide anomalies. One along
+ column zero is clearly caused by the NX windowing logic. It is
+ not certain if these are consequences of the 4bpp logic or if these
+ are anomalies that have always been in NX, but are only visible
+ now at the low resolution of the p14201 LCD (128x96).
+ Status: Open
+ Priority: Low (unless you need the p13201 then it is certainly higher).
+
o Pascal Add-On (pcode/)
^^^^^^^^^^^^^^^^^^^^^^
diff --git a/nuttx/arch/arm/src/lpc313x/Make.defs b/nuttx/arch/arm/src/lpc313x/Make.defs
index c29070aa5..799984b07 100755
--- a/nuttx/arch/arm/src/lpc313x/Make.defs
+++ b/nuttx/arch/arm/src/lpc313x/Make.defs
@@ -40,6 +40,7 @@ CMN_ASRCS = up_cache.S up_fullcontextrestore.S up_saveusercontext.S \
CMN_CSRCS = up_assert.c up_blocktask.c up_copystate.c up_createstack.c \
up_dataabort.c up_mdelay.c up_udelay.c up_exit.c up_idle.c \
up_initialize.c up_initialstate.c up_interruptcontext.c \
+ up_modifyreg8.c up_modifyreg16.c up_modifyreg32.c \
up_prefetchabort.c up_releasepending.c up_releasestack.c \
up_reprioritizertr.c up_schedulesigaction.c \
up_sigdeliver.c up_syscall.c up_unblocktask.c \
@@ -54,5 +55,9 @@ CGU_CSRCS = lpc313x_bcrndx.c lpc313x_clkdomain.c lpc313x_clkexten.c \
CHIP_ASRCS = $(CGU_ASRCS)
CHIP_CSRCS = lpc313x_allocateheap.c lpc313x_boot.c lpc313x_decodeirq.c \
- lpc313x_irq.c lpc313x_lowputc.c lpc313x_serial.c \
- lpc313x_timerisr.c lpc313x_usbdev.c $(CGU_CSRCS)
+ lpc313x_irq.c lpc313x_lowputc.c lpc313x_serial.c lpc313x_i2c.c \
+ lpc313x_spi.c lpc313x_timerisr.c $(CGU_CSRCS)
+
+ifeq ($(CONFIG_USBDEV),y)
+CHIP_CSRCS += lpc313x_usbdev.c
+endif
diff --git a/nuttx/arch/arm/src/lpc313x/lpc313x_adc.h b/nuttx/arch/arm/src/lpc313x/lpc313x_adc.h
index 67f07495b..f86013106 100755
--- a/nuttx/arch/arm/src/lpc313x/lpc313x_adc.h
+++ b/nuttx/arch/arm/src/lpc313x/lpc313x_adc.h
@@ -85,7 +85,7 @@
*/
#define ADC_RX_SHIFT (0) /* Bits 0-9: Digital conversion data */
-#define ADC_RX_MASK (0x3ff << LPC313X_ADC_RX_SHIFT)
+#define ADC_RX_MASK (0x3ff << ADC_RX_SHIFT)
/* ADC_CON, address 0x13002020 */
diff --git a/nuttx/arch/arm/src/lpc313x/lpc313x_evntrtr.h b/nuttx/arch/arm/src/lpc313x/lpc313x_evntrtr.h
index d818e3464..95e6522df 100755
--- a/nuttx/arch/arm/src/lpc313x/lpc313x_evntrtr.h
+++ b/nuttx/arch/arm/src/lpc313x/lpc313x_evntrtr.h
@@ -86,7 +86,7 @@
#define LPC313X_EVNTRTR_CGUWKUPMASK_OFFSET(b) (0x1400+_OB(4,b)) /* cgu_wakeup mask */
#define LPC313X_EVNTRTR_INTOUTMASKCLR_OFFSET(o,b) (0x1800+_OB(o,b)) /* Interrupt output 'o' mask clear */
#define LPC313X_EVNTRTR_CGUWKUPMASKCLR_OFFSET(b) (0x1800+_OB(4,b)) /* cgu_wakeup mask clear */
-#define LPC313X_EVNTRTR_INTOUTPMASKSET_OFFSET(o,b) (0x1c00+_OB(o,b)) /* Interrupt output 'o' mask set */
+#define LPC313X_EVNTRTR_INTOUTMASKSET_OFFSET(o,b) (0x1c00+_OB(o,b)) /* Interrupt output 'o' mask set */
#define LPC313X_EVNTRTR_CGUWKUPMASKSET_OFFSET(b) (0x1c00+_OB(4,b)) /* cgu_wakeup mask set */
/* EVNTRTR register (virtual) addresses *********************************************************************/
@@ -96,7 +96,7 @@
#define LPC313X_EVNTRTR_INTSET(b) (LPC313X_EVNTRTR_VBASE+LPC313X_EVNTRTR_INTSET_OFFSET(b))
#define LPC313X_EVNTRTR_MASK(b) (LPC313X_EVNTRTR_VBASE+LPC313X_EVNTRTR_MASK_OFFSET(b))
#define LPC313X_EVNTRTR_MASKCLR(b) (LPC313X_EVNTRTR_VBASE+LPC313X_EVNTRTR_MASKCLR_OFFSET(b))
-#define LPC313X_EVNTRTR_PEND(b) (LPC313X_EVNTRTR_VBASE+LPC313X_EVNTRTR_PEND_OFFSET(b))
+#define LPC313X_EVNTRTR_MASKSET(b) (LPC313X_EVNTRTR_VBASE+LPC313X_EVNTRTR_MASKSET_OFFSET(b))
#define LPC313X_EVNTRTR_APR(b) (LPC313X_EVNTRTR_VBASE+LPC313X_EVNTRTR_APR_OFFSET(b))
#define LPC313X_EVNTRTR_ATR(b) (LPC313X_EVNTRTR_VBASE+LPC313X_EVNTRTR_ATR_OFFSET(b))
#define LPC313X_EVNTRTR_RSR(b) (LPC313X_EVNTRTR_VBASE+LPC313X_EVNTRTR_RSR_OFFSET(b))
@@ -107,8 +107,8 @@
#define LPC313X_EVNTRTR_CGUWKUPMASK(b) (LPC313X_EVNTRTR_VBASE+LPC313X_EVNTRTR_CGUWKUPMASK_OFFSET(b))
#define LPC313X_EVNTRTR_INTOUTMASKCLR(o,b) (LPC313X_EVNTRTR_VBASE+LPC313X_EVNTRTR_INTOUTMASKCLR_OFFSET(o,b))
#define LPC313X_EVNTRTR_CGUWKUPMASKCLR(b) (LPC313X_EVNTRTR_VBASE+LPC313X_EVNTRTR_CGUWKUPMASKCLR_OFFSET(b))
-#define LPC313X_EVNTRTR_INTOUTPMASKSET(o,b) (LPC313X_EVNTRTR_VBASE+LPC313X_EVNTRTR_INTOUTPMASKSET_OFFSET(o,b))
-#define LPC313X_EVNTRTR_CGUWKUPMASKSET(b) (LPC313X_EVNTRTR_VBASE+LPC313X_EVNTRTR_INTOUTPMASKSET_OFFSET(b)
+#define LPC313X_EVNTRTR_INTOUTMASKSET(o,b) (LPC313X_EVNTRTR_VBASE+LPC313X_EVNTRTR_INTOUTMASKSET_OFFSET(o,b))
+#define LPC313X_EVNTRTR_CGUWKUPMASKSET(b) (LPC313X_EVNTRTR_VBASE+LPC313X_EVNTRTR_CGUWKUPMASKSET_OFFSET(b)
/* EVNTRTR event definitions ********************************************************************************/
/* Bank 0 */
diff --git a/nuttx/arch/arm/src/lpc313x/lpc313x_i2c.c b/nuttx/arch/arm/src/lpc313x/lpc313x_i2c.c
new file mode 100644
index 000000000..0d03390c2
--- /dev/null
+++ b/nuttx/arch/arm/src/lpc313x/lpc313x_i2c.c
@@ -0,0 +1,605 @@
+/*******************************************************************************
+ * arch/arm/src/lpc313x/lpc313x_i2c.c
+ *
+ * Author: David Hewson
+ *
+ * Copyright (C) 2010 Gregory Nutt. All rights reserved.
+ * Author: Gregory Nutt <spudmonkey@racsa.co.cr>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ *******************************************************************************/
+
+/*******************************************************************************
+ * Included Files
+ *******************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <stdlib.h>
+#include <string.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/i2c.h>
+
+#include <arch/irq.h>
+#include <arch/board/board.h>
+
+#include "wdog.h"
+#include "chip.h"
+#include "up_arch.h"
+#include "up_internal.h"
+
+#include "lpc313x_i2c.h"
+#include "lpc313x_evntrtr.h"
+#include "lpc313x_syscreg.h"
+
+/*******************************************************************************
+ * Definitions
+ *******************************************************************************/
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+#define I2C_TIMEOUT ((20 * CLK_TCK) / 1000) /* 20 mS */
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+struct lpc313x_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 clkid; /* Clock for this device */
+ uint16_t rstid; /* Reset for this device */
+ 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 */
+
+ struct i2c_msg_s *msgs; /* remaining transfers - first one is in progress */
+ unsigned int nmsg; /* number of transfer remaining */
+
+ uint16_t header[3]; /* I2C address header */
+ uint16_t hdrcnt; /* number of bytes of header */
+ uint16_t wrcnt; /* number of bytes sent to tx fifo */
+ uint16_t rdcnt; /* number of bytes read from rx fifo */
+};
+
+#define I2C_STATE_DONE 0
+#define I2C_STATE_START 1
+#define I2C_STATE_HEADER 2
+#define I2C_STATE_TRANSFER 3
+
+static struct lpc313x_i2cdev_s i2cdevices[2];
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+static int i2c_interrupt (int irq, FAR void *context);
+static void i2c_progress (struct lpc313x_i2cdev_s *priv);
+static void i2c_timeout (int argc, uint32_t arg, ...);
+static void i2c_reset (struct lpc313x_i2cdev_s *priv);
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * 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);
+static int i2c_write(FAR struct i2c_dev_s *dev, const uint8_t *buffer, int buflen);
+static int i2c_read(FAR struct i2c_dev_s *dev, uint8_t *buffer, int buflen);
+static int i2c_transfer(FAR struct i2c_dev_s *dev, FAR struct i2c_msg_s *msgs, int count);
+
+struct i2c_ops_s lpc313x_i2c_ops = {
+ .setfrequency = i2c_setfrequency,
+ .setaddress = i2c_setaddress,
+ .write = i2c_write,
+ .read = i2c_read,
+#ifdef CONFIG_I2C_TRANSFER
+ .transfer = i2c_transfer
+#endif
+};
+
+/*******************************************************************************
+ * Name: up_i2cinitialize
+ *
+ * Description:
+ * Initialise an I2C device
+ *
+ *******************************************************************************/
+
+struct i2c_dev_s *up_i2cinitialize(int port)
+{
+ struct lpc313x_i2cdev_s *priv = &i2cdevices[port];
+
+ priv->base = (port == 0) ? LPC313X_I2C0_VBASE : LPC313X_I2C1_VBASE;
+ priv->clkid = (port == 0) ? CLKID_I2C0PCLK : CLKID_I2C1PCLK;
+ priv->rstid = (port == 0) ? RESETID_I2C0RST : RESETID_I2C1RST;
+ priv->irqid = (port == 0) ? LPC313X_IRQ_I2C0 : LPC313X_IRQ_I2C1;
+
+ sem_init (&priv->mutex, 0, 1);
+ sem_init (&priv->wait, 0, 0);
+
+ /* Enable I2C system clocks */
+
+ lpc313x_enableclock (priv->clkid);
+
+ /* Reset I2C blocks */
+
+ lpc313x_softreset (priv->rstid);
+
+ /* Soft reset the device */
+
+ i2c_reset (priv);
+
+ /* Allocate a watchdog timer */
+ priv->timeout = wd_create();
+
+ DEBUGASSERT(priv->timeout != 0);
+
+ /* Attach Interrupt Handler */
+ irq_attach (priv->irqid, i2c_interrupt);
+
+ /* Enable Interrupt Handler */
+ up_enable_irq(priv->irqid);
+
+ /* Install our operations */
+ priv->dev.ops = &lpc313x_i2c_ops;
+
+ return &priv->dev;
+}
+
+/*******************************************************************************
+ * Name: up_i2cuninitalize
+ *
+ * Description:
+ * Uninitialise an I2C device
+ *
+ *******************************************************************************/
+
+void up_i2cuninitalize (struct lpc313x_i2cdev_s *priv)
+{
+ /* Disable All Interrupts, soft reset the device */
+
+ i2c_reset (priv);
+
+ /* Detach Interrupt Handler */
+
+ irq_detach (priv->irqid);
+
+ /* Reset I2C blocks */
+
+ lpc313x_softreset (priv->rstid);
+
+ /* Disable I2C system clocks */
+
+ lpc313x_disableclock (priv->clkid);
+}
+
+/*******************************************************************************
+ * Name: lpc313x_i2c_setfrequency
+ *
+ * Description:
+ * Set the frequence for the next transfer
+ *
+ *******************************************************************************/
+
+static uint32_t i2c_setfrequency(FAR struct i2c_dev_s *dev, uint32_t frequency)
+{
+ struct lpc313x_i2cdev_s *priv = (struct lpc313x_i2cdev_s *) dev;
+
+ uint32_t freq = lpc313x_clkfreq (priv->clkid, DOMAINID_AHB0APB1);
+
+ if (freq > 100000)
+ {
+ /* asymetric per 400Khz I2C spec */
+ putreg32 (((47 * freq) / (83 + 47)) / frequency, priv->base + LPC313X_I2C_CLKHI_OFFSET);
+ putreg32 (((83 * freq) / (83 + 47)) / frequency, priv->base + LPC313X_I2C_CLKLO_OFFSET);
+ }
+ else
+ {
+ /* 50/50 mark space ratio */
+ putreg32 (((50 * freq) / 100) / frequency, priv->base + LPC313X_I2C_CLKLO_OFFSET);
+ putreg32 (((50 * freq) / 100) / frequency, priv->base + LPC313X_I2C_CLKHI_OFFSET);
+ }
+}
+
+/*******************************************************************************
+ * Name: lpc313x_i2c_setaddress
+ *
+ * Description:
+ * 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 lpc313x_i2cdev_s *priv = (struct lpc313x_i2cdev_s *) dev;
+
+ DEBUGASSERT(dev != NULL);
+ DEBUGASSERT(nbits == 7 || nbits == 10);
+
+ priv->msg.addr = addr;
+ priv->msg.flags = (nbits == 7) ? 0 : I2C_M_TEN;
+
+ return OK;
+}
+
+/*******************************************************************************
+ * Name: lpc313x_i2c_write
+ *
+ * Description:
+ * Send a block of data on I2C using the previously selected I2C
+ * frequency and slave address.
+ *
+ *******************************************************************************/
+static int i2c_write(FAR struct i2c_dev_s *dev, const uint8_t *buffer, int buflen)
+{
+ struct lpc313x_i2cdev_s *priv = (struct lpc313x_i2cdev_s *) dev;
+ int ret;
+
+ DEBUGASSERT (dev != NULL);
+
+ priv->msg.flags &= ~I2C_M_READ;
+ priv->msg.buffer = buffer;
+ priv->msg.length = buflen;
+
+ ret = i2c_transfer (dev, &priv->msg, 1);
+
+ return ret == 1 ? OK : -ETIMEDOUT;
+}
+
+/*******************************************************************************
+ * Name: lpc313x_i2c_read
+ *
+ * Description:
+ * Receive a block of data on I2C using the previously selected I2C
+ * frequency and slave address.
+ *
+ *******************************************************************************/
+static int i2c_read(FAR struct i2c_dev_s *dev, uint8_t *buffer, int buflen)
+{
+ struct lpc313x_i2cdev_s *priv = (struct lpc313x_i2cdev_s *) dev;
+ int ret;
+
+ DEBUGASSERT (dev != NULL);
+
+ priv->msg.flags |= I2C_M_READ;
+ priv->msg.buffer = buffer;
+ priv->msg.length = buflen;
+
+ ret = i2c_transfer (dev, &priv->msg, 1);
+
+ return ret == 1 ? OK : -ETIMEDOUT;
+}
+
+/*******************************************************************************
+ * Name: i2c_transfer
+ *
+ * Description:
+ * Perform a sequence of I2C transfers
+ *
+ *******************************************************************************/
+
+static int i2c_transfer (FAR struct i2c_dev_s *dev, FAR struct i2c_msg_s *msgs, int count)
+{
+ struct lpc313x_i2cdev_s *priv = (struct lpc313x_i2cdev_s *) dev;
+ irqstate_t flags;
+ int ret;
+
+ sem_wait (&priv->mutex);
+ flags = irqsave();
+
+ priv->state = I2C_STATE_START;
+ priv->msgs = msgs;
+ priv->nmsg = count;
+
+ i2c_progress (priv);
+
+ /* start a watchdog to timeout the transfer if
+ * the bus is locked up... */
+ wd_start (priv->timeout, I2C_TIMEOUT, i2c_timeout, 1, (uint32_t)priv);
+
+ while (priv->state != I2C_STATE_DONE)
+ {
+ sem_wait (&priv->wait);
+ }
+
+ wd_cancel (priv->timeout);
+
+ ret = count - priv->nmsg;
+
+ irqrestore (flags);
+ sem_post (&priv->mutex);
+
+ return ret;
+}
+
+/*******************************************************************************
+ * Name: i2c_interrupt
+ *
+ * Description:
+ * The I2C Interrupt Handler
+ *
+ *******************************************************************************/
+
+static int i2c_interrupt (int irq, FAR void *context)
+{
+ if (irq == LPC313X_IRQ_I2C0)
+ {
+ i2c_progress (&i2cdevices[0]);
+ }
+
+ if (irq == LPC313X_IRQ_I2C1)
+ {
+ i2c_progress (&i2cdevices[1]);
+ }
+
+ return OK;
+}
+
+/*******************************************************************************
+ * Name: i2c_progress
+ *
+ * Description:
+ * Progress any remaining I2C transfers
+ *
+ *******************************************************************************/
+
+static void i2c_progress (struct lpc313x_i2cdev_s *priv)
+{
+ struct i2c_msg_s *msg;
+ uint32_t stat, ctrl;
+
+ stat = getreg32 (priv->base + LPC313X_I2C_STAT_OFFSET);
+
+ /* Were there arbitration problems? */
+ if ((stat & I2C_STAT_AFI) != 0)
+ {
+ /* Perform a soft reset */
+ i2c_reset (priv);
+
+ /* FIXME: automatic retry? */
+
+ priv->state = I2C_STATE_DONE;
+ sem_post (&priv->wait);
+ return;
+ }
+
+ while (priv->nmsg > 0)
+ {
+ ctrl = I2C_CTRL_NAIE | I2C_CTRL_AFIE | I2C_CTRL_TDIE;
+ msg = priv->msgs;
+
+ switch (priv->state)
+ {
+ case I2C_STATE_START:
+ if ((msg->flags & I2C_M_TEN) != 0)
+ {
+ priv->header[0] = I2C_TX_START | 0xF0 | ((msg->addr & 0x300) >> 7);
+ priv->header[1] = msg->addr & 0xFF;
+ priv->hdrcnt = 2;
+ if (msg->flags & I2C_M_READ)
+ {
+ priv->header[2] = priv->header[0] | 1;
+ priv->hdrcnt++;
+ }
+ }
+ else
+ {
+ priv->header[0] = I2C_TX_START | (msg->addr << 1) | (msg->flags & I2C_M_READ);
+ priv->hdrcnt = 1;
+ }
+
+ putreg32 (ctrl, priv->base + LPC313X_I2C_CTRL_OFFSET);
+
+ priv->state = I2C_STATE_HEADER;
+ priv->wrcnt = 0;
+ /* DROP THROUGH */
+
+ case I2C_STATE_HEADER:
+ while ((priv->wrcnt != priv->hdrcnt) && (stat & I2C_STAT_TFF) == 0)
+ {
+ putreg32(priv->header[priv->wrcnt], priv->base + LPC313X_I2C_TX_OFFSET);
+ priv->wrcnt++;
+
+ stat = getreg32 (priv->base + LPC313X_I2C_STAT_OFFSET);
+ }
+
+ if (priv->wrcnt < priv->hdrcnt)
+ {
+ /* Enable Tx FIFO Not Full Interrupt */
+ putreg32 (ctrl | I2C_CTRL_TFFIE, priv->base + LPC313X_I2C_CTRL_OFFSET);
+ goto out;
+ }
+
+ priv->state = I2C_STATE_TRANSFER;
+ priv->wrcnt = 0;
+ priv->rdcnt = 0;
+ /* DROP THROUGH */
+
+ case I2C_STATE_TRANSFER:
+ if (msg->flags & I2C_M_READ)
+ {
+ while ((priv->rdcnt != msg->length) && (stat & I2C_STAT_RFE) == 0)
+ {
+ msg->buffer[priv->rdcnt] = getreg32 (priv->base + LPC313X_I2C_RX_OFFSET);
+ priv->rdcnt++;
+
+ stat = getreg32 (priv->base + LPC313X_I2C_STAT_OFFSET);
+ }
+
+ if (priv->rdcnt < msg->length)
+ {
+ /* Not all data received, fill the Tx FIFO with more dummies */
+ while ((priv->wrcnt != msg->length) && (stat & I2C_STAT_TFF) == 0)
+ {
+ if ((priv->wrcnt + 1) == msg->length && priv->nmsg == 1)
+ putreg32 (I2C_TX_STOP, priv->base + LPC313X_I2C_TX_OFFSET);
+ else
+ putreg32 (0, priv->base + LPC313X_I2C_TX_OFFSET);
+ priv->wrcnt++;
+
+ stat = getreg32 (priv->base + LPC313X_I2C_STAT_OFFSET);
+ }
+
+ if (priv->wrcnt < msg->length)
+ {
+ /* Enable Tx FIFO not full and Rx Fifo Avail Interrupts */
+ putreg32 (ctrl | I2C_CTRL_TFFIE | I2C_CTRL_RFDAIE, priv->base + LPC313X_I2C_CTRL_OFFSET);
+ }
+ else
+ {
+ /* Enable Rx Fifo Avail Interrupts */
+ putreg32 (ctrl | I2C_CTRL_RFDAIE, priv->base + LPC313X_I2C_CTRL_OFFSET);
+ }
+ goto out;
+ }
+ }
+ else /* WRITE */
+ {
+ while ((priv->wrcnt != msg->length) && (stat & I2C_STAT_TFF) == 0)
+ {
+ if ((priv->wrcnt + 1) == msg->length && priv->nmsg == 1)
+ putreg32 (I2C_TX_STOP | msg->buffer[priv->wrcnt], priv->base + LPC313X_I2C_TX_OFFSET);
+ else
+ putreg32 (msg->buffer[priv->wrcnt], priv->base + LPC313X_I2C_TX_OFFSET);
+
+ priv->wrcnt++;
+
+ stat = getreg32 (priv->base + LPC313X_I2C_STAT_OFFSET);
+ }
+
+ if (priv->wrcnt < msg->length)
+ {
+ /* Enable Tx Fifo not full Interrupt */
+ putreg32 (ctrl | I2C_CTRL_TFFIE, priv->base + LPC313X_I2C_CTRL_OFFSET);
+ goto out;
+ }
+ }
+
+ /* Transfer completed, move onto the next one */
+ priv->state = I2C_STATE_START;
+
+ if (--priv->nmsg == 0)
+ {
+ /* Final transfer, wait for Transmit Done Interrupt */
+ putreg32 (ctrl, priv->base + LPC313X_I2C_CTRL_OFFSET);
+ goto out;
+ }
+ priv->msgs++;
+ break;
+ }
+ }
+
+out:
+ if (stat & I2C_STAT_TDI)
+ {
+ putreg32 (I2C_STAT_TDI, priv->base + LPC313X_I2C_STAT_OFFSET);
+
+ /* You'd expect the NAI bit to be set when no acknowledge was
+ * received - but it gets cleared whenever a write it done to
+ * the TXFIFO - so we've gone and cleared it while priming the
+ * rest of the transfer! */
+ if ((stat = getreg32 (priv->base + LPC313X_I2C_TXFL_OFFSET)) != 0)
+ {
+ if (priv->nmsg == 0)
+ priv->nmsg++;
+ i2c_reset (priv);
+ }
+
+ priv->state = I2C_STATE_DONE;
+ sem_post (&priv->wait);
+ }
+}
+
+/*******************************************************************************
+ * Name: i2c_timeout
+ *
+ * Description:
+ * Watchdog timer for timeout of I2C operation
+ *
+ *******************************************************************************/
+
+static void i2c_timeout (int argc, uint32_t arg, ...)
+{
+ struct lpc313x_i2cdev_s *priv = (struct lpc313x_i2cdev_s *) arg;
+
+ irqstate_t flags = irqsave();
+
+ if (priv->state != I2C_STATE_DONE)
+ {
+ /* If there's data remaining in the TXFIFO, then ensure at least
+ * one transfer has failed to complete.. */
+
+ if (getreg32 (priv->base + LPC313X_I2C_TXFL_OFFSET) != 0)
+ {
+ if (priv->nmsg == 0)
+ priv->nmsg++;
+ }
+
+ /* Soft reset the USB controller */
+ i2c_reset (priv);
+
+ /* Mark the transfer as finished */
+ priv->state = I2C_STATE_DONE;
+ sem_post (&priv->wait);
+ }
+
+ irqrestore (flags);
+}
+
+/*******************************************************************************
+ * Name: i2c_reset
+ *
+ * Description:
+ * Perform a soft reset of the I2C controller
+ *
+ *******************************************************************************/
+static void i2c_reset (struct lpc313x_i2cdev_s *priv)
+{
+ putreg32 (I2C_CTRL_RESET, priv->base + LPC313X_I2C_CTRL_OFFSET);
+
+ /* Wait for Reset to complete */
+ while ((getreg32 (priv->base + LPC313X_I2C_CTRL_OFFSET) & I2C_CTRL_RESET) != 0)
+ ;
+}
diff --git a/nuttx/arch/arm/src/lpc313x/lpc313x_ioconfig.h b/nuttx/arch/arm/src/lpc313x/lpc313x_ioconfig.h
index 904079756..58752978d 100755
--- a/nuttx/arch/arm/src/lpc313x/lpc313x_ioconfig.h
+++ b/nuttx/arch/arm/src/lpc313x/lpc313x_ioconfig.h
@@ -76,9 +76,9 @@
#define LPC313X_IOCONFIG_MODE0SET_OFFSET 0x014 /* WR:Set Bits RD:Read Mode 0 */
#define LPC313X_IOCONFIG_MODE0RESET_OFFSET 0x018 /* WR:Reset Bits RD: */
/* 0x01c: Reserved */
-#define LPC313X_IOCONFIG_MODE1_OFFSET 0x010 /* WR:Load RD: */
-#define LPC313X_IOCONFIG_MODE1SET_OFFSET 0x014 /* WR:Set Bits RD:Read Mode 1 */
-#define LPC313X_IOCONFIG_MODE1RESET_OFFSET 0x018 /* WR:Reset Bits RD: */
+#define LPC313X_IOCONFIG_MODE1_OFFSET 0x020 /* WR:Load RD: */
+#define LPC313X_IOCONFIG_MODE1SET_OFFSET 0x024 /* WR:Set Bits RD:Read Mode 1 */
+#define LPC313X_IOCONFIG_MODE1RESET_OFFSET 0x028 /* WR:Reset Bits RD: */
/* 0x02c-0x3c: Reserved */
/* IOCONFIG function block (virtual) base addresses *********************************************/
diff --git a/nuttx/arch/arm/src/lpc313x/lpc313x_spi.c b/nuttx/arch/arm/src/lpc313x/lpc313x_spi.c
new file mode 100644
index 000000000..780969576
--- /dev/null
+++ b/nuttx/arch/arm/src/lpc313x/lpc313x_spi.c
@@ -0,0 +1,781 @@
+/************************************************************************************
+ * arm/arm/src/lpc313x/lpc313x_spi.c
+ *
+ * Copyright (C) 2009 Gregory Nutt. All rights reserved.
+ * Author: Gregory Nutt <spudmonkey@racsa.co.cr>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ************************************************************************************/
+
+/************************************************************************************
+ * Included Files
+ ************************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <semaphore.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/spi.h>
+
+#include <arch/board/board.h>
+
+#include "lpc313x_spi.h"
+#include "lpc313x_ioconfig.h"
+
+/************************************************************************************
+ * Definitions
+ ************************************************************************************/
+
+#define SPI_MAX_DIVIDER 65024 /* = 254 * (255 + 1) */
+#define SPI_MIN_DIVIDER 2
+
+/* Configuration ********************************************************************/
+
+/************************************************************************************
+ * Private Types
+ ************************************************************************************/
+
+struct lpc313x_spidev_s
+{
+ struct spi_dev_s spidev; /* Externally visible part of the SPI interface */
+ sem_t exclsem; /* Held while chip is selected for mutual exclusion */
+ uint32_t frequency; /* Requested clock frequency */
+ uint32_t actual; /* Actual clock frequency */
+ uint8_t nbits; /* Width of work in bits (8 or 16) */
+ uint8_t mode; /* Mode 0,1,2,3 */
+
+ uint32_t slv1;
+ uint32_t slv2;
+};
+
+/************************************************************************************
+ * Private Function Prototypes
+ ************************************************************************************/
+static inline void spi_drive_cs(FAR struct lpc313x_spidev_s *priv, uint8_t slave, uint8_t val);
+static inline void spi_select_slave(FAR struct lpc313x_spidev_s *priv, uint8_t slave);
+static inline uint16_t spi_readword(FAR struct lpc313x_spidev_s *priv);
+static inline void spi_writeword(FAR struct lpc313x_spidev_s *priv, uint16_t word);
+
+static int spi_lock(FAR struct spi_dev_s *dev, bool lock);
+static void spi_select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected);
+static uint32_t spi_setfrequency(FAR struct spi_dev_s *dev, uint32_t frequency);
+static void spi_setmode(FAR struct spi_dev_s *dev, enum spi_mode_e mode);
+static void spi_setbits(FAR struct spi_dev_s *dev, int nbits);
+static uint8_t spi_status(FAR struct spi_dev_s *dev, enum spi_dev_e devid);
+static uint16_t spi_send(FAR struct spi_dev_s *dev, uint16_t wd);
+static void spi_exchange(FAR struct spi_dev_s *dev, FAR const void *txbuffer,
+ FAR void *rxbuffer, size_t nwords);
+#ifndef CONFIG_SPI_EXCHANGE
+static void spi_sndblock(FAR struct spi_dev_s *dev, FAR const void *txbuffer,
+ size_t nwords);
+static void spi_recvblock(FAR struct spi_dev_s *dev, FAR void *rxbuffer,
+ size_t nwords);
+#endif
+
+
+/************************************************************************************
+ * Private Data
+ ************************************************************************************/
+
+static const struct spi_ops_s g_spiops =
+{
+ .lock = spi_lock,
+ .select = spi_select,
+ .setfrequency = spi_setfrequency,
+ .setmode = spi_setmode,
+ .setbits = spi_setbits,
+ .status = spi_status,
+ .send = spi_send,
+#ifdef CONFIG_SPI_EXCHANGE
+ .exchange = spi_exchange,
+#else
+ .sndblock = spi_sndblock,
+ .recvblock = spi_recvblock,
+#endif
+ .registercallback = 0,
+};
+
+static struct lpc313x_spidev_s g_spidev =
+{
+ .spidev = { &g_spiops },
+};
+
+/************************************************************************************
+ * Public Data
+ ************************************************************************************/
+
+/************************************************************************************
+ * Private Functions
+ ************************************************************************************/
+
+/****************************************************************************
+ * Name: spi_drive_cs
+ *
+ * Description:
+ * Drive the chip select signal for this slave
+ *
+ * Input Parameters:
+ * dev - Device-specific state data
+ * slave - slave id
+ * value - value (0 for assert)
+ *
+ * Returned Value:
+ * None
+ *
+ ****************************************************************************/
+
+static inline void spi_drive_cs(FAR struct lpc313x_spidev_s *priv, uint8_t slave, uint8_t val)
+{
+ switch (slave)
+ {
+ case 0:
+ if (val == 0)
+ putreg32 (IOCONFIG_SPI_CSOUT0, LPC313X_IOCONFIG_SPI_MODE0RESET);
+ else
+ putreg32 (IOCONFIG_SPI_CSOUT0, LPC313X_IOCONFIG_SPI_MODE0SET);
+ putreg32 (IOCONFIG_SPI_CSOUT0, LPC313X_IOCONFIG_SPI_MODE1SET);
+ break;
+
+ case 1:
+ if (val == 0)
+ putreg32 (IOCONFIG_EBII2STX0_MUARTCTSN, LPC313X_IOCONFIG_EBII2STX0_MODE0RESET);
+ else
+ putreg32 (IOCONFIG_EBII2STX0_MUARTCTSN, LPC313X_IOCONFIG_EBII2STX0_MODE0SET);
+ putreg32 (IOCONFIG_EBII2STX0_MUARTCTSN, LPC313X_IOCONFIG_EBII2STX0_MODE1SET);
+ break;
+
+ case 2:
+ if (val == 0)
+ putreg32 (IOCONFIG_EBII2STX0_MUARTRTSN, LPC313X_IOCONFIG_EBII2STX0_MODE0RESET);
+ else
+ putreg32 (IOCONFIG_EBII2STX0_MUARTRTSN, LPC313X_IOCONFIG_EBII2STX0_MODE0SET);
+ putreg32 (IOCONFIG_EBII2STX0_MUARTRTSN, LPC313X_IOCONFIG_EBII2STX0_MODE1SET);
+ break;
+ }
+}
+
+/****************************************************************************
+ * Name: spi_select_slave
+ *
+ * Description:
+ * Select the slave device for the next transfer
+ *
+ * Input Parameters:
+ * dev - Device-specific state data
+ * slave - slave id
+ *
+ * Returned Value:
+ * None
+ *
+ ****************************************************************************/
+
+static inline void spi_select_slave(FAR struct lpc313x_spidev_s *priv, uint8_t slave)
+{
+ switch (slave)
+ {
+ case 0:
+ putreg32 (priv->slv1, LPC313X_SPI_SLV0_1);
+ putreg32 (priv->slv2, LPC313X_SPI_SLV0_2);
+ putreg32 (SPI_SLVENABLE1_ENABLED, LPC313X_SPI_SLVENABLE);
+ break;
+
+ case 1:
+ putreg32 (priv->slv1, LPC313X_SPI_SLV1_1);
+ putreg32 (priv->slv2, LPC313X_SPI_SLV1_2);
+ putreg32 (SPI_SLVENABLE2_ENABLED, LPC313X_SPI_SLVENABLE);
+
+ case 2:
+ putreg32 (priv->slv1, LPC313X_SPI_SLV2_1);
+ putreg32 (priv->slv2, LPC313X_SPI_SLV2_2);
+ putreg32 (SPI_SLVENABLE3_ENABLED, LPC313X_SPI_SLVENABLE);
+ break;
+ }
+}
+
+/************************************************************************************
+ * Name: spi_readword
+ *
+ * Description:
+ * Read one byte from SPI
+ *
+ * Input Parameters:
+ * priv - Device-specific state data
+ *
+ * Returned Value:
+ * Byte as read
+ *
+ ************************************************************************************/
+
+static inline uint16_t spi_readword(FAR struct lpc313x_spidev_s *priv)
+{
+ /* Wait until the receive buffer is not empty */
+ while ((getreg32 (LPC313X_SPI_STATUS) & SPI_STATUS_RXFIFOEMPTY) != 0)
+ ;
+
+ /* Then return the received byte */
+ uint32_t val = getreg32 (LPC313X_SPI_FIFODATA);
+
+ return val;
+}
+
+/************************************************************************************
+ * Name: spi_writeword
+ *
+ * Description:
+ * Write one byte to SPI
+ *
+ * Input Parameters:
+ * priv - Device-specific state data
+ * byte - Byte to send
+ *
+ * Returned Value:
+ * None
+ *
+ ************************************************************************************/
+
+static inline void spi_writeword(FAR struct lpc313x_spidev_s *priv, uint16_t word)
+{
+ /* Wait until the transmit buffer is empty */
+
+ while ((getreg32 (LPC313X_SPI_STATUS) & SPI_STATUS_TXFIFOFULL) != 0)
+ ;
+
+ /* Then send the byte */
+
+ putreg32 (word, LPC313X_SPI_FIFODATA);
+}
+
+/****************************************************************************
+ * Name: spi_lock
+ *
+ * Description:
+ * On SPI busses where there are multiple devices, it will be necessary to
+ * lock SPI to have exclusive access to the busses for a sequence of
+ * transfers. The bus should be locked before the chip is selected. After
+ * locking the SPI bus, the caller should then also call the setfrequency,
+ * setbits, and setmode methods to make sure that the SPI is properly
+ * configured for the device. If the SPI buss is being shared, then it
+ * may have been left in an incompatible state.
+ *
+ * Input Parameters:
+ * dev - Device-specific state data
+ * lock - true: Lock spi bus, false: unlock SPI bus
+ *
+ * Returned Value:
+ * None
+ *
+ ****************************************************************************/
+
+static int spi_lock(FAR struct spi_dev_s *dev, bool lock)
+{
+ FAR struct lpc313x_spidev_s *priv = (FAR struct lpc313x_spidev_s *)dev;
+
+ if (lock)
+ {
+ /* Take the semaphore (perhaps waiting) */
+
+ while (sem_wait(&priv->exclsem) != 0)
+ {
+ /* The only case that an error should occur here is if the wait was awakened
+ * by a signal.
+ */
+
+ ASSERT(errno == EINTR);
+ }
+ }
+ else
+ {
+ (void)sem_post(&priv->exclsem);
+ }
+ return OK;
+}
+
+/****************************************************************************
+ * Name: spi_select
+ *
+ * Description:
+ * Enable/disable the SPI slave select. The implementation of this method
+ * must include handshaking: If a device is selected, it must hold off
+ * all other attempts to select the device until the device is deselecte.
+ *
+ * Input Parameters:
+ * dev - Device-specific state data
+ * devid - Identifies the device to select
+ * selected - true: slave selected, false: slave de-selected
+ *
+ * Returned Value:
+ * None
+ *
+ ****************************************************************************/
+
+static void spi_select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
+{
+ struct lpc313x_spidev_s *priv = (struct lpc313x_spidev_s *) dev;
+ uint8_t slave = 0;
+
+ /* FIXME: map the devid to the SPI slave - this should really
+ * be in board specific code..... */
+ switch (devid)
+ {
+ case SPIDEV_FLASH:
+ slave = 0;
+ break;
+ case SPIDEV_MMCSD:
+ slave = 1;
+ break;
+ case SPIDEV_ETHERNET:
+ slave = 2;
+ break;
+ default:
+ return;
+ }
+
+ /*
+ * Since we don't use sequential multi-slave mode, but rather
+ * perform the transfer piecemeal by consecutive calls to
+ * SPI_SEND, then we must manually assert the chip select
+ * across the whole transfer
+ */
+
+ if (selected)
+ {
+ spi_drive_cs (priv, slave, 0);
+ spi_select_slave (priv, slave);
+
+ /* Enable SPI as master and notify of slave enables change */
+
+ putreg32 ((1 << SPI_CONFIG_INTERSLVDELAY_SHIFT) | SPI_CONFIG_UPDENABLE | SPI_CONFIG_SPIENABLE, LPC313X_SPI_CONFIG);
+ }
+ else
+ {
+ spi_drive_cs (priv, slave, 1);
+
+ /* Disable all slaves */
+
+ putreg32 (0, LPC313X_SPI_SLVENABLE);
+
+ /* Disable SPI as master */
+
+ putreg32 (SPI_CONFIG_UPDENABLE, LPC313X_SPI_CONFIG);
+ }
+}
+
+/************************************************************************************
+ * Name: spi_setfrequency
+ *
+ * Description:
+ * Set the SPI frequency.
+ *
+ * Input Parameters:
+ * dev - Device-specific state data
+ * frequency - The SPI frequency requested
+ *
+ * Returned Value:
+ * Returns the actual frequency selected
+ *
+ ************************************************************************************/
+
+static uint32_t spi_setfrequency(FAR struct spi_dev_s *dev, uint32_t frequency)
+{
+ FAR struct lpc313x_spidev_s *priv = (FAR struct lpc313x_spidev_s *)dev;
+ uint32_t spi_clk, div, div1, div2;
+
+ if (priv->frequency != frequency)
+ {
+ /* The SPI clock is derived from the (main system oscillator / 2),
+ * so compute the best divider from that clock */
+
+ spi_clk = lpc313x_clkfreq (CLKID_SPICLK, DOMAINID_SPI);
+
+ /* Find closest divider to get at or under the target frequency */
+
+ div = (spi_clk + frequency / 2) / frequency;
+
+ if (div > SPI_MAX_DIVIDER)
+ div = SPI_MAX_DIVIDER;
+ if (div < SPI_MIN_DIVIDER)
+ div = SPI_MIN_DIVIDER;
+
+ div2 = (((div-1) / 512) + 2) * 2;
+ div1 = ((((div + div2 / 2) / div2) - 1));
+
+ priv->slv1 = (priv->slv1 & ~(SPI_SLV_1_CLKDIV2_MASK | SPI_SLV_1_CLKDIV1_MASK)) | (div2 << SPI_SLV_1_CLKDIV2_SHIFT) | (div1 << SPI_SLV_1_CLKDIV1_SHIFT);
+
+ priv->frequency = frequency;
+ priv->actual = frequency; // FIXME
+ }
+
+ return priv->actual;
+}
+
+/************************************************************************************
+ * Name: spi_setmode
+ *
+ * Description:
+ * Set the SPI mode. see enum spi_mode_e for mode definitions
+ *
+ * Input Parameters:
+ * dev - Device-specific state data
+ * mode - The SPI mode requested
+ *
+ * Returned Value:
+ * Returns the actual frequency selected
+ *
+ ************************************************************************************/
+
+static void spi_setmode(FAR struct spi_dev_s *dev, enum spi_mode_e mode)
+{
+ FAR struct lpc313x_spidev_s *priv = (FAR struct lpc313x_spidev_s *)dev;
+ uint16_t setbits;
+ uint16_t clrbits;
+
+/* Has the mode changed? */
+
+ if (mode != priv->mode)
+ {
+ /* Yes... Set CR1 appropriately */
+
+ switch (mode)
+ {
+ case SPIDEV_MODE0: /* SPO=0; SPH=0 */
+ setbits = 0;
+ clrbits = SPI_SLV_2_SPO|SPI_SLV_2_SPH;
+ break;
+
+ case SPIDEV_MODE1: /* SPO=0; SPH=1 */
+ setbits = SPI_SLV_2_SPH;
+ clrbits = SPI_SLV_2_SPO;
+ break;
+
+ case SPIDEV_MODE2: /* SPO=1; SPH=0 */
+ setbits = SPI_SLV_2_SPO;
+ clrbits = SPI_SLV_2_SPH;
+ break;
+
+ case SPIDEV_MODE3: /* SPO=1; SPH=1 */
+ setbits = SPI_SLV_2_SPO|SPI_SLV_2_SPH;
+ clrbits = 0;
+ break;
+
+ default:
+ return;
+ }
+
+ priv->slv2 = (priv->slv2 & ~clrbits) | setbits;
+ priv->mode = mode;
+ }
+}
+
+/************************************************************************************
+ * Name: spi_setbits
+ *
+ * Description:
+ * Set the number of bits per word.
+ *
+ * Input Parameters:
+ * dev - Device-specific state data
+ * nbits - The number of bits requested
+ *
+ * Returned Value:
+ * None
+ *
+ ************************************************************************************/
+
+static void spi_setbits(FAR struct spi_dev_s *dev, int nbits)
+{
+ FAR struct lpc313x_spidev_s *priv = (FAR struct lpc313x_spidev_s *)dev;
+
+ /* Has the number of bits changed? */
+
+ if (nbits != priv->nbits)
+ {
+ priv->slv2 = (priv->slv2 & ~SPI_SLV_2_WDSIZE_MASK) | ((nbits-1) << SPI_SLV_2_WDSIZE_SHIFT);
+ priv->nbits = nbits;
+ }
+}
+
+/****************************************************************************
+ * Name: spi_status
+ *
+ * Description:
+ * Get SPI/MMC status
+ *
+ * Input Parameters:
+ * dev - Device-specific state data
+ * devid - Identifies the device to report status on
+ *
+ * Returned Value:
+ * Returns a bitset of status values (see SPI_STATUS_* defines
+ *
+ ****************************************************************************/
+
+static uint8_t spi_status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)
+{
+ /* FIXME: is there anyway to determine this
+ * it should probably be board dependant anyway */
+
+ return SPI_STATUS_PRESENT;
+}
+
+/************************************************************************************
+ * Name: spi_send
+ *
+ * Description:
+ * Exchange one word on SPI
+ *
+ * Input Parameters:
+ * dev - Device-specific state data
+ * wd - The word to send. the size of the data is determined by the
+ * number of bits selected for the SPI interface.
+ *
+ * Returned Value:
+ * response
+ *
+ ************************************************************************************/
+
+static uint16_t spi_send(FAR struct spi_dev_s *dev, uint16_t wd)
+{
+ FAR struct lpc313x_spidev_s *priv = (FAR struct lpc313x_spidev_s *)dev;
+
+ DEBUGASSERT(priv && priv->spibase);
+
+ spi_writeword(priv, wd);
+ return spi_readword(priv);
+}
+
+/*************************************************************************
+ * Name: spi_exchange
+ *
+ * Description:
+ * Exchange a block of data on SPI
+ *
+ * Input Parameters:
+ * dev - Device-specific state data
+ * txbuffer - A pointer to the buffer of data to be sent
+ * rxbuffer - A pointer to a buffer in which to receive data
+ * nwords - the length of data to be exchaned in units of words.
+ * The wordsize is determined by the number of bits-per-word
+ * selected for the SPI interface. If nbits <= 8, the data is
+ * packed into uint8_t's; if nbits >8, the data is packed into uint16_t's
+ *
+ * Returned Value:
+ * None
+ *
+ ************************************************************************************/
+
+static void spi_exchange(FAR struct spi_dev_s *dev, FAR const void *txbuffer,
+ FAR void *rxbuffer, size_t nwords)
+{
+ FAR struct lpc313x_spidev_s *priv = (FAR struct lpc313x_spidev_s *)dev;
+ DEBUGASSERT(priv);
+
+ /* 8- or 16-bit mode? */
+
+ if (priv->nbits == 16)
+ {
+ /* 16-bit mode */
+
+ const uint16_t *src = (const uint16_t*)txbuffer;;
+ uint16_t *dest = (uint16_t*)rxbuffer;
+ uint16_t word;
+
+ while (nwords-- > 0)
+ {
+ /* Get the next word to write. Is there a source buffer? */
+
+ word = src ? *src++ : 0xffff;
+
+ /* Exchange one word */
+
+ word = spi_send(dev, word);
+
+ /* Is there a buffer to receive the return value? */
+
+ if (dest)
+ {
+ *dest++ = word;
+ }
+ }
+ }
+ else
+ {
+ /* 8-bit mode */
+
+ const uint8_t *src = (const uint8_t*)txbuffer;;
+ uint8_t *dest = (uint8_t*)rxbuffer;
+ uint8_t word;
+
+ while (nwords-- > 0)
+ {
+ /* Get the next word to write. Is there a source buffer? */
+
+ word = src ? *src++ : 0xff;
+
+ /* Exchange one word */
+
+ word = (uint8_t)spi_send(dev, (uint16_t)word);
+
+ /* Is there a buffer to receive the return value? */
+
+ if (dest)
+ {
+ *dest++ = word;
+ }
+ }
+ }
+}
+
+/*************************************************************************
+ * Name: spi_sndblock
+ *
+ * Description:
+ * Send a block of data on SPI
+ *
+ * Input Parameters:
+ * dev - Device-specific state data
+ * txbuffer - A pointer to the buffer of data to be sent
+ * nwords - the length of data to send from the buffer in number of words.
+ * The wordsize is determined by the number of bits-per-word
+ * selected for the SPI interface. If nbits <= 8, the data is
+ * packed into uint8_t's; if nbits >8, the data is packed into uint16_t's
+ *
+ * Returned Value:
+ * None
+ *
+ ************************************************************************************/
+
+#ifndef CONFIG_SPI_EXCHANGE
+static void spi_sndblock(FAR struct spi_dev_s *dev, FAR const void *txbuffer, size_t nwords)
+{
+ return spi_exchange(dev, txbuffer, NULL, nwords);
+}
+#endif
+
+/************************************************************************************
+ * Name: spi_recvblock
+ *
+ * Description:
+ * Revice a block of data from SPI
+ *
+ * Input Parameters:
+ * dev - Device-specific state data
+ * rxbuffer - A pointer to the buffer in which to recieve data
+ * nwords - the length of data that can be received in the buffer in number
+ * of words. The wordsize is determined by the number of bits-per-word
+ * selected for the SPI interface. If nbits <= 8, the data is
+ * packed into uint8_t's; if nbits >8, the data is packed into uint16_t's
+ *
+ * Returned Value:
+ * None
+ *
+ ************************************************************************************/
+
+#ifndef CONFIG_SPI_EXCHANGE
+static void spi_recvblock(FAR struct spi_dev_s *dev, FAR void *rxbuffer, size_t nwords)
+{
+ return spi_exchange(dev, NULL, rxbuffer, nwords);
+}
+#endif
+
+/************************************************************************************
+ * Public Functions
+ ************************************************************************************/
+
+/************************************************************************************
+ * Name: up_spiinitialize
+ *
+ * Description:
+ * Initialize the selected SPI port
+ *
+ * Input Parameter:
+ * Port number (for hardware that has mutiple SPI interfaces)
+ *
+ * Returned Value:
+ * Valid SPI device structure reference on succcess; a NULL on failure
+ *
+ ************************************************************************************/
+
+FAR struct spi_dev_s *up_spiinitialize(int port)
+{
+ FAR struct lpc313x_spidev_s *priv = &g_spidev;
+
+ /* Only the SPI0 interface is supported */
+ if (port != 0)
+ {
+ return NULL;
+ }
+
+ /* Enable SPI clocks */
+
+ lpc313x_enableclock (CLKID_SPIPCLK);
+ lpc313x_enableclock (CLKID_SPIPCLKGATED);
+ lpc313x_enableclock (CLKID_SPICLK);
+ lpc313x_enableclock (CLKID_SPICLKGATED);
+
+ /* Soft Reset the module */
+
+ lpc313x_softreset (RESETID_SPIRSTAPB);
+ lpc313x_softreset (RESETID_SPIRSTIP);
+
+ /* Initialize the SPI semaphore that enforces mutually exclusive access */
+
+ sem_init(&priv->exclsem, 0, 1);
+
+ /* Reset the SPI block */
+
+ putreg32 (SPI_CONFIG_SOFTRST, LPC313X_SPI_CONFIG);
+
+ /* Initialise Slave 0 settings registers */
+
+ priv->slv1 = 0;
+ priv->slv2 = 0;
+
+ /* Configure initial default mode */
+
+ priv->mode = SPIDEV_MODE1;
+ spi_setmode (&priv->spidev, SPIDEV_MODE0);
+
+ /* Configure word width */
+
+ priv->nbits = 0;
+ spi_setbits (&priv->spidev, 8);
+
+ /* Select a default frequency of approx. 400KHz */
+
+ priv->frequency = 0;
+ spi_setfrequency(&priv->spidev, 400000);
+
+ return (FAR struct spi_dev_s *)priv;
+}
diff --git a/nuttx/arch/arm/src/lpc313x/lpc313x_spi.h b/nuttx/arch/arm/src/lpc313x/lpc313x_spi.h
index 15976bf5c..e9a5f3550 100755
--- a/nuttx/arch/arm/src/lpc313x/lpc313x_spi.h
+++ b/nuttx/arch/arm/src/lpc313x/lpc313x_spi.h
@@ -143,7 +143,7 @@
# define SPI_SLVENABLE2_DISABLED (0 << SPI_SLVENABLE2_SHIFT) /* Disabled */
# define SPI_SLVENABLE2_ENABLED (1 << SPI_SLVENABLE2_SHIFT) /* Enabled */
# define SPI_SLVENABLE2_SUSPENDED (3 << SPI_SLVENABLE2_SHIFT) /* Suspended */
-#define SPI_SLVENABLE1_SHIFT (0) /* Bits 0-1: Slave 3 enable bits */
+#define SPI_SLVENABLE1_SHIFT (0) /* Bits 0-1: Slave 1 enable bits */
#define SPI_SLVENABLE1_MASK (3 << SPI_SLVENABLE1_SHIFT)
# define SPI_SLVENABLE1_DISABLED (0 << SPI_SLVENABLE1_SHIFT) /* Disabled */
# define SPI_SLVENABLE1_ENABLED (1 << SPI_SLVENABLE1_SHIFT) /* Enabled */
diff --git a/nuttx/arch/arm/src/lpc313x/lpc313x_usbdev.c b/nuttx/arch/arm/src/lpc313x/lpc313x_usbdev.c
index 29c9a1122..323648fa9 100755
--- a/nuttx/arch/arm/src/lpc313x/lpc313x_usbdev.c
+++ b/nuttx/arch/arm/src/lpc313x/lpc313x_usbdev.c
@@ -282,7 +282,6 @@ struct lpc313x_ep_s
struct lpc313x_req_s *tail;
uint8_t epphy; /* Physical EP address */
uint8_t stalled:1; /* 1: Endpoint is stalled */
- uint8_t halted:1; /* 1: Endpoint feature halted */
};
/* This structure retains the state of the USB device controller */
@@ -364,12 +363,11 @@ static void lpc313x_readsetup(uint8_t epphy, struct usb_ctrlreq_s *ctrl);
static inline void lpc313x_set_address(struct lpc313x_usbdev_s *priv, uint16_t address);
static void lpc313x_flushep(struct lpc313x_ep_s *privep);
-static inline bool lpc313x_epstalled(struct lpc313x_ep_s *privep);
static int lpc313x_progressep(struct lpc313x_ep_s *privep);
static inline void lpc313x_abortrequest(struct lpc313x_ep_s *privep,
struct lpc313x_req_s *privreq, int16_t result);
-static void lpc313x_reqcomplete(struct lpc313x_ep_s *privep, int16_t result);
+static void lpc313x_reqcomplete(struct lpc313x_ep_s *privep, struct lpc313x_req_s *privreq, int16_t result);
static void lpc313x_cancelrequests(struct lpc313x_ep_s *privep, int16_t status);
@@ -769,24 +767,6 @@ static void lpc313x_flushep(struct lpc313x_ep_s *privep)
/*******************************************************************************
- * Name: lpc313x_epstalled
- *
- * Description:
- * Return whether the endpoint is stalled or not
- *
- *******************************************************************************/
-
-static inline bool lpc313x_epstalled(struct lpc313x_ep_s *privep)
-{
- uint32_t ctrl = lpc313x_getreg (LPC313X_USBDEV_ENDPTCTRL(privep->epphy));
-
- if (LPC313X_EPPHYIN(privep->epphy))
- return (ctrl & USBDEV_ENDPTCTRL_TXS);
- else
- return (ctrl & USBDEV_ENDPTCTRL_RXS);
-}
-
-/*******************************************************************************
* Name: lpc313x_progressep
*
* Description:
@@ -825,7 +805,7 @@ static int lpc313x_progressep(struct lpc313x_ep_s *privep)
usbtrace(TRACE_DEVERROR(LPC313X_TRACEERR_EPOUTNULLPACKET), 0);
}
- lpc313x_reqcomplete(privep, OK);
+ lpc313x_reqcomplete(privep, lpc313x_rqdequeue(privep), OK);
return OK;
}
@@ -836,15 +816,13 @@ static int lpc313x_progressep(struct lpc313x_ep_s *privep)
int bytesleft = privreq->req.len - privreq->req.xfrd;
- if (bytesleft > privep->ep.maxpacket)
- bytesleft = privep->ep.maxpacket;
-
if (LPC313X_EPPHYIN(privep->epphy))
usbtrace(TRACE_WRITE(privep->epphy), privreq->req.xfrd);
else
usbtrace(TRACE_READ(privep->epphy), privreq->req.xfrd);
/* Initialise the DTD to transfer the next chunk */
+
lpc313x_writedtd (dtd, privreq->req.buf + privreq->req.xfrd, bytesleft);
/* then queue onto the DQH */
@@ -884,39 +862,27 @@ static inline void lpc313x_abortrequest(struct lpc313x_ep_s *privep,
*
*******************************************************************************/
-static void lpc313x_reqcomplete(struct lpc313x_ep_s *privep, int16_t result)
+static void lpc313x_reqcomplete(struct lpc313x_ep_s *privep, struct lpc313x_req_s *privreq, int16_t result)
{
- struct lpc313x_req_s *privreq;
- irqstate_t flags;
-
- /* Remove the completed request at the head of the endpoint request list */
-
- flags = irqsave();
- privreq = lpc313x_rqdequeue(privep);
- irqrestore(flags);
-
- if (privreq)
- {
- /* If endpoint 0, temporarily reflect the state of protocol stalled
- * in the callback.
- */
+ /* If endpoint 0, temporarily reflect the state of protocol stalled
+ * in the callback.
+ */
- bool stalled = privep->stalled;
- if (privep->epphy == LPC313X_EP0_IN)
- privep->stalled = privep->dev->stalled;
+ bool stalled = privep->stalled;
+ if (privep->epphy == LPC313X_EP0_IN)
+ privep->stalled = privep->dev->stalled;
- /* Save the result in the request structure */
+ /* Save the result in the request structure */
- privreq->req.result = result;
+ privreq->req.result = result;
- /* Callback to the request completion handler */
+ /* Callback to the request completion handler */
- privreq->req.callback(&privep->ep, &privreq->req);
+ privreq->req.callback(&privep->ep, &privreq->req);
- /* Restore the stalled indication */
+ /* Restore the stalled indication */
- privep->stalled = stalled;
- }
+ privep->stalled = stalled;
}
/*******************************************************************************
@@ -938,7 +904,7 @@ static void lpc313x_cancelrequests(struct lpc313x_ep_s *privep, int16_t status)
// FIXME: only report the error status if the transfer hasn't completed
usbtrace(TRACE_COMPLETE(privep->epphy),
(lpc313x_rqpeek(privep))->req.xfrd);
- lpc313x_reqcomplete(privep, status);
+ lpc313x_reqcomplete(privep, lpc313x_rqdequeue(privep), status);
}
}
@@ -1083,7 +1049,6 @@ static void lpc313x_usbreset(struct lpc313x_usbdev_s *priv)
/* Reset endpoint status */
privep->stalled = false;
- privep->halted = false;
}
/* Tell the class driver that we are disconnected. The class
@@ -1221,7 +1186,7 @@ static inline void lpc313x_ep0setup(struct lpc313x_usbdev_s *priv)
}
else
{
- if (lpc313x_epstalled(privep))
+ if (privep->stalled)
priv->ep0buf[0] = 1; /* Stalled */
else
priv->ep0buf[0] = 0; /* Not stalled */
@@ -1294,8 +1259,6 @@ static inline void lpc313x_ep0setup(struct lpc313x_usbdev_s *priv)
else if (priv->paddrset != 0 && value == USB_FEATURE_ENDPOINTHALT && len == 0 &&
(privep = lpc313x_epfindbyaddr(priv, index)) != NULL)
{
- privep->halted = 0;
-
lpc313x_epstall(&privep->ep, true);
lpc313x_ep0state (priv, EP0STATE_WAIT_NAK_IN);
@@ -1329,8 +1292,6 @@ static inline void lpc313x_ep0setup(struct lpc313x_usbdev_s *priv)
else if (priv->paddrset != 0 && value == USB_FEATURE_ENDPOINTHALT && len == 0 &&
(privep = lpc313x_epfindbyaddr(priv, index)) != NULL)
{
- privep->halted = 1;
-
lpc313x_epstall(&privep->ep, false);
lpc313x_ep0state (priv, EP0STATE_WAIT_NAK_IN);
@@ -1623,13 +1584,11 @@ lpc313x_epcomplete(struct lpc313x_usbdev_s *priv, uint8_t epphy)
privreq->req.xfrd += xfrd;
- bool complete;
+ bool complete = true;
if (LPC313X_EPPHYOUT(privep->epphy))
{
/* read(OUT) completes when request filled, or a short transfer is received */
- complete = (privreq->req.xfrd >= privreq->req.len || xfrd < privep->ep.maxpacket);
-
usbtrace(TRACE_INTDECODE(LPC313X_TRACEINTID_EPIN), complete);
}
else
@@ -1643,15 +1602,25 @@ lpc313x_epcomplete(struct lpc313x_usbdev_s *priv, uint8_t epphy)
usbtrace(TRACE_INTDECODE(LPC313X_TRACEINTID_EPOUT), complete);
}
+ /* If the transfer is complete, then dequeue and progress any further queued requests */
+
if (complete)
{
- usbtrace(TRACE_COMPLETE(privep->epphy), privreq->req.xfrd);
- lpc313x_reqcomplete(privep, OK);
+ privreq = lpc313x_rqdequeue (privep);
}
- /* If there's more requests, then progress them */
if (!lpc313x_rqempty(privep))
- lpc313x_progressep(privep);
+ {
+ lpc313x_progressep(privep);
+ }
+
+ /* Now it's safe to call the completion callback as it may well submit a new request */
+
+ if (complete)
+ {
+ usbtrace(TRACE_COMPLETE(privep->epphy), privreq->req.xfrd);
+ lpc313x_reqcomplete(privep, privreq, OK);
+ }
return complete;
}
@@ -1863,6 +1832,9 @@ static int lpc313x_epconfigure(FAR struct usbdev_ep_s *ep,
lpc313x_chgbits (0x0000FFFF, cfg, LPC313X_USBDEV_ENDPTCTRL(privep->epphy));
}
+ /* Reset endpoint status */
+ privep->stalled = false;
+
/* Enable the endpoint */
if (LPC313X_EPPHYIN(privep->epphy))
lpc313x_setbits (USBDEV_ENDPTCTRL_TXE, LPC313X_USBDEV_ENDPTCTRL(privep->epphy));
@@ -1902,11 +1874,11 @@ static int lpc313x_epdisable(FAR struct usbdev_ep_s *ep)
else
lpc313x_clrbits (USBDEV_ENDPTCTRL_RXE, LPC313X_USBDEV_ENDPTCTRL(privep->epphy));
+ privep->stalled = true;
+
/* Cancel any ongoing activity */
lpc313x_cancelrequests(privep, -ESHUTDOWN);
- privep->halted = 1;
-
irqrestore(flags);
return OK;
}
@@ -2048,12 +2020,8 @@ static int lpc313x_epsubmit(FAR struct usbdev_ep_s *ep, FAR struct usbdev_req_s
if (privep->stalled)
{
- lpc313x_abortrequest(privep, privreq, -EBUSY);
ret = -EBUSY;
}
-
- /* Handle IN (device-to-host) requests */
-
else
{
/* Add the new request to the request queue for the endpoint */
@@ -2061,10 +2029,12 @@ static int lpc313x_epsubmit(FAR struct usbdev_ep_s *ep, FAR struct usbdev_req_s
if (LPC313X_EPPHYIN(privep->epphy))
usbtrace(TRACE_INREQQUEUED(privep->epphy), privreq->req.len);
else
- usbtrace(TRACE_OUTREQQUEUED(privep->epphy), privreq->req.len);
+ usbtrace(TRACE_OUTREQQUEUED(privep->epphy), privreq->req.len);
if (lpc313x_rqenqueue(privep, privreq))
+ {
lpc313x_progressep(privep);
+ }
}
irqrestore(flags);
@@ -2676,3 +2646,4 @@ int usbdev_unregister(struct usbdevclass_driver_s *driver)
g_usbdev.driver = NULL;
return OK;
}
+