From 4f56a72bea99bc7c58f79cef50333ec8c24f15d3 Mon Sep 17 00:00:00 2001 From: patacongo Date: Mon, 20 Aug 2012 13:55:19 +0000 Subject: STM32 USB host driver is code compelte (but untested) git-svn-id: https://nuttx.svn.sourceforge.net/svnroot/nuttx/trunk@5039 7fd9a85b-ad96-42d3-883c-3090e2eb8679 --- nuttx/include/nuttx/usb/usbhost.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'nuttx/include') diff --git a/nuttx/include/nuttx/usb/usbhost.h b/nuttx/include/nuttx/usb/usbhost.h index 133fd4387..058f5403e 100644 --- a/nuttx/include/nuttx/usb/usbhost.h +++ b/nuttx/include/nuttx/usb/usbhost.h @@ -553,7 +553,7 @@ struct usbhost_class_s }; /* This structure describes one endpoint. It is used as an input to the - * allocep() method. Most of this information comes from the endpoint + * epalloc() method. Most of this information comes from the endpoint * descriptor. */ @@ -567,7 +567,7 @@ struct usbhost_epdesc_s uint16_t mxpacketsize; /* Max packetsize */ }; -/* This type represents one endpoint configured by the allocep() method. +/* This type represents one endpoint configured by the epalloc() method. * The actual form is know only internally to the USB host controller * (for example, for an OHCI driver, this would probably be a pointer * to an endpoint descriptor). -- cgit v1.2.3 From 27bd0bbee4acfbafd97be1a6a23db84762757906 Mon Sep 17 00:00:00 2001 From: patacongo Date: Mon, 20 Aug 2012 16:06:39 +0000 Subject: Changes for clean STM32 USB host driver build git-svn-id: https://nuttx.svn.sourceforge.net/svnroot/nuttx/trunk@5040 7fd9a85b-ad96-42d3-883c-3090e2eb8679 --- nuttx/arch/arm/src/stm32/stm32_otgfshost.c | 215 +++++++++++++++-------------- nuttx/arch/arm/src/stm32/stm32_usbhost.h | 26 ++++ nuttx/configs/stm3220g-eval/README.txt | 31 ++++- nuttx/configs/stm3220g-eval/nsh/defconfig | 32 ++++- nuttx/configs/stm3220g-eval/src/up_nsh.c | 64 +++++---- nuttx/configs/stm3220g-eval/src/up_usb.c | 10 +- nuttx/configs/stm3240g-eval/README.txt | 32 ++++- nuttx/configs/stm32f4discovery/README.txt | 23 +++ nuttx/drivers/usbhost/usbhost_enumerate.c | 8 +- nuttx/drivers/usbhost/usbhost_hidkbd.c | 6 +- nuttx/include/nuttx/usb/usbhost.h | 2 +- 11 files changed, 309 insertions(+), 140 deletions(-) (limited to 'nuttx/include') diff --git a/nuttx/arch/arm/src/stm32/stm32_otgfshost.c b/nuttx/arch/arm/src/stm32/stm32_otgfshost.c index 74ee71169..821194eb8 100644 --- a/nuttx/arch/arm/src/stm32/stm32_otgfshost.c +++ b/nuttx/arch/arm/src/stm32/stm32_otgfshost.c @@ -60,15 +60,37 @@ #include "up_arch.h" #include "up_internal.h" -#include "stm32_otgfs.h" +#include "stm32_usbhost.h" #if defined(CONFIG_USBHOST) && defined(CONFIG_STM32_OTGFS) /******************************************************************************* * Definitions *******************************************************************************/ - /* Configuration ***************************************************************/ +/* + * STM32 USB OTG FS Host Driver Support + * + * Pre-requisites + * + * CONFIG_USBHOST - Enable general USB host support + * CONFIG_STM32_OTGFS - Enable the STM32 USB OTG FS block + * CONFIG_STM32_SYSCFG - Needed + * + * Options: + * + * CONFIG_STM32_OTGFS_RXFIFO_SIZE - Size of the RX FIFO in 32-bit words. + * Default 128 (512 bytes) + * CONFIG_STM32_OTGFS_NPTXFIFO_SIZE - Size of the non-periodic Tx FIFO + * in 32-bit words. Default 96 (384 bytes) + * CONFIG_STM32_OTGFS_PTXFIFO_SIZE - Size of the periodic Tx FIFO in 32-bit + * words. Default 96 (384 bytes) + * CONFIG_STM32_OTGFS_SOFINTR - Enable SOF interrupts. Why would you ever + * want to do that? + * CONFIG_STM32_USBHOST_REGDEBUG - Enable very low-level register access + * debug. Depends on CONFIG_DEBUG. + */ + /* Pre-requistites (partial) */ #ifndef CONFIG_STM32_SYSCFG @@ -83,12 +105,6 @@ /* Default host non-periodic transmit FIFO size */ -#ifndef CONFIG_STM32_OTGFS_RXFIFO_SIZE -# define CONFIG_STM32_OTGFS_RXFIFO_SIZE 128 -#endif - -/* Default host non-periodic transmit FIFO size */ - #ifndef CONFIG_STM32_OTGFS_NPTXFIFO_SIZE # define CONFIG_STM32_OTGFS_NPTXFIFO_SIZE 96 #endif @@ -99,6 +115,12 @@ # define CONFIG_STM32_OTGFS_PTXFIFO_SIZE 96 #endif +/* Register debug depends on CONFIG_DEBUG */ + +#ifndef CONFIG_DEBUG +# undef CONFIG_STM32_USBHOST_REGDEBUG +#endif + /* HCD Setup *******************************************************************/ /* Hardware capabilities */ @@ -169,7 +191,7 @@ struct stm32_chan_s bool inuse; /* True: This channel is "in use" */ bool indata1; /* IN data toggle. True: DATA01 */ bool outdata1; /* OUT data toggle. True: DATA01 */ - bool isin; /* True: IN endpoint */ + bool in; /* True: IN endpoint */ volatile bool waiter; /* True: Thread is waiting for a channel event */ volatile uint16_t nerrors; /* Number of errors detecgted */ volatile uint16_t xfrd; /* Number of bytes transferred */ @@ -248,15 +270,15 @@ static inline void stm32_chan_freeall(FAR struct stm32_usbhost_s *priv); static void stm32_chan_configure(FAR struct stm32_usbhost_s *priv, int chidx); static int stm32_chan_waitsetup(FAR struct stm32_usbhost_s *priv, FAR struct stm32_chan_s *chan); -static void stm32_chan_wait(FAR struct stm32_usbhost_s *priv, - FAR struct stm32_chan_s *chan); +static int stm32_chan_wait(FAR struct stm32_usbhost_s *priv, + FAR struct stm32_chan_s *chan); static void stm32_chan_wakeup(FAR struct stm32_usbhost_s *priv, FAR struct stm32_chan_s *chan); /* Control/data transfer logic *************************************************/ static void stm32_transfer_start(FAR struct stm32_usbhost_s *priv, int chidx); -static inline uint32_t stm32_getframe(void); +static inline uint16_t stm32_getframe(void); static int stm32_ctrl_sendsetup(FAR struct stm32_usbhost_s *priv, FAR const struct usb_ctrlreq_s *req); static int stm32_ctrl_senddata(FAR struct stm32_usbhost_s *priv, @@ -298,7 +320,7 @@ static int stm32_gint_isr(int irq, FAR void *context); static void stm32_gint_enable(void); static void stm32_gint_disable(void); -static inline int stm32_hostinit_enable(void); +static inline void stm32_hostinit_enable(void); /* USB host controller operations **********************************************/ @@ -645,7 +667,7 @@ static void stm32_chan_configure(FAR struct stm32_usbhost_s *priv, int chidx) /* Additional setting for IN/OUT endpoints */ - if (priv->chan[chidx].isin) + if (priv->chan[chidx].in) { regval |= OTGFS_HCINT_BBERR; } @@ -665,7 +687,7 @@ static void stm32_chan_configure(FAR struct stm32_usbhost_s *priv, int chidx) /* Additional setting for IN endpoints */ - if (priv->chan[chidx].isin) + if (priv->chan[chidx].in) { regval |= OTGFS_HCINT_BBERR; } @@ -680,7 +702,7 @@ static void stm32_chan_configure(FAR struct stm32_usbhost_s *priv, int chidx) /* Additional setting for IN endpoints */ - if (priv->chan[chidx].isin) + if (priv->chan[chidx].in) { regval |= (OTGFS_HCINT_TXERR | OTGFS_HCINT_BBERR); } @@ -688,7 +710,7 @@ static void stm32_chan_configure(FAR struct stm32_usbhost_s *priv, int chidx) break; } - stm32_putreg(STM32_OTGFS_HCINTMSK(chidx), hcintmsk); + stm32_putreg(STM32_OTGFS_HCINTMSK(chidx), regval); /* Enable the top level host channel interrupt. */ @@ -703,7 +725,7 @@ static void stm32_chan_configure(FAR struct stm32_usbhost_s *priv, int chidx) regval = ((uint32_t)priv->chan[chidx].maxpacket << OTGFS_HCCHAR_MPSIZ_SHIFT) | ((uint32_t)priv->chan[chidx].epno << OTGFS_HCCHAR_EPNUM_SHIFT) | ((uint32_t)priv->chan[chidx].eptype << OTGFS_HCCHAR_EPTYP_SHIFT) | - ((uint32_t)priv->devaddr << OTGFS_HCCHAR_DAD_SHIFT) | + ((uint32_t)priv->devaddr << OTGFS_HCCHAR_DAD_SHIFT); /* Special case settings for low speed devices */ @@ -714,7 +736,7 @@ static void stm32_chan_configure(FAR struct stm32_usbhost_s *priv, int chidx) /* Special case settings for IN endpoints */ - if (priv->chan[chidx].isin) + if (priv->chan[chidx].in) { regval |= OTGFS_HCCHAR_EPDIR_IN; } @@ -805,7 +827,7 @@ static int stm32_chan_wait(FAR struct stm32_usbhost_s *priv, * wait here. */ - ret = sem_wait(chan->waitsem); + ret = sem_wait(&chan->waitsem); /* sem_wait should succeeed. But it is possible that we could be * awakened by a signal too. @@ -840,7 +862,7 @@ static void stm32_chan_wakeup(FAR struct stm32_usbhost_s *priv, { /* Is there a thread waiting for this transfer to complete? */ - if (priv->chan[chidx].result != EBUSY && chan->waiter) + if (chan->result != EBUSY && chan->waiter) { stm32_givesem(&chan->waitsem); chan->waiter = false; @@ -873,8 +895,8 @@ static void stm32_transfer_start(FAR struct stm32_usbhost_s *priv, int chidx) * of full, maximally sized packets that can fit in the read buffer. */ - mxpacket = priv->chan[chidx].maxpacket; - if (priv->chan[chidx].isin) + maxpacket = priv->chan[chidx].maxpacket; + if (priv->chan[chidx].in) { npackets = priv->chan[chidx].buflen / maxpacket; @@ -939,7 +961,7 @@ static void stm32_transfer_start(FAR struct stm32_usbhost_s *priv, int chidx) * outgoing data into the correct TxFIFO. */ - if (!priv->chan[chidx].isin && priv->chan[chidx].buflen > 0) + if (!priv->chan[chidx].in && priv->chan[chidx].buflen > 0) { /* Handle non-periodic (CTRL and BULK) OUT transfers differently than * perioci (INTR and ISOC) OUT transfers. @@ -1009,7 +1031,7 @@ static void stm32_transfer_start(FAR struct stm32_usbhost_s *priv, int chidx) static inline uint16_t stm32_getframe(void) { - return (stm32_getreg(STM32_OTGFS_HFNUM) & OTGFS_HFNUM_FRNUM_MASK); + return (uint16_t)(stm32_getreg(STM32_OTGFS_HFNUM) & OTGFS_HFNUM_FRNUM_MASK); } /******************************************************************************* @@ -1042,7 +1064,7 @@ static int stm32_ctrl_sendsetup(FAR struct stm32_usbhost_s *priv, /* Set up for the wait BEFORE starting the transfer */ - ret = stm32_chan_waitsetup(priv, chan) + ret = stm32_chan_waitsetup(priv, chan); if (ret != OK) { udbg("ERROR: Device disconnected\n"); @@ -1056,7 +1078,7 @@ static int stm32_ctrl_sendsetup(FAR struct stm32_usbhost_s *priv, /* Wait for the transfer to complete */ ret = stm32_chan_wait(priv, chan); - if (ret != -EGAIN) + if (ret != -EAGAIN) { udbg("Transfer failed: %d\n", ret); return ret; @@ -1084,6 +1106,7 @@ static int stm32_ctrl_senddata(FAR struct stm32_usbhost_s *priv, FAR uint8_t *buffer, unsigned int buflen) { FAR struct stm32_chan_s *chan = &priv->chan[priv->ep0out]; + int ret; /* Save buffer information */ @@ -1101,22 +1124,11 @@ static int stm32_ctrl_senddata(FAR struct stm32_usbhost_s *priv, /* Set the Data PID as per the outdata1 boolean */ - if (!chan->outdata1) - { - /* Use PID == DATA0 */ - - chan->pid = OTGFS_PID_DATA0; - } - else - { - /* Put the PID 1 */ - - chan->pid = OTGFS_PID_DATA1 ; - } + chan->pid = chan->outdata1 ? OTGFS_PID_DATA1 : OTGFS_PID_DATA0; /* Set up for the wait BEFORE starting the transfer */ - ret = stm32_chan_waitsetup(priv, chan) + ret = stm32_chan_waitsetup(priv, chan); if (ret != OK) { udbg("ERROR: Device disconnected\n"); @@ -1142,9 +1154,10 @@ static int stm32_ctrl_senddata(FAR struct stm32_usbhost_s *priv, *******************************************************************************/ static int stm32_ctrl_recvdata(FAR struct stm32_usbhost_s *priv, - FAR uint8_t *buffer, unsigned int buflen); + FAR uint8_t *buffer, unsigned int buflen) { - FAR struct stm32_chan_s *chan = &priv->chan[pric->ep0in]; + FAR struct stm32_chan_s *chan = &priv->chan[priv->ep0in]; + int ret; /* Save buffer information */ @@ -1154,7 +1167,7 @@ static int stm32_ctrl_recvdata(FAR struct stm32_usbhost_s *priv, /* Set up for the wait BEFORE starting the transfer */ - ret = stm32_chan_waitsetup(priv, chan) + ret = stm32_chan_waitsetup(priv, chan); if (ret != OK) { udbg("ERROR: Device disconnected\n"); @@ -1163,7 +1176,7 @@ static int stm32_ctrl_recvdata(FAR struct stm32_usbhost_s *priv, /* Start the transfer */ - stm32_transfer_start(priv, pric->ep0in); + stm32_transfer_start(priv, priv->ep0in); /* Wait for the transfer to complete and return the result */ @@ -1182,8 +1195,8 @@ static int stm32_ctrl_recvdata(FAR struct stm32_usbhost_s *priv, static void stm32_gint_wrpacket(FAR struct stm32_usbhost_s *priv, FAR uint8_t *buffer, int chidx, int buflen) { - FAR volatile uint32_t *fifo; FAR uint32_t *src; + uint32_t fifo; int buflen32; /* Get the number of 32-byte words associated with this byte size */ @@ -1275,7 +1288,7 @@ static inline void stm32_gint_hcinisr(FAR struct stm32_usbhost_s *priv, uint32_t hcint; uint32_t hcintmsk; uint32_t hcchar; - uint32_t hctsiz; + uint32_t pending; unsigned int eptype; /* Read the HCINT register to get the pending HC interrupts. Read the @@ -1647,8 +1660,6 @@ static inline void stm32_gint_hcoutisr(FAR struct stm32_usbhost_s *priv, else if ((pending & OTGFS_HCINT_NAK) != 0) { - uint32_t regval; - /* Clear the error count */ chan->nerrors = 0; @@ -1782,7 +1793,7 @@ static inline void stm32_gint_hcoutisr(FAR struct stm32_usbhost_s *priv, else if (chan->chstate == CHSTATE_NAK || chan->chstate == CHSTATE_NYET) { - chan->result = EGAIN; + chan->result = EAGAIN; } else if (chan->chstate == CHSTATE_STALL) { @@ -1832,7 +1843,7 @@ static void stm32_gint_connected(FAR struct stm32_usbhost_s *priv) ullvdbg("Connected\n"); priv->connected = true; - DEBUGASSERT(priv->smstate == SMTATE_IDLE); + DEBUGASSERT(priv->smstate == SMSTATE_DETACHED); /* Notify any waiters */ @@ -1857,12 +1868,11 @@ static void stm32_gint_disconnected(FAR struct stm32_usbhost_s *priv) { /* Were we previously connected? */ - if !priv->connected) + if (!priv->connected) { /* Yes.. then we no longer connected */ ullvdbg("Disconnected\n"); - priv->connected = false; /* Are we bound to a class driver? */ @@ -1876,9 +1886,12 @@ static void stm32_gint_disconnected(FAR struct stm32_usbhost_s *priv) /* Re-Initilaize Host for new Enumeration */ - stm32_swreset(priv); + priv->smstate = SMSTATE_DETACHED; + priv->ep0size = STM32_EP0_MAX_PACKET_SIZE; + priv->devaddr = STM32_DEF_DEVADDR; + priv->connected = false; + priv->lowspeed = false; stm32_chan_freeall(priv); - priv->smstate = SMSTATE_DETACHED; /* Notify any waiters that there is a change in the connection state */ @@ -1933,7 +1946,7 @@ static inline void stm32_gint_rxflvlisr(FAR struct stm32_usbhost_s *priv) /* Disable the RxFIFO non-empty interrupt */ - intmsk = stm32_getreg(STM32_OTGFS_GINTMSK) + intmsk = stm32_getreg(STM32_OTGFS_GINTMSK); intmsk &= ~OTGFS_GINT_RXFLVL; stm32_putreg(STM32_OTGFS_GINTMSK, intmsk); @@ -1957,18 +1970,16 @@ static inline void stm32_gint_rxflvlisr(FAR struct stm32_usbhost_s *priv) { /* Read the data into the host buffer. */ - int bcnt = (grxsts & OTGFS_GRXSTSH_BCNT_MASK) >> OTGFS_GRXSTSH_BCNT_SHIFT; - + bcnt = (grxsts & OTGFS_GRXSTSH_BCNT_MASK) >> OTGFS_GRXSTSH_BCNT_SHIFT; if (bcnt > 0 && priv->chan[chidx].buffer != NULL) { /* Transfer the packet from the Rx FIFO into the user buffer */ - FAR uint32_t *dest = (FAR uint32_t *)priv->chan[chidx].buffer; - uint32_t fifo = STM32_OTGFS_DFIFO_HCH(0); - uint32_t hctsiz; - int bcnt32 = (bcnt + 3) >> 2; + dest = (FAR uint32_t *)priv->chan[chidx].buffer; + fifo = STM32_OTGFS_DFIFO_HCH(0); + bcnt32 = (bcnt + 3) >> 2; - for (i = 0; i < count32b; i++, dest += 4) + for (i = 0; i < bcnt32; i++) { *dest++ = stm32_getreg(fifo); } @@ -2043,7 +2054,7 @@ static inline void stm32_gint_nptxfeisr(FAR struct stm32_usbhost_s *priv) /* Get the number of words remaining to be sent */ - buflen = priv->chan[chidx].buflen + buflen = priv->chan[chidx].buflen; buflen32 = (buflen + 3) >> 2; /* Break out of the loop if either (a) there is nothing more to be @@ -2120,7 +2131,7 @@ static inline void stm32_gint_ptxfeisr(FAR struct stm32_usbhost_s *priv) /* Get the number of words remaining to be sent */ - buflen = priv->chan[chidx].buflen + buflen = priv->chan[chidx].buflen; buflen32 = (buflen + 3) >> 2; /* Break out of the loop if either (a) there is nothing more to be @@ -2269,8 +2280,6 @@ static inline void stm32_gint_hprtisr(FAR struct stm32_usbhost_s *priv) if ((hprt & OTGFS_HPRT_PENA) != 0) { - uint32_t hcfg; - /* Yes.. handle the new connection event */ stm32_gint_connected(priv); @@ -2328,8 +2337,6 @@ static inline void stm32_gint_hprtisr(FAR struct stm32_usbhost_s *priv) static inline void stm32_gint_discisr(FAR struct stm32_usbhost_s *priv) { - uint32_t regval; - /* Handle the disconnection event */ stm32_gint_disconnected(priv); @@ -2410,49 +2417,49 @@ static int stm32_gint_isr(int irq, FAR void *context) /* Handle the RxFIFO non-empty interrupt */ - if ((pending & OTGFS_GINT_RXFLVL)) != 0) + if ((pending & OTGFS_GINT_RXFLVL) != 0) { stm32_gint_rxflvlisr(priv); } /* Handle the non-periodic TxFIFO empty interrupt */ - if ((pending & OTGFS_GINT_NPTXFE)) != 0) + if ((pending & OTGFS_GINT_NPTXFE) != 0) { stm32_gint_nptxfeisr(priv); } /* Handle the periodic TxFIFO empty interrupt */ - if ((pending & OTGFS_GINT_PTXFE)) != 0) + if ((pending & OTGFS_GINT_PTXFE) != 0) { stm32_gint_ptxfeisr(priv); } /* Handle the host channels interrupt */ - if ((pending & OTGFS_GINT_HC)) != 0) + if ((pending & OTGFS_GINT_HC) != 0) { stm32_gint_hcisr(priv); } /* Handle the host port interrupt */ - if ((pending & OTGFS_GINT_HPRT)) != 0) + if ((pending & OTGFS_GINT_HPRT) != 0) { stm32_gint_hprtisr(priv); } /* Handle the disconnect detected interrupt */ - if ((pending & OTGFS_GINT_DISC)) != 0) + if ((pending & OTGFS_GINT_DISC) != 0) { stm32_gint_discisr(priv); } /* Handle the incomplete isochronous OUT transfer */ - if ((pending & OTGFS_GINT_IISOOXFR)) != 0) + if ((pending & OTGFS_GINT_IISOOXFR) != 0) { stm32_gint_iisooxfrisr(priv); } @@ -2481,7 +2488,7 @@ static void stm32_gint_enable(void) /* Set the GINTMSK bit to unmask the interrupt */ - regval = stm32_getreg(STM32_OTGFS_GAHBCFG) + regval = stm32_getreg(STM32_OTGFS_GAHBCFG); regval |= OTGFS_GAHBCFG_GINTMSK; stm32_putreg(OTGFS_GAHBCFG_GINTMSK, regval); } @@ -2492,7 +2499,7 @@ static void stm32_gint_disable(void) /* Clear the GINTMSK bit to mask the interrupt */ - regval = stm32_getreg(STM32_OTGFS_GAHBCFG) + regval = stm32_getreg(STM32_OTGFS_GAHBCFG); regval &= ~OTGFS_GAHBCFG_GINTMSK; stm32_putreg(OTGFS_GAHBCFG_GINTMSK, regval); } @@ -2513,6 +2520,8 @@ static void stm32_gint_disable(void) static inline void stm32_hostinit_enable(void) { + uint32_t regval; + /* Disable all interrupts. */ stm32_putreg(STM32_OTGFS_GINTMSK, 0); @@ -2671,7 +2680,7 @@ static int stm32_enumerate(FAR struct usbhost_driver_s *drvr) priv->ep0out = chidx; priv->chan[chidx].epno = 0; - priv->chan[chidx].isin = false; + priv->chan[chidx].in = false; priv->chan[chidx].eptype = OTGFS_EPTYPE_CTRL; priv->chan[chidx].maxpacket = STM32_EP0_DEF_PACKET_SIZE; priv->chan[chidx].indata1 = false; @@ -2684,7 +2693,7 @@ static int stm32_enumerate(FAR struct usbhost_driver_s *drvr) priv->ep0in = chidx; priv->chan[chidx].epno = 0; - priv->chan[chidx].isin = true; + priv->chan[chidx].in = true; priv->chan[chidx].eptype = OTGFS_EPTYPE_CTRL; priv->chan[chidx].maxpacket = STM32_EP0_DEF_PACKET_SIZE; priv->chan[chidx].indata1 = false; @@ -2713,7 +2722,7 @@ static int stm32_enumerate(FAR struct usbhost_driver_s *drvr) */ uvdbg("Enumerate the device\n"); - priv->smstate = SMSTATE_ENUM + priv->smstate = SMSTATE_ENUM; ret = usbhost_enumerate(drvr, 1, &priv->class); /* The enumeration may fail either because of some HCD interfaces failure @@ -2810,7 +2819,7 @@ static int stm32_ep0configure(FAR struct usbhost_driver_s *drvr, uint8_t funcadd ************************************************************************************/ static int stm32_epalloc(FAR struct usbhost_driver_s *drvr, - FAR const FAR struct usbhost_epdesc_s *epdesc, + FAR const struct usbhost_epdesc_s *epdesc, FAR usbhost_ep_t *ep) { FAR struct stm32_usbhost_s *priv = (FAR struct stm32_usbhost_s *)drvr; @@ -2830,7 +2839,7 @@ static int stm32_epalloc(FAR struct usbhost_driver_s *drvr, /* Allocate a host channel for the endpoint */ - chidx = stm32_chan_alloc(priv) + chidx = stm32_chan_alloc(priv); if (chidx < 0) { udbg("Failed to allocate a host channel\n"); @@ -2846,9 +2855,9 @@ static int stm32_epalloc(FAR struct usbhost_driver_s *drvr, chan = &priv->chan[chidx]; chan->epno = epdesc->addr & USB_EPNO_MASK; - chan->isin = ((epdesc->addr & USB_DIR_MASK) == USB_DIR_IN); - chan->eptype = eptype->attr & USB_EP_ATTR_XFERTYPE_MASK; - chan->maxpacket = stm32_getle16(epdesc->maxpacket); + chan->in = epdesc->in; + chan->eptype = epdesc->xfrtype; + chan->maxpacket = epdesc->mxpacketsize; chan->indata1 = false; chan->outdata1 = false; @@ -2891,7 +2900,7 @@ static int stm32_epfree(FAR struct usbhost_driver_s *drvr, usbhost_ep_t ep) struct stm32_usbhost_s *priv = (struct stm32_usbhost_s *)drvr; int chidx = (int)ep; - DEBUGASSERT(priv && ep < STM32_MAX_TX_FIFOS); + DEBUGASSERT(priv && chidx < STM32_MAX_TX_FIFOS); /* We must have exclusive access to the USB host hardware and state structures */ @@ -3109,7 +3118,8 @@ static int stm32_ctrlin(FAR struct usbhost_driver_s *drvr, { struct stm32_usbhost_s *priv = (struct stm32_usbhost_s *)drvr; uint16_t buflen; - int ret; + int retries; + int ret; DEBUGASSERT(drvr && req); uvdbg("type:%02x req:%02x value:%02x%02x index:%02x%02x len:%02x%02x\n", @@ -3173,7 +3183,7 @@ static int stm32_ctrlout(FAR struct usbhost_driver_s *drvr, struct stm32_usbhost_s *priv = (struct stm32_usbhost_s *)drvr; uint16_t buflen; int retries; - int ret; + int ret; DEBUGASSERT(drvr && req); uvdbg("type:%02x req:%02x value:%02x%02x index:%02x%02x len:%02x%02x\n", @@ -3272,7 +3282,7 @@ static int stm32_transfer(FAR struct usbhost_driver_s *drvr, usbhost_ep_t ep, unsigned int chidx = (unsigned int)ep; int ret; - DEBUGASSERT(priv && buffer && ep < STM32_MAX_TX_FIFOS && buflen > 0); + DEBUGASSERT(priv && buffer && chidx < STM32_MAX_TX_FIFOS && buflen > 0); chan = &priv->chan[chidx]; /* We must have exclusive access to the USB host hardware and state structures */ @@ -3315,7 +3325,7 @@ static int stm32_transfer(FAR struct usbhost_driver_s *drvr, usbhost_ep_t ep, { /* Handle the bulk transfer based on the direction of the transfer. */ - if (chan->isin) + if (chan->in) { /* Setup the IN data PID */ @@ -3336,7 +3346,7 @@ static int stm32_transfer(FAR struct usbhost_driver_s *drvr, usbhost_ep_t ep, * transfer. */ - if (chan->isin) + if (chan->in) { /* Setup the OUT data PID */ @@ -3436,7 +3446,7 @@ static void stm32_portreset(FAR struct stm32_usbhost_s *priv) uint32_t regval; regval = stm32_getreg(STM32_OTGFS_HPRT); - retval &= ~(OTGFS_HPRT_PENA|OTGFS_HPRT_PCDET|OTGFS_HPRT_PENCHNG|OTGFS_HPRT_POCCHNG); + regval &= ~(OTGFS_HPRT_PENA|OTGFS_HPRT_PCDET|OTGFS_HPRT_PENCHNG|OTGFS_HPRT_POCCHNG); regval |= OTGFS_HPRT_PRST; stm32_putreg(STM32_OTGFS_HPRT, regval); @@ -3548,7 +3558,7 @@ static void stm32_vbusdrive(FAR struct stm32_usbhost_s *priv, bool state) /* Enable/disable the external charge pump */ - stm32_usbhost_vbusdrive(state); + stm32_usbhost_vbusdrive(0, state); /* Turn on the Host port power. */ @@ -3591,7 +3601,6 @@ static void stm32_host_initialize(FAR struct stm32_usbhost_s *priv) { uint32_t regval; uint32_t offset; - int ret; int i; /* Restart the PHY Clock */ @@ -3623,13 +3632,13 @@ static void stm32_host_initialize(FAR struct stm32_usbhost_s *priv) /* Setup the host non-periodic Tx FIFO size (HNPTXFSIZ) */ - regval = (offset | (CONFIG_STM32_OTGFS_NPTXFIFO_SIZE << OTGFS_HNPTXFSIZ_NPTXFD_MASK); - stm32_putreg(STM32_OTGFS_DIEPTXF0_HNPTXFSIZ, regval); - offset += CONFIG_STM32_OTGFS_NPTXFIFO_SIZE + regval = (offset | (CONFIG_STM32_OTGFS_NPTXFIFO_SIZE << OTGFS_HNPTXFSIZ_NPTXFD_SHIFT)); + stm32_putreg(STM32_OTGFS_HNPTXFSIZ, regval); + offset += CONFIG_STM32_OTGFS_NPTXFIFO_SIZE; /* Set up the host periodic Tx fifo size register (HPTXFSIZ) */ - regval = (offset | (CONFIG_STM32_OTGFS_PTXFIFO_SIZE << OTGFS_HPTXFSIZ_PTXFD_SHIFT); + regval = (offset | (CONFIG_STM32_OTGFS_PTXFIFO_SIZE << OTGFS_HPTXFSIZ_PTXFD_SHIFT)); stm32_putreg(STM32_OTGFS_HPTXFSIZ, regval); /* If OTG were supported, we sould need to clear HNP enable bit in the @@ -3657,8 +3666,7 @@ static void stm32_host_initialize(FAR struct stm32_usbhost_s *priv) /* Enable host interrupts */ - stm32_hostinit_enable(priv); - return OK; + stm32_hostinit_enable(); } /******************************************************************************* @@ -3692,7 +3700,7 @@ static inline void stm32_sw_initialize(FAR struct stm32_usbhost_s *priv) /* Put all of the channels back in their initial, allocated state */ - memset(priv-chan, 0, STM32_MAX_TX_FIFOS * sizeof(struct stm32_chan_s)); + memset(priv->chan, 0, STM32_MAX_TX_FIFOS * sizeof(struct stm32_chan_s)); /* Initialize each channel */ @@ -3720,7 +3728,7 @@ static inline void stm32_sw_initialize(FAR struct stm32_usbhost_s *priv) static inline int stm32_hw_initialize(FAR struct stm32_usbhost_s *priv) { uint32_t regval; - int ret; + unsigned long timeout; /* Set the PHYSEL bit in the GUSBCFG register to select the OTG FS serial * transceiver: "This bit is always 1 with write-only access" @@ -3828,7 +3836,6 @@ FAR struct usbhost_driver_s *usbhost_initialize(int controller) */ FAR struct stm32_usbhost_s *priv = &g_usbhost; - int ret; /* Sanity checks */ @@ -3896,3 +3903,5 @@ FAR struct usbhost_driver_s *usbhost_initialize(int controller) up_enable_irq(STM32_IRQ_OTGFS); return &priv->drvr; } + +#endif /* CONFIG_USBHOST && CONFIG_STM32_OTGFS */ diff --git a/nuttx/arch/arm/src/stm32/stm32_usbhost.h b/nuttx/arch/arm/src/stm32/stm32_usbhost.h index b4f893267..d9bce33f7 100644 --- a/nuttx/arch/arm/src/stm32/stm32_usbhost.h +++ b/nuttx/arch/arm/src/stm32/stm32_usbhost.h @@ -49,6 +49,32 @@ #if defined(CONFIG_STM32_OTGFS) && defined(CONFIG_USBHOST) +/************************************************************************************ + * Public Functions + ************************************************************************************/ +/* + * STM32 USB OTG FS Host Driver Support + * + * Pre-requisites + * + * CONFIG_USBHOST - Enable general USB host support + * CONFIG_STM32_OTGFS - Enable the STM32 USB OTG FS block + * CONFIG_STM32_SYSCFG - Needed + * + * Options: + * + * CONFIG_STM32_OTGFS_RXFIFO_SIZE - Size of the RX FIFO in 32-bit words. + * Default 128 (512 bytes) + * CONFIG_STM32_OTGFS_NPTXFIFO_SIZE - Size of the non-periodic Tx FIFO + * in 32-bit words. Default 96 (384 bytes) + * CONFIG_STM32_OTGFS_PTXFIFO_SIZE - Size of the periodic Tx FIFO in 32-bit + * words. Default 96 (384 bytes) + * CONFIG_STM32_OTGFS_SOFINTR - Enable SOF interrupts. Why would you ever + * want to do that? + * CONFIG_STM32_USBHOST_REGDEBUG - Enable very low-level register access + * debug. Depends on CONFIG_DEBUG. + */ + /************************************************************************************ * Public Functions ************************************************************************************/ diff --git a/nuttx/configs/stm3220g-eval/README.txt b/nuttx/configs/stm3220g-eval/README.txt index 43d9b382e..ad049ea4c 100644 --- a/nuttx/configs/stm3220g-eval/README.txt +++ b/nuttx/configs/stm3220g-eval/README.txt @@ -669,6 +669,27 @@ STM3220G-EVAL-specific Configuration Options STM3220G-EVAL LCD Hardware Configuration + STM32 USB OTG FS Host Driver Support + + Pre-requisites + + CONFIG_USBHOST - Enable general USB host support + CONFIG_STM32_OTGFS - Enable the STM32 USB OTG FS block + CONFIG_STM32_SYSCFG - Needed + + Options: + + CONFIG_STM32_OTGFS_RXFIFO_SIZE - Size of the RX FIFO in 32-bit words. + Default 128 (512 bytes) + CONFIG_STM32_OTGFS_NPTXFIFO_SIZE - Size of the non-periodic Tx FIFO + in 32-bit words. Default 96 (384 bytes) + CONFIG_STM32_OTGFS_PTXFIFO_SIZE - Size of the periodic Tx FIFO in 32-bit + words. Default 96 (384 bytes) + CONFIG_STM32_OTGFS_SOFINTR - Enable SOF interrupts. Why would you ever + want to do that? + CONFIG_STM32_USBHOST_REGDEBUG - Enable very low-level register access + debug. Depends on CONFIG_DEBUG. + Configurations ============== @@ -834,7 +855,15 @@ Where is one of the following: CONFIG_NX=y : Enable graphics suppport CONFIG_MM_REGIONS=2 : When FSMC is enabled, so is the on-board SRAM memory region - 8. This configuration requires that jumper JP22 be set to enable RS-232 operation. + 8. USB OTG FS Device or Host Support + + CONFIG_USBDEV - Enable USB device support + CONFIG_USBHOST - Enable USB host support + CONFIG_STM32_OTGFS - Enable the STM32 USB OTG FS block + CONFIG_STM32_SYSCFG - Needed + CONFIG_SCHED_WORKQUEUE - Worker thread support is required + + 9. This configuration requires that jumper JP22 be set to enable RS-232 operation. nsh2: ----- diff --git a/nuttx/configs/stm3220g-eval/nsh/defconfig b/nuttx/configs/stm3220g-eval/nsh/defconfig index a0f7a64be..a6c4db254 100644 --- a/nuttx/configs/stm3220g-eval/nsh/defconfig +++ b/nuttx/configs/stm3220g-eval/nsh/defconfig @@ -998,7 +998,7 @@ CONFIG_STMPE811_THRESHX=26 CONFIG_STMPE811_THRESHY=34 # -# USB Device Configuration +# STM32 USB OTG FS Device Configuration # # CONFIG_USBDEV # Enables USB device support @@ -1025,6 +1025,36 @@ CONFIG_USBDEV_MAXPOWER=100 CONFIG_USBDEV_TRACE=n CONFIG_USBDEV_TRACE_NRECORDS=128 +# +# STM32 USB OTG FS Host Configuration +# +# Pre-requisites +# +# CONFIG_USBHOST - Enable general USB host support +# CONFIG_STM32_OTGFS - Enable the STM32 USB OTG FS block +# CONFIG_STM32_SYSCFG - Needed +# CONFIG_SCHED_WORKQUEUE - Worker thread support is required +# +# Options: +# +# CONFIG_STM32_OTGFS_RXFIFO_SIZE - Size of the RX FIFO in 32-bit words. +# Default 128 (512 bytes) +# CONFIG_STM32_OTGFS_NPTXFIFO_SIZE - Size of the non-periodic Tx FIFO +# in 32-bit words. Default 96 (384 bytes) +# CONFIG_STM32_OTGFS_PTXFIFO_SIZE - Size of the periodic Tx FIFO in 32-bit +# words. Default 96 (384 bytes) +# CONFIG_STM32_OTGFS_SOFINTR - Enable SOF interrupts. Why would you ever +# want to do that? +# CONFIG_STM32_USBHOST_REGDEBUG - Enable very low-level register access +# debug. Depends on CONFIG_DEBUG. +# +CONFIG_USBHOST=n +# ONFIG_STM32_OTGFS_RXFIFO_SIZE +#CONFIG_STM32_OTGFS_NPTXFIFO_SIZE +#CONFIG_STM32_OTGFS_PTXFIFO_SIZE +CONFIG_STM32_OTGFS_SOFINTR=n +CONFIG_STM32_USBHOST_REGDEBUG=n + # # USB Serial Device Configuration # diff --git a/nuttx/configs/stm3220g-eval/src/up_nsh.c b/nuttx/configs/stm3220g-eval/src/up_nsh.c index 3b061902b..e1359de96 100644 --- a/nuttx/configs/stm3220g-eval/src/up_nsh.c +++ b/nuttx/configs/stm3220g-eval/src/up_nsh.c @@ -55,7 +55,12 @@ # include #endif +#ifdef CONFIG_STM32_OTGFS +# include "stm32_usbhost.h" +#endif + #include "stm32_internal.h" +#include "stm3220g-internal.h" /**************************************************************************** * Pre-Processor Definitions @@ -67,29 +72,19 @@ #undef CONFIG_STM32_SPI1 -/* PORT and SLOT number probably depend on the board configuration */ +/* MMCSD PORT and SLOT number probably depend on the board configuration */ -#ifdef CONFIG_ARCH_BOARD_STM3220G_EVAL -# define CONFIG_NSH_HAVEUSBDEV 1 -# define CONFIG_NSH_HAVEMMCSD 1 -# if defined(CONFIG_NSH_MMCSDSLOTNO) && CONFIG_NSH_MMCSDSLOTNO != 0 -# error "Only one MMC/SD slot" -# undef CONFIG_NSH_MMCSDSLOTNO -# endif -# ifndef CONFIG_NSH_MMCSDSLOTNO -# define CONFIG_NSH_MMCSDSLOTNO 0 -# endif -#else - /* Add configuration for new STM32 boards here */ -# error "Unrecognized STM32 board" -# undef CONFIG_NSH_HAVEUSBDEV -# undef CONFIG_NSH_HAVEMMCSD -#endif +#define HAVE_USBDEV 1 +#define HAVE_MMCSD 1 +#define HAVE_USBHOST 1 -/* Can't support USB features if USB is not enabled */ +#if defined(CONFIG_NSH_MMCSDSLOTNO) && CONFIG_NSH_MMCSDSLOTNO != 0 +# error "Only one MMC/SD slot" +# undef CONFIG_NSH_MMCSDSLOTNO +#endif -#ifndef CONFIG_USBDEV -# undef CONFIG_NSH_HAVEUSBDEV +#ifndef CONFIG_NSH_MMCSDSLOTNO +# define CONFIG_NSH_MMCSDSLOTNO 0 #endif /* Can't support MMC/SD features if mountpoints are disabled or if SDIO support @@ -97,13 +92,32 @@ */ #if defined(CONFIG_DISABLE_MOUNTPOINT) || !defined(CONFIG_STM32_SDIO) -# undef CONFIG_NSH_HAVEMMCSD +# undef HAVE_MMCSD #endif #ifndef CONFIG_NSH_MMCSDMINOR # define CONFIG_NSH_MMCSDMINOR 0 #endif +/* Can't support USB host or device features if USB OTG FS is not enabled */ + +#ifndef CONFIG_STM32_OTGFS +# undef HAVE_USBDEV +# undef HAVE_USBHOST +#endif + +/* Can't support USB device is USB device is not enabled */ + +#ifndef CONFIG_USBDEV +# undef HAVE_USBDEV +#endif + +/* Can't support USB host is USB host is not enabled */ + +#ifndef CONFIG_USBHOST +# undef HAVE_USBHOST +#endif + /* Debug ********************************************************************/ #ifdef CONFIG_CPP_HAVE_VARARGS @@ -138,8 +152,10 @@ int nsh_archinitialize(void) FAR struct spi_dev_s *spi; FAR struct mtd_dev_s *mtd; #endif -#ifdef CONFIG_NSH_HAVEMMCSD +#ifdef HAVE_MMCSD FAR struct sdio_dev_s *sdio; +#endif +#if defined(HAVE_MMCSD) || defined (HAVE_USBHOST) int ret; #endif @@ -169,7 +185,7 @@ int nsh_archinitialize(void) /* Mount the SDIO-based MMC/SD block driver */ -#ifdef CONFIG_NSH_HAVEMMCSD +#ifdef HAVE_MMCSD /* First, get an instance of the SDIO interface */ message("nsh_archinitialize: Initializing SDIO slot %d\n", @@ -203,7 +219,7 @@ int nsh_archinitialize(void) * will monitor for USB connection and disconnection events. */ -#if defined(CONFIG_STM32_OTGFS) && defined(CONFIG_USBHOST) +#ifdef HAVE_USBHOST ret = stm32_usbhost_initialize(); if (ret != OK) { diff --git a/nuttx/configs/stm3220g-eval/src/up_usb.c b/nuttx/configs/stm3220g-eval/src/up_usb.c index 374d974ff..be303a8a5 100644 --- a/nuttx/configs/stm3220g-eval/src/up_usb.c +++ b/nuttx/configs/stm3220g-eval/src/up_usb.c @@ -43,6 +43,8 @@ #include #include #include +#include +#include #include #include @@ -59,10 +61,10 @@ * Definitions ************************************************************************************/ -#if defined(CONFIG_USBDEV) || defined(CONFIG_USBDEV) +#if defined(CONFIG_USBDEV) || defined(CONFIG_USBHOST) # define HAVE_USB 1 #else -# warning "CONFIG_STM32_OTGFS is enabled but neither CONFIG_USBDEV nor CONFIG_USBDEV" +# warning "CONFIG_STM32_OTGFS is enabled but neither CONFIG_USBDEV nor CONFIG_USBHOST" # undef HAVE_USB #endif @@ -78,7 +80,7 @@ * Private Data ************************************************************************************/ -#ifdef CONFIG_NSH_HAVEUSBHOST +#ifdef CONFIG_USBHOST static struct usbhost_driver_s *g_drvr; #endif @@ -94,7 +96,7 @@ static struct usbhost_driver_s *g_drvr; * ****************************************************************************/ -#ifdef CONFIG_NSH_HAVEUSBHOST +#ifdef CONFIG_USBHOST static int usbhost_waiter(int argc, char *argv[]) { bool connected = false; diff --git a/nuttx/configs/stm3240g-eval/README.txt b/nuttx/configs/stm3240g-eval/README.txt index 49e939a23..c8c1fdfa7 100755 --- a/nuttx/configs/stm3240g-eval/README.txt +++ b/nuttx/configs/stm3240g-eval/README.txt @@ -831,6 +831,28 @@ STM3240G-EVAL-specific Configuration Options CONFIG_STM32_ILI9320_DISABLE (includes ILI9321) CONFIG_STM32_ILI9325_DISABLE + STM32 USB OTG FS Host Driver Support + + Pre-requisites + + CONFIG_USBHOST - Enable USB host support + CONFIG_STM32_OTGFS - Enable the STM32 USB OTG FS block + CONFIG_STM32_SYSCFG - Needed + CONFIG_SCHED_WORKQUEUE - Worker thread support is required + + Options: + + CONFIG_STM32_OTGFS_RXFIFO_SIZE - Size of the RX FIFO in 32-bit words. + Default 128 (512 bytes) + CONFIG_STM32_OTGFS_NPTXFIFO_SIZE - Size of the non-periodic Tx FIFO + in 32-bit words. Default 96 (384 bytes) + CONFIG_STM32_OTGFS_PTXFIFO_SIZE - Size of the periodic Tx FIFO in 32-bit + words. Default 96 (384 bytes) + CONFIG_STM32_OTGFS_SOFINTR - Enable SOF interrupts. Why would you ever + want to do that? + CONFIG_STM32_USBHOST_REGDEBUG - Enable very low-level register access + debug. Depends on CONFIG_DEBUG. + Configurations ============== @@ -996,7 +1018,15 @@ Where is one of the following: CONFIG_NX=y : Enable graphics suppport CONFIG_MM_REGIONS=3 : When FSMC is enabled, so is the on-board SRAM memory region - 8. This configuration requires that jumper JP22 be set to enable RS-232 + 8. USB OTG FS Device or Host Support + + CONFIG_USBDEV - Enable USB device support + CONFIG_USBHOST - Enable USB host support + CONFIG_STM32_OTGFS - Enable the STM32 USB OTG FS block + CONFIG_STM32_SYSCFG - Needed + CONFIG_SCHED_WORKQUEUE - Worker thread support is required + + 9. This configuration requires that jumper JP22 be set to enable RS-232 operation. nsh2: diff --git a/nuttx/configs/stm32f4discovery/README.txt b/nuttx/configs/stm32f4discovery/README.txt index 09a2bb2b0..a1fe2c356 100755 --- a/nuttx/configs/stm32f4discovery/README.txt +++ b/nuttx/configs/stm32f4discovery/README.txt @@ -883,6 +883,29 @@ STM32F4Discovery-specific Configuration Options CONFIG_SDIO_WIDTH_D1_ONLY - Select 1-bit transfer mode. Default: 4-bit transfer mode. + STM32 USB OTG FS Host Driver Support + + Pre-requisites + + CONFIG_USBDEV - Enable USB device support + CONFIG_USBHOST - Enable USB host support + CONFIG_STM32_OTGFS - Enable the STM32 USB OTG FS block + CONFIG_STM32_SYSCFG - Needed + CONFIG_SCHED_WORKQUEUE - Worker thread support is required + + Options: + + CONFIG_STM32_OTGFS_RXFIFO_SIZE - Size of the RX FIFO in 32-bit words. + Default 128 (512 bytes) + CONFIG_STM32_OTGFS_NPTXFIFO_SIZE - Size of the non-periodic Tx FIFO + in 32-bit words. Default 96 (384 bytes) + CONFIG_STM32_OTGFS_PTXFIFO_SIZE - Size of the periodic Tx FIFO in 32-bit + words. Default 96 (384 bytes) + CONFIG_STM32_OTGFS_SOFINTR - Enable SOF interrupts. Why would you ever + want to do that? + CONFIG_STM32_USBHOST_REGDEBUG - Enable very low-level register access + debug. Depends on CONFIG_DEBUG. + Configurations ============== diff --git a/nuttx/drivers/usbhost/usbhost_enumerate.c b/nuttx/drivers/usbhost/usbhost_enumerate.c index 8e1cd80e7..7ab055b57 100644 --- a/nuttx/drivers/usbhost/usbhost_enumerate.c +++ b/nuttx/drivers/usbhost/usbhost_enumerate.c @@ -122,8 +122,8 @@ static void usbhost_putle16(uint8_t *dest, uint16_t val) * *******************************************************************************/ -static inline int usbhost_devdesc(const struct usb_devdesc_s *devdesc, - struct usbhost_id_s *id) +static inline int usbhost_devdesc(FAR const struct usb_devdesc_s *devdesc, + FAR struct usbhost_id_s *id) { /* Clear the ID info */ @@ -131,7 +131,7 @@ static inline int usbhost_devdesc(const struct usb_devdesc_s *devdesc, /* Pick off the class ID info */ - id->base = devdesc->class; + id->base = devdesc->classid; id->subclass = devdesc->subclass; id->proto = devdesc->protocol; @@ -194,7 +194,7 @@ static inline int usbhost_configdesc(const uint8_t *configdesc, int cfglen, */ DEBUGASSERT(remaining >= sizeof(struct usb_ifdesc_s)); - id->base = ifdesc->class; + id->base = ifdesc->classid; id->subclass = ifdesc->subclass; id->proto = ifdesc->protocol; uvdbg("class:%d subclass:%d protocol:%d\n", diff --git a/nuttx/drivers/usbhost/usbhost_hidkbd.c b/nuttx/drivers/usbhost/usbhost_hidkbd.c index ce951e1e6..73224ff5c 100644 --- a/nuttx/drivers/usbhost/usbhost_hidkbd.c +++ b/nuttx/drivers/usbhost/usbhost_hidkbd.c @@ -255,7 +255,9 @@ static inline int usbhost_devinit(FAR struct usbhost_state_s *priv); static inline uint16_t usbhost_getle16(const uint8_t *val); static inline void usbhost_putle16(uint8_t *dest, uint16_t val); static inline uint32_t usbhost_getle32(const uint8_t *val); +#if 0 /* Not used */ static void usbhost_putle32(uint8_t *dest, uint32_t val); +#endif /* Transfer descriptor memory management */ @@ -701,7 +703,7 @@ static int usbhost_kbdpoll(int argc, char *argv[]) #if defined(CONFIG_DEBUG_USB) && defined(CONFIG_DEBUG_VERBOSE) unsigned int npolls = 0; #endif - unsigned int nerrors; + unsigned int nerrors = 0; int ret; uvdbg("Started\n"); @@ -1381,6 +1383,7 @@ static inline uint32_t usbhost_getle32(const uint8_t *val) * ****************************************************************************/ +#if 0 /* Not used */ static void usbhost_putle32(uint8_t *dest, uint32_t val) { /* Little endian means LS halfword first in byte stream */ @@ -1388,6 +1391,7 @@ static void usbhost_putle32(uint8_t *dest, uint32_t val) usbhost_putle16(dest, (uint16_t)(val & 0xffff)); usbhost_putle16(dest+2, (uint16_t)(val >> 16)); } +#endif /**************************************************************************** * Name: usbhost_tdalloc diff --git a/nuttx/include/nuttx/usb/usbhost.h b/nuttx/include/nuttx/usb/usbhost.h index 058f5403e..c68cb4712 100644 --- a/nuttx/include/nuttx/usb/usbhost.h +++ b/nuttx/include/nuttx/usb/usbhost.h @@ -562,7 +562,7 @@ struct usbhost_epdesc_s uint8_t addr; /* Endpoint address */ bool in; /* Direction: true->IN */ uint8_t funcaddr; /* USB address of function containing endpoint */ - uint8_t xfrtype; /* Transfer type. See SB_EP_ATTR_XFER_* in usb.h */ + uint8_t xfrtype; /* Transfer type. See USB_EP_ATTR_XFER_* in usb.h */ uint8_t interval; /* Polling interval */ uint16_t mxpacketsize; /* Max packetsize */ }; -- cgit v1.2.3 From 451e17f1831042ea35ec61c073f2fff47082fc4e Mon Sep 17 00:00:00 2001 From: patacongo Date: Tue, 21 Aug 2012 20:14:42 +0000 Subject: Several more bug fixes for STM32 OTG FS host driver git-svn-id: https://nuttx.svn.sourceforge.net/svnroot/nuttx/trunk@5044 7fd9a85b-ad96-42d3-883c-3090e2eb8679 --- nuttx/arch/arm/src/lpc17xx/lpc17_usbhost.c | 12 +++++----- nuttx/arch/arm/src/stm32/stm32_otgfshost.c | 38 +++++++++++++++++++++++------- nuttx/configs/stm3220g-eval/README.txt | 4 +++- nuttx/configs/stm3220g-eval/nsh/defconfig | 3 +++ nuttx/configs/stm3240g-eval/README.txt | 4 +++- nuttx/configs/stm32f4discovery/README.txt | 4 +++- nuttx/drivers/usbhost/usbhost_enumerate.c | 4 ++-- nuttx/include/nuttx/usb/usbhost.h | 14 +++++------ 8 files changed, 56 insertions(+), 27 deletions(-) (limited to 'nuttx/include') diff --git a/nuttx/arch/arm/src/lpc17xx/lpc17_usbhost.c b/nuttx/arch/arm/src/lpc17xx/lpc17_usbhost.c index 17a14fea8..154b953b7 100644 --- a/nuttx/arch/arm/src/lpc17xx/lpc17_usbhost.c +++ b/nuttx/arch/arm/src/lpc17xx/lpc17_usbhost.c @@ -1,9 +1,9 @@ /******************************************************************************* * arch/arm/src/lpc17xx/lpc17_usbhost.c * - * Copyright (C) 2010-2011 Gregory Nutt. All rights reserved. + * Copyright (C) 2010-2012 Gregory Nutt. All rights reserved. * Authors: Rafael Noronha - * Gregory Nutt + * Gregory Nutt * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -1898,7 +1898,7 @@ static int lpc17_epfree(FAR struct usbhost_driver_s *drvr, usbhost_ep_t ep) * Some hardware supports special memory in which request and descriptor data can * be accessed more efficiently. This method provides a mechanism to allocate * the request/descriptor memory. If the underlying hardware does not support - * such "special" memory, this functions may simply map to malloc. + * such "special" memory, this functions may simply map to kmalloc. * * This interface was optimized under a particular assumption. It was assumed * that the driver maintains a pool of small, pre-allocated buffers for descriptor @@ -1952,7 +1952,7 @@ static int lpc17_alloc(FAR struct usbhost_driver_s *drvr, * Some hardware supports special memory in which request and descriptor data can * be accessed more efficiently. This method provides a mechanism to free that * request/descriptor memory. If the underlying hardware does not support - * such "special" memory, this functions may simply map to free(). + * such "special" memory, this functions may simply map to kfree(). * * Input Parameters: * drvr - The USB host driver instance obtained as a parameter from the call to @@ -1988,7 +1988,7 @@ static int lpc17_free(FAR struct usbhost_driver_s *drvr, FAR uint8_t *buffer) * Some hardware supports special memory in which larger IO buffers can * be accessed more efficiently. This method provides a mechanism to allocate * the request/descriptor memory. If the underlying hardware does not support - * such "special" memory, this functions may simply map to malloc. + * such "special" memory, this functions may simply map to kmalloc. * * This interface differs from DRVR_ALLOC in that the buffers are variable-sized. * @@ -2036,7 +2036,7 @@ static int lpc17_ioalloc(FAR struct usbhost_driver_s *drvr, * Some hardware supports special memory in which IO data can be accessed more * efficiently. This method provides a mechanism to free that IO buffer * memory. If the underlying hardware does not support such "special" memory, - * this functions may simply map to free(). + * this functions may simply map to kfree(). * * Input Parameters: * drvr - The USB host driver instance obtained as a parameter from the call to diff --git a/nuttx/arch/arm/src/stm32/stm32_otgfshost.c b/nuttx/arch/arm/src/stm32/stm32_otgfshost.c index e6ea2de06..7eb2313a4 100644 --- a/nuttx/arch/arm/src/stm32/stm32_otgfshost.c +++ b/nuttx/arch/arm/src/stm32/stm32_otgfshost.c @@ -49,6 +49,7 @@ #include #include +#include #include #include @@ -89,6 +90,8 @@ * want to do that? * CONFIG_STM32_USBHOST_REGDEBUG - Enable very low-level register access * debug. Depends on CONFIG_DEBUG. + * CONFIG_STM32_USBHOST_PKTDUMP - Dump all incoming and outgoing USB + * packets. Depends on CONFIG_DEBUG. */ /* Pre-requistites (partial) */ @@ -119,6 +122,7 @@ #ifndef CONFIG_DEBUG # undef CONFIG_STM32_USBHOST_REGDEBUG +# undef CONFIG_STM32_USBHOST_PKTDUMP #endif /* HCD Setup *******************************************************************/ @@ -253,6 +257,12 @@ static void stm32_putreg(uint32_t addr, uint32_t value); static inline void stm32_modifyreg(uint32_t addr, uint32_t clrbits, uint32_t setbits); +#ifdef CONFIG_STM32_USBHOST_PKTDUMP +# define stm32_pktdump(m,b,n) lib_dumpbuffer(m,b,n) +#else +# define stm32_pktdump(m,b,n) +#endif + /* Semaphores ******************************************************************/ static void stm32_takesem(sem_t *sem); @@ -1298,6 +1308,8 @@ static void stm32_gint_wrpacket(FAR struct stm32_usbhost_s *priv, uint32_t fifo; int buflen32; + stm32_pktdump("Sending", buffer, buflen); + /* Get the number of 32-byte words associated with this byte size */ buflen32 = (buflen + 3) >> 2; @@ -1958,6 +1970,8 @@ static inline void stm32_gint_rxflvlisr(FAR struct stm32_usbhost_s *priv) *dest++ = stm32_getreg(fifo); } + stm32_pktdump("Received", priv->chan[chidx].buffer, bcnt); + /* Manage multiple packet transfers */ priv->chan[chidx].buffer += bcnt; @@ -2924,7 +2938,7 @@ static int stm32_epfree(FAR struct usbhost_driver_s *drvr, usbhost_ep_t ep) * Some hardware supports special memory in which request and descriptor data can * be accessed more efficiently. This method provides a mechanism to allocate * the request/descriptor memory. If the underlying hardware does not support - * such "special" memory, this functions may simply map to malloc. + * such "special" memory, this functions may simply map to kmalloc. * * This interface was optimized under a particular assumption. It was assumed * that the driver maintains a pool of small, pre-allocated buffers for descriptor @@ -2958,12 +2972,15 @@ static int stm32_alloc(FAR struct usbhost_driver_s *drvr, /* There is no special memory requirement */ - alloc = (FAR uint8_t *)malloc(*maxlen); + alloc = (FAR uint8_t *)kmalloc(*maxlen); if (!alloc) { return -ENOMEM; } + /* Return the allocated buffer */ + + *buffer = alloc; return OK; } @@ -2974,7 +2991,7 @@ static int stm32_alloc(FAR struct usbhost_driver_s *drvr, * Some hardware supports special memory in which request and descriptor data can * be accessed more efficiently. This method provides a mechanism to free that * request/descriptor memory. If the underlying hardware does not support - * such "special" memory, this functions may simply map to free(). + * such "special" memory, this functions may simply map to kfree(). * * Input Parameters: * drvr - The USB host driver instance obtained as a parameter from the call to @@ -2995,7 +3012,7 @@ static int stm32_free(FAR struct usbhost_driver_s *drvr, FAR uint8_t *buffer) /* There is no special memory requirement */ DEBUGASSERT(drvr && buffer); - free(buffer); + kfree(buffer); return OK; } @@ -3006,7 +3023,7 @@ static int stm32_free(FAR struct usbhost_driver_s *drvr, FAR uint8_t *buffer) * Some hardware supports special memory in which larger IO buffers can * be accessed more efficiently. This method provides a mechanism to allocate * the request/descriptor memory. If the underlying hardware does not support - * such "special" memory, this functions may simply map to malloc. + * such "special" memory, this functions may simply map to kmalloc. * * This interface differs from DRVR_ALLOC in that the buffers are variable-sized. * @@ -3035,12 +3052,15 @@ static int stm32_ioalloc(FAR struct usbhost_driver_s *drvr, /* There is no special memory requirement */ - alloc = (FAR uint8_t *)malloc(buflen); + alloc = (FAR uint8_t *)kmalloc(buflen); if (!alloc) { return -ENOMEM; } + /* Return the allocated buffer */ + + *buffer = alloc; return OK; } @@ -3051,7 +3071,7 @@ static int stm32_ioalloc(FAR struct usbhost_driver_s *drvr, * Some hardware supports special memory in which IO data can be accessed more * efficiently. This method provides a mechanism to free that IO buffer * memory. If the underlying hardware does not support such "special" memory, - * this functions may simply map to free(). + * this functions may simply map to kfree(). * * Input Parameters: * drvr - The USB host driver instance obtained as a parameter from the call to @@ -3072,7 +3092,7 @@ static int stm32_iofree(FAR struct usbhost_driver_s *drvr, FAR uint8_t *buffer) /* There is no special memory requirement */ DEBUGASSERT(drvr && buffer); - free(buffer); + kfree(buffer); return OK; } @@ -3161,7 +3181,7 @@ static int stm32_ctrlin(FAR struct usbhost_driver_s *drvr, /* Handle the status OUT phase */ priv->chan[priv->ep0out].outdata1 ^= true; - ret = stm32_ctrl_senddata(priv, buffer, buflen); + ret = stm32_ctrl_senddata(priv, NULL, 0); if (ret == OK) { break; diff --git a/nuttx/configs/stm3220g-eval/README.txt b/nuttx/configs/stm3220g-eval/README.txt index ad049ea4c..a25db07a5 100644 --- a/nuttx/configs/stm3220g-eval/README.txt +++ b/nuttx/configs/stm3220g-eval/README.txt @@ -689,7 +689,9 @@ STM3220G-EVAL-specific Configuration Options want to do that? CONFIG_STM32_USBHOST_REGDEBUG - Enable very low-level register access debug. Depends on CONFIG_DEBUG. - + CONFIG_STM32_USBHOST_PKTDUMP - Dump all incoming and outgoing USB + packets. Depends on CONFIG_DEBUG. + Configurations ============== diff --git a/nuttx/configs/stm3220g-eval/nsh/defconfig b/nuttx/configs/stm3220g-eval/nsh/defconfig index a6c4db254..5a3dd6643 100644 --- a/nuttx/configs/stm3220g-eval/nsh/defconfig +++ b/nuttx/configs/stm3220g-eval/nsh/defconfig @@ -1047,6 +1047,8 @@ CONFIG_USBDEV_TRACE_NRECORDS=128 # want to do that? # CONFIG_STM32_USBHOST_REGDEBUG - Enable very low-level register access # debug. Depends on CONFIG_DEBUG. +# CONFIG_STM32_USBHOST_PKTDUMP - Dump all incoming and outgoing USB +# packets. Depends on CONFIG_DEBUG. # CONFIG_USBHOST=n # ONFIG_STM32_OTGFS_RXFIFO_SIZE @@ -1054,6 +1056,7 @@ CONFIG_USBHOST=n #CONFIG_STM32_OTGFS_PTXFIFO_SIZE CONFIG_STM32_OTGFS_SOFINTR=n CONFIG_STM32_USBHOST_REGDEBUG=n +CONFIG_STM32_USBHOST_PKTDUMP=n # # USB Serial Device Configuration diff --git a/nuttx/configs/stm3240g-eval/README.txt b/nuttx/configs/stm3240g-eval/README.txt index c8c1fdfa7..5f942819c 100755 --- a/nuttx/configs/stm3240g-eval/README.txt +++ b/nuttx/configs/stm3240g-eval/README.txt @@ -852,7 +852,9 @@ STM3240G-EVAL-specific Configuration Options want to do that? CONFIG_STM32_USBHOST_REGDEBUG - Enable very low-level register access debug. Depends on CONFIG_DEBUG. - + CONFIG_STM32_USBHOST_PKTDUMP - Dump all incoming and outgoing USB + packets. Depends on CONFIG_DEBUG. + Configurations ============== diff --git a/nuttx/configs/stm32f4discovery/README.txt b/nuttx/configs/stm32f4discovery/README.txt index a1fe2c356..e7dc57353 100755 --- a/nuttx/configs/stm32f4discovery/README.txt +++ b/nuttx/configs/stm32f4discovery/README.txt @@ -905,7 +905,9 @@ STM32F4Discovery-specific Configuration Options want to do that? CONFIG_STM32_USBHOST_REGDEBUG - Enable very low-level register access debug. Depends on CONFIG_DEBUG. - + CONFIG_STM32_USBHOST_PKTDUMP - Dump all incoming and outgoing USB + packets. Depends on CONFIG_DEBUG. + Configurations ============== diff --git a/nuttx/drivers/usbhost/usbhost_enumerate.c b/nuttx/drivers/usbhost/usbhost_enumerate.c index 7ab055b57..36bfa20d1 100644 --- a/nuttx/drivers/usbhost/usbhost_enumerate.c +++ b/nuttx/drivers/usbhost/usbhost_enumerate.c @@ -1,8 +1,8 @@ /******************************************************************************* * drivers/usbhost/usbhost_enumerate.c * - * Copyright (C) 2011 Gregory Nutt. All rights reserved. - * Authors: Gregory Nutt + * Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. + * Authors: Gregory Nutt * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/nuttx/include/nuttx/usb/usbhost.h b/nuttx/include/nuttx/usb/usbhost.h index c68cb4712..b28db9551 100644 --- a/nuttx/include/nuttx/usb/usbhost.h +++ b/nuttx/include/nuttx/usb/usbhost.h @@ -1,7 +1,7 @@ /************************************************************************************ * include/nuttx/usb/usbhost.h * - * Copyright (C) 2010-2011 Gregory Nutt. All rights reserved. + * Copyright (C) 2010-2012 Gregory Nutt. All rights reserved. * Author: Gregory Nutt * * References: @@ -284,7 +284,7 @@ * Some hardware supports special memory in which request and descriptor data can * be accessed more efficiently. This method provides a mechanism to allocate * the request/descriptor memory. If the underlying hardware does not support - * such "special" memory, this functions may simply map to malloc. + * such "special" memory, this functions may simply map to kmalloc. * * This interface was optimized under a particular assumption. It was assumed * that the driver maintains a pool of small, pre-allocated buffers for descriptor @@ -317,7 +317,7 @@ * Some hardware supports special memory in which request and descriptor data can * be accessed more efficiently. This method provides a mechanism to free that * request/descriptor memory. If the underlying hardware does not support - * such "special" memory, this functions may simply map to free(). + * such "special" memory, this functions may simply map to kfree(). * * Input Parameters: * drvr - The USB host driver instance obtained as a parameter from the call to @@ -342,7 +342,7 @@ * Some hardware supports special memory in which larger IO buffers can * be accessed more efficiently. This method provides a mechanism to allocate * the request/descriptor memory. If the underlying hardware does not support - * such "special" memory, this functions may simply map to malloc. + * such "special" memory, this functions may simply map to kmalloc. * * This interface differs from DRVR_ALLOC in that the buffers are variable-sized. * @@ -371,7 +371,7 @@ * Some hardware supports special memory in which IO data can be accessed more * efficiently. This method provides a mechanism to free that IO buffer * memory. If the underlying hardware does not support such "special" memory, - * this functions may simply map to free(). + * this functions may simply map to kfree(). * * Input Parameters: * drvr - The USB host driver instance obtained as a parameter from the call to @@ -615,7 +615,7 @@ struct usbhost_driver_s * be accessed more efficiently. The following methods provide a mechanism * to allocate and free the transfer descriptor memory. If the underlying * hardware does not support such "special" memory, these functions may - * simply map to malloc and free. + * simply map to kmalloc and kfree. * * This interface was optimized under a particular assumption. It was assumed * that the driver maintains a pool of small, pre-allocated buffers for descriptor @@ -630,7 +630,7 @@ struct usbhost_driver_s /* Some hardware supports special memory in which larger IO buffers can * be accessed more efficiently. This method provides a mechanism to allocate * the request/descriptor memory. If the underlying hardware does not support - * such "special" memory, this functions may simply map to malloc. + * such "special" memory, this functions may simply map to kmalloc. * * This interface differs from DRVR_ALLOC in that the buffers are variable-sized. */ -- cgit v1.2.3 From 20c7cf9db9c06fb0f12bc4bc310c275a325665c9 Mon Sep 17 00:00:00 2001 From: patacongo Date: Fri, 24 Aug 2012 22:16:09 +0000 Subject: Update STM32 USB OTG FS host driver -- the driver is now marginally functional git-svn-id: https://nuttx.svn.sourceforge.net/svnroot/nuttx/trunk@5051 7fd9a85b-ad96-42d3-883c-3090e2eb8679 --- nuttx/ChangeLog | 4 + nuttx/arch/arm/src/lpc17xx/lpc17_usbhost.c | 8 +- nuttx/arch/arm/src/stm32/stm32_otgfshost.c | 64 ++++++++++------ nuttx/drivers/usbhost/usbhost_storage.c | 113 +++++++++++++++-------------- nuttx/include/nuttx/usb/usbhost.h | 8 +- 5 files changed, 120 insertions(+), 77 deletions(-) (limited to 'nuttx/include') diff --git a/nuttx/ChangeLog b/nuttx/ChangeLog index 502c0e352..993719b11 100644 --- a/nuttx/ChangeLog +++ b/nuttx/ChangeLog @@ -3173,3 +3173,7 @@ * arch/arm/src/stm32/stm32_otgfshost.c: Renamed from stm32_usbhost.c. This is nearly code complete and, with any luck, will be available in NuttX-6.22. + * configs/*/defconfig: Update all defconfig files to remove syntax + that is incompatible with the mconf configuration tool. + * arch/arm/src/stm32/stm32_otgfshost.c: This driver now appears to be + functional (although more testing is necesary). \ No newline at end of file diff --git a/nuttx/arch/arm/src/lpc17xx/lpc17_usbhost.c b/nuttx/arch/arm/src/lpc17xx/lpc17_usbhost.c index 154b953b7..de880cac1 100644 --- a/nuttx/arch/arm/src/lpc17xx/lpc17_usbhost.c +++ b/nuttx/arch/arm/src/lpc17xx/lpc17_usbhost.c @@ -2194,7 +2194,13 @@ static int lpc17_ctrlout(FAR struct usbhost_driver_s *drvr, * * Returned Values: * On success, zero (OK) is returned. On a failure, a negated errno value is - * returned indicating the nature of the failure + * returned indicating the nature of the failure: + * + * EAGAIN - If devices NAKs the transfer (or NYET or other error where + * it may be appropriate to restart the entire transaction). + * EPERM - If the endpoint stalls + * EIO - On a TX or data toggle error + * EPIPE - Overrun errors * * Assumptions: * - Only a single class bound to a single device is supported. diff --git a/nuttx/arch/arm/src/stm32/stm32_otgfshost.c b/nuttx/arch/arm/src/stm32/stm32_otgfshost.c index 06d35a04a..d84ba6884 100644 --- a/nuttx/arch/arm/src/stm32/stm32_otgfshost.c +++ b/nuttx/arch/arm/src/stm32/stm32_otgfshost.c @@ -201,6 +201,7 @@ struct stm32_chan_s uint8_t epno; /* Device endpoint number (0-127) */ uint8_t eptype; /* See OTGFS_EPTYPE_* definitions */ uint8_t pid; /* Data PID */ + uint8_t npackets; /* Number of packets (for data toggle) */ bool inuse; /* True: This channel is "in use" */ volatile bool indata1; /* IN data toggle. True: DATA01 (Bulk and INTR only) */ volatile bool outdata1; /* OUT data toggle. True: DATA01 */ @@ -1057,6 +1058,13 @@ static void stm32_transfer_start(FAR struct stm32_usbhost_s *priv, int chidx) } #endif + /* Save the number of packets in the transfer. We will need this in + * order to set the next data toggle correctly when the transfer + * completes. + */ + + chan->npackets = (uint8_t)npackets; + /* Setup the HCTSIZn register */ regval = ((uint32_t)chan->buflen << OTGFS_HCTSIZ_XFRSIZ_SHIFT) | @@ -1387,7 +1395,7 @@ static void stm32_gint_wrpacket(FAR struct stm32_usbhost_s *priv, * OK - Transfer completed successfully * EAGAIN - If devices NAKs the transfer or NYET occurs * EPERM - If the endpoint stalls - * EIO - On a TX or data error + * EIO - On a TX or data toggle error * EPIPE - Frame overrun * * EBUSY in the result field indicates that the transfer has not completed. @@ -1405,8 +1413,8 @@ static inline void stm32_gint_hcinisr(FAR struct stm32_usbhost_s *priv, * HCINTMSK register to get the set of enabled HC interrupts. */ - pending = stm32_getreg(STM32_OTGFS_HCINT(chidx)); - regval = stm32_getreg(STM32_OTGFS_HCINTMSK(chidx)); + pending = stm32_getreg(STM32_OTGFS_HCINT(chidx)); + regval = stm32_getreg(STM32_OTGFS_HCINTMSK(chidx)); /* AND the two to get the set of enabled, pending HC interrupts */ @@ -1588,11 +1596,18 @@ static inline void stm32_gint_hcinisr(FAR struct stm32_usbhost_s *priv, else if ((pending & OTGFS_HCINT_NAK) != 0) { + /* For a BULK tranfer, the hardware is capable of retrying + * automatically on a NAK. However, this is not always + * what we need to do. So we always halt the transfer and + * return control to high level logic in the even of a NAK. + */ + +#if 0 /* Halt the interrupt channel */ if (chan->eptype == OTGFS_EPTYPE_CTRL) { - /* Halt the channel -- the CHH interrrupt is expected next */ + /* Halt the channel -- the CHH interrrupt is expected next */ stm32_chan_halt(priv, chidx, CHREASON_NAK); } @@ -1611,7 +1626,11 @@ static inline void stm32_gint_hcinisr(FAR struct stm32_usbhost_s *priv, regval &= ~OTGFS_HCCHAR_CHDIS; stm32_putreg(STM32_OTGFS_HCCHAR(chidx), regval); } +#else + /* Halt all transfers on the NAK -- the CHH interrrupt is expected next */ + stm32_chan_halt(priv, chidx, CHREASON_NAK); +#endif /* Clear the NAK condition */ stm32_putreg(STM32_OTGFS_HCINT(chidx), OTGFS_HCINT_NAK); @@ -1634,7 +1653,7 @@ static inline void stm32_gint_hcinisr(FAR struct stm32_usbhost_s *priv, * OK - Transfer completed successfully * EAGAIN - If devices NAKs the transfer or NYET occurs * EPERM - If the endpoint stalls - * EIO - On a TX or data error + * EIO - On a TX or data toggle error * EPIPE - Frame overrun * * EBUSY in the result field indicates that the transfer has not completed. @@ -1652,8 +1671,8 @@ static inline void stm32_gint_hcoutisr(FAR struct stm32_usbhost_s *priv, * HCINTMSK register to get the set of enabled HC interrupts. */ - pending = stm32_getreg(STM32_OTGFS_HCINT(chidx)); - regval = stm32_getreg(STM32_OTGFS_HCINTMSK(chidx)); + pending = stm32_getreg(STM32_OTGFS_HCINT(chidx)); + regval = stm32_getreg(STM32_OTGFS_HCINTMSK(chidx)); /* AND the two to get the set of enabled, pending HC interrupts */ @@ -1798,11 +1817,14 @@ static inline void stm32_gint_hcoutisr(FAR struct stm32_usbhost_s *priv, regval = stm32_getreg(STM32_OTGFS_HCCHAR(chidx)); - /* Is it a bulk endpoint */ + /* Is it a bulk endpoint? Were an odd number of packets + * transferred? + */ - if ((regval & OTGFS_HCCHAR_EPTYP_MASK) == OTGFS_HCCHAR_EPTYP_BULK) + if ((regval & OTGFS_HCCHAR_EPTYP_MASK) == OTGFS_HCCHAR_EPTYP_BULK && + (chan->npackets & 1) != 0) { - /* Yes... toggle the data out PID */ + /* Yes to both... toggle the data out PID */ chan->outdata1 ^= true; } @@ -3405,7 +3427,13 @@ static int stm32_ctrlout(FAR struct usbhost_driver_s *drvr, * * Returned Values: * On success, zero (OK) is returned. On a failure, a negated errno value is - * returned indicating the nature of the failure + * returned indicating the nature of the failure: + * + * EAGAIN - If devices NAKs the transfer (or NYET or other error where + * it may be appropriate to restart the entire transaction). + * EPERM - If the endpoint stalls + * EIO - On a TX or data toggle error + * EPIPE - Overrun errors * * Assumptions: * - Only a single class bound to a single device is supported. @@ -3528,22 +3556,14 @@ static int stm32_transfer(FAR struct usbhost_driver_s *drvr, usbhost_ep_t ep, ret = stm32_chan_wait(priv, chan); - /* EGAIN indicates that the device NAKed the transfer and we need + /* EAGAIN indicates that the device NAKed the transfer and we need * do try again. Anything else (success or other errors) will * cause use to return */ - if (ret != -EAGAIN) + if (ret != OK) { - /* Output some debug info on an error */ - - if (ret < 0) - { - udbg("Transfer failed: %d\n", ret); - } - - /* Break out and return this result */ - + udbg("Transfer failed: %d\n", ret); break; } } diff --git a/nuttx/drivers/usbhost/usbhost_storage.c b/nuttx/drivers/usbhost/usbhost_storage.c index 2817c2446..853287371 100644 --- a/nuttx/drivers/usbhost/usbhost_storage.c +++ b/nuttx/drivers/usbhost/usbhost_storage.c @@ -1,8 +1,8 @@ /**************************************************************************** * drivers/usbhost/usbhost_storage.c * - * Copyright (C) 2010-2011 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt + * Copyright (C) 2010-2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -123,7 +123,7 @@ struct usbhost_state_s struct usbhost_driver_s *drvr; /* The remainder of the fields are provide to the mass storage class */ - + char sdchar; /* Character identifying the /dev/sd[n] device */ volatile bool disconnected; /* TRUE: Device has been disconnected */ uint8_t ifno; /* Interface number */ @@ -217,7 +217,7 @@ static inline int usbhost_tfree(FAR struct usbhost_state_s *priv); static FAR struct usbmsc_cbw_s *usbhost_cbwalloc(FAR struct usbhost_state_s *priv); /* struct usbhost_registry_s methods */ - + static struct usbhost_class_s *usbhost_create(FAR struct usbhost_driver_s *drvr, FAR const struct usbhost_id_s *id); @@ -248,7 +248,7 @@ static int usbhost_ioctl(FAR struct inode *inode, int cmd, * Private Data ****************************************************************************/ -/* This structure provides the registry entry ID informatino that will be +/* This structure provides the registry entry ID informatino that will be * used to associate the USB host mass storage class to a connected USB * device. */ @@ -416,7 +416,7 @@ static inline void usbhost_freeclass(FAR struct usbhost_state_s *class) flags = irqsave(); class->class.flink = g_freelist; g_freelist = class; - irqrestore(flags); + irqrestore(flags); } #else static inline void usbhost_freeclass(FAR struct usbhost_state_s *class) @@ -700,16 +700,16 @@ static inline int usbhost_testunitready(FAR struct usbhost_state_s *priv) int result; /* Initialize a CBW (re-using the allocated transfer buffer) */ - + cbw = usbhost_cbwalloc(priv); if (!cbw) { udbg("ERROR: Failed to create CBW\n"); return -ENOMEM; } - + /* Construct and send the CBW */ - + usbhost_testunitreadycbw(cbw); result = DRVR_TRANSFER(priv->drvr, priv->bulkout, (uint8_t*)cbw, USBMSC_CBW_SIZEOF); @@ -733,7 +733,7 @@ static inline int usbhost_requestsense(FAR struct usbhost_state_s *priv) int result; /* Initialize a CBW (re-using the allocated transfer buffer) */ - + cbw = usbhost_cbwalloc(priv); if (!cbw) { @@ -742,7 +742,7 @@ static inline int usbhost_requestsense(FAR struct usbhost_state_s *priv) } /* Construct and send the CBW */ - + usbhost_requestsensecbw(cbw); result = DRVR_TRANSFER(priv->drvr, priv->bulkout, (uint8_t*)cbw, USBMSC_CBW_SIZEOF); @@ -775,7 +775,7 @@ static inline int usbhost_readcapacity(FAR struct usbhost_state_s *priv) int result; /* Initialize a CBW (re-using the allocated transfer buffer) */ - + cbw = usbhost_cbwalloc(priv); if (!cbw) { @@ -784,7 +784,7 @@ static inline int usbhost_readcapacity(FAR struct usbhost_state_s *priv) } /* Construct and send the CBW */ - + usbhost_readcapacitycbw(cbw); result = DRVR_TRANSFER(priv->drvr, priv->bulkout, (uint8_t*)cbw, USBMSC_CBW_SIZEOF); @@ -823,7 +823,7 @@ static inline int usbhost_inquiry(FAR struct usbhost_state_s *priv) int result; /* Initialize a CBW (re-using the allocated transfer buffer) */ - + cbw = usbhost_cbwalloc(priv); if (!cbw) { @@ -832,7 +832,7 @@ static inline int usbhost_inquiry(FAR struct usbhost_state_s *priv) } /* Construct and send the CBW */ - + usbhost_inquirycbw(cbw); result = DRVR_TRANSFER(priv->drvr, priv->bulkout, (uint8_t*)cbw, USBMSC_CBW_SIZEOF); @@ -885,7 +885,7 @@ static void usbhost_destroy(FAR void *arg) DEBUGASSERT(priv != NULL); uvdbg("crefs: %d\n", priv->crefs); - + /* Unregister the block driver */ usbhost_mkdevname(priv, devname); @@ -965,10 +965,10 @@ static inline int usbhost_cfgdesc(FAR struct usbhost_state_s *priv, uint8_t found = 0; int ret; - DEBUGASSERT(priv != NULL && + DEBUGASSERT(priv != NULL && configdesc != NULL && desclen >= sizeof(struct usb_cfgdesc_s)); - + /* Verify that we were passed a configuration descriptor */ cfgdesc = (FAR struct usb_cfgdesc_s *)configdesc; @@ -1004,7 +1004,7 @@ static inline int usbhost_cfgdesc(FAR struct usbhost_state_s *priv, case USB_DESC_TYPE_INTERFACE: { FAR struct usb_ifdesc_s *ifdesc = (FAR struct usb_ifdesc_s *)configdesc; - + uvdbg("Interface descriptor\n"); DEBUGASSERT(remaining >= USB_SIZEOF_IFDESC); @@ -1078,7 +1078,7 @@ static inline int usbhost_cfgdesc(FAR struct usbhost_state_s *priv, found |= USBHOST_BINFOUND; /* Save the bulk IN endpoint information */ - + bindesc.addr = epdesc->addr & USB_EP_ADDR_NUMBER_MASK; bindesc.in = 1; bindesc.funcaddr = funcaddr; @@ -1108,7 +1108,7 @@ static inline int usbhost_cfgdesc(FAR struct usbhost_state_s *priv, } /* Increment the address of the next descriptor */ - + configdesc += desc->len; remaining -= desc->len; } @@ -1116,7 +1116,7 @@ static inline int usbhost_cfgdesc(FAR struct usbhost_state_s *priv, /* Sanity checking... did we find all of things that we need? Hmmm.. I wonder.. * can we work read-only or write-only if only one bulk endpoint found? */ - + if (found != USBHOST_ALLFOUND) { ulldbg("ERROR: Found IF:%s BIN:%s BOUT:%s\n", @@ -1202,7 +1202,7 @@ static inline int usbhost_initvolume(FAR struct usbhost_state_s *priv) uvdbg("Test unit ready, retries=%d\n", retries); /* Send TESTUNITREADY to see the unit is ready */ - + ret = usbhost_testunitready(priv); if (ret == OK) { @@ -1585,7 +1585,7 @@ static FAR struct usbmsc_cbw_s *usbhost_cbwalloc(FAR struct usbhost_state_s *pri * Name: usbhost_create * * Description: - * This function implements the create() method of struct usbhost_registry_s. + * This function implements the create() method of struct usbhost_registry_s. * The create() method is a callback into the class implementation. It is * used to (1) create a new instance of the USB host class state and to (2) * bind a USB host driver "session" to the class instance. Use of this @@ -1644,9 +1644,9 @@ static FAR struct usbhost_class_s *usbhost_create(FAR struct usbhost_driver_s *d priv->drvr = drvr; /* NOTE: We do not yet know the geometry of the USB mass storage device */ - + /* Return the instance of the USB mass storage class */ - + return &priv->class; } } @@ -1701,7 +1701,7 @@ static int usbhost_connect(FAR struct usbhost_class_s *class, FAR struct usbhost_state_s *priv = (FAR struct usbhost_state_s *)class; int ret; - DEBUGASSERT(priv != NULL && + DEBUGASSERT(priv != NULL && configdesc != NULL && desclen >= sizeof(struct usb_cfgdesc_s)); @@ -1722,7 +1722,7 @@ static int usbhost_connect(FAR struct usbhost_class_s *class, udbg("usbhost_initvolume() failed: %d\n", ret); } } - + return ret; } @@ -1792,7 +1792,7 @@ static int usbhost_disconnected(struct usbhost_class_s *class) } } - irqrestore(flags); + irqrestore(flags); return OK; } @@ -1945,45 +1945,52 @@ static ssize_t usbhost_read(FAR struct inode *inode, unsigned char *buffer, ret = -ENOMEM; /* Initialize a CBW (re-using the allocated transfer buffer) */ - + cbw = usbhost_cbwalloc(priv); if (cbw) { - /* Assume some device failure */ - - ret = -ENODEV; + /* Loop in the event that EAGAIN is returned (mean that the + * transaction was NAKed and we should try again. + */ - /* Construct and send the CBW */ - - usbhost_readcbw(startsector, priv->blocksize, nsectors, cbw); - result = DRVR_TRANSFER(priv->drvr, priv->bulkout, - (uint8_t*)cbw, USBMSC_CBW_SIZEOF); - if (result == OK) + do { - /* Receive the user data */ + /* Assume some device failure */ + + ret = -ENODEV; - result = DRVR_TRANSFER(priv->drvr, priv->bulkin, - buffer, priv->blocksize * nsectors); + /* Construct and send the CBW */ + + usbhost_readcbw(startsector, priv->blocksize, nsectors, cbw); + result = DRVR_TRANSFER(priv->drvr, priv->bulkout, + (uint8_t*)cbw, USBMSC_CBW_SIZEOF); if (result == OK) { - /* Receive the CSW */ + /* Receive the user data */ result = DRVR_TRANSFER(priv->drvr, priv->bulkin, - priv->tbuffer, USBMSC_CSW_SIZEOF); + buffer, priv->blocksize * nsectors); if (result == OK) { - FAR struct usbmsc_csw_s *csw; - - /* Check the CSW status */ + /* Receive the CSW */ - csw = (FAR struct usbmsc_csw_s *)priv->tbuffer; - if (csw->status == 0) + result = DRVR_TRANSFER(priv->drvr, priv->bulkin, + priv->tbuffer, USBMSC_CSW_SIZEOF); + if (result == OK) { - ret = nsectors; + FAR struct usbmsc_csw_s *csw; + + /* Check the CSW status */ + + csw = (FAR struct usbmsc_csw_s *)priv->tbuffer; + if (csw->status == 0) + { + ret = nsectors; + } } } } - } + } while (result == -EAGAIN); } usbhost_givesem(&priv->exclsem); @@ -2037,7 +2044,7 @@ static ssize_t usbhost_write(FAR struct inode *inode, const unsigned char *buffe ret = -ENOMEM; /* Initialize a CBW (re-using the allocated transfer buffer) */ - + cbw = usbhost_cbwalloc(priv); if (cbw) { @@ -2046,7 +2053,7 @@ static ssize_t usbhost_write(FAR struct inode *inode, const unsigned char *buffe ret = -ENODEV; /* Construct and send the CBW */ - + usbhost_writecbw(startsector, priv->blocksize, nsectors, cbw); result = DRVR_TRANSFER(priv->drvr, priv->bulkout, (uint8_t*)cbw, USBMSC_CBW_SIZEOF); diff --git a/nuttx/include/nuttx/usb/usbhost.h b/nuttx/include/nuttx/usb/usbhost.h index b28db9551..acfe9a829 100644 --- a/nuttx/include/nuttx/usb/usbhost.h +++ b/nuttx/include/nuttx/usb/usbhost.h @@ -448,7 +448,13 @@ * * Returned Values: * On success, zero (OK) is returned. On a failure, a negated errno value is - * returned indicating the nature of the failure + * returned indicating the nature of the failure: + * + * EAGAIN - If devices NAKs the transfer (or NYET or other error where + * it may be appropriate to restart the entire transaction). + * EPERM - If the endpoint stalls + * EIO - On a TX or data toggle error + * EPIPE - Overrun errors * * Assumptions: * This function will *not* be called from an interrupt handler. -- cgit v1.2.3 From fe493d8bb89c6f052f2cd8a66c00f50fd77ad442 Mon Sep 17 00:00:00 2001 From: patacongo Date: Sun, 26 Aug 2012 21:35:14 +0000 Subject: Fix some list handling associated with priority inheritance git-svn-id: https://nuttx.svn.sourceforge.net/svnroot/nuttx/trunk@5053 7fd9a85b-ad96-42d3-883c-3090e2eb8679 --- apps/examples/ostest/prioinherit.c | 14 ++++- nuttx/ChangeLog | 5 ++ nuttx/configs/stm3220g-eval/README.txt | 2 +- nuttx/configs/stm3240g-eval/README.txt | 2 +- nuttx/include/semaphore.h | 23 ++++--- nuttx/lib/semaphore/sem_init.c | 9 +-- nuttx/sched/os_start.c | 21 ++++--- nuttx/sched/sem_holder.c | 112 +++++++++++++++++---------------- 8 files changed, 111 insertions(+), 77 deletions(-) (limited to 'nuttx/include') diff --git a/apps/examples/ostest/prioinherit.c b/apps/examples/ostest/prioinherit.c index 993c9e14a..87849abcf 100644 --- a/apps/examples/ostest/prioinherit.c +++ b/apps/examples/ostest/prioinherit.c @@ -58,12 +58,22 @@ #ifndef CONFIG_SEM_PREALLOCHOLDERS # define CONFIG_SEM_PREALLOCHOLDERS 0 #endif -#define NLOWPRI_THREADS (CONFIG_SEM_PREALLOCHOLDERS+1) + +#if CONFIG_SEM_PREALLOCHOLDERS > 3 +# define NLOWPRI_THREADS 3 +#else +# define NLOWPRI_THREADS (CONFIG_SEM_PREALLOCHOLDERS+1) +#endif #ifndef CONFIG_SEM_NNESTPRIO # define CONFIG_SEM_NNESTPRIO 0 #endif -#define NHIGHPRI_THREADS (CONFIG_SEM_NNESTPRIO+1) + +#if CONFIG_SEM_NNESTPRIO > 3 +# define NHIGHPRI_THREADS 3 +#else +# define NHIGHPRI_THREADS (CONFIG_SEM_NNESTPRIO+1) +#endif /**************************************************************************** * Private Data diff --git a/nuttx/ChangeLog b/nuttx/ChangeLog index 7a6e3f6eb..4135bd19a 100644 --- a/nuttx/ChangeLog +++ b/nuttx/ChangeLog @@ -3178,3 +3178,8 @@ functional (although more testing is necesary). 6.22 2012-xx-xx Gregory Nutt + + * include/semaphore.h, sched/sem_holders.c, and lib/semaphore/sem_init.c: + Fix some strange (and probably wrong) list handling when + CONFIG_PRIORITY_INHERITANCE and CONFIG_SEM_PREALLOCHOLDERS are defined. + This list handling was probably causing errors reported by Mike Smith diff --git a/nuttx/configs/stm3220g-eval/README.txt b/nuttx/configs/stm3220g-eval/README.txt index da29b1592..64a86c4dd 100644 --- a/nuttx/configs/stm3220g-eval/README.txt +++ b/nuttx/configs/stm3220g-eval/README.txt @@ -860,7 +860,7 @@ Where is one of the following: 8. USB OTG FS Device or Host Support - CONFIG_USBDEV - Enable USB device support + CONFIG_USBDEV - Enable USB device support, OR CONFIG_USBHOST - Enable USB host support CONFIG_STM32_OTGFS - Enable the STM32 USB OTG FS block CONFIG_STM32_SYSCFG - Needed diff --git a/nuttx/configs/stm3240g-eval/README.txt b/nuttx/configs/stm3240g-eval/README.txt index 908fb93c8..949db1891 100755 --- a/nuttx/configs/stm3240g-eval/README.txt +++ b/nuttx/configs/stm3240g-eval/README.txt @@ -1023,7 +1023,7 @@ Where is one of the following: 8. USB OTG FS Device or Host Support - CONFIG_USBDEV - Enable USB device support + CONFIG_USBDEV - Enable USB device support, OR CONFIG_USBHOST - Enable USB host support CONFIG_STM32_OTGFS - Enable the STM32 USB OTG FS block CONFIG_STM32_SYSCFG - Needed diff --git a/nuttx/include/semaphore.h b/nuttx/include/semaphore.h index 85214b9c2..257a5826f 100644 --- a/nuttx/include/semaphore.h +++ b/nuttx/include/semaphore.h @@ -60,16 +60,16 @@ extern "C" { * Public Type Declarations ****************************************************************************/ -/* This structure contains the holder of a semaphore */ +/* This structure contains information about the holder of a semaphore */ #ifdef CONFIG_PRIORITY_INHERITANCE struct semholder_s { #if CONFIG_SEM_PREALLOCHOLDERS > 0 - struct semholder_s *flink; /* Implements singly linked list */ + struct semholder_s *flink; /* Implements singly linked list */ #endif - void *holder; /* Holder TCB (actual type is _TCB) */ - int16_t counts; /* Number of counts owned by this holder */ + void *htcb; /* Holder TCB (actual type is _TCB) */ + int16_t counts; /* Number of counts owned by this holder */ }; #if CONFIG_SEM_PREALLOCHOLDERS > 0 @@ -83,12 +83,21 @@ struct semholder_s struct sem_s { - int16_t semcount; /* >0 -> Num counts available */ - /* <0 -> Num tasks waiting for semaphore */ + int16_t semcount; /* >0 -> Num counts available */ + /* <0 -> Num tasks waiting for semaphore */ + /* If priority inheritance is enabled, then we have to keep track of which + * tasks hold references to the semaphore. + */ + #ifdef CONFIG_PRIORITY_INHERITANCE - struct semholder_s hlist; /* List of holders of semaphore counts */ +# if CONFIG_SEM_PREALLOCHOLDERS > 0 + FAR struct semholder_s *hhead; /* List of holders of semaphore counts */ +# else + struct semholder_s holder; /* Single holder */ +# endif #endif }; + typedef struct sem_s sem_t; /* Initializers */ diff --git a/nuttx/lib/semaphore/sem_init.c b/nuttx/lib/semaphore/sem_init.c index ea1c6e84e..bc14415f9 100644 --- a/nuttx/lib/semaphore/sem_init.c +++ b/nuttx/lib/semaphore/sem_init.c @@ -103,16 +103,17 @@ int sem_init(FAR sem_t *sem, int pshared, unsigned int value) { /* Initialize the seamphore count */ - sem->semcount = (int16_t)value; + sem->semcount = (int16_t)value; /* Initialize to support priority inheritance */ #ifdef CONFIG_PRIORITY_INHERITANCE # if CONFIG_SEM_PREALLOCHOLDERS > 0 - sem->hlist.flink = NULL; + sem->hhead = NULL; +# else + sem->holder.htcb = NULL; + sem->holder.counts = 0; # endif - sem->hlist.holder = NULL; - sem->hlist.counts = 0; #endif return OK; } diff --git a/nuttx/sched/os_start.c b/nuttx/sched/os_start.c index 6cd508b8c..c0b16236d 100644 --- a/nuttx/sched/os_start.c +++ b/nuttx/sched/os_start.c @@ -289,6 +289,18 @@ void os_start(void) g_idletcb.flags = TCB_FLAG_TTYPE_KERNEL; up_initial_state(&g_idletcb); + /* Initialize the semaphore facility(if in link). This has to be done + * very early because many subsystems depend upon fully functional + * semaphores. + */ + +#ifdef CONFIG_HAVE_WEAKFUNCTIONS + if (sem_initialize != NULL) +#endif + { + sem_initialize(); + } + /* Initialize the memory manager */ #ifndef CONFIG_HEAP_BASE @@ -351,15 +363,6 @@ void os_start(void) } #endif - /* Initialize the semaphore facility. (if in link) */ - -#ifdef CONFIG_HAVE_WEAKFUNCTIONS - if (sem_initialize != NULL) -#endif - { - sem_initialize(); - } - /* Initialize the named message queue facility (if in link) */ #ifndef CONFIG_DISABLE_MQUEUE diff --git a/nuttx/sched/sem_holder.c b/nuttx/sched/sem_holder.c index 6003c563d..34f88185a 100644 --- a/nuttx/sched/sem_holder.c +++ b/nuttx/sched/sem_holder.c @@ -99,32 +99,31 @@ static inline FAR struct semholder_s *sem_allocholder(sem_t *sem) * used to implement mutexes. */ - if (!sem->hlist.holder) - { - pholder = &sem->hlist; - pholder->counts = 0; - } - else - { #if CONFIG_SEM_PREALLOCHOLDERS > 0 - pholder = g_freeholders; - if (pholder) - { - /* Remove the holder from the free list an put it into the semaphore's holder list */ + pholder = g_freeholders; + if (pholder) + { + /* Remove the holder from the free list an put it into the semaphore's holder list */ - g_freeholders = pholder->flink; - pholder->flink = sem->hlist.flink; - sem->hlist.flink = pholder; + g_freeholders = pholder->flink; + pholder->flink = sem->hhead; + sem->hhead = pholder; - /* Make sure the initial count is zero */ + /* Make sure the initial count is zero */ - pholder->counts = 0; - } - else + pholder->counts = 0; + } #else - pholder = NULL; + if (!sem->holder.htcb) + { + pholder = &sem->holder; + pholder->counts = 0; + } #endif + else + { sdbg("Insufficient pre-allocated holders\n"); + pholder = NULL; } return pholder; @@ -140,12 +139,13 @@ static FAR struct semholder_s *sem_findholder(sem_t *sem, FAR _TCB *htcb) /* Try to find the holder in the list of holders associated with this semaphore */ - pholder = &sem->hlist; #if CONFIG_SEM_PREALLOCHOLDERS > 0 - for (; pholder; pholder = pholder->flink) + for (pholder = sem->hhead; pholder; pholder = pholder->flink) +#else + pholder = &sem->holder; #endif { - if (pholder->holder == htcb) + if (pholder->htcb == htcb) { /* Got it! */ @@ -186,31 +186,33 @@ static inline void sem_freeholder(sem_t *sem, FAR struct semholder_s *pholder) /* Release the holder and counts */ - pholder->holder = 0; + pholder->htcb = NULL; pholder->counts = 0; #if CONFIG_SEM_PREALLOCHOLDERS > 0 - /* If this is the holder inside the semaphore, then do nothing more */ + /* Search the list for the matching holder */ - if (pholder != &sem->hlist) - { - /* Otherwise, search the list for the matching holder */ + for (prev = NULL, curr = sem->hhead; + curr && curr != pholder; + prev = curr, curr = curr->flink); - for (prev = &sem->hlist, curr = sem->hlist.flink; - curr && curr != pholder; - prev = curr, curr = curr->flink); + if (curr) + { + /* Remove the holder from the list */ - if (curr) + if (prev) { - /* Remove the holder from the list */ - prev->flink = pholder->flink; + } + else + { + sem->hhead = pholder->flink; + } - /* And put it in the free list */ + /* And put it in the free list */ - pholder->flink = g_freeholders; - g_freeholders = pholder; - } + pholder->flink = g_freeholders; + g_freeholders = pholder; } #endif } @@ -221,14 +223,16 @@ static inline void sem_freeholder(sem_t *sem, FAR struct semholder_s *pholder) static int sem_foreachholder(FAR sem_t *sem, holderhandler_t handler, FAR void *arg) { - FAR struct semholder_s *pholder = &sem->hlist; + FAR struct semholder_s *pholder; #if CONFIG_SEM_PREALLOCHOLDERS > 0 FAR struct semholder_s *next; #endif int ret = 0; #if CONFIG_SEM_PREALLOCHOLDERS > 0 - for (; pholder && ret == 0; pholder = next) + for (pholder = sem->hhead; pholder && ret == 0; pholder = next) +#else + pholder = &sem->holder; #endif { #if CONFIG_SEM_PREALLOCHOLDERS > 0 @@ -238,7 +242,7 @@ static int sem_foreachholder(FAR sem_t *sem, holderhandler_t handler, FAR void * #endif /* The initial "built-in" container may hold a NULL holder */ - if (pholder->holder) + if (pholder->htcb) { /* Call the handler */ @@ -268,7 +272,7 @@ static int sem_recoverholders(FAR struct semholder_s *pholder, FAR sem_t *sem, F static int sem_boostholderprio(FAR struct semholder_s *pholder, FAR sem_t *sem, FAR void *arg) { - FAR _TCB *htcb = (FAR _TCB *)pholder->holder; + FAR _TCB *htcb = (FAR _TCB *)pholder->htcb; FAR _TCB *rtcb = (FAR _TCB*)arg; /* Make sure that the thread is still active. If it exited without releasing @@ -371,7 +375,7 @@ static int sem_boostholderprio(FAR struct semholder_s *pholder, static int sem_verifyholder(FAR struct semholder_s *pholder, FAR sem_t *sem, FAR void *arg) { #if 0 // Need to revisit this, but these assumptions seem to be untrue -- OR there is a bug??? - FAR _TCB *htcb = (FAR _TCB *)pholder->holder; + FAR _TCB *htcb = (FAR _TCB *)pholder->htcb; /* Called after a semaphore has been released (incremented), the semaphore * could is non-negative, and there is no thread waiting for the count. @@ -396,9 +400,9 @@ static int sem_dumpholder(FAR struct semholder_s *pholder, FAR sem_t *sem, FAR v { #if CONFIG_SEM_PREALLOCHOLDERS > 0 dbg(" %08x: %08x %08x %04x\n", - pholder, pholder->flink, pholder->holder, pholder->counts); + pholder, pholder->flink, pholder->htcb, pholder->counts); #else - dbg(" %08x: %08x %04x\n", pholder, pholder->holder, pholder->counts); + dbg(" %08x: %08x %04x\n", pholder, pholder->htcb, pholder->counts); #endif return 0; } @@ -410,7 +414,7 @@ static int sem_dumpholder(FAR struct semholder_s *pholder, FAR sem_t *sem, FAR v static int sem_restoreholderprio(FAR struct semholder_s *pholder, FAR sem_t *sem, FAR void *arg) { - FAR _TCB *htcb = (FAR _TCB *)pholder->holder; + FAR _TCB *htcb = (FAR _TCB *)pholder->htcb; #if CONFIG_SEM_NNESTPRIO > 0 FAR _TCB *stcb = (FAR _TCB *)arg; int rpriority; @@ -510,6 +514,7 @@ static int sem_restoreholderprio(FAR struct semholder_s *pholder, FAR sem_t *sem { htcb->pend_reprios[i] = htcb->pend_reprios[j]; } + htcb->npend_reprio = j; break; } @@ -534,7 +539,7 @@ static int sem_restoreholderprio(FAR struct semholder_s *pholder, FAR sem_t *sem static int sem_restoreholderprioA(FAR struct semholder_s *pholder, FAR sem_t *sem, FAR void *arg) { FAR _TCB *rtcb = (FAR _TCB*)g_readytorun.head; - if (pholder->holder != rtcb) + if (pholder->htcb != rtcb) { return sem_restoreholderprio(pholder, sem, arg); } @@ -545,7 +550,7 @@ static int sem_restoreholderprioA(FAR struct semholder_s *pholder, FAR sem_t *se static int sem_restoreholderprioB(FAR struct semholder_s *pholder, FAR sem_t *sem, FAR void *arg) { FAR _TCB *rtcb = (FAR _TCB*)g_readytorun.head; - if (pholder->holder == rtcb) + if (pholder->htcb == rtcb) { (void)sem_restoreholderprio(pholder, sem, arg); return 1; @@ -586,6 +591,7 @@ void sem_initholders(void) { g_holderalloc[i].flink = &g_holderalloc[i+1]; } + g_holderalloc[CONFIG_SEM_PREALLOCHOLDERS-1].flink = NULL; #endif } @@ -609,8 +615,8 @@ void sem_initholders(void) void sem_destroyholder(FAR sem_t *sem) { - /* It is an error is a semaphore is destroyed while there are any holders - * (except perhaps the thread releas the semaphore itself). Hmmm.. but + /* It is an error if a semaphore is destroyed while there are any holders + * (except perhaps the thread release the semaphore itself). Hmmm.. but * we actually have to assume that the caller knows what it is doing because * could have killed another thread that is the actual holder of the semaphore. * We cannot make any assumptions about the state of the semaphore or the @@ -621,18 +627,18 @@ void sem_destroyholder(FAR sem_t *sem) */ #if CONFIG_SEM_PREALLOCHOLDERS > 0 - if (sem->hlist.holder || sem->hlist.flink) + if (sem->hhead) { sdbg("Semaphore destroyed with holders\n"); (void)sem_foreachholder(sem, sem_recoverholders, NULL); } #else - if (sem->hlist.holder) + if (sem->holder.htcb) { sdbg("Semaphore destroyed with holder\n"); } - sem->hlist.holder = NULL; + sem->holder.htcb = NULL; #endif } @@ -664,7 +670,7 @@ void sem_addholder(FAR sem_t *sem) { /* Then set the holder and increment the number of counts held by this holder */ - pholder->holder = rtcb; + pholder->htcb = rtcb; pholder->counts++; } } -- cgit v1.2.3