From 73914fb947019bd12dc1bd95e6e425db0c0b0550 Mon Sep 17 00:00:00 2001 From: patacongo Date: Tue, 24 Jan 2012 21:51:26 +0000 Subject: Lots of re-organization -- getting ready to support a composite USB device git-svn-id: svn://svn.code.sf.net/p/nuttx/code/trunk@4329 42af7a65-404d-4744-a932-0658087f49c3 --- nuttx/drivers/usbdev/Make.defs | 14 +- nuttx/drivers/usbdev/cdc_serdesc.c | 590 ------ nuttx/drivers/usbdev/cdc_serial.c | 1990 ------------------- nuttx/drivers/usbdev/cdc_serial.h | 196 -- nuttx/drivers/usbdev/cdcacm.c | 2003 +++++++++++++++++++ nuttx/drivers/usbdev/cdcacm.h | 296 +++ nuttx/drivers/usbdev/cdcacm_descriptors.c | 584 ++++++ nuttx/drivers/usbdev/composite.c | 292 +++ nuttx/drivers/usbdev/composite.h | 176 ++ nuttx/drivers/usbdev/composite_descriptors.c | 366 ++++ nuttx/drivers/usbdev/msc.c | 1660 ++++++++++++++++ nuttx/drivers/usbdev/msc.h | 659 +++++++ nuttx/drivers/usbdev/msc_descriptors.c | 435 +++++ nuttx/drivers/usbdev/msc_scsi.c | 2664 ++++++++++++++++++++++++++ nuttx/drivers/usbdev/pl2303.c | 2216 +++++++++++++++++++++ nuttx/drivers/usbdev/usbdev_composite.c | 85 - nuttx/drivers/usbdev/usbdev_composite.h | 86 - nuttx/drivers/usbdev/usbdev_scsi.c | 2664 -------------------------- nuttx/drivers/usbdev/usbdev_serial.c | 2216 --------------------- nuttx/drivers/usbdev/usbdev_storage.c | 1651 ---------------- nuttx/drivers/usbdev/usbdev_storage.h | 608 ------ nuttx/drivers/usbdev/usbdev_stordesc.c | 408 ---- nuttx/drivers/usbdev/usbdev_trace.c | 4 +- nuttx/drivers/usbdev/usbdev_trprintf.c | 4 +- 24 files changed, 11362 insertions(+), 10505 deletions(-) delete mode 100644 nuttx/drivers/usbdev/cdc_serdesc.c delete mode 100644 nuttx/drivers/usbdev/cdc_serial.c delete mode 100644 nuttx/drivers/usbdev/cdc_serial.h create mode 100644 nuttx/drivers/usbdev/cdcacm.c create mode 100644 nuttx/drivers/usbdev/cdcacm.h create mode 100644 nuttx/drivers/usbdev/cdcacm_descriptors.c create mode 100644 nuttx/drivers/usbdev/composite.c create mode 100644 nuttx/drivers/usbdev/composite.h create mode 100644 nuttx/drivers/usbdev/composite_descriptors.c create mode 100644 nuttx/drivers/usbdev/msc.c create mode 100644 nuttx/drivers/usbdev/msc.h create mode 100644 nuttx/drivers/usbdev/msc_descriptors.c create mode 100644 nuttx/drivers/usbdev/msc_scsi.c create mode 100644 nuttx/drivers/usbdev/pl2303.c delete mode 100644 nuttx/drivers/usbdev/usbdev_composite.c delete mode 100644 nuttx/drivers/usbdev/usbdev_composite.h delete mode 100644 nuttx/drivers/usbdev/usbdev_scsi.c delete mode 100644 nuttx/drivers/usbdev/usbdev_serial.c delete mode 100644 nuttx/drivers/usbdev/usbdev_storage.c delete mode 100644 nuttx/drivers/usbdev/usbdev_storage.h delete mode 100644 nuttx/drivers/usbdev/usbdev_stordesc.c (limited to 'nuttx/drivers') diff --git a/nuttx/drivers/usbdev/Make.defs b/nuttx/drivers/usbdev/Make.defs index 4ae4a49e1..348799849 100644 --- a/nuttx/drivers/usbdev/Make.defs +++ b/nuttx/drivers/usbdev/Make.defs @@ -1,8 +1,8 @@ ############################################################################ # drivers/usbdev/Make.defs # -# Copyright (C) 2008, 2010-2011 Gregory Nutt. All rights reserved. -# Author: Gregory Nutt +# Copyright (C) 2008, 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 @@ -38,22 +38,22 @@ ifeq ($(CONFIG_USBDEV),y) # Include USB device drivers ifeq ($(CONFIG_USBSER),y) - CSRCS += usbdev_serial.c + CSRCS += pl2303.c endif ifeq ($(CONFIG_CDCSER),y) - CSRCS += cdc_serial.c cdc_serdesc.c + CSRCS += cdcacm.c cdcacm_descriptors.c endif ifeq ($(CONFIG_USBSTRG),y) - CSRCS += usbdev_storage.c usbdev_stordesc.c usbdev_scsi.c + CSRCS += msc.c msc_descriptors.c msc_scsi.c endif ifeq ($(CONFIG_USBDEV_COMPOSITE),y) - CSRCS += usbdev_composite.c + CSRCS += composite.c composite_descriptors.c endif -CSRCS += usbdev_trace.c usbdev_trprintf.c +CSRCS += usbdev_trace.c usbdev_trprintf.c # Include USB device build support diff --git a/nuttx/drivers/usbdev/cdc_serdesc.c b/nuttx/drivers/usbdev/cdc_serdesc.c deleted file mode 100644 index 990704a1b..000000000 --- a/nuttx/drivers/usbdev/cdc_serdesc.c +++ /dev/null @@ -1,590 +0,0 @@ -/**************************************************************************** - * drivers/usbdev/cdc_serdesc.c - * - * Copyright (C) 2011 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 - * 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 - -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include "cdc_serial.h" - -/**************************************************************************** - * Definitions - ****************************************************************************/ -/* Descriptors **************************************************************/ -/* These settings are not modifiable via the NuttX configuration */ - -#define CDC_VERSIONNO 0x0110 /* CDC version number 1.10 (BCD) */ - -/* Device descriptor values */ - -#define CDCSER_VERSIONNO (0x0101) /* Device version number 1.1 (BCD) */ -#define CDCSER_NCONFIGS (1) /* Number of configurations supported */ - -/* Configuration descriptor values */ - -#define CDCSER_NINTERFACES (2) /* Number of interfaces in the configuration */ - -/* String language */ - -#define CDCSER_STR_LANGUAGE (0x0409) /* en-us */ - -/* Descriptor strings */ - -#define CDCSER_MANUFACTURERSTRID (1) -#define CDCSER_PRODUCTSTRID (2) -#define CDCSER_SERIALSTRID (3) -#define CDCSER_CONFIGSTRID (4) -#define CDCSER_NOTIFSTRID (5) -#define CDCSER_DATAIFSTRID (6) - -/* Number of individual descriptors in the configuration descriptor */ - -#define CDCSER_CFGGROUP_SIZE (9) - -/* The size of the config descriptor: (9 + 2*9 + 3*7 + 4 + 5 + 5) = 62 */ - -#define SIZEOF_CDCSER_CFGDESC \ - (USB_SIZEOF_CFGDESC + 2*USB_SIZEOF_IFDESC + 3*USB_SIZEOF_EPDESC + SIZEOF_ACM_FUNCDESC + SIZEOF_HDR_FUNCDESC + SIZEOF_UNION_FUNCDESC(1)) - -/**************************************************************************** - * Private Types - ****************************************************************************/ - -/* Describes one description in the group of descriptors forming the - * total configuration descriptor. - */ - -struct cfgdecsc_group_s -{ - uint16_t descsize; /* Size of the descriptor in bytes */ - uint16_t hsepsize; /* High speed max packet size */ - FAR void *desc; /* A pointer to the descriptor */ -}; - -/**************************************************************************** - * Private Function Prototypes - ****************************************************************************/ - -/**************************************************************************** - * Private Data - ****************************************************************************/ - -/* USB descriptor templates these will be copied and modified **************/ - -static const struct usb_devdesc_s g_devdesc = -{ - USB_SIZEOF_DEVDESC, /* len */ - USB_DESC_TYPE_DEVICE, /* type */ - { /* usb */ - LSBYTE(0x0200), - MSBYTE(0x0200) - }, - USB_CLASS_CDC, /* class */ - CDC_SUBCLASS_NONE, /* subclass */ - CDC_PROTO_NONE, /* protocol */ - CONFIG_CDCSER_EP0MAXPACKET, /* maxpacketsize */ - { - LSBYTE(CONFIG_CDCSER_VENDORID), /* vendor */ - MSBYTE(CONFIG_CDCSER_VENDORID) - }, - { - LSBYTE(CONFIG_CDCSER_PRODUCTID), /* product */ - MSBYTE(CONFIG_CDCSER_PRODUCTID) - }, - { - LSBYTE(CDCSER_VERSIONNO), /* device */ - MSBYTE(CDCSER_VERSIONNO) - }, - CDCSER_MANUFACTURERSTRID, /* imfgr */ - CDCSER_PRODUCTSTRID, /* iproduct */ - CDCSER_SERIALSTRID, /* serno */ - CDCSER_NCONFIGS /* nconfigs */ -}; - -/* Configuration descriptor */ - -static const struct usb_cfgdesc_s g_cfgdesc = -{ - USB_SIZEOF_CFGDESC, /* len */ - USB_DESC_TYPE_CONFIG, /* type */ - { - LSBYTE(SIZEOF_CDCSER_CFGDESC), /* LS totallen */ - MSBYTE(SIZEOF_CDCSER_CFGDESC) /* MS totallen */ - }, - CDCSER_NINTERFACES, /* ninterfaces */ - CDCSER_CONFIGID, /* cfgvalue */ - CDCSER_CONFIGSTRID, /* icfg */ - USB_CONFIG_ATTR_ONE|SELFPOWERED|REMOTEWAKEUP, /* attr */ - (CONFIG_USBDEV_MAXPOWER + 1) / 2 /* mxpower */ -}; - -/* Notification interface */ - -static const struct usb_ifdesc_s g_notifdesc = -{ - USB_SIZEOF_IFDESC, /* len */ - USB_DESC_TYPE_INTERFACE, /* type */ - 0, /* ifno */ - 0, /* alt */ - 1, /* neps */ - USB_CLASS_CDC, /* class */ - CDC_SUBCLASS_ACM, /* subclass */ - CDC_PROTO_ATM, /* proto */ -#ifdef CONFIG_CDCSER_NOTIFSTR - CDCSER_NOTIFSTRID /* iif */ -#else - 0 /* iif */ -#endif -}; - -/* Header functional descriptor */ - -static const struct cdc_hdr_funcdesc_s g_funchdr = -{ - SIZEOF_HDR_FUNCDESC, /* size */ - USB_DESC_TYPE_CSINTERFACE, /* type */ - CDC_DSUBTYPE_HDR, /* subtype */ - { - LSBYTE(CDC_VERSIONNO), /* LS cdc */ - MSBYTE(CDC_VERSIONNO) /* MS cdc */ - } -}; - -/* ACM functional descriptor */ - -static const struct cdc_acm_funcdesc_s g_acmfunc = -{ - SIZEOF_ACM_FUNCDESC, /* size */ - USB_DESC_TYPE_CSINTERFACE, /* type */ - CDC_DSUBTYPE_ACM, /* subtype */ - 0x06 /* caps */ -}; - -/* Union functional descriptor */ - -static const struct cdc_union_funcdesc_s g_unionfunc = -{ - SIZEOF_UNION_FUNCDESC(1), /* size */ - USB_DESC_TYPE_CSINTERFACE, /* type */ - CDC_DSUBTYPE_UNION, /* subtype */ - 0, /* master */ - {1} /* slave[0] */ -}; - -/* Interrupt IN endpoint descriptor */ - -static const struct usb_epdesc_s g_epintindesc = -{ - USB_SIZEOF_EPDESC, /* len */ - USB_DESC_TYPE_ENDPOINT, /* type */ - CDCSER_EPINTIN_ADDR, /* addr */ - CDCSER_EPINTIN_ATTR, /* attr */ - { - LSBYTE(CONFIG_CDCSER_EPINTIN_FSSIZE), /* maxpacket (full speed) */ - MSBYTE(CONFIG_CDCSER_EPINTIN_FSSIZE) - }, - 0xff /* interval */ -}; - -/* Data interface descriptor */ - -static const struct usb_ifdesc_s g_dataifdesc = -{ - USB_SIZEOF_IFDESC, /* len */ - USB_DESC_TYPE_INTERFACE, /* type */ - 1, /* ifno */ - 0, /* alt */ - 2, /* neps */ - USB_CLASS_CDC_DATA, /* class */ - CDC_DATA_SUBCLASS_NONE, /* subclass */ - CDC_DATA_PROTO_NONE, /* proto */ -#ifdef CONFIG_CDCSER_DATAIFSTR - CDCSER_DATAIFSTRID /* iif */ -#else - 0 /* iif */ -#endif -}; - -/* Bulk OUT endpoint descriptor */ - -static const struct usb_epdesc_s g_epbulkoutdesc = -{ - USB_SIZEOF_EPDESC, /* len */ - USB_DESC_TYPE_ENDPOINT, /* type */ - CDCSER_EPOUTBULK_ADDR, /* addr */ - CDCSER_EPOUTBULK_ATTR, /* attr */ - { - LSBYTE(CONFIG_CDCSER_EPBULKOUT_FSSIZE), /* maxpacket (full speed) */ - MSBYTE(CONFIG_CDCSER_EPBULKOUT_FSSIZE) - }, - 1 /* interval */ -}; - -/* Bulk IN endpoint descriptor */ - -static const struct usb_epdesc_s g_epbulkindesc = -{ - USB_SIZEOF_EPDESC, /* len */ - USB_DESC_TYPE_ENDPOINT, /* type */ - CDCSER_EPINBULK_ADDR, /* addr */ - CDCSER_EPINBULK_ATTR, /* attr */ - { - LSBYTE(CONFIG_CDCSER_EPBULKIN_FSSIZE), /* maxpacket (full speed) */ - MSBYTE(CONFIG_CDCSER_EPBULKIN_FSSIZE) - }, - 1 /* interval */ -}; - -/* The components of the the configuration descriptor are maintained as - * a collection of separate descriptor structure coordinated by the - * following array. These descriptors could have been combined into - * one larger "super" configuration descriptor structure. However, I - * have concerns about compiler-dependent alignment and packing. Since - * the individual structures consist only of byte types, alignment and - * packing is not an issue. And since the are concatentated at run time - * instead of compile time, there should no issues there either. - */ - -static const struct cfgdecsc_group_s g_cfggroup[CDCSER_CFGGROUP_SIZE] = { - { - USB_SIZEOF_CFGDESC, /* 1. Configuration descriptor */ - 0, - (FAR void *)&g_cfgdesc - }, - { - USB_SIZEOF_IFDESC, /* 2. Notification interface */ - 0, - (FAR void *)&g_notifdesc - }, - { - SIZEOF_HDR_FUNCDESC, /* 3. Header functional descriptor */ - 0, - (FAR void *)&g_funchdr - }, - { - SIZEOF_ACM_FUNCDESC, /* 4. ACM functional descriptor */ - 0, - (FAR void *)&g_acmfunc - }, - { - SIZEOF_UNION_FUNCDESC(1), /* 5. Union functional descriptor */ - 0, - (FAR void *)&g_unionfunc - }, - { - USB_SIZEOF_EPDESC, /* 6. Interrupt IN endpoint descriptor */ - CONFIG_CDCSER_EPINTIN_HSSIZE, - (FAR void *)&g_epintindesc - }, - { - USB_SIZEOF_IFDESC, /* 7. Data interface descriptor */ - 0, - (FAR void *)&g_dataifdesc - }, - { - USB_SIZEOF_EPDESC, /* 8. Bulk OUT endpoint descriptor */ - CONFIG_CDCSER_EPBULKOUT_HSSIZE, - (FAR void *)&g_epbulkoutdesc - }, - { - USB_SIZEOF_EPDESC, /* 9. Bulk OUT endpoint descriptor */ - CONFIG_CDCSER_EPBULKIN_HSSIZE, - (FAR void *)&g_epbulkindesc - } -}; - -#ifdef CONFIG_USBDEV_DUALSPEED -static const struct usb_qualdesc_s g_qualdesc = -{ - USB_SIZEOF_QUALDESC, /* len */ - USB_DESC_TYPE_DEVICEQUALIFIER, /* type */ - { /* usb */ - LSBYTE(0x0200), - MSBYTE(0x0200) - }, - USB_CLASS_VENDOR_SPEC, /* class */ - 0, /* subclass */ - 0, /* protocol */ - CONFIG_CDCSER_EP0MAXPACKET, /* mxpacketsize */ - CDCSER_NCONFIGS, /* nconfigs */ - 0, /* reserved */ -}; -#endif - -/**************************************************************************** - * Private Functions - ****************************************************************************/ - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -/**************************************************************************** - * Name: cdcser_mkstrdesc - * - * Description: - * Construct a string descriptor - * - ****************************************************************************/ - -int cdcser_mkstrdesc(uint8_t id, struct usb_strdesc_s *strdesc) -{ - const char *str; - int len; - int ndata; - int i; - - switch (id) - { - case 0: - { - /* Descriptor 0 is the language id */ - - strdesc->len = 4; - strdesc->type = USB_DESC_TYPE_STRING; - strdesc->data[0] = LSBYTE(CDCSER_STR_LANGUAGE); - strdesc->data[1] = MSBYTE(CDCSER_STR_LANGUAGE); - return 4; - } - - case CDCSER_MANUFACTURERSTRID: - str = CONFIG_CDCSER_VENDORSTR; - break; - - case CDCSER_PRODUCTSTRID: - str = CONFIG_CDCSER_PRODUCTSTR; - break; - - case CDCSER_SERIALSTRID: - str = CONFIG_CDCSER_SERIALSTR; - break; - - case CDCSER_CONFIGSTRID: - str = CONFIG_CDCSER_CONFIGSTR; - break; - -#ifdef CONFIG_CDCSER_NOTIFSTR - case CDCSER_NOTIFSTRID: - str = CONFIG_CDCSER_NOTIFSTR; - break; -#endif - -#ifdef CONFIG_CDCSER_DATAIFSTR - case CDCSER_DATAIFSTRID: - str = CONFIG_CDCSER_DATAIFSTR; - break; -#endif - - default: - return -EINVAL; - } - - /* The string is utf16-le. The poor man's utf-8 to utf16-le - * conversion below will only handle 7-bit en-us ascii - */ - - len = strlen(str); - for (i = 0, ndata = 0; i < len; i++, ndata += 2) - { - strdesc->data[ndata] = str[i]; - strdesc->data[ndata+1] = 0; - } - - strdesc->len = ndata+2; - strdesc->type = USB_DESC_TYPE_STRING; - return strdesc->len; -} - -/**************************************************************************** - * Name: cdcser_getepdesc - * - * Description: - * Return a pointer to the raw device descriptor - * - ****************************************************************************/ - -FAR const struct usb_devdesc_s *cdcser_getdevdesc(void) -{ - return &g_devdesc; -} - -/**************************************************************************** - * Name: cdcser_getepdesc - * - * Description: - * Return a pointer to the raw endpoint struct (used for configuring - * endpoints) - * - ****************************************************************************/ - -FAR const struct usb_epdesc_s *cdcser_getepdesc(enum cdcser_epdesc_e epid) -{ - switch (epid) - { - case CDCSER_EPINTIN: /* Interrupt IN endpoint */ - return &g_epintindesc; - - case CDCSER_EPBULKOUT: /* Bulk OUT endpoint */ - return &g_epbulkoutdesc; - - case CDCSER_EPBULKIN: /* Bulk IN endpoint */ - return &g_epbulkindesc; - - default: - return NULL; - } -} - -/**************************************************************************** - * Name: cdcser_mkepdesc - * - * Description: - * Construct the endpoint descriptor using the correct max packet size. - * - ****************************************************************************/ - -#ifdef CONFIG_USBDEV_DUALSPEED -void cdcser_mkepdesc(num cdcser_epdesc_e epid, uint16_t mxpacket, - FAR struct usb_epdesc_s *outdesc) -{ - FAR const struct usb_epdesc_s *indesc; - - /* Copy the "canned" descriptor */ - - indesc = cdcser_getepdesc(epid) - memcpy(outdesc, indesc, USB_SIZEOF_EPDESC); - - /* Then add the correct max packet size */ - - outdesc->mxpacketsize[0] = LSBYTE(mxpacket); - outdesc->mxpacketsize[1] = MSBYTE(mxpacket); -} -#endif - -/**************************************************************************** - * Name: cdcser_mkcfgdesc - * - * Description: - * Construct the configuration descriptor - * - ****************************************************************************/ - -#ifdef CONFIG_USBDEV_DUALSPEED -int16_t cdcser_mkcfgdesc(FAR uint8_t *buf, uint8_t speed, uint8_t type) -#else -int16_t cdcser_mkcfgdesc(FAR uint8_t *buf) -#endif -{ - FAR const struct cfgdecsc_group_s *group; - FAR uint8_t *dest = buf; - int i; - -#ifdef CONFIG_USBDEV_DUALSPEED - bool hispeed = (speed == USB_SPEED_HIGH); - - /* Check for switches between high and full speed */ - - if (type == USB_DESC_TYPE_OTHERSPEEDCONFIG) - { - hispeed = !hispeed; - } -#endif - - /* Copy all of the descriptors in the group */ - - for (i = 0, dest = buf; i < CDCSER_CFGGROUP_SIZE; i++) - { - group = &g_cfggroup[i]; - - /* The "canned" descriptors all have full speed endpoint maxpacket - * sizes. If high speed is selected, we will have to change the - * endpoint maxpacket size. - * - * Is there a alternative high speed maxpacket size in the table? - * If so, that is sufficient proof that the descriptor that we - * just copied is an endpoint descriptor and needs the fixup - */ - -#ifdef CONFIG_USBDEV_DUALSPEED - if (highspeed && group->hsepsize != 0) - { - cdcser_mkepdesc(group->desc, group->hsepsize, - (FAR struct usb_epdesc_s*)dest); - } - else -#endif - /* Copy the "canned" descriptor with the full speed max packet - * size - */ - - { - memcpy(dest, group->desc, group->descsize); - } - - /* Advance to the destination location for the next descriptor */ - - dest += group->descsize; - } - - return SIZEOF_CDCSER_CFGDESC; -} - -/**************************************************************************** - * Name: cdcser_getqualdesc - * - * Description: - * Return a pointer to the raw qual descriptor - * - ****************************************************************************/ - -#ifdef CONFIG_USBDEV_DUALSPEED -FAR const struct usb_qualdesc_s *cdcser_getqualdesc(void) -{ - return &g_qualdesc; -} -#endif diff --git a/nuttx/drivers/usbdev/cdc_serial.c b/nuttx/drivers/usbdev/cdc_serial.c deleted file mode 100644 index c91b451c3..000000000 --- a/nuttx/drivers/usbdev/cdc_serial.c +++ /dev/null @@ -1,1990 +0,0 @@ -/**************************************************************************** - * drivers/usbdev/cdc_serial.c - * - * Copyright (C) 2011 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 - * 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 - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -#include -#include -#include - -#include "cdc_serial.h" - -/**************************************************************************** - * Pre-processor Definitions - ****************************************************************************/ - -/**************************************************************************** - * Private Types - ****************************************************************************/ - -/* Container to support a list of requests */ - -struct cdcser_req_s -{ - FAR struct cdcser_req_s *flink; /* Implements a singly linked list */ - FAR struct usbdev_req_s *req; /* The contained request */ -}; - -/* This structure describes the internal state of the driver */ - -struct cdcser_dev_s -{ - FAR struct uart_dev_s serdev; /* Serial device structure */ - FAR struct usbdev_s *usbdev; /* usbdev driver pointer */ - - uint8_t config; /* Configuration number */ - uint8_t nwrq; /* Number of queue write requests (in reqlist)*/ - uint8_t nrdq; /* Number of queue read requests (in epbulkout) */ - bool rxenabled; /* true: UART RX "interrupts" enabled */ - int16_t rxhead; /* Working head; used when rx int disabled */ - - uint8_t ctrlline; /* Buffered control line state */ - struct cdc_linecoding_s linecoding; /* Buffered line status */ - cdcser_callback_t callback; /* Serial event callback function */ - - FAR struct usbdev_ep_s *epintin; /* Interrupt IN endpoint structure */ - FAR struct usbdev_ep_s *epbulkin; /* Bulk IN endpoint structure */ - FAR struct usbdev_ep_s *epbulkout; /* Bulk OUT endpoint structure */ - FAR struct usbdev_req_s *ctrlreq; /* Control request */ - struct sq_queue_s reqlist; /* List of write request containers */ - - /* Pre-allocated write request containers. The write requests will - * be linked in a free list (reqlist), and used to send requests to - * EPBULKIN; Read requests will be queued in the EBULKOUT. - */ - - struct cdcser_req_s wrreqs[CONFIG_CDCSER_NWRREQS]; - struct cdcser_req_s rdreqs[CONFIG_CDCSER_NWRREQS]; - - /* Serial I/O buffers */ - - char rxbuffer[CONFIG_CDCSER_RXBUFSIZE]; - char txbuffer[CONFIG_CDCSER_TXBUFSIZE]; -}; - -/* The internal version of the class driver */ - -struct cdcser_driver_s -{ - struct usbdevclass_driver_s drvr; - FAR struct cdcser_dev_s *dev; -}; - -/* This is what is allocated */ - -struct cdcser_alloc_s -{ - struct cdcser_dev_s dev; - struct cdcser_driver_s drvr; -}; - -/**************************************************************************** - * Private Function Prototypes - ****************************************************************************/ - -/* Transfer helpers *********************************************************/ - -static uint16_t cdcser_fillrequest(FAR struct cdcser_dev_s *priv, - uint8_t *reqbuf, uint16_t reqlen); -static int cdcser_sndpacket(FAR struct cdcser_dev_s *priv); -static inline int cdcser_recvpacket(FAR struct cdcser_dev_s *priv, - uint8_t *reqbuf, uint16_t reqlen); - -/* Request helpers *********************************************************/ - -static struct usbdev_req_s *cdcser_allocreq(FAR struct usbdev_ep_s *ep, - uint16_t len); -static void cdcser_freereq(FAR struct usbdev_ep_s *ep, - FAR struct usbdev_req_s *req); - -/* Configuration ***********************************************************/ - -static void cdcser_resetconfig(FAR struct cdcser_dev_s *priv); -#ifdef CONFIG_USBDEV_DUALSPEED -static int cdcser_epconfigure(FAR struct usbdev_ep_s *ep, - enum cdcser_epdesc_e epid, uint16_t mxpacket, bool last); -#endif -static int cdcser_setconfig(FAR struct cdcser_dev_s *priv, - uint8_t config); - -/* Completion event handlers ***********************************************/ - -static void cdcser_ep0incomplete(FAR struct usbdev_ep_s *ep, - FAR struct usbdev_req_s *req); -static void cdcser_rdcomplete(FAR struct usbdev_ep_s *ep, - FAR struct usbdev_req_s *req); -static void cdcser_wrcomplete(FAR struct usbdev_ep_s *ep, - FAR struct usbdev_req_s *req); - -/* USB class device ********************************************************/ - -static int cdcser_bind(FAR struct usbdev_s *dev, - FAR struct usbdevclass_driver_s *driver); -static void cdcser_unbind(FAR struct usbdev_s *dev); -static int cdcser_setup(FAR struct usbdev_s *dev, - const struct usb_ctrlreq_s *ctrl); -static void cdcser_disconnect(FAR struct usbdev_s *dev); - -/* UART Operationt **********************************************************/ - -static int cdcuart_setup(FAR struct uart_dev_s *dev); -static void cdcuart_shutdown(FAR struct uart_dev_s *dev); -static int cdcuart_attach(FAR struct uart_dev_s *dev); -static void cdcuart_detach(FAR struct uart_dev_s *dev); -static int cdcuart_ioctl(FAR struct file *filep,int cmd,unsigned long arg); -static void cdcuart_rxint(FAR struct uart_dev_s *dev, bool enable); -static void cdcuart_txint(FAR struct uart_dev_s *dev, bool enable); -static bool cdcuart_txempty(FAR struct uart_dev_s *dev); - -/**************************************************************************** - * Private Variables - ****************************************************************************/ -/* USB class device *********************************************************/ - -static const struct usbdevclass_driverops_s g_driverops = -{ - cdcser_bind, /* bind */ - cdcser_unbind, /* unbind */ - cdcser_setup, /* setup */ - cdcser_disconnect, /* disconnect */ - NULL, /* suspend */ - NULL, /* resume */ -}; - -/* Serial port **************************************************************/ - -static const struct uart_ops_s g_uartops = -{ - cdcuart_setup, /* setup */ - cdcuart_shutdown, /* shutdown */ - cdcuart_attach, /* attach */ - cdcuart_detach, /* detach */ - cdcuart_ioctl, /* ioctl */ - NULL, /* receive */ - cdcuart_rxint, /* rxinit */ - NULL, /* rxavailable */ - NULL, /* send */ - cdcuart_txint, /* txinit */ - NULL, /* txready */ - cdcuart_txempty /* txempty */ -}; - -/**************************************************************************** - * Private Functions - ****************************************************************************/ - -/**************************************************************************** - * Name: cdcser_fillrequest - * - * Description: - * If there is data to send it is copied to the given buffer. Called - * either to initiate the first write operation, or from the completion - * interrupt handler service consecutive write operations. - * - * NOTE: The USB serial driver does not use the serial drivers - * uart_xmitchars() API. That logic is essentially duplicated here because - * unlike UART hardware, we need to be able to handle writes not byte-by-byte, - * but packet-by-packet. Unfortunately, that decision also exposes some - * internals of the serial driver in the following. - * - ****************************************************************************/ - -static uint16_t cdcser_fillrequest(FAR struct cdcser_dev_s *priv, uint8_t *reqbuf, - uint16_t reqlen) -{ - FAR uart_dev_t *serdev = &priv->serdev; - FAR struct uart_buffer_s *xmit = &serdev->xmit; - irqstate_t flags; - uint16_t nbytes = 0; - - /* Disable interrupts */ - - flags = irqsave(); - - /* Transfer bytes while we have bytes available and there is room in the request */ - - while (xmit->head != xmit->tail && nbytes < reqlen) - { - *reqbuf++ = xmit->buffer[xmit->tail]; - nbytes++; - - /* Increment the tail pointer */ - - if (++(xmit->tail) >= xmit->size) - { - xmit->tail = 0; - } - } - - /* When all of the characters have been sent from the buffer - * disable the "TX interrupt". - */ - - if (xmit->head == xmit->tail) - { - uart_disabletxint(serdev); - } - - /* If any bytes were removed from the buffer, inform any waiters - * there there is space available. - */ - - if (nbytes) - { - uart_datasent(serdev); - } - - irqrestore(flags); - return nbytes; -} - -/**************************************************************************** - * Name: cdcser_sndpacket - * - * Description: - * This function obtains write requests, transfers the TX data into the - * request, and submits the requests to the USB controller. This continues - * untils either (1) there are no further packets available, or (2) thre is - * no further data to send. - * - ****************************************************************************/ - -static int cdcser_sndpacket(FAR struct cdcser_dev_s *priv) -{ - FAR struct usbdev_ep_s *ep; - FAR struct usbdev_req_s *req; - FAR struct cdcser_req_s *reqcontainer; - irqstate_t flags; - int len; - int ret = OK; - -#ifdef CONFIG_DEBUG - if (priv == NULL) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_INVALIDARG), 0); - return -ENODEV; - } -#endif - - flags = irqsave(); - - /* Use our IN endpoint for the transfer */ - - ep = priv->epbulkin; - - /* Loop until either (1) we run out or write requests, or (2) cdcser_fillrequest() - * is unable to fill the request with data (i.e., untilthere is no more data - * to be sent). - */ - - uvdbg("head=%d tail=%d nwrq=%d empty=%d\n", - priv->serdev.xmit.head, priv->serdev.xmit.tail, - priv->nwrq, sq_empty(&priv->reqlist)); - - while (!sq_empty(&priv->reqlist)) - { - /* Peek at the request in the container at the head of the list */ - - reqcontainer = (struct cdcser_req_s *)sq_peek(&priv->reqlist); - req = reqcontainer->req; - - /* Fill the request with serial TX data */ - - len = cdcser_fillrequest(priv, req->buf, req->len); - if (len > 0) - { - /* Remove the empty container from the request list */ - - (void)sq_remfirst(&priv->reqlist); - priv->nwrq--; - - /* Then submit the request to the endpoint */ - - req->len = len; - req->priv = reqcontainer; - req->flags = USBDEV_REQFLAGS_NULLPKT; - ret = EP_SUBMIT(ep, req); - if (ret != OK) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_SUBMITFAIL), (uint16_t)-ret); - break; - } - } - else - { - break; - } - } - - irqrestore(flags); - return ret; -} - -/**************************************************************************** - * Name: cdcser_recvpacket - * - * Description: - * A normal completion event was received by the read completion handler - * at the interrupt level (with interrupts disabled). This function handles - * the USB packet and provides the received data to the uart RX buffer. - * - * Assumptions: - * Called from the USB interrupt handler with interrupts disabled. - * - ****************************************************************************/ - -static inline int cdcser_recvpacket(FAR struct cdcser_dev_s *priv, - uint8_t *reqbuf, uint16_t reqlen) -{ - FAR uart_dev_t *serdev = &priv->serdev; - FAR struct uart_buffer_s *recv = &serdev->recv; - uint16_t currhead; - uint16_t nexthead; - uint16_t nbytes = 0; - - uvdbg("head=%d tail=%d nrdq=%d reqlen=%d\n", - priv->serdev.recv.head, priv->serdev.recv.tail, priv->nrdq, reqlen); - - /* Get the next head index. During the time that RX interrupts are disabled, the - * the serial driver will be extracting data from the circular buffer and modifying - * recv.tail. During this time, we should avoid modifying recv.head; Instead we will - * use a shadow copy of the index. When interrupts are restored, the real recv.head - * will be updated with this indes. - */ - - if (priv->rxenabled) - { - currhead = recv->head; - } - else - { - currhead = priv->rxhead; - } - - /* Pre-calculate the head index and check for wrap around. We need to do this - * so that we can determine if the circular buffer will overrun BEFORE we - * overrun the buffer! - */ - - nexthead = currhead + 1; - if (nexthead >= recv->size) - { - nexthead = 0; - } - - /* Then copy data into the RX buffer until either: (1) all of the data has been - * copied, or (2) the RX buffer is full. NOTE: If the RX buffer becomes full, - * then we have overrun the serial driver and data will be lost. - */ - - while (nexthead != recv->tail && nbytes < reqlen) - { - /* Copy one byte to the head of the circular RX buffer */ - - recv->buffer[currhead] = *reqbuf++; - - /* Update counts and indices */ - - currhead = nexthead; - nbytes++; - - /* Increment the head index and check for wrap around */ - - nexthead = currhead + 1; - if (nexthead >= recv->size) - { - nexthead = 0; - } - } - - /* Write back the head pointer using the shadow index if RX "interrupts" - * are disabled. - */ - - if (priv->rxenabled) - { - recv->head = currhead; - } - else - { - priv->rxhead = currhead; - } - - /* If data was added to the incoming serial buffer, then wake up any - * threads is waiting for incoming data. If we are running in an interrupt - * handler, then the serial driver will not run until the interrupt handler - * returns. - */ - - if (priv->rxenabled && nbytes > 0) - { - uart_datareceived(serdev); - } - - /* Return an error if the entire packet could not be transferred */ - - if (nbytes < reqlen) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_RXOVERRUN), 0); - return -ENOSPC; - } - return OK; -} - -/**************************************************************************** - * Name: cdcser_allocreq - * - * Description: - * Allocate a request instance along with its buffer - * - ****************************************************************************/ - -static struct usbdev_req_s *cdcser_allocreq(FAR struct usbdev_ep_s *ep, - uint16_t len) -{ - FAR struct usbdev_req_s *req; - - req = EP_ALLOCREQ(ep); - if (req != NULL) - { - req->len = len; - req->buf = EP_ALLOCBUFFER(ep, len); - if (!req->buf) - { - EP_FREEREQ(ep, req); - req = NULL; - } - } - return req; -} - -/**************************************************************************** - * Name: cdcser_freereq - * - * Description: - * Free a request instance along with its buffer - * - ****************************************************************************/ - -static void cdcser_freereq(FAR struct usbdev_ep_s *ep, - FAR struct usbdev_req_s *req) -{ - if (ep != NULL && req != NULL) - { - if (req->buf != NULL) - { - EP_FREEBUFFER(ep, req->buf); - } - EP_FREEREQ(ep, req); - } -} - -/**************************************************************************** - * Name: cdcser_resetconfig - * - * Description: - * Mark the device as not configured and disable all endpoints. - * - ****************************************************************************/ - -static void cdcser_resetconfig(FAR struct cdcser_dev_s *priv) -{ - /* Are we configured? */ - - if (priv->config != CDCSER_CONFIGIDNONE) - { - /* Yes.. but not anymore */ - - priv->config = CDCSER_CONFIGIDNONE; - - /* Disable endpoints. This should force completion of all pending - * transfers. - */ - - EP_DISABLE(priv->epintin); - EP_DISABLE(priv->epbulkin); - EP_DISABLE(priv->epbulkout); - } -} - -/**************************************************************************** - * Name: cdcser_epconfigure - * - * Description: - * Configure one endpoint. - * - ****************************************************************************/ - -#ifdef CONFIG_USBDEV_DUALSPEED -static int cdcser_epconfigure(FAR struct usbdev_ep_s *ep, - enum cdcser_epdesc_e epid, uint16_t mxpacket, - bool last) -{ - struct usb_epdesc_s epdesc; - cdcser_mkepdesc(epid, mxpacket, &epdesc); - return EP_CONFIGURE(ep, &epdesc, last); -} -#endif - -/**************************************************************************** - * Name: cdcser_setconfig - * - * Description: - * Set the device configuration by allocating and configuring endpoints and - * by allocating and queue read and write requests. - * - ****************************************************************************/ - -static int cdcser_setconfig(FAR struct cdcser_dev_s *priv, uint8_t config) -{ - FAR struct usbdev_req_s *req; - int i; - int ret = 0; - -#if CONFIG_DEBUG - if (priv == NULL) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_INVALIDARG), 0); - return -EIO; - } -#endif - - if (config == priv->config) - { - /* Already configured -- Do nothing */ - - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_ALREADYCONFIGURED), 0); - return 0; - } - - /* Discard the previous configuration data */ - - cdcser_resetconfig(priv); - - /* Was this a request to simply discard the current configuration? */ - - if (config == CDCSER_CONFIGIDNONE) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_CONFIGNONE), 0); - return 0; - } - - /* We only accept one configuration */ - - if (config != CDCSER_CONFIGID) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_CONFIGIDBAD), 0); - return -EINVAL; - } - - /* Configure the IN interrupt endpoint */ - -#ifdef CONFIG_USBDEV_DUALSPEED - if (priv->usbdev->speed == USB_SPEED_HIGH) - { - ret = cdcser_epconfigure(priv->epintin, CDCSER_EPINTIN, - CONFIG_CDCSER_EPINTIN_HSSIZE, false); - } - else -#endif - { - ret = EP_CONFIGURE(priv->epintin, - cdcser_getepdesc(CDCSER_EPINTIN), false); - } - - if (ret < 0) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_EPINTINCONFIGFAIL), 0); - goto errout; - } - priv->epintin->priv = priv; - - /* Configure the IN bulk endpoint */ - -#ifdef CONFIG_USBDEV_DUALSPEED - if (priv->usbdev->speed == USB_SPEED_HIGH) - { - ret = cdcser_epconfigure(priv->epbulkin, CDCSER_EPBULKIN, - CONFIG_CDCSER_EPBULKIN_HSSIZE, false); - } - else -#endif - { - ret = EP_CONFIGURE(priv->epbulkin, - cdcser_getepdesc(CDCSER_EPBULKIN), false); - } - - if (ret < 0) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_EPBULKINCONFIGFAIL), 0); - goto errout; - } - - priv->epbulkin->priv = priv; - - /* Configure the OUT bulk endpoint */ - -#ifdef CONFIG_USBDEV_DUALSPEED - if (priv->usbdev->speed == USB_SPEED_HIGH) - { - ret = cdcser_epconfigure(priv->epbulkout, CDCSER_EPBULKOUT, - CONFIG_CDCSER_EPBULKOUT_HSSIZE, true); - } - else -#endif - { - ret = EP_CONFIGURE(priv->epbulkout, - cdcser_getepdesc(CDCSER_EPBULKOUT), true); - } - - if (ret < 0) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_EPBULKOUTCONFIGFAIL), 0); - goto errout; - } - - priv->epbulkout->priv = priv; - - /* Queue read requests in the bulk OUT endpoint */ - - DEBUGASSERT(priv->nrdq == 0); - for (i = 0; i < CONFIG_CDCSER_NRDREQS; i++) - { - req = priv->rdreqs[i].req; - req->callback = cdcser_rdcomplete; - ret = EP_SUBMIT(priv->epbulkout, req); - if (ret != OK) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_RDSUBMIT), (uint16_t)-ret); - goto errout; - } - priv->nrdq++; - } - - priv->config = config; - return OK; - -errout: - cdcser_resetconfig(priv); - return ret; -} - -/**************************************************************************** - * Name: cdcser_ep0incomplete - * - * Description: - * Handle completion of EP0 control operations - * - ****************************************************************************/ - -static void cdcser_ep0incomplete(FAR struct usbdev_ep_s *ep, - FAR struct usbdev_req_s *req) -{ - if (req->result || req->xfrd != req->len) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_REQRESULT), (uint16_t)-req->result); - } -} - -/**************************************************************************** - * Name: cdcser_rdcomplete - * - * Description: - * Handle completion of read request on the bulk OUT endpoint. This - * is handled like the receipt of serial data on the "UART" - * - ****************************************************************************/ - -static void cdcser_rdcomplete(FAR struct usbdev_ep_s *ep, - FAR struct usbdev_req_s *req) -{ - FAR struct cdcser_dev_s *priv; - irqstate_t flags; - int ret; - - /* Sanity check */ - -#ifdef CONFIG_DEBUG - if (!ep || !ep->priv || !req) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_INVALIDARG), 0); - return; - } -#endif - - /* Extract references to private data */ - - priv = (FAR struct cdcser_dev_s*)ep->priv; - - /* Process the received data unless this is some unusual condition */ - - flags = irqsave(); - switch (req->result) - { - case 0: /* Normal completion */ - usbtrace(TRACE_CLASSRDCOMPLETE, priv->nrdq); - cdcser_recvpacket(priv, req->buf, req->xfrd); - break; - - case -ESHUTDOWN: /* Disconnection */ - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_RDSHUTDOWN), 0); - priv->nrdq--; - irqrestore(flags); - return; - - default: /* Some other error occurred */ - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_RDUNEXPECTED), (uint16_t)-req->result); - break; - }; - - /* Requeue the read request */ - -#ifdef CONFIG_CDCSER_BULKREQLEN - req->len = max(CONFIG_CDCSER_BULKREQLEN, ep->maxpacket); -#else - req->len = ep->maxpacket; -#endif - - ret = EP_SUBMIT(ep, req); - if (ret != OK) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_RDSUBMIT), (uint16_t)-req->result); - } - irqrestore(flags); -} - -/**************************************************************************** - * Name: cdcser_wrcomplete - * - * Description: - * Handle completion of write request. This function probably executes - * in the context of an interrupt handler. - * - ****************************************************************************/ - -static void cdcser_wrcomplete(FAR struct usbdev_ep_s *ep, - FAR struct usbdev_req_s *req) -{ - FAR struct cdcser_dev_s *priv; - FAR struct cdcser_req_s *reqcontainer; - irqstate_t flags; - - /* Sanity check */ - -#ifdef CONFIG_DEBUG - if (!ep || !ep->priv || !req || !req->priv) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_INVALIDARG), 0); - return; - } -#endif - - /* Extract references to our private data */ - - priv = (FAR struct cdcser_dev_s *)ep->priv; - reqcontainer = (FAR struct cdcser_req_s *)req->priv; - - /* Return the write request to the free list */ - - flags = irqsave(); - sq_addlast((sq_entry_t*)reqcontainer, &priv->reqlist); - priv->nwrq++; - irqrestore(flags); - - /* Send the next packet unless this was some unusual termination - * condition - */ - - switch (req->result) - { - case OK: /* Normal completion */ - usbtrace(TRACE_CLASSWRCOMPLETE, priv->nwrq); - cdcser_sndpacket(priv); - break; - - case -ESHUTDOWN: /* Disconnection */ - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_WRSHUTDOWN), priv->nwrq); - break; - - default: /* Some other error occurred */ - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_WRUNEXPECTED), (uint16_t)-req->result); - break; - } -} - -/**************************************************************************** - * USB Class Driver Methods - ****************************************************************************/ - -/**************************************************************************** - * Name: cdcser_bind - * - * Description: - * Invoked when the driver is bound to a USB device driver - * - ****************************************************************************/ - -static int cdcser_bind(FAR struct usbdev_s *dev, FAR struct usbdevclass_driver_s *driver) -{ - FAR struct cdcser_dev_s *priv = ((struct cdcser_driver_s*)driver)->dev; - FAR struct cdcser_req_s *reqcontainer; - irqstate_t flags; - uint16_t reqlen; - int ret; - int i; - - usbtrace(TRACE_CLASSBIND, 0); - - /* Bind the structures */ - - priv->usbdev = dev; - dev->ep0->priv = priv; - - /* Preallocate control request */ - - priv->ctrlreq = cdcser_allocreq(dev->ep0, CDCSER_MXDESCLEN); - if (priv->ctrlreq == NULL) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_ALLOCCTRLREQ), 0); - ret = -ENOMEM; - goto errout; - } - priv->ctrlreq->callback = cdcser_ep0incomplete; - - /* Pre-allocate all endpoints... the endpoints will not be functional - * until the SET CONFIGURATION request is processed in cdcser_setconfig. - * This is done here because there may be calls to kmalloc and the SET - * CONFIGURATION processing probably occurrs within interrupt handling - * logic where kmalloc calls will fail. - */ - - /* Pre-allocate the IN interrupt endpoint */ - - priv->epintin = DEV_ALLOCEP(dev, CDCSER_EPINTIN_ADDR, true, USB_EP_ATTR_XFER_INT); - if (!priv->epintin) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_EPINTINALLOCFAIL), 0); - ret = -ENODEV; - goto errout; - } - priv->epintin->priv = priv; - - /* Pre-allocate the IN bulk endpoint */ - - priv->epbulkin = DEV_ALLOCEP(dev, CDCSER_EPINBULK_ADDR, true, USB_EP_ATTR_XFER_BULK); - if (!priv->epbulkin) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_EPBULKINALLOCFAIL), 0); - ret = -ENODEV; - goto errout; - } - priv->epbulkin->priv = priv; - - /* Pre-allocate the OUT bulk endpoint */ - - priv->epbulkout = DEV_ALLOCEP(dev, CDCSER_EPOUTBULK_ADDR, false, USB_EP_ATTR_XFER_BULK); - if (!priv->epbulkout) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_EPBULKOUTALLOCFAIL), 0); - ret = -ENODEV; - goto errout; - } - priv->epbulkout->priv = priv; - - /* Pre-allocate read requests */ - -#ifdef CONFIG_CDCSER_BULKREQLEN - reqlen = max(CONFIG_CDCSER_BULKREQLEN, priv->epbulkout->maxpacket); -#else - reqlen = priv->epbulkout->maxpacket; -#endif - - for (i = 0; i < CONFIG_CDCSER_NRDREQS; i++) - { - reqcontainer = &priv->rdreqs[i]; - reqcontainer->req = cdcser_allocreq(priv->epbulkout, reqlen); - if (reqcontainer->req == NULL) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_RDALLOCREQ), -ENOMEM); - ret = -ENOMEM; - goto errout; - } - reqcontainer->req->priv = reqcontainer; - reqcontainer->req->callback = cdcser_rdcomplete; - } - - /* Pre-allocate write request containers and put in a free list */ - -#ifdef CONFIG_CDCSER_BULKREQLEN - reqlen = max(CONFIG_CDCSER_BULKREQLEN, priv->epbulkin->maxpacket); -#else - reqlen = priv->epbulkin->maxpacket; -#endif - - for (i = 0; i < CONFIG_CDCSER_NWRREQS; i++) - { - reqcontainer = &priv->wrreqs[i]; - reqcontainer->req = cdcser_allocreq(priv->epbulkin, reqlen); - if (reqcontainer->req == NULL) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_WRALLOCREQ), -ENOMEM); - ret = -ENOMEM; - goto errout; - } - reqcontainer->req->priv = reqcontainer; - reqcontainer->req->callback = cdcser_wrcomplete; - - flags = irqsave(); - sq_addlast((sq_entry_t*)reqcontainer, &priv->reqlist); - priv->nwrq++; /* Count of write requests available */ - irqrestore(flags); - } - - /* Report if we are selfpowered */ - -#ifdef CONFIG_USBDEV_SELFPOWERED - DEV_SETSELFPOWERED(dev); -#endif - - /* And pull-up the data line for the soft connect function */ - - DEV_CONNECT(dev); - return OK; - -errout: - cdcser_unbind(dev); - return ret; -} - -/**************************************************************************** - * Name: cdcser_unbind - * - * Description: - * Invoked when the driver is unbound from a USB device driver - * - ****************************************************************************/ - -static void cdcser_unbind(FAR struct usbdev_s *dev) -{ - FAR struct cdcser_dev_s *priv; - FAR struct cdcser_req_s *reqcontainer; - irqstate_t flags; - int i; - - usbtrace(TRACE_CLASSUNBIND, 0); - -#ifdef CONFIG_DEBUG - if (!dev || !dev->ep0) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_INVALIDARG), 0); - return; - } -#endif - - /* Extract reference to private data */ - - priv = (FAR struct cdcser_dev_s *)dev->ep0->priv; - -#ifdef CONFIG_DEBUG - if (!priv) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_EP0NOTBOUND), 0); - return; - } -#endif - - /* Make sure that we are not already unbound */ - - if (priv != NULL) - { - /* Make sure that the endpoints have been unconfigured. If - * we were terminated gracefully, then the configuration should - * already have been reset. If not, then calling cdcser_resetconfig - * should cause the endpoints to immediately terminate all - * transfers and return the requests to us (with result == -ESHUTDOWN) - */ - - cdcser_resetconfig(priv); - up_mdelay(50); - - /* Free the interrupt IN endpoint */ - - if (priv->epintin) - { - DEV_FREEEP(dev, priv->epintin); - priv->epintin = NULL; - } - - /* Free the bulk IN endpoint */ - - if (priv->epbulkin) - { - DEV_FREEEP(dev, priv->epbulkin); - priv->epbulkin = NULL; - } - - /* Free the pre-allocated control request */ - - if (priv->ctrlreq != NULL) - { - cdcser_freereq(dev->ep0, priv->ctrlreq); - priv->ctrlreq = NULL; - } - - /* Free pre-allocated read requests (which should all have - * been returned to the free list at this time -- we don't check) - */ - - DEBUGASSERT(priv->nrdq == 0); - for (i = 0; i < CONFIG_CDCSER_NRDREQS; i++) - { - reqcontainer = &priv->rdreqs[i]; - if (reqcontainer->req) - { - cdcser_freereq(priv->epbulkout, reqcontainer->req); - reqcontainer->req = NULL; - } - } - - /* Free the bulk OUT endpoint */ - - if (priv->epbulkout) - { - DEV_FREEEP(dev, priv->epbulkout); - priv->epbulkout = NULL; - } - - /* Free write requests that are not in use (which should be all - * of them - */ - - flags = irqsave(); - DEBUGASSERT(priv->nwrq == CONFIG_CDCSER_NWRREQS); - while (!sq_empty(&priv->reqlist)) - { - reqcontainer = (struct cdcser_req_s *)sq_remfirst(&priv->reqlist); - if (reqcontainer->req != NULL) - { - cdcser_freereq(priv->epbulkin, reqcontainer->req); - priv->nwrq--; /* Number of write requests queued */ - } - } - DEBUGASSERT(priv->nwrq == 0); - irqrestore(flags); - } - - /* Clear out all data in the circular buffer */ - - priv->serdev.xmit.head = 0; - priv->serdev.xmit.tail = 0; -} - -/**************************************************************************** - * Name: cdcser_setup - * - * Description: - * Invoked for ep0 control requests. This function probably executes - * in the context of an interrupt handler. - * - ****************************************************************************/ - -static int cdcser_setup(FAR struct usbdev_s *dev, const struct usb_ctrlreq_s *ctrl) -{ - FAR struct cdcser_dev_s *priv; - FAR struct usbdev_req_s *ctrlreq; - uint16_t value; - uint16_t index; - uint16_t len; - int ret = -EOPNOTSUPP; - -#ifdef CONFIG_DEBUG - if (!dev || !dev->ep0 || !ctrl) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_INVALIDARG), 0); - return -EIO; - } -#endif - - /* Extract reference to private data */ - - usbtrace(TRACE_CLASSSETUP, ctrl->req); - priv = (FAR struct cdcser_dev_s *)dev->ep0->priv; - -#ifdef CONFIG_DEBUG - if (!priv || !priv->ctrlreq) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_EP0NOTBOUND), 0); - return -ENODEV; - } -#endif - ctrlreq = priv->ctrlreq; - - /* Extract the little-endian 16-bit values to host order */ - - value = GETUINT16(ctrl->value); - index = GETUINT16(ctrl->index); - len = GETUINT16(ctrl->len); - - uvdbg("type=%02x req=%02x value=%04x index=%04x len=%04x\n", - ctrl->type, ctrl->req, value, index, len); - - switch (ctrl->type & USB_REQ_TYPE_MASK) - { - /*********************************************************************** - * Standard Requests - ***********************************************************************/ - - case USB_REQ_TYPE_STANDARD: - { - switch (ctrl->req) - { - case USB_REQ_GETDESCRIPTOR: - { - /* The value field specifies the descriptor type in the MS byte and the - * descriptor index in the LS byte (order is little endian) - */ - - switch (ctrl->value[1]) - { - case USB_DESC_TYPE_DEVICE: - { - ret = USB_SIZEOF_DEVDESC; - memcpy(ctrlreq->buf, cdcser_getdevdesc(), ret); - } - break; - -#ifdef CONFIG_USBDEV_DUALSPEED - case USB_DESC_TYPE_DEVICEQUALIFIER: - { - ret = USB_SIZEOF_QUALDESC; - memcpy(ctrlreq->buf, cdcser_getqualdesc(), ret); - } - break; - - case USB_DESC_TYPE_OTHERSPEEDCONFIG: -#endif /* CONFIG_USBDEV_DUALSPEED */ - - case USB_DESC_TYPE_CONFIG: - { -#ifdef CONFIG_USBDEV_DUALSPEED - ret = cdcser_mkcfgdesc(ctrlreq->buf, dev->speed, ctrl->req); -#else - ret = cdcser_mkcfgdesc(ctrlreq->buf); -#endif - } - break; - - case USB_DESC_TYPE_STRING: - { - /* index == language code. */ - - ret = cdcser_mkstrdesc(ctrl->value[0], (struct usb_strdesc_s *)ctrlreq->buf); - } - break; - - default: - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_GETUNKNOWNDESC), value); - } - break; - } - } - break; - - case USB_REQ_SETCONFIGURATION: - { - if (ctrl->type == 0) - { - ret = cdcser_setconfig(priv, value); - } - } - break; - - case USB_REQ_GETCONFIGURATION: - { - if (ctrl->type == USB_DIR_IN) - { - *(uint8_t*)ctrlreq->buf = priv->config; - ret = 1; - } - } - break; - - case USB_REQ_SETINTERFACE: - { - if (ctrl->type == USB_REQ_RECIPIENT_INTERFACE) - { - if (priv->config == CDCSER_CONFIGID && - index == CDCSER_INTERFACEID && - value == CDCSER_ALTINTERFACEID) - { - cdcser_resetconfig(priv); - cdcser_setconfig(priv, priv->config); - ret = 0; - } - } - } - break; - - case USB_REQ_GETINTERFACE: - { - if (ctrl->type == (USB_DIR_IN|USB_REQ_RECIPIENT_INTERFACE) && - priv->config == CDCSER_CONFIGIDNONE) - { - if (index != CDCSER_INTERFACEID) - { - ret = -EDOM; - } - else - { - *(uint8_t*) ctrlreq->buf = CDCSER_ALTINTERFACEID; - ret = 1; - } - } - } - break; - - default: - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_UNSUPPORTEDSTDREQ), ctrl->req); - break; - } - } - break; - - /************************************************************************ - * CDC ACM-Specific Requests - ************************************************************************/ - - /* ACM_GET_LINE_CODING requests current DTE rate, stop-bits, parity, and - * number-of-character bits. (Optional) - */ - - case ACM_GET_LINE_CODING: - { - if (ctrl->type == (USB_DIR_IN|USB_REQ_TYPE_CLASS|USB_REQ_RECIPIENT_INTERFACE)) - { - /* Return the current line status from the private data structure */ - - memcpy(ctrlreq->buf, &priv->linecoding, SIZEOF_CDC_LINECODING); - ret = SIZEOF_CDC_LINECODING; - } - else - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_UNSUPPORTEDCLASSREQ), ctrl->type); - } - } - break; - - /* ACM_SET_LINE_CODING configures DTE rate, stop-bits, parity, and - * number-of-character bits. (Optional) - */ - - case ACM_SET_LINE_CODING: - { - if (ctrl->type == (USB_DIR_OUT|USB_REQ_TYPE_CLASS|USB_REQ_RECIPIENT_INTERFACE)) - { - /* Save the new line coding in the private data structure */ - - memcpy(&priv->linecoding, ctrlreq->buf, min(len, 7)); - ret = 0; - - /* If there is a registered callback to receive line status info, then - * callout now. - */ - - if (priv->callback) - { - priv->callback(CDCSER_EVENT_LINECODING); - } - } - else - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_UNSUPPORTEDCLASSREQ), ctrl->type); - } - } - break; - - /* ACM_SET_CTRL_LINE_STATE: RS-232 signal used to tell the DCE device the - * DTE device is now present. (Optional) - */ - - case ACM_SET_CTRL_LINE_STATE: - { - if (ctrl->type == (USB_DIR_OUT|USB_REQ_TYPE_CLASS|USB_REQ_RECIPIENT_INTERFACE)) - { - /* Save the control line state in the private data structure. Only bits - * 0 and 1 have meaning. - */ - - priv->ctrlline = value & 3; - ret = 0; - - /* If there is a registered callback to receive control line status info, - * then callout now. - */ - - if (priv->callback) - { - priv->callback(CDCSER_EVENT_CTRLLINE); - } - } - else - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_UNSUPPORTEDCLASSREQ), ctrl->type); - } - } - break; - - /* Sends special carrier*/ - - case ACM_SEND_BREAK: - { - if (ctrl->type == (USB_DIR_OUT|USB_REQ_TYPE_CLASS|USB_REQ_RECIPIENT_INTERFACE)) - { - /* If there is a registered callback to handle the SendBreak request, - * then callout now. - */ - - ret = 0; - if (priv->callback) - { - priv->callback(CDCSER_EVENT_SENDBREAK); - } - } - else - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_UNSUPPORTEDCLASSREQ), ctrl->type); - } - } - break; - - default: - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_UNSUPPORTEDTYPE), ctrl->type); - break; - } - - /* Respond to the setup command if data was returned. On an error return - * value (ret < 0), the USB driver will stall. - */ - - if (ret >= 0) - { - ctrlreq->len = min(len, ret); - ctrlreq->flags = USBDEV_REQFLAGS_NULLPKT; - ret = EP_SUBMIT(dev->ep0, ctrlreq); - if (ret < 0) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_EPRESPQ), (uint16_t)-ret); - ctrlreq->result = OK; - cdcser_ep0incomplete(dev->ep0, ctrlreq); - } - } - return ret; -} - -/**************************************************************************** - * Name: cdcser_disconnect - * - * Description: - * Invoked after all transfers have been stopped, when the host is - * disconnected. This function is probably called from the context of an - * interrupt handler. - * - ****************************************************************************/ - -static void cdcser_disconnect(FAR struct usbdev_s *dev) -{ - FAR struct cdcser_dev_s *priv; - irqstate_t flags; - - usbtrace(TRACE_CLASSDISCONNECT, 0); - -#ifdef CONFIG_DEBUG - if (!dev || !dev->ep0) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_INVALIDARG), 0); - return; - } -#endif - - /* Extract reference to private data */ - - priv = (FAR struct cdcser_dev_s *)dev->ep0->priv; - -#ifdef CONFIG_DEBUG - if (!priv) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_EP0NOTBOUND), 0); - return; - } -#endif - - /* Reset the configuration */ - - flags = irqsave(); - cdcser_resetconfig(priv); - - /* Clear out all data in the circular buffer */ - - priv->serdev.xmit.head = 0; - priv->serdev.xmit.tail = 0; - irqrestore(flags); - - /* Perform the soft connect function so that we will we can be - * re-enumerated. - */ - - DEV_CONNECT(dev); -} - -/**************************************************************************** - * Serial Device Methods - ****************************************************************************/ - -/**************************************************************************** - * Name: cdcuart_setup - * - * Description: - * This method is called the first time that the serial port is opened. - * - ****************************************************************************/ - -static int cdcuart_setup(FAR struct uart_dev_s *dev) -{ - FAR struct cdcser_dev_s *priv; - - usbtrace(CDCSER_CLASSAPI_SETUP, 0); - - /* Sanity check */ - -#if CONFIG_DEBUG - if (!dev || !dev->priv) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_INVALIDARG), 0); - return -EIO; - } -#endif - - /* Extract reference to private data */ - - priv = (FAR struct cdcser_dev_s*)dev->priv; - - /* Check if we have been configured */ - - if (priv->config == CDCSER_CONFIGIDNONE) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_SETUPNOTCONNECTED), 0); - return -ENOTCONN; - } - - return OK; -} - -/**************************************************************************** - * Name: cdcuart_shutdown - * - * Description: - * This method is called when the serial port is closed. This operation - * is very simple for the USB serial backend because the serial driver - * has already assured that the TX data has full drained -- it calls - * cdcuart_txempty() until that function returns true before calling this - * function. - * - ****************************************************************************/ - -static void cdcuart_shutdown(FAR struct uart_dev_s *dev) -{ - usbtrace(CDCSER_CLASSAPI_SHUTDOWN, 0); - - /* Sanity check */ - -#if CONFIG_DEBUG - if (!dev || !dev->priv) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_INVALIDARG), 0); - } -#endif -} - -/**************************************************************************** - * Name: cdcuart_attach - * - * Description: - * Does not apply to the USB serial class device - * - ****************************************************************************/ - -static int cdcuart_attach(FAR struct uart_dev_s *dev) -{ - usbtrace(CDCSER_CLASSAPI_ATTACH, 0); - return OK; -} - -/**************************************************************************** - * Name: cdcuart_detach - * - * Description: -* Does not apply to the USB serial class device - * - ****************************************************************************/ - -static void cdcuart_detach(FAR struct uart_dev_s *dev) -{ - usbtrace(CDCSER_CLASSAPI_DETACH, 0); -} - -/**************************************************************************** - * Name: cdcuart_ioctl - * - * Description: - * All ioctl calls will be routed through this method - * - ****************************************************************************/ - -static int cdcuart_ioctl(FAR struct file *filep,int cmd,unsigned long arg) -{ - struct inode *inode = filep->f_inode; - struct cdcser_dev_s *priv = inode->i_private; - int ret = OK; - - switch (cmd) - { - /* CAICO_REGISTERCB - * Register a callback for serial event notification. Argument: - * cdcser_callback_t. See cdcser_callback_t type definition below. - * NOTE: The callback will most likely invoked at the interrupt level. - * The called back function should, therefore, limit its operations to - * invoking some kind of IPC to handle the serial event in some normal - * task environment. - */ - - case CAIOC_REGISTERCB: - { - /* Save the new callback function */ - - priv->callback = (cdcser_callback_t)((uintptr_t)arg); - } - break; - - /* CAIOC_GETLINECODING - * Get current line coding. Argument: struct cdc_linecoding_s*. - * See include/nuttx/usb/cdc.h for structure definition. This IOCTL - * should be called to get the data associated with the - * CDCSER_EVENT_LINECODING event). - */ - - case CAIOC_GETLINECODING: - { - FAR struct cdc_linecoding_s *ptr = (FAR struct cdc_linecoding_s *)((uintptr_t)arg); - if (ptr) - { - memcpy(ptr, &priv->linecoding, sizeof(struct cdc_linecoding_s)); - } - else - { - ret = -EINVAL; - } - } - break; - - /* CAIOC_GETCTRLLINE - * Get control line status bits. Argument FAR int*. See - * include/nuttx/usb/cdc.h for bit definitions. This IOCTL should be - * called to get the data associated CDCSER_EVENT_CTRLLINE event. - */ - - case CAIOC_GETCTRLLINE: - { - FAR int *ptr = (FAR int *)((uintptr_t)arg); - if (ptr) - { - *ptr = priv->ctrlline; - } - else - { - ret = -EINVAL; - } - } - break; - - /* CAIOC_NOTIFY - * Send a serial state to the host via the Interrupt IN endpoint. - * Argument: int. This includes the current state of the carrier detect, - * DSR, break, and ring signal. See "Table 69: UART State Bitmap Values" - * and CDC_UART_definitions in include/nuttx/usb/cdc.h. - */ - - case CAIOC_NOTIFY: - { - /* Not yet implemented. I probably won't bother to implement until - * I comr up with a usage model that needs it. - * - * Here is what the needs to be done: - * - * 1. Format and send a request header with: - * - * bmRequestType: - * USB_REQ_DIR_IN|USB_REQ_TYPE_CLASS|USB_REQ_RECIPIENT_INTERFACE - * bRequest: ACM_SERIAL_STATE - * wValue: 0 - * wIndex: 0 - * wLength: Length of data - * - * 2. Followed by the notification data (in a separate packet) - */ - - ret = -ENOSYS; - } - break; - - default: - ret = -ENOTTY; - break; - } - - return ret; -} - -/**************************************************************************** - * Name: cdcuart_rxint - * - * Description: - * Called by the serial driver to enable or disable RX interrupts. We, of - * course, have no RX interrupts but must behave consistently. This method - * is called under the conditions: - * - * 1. With enable==true when the port is opened (just after cdcuart_setup - * and cdcuart_attach are called called) - * 2. With enable==false while transferring data from the RX buffer - * 2. With enable==true while waiting for more incoming data - * 3. With enable==false when the port is closed (just before cdcuart_detach - * and cdcuart_shutdown are called). - * - ****************************************************************************/ - -static void cdcuart_rxint(FAR struct uart_dev_s *dev, bool enable) -{ - FAR struct cdcser_dev_s *priv; - FAR uart_dev_t *serdev; - irqstate_t flags; - - usbtrace(CDCSER_CLASSAPI_RXINT, (uint16_t)enable); - - /* Sanity check */ - -#if CONFIG_DEBUG - if (!dev || !dev->priv) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_INVALIDARG), 0); - return; - } -#endif - - /* Extract reference to private data */ - - priv = (FAR struct cdcser_dev_s*)dev->priv; - serdev = &priv->serdev; - - /* We need exclusive access to the RX buffer and private structure - * in the following. - */ - - flags = irqsave(); - if (enable) - { - /* RX "interrupts" are enabled. Is this a transition from disabled - * to enabled state? - */ - - if (!priv->rxenabled) - { - /* Yes. During the time that RX interrupts are disabled, the - * the serial driver will be extracting data from the circular - * buffer and modifying recv.tail. During this time, we - * should avoid modifying recv.head; When interrupts are restored, - * we can update the head pointer for all of the data that we - * put into cicular buffer while "interrupts" were disabled. - */ - - if (priv->rxhead != serdev->recv.head) - { - serdev->recv.head = priv->rxhead; - - /* Yes... signal the availability of new data */ - - uart_datareceived(serdev); - } - - /* RX "interrupts are no longer disabled */ - - priv->rxenabled = true; - } - } - - /* RX "interrupts" are disabled. Is this a transition from enabled - * to disabled state? - */ - - else if (priv->rxenabled) - { - /* Yes. During the time that RX interrupts are disabled, the - * the serial driver will be extracting data from the circular - * buffer and modifying recv.tail. During this time, we - * should avoid modifying recv.head; When interrupts are disabled, - * we use a shadow index and continue adding data to the circular - * buffer. - */ - - priv->rxhead = serdev->recv.head; - priv->rxenabled = false; - } - irqrestore(flags); -} - -/**************************************************************************** - * Name: cdcuart_txint - * - * Description: - * Called by the serial driver to enable or disable TX interrupts. We, of - * course, have no TX interrupts but must behave consistently. Initially, - * TX interrupts are disabled. This method is called under the conditions: - * - * 1. With enable==false while transferring data into the TX buffer - * 2. With enable==true when data may be taken from the buffer. - * 3. With enable==false when the TX buffer is empty - * - ****************************************************************************/ - -static void cdcuart_txint(FAR struct uart_dev_s *dev, bool enable) -{ - FAR struct cdcser_dev_s *priv; - - usbtrace(CDCSER_CLASSAPI_TXINT, (uint16_t)enable); - - /* Sanity checks */ - -#if CONFIG_DEBUG - if (!dev || !dev->priv) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_INVALIDARG), 0); - return; - } -#endif - - /* Extract references to private data */ - - priv = (FAR struct cdcser_dev_s*)dev->priv; - - /* If the new state is enabled and if there is data in the XMIT buffer, - * send the next packet now. - */ - - uvdbg("enable=%d head=%d tail=%d\n", - enable, priv->serdev.xmit.head, priv->serdev.xmit.tail); - - if (enable && priv->serdev.xmit.head != priv->serdev.xmit.tail) - { - cdcser_sndpacket(priv); - } -} - -/**************************************************************************** - * Name: cdcuart_txempty - * - * Description: - * Return true when all data has been sent. This is called from the - * serial driver when the driver is closed. It will call this API - * periodically until it reports true. NOTE that the serial driver takes all - * responsibility for flushing TX data through the hardware so we can be - * a bit sloppy about that. - * - ****************************************************************************/ - -static bool cdcuart_txempty(FAR struct uart_dev_s *dev) -{ - FAR struct cdcser_dev_s *priv = (FAR struct cdcser_dev_s*)dev->priv; - - usbtrace(CDCSER_CLASSAPI_TXEMPTY, 0); - -#if CONFIG_DEBUG - if (!priv) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_INVALIDARG), 0); - return true; - } -#endif - - /* When all of the allocated write requests have been returned to the - * reqlist, then there is no longer any TX data in flight. - */ - - return priv->nwrq >= CONFIG_CDCSER_NWRREQS; -} - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -/**************************************************************************** - * Name: cdcser_initialize - * - * Description: - * Register USB serial port (and USB serial console if so configured). - * - * Input Parameter: - * Device minor number. E.g., minor 0 would correspond to /dev/ttyUSB0. - * - * Returned Value: - * Zero (OK) means that the driver was successfully registered. On any - * failure, a negated errno value is retured. - * - ****************************************************************************/ - -int cdcser_initialize(int minor) -{ - FAR struct cdcser_alloc_s *alloc; - FAR struct cdcser_dev_s *priv; - FAR struct cdcser_driver_s *drvr; - char devname[16]; - int ret; - - /* Allocate the structures needed */ - - alloc = (FAR struct cdcser_alloc_s*)kmalloc(sizeof(struct cdcser_alloc_s)); - if (!alloc) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_ALLOCDEVSTRUCT), 0); - return -ENOMEM; - } - - /* Convenience pointers into the allocated blob */ - - priv = &alloc->dev; - drvr = &alloc->drvr; - - /* Initialize the USB serial driver structure */ - - memset(priv, 0, sizeof(struct cdcser_dev_s)); - sq_init(&priv->reqlist); - - /* Fake line status */ - - priv->linecoding.baud[0] = (115200) & 0xff; /* Baud=115200 */ - priv->linecoding.baud[1] = (115200 >> 8) & 0xff; - priv->linecoding.baud[2] = (115200 >> 16) & 0xff; - priv->linecoding.baud[3] = (115200 >> 24) & 0xff; - priv->linecoding.stop = CDC_CHFMT_STOP1; /* One stop bit */ - priv->linecoding.parity = CDC_PARITY_NONE; /* No parity */ - priv->linecoding.nbits = 8; /* 8 data bits */ - - /* Initialize the serial driver sub-structure */ - - priv->serdev.recv.size = CONFIG_CDCSER_RXBUFSIZE; - priv->serdev.recv.buffer = priv->rxbuffer; - priv->serdev.xmit.size = CONFIG_CDCSER_TXBUFSIZE; - priv->serdev.xmit.buffer = priv->txbuffer; - priv->serdev.ops = &g_uartops; - priv->serdev.priv = priv; - - /* Initialize the USB class driver structure */ - -#ifdef CONFIG_USBDEV_DUALSPEED - drvr->drvr.speed = USB_SPEED_HIGH; -#else - drvr->drvr.speed = USB_SPEED_FULL; -#endif - drvr->drvr.ops = &g_driverops; - drvr->dev = priv; - - /* Register the USB serial class driver */ - - ret = usbdev_register(&drvr->drvr); - if (ret) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_DEVREGISTER), (uint16_t)-ret); - goto errout_with_alloc; - } - - /* Register the USB serial console */ - -#ifdef CONFIG_CDCSER_CONSOLE - g_usbserialport.isconsole = true; - ret = uart_register("/dev/console", &pri->serdev); - if (ret < 0) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_CONSOLEREGISTER), (uint16_t)-ret); - goto errout_with_class; - } -#endif - - /* Register the single port supported by this implementation */ - - sprintf(devname, "/dev/ttyUSB%d", minor); - ret = uart_register(devname, &priv->serdev); - if (ret) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_UARTREGISTER), (uint16_t)-ret); - goto errout_with_class; - } - return OK; - -errout_with_class: - usbdev_unregister(&drvr->drvr); -errout_with_alloc: - kfree(alloc); - return ret; -} diff --git a/nuttx/drivers/usbdev/cdc_serial.h b/nuttx/drivers/usbdev/cdc_serial.h deleted file mode 100644 index 818cd4935..000000000 --- a/nuttx/drivers/usbdev/cdc_serial.h +++ /dev/null @@ -1,196 +0,0 @@ -/**************************************************************************** - * drivers/usbdev/cdc_serial.h - * - * Copyright (C) 2011 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 - * 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. - * - ****************************************************************************/ - -#ifndef __DRIVERS_USBDEV_USBDEV_CDC_SERIAL_H -#define __DRIVERS_USBDEV_USBDEV_CDC_SERIAL_H 1 - -/**************************************************************************** - * Included Files - ****************************************************************************/ - -#include - -#include - -#include -#include - -/**************************************************************************** - * Definitions - ****************************************************************************/ -/* Descriptors **************************************************************/ -/* These settings are not modifiable via the NuttX configuration */ - -#define CDCSER_CONFIGIDNONE (0) /* Config ID means to return to address mode */ -#define CDCSER_INTERFACEID (0) -#define CDCSER_ALTINTERFACEID (0) - -/* Configuration descriptor values */ - -#define CDCSER_CONFIGID (1) /* The only supported configuration ID */ - -/* Endpoint configuration */ - -#define CDCSER_EPINTIN_ADDR (USB_DIR_IN|CONFIG_CDCSER_EPINTIN) -#define CDCSER_EPINTIN_ATTR (USB_EP_ATTR_XFER_INT) - -#define CDCSER_EPOUTBULK_ADDR (CONFIG_CDCSER_EPBULKOUT) -#define CDCSER_EPOUTBULK_ATTR (USB_EP_ATTR_XFER_BULK) - -#define CDCSER_EPINBULK_ADDR (USB_DIR_IN|CONFIG_CDCSER_EPBULKIN) -#define CDCSER_EPINBULK_ATTR (USB_EP_ATTR_XFER_BULK) - -/* Buffer big enough for any of our descriptors (the config descriptor is the - * biggest). - */ - -#define CDCSER_MXDESCLEN (64) - -/* Misc Macros **************************************************************/ -/* min/max macros */ - -#ifndef min -# define min(a,b) ((a)<(b)?(a):(b)) -#endif - -#ifndef max -# define max(a,b) ((a)>(b)?(a):(b)) -#endif - -/* Trace values *************************************************************/ - -#define CDCSER_CLASSAPI_SETUP TRACE_EVENT(TRACE_CLASSAPI_ID, USBSER_TRACECLASSAPI_SETUP) -#define CDCSER_CLASSAPI_SHUTDOWN TRACE_EVENT(TRACE_CLASSAPI_ID, USBSER_TRACECLASSAPI_SHUTDOWN) -#define CDCSER_CLASSAPI_ATTACH TRACE_EVENT(TRACE_CLASSAPI_ID, USBSER_TRACECLASSAPI_ATTACH) -#define CDCSER_CLASSAPI_DETACH TRACE_EVENT(TRACE_CLASSAPI_ID, USBSER_TRACECLASSAPI_DETACH) -#define CDCSER_CLASSAPI_IOCTL TRACE_EVENT(TRACE_CLASSAPI_ID, USBSER_TRACECLASSAPI_IOCTL) -#define CDCSER_CLASSAPI_RECEIVE TRACE_EVENT(TRACE_CLASSAPI_ID, USBSER_TRACECLASSAPI_RECEIVE) -#define CDCSER_CLASSAPI_RXINT TRACE_EVENT(TRACE_CLASSAPI_ID, USBSER_TRACECLASSAPI_RXINT) -#define CDCSER_CLASSAPI_RXAVAILABLE TRACE_EVENT(TRACE_CLASSAPI_ID, USBSER_TRACECLASSAPI_RXAVAILABLE) -#define CDCSER_CLASSAPI_SEND TRACE_EVENT(TRACE_CLASSAPI_ID, USBSER_TRACECLASSAPI_SEND) -#define CDCSER_CLASSAPI_TXINT TRACE_EVENT(TRACE_CLASSAPI_ID, USBSER_TRACECLASSAPI_TXINT) -#define CDCSER_CLASSAPI_TXREADY TRACE_EVENT(TRACE_CLASSAPI_ID, USBSER_TRACECLASSAPI_TXREADY) -#define CDCSER_CLASSAPI_TXEMPTY TRACE_EVENT(TRACE_CLASSAPI_ID, USBSER_TRACECLASSAPI_TXEMPTY) - -/**************************************************************************** - * Public Types - ****************************************************************************/ - -enum cdcser_epdesc_e -{ - CDCSER_EPINTIN = 0, /* Interrupt IN endpoint descriptor */ - CDCSER_EPBULKOUT, /* Bulk OUT endpoint descriptor */ - CDCSER_EPBULKIN /* Bulk IN endpoint descriptor */ -}; - -/**************************************************************************** - * Public Data - ****************************************************************************/ - -/**************************************************************************** - * Public Function Prototypes - ****************************************************************************/ - -/**************************************************************************** - * Name: cdcser_mkstrdesc - * - * Description: - * Construct a string descriptor - * - ****************************************************************************/ - -int cdcser_mkstrdesc(uint8_t id, struct usb_strdesc_s *strdesc); - -/**************************************************************************** - * Name: cdcser_getepdesc - * - * Description: - * Return a pointer to the raw device descriptor - * - ****************************************************************************/ - -FAR const struct usb_devdesc_s *cdcser_getdevdesc(void); - -/**************************************************************************** - * Name: cdcser_getepdesc - * - * Description: - * Return a pointer to the raw endpoint descriptor (used for configuring - * endpoints) - * - ****************************************************************************/ - -FAR const struct usb_epdesc_s *cdcser_getepdesc(enum cdcser_epdesc_e epid); - -/**************************************************************************** - * Name: cdcser_mkepdesc - * - * Description: - * Construct the endpoint descriptor using the correct max packet size. - * - ****************************************************************************/ - -#ifdef CONFIG_USBDEV_DUALSPEED -void cdcser_mkepdesc(enum cdcser_epdesc_e epid, - uint16_t mxpacket, FAR struct usb_epdesc_s *outdesc); -#endif - -/**************************************************************************** - * Name: cdcser_mkcfgdesc - * - * Description: - * Construct the configuration descriptor - * - ****************************************************************************/ - -#ifdef CONFIG_USBDEV_DUALSPEED -int16_t cdcser_mkcfgdesc(FAR uint8_t *buf, uint8_t speed, uint8_t type); -#else -int16_t cdcser_mkcfgdesc(FAR uint8_t *buf); -#endif - -/**************************************************************************** - * Name: cdcser_getqualdesc - * - * Description: - * Return a pointer to the raw qual descriptor - * - ****************************************************************************/ - -#ifdef CONFIG_USBDEV_DUALSPEED -FAR const struct usb_qualdesc_s *cdcser_getqualdesc(void); -#endif - -#endif /* __DRIVERS_USBDEV_USBDEV_CDC_SERIAL_H */ diff --git a/nuttx/drivers/usbdev/cdcacm.c b/nuttx/drivers/usbdev/cdcacm.c new file mode 100644 index 000000000..9c6c36625 --- /dev/null +++ b/nuttx/drivers/usbdev/cdcacm.c @@ -0,0 +1,2003 @@ +/**************************************************************************** + * drivers/usbdev/cdcacm.c + * + * Copyright (C) 2011-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 + * 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 + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "cdcacm.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/* Container to support a list of requests */ + +struct cdcser_req_s +{ + FAR struct cdcser_req_s *flink; /* Implements a singly linked list */ + FAR struct usbdev_req_s *req; /* The contained request */ +}; + +/* This structure describes the internal state of the driver */ + +struct cdcser_dev_s +{ + FAR struct uart_dev_s serdev; /* Serial device structure */ + FAR struct usbdev_s *usbdev; /* usbdev driver pointer */ + + uint8_t config; /* Configuration number */ + uint8_t nwrq; /* Number of queue write requests (in reqlist)*/ + uint8_t nrdq; /* Number of queue read requests (in epbulkout) */ + bool rxenabled; /* true: UART RX "interrupts" enabled */ + int16_t rxhead; /* Working head; used when rx int disabled */ + + uint8_t ctrlline; /* Buffered control line state */ + struct cdc_linecoding_s linecoding; /* Buffered line status */ + cdcser_callback_t callback; /* Serial event callback function */ + + FAR struct usbdev_ep_s *epintin; /* Interrupt IN endpoint structure */ + FAR struct usbdev_ep_s *epbulkin; /* Bulk IN endpoint structure */ + FAR struct usbdev_ep_s *epbulkout; /* Bulk OUT endpoint structure */ + FAR struct usbdev_req_s *ctrlreq; /* Control request */ + struct sq_queue_s reqlist; /* List of write request containers */ + + /* Pre-allocated write request containers. The write requests will + * be linked in a free list (reqlist), and used to send requests to + * EPBULKIN; Read requests will be queued in the EBULKOUT. + */ + + struct cdcser_req_s wrreqs[CONFIG_CDCSER_NWRREQS]; + struct cdcser_req_s rdreqs[CONFIG_CDCSER_NWRREQS]; + + /* Serial I/O buffers */ + + char rxbuffer[CONFIG_CDCSER_RXBUFSIZE]; + char txbuffer[CONFIG_CDCSER_TXBUFSIZE]; +}; + +/* The internal version of the class driver */ + +struct cdcser_driver_s +{ + struct usbdevclass_driver_s drvr; + FAR struct cdcser_dev_s *dev; +}; + +/* This is what is allocated */ + +struct cdcser_alloc_s +{ + struct cdcser_dev_s dev; + struct cdcser_driver_s drvr; +}; + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/* Transfer helpers *********************************************************/ + +static uint16_t cdcser_fillrequest(FAR struct cdcser_dev_s *priv, + uint8_t *reqbuf, uint16_t reqlen); +static int cdcser_sndpacket(FAR struct cdcser_dev_s *priv); +static inline int cdcser_recvpacket(FAR struct cdcser_dev_s *priv, + uint8_t *reqbuf, uint16_t reqlen); + +/* Request helpers *********************************************************/ + +static struct usbdev_req_s *cdcser_allocreq(FAR struct usbdev_ep_s *ep, + uint16_t len); +static void cdcser_freereq(FAR struct usbdev_ep_s *ep, + FAR struct usbdev_req_s *req); + +/* Configuration ***********************************************************/ + +static void cdcser_resetconfig(FAR struct cdcser_dev_s *priv); +#ifdef CONFIG_USBDEV_DUALSPEED +static int cdcser_epconfigure(FAR struct usbdev_ep_s *ep, + enum cdcser_epdesc_e epid, uint16_t mxpacket, bool last); +#endif +#ifndef CONFIG_CDCSER_COMPOSITE +static int cdcser_setconfig(FAR struct cdcser_dev_s *priv, + uint8_t config); +#endif + +/* Completion event handlers ***********************************************/ + +static void cdcser_ep0incomplete(FAR struct usbdev_ep_s *ep, + FAR struct usbdev_req_s *req); +static void cdcser_rdcomplete(FAR struct usbdev_ep_s *ep, + FAR struct usbdev_req_s *req); +static void cdcser_wrcomplete(FAR struct usbdev_ep_s *ep, + FAR struct usbdev_req_s *req); + +/* USB class device ********************************************************/ + +static int cdcser_bind(FAR struct usbdev_s *dev, + FAR struct usbdevclass_driver_s *driver); +static void cdcser_unbind(FAR struct usbdev_s *dev); +static int cdcser_setup(FAR struct usbdev_s *dev, + const struct usb_ctrlreq_s *ctrl); +static void cdcser_disconnect(FAR struct usbdev_s *dev); + +/* UART Operationt **********************************************************/ + +static int cdcuart_setup(FAR struct uart_dev_s *dev); +static void cdcuart_shutdown(FAR struct uart_dev_s *dev); +static int cdcuart_attach(FAR struct uart_dev_s *dev); +static void cdcuart_detach(FAR struct uart_dev_s *dev); +static int cdcuart_ioctl(FAR struct file *filep,int cmd,unsigned long arg); +static void cdcuart_rxint(FAR struct uart_dev_s *dev, bool enable); +static void cdcuart_txint(FAR struct uart_dev_s *dev, bool enable); +static bool cdcuart_txempty(FAR struct uart_dev_s *dev); + +/**************************************************************************** + * Private Variables + ****************************************************************************/ +/* USB class device *********************************************************/ + +static const struct usbdevclass_driverops_s g_driverops = +{ + cdcser_bind, /* bind */ + cdcser_unbind, /* unbind */ + cdcser_setup, /* setup */ + cdcser_disconnect, /* disconnect */ + NULL, /* suspend */ + NULL, /* resume */ +}; + +/* Serial port **************************************************************/ + +static const struct uart_ops_s g_uartops = +{ + cdcuart_setup, /* setup */ + cdcuart_shutdown, /* shutdown */ + cdcuart_attach, /* attach */ + cdcuart_detach, /* detach */ + cdcuart_ioctl, /* ioctl */ + NULL, /* receive */ + cdcuart_rxint, /* rxinit */ + NULL, /* rxavailable */ + NULL, /* send */ + cdcuart_txint, /* txinit */ + NULL, /* txready */ + cdcuart_txempty /* txempty */ +}; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: cdcser_fillrequest + * + * Description: + * If there is data to send it is copied to the given buffer. Called + * either to initiate the first write operation, or from the completion + * interrupt handler service consecutive write operations. + * + * NOTE: The USB serial driver does not use the serial drivers + * uart_xmitchars() API. That logic is essentially duplicated here because + * unlike UART hardware, we need to be able to handle writes not byte-by-byte, + * but packet-by-packet. Unfortunately, that decision also exposes some + * internals of the serial driver in the following. + * + ****************************************************************************/ + +static uint16_t cdcser_fillrequest(FAR struct cdcser_dev_s *priv, uint8_t *reqbuf, + uint16_t reqlen) +{ + FAR uart_dev_t *serdev = &priv->serdev; + FAR struct uart_buffer_s *xmit = &serdev->xmit; + irqstate_t flags; + uint16_t nbytes = 0; + + /* Disable interrupts */ + + flags = irqsave(); + + /* Transfer bytes while we have bytes available and there is room in the request */ + + while (xmit->head != xmit->tail && nbytes < reqlen) + { + *reqbuf++ = xmit->buffer[xmit->tail]; + nbytes++; + + /* Increment the tail pointer */ + + if (++(xmit->tail) >= xmit->size) + { + xmit->tail = 0; + } + } + + /* When all of the characters have been sent from the buffer + * disable the "TX interrupt". + */ + + if (xmit->head == xmit->tail) + { + uart_disabletxint(serdev); + } + + /* If any bytes were removed from the buffer, inform any waiters + * there there is space available. + */ + + if (nbytes) + { + uart_datasent(serdev); + } + + irqrestore(flags); + return nbytes; +} + +/**************************************************************************** + * Name: cdcser_sndpacket + * + * Description: + * This function obtains write requests, transfers the TX data into the + * request, and submits the requests to the USB controller. This continues + * untils either (1) there are no further packets available, or (2) thre is + * no further data to send. + * + ****************************************************************************/ + +static int cdcser_sndpacket(FAR struct cdcser_dev_s *priv) +{ + FAR struct usbdev_ep_s *ep; + FAR struct usbdev_req_s *req; + FAR struct cdcser_req_s *reqcontainer; + irqstate_t flags; + int len; + int ret = OK; + +#ifdef CONFIG_DEBUG + if (priv == NULL) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_INVALIDARG), 0); + return -ENODEV; + } +#endif + + flags = irqsave(); + + /* Use our IN endpoint for the transfer */ + + ep = priv->epbulkin; + + /* Loop until either (1) we run out or write requests, or (2) cdcser_fillrequest() + * is unable to fill the request with data (i.e., untilthere is no more data + * to be sent). + */ + + uvdbg("head=%d tail=%d nwrq=%d empty=%d\n", + priv->serdev.xmit.head, priv->serdev.xmit.tail, + priv->nwrq, sq_empty(&priv->reqlist)); + + while (!sq_empty(&priv->reqlist)) + { + /* Peek at the request in the container at the head of the list */ + + reqcontainer = (struct cdcser_req_s *)sq_peek(&priv->reqlist); + req = reqcontainer->req; + + /* Fill the request with serial TX data */ + + len = cdcser_fillrequest(priv, req->buf, req->len); + if (len > 0) + { + /* Remove the empty container from the request list */ + + (void)sq_remfirst(&priv->reqlist); + priv->nwrq--; + + /* Then submit the request to the endpoint */ + + req->len = len; + req->priv = reqcontainer; + req->flags = USBDEV_REQFLAGS_NULLPKT; + ret = EP_SUBMIT(ep, req); + if (ret != OK) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_SUBMITFAIL), (uint16_t)-ret); + break; + } + } + else + { + break; + } + } + + irqrestore(flags); + return ret; +} + +/**************************************************************************** + * Name: cdcser_recvpacket + * + * Description: + * A normal completion event was received by the read completion handler + * at the interrupt level (with interrupts disabled). This function handles + * the USB packet and provides the received data to the uart RX buffer. + * + * Assumptions: + * Called from the USB interrupt handler with interrupts disabled. + * + ****************************************************************************/ + +static inline int cdcser_recvpacket(FAR struct cdcser_dev_s *priv, + uint8_t *reqbuf, uint16_t reqlen) +{ + FAR uart_dev_t *serdev = &priv->serdev; + FAR struct uart_buffer_s *recv = &serdev->recv; + uint16_t currhead; + uint16_t nexthead; + uint16_t nbytes = 0; + + uvdbg("head=%d tail=%d nrdq=%d reqlen=%d\n", + priv->serdev.recv.head, priv->serdev.recv.tail, priv->nrdq, reqlen); + + /* Get the next head index. During the time that RX interrupts are disabled, the + * the serial driver will be extracting data from the circular buffer and modifying + * recv.tail. During this time, we should avoid modifying recv.head; Instead we will + * use a shadow copy of the index. When interrupts are restored, the real recv.head + * will be updated with this indes. + */ + + if (priv->rxenabled) + { + currhead = recv->head; + } + else + { + currhead = priv->rxhead; + } + + /* Pre-calculate the head index and check for wrap around. We need to do this + * so that we can determine if the circular buffer will overrun BEFORE we + * overrun the buffer! + */ + + nexthead = currhead + 1; + if (nexthead >= recv->size) + { + nexthead = 0; + } + + /* Then copy data into the RX buffer until either: (1) all of the data has been + * copied, or (2) the RX buffer is full. NOTE: If the RX buffer becomes full, + * then we have overrun the serial driver and data will be lost. + */ + + while (nexthead != recv->tail && nbytes < reqlen) + { + /* Copy one byte to the head of the circular RX buffer */ + + recv->buffer[currhead] = *reqbuf++; + + /* Update counts and indices */ + + currhead = nexthead; + nbytes++; + + /* Increment the head index and check for wrap around */ + + nexthead = currhead + 1; + if (nexthead >= recv->size) + { + nexthead = 0; + } + } + + /* Write back the head pointer using the shadow index if RX "interrupts" + * are disabled. + */ + + if (priv->rxenabled) + { + recv->head = currhead; + } + else + { + priv->rxhead = currhead; + } + + /* If data was added to the incoming serial buffer, then wake up any + * threads is waiting for incoming data. If we are running in an interrupt + * handler, then the serial driver will not run until the interrupt handler + * returns. + */ + + if (priv->rxenabled && nbytes > 0) + { + uart_datareceived(serdev); + } + + /* Return an error if the entire packet could not be transferred */ + + if (nbytes < reqlen) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_RXOVERRUN), 0); + return -ENOSPC; + } + return OK; +} + +/**************************************************************************** + * Name: cdcser_allocreq + * + * Description: + * Allocate a request instance along with its buffer + * + ****************************************************************************/ + +static struct usbdev_req_s *cdcser_allocreq(FAR struct usbdev_ep_s *ep, + uint16_t len) +{ + FAR struct usbdev_req_s *req; + + req = EP_ALLOCREQ(ep); + if (req != NULL) + { + req->len = len; + req->buf = EP_ALLOCBUFFER(ep, len); + if (!req->buf) + { + EP_FREEREQ(ep, req); + req = NULL; + } + } + return req; +} + +/**************************************************************************** + * Name: cdcser_freereq + * + * Description: + * Free a request instance along with its buffer + * + ****************************************************************************/ + +static void cdcser_freereq(FAR struct usbdev_ep_s *ep, + FAR struct usbdev_req_s *req) +{ + if (ep != NULL && req != NULL) + { + if (req->buf != NULL) + { + EP_FREEBUFFER(ep, req->buf); + } + EP_FREEREQ(ep, req); + } +} + +/**************************************************************************** + * Name: cdcser_resetconfig + * + * Description: + * Mark the device as not configured and disable all endpoints. + * + ****************************************************************************/ + +static void cdcser_resetconfig(FAR struct cdcser_dev_s *priv) +{ + /* Are we configured? */ + + if (priv->config != CDCSER_CONFIGIDNONE) + { + /* Yes.. but not anymore */ + + priv->config = CDCSER_CONFIGIDNONE; + + /* Disable endpoints. This should force completion of all pending + * transfers. + */ + + EP_DISABLE(priv->epintin); + EP_DISABLE(priv->epbulkin); + EP_DISABLE(priv->epbulkout); + } +} + +/**************************************************************************** + * Name: cdcser_epconfigure + * + * Description: + * Configure one endpoint. + * + ****************************************************************************/ + +#ifdef CONFIG_USBDEV_DUALSPEED +static int cdcser_epconfigure(FAR struct usbdev_ep_s *ep, + enum cdcser_epdesc_e epid, uint16_t mxpacket, + bool last) +{ + struct usb_epdesc_s epdesc; + cdcser_mkepdesc(epid, mxpacket, &epdesc); + return EP_CONFIGURE(ep, &epdesc, last); +} +#endif + +/**************************************************************************** + * Name: cdcser_setconfig + * + * Description: + * Set the device configuration by allocating and configuring endpoints and + * by allocating and queue read and write requests. + * + ****************************************************************************/ + +#ifndef CONFIG_CDCSER_COMPOSITE +static int cdcser_setconfig(FAR struct cdcser_dev_s *priv, uint8_t config) +{ + FAR struct usbdev_req_s *req; + int i; + int ret = 0; + +#if CONFIG_DEBUG + if (priv == NULL) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_INVALIDARG), 0); + return -EIO; + } +#endif + + if (config == priv->config) + { + /* Already configured -- Do nothing */ + + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_ALREADYCONFIGURED), 0); + return 0; + } + + /* Discard the previous configuration data */ + + cdcser_resetconfig(priv); + + /* Was this a request to simply discard the current configuration? */ + + if (config == CDCSER_CONFIGIDNONE) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_CONFIGNONE), 0); + return 0; + } + + /* We only accept one configuration */ + + if (config != CDCSER_CONFIGID) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_CONFIGIDBAD), 0); + return -EINVAL; + } + + /* Configure the IN interrupt endpoint */ + +#ifdef CONFIG_USBDEV_DUALSPEED + if (priv->usbdev->speed == USB_SPEED_HIGH) + { + ret = cdcser_epconfigure(priv->epintin, CDCSER_EPINTIN, + CONFIG_CDCSER_EPINTIN_HSSIZE, false); + } + else +#endif + { + ret = EP_CONFIGURE(priv->epintin, + cdcser_getepdesc(CDCSER_EPINTIN), false); + } + + if (ret < 0) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_EPINTINCONFIGFAIL), 0); + goto errout; + } + priv->epintin->priv = priv; + + /* Configure the IN bulk endpoint */ + +#ifdef CONFIG_USBDEV_DUALSPEED + if (priv->usbdev->speed == USB_SPEED_HIGH) + { + ret = cdcser_epconfigure(priv->epbulkin, CDCSER_EPBULKIN, + CONFIG_CDCSER_EPBULKIN_HSSIZE, false); + } + else +#endif + { + ret = EP_CONFIGURE(priv->epbulkin, + cdcser_getepdesc(CDCSER_EPBULKIN), false); + } + + if (ret < 0) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_EPBULKINCONFIGFAIL), 0); + goto errout; + } + + priv->epbulkin->priv = priv; + + /* Configure the OUT bulk endpoint */ + +#ifdef CONFIG_USBDEV_DUALSPEED + if (priv->usbdev->speed == USB_SPEED_HIGH) + { + ret = cdcser_epconfigure(priv->epbulkout, CDCSER_EPBULKOUT, + CONFIG_CDCSER_EPBULKOUT_HSSIZE, true); + } + else +#endif + { + ret = EP_CONFIGURE(priv->epbulkout, + cdcser_getepdesc(CDCSER_EPBULKOUT), true); + } + + if (ret < 0) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_EPBULKOUTCONFIGFAIL), 0); + goto errout; + } + + priv->epbulkout->priv = priv; + + /* Queue read requests in the bulk OUT endpoint */ + + DEBUGASSERT(priv->nrdq == 0); + for (i = 0; i < CONFIG_CDCSER_NRDREQS; i++) + { + req = priv->rdreqs[i].req; + req->callback = cdcser_rdcomplete; + ret = EP_SUBMIT(priv->epbulkout, req); + if (ret != OK) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_RDSUBMIT), (uint16_t)-ret); + goto errout; + } + priv->nrdq++; + } + + priv->config = config; + return OK; + +errout: + cdcser_resetconfig(priv); + return ret; +} +#endif + +/**************************************************************************** + * Name: cdcser_ep0incomplete + * + * Description: + * Handle completion of EP0 control operations + * + ****************************************************************************/ + +static void cdcser_ep0incomplete(FAR struct usbdev_ep_s *ep, + FAR struct usbdev_req_s *req) +{ + if (req->result || req->xfrd != req->len) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_REQRESULT), (uint16_t)-req->result); + } +} + +/**************************************************************************** + * Name: cdcser_rdcomplete + * + * Description: + * Handle completion of read request on the bulk OUT endpoint. This + * is handled like the receipt of serial data on the "UART" + * + ****************************************************************************/ + +static void cdcser_rdcomplete(FAR struct usbdev_ep_s *ep, + FAR struct usbdev_req_s *req) +{ + FAR struct cdcser_dev_s *priv; + irqstate_t flags; + int ret; + + /* Sanity check */ + +#ifdef CONFIG_DEBUG + if (!ep || !ep->priv || !req) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_INVALIDARG), 0); + return; + } +#endif + + /* Extract references to private data */ + + priv = (FAR struct cdcser_dev_s*)ep->priv; + + /* Process the received data unless this is some unusual condition */ + + flags = irqsave(); + switch (req->result) + { + case 0: /* Normal completion */ + usbtrace(TRACE_CLASSRDCOMPLETE, priv->nrdq); + cdcser_recvpacket(priv, req->buf, req->xfrd); + break; + + case -ESHUTDOWN: /* Disconnection */ + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_RDSHUTDOWN), 0); + priv->nrdq--; + irqrestore(flags); + return; + + default: /* Some other error occurred */ + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_RDUNEXPECTED), (uint16_t)-req->result); + break; + }; + + /* Requeue the read request */ + +#ifdef CONFIG_CDCSER_BULKREQLEN + req->len = max(CONFIG_CDCSER_BULKREQLEN, ep->maxpacket); +#else + req->len = ep->maxpacket; +#endif + + ret = EP_SUBMIT(ep, req); + if (ret != OK) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_RDSUBMIT), (uint16_t)-req->result); + } + irqrestore(flags); +} + +/**************************************************************************** + * Name: cdcser_wrcomplete + * + * Description: + * Handle completion of write request. This function probably executes + * in the context of an interrupt handler. + * + ****************************************************************************/ + +static void cdcser_wrcomplete(FAR struct usbdev_ep_s *ep, + FAR struct usbdev_req_s *req) +{ + FAR struct cdcser_dev_s *priv; + FAR struct cdcser_req_s *reqcontainer; + irqstate_t flags; + + /* Sanity check */ + +#ifdef CONFIG_DEBUG + if (!ep || !ep->priv || !req || !req->priv) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_INVALIDARG), 0); + return; + } +#endif + + /* Extract references to our private data */ + + priv = (FAR struct cdcser_dev_s *)ep->priv; + reqcontainer = (FAR struct cdcser_req_s *)req->priv; + + /* Return the write request to the free list */ + + flags = irqsave(); + sq_addlast((sq_entry_t*)reqcontainer, &priv->reqlist); + priv->nwrq++; + irqrestore(flags); + + /* Send the next packet unless this was some unusual termination + * condition + */ + + switch (req->result) + { + case OK: /* Normal completion */ + usbtrace(TRACE_CLASSWRCOMPLETE, priv->nwrq); + cdcser_sndpacket(priv); + break; + + case -ESHUTDOWN: /* Disconnection */ + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_WRSHUTDOWN), priv->nwrq); + break; + + default: /* Some other error occurred */ + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_WRUNEXPECTED), (uint16_t)-req->result); + break; + } +} + +/**************************************************************************** + * USB Class Driver Methods + ****************************************************************************/ + +/**************************************************************************** + * Name: cdcser_bind + * + * Description: + * Invoked when the driver is bound to a USB device driver + * + ****************************************************************************/ + +static int cdcser_bind(FAR struct usbdev_s *dev, FAR struct usbdevclass_driver_s *driver) +{ + FAR struct cdcser_dev_s *priv = ((struct cdcser_driver_s*)driver)->dev; + FAR struct cdcser_req_s *reqcontainer; + irqstate_t flags; + uint16_t reqlen; + int ret; + int i; + + usbtrace(TRACE_CLASSBIND, 0); + + /* Bind the structures */ + + priv->usbdev = dev; + dev->ep0->priv = priv; + + /* Preallocate control request */ + + priv->ctrlreq = cdcser_allocreq(dev->ep0, CDCSER_MXDESCLEN); + if (priv->ctrlreq == NULL) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_ALLOCCTRLREQ), 0); + ret = -ENOMEM; + goto errout; + } + priv->ctrlreq->callback = cdcser_ep0incomplete; + + /* Pre-allocate all endpoints... the endpoints will not be functional + * until the SET CONFIGURATION request is processed in cdcser_setconfig. + * This is done here because there may be calls to kmalloc and the SET + * CONFIGURATION processing probably occurrs within interrupt handling + * logic where kmalloc calls will fail. + */ + + /* Pre-allocate the IN interrupt endpoint */ + + priv->epintin = DEV_ALLOCEP(dev, CDCSER_EPINTIN_ADDR, true, USB_EP_ATTR_XFER_INT); + if (!priv->epintin) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_EPINTINALLOCFAIL), 0); + ret = -ENODEV; + goto errout; + } + priv->epintin->priv = priv; + + /* Pre-allocate the IN bulk endpoint */ + + priv->epbulkin = DEV_ALLOCEP(dev, CDCSER_EPINBULK_ADDR, true, USB_EP_ATTR_XFER_BULK); + if (!priv->epbulkin) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_EPBULKINALLOCFAIL), 0); + ret = -ENODEV; + goto errout; + } + priv->epbulkin->priv = priv; + + /* Pre-allocate the OUT bulk endpoint */ + + priv->epbulkout = DEV_ALLOCEP(dev, CDCSER_EPOUTBULK_ADDR, false, USB_EP_ATTR_XFER_BULK); + if (!priv->epbulkout) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_EPBULKOUTALLOCFAIL), 0); + ret = -ENODEV; + goto errout; + } + priv->epbulkout->priv = priv; + + /* Pre-allocate read requests */ + +#ifdef CONFIG_CDCSER_BULKREQLEN + reqlen = max(CONFIG_CDCSER_BULKREQLEN, priv->epbulkout->maxpacket); +#else + reqlen = priv->epbulkout->maxpacket; +#endif + + for (i = 0; i < CONFIG_CDCSER_NRDREQS; i++) + { + reqcontainer = &priv->rdreqs[i]; + reqcontainer->req = cdcser_allocreq(priv->epbulkout, reqlen); + if (reqcontainer->req == NULL) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_RDALLOCREQ), -ENOMEM); + ret = -ENOMEM; + goto errout; + } + reqcontainer->req->priv = reqcontainer; + reqcontainer->req->callback = cdcser_rdcomplete; + } + + /* Pre-allocate write request containers and put in a free list */ + +#ifdef CONFIG_CDCSER_BULKREQLEN + reqlen = max(CONFIG_CDCSER_BULKREQLEN, priv->epbulkin->maxpacket); +#else + reqlen = priv->epbulkin->maxpacket; +#endif + + for (i = 0; i < CONFIG_CDCSER_NWRREQS; i++) + { + reqcontainer = &priv->wrreqs[i]; + reqcontainer->req = cdcser_allocreq(priv->epbulkin, reqlen); + if (reqcontainer->req == NULL) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_WRALLOCREQ), -ENOMEM); + ret = -ENOMEM; + goto errout; + } + reqcontainer->req->priv = reqcontainer; + reqcontainer->req->callback = cdcser_wrcomplete; + + flags = irqsave(); + sq_addlast((sq_entry_t*)reqcontainer, &priv->reqlist); + priv->nwrq++; /* Count of write requests available */ + irqrestore(flags); + } + + /* Report if we are selfpowered */ + +#ifdef CONFIG_USBDEV_SELFPOWERED + DEV_SETSELFPOWERED(dev); +#endif + + /* And pull-up the data line for the soft connect function */ + + DEV_CONNECT(dev); + return OK; + +errout: + cdcser_unbind(dev); + return ret; +} + +/**************************************************************************** + * Name: cdcser_unbind + * + * Description: + * Invoked when the driver is unbound from a USB device driver + * + ****************************************************************************/ + +static void cdcser_unbind(FAR struct usbdev_s *dev) +{ + FAR struct cdcser_dev_s *priv; + FAR struct cdcser_req_s *reqcontainer; + irqstate_t flags; + int i; + + usbtrace(TRACE_CLASSUNBIND, 0); + +#ifdef CONFIG_DEBUG + if (!dev || !dev->ep0) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_INVALIDARG), 0); + return; + } +#endif + + /* Extract reference to private data */ + + priv = (FAR struct cdcser_dev_s *)dev->ep0->priv; + +#ifdef CONFIG_DEBUG + if (!priv) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_EP0NOTBOUND), 0); + return; + } +#endif + + /* Make sure that we are not already unbound */ + + if (priv != NULL) + { + /* Make sure that the endpoints have been unconfigured. If + * we were terminated gracefully, then the configuration should + * already have been reset. If not, then calling cdcser_resetconfig + * should cause the endpoints to immediately terminate all + * transfers and return the requests to us (with result == -ESHUTDOWN) + */ + + cdcser_resetconfig(priv); + up_mdelay(50); + + /* Free the interrupt IN endpoint */ + + if (priv->epintin) + { + DEV_FREEEP(dev, priv->epintin); + priv->epintin = NULL; + } + + /* Free the bulk IN endpoint */ + + if (priv->epbulkin) + { + DEV_FREEEP(dev, priv->epbulkin); + priv->epbulkin = NULL; + } + + /* Free the pre-allocated control request */ + + if (priv->ctrlreq != NULL) + { + cdcser_freereq(dev->ep0, priv->ctrlreq); + priv->ctrlreq = NULL; + } + + /* Free pre-allocated read requests (which should all have + * been returned to the free list at this time -- we don't check) + */ + + DEBUGASSERT(priv->nrdq == 0); + for (i = 0; i < CONFIG_CDCSER_NRDREQS; i++) + { + reqcontainer = &priv->rdreqs[i]; + if (reqcontainer->req) + { + cdcser_freereq(priv->epbulkout, reqcontainer->req); + reqcontainer->req = NULL; + } + } + + /* Free the bulk OUT endpoint */ + + if (priv->epbulkout) + { + DEV_FREEEP(dev, priv->epbulkout); + priv->epbulkout = NULL; + } + + /* Free write requests that are not in use (which should be all + * of them + */ + + flags = irqsave(); + DEBUGASSERT(priv->nwrq == CONFIG_CDCSER_NWRREQS); + while (!sq_empty(&priv->reqlist)) + { + reqcontainer = (struct cdcser_req_s *)sq_remfirst(&priv->reqlist); + if (reqcontainer->req != NULL) + { + cdcser_freereq(priv->epbulkin, reqcontainer->req); + priv->nwrq--; /* Number of write requests queued */ + } + } + DEBUGASSERT(priv->nwrq == 0); + irqrestore(flags); + } + + /* Clear out all data in the circular buffer */ + + priv->serdev.xmit.head = 0; + priv->serdev.xmit.tail = 0; +} + +/**************************************************************************** + * Name: cdcser_setup + * + * Description: + * Invoked for ep0 control requests. This function probably executes + * in the context of an interrupt handler. + * + ****************************************************************************/ + +static int cdcser_setup(FAR struct usbdev_s *dev, const struct usb_ctrlreq_s *ctrl) +{ + FAR struct cdcser_dev_s *priv; + FAR struct usbdev_req_s *ctrlreq; + uint16_t value; + uint16_t index; + uint16_t len; + int ret = -EOPNOTSUPP; + +#ifdef CONFIG_DEBUG + if (!dev || !dev->ep0 || !ctrl) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_INVALIDARG), 0); + return -EIO; + } +#endif + + /* Extract reference to private data */ + + usbtrace(TRACE_CLASSSETUP, ctrl->req); + priv = (FAR struct cdcser_dev_s *)dev->ep0->priv; + +#ifdef CONFIG_DEBUG + if (!priv || !priv->ctrlreq) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_EP0NOTBOUND), 0); + return -ENODEV; + } +#endif + ctrlreq = priv->ctrlreq; + + /* Extract the little-endian 16-bit values to host order */ + + value = GETUINT16(ctrl->value); + index = GETUINT16(ctrl->index); + len = GETUINT16(ctrl->len); + + uvdbg("type=%02x req=%02x value=%04x index=%04x len=%04x\n", + ctrl->type, ctrl->req, value, index, len); + + switch (ctrl->type & USB_REQ_TYPE_MASK) + { + /*********************************************************************** + * Standard Requests + ***********************************************************************/ + + case USB_REQ_TYPE_STANDARD: + { + /* If the serial device is used in as part of a composite device, + * then standard descriptors are handled by logic in the composite + * device logic. + */ + +#ifdef CONFIG_CDCSER_COMPOSITE + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_UNSUPPORTEDSTDREQ), ctrl->req); +#else + switch (ctrl->req) + { + case USB_REQ_GETDESCRIPTOR: + { + /* The value field specifies the descriptor type in the MS byte and the + * descriptor index in the LS byte (order is little endian) + */ + + switch (ctrl->value[1]) + { + case USB_DESC_TYPE_DEVICE: + { + ret = USB_SIZEOF_DEVDESC; + memcpy(ctrlreq->buf, cdcser_getdevdesc(), ret); + } + break; + +#ifdef CONFIG_USBDEV_DUALSPEED + case USB_DESC_TYPE_DEVICEQUALIFIER: + { + ret = USB_SIZEOF_QUALDESC; + memcpy(ctrlreq->buf, cdcser_getqualdesc(), ret); + } + break; + + case USB_DESC_TYPE_OTHERSPEEDCONFIG: +#endif /* CONFIG_USBDEV_DUALSPEED */ + + case USB_DESC_TYPE_CONFIG: + { +#ifdef CONFIG_USBDEV_DUALSPEED + ret = cdcser_mkcfgdesc(ctrlreq->buf, dev->speed, ctrl->req); +#else + ret = cdcser_mkcfgdesc(ctrlreq->buf); +#endif + } + break; + + case USB_DESC_TYPE_STRING: + { + /* index == language code. */ + + ret = cdcser_mkstrdesc(ctrl->value[0], (struct usb_strdesc_s *)ctrlreq->buf); + } + break; + + default: + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_GETUNKNOWNDESC), value); + } + break; + } + } + break; + + case USB_REQ_SETCONFIGURATION: + { + if (ctrl->type == 0) + { + ret = cdcser_setconfig(priv, value); + } + } + break; + + case USB_REQ_GETCONFIGURATION: + { + if (ctrl->type == USB_DIR_IN) + { + *(uint8_t*)ctrlreq->buf = priv->config; + ret = 1; + } + } + break; + + case USB_REQ_SETINTERFACE: + { + if (ctrl->type == USB_REQ_RECIPIENT_INTERFACE) + { + if (priv->config == CDCSER_CONFIGID && + index == CDCSER_INTERFACEID && + value == CDCSER_ALTINTERFACEID) + { + cdcser_resetconfig(priv); + cdcser_setconfig(priv, priv->config); + ret = 0; + } + } + } + break; + + case USB_REQ_GETINTERFACE: + { + if (ctrl->type == (USB_DIR_IN|USB_REQ_RECIPIENT_INTERFACE) && + priv->config == CDCSER_CONFIGIDNONE) + { + if (index != CDCSER_INTERFACEID) + { + ret = -EDOM; + } + else + { + *(uint8_t*) ctrlreq->buf = CDCSER_ALTINTERFACEID; + ret = 1; + } + } + } + break; + + default: + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_UNSUPPORTEDSTDREQ), ctrl->req); + break; + } +#endif + } + break; + + /************************************************************************ + * CDC ACM-Specific Requests + ************************************************************************/ + + /* ACM_GET_LINE_CODING requests current DTE rate, stop-bits, parity, and + * number-of-character bits. (Optional) + */ + + case ACM_GET_LINE_CODING: + { + if (ctrl->type == (USB_DIR_IN|USB_REQ_TYPE_CLASS|USB_REQ_RECIPIENT_INTERFACE)) + { + /* Return the current line status from the private data structure */ + + memcpy(ctrlreq->buf, &priv->linecoding, SIZEOF_CDC_LINECODING); + ret = SIZEOF_CDC_LINECODING; + } + else + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_UNSUPPORTEDCLASSREQ), ctrl->type); + } + } + break; + + /* ACM_SET_LINE_CODING configures DTE rate, stop-bits, parity, and + * number-of-character bits. (Optional) + */ + + case ACM_SET_LINE_CODING: + { + if (ctrl->type == (USB_DIR_OUT|USB_REQ_TYPE_CLASS|USB_REQ_RECIPIENT_INTERFACE)) + { + /* Save the new line coding in the private data structure */ + + memcpy(&priv->linecoding, ctrlreq->buf, min(len, 7)); + ret = 0; + + /* If there is a registered callback to receive line status info, then + * callout now. + */ + + if (priv->callback) + { + priv->callback(CDCSER_EVENT_LINECODING); + } + } + else + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_UNSUPPORTEDCLASSREQ), ctrl->type); + } + } + break; + + /* ACM_SET_CTRL_LINE_STATE: RS-232 signal used to tell the DCE device the + * DTE device is now present. (Optional) + */ + + case ACM_SET_CTRL_LINE_STATE: + { + if (ctrl->type == (USB_DIR_OUT|USB_REQ_TYPE_CLASS|USB_REQ_RECIPIENT_INTERFACE)) + { + /* Save the control line state in the private data structure. Only bits + * 0 and 1 have meaning. + */ + + priv->ctrlline = value & 3; + ret = 0; + + /* If there is a registered callback to receive control line status info, + * then callout now. + */ + + if (priv->callback) + { + priv->callback(CDCSER_EVENT_CTRLLINE); + } + } + else + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_UNSUPPORTEDCLASSREQ), ctrl->type); + } + } + break; + + /* Sends special carrier*/ + + case ACM_SEND_BREAK: + { + if (ctrl->type == (USB_DIR_OUT|USB_REQ_TYPE_CLASS|USB_REQ_RECIPIENT_INTERFACE)) + { + /* If there is a registered callback to handle the SendBreak request, + * then callout now. + */ + + ret = 0; + if (priv->callback) + { + priv->callback(CDCSER_EVENT_SENDBREAK); + } + } + else + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_UNSUPPORTEDCLASSREQ), ctrl->type); + } + } + break; + + default: + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_UNSUPPORTEDTYPE), ctrl->type); + break; + } + + /* Respond to the setup command if data was returned. On an error return + * value (ret < 0), the USB driver will stall. + */ + + if (ret >= 0) + { + ctrlreq->len = min(len, ret); + ctrlreq->flags = USBDEV_REQFLAGS_NULLPKT; + ret = EP_SUBMIT(dev->ep0, ctrlreq); + if (ret < 0) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_EPRESPQ), (uint16_t)-ret); + ctrlreq->result = OK; + cdcser_ep0incomplete(dev->ep0, ctrlreq); + } + } + return ret; +} + +/**************************************************************************** + * Name: cdcser_disconnect + * + * Description: + * Invoked after all transfers have been stopped, when the host is + * disconnected. This function is probably called from the context of an + * interrupt handler. + * + ****************************************************************************/ + +static void cdcser_disconnect(FAR struct usbdev_s *dev) +{ + FAR struct cdcser_dev_s *priv; + irqstate_t flags; + + usbtrace(TRACE_CLASSDISCONNECT, 0); + +#ifdef CONFIG_DEBUG + if (!dev || !dev->ep0) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_INVALIDARG), 0); + return; + } +#endif + + /* Extract reference to private data */ + + priv = (FAR struct cdcser_dev_s *)dev->ep0->priv; + +#ifdef CONFIG_DEBUG + if (!priv) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_EP0NOTBOUND), 0); + return; + } +#endif + + /* Reset the configuration */ + + flags = irqsave(); + cdcser_resetconfig(priv); + + /* Clear out all data in the circular buffer */ + + priv->serdev.xmit.head = 0; + priv->serdev.xmit.tail = 0; + irqrestore(flags); + + /* Perform the soft connect function so that we will we can be + * re-enumerated. + */ + + DEV_CONNECT(dev); +} + +/**************************************************************************** + * Serial Device Methods + ****************************************************************************/ + +/**************************************************************************** + * Name: cdcuart_setup + * + * Description: + * This method is called the first time that the serial port is opened. + * + ****************************************************************************/ + +static int cdcuart_setup(FAR struct uart_dev_s *dev) +{ + FAR struct cdcser_dev_s *priv; + + usbtrace(CDCSER_CLASSAPI_SETUP, 0); + + /* Sanity check */ + +#if CONFIG_DEBUG + if (!dev || !dev->priv) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_INVALIDARG), 0); + return -EIO; + } +#endif + + /* Extract reference to private data */ + + priv = (FAR struct cdcser_dev_s*)dev->priv; + + /* Check if we have been configured */ + + if (priv->config == CDCSER_CONFIGIDNONE) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_SETUPNOTCONNECTED), 0); + return -ENOTCONN; + } + + return OK; +} + +/**************************************************************************** + * Name: cdcuart_shutdown + * + * Description: + * This method is called when the serial port is closed. This operation + * is very simple for the USB serial backend because the serial driver + * has already assured that the TX data has full drained -- it calls + * cdcuart_txempty() until that function returns true before calling this + * function. + * + ****************************************************************************/ + +static void cdcuart_shutdown(FAR struct uart_dev_s *dev) +{ + usbtrace(CDCSER_CLASSAPI_SHUTDOWN, 0); + + /* Sanity check */ + +#if CONFIG_DEBUG + if (!dev || !dev->priv) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_INVALIDARG), 0); + } +#endif +} + +/**************************************************************************** + * Name: cdcuart_attach + * + * Description: + * Does not apply to the USB serial class device + * + ****************************************************************************/ + +static int cdcuart_attach(FAR struct uart_dev_s *dev) +{ + usbtrace(CDCSER_CLASSAPI_ATTACH, 0); + return OK; +} + +/**************************************************************************** + * Name: cdcuart_detach + * + * Description: +* Does not apply to the USB serial class device + * + ****************************************************************************/ + +static void cdcuart_detach(FAR struct uart_dev_s *dev) +{ + usbtrace(CDCSER_CLASSAPI_DETACH, 0); +} + +/**************************************************************************** + * Name: cdcuart_ioctl + * + * Description: + * All ioctl calls will be routed through this method + * + ****************************************************************************/ + +static int cdcuart_ioctl(FAR struct file *filep,int cmd,unsigned long arg) +{ + struct inode *inode = filep->f_inode; + struct cdcser_dev_s *priv = inode->i_private; + int ret = OK; + + switch (cmd) + { + /* CAICO_REGISTERCB + * Register a callback for serial event notification. Argument: + * cdcser_callback_t. See cdcser_callback_t type definition below. + * NOTE: The callback will most likely invoked at the interrupt level. + * The called back function should, therefore, limit its operations to + * invoking some kind of IPC to handle the serial event in some normal + * task environment. + */ + + case CAIOC_REGISTERCB: + { + /* Save the new callback function */ + + priv->callback = (cdcser_callback_t)((uintptr_t)arg); + } + break; + + /* CAIOC_GETLINECODING + * Get current line coding. Argument: struct cdc_linecoding_s*. + * See include/nuttx/usb/cdc.h for structure definition. This IOCTL + * should be called to get the data associated with the + * CDCSER_EVENT_LINECODING event). + */ + + case CAIOC_GETLINECODING: + { + FAR struct cdc_linecoding_s *ptr = (FAR struct cdc_linecoding_s *)((uintptr_t)arg); + if (ptr) + { + memcpy(ptr, &priv->linecoding, sizeof(struct cdc_linecoding_s)); + } + else + { + ret = -EINVAL; + } + } + break; + + /* CAIOC_GETCTRLLINE + * Get control line status bits. Argument FAR int*. See + * include/nuttx/usb/cdc.h for bit definitions. This IOCTL should be + * called to get the data associated CDCSER_EVENT_CTRLLINE event. + */ + + case CAIOC_GETCTRLLINE: + { + FAR int *ptr = (FAR int *)((uintptr_t)arg); + if (ptr) + { + *ptr = priv->ctrlline; + } + else + { + ret = -EINVAL; + } + } + break; + + /* CAIOC_NOTIFY + * Send a serial state to the host via the Interrupt IN endpoint. + * Argument: int. This includes the current state of the carrier detect, + * DSR, break, and ring signal. See "Table 69: UART State Bitmap Values" + * and CDC_UART_definitions in include/nuttx/usb/cdc.h. + */ + + case CAIOC_NOTIFY: + { + /* Not yet implemented. I probably won't bother to implement until + * I comr up with a usage model that needs it. + * + * Here is what the needs to be done: + * + * 1. Format and send a request header with: + * + * bmRequestType: + * USB_REQ_DIR_IN|USB_REQ_TYPE_CLASS|USB_REQ_RECIPIENT_INTERFACE + * bRequest: ACM_SERIAL_STATE + * wValue: 0 + * wIndex: 0 + * wLength: Length of data + * + * 2. Followed by the notification data (in a separate packet) + */ + + ret = -ENOSYS; + } + break; + + default: + ret = -ENOTTY; + break; + } + + return ret; +} + +/**************************************************************************** + * Name: cdcuart_rxint + * + * Description: + * Called by the serial driver to enable or disable RX interrupts. We, of + * course, have no RX interrupts but must behave consistently. This method + * is called under the conditions: + * + * 1. With enable==true when the port is opened (just after cdcuart_setup + * and cdcuart_attach are called called) + * 2. With enable==false while transferring data from the RX buffer + * 2. With enable==true while waiting for more incoming data + * 3. With enable==false when the port is closed (just before cdcuart_detach + * and cdcuart_shutdown are called). + * + ****************************************************************************/ + +static void cdcuart_rxint(FAR struct uart_dev_s *dev, bool enable) +{ + FAR struct cdcser_dev_s *priv; + FAR uart_dev_t *serdev; + irqstate_t flags; + + usbtrace(CDCSER_CLASSAPI_RXINT, (uint16_t)enable); + + /* Sanity check */ + +#if CONFIG_DEBUG + if (!dev || !dev->priv) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_INVALIDARG), 0); + return; + } +#endif + + /* Extract reference to private data */ + + priv = (FAR struct cdcser_dev_s*)dev->priv; + serdev = &priv->serdev; + + /* We need exclusive access to the RX buffer and private structure + * in the following. + */ + + flags = irqsave(); + if (enable) + { + /* RX "interrupts" are enabled. Is this a transition from disabled + * to enabled state? + */ + + if (!priv->rxenabled) + { + /* Yes. During the time that RX interrupts are disabled, the + * the serial driver will be extracting data from the circular + * buffer and modifying recv.tail. During this time, we + * should avoid modifying recv.head; When interrupts are restored, + * we can update the head pointer for all of the data that we + * put into cicular buffer while "interrupts" were disabled. + */ + + if (priv->rxhead != serdev->recv.head) + { + serdev->recv.head = priv->rxhead; + + /* Yes... signal the availability of new data */ + + uart_datareceived(serdev); + } + + /* RX "interrupts are no longer disabled */ + + priv->rxenabled = true; + } + } + + /* RX "interrupts" are disabled. Is this a transition from enabled + * to disabled state? + */ + + else if (priv->rxenabled) + { + /* Yes. During the time that RX interrupts are disabled, the + * the serial driver will be extracting data from the circular + * buffer and modifying recv.tail. During this time, we + * should avoid modifying recv.head; When interrupts are disabled, + * we use a shadow index and continue adding data to the circular + * buffer. + */ + + priv->rxhead = serdev->recv.head; + priv->rxenabled = false; + } + irqrestore(flags); +} + +/**************************************************************************** + * Name: cdcuart_txint + * + * Description: + * Called by the serial driver to enable or disable TX interrupts. We, of + * course, have no TX interrupts but must behave consistently. Initially, + * TX interrupts are disabled. This method is called under the conditions: + * + * 1. With enable==false while transferring data into the TX buffer + * 2. With enable==true when data may be taken from the buffer. + * 3. With enable==false when the TX buffer is empty + * + ****************************************************************************/ + +static void cdcuart_txint(FAR struct uart_dev_s *dev, bool enable) +{ + FAR struct cdcser_dev_s *priv; + + usbtrace(CDCSER_CLASSAPI_TXINT, (uint16_t)enable); + + /* Sanity checks */ + +#if CONFIG_DEBUG + if (!dev || !dev->priv) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_INVALIDARG), 0); + return; + } +#endif + + /* Extract references to private data */ + + priv = (FAR struct cdcser_dev_s*)dev->priv; + + /* If the new state is enabled and if there is data in the XMIT buffer, + * send the next packet now. + */ + + uvdbg("enable=%d head=%d tail=%d\n", + enable, priv->serdev.xmit.head, priv->serdev.xmit.tail); + + if (enable && priv->serdev.xmit.head != priv->serdev.xmit.tail) + { + cdcser_sndpacket(priv); + } +} + +/**************************************************************************** + * Name: cdcuart_txempty + * + * Description: + * Return true when all data has been sent. This is called from the + * serial driver when the driver is closed. It will call this API + * periodically until it reports true. NOTE that the serial driver takes all + * responsibility for flushing TX data through the hardware so we can be + * a bit sloppy about that. + * + ****************************************************************************/ + +static bool cdcuart_txempty(FAR struct uart_dev_s *dev) +{ + FAR struct cdcser_dev_s *priv = (FAR struct cdcser_dev_s*)dev->priv; + + usbtrace(CDCSER_CLASSAPI_TXEMPTY, 0); + +#if CONFIG_DEBUG + if (!priv) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_INVALIDARG), 0); + return true; + } +#endif + + /* When all of the allocated write requests have been returned to the + * reqlist, then there is no longer any TX data in flight. + */ + + return priv->nwrq >= CONFIG_CDCSER_NWRREQS; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: cdcser_initialize + * + * Description: + * Register USB serial port (and USB serial console if so configured). + * + * Input Parameter: + * Device minor number. E.g., minor 0 would correspond to /dev/ttyUSB0. + * + * Returned Value: + * Zero (OK) means that the driver was successfully registered. On any + * failure, a negated errno value is retured. + * + ****************************************************************************/ + +int cdcser_initialize(int minor) +{ + FAR struct cdcser_alloc_s *alloc; + FAR struct cdcser_dev_s *priv; + FAR struct cdcser_driver_s *drvr; + char devname[16]; + int ret; + + /* Allocate the structures needed */ + + alloc = (FAR struct cdcser_alloc_s*)kmalloc(sizeof(struct cdcser_alloc_s)); + if (!alloc) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_ALLOCDEVSTRUCT), 0); + return -ENOMEM; + } + + /* Convenience pointers into the allocated blob */ + + priv = &alloc->dev; + drvr = &alloc->drvr; + + /* Initialize the USB serial driver structure */ + + memset(priv, 0, sizeof(struct cdcser_dev_s)); + sq_init(&priv->reqlist); + + /* Fake line status */ + + priv->linecoding.baud[0] = (115200) & 0xff; /* Baud=115200 */ + priv->linecoding.baud[1] = (115200 >> 8) & 0xff; + priv->linecoding.baud[2] = (115200 >> 16) & 0xff; + priv->linecoding.baud[3] = (115200 >> 24) & 0xff; + priv->linecoding.stop = CDC_CHFMT_STOP1; /* One stop bit */ + priv->linecoding.parity = CDC_PARITY_NONE; /* No parity */ + priv->linecoding.nbits = 8; /* 8 data bits */ + + /* Initialize the serial driver sub-structure */ + + priv->serdev.recv.size = CONFIG_CDCSER_RXBUFSIZE; + priv->serdev.recv.buffer = priv->rxbuffer; + priv->serdev.xmit.size = CONFIG_CDCSER_TXBUFSIZE; + priv->serdev.xmit.buffer = priv->txbuffer; + priv->serdev.ops = &g_uartops; + priv->serdev.priv = priv; + + /* Initialize the USB class driver structure */ + +#ifdef CONFIG_USBDEV_DUALSPEED + drvr->drvr.speed = USB_SPEED_HIGH; +#else + drvr->drvr.speed = USB_SPEED_FULL; +#endif + drvr->drvr.ops = &g_driverops; + drvr->dev = priv; + + /* Register the USB serial class driver */ + + ret = usbdev_register(&drvr->drvr); + if (ret) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_DEVREGISTER), (uint16_t)-ret); + goto errout_with_alloc; + } + + /* Register the USB serial console */ + +#ifdef CONFIG_CDCSER_CONSOLE + g_usbserialport.isconsole = true; + ret = uart_register("/dev/console", &pri->serdev); + if (ret < 0) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_CONSOLEREGISTER), (uint16_t)-ret); + goto errout_with_class; + } +#endif + + /* Register the single port supported by this implementation */ + + sprintf(devname, "/dev/ttyUSB%d", minor); + ret = uart_register(devname, &priv->serdev); + if (ret) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_UARTREGISTER), (uint16_t)-ret); + goto errout_with_class; + } + return OK; + +errout_with_class: + usbdev_unregister(&drvr->drvr); +errout_with_alloc: + kfree(alloc); + return ret; +} diff --git a/nuttx/drivers/usbdev/cdcacm.h b/nuttx/drivers/usbdev/cdcacm.h new file mode 100644 index 000000000..ca7352da8 --- /dev/null +++ b/nuttx/drivers/usbdev/cdcacm.h @@ -0,0 +1,296 @@ +/**************************************************************************** + * drivers/usbdev/cdcacm.h + * + * Copyright (C) 2011-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 + * 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. + * + ****************************************************************************/ + +#ifndef __DRIVERS_USBDEV_CDCACM_H +#define __DRIVERS_USBDEV_CDCACM_H 1 + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include + +#include +#include +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ +/* Configuration ************************************************************/ +/* If the serial device is configured as part of a composite device than both + * CONFIG_USBDEV_COMPOSITE and CONFIG_CDCSER_COMPOSITE must be defined. + */ + +#ifndef CONFIG_USBDEV_COMPOSITE +# undef CONFIG_CDCSER_COMPOSITE +#endif + +#if defined(CONFIG_CDCSER_COMPOSITE) && !defined(CONFIG_CDCSER_STRBASE) +# define CONFIG_CDCSER_STRBASE (4) +#endif + +/* Packet and request buffer sizes */ + +#ifndef CONFIG_CDCSER_COMPOSITE +# ifndef CONFIG_CDCSER_EP0MAXPACKET +# define CONFIG_CDCSER_EP0MAXPACKET 64 +# endif +#endif + +/* Descriptors **************************************************************/ +/* These settings are not modifiable via the NuttX configuration */ + +#define CDC_VERSIONNO 0x0110 /* CDC version number 1.10 (BCD) */ +#define CDCSER_CONFIGIDNONE (0) /* Config ID means to return to address mode */ +#define CDCSER_INTERFACEID (0) +#define CDCSER_ALTINTERFACEID (0) + +/* Configuration descriptor values */ + +#define CDCSER_CONFIGID (1) /* The only supported configuration ID */ + +/* Buffer big enough for any of our descriptors (the config descriptor is the + * biggest). + */ + +#define CDCSER_MXDESCLEN (64) + +/* Device descriptor values */ + +#define CDCSER_VERSIONNO (0x0101) /* Device version number 1.1 (BCD) */ +#define CDCSER_NCONFIGS (1) /* Number of configurations supported */ + +/* Configuration descriptor values */ + +#define CDCSER_NINTERFACES (2) /* Number of interfaces in the configuration */ + +/* String language */ + +#define CDCSER_STR_LANGUAGE (0x0409) /* en-us */ + +/* Descriptor strings. If there serial device is part of a composite device + * then the manufacturer, product, and serial number strings will be provided + * by the composite logic. + */ + +#ifndef CONFIG_CDCSER_COMPOSITE +# define CDCSER_MANUFACTURERSTRID (1) +# define CDCSER_PRODUCTSTRID (2) +# define CDCSER_SERIALSTRID (3) +# define CDCSER_CONFIGSTRID (4) + +# undef CONFIG_CDCSER_STRBASE +# define CONFIG_CDCSER_STRBASE (4) +#endif + +/* These string IDs only exist if a user-defined string is provided */ + +#ifdef CONFIG_CDCSER_NOTIFSTR +# define CDCSER_NOTIFSTRID (CONFIG_CDCSER_STRBASE+1) +#else +# define CDCSER_NOTIFSTRID CONFIG_CDCSER_STRBASE +#endif + +#ifdef CONFIG_CDCSER_DATAIFSTR +# define CDCSER_DATAIFSTRID (CDCSER_NOTIFSTRID+1) +#else +# define CDCSER_DATAIFSTRID CDCSER_NOTIFSTRID +#endif + +#define CDCSER_LASTSTRID CDCSER_DATAIFSTRID + +/* Configuration descriptor size */ + +#ifndef CONFIG_CDCSER_COMPOSITE + +/* Number of individual descriptors in the configuration descriptor: + * Configuration descriptor + (2) interface descriptors + (3) endpoint + * descriptors + (3) ACM descriptors. + */ + +# define CDCSER_CFGGROUP_SIZE (9) + +/* The size of the config descriptor: (9 + 2*9 + 3*7 + 4 + 5 + 5) = 62 */ + +# define SIZEOF_CDCSER_CFGDESC \ + (USB_SIZEOF_CFGDESC + 2*USB_SIZEOF_IFDESC + 3*USB_SIZEOF_EPDESC + \ + SIZEOF_ACM_FUNCDESC + SIZEOF_HDR_FUNCDESC + SIZEOF_UNION_FUNCDESC(1)) +#else + +/* Number of individual descriptors in the configuration descriptor: + * (2) interface descriptors + (3) endpoint descriptors + (3) ACM descriptors. + */ + +# define CDCSER_CFGGROUP_SIZE (8) + +/* The size of the config descriptor: (2*9 + 3*7 + 4 + 5 + 5) = 53 */ + +# define SIZEOF_CDCSER_CFGDESC \ + (2*USB_SIZEOF_IFDESC + 3*USB_SIZEOF_EPDESC + SIZEOF_ACM_FUNCDESC + \ + SIZEOF_HDR_FUNCDESC + SIZEOF_UNION_FUNCDESC(1)) +#endif + +/* Endpoint configuration ****************************************************/ + +#define CDCSER_EPINTIN_ADDR (USB_DIR_IN|CONFIG_CDCSER_EPINTIN) +#define CDCSER_EPINTIN_ATTR (USB_EP_ATTR_XFER_INT) + +#define CDCSER_EPOUTBULK_ADDR (CONFIG_CDCSER_EPBULKOUT) +#define CDCSER_EPOUTBULK_ATTR (USB_EP_ATTR_XFER_BULK) + +#define CDCSER_EPINBULK_ADDR (USB_DIR_IN|CONFIG_CDCSER_EPBULKIN) +#define CDCSER_EPINBULK_ATTR (USB_EP_ATTR_XFER_BULK) + +/* Misc Macros **************************************************************/ +/* min/max macros */ + +#ifndef min +# define min(a,b) ((a)<(b)?(a):(b)) +#endif + +#ifndef max +# define max(a,b) ((a)>(b)?(a):(b)) +#endif + +/* Trace values *************************************************************/ + +#define CDCSER_CLASSAPI_SETUP TRACE_EVENT(TRACE_CLASSAPI_ID, USBSER_TRACECLASSAPI_SETUP) +#define CDCSER_CLASSAPI_SHUTDOWN TRACE_EVENT(TRACE_CLASSAPI_ID, USBSER_TRACECLASSAPI_SHUTDOWN) +#define CDCSER_CLASSAPI_ATTACH TRACE_EVENT(TRACE_CLASSAPI_ID, USBSER_TRACECLASSAPI_ATTACH) +#define CDCSER_CLASSAPI_DETACH TRACE_EVENT(TRACE_CLASSAPI_ID, USBSER_TRACECLASSAPI_DETACH) +#define CDCSER_CLASSAPI_IOCTL TRACE_EVENT(TRACE_CLASSAPI_ID, USBSER_TRACECLASSAPI_IOCTL) +#define CDCSER_CLASSAPI_RECEIVE TRACE_EVENT(TRACE_CLASSAPI_ID, USBSER_TRACECLASSAPI_RECEIVE) +#define CDCSER_CLASSAPI_RXINT TRACE_EVENT(TRACE_CLASSAPI_ID, USBSER_TRACECLASSAPI_RXINT) +#define CDCSER_CLASSAPI_RXAVAILABLE TRACE_EVENT(TRACE_CLASSAPI_ID, USBSER_TRACECLASSAPI_RXAVAILABLE) +#define CDCSER_CLASSAPI_SEND TRACE_EVENT(TRACE_CLASSAPI_ID, USBSER_TRACECLASSAPI_SEND) +#define CDCSER_CLASSAPI_TXINT TRACE_EVENT(TRACE_CLASSAPI_ID, USBSER_TRACECLASSAPI_TXINT) +#define CDCSER_CLASSAPI_TXREADY TRACE_EVENT(TRACE_CLASSAPI_ID, USBSER_TRACECLASSAPI_TXREADY) +#define CDCSER_CLASSAPI_TXEMPTY TRACE_EVENT(TRACE_CLASSAPI_ID, USBSER_TRACECLASSAPI_TXEMPTY) + +/**************************************************************************** + * Public Types + ****************************************************************************/ + +enum cdcser_epdesc_e +{ + CDCSER_EPINTIN = 0, /* Interrupt IN endpoint descriptor */ + CDCSER_EPBULKOUT, /* Bulk OUT endpoint descriptor */ + CDCSER_EPBULKIN /* Bulk IN endpoint descriptor */ +}; + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Name: cdcser_mkstrdesc + * + * Description: + * Construct a string descriptor + * + ****************************************************************************/ + +int cdcser_mkstrdesc(uint8_t id, struct usb_strdesc_s *strdesc); + +/**************************************************************************** + * Name: cdcser_getepdesc + * + * Description: + * Return a pointer to the raw device descriptor + * + ****************************************************************************/ + +#ifndef CONFIG_CDCSER_COMPOSITE +FAR const struct usb_devdesc_s *cdcser_getdevdesc(void); +#endif + +/**************************************************************************** + * Name: cdcser_getepdesc + * + * Description: + * Return a pointer to the raw endpoint descriptor (used for configuring + * endpoints) + * + ****************************************************************************/ + +FAR const struct usb_epdesc_s *cdcser_getepdesc(enum cdcser_epdesc_e epid); + +/**************************************************************************** + * Name: cdcser_mkepdesc + * + * Description: + * Construct the endpoint descriptor using the correct max packet size. + * + ****************************************************************************/ + +#ifdef CONFIG_USBDEV_DUALSPEED +void cdcser_mkepdesc(enum cdcser_epdesc_e epid, + uint16_t mxpacket, FAR struct usb_epdesc_s *outdesc); +#endif + +/**************************************************************************** + * Name: cdcser_mkcfgdesc + * + * Description: + * Construct the configuration descriptor + * + ****************************************************************************/ + +#ifdef CONFIG_USBDEV_DUALSPEED +int16_t cdcser_mkcfgdesc(FAR uint8_t *buf, uint8_t speed, uint8_t type); +#else +int16_t cdcser_mkcfgdesc(FAR uint8_t *buf); +#endif + +/**************************************************************************** + * Name: cdcser_getqualdesc + * + * Description: + * Return a pointer to the raw qual descriptor + * + ****************************************************************************/ + +#if !defined(CONFIG_CDCSER_COMPOSITE) && defined(CONFIG_USBDEV_DUALSPEED) +FAR const struct usb_qualdesc_s *cdcser_getqualdesc(void); +#endif + +#endif /* __DRIVERS_USBDEV_CDCACM_H */ diff --git a/nuttx/drivers/usbdev/cdcacm_descriptors.c b/nuttx/drivers/usbdev/cdcacm_descriptors.c new file mode 100644 index 000000000..b8affe3cc --- /dev/null +++ b/nuttx/drivers/usbdev/cdcacm_descriptors.c @@ -0,0 +1,584 @@ +/**************************************************************************** + * drivers/usbdev/cdcacm_descriptors.c + * + * Copyright (C) 2011-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 + * 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 + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "cdcacm.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/* Describes one description in the group of descriptors forming the + * total configuration descriptor. + */ + +struct cfgdecsc_group_s +{ + uint16_t descsize; /* Size of the descriptor in bytes */ + uint16_t hsepsize; /* High speed max packet size */ + FAR void *desc; /* A pointer to the descriptor */ +}; + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/* USB descriptor templates these will be copied and modified **************/ +/* Device Descriptor. If the USB serial device is configured as part of + * composite device, then the device descriptor will be provided by the + * composite device logic. + */ + +#ifndef CONFIG_CDCSER_COMPOSITE +static const struct usb_devdesc_s g_devdesc = +{ + USB_SIZEOF_DEVDESC, /* len */ + USB_DESC_TYPE_DEVICE, /* type */ + { /* usb */ + LSBYTE(0x0200), + MSBYTE(0x0200) + }, + USB_CLASS_CDC, /* class */ + CDC_SUBCLASS_NONE, /* subclass */ + CDC_PROTO_NONE, /* protocol */ + CONFIG_CDCSER_EP0MAXPACKET, /* maxpacketsize */ + { + LSBYTE(CONFIG_CDCSER_VENDORID), /* vendor */ + MSBYTE(CONFIG_CDCSER_VENDORID) + }, + { + LSBYTE(CONFIG_CDCSER_PRODUCTID), /* product */ + MSBYTE(CONFIG_CDCSER_PRODUCTID) + }, + { + LSBYTE(CDCSER_VERSIONNO), /* device */ + MSBYTE(CDCSER_VERSIONNO) + }, + CDCSER_MANUFACTURERSTRID, /* imfgr */ + CDCSER_PRODUCTSTRID, /* iproduct */ + CDCSER_SERIALSTRID, /* serno */ + CDCSER_NCONFIGS /* nconfigs */ +}; +#endif + +/* Configuration descriptor. If the USB serial device is configured as part of + * composite device, then the configuration descriptor will be provided by the + * composite device logic. + */ + +#ifndef CONFIG_CDCSER_COMPOSITE +static const struct usb_cfgdesc_s g_cfgdesc = +{ + USB_SIZEOF_CFGDESC, /* len */ + USB_DESC_TYPE_CONFIG, /* type */ + { + LSBYTE(SIZEOF_CDCSER_CFGDESC), /* LS totallen */ + MSBYTE(SIZEOF_CDCSER_CFGDESC) /* MS totallen */ + }, + CDCSER_NINTERFACES, /* ninterfaces */ + CDCSER_CONFIGID, /* cfgvalue */ + CDCSER_CONFIGSTRID, /* icfg */ + USB_CONFIG_ATTR_ONE|SELFPOWERED|REMOTEWAKEUP, /* attr */ + (CONFIG_USBDEV_MAXPOWER + 1) / 2 /* mxpower */ +}; +#endif + +/* Notification interface */ + +static const struct usb_ifdesc_s g_notifdesc = +{ + USB_SIZEOF_IFDESC, /* len */ + USB_DESC_TYPE_INTERFACE, /* type */ + 0, /* ifno */ + 0, /* alt */ + 1, /* neps */ + USB_CLASS_CDC, /* class */ + CDC_SUBCLASS_ACM, /* subclass */ + CDC_PROTO_ATM, /* proto */ +#ifdef CONFIG_CDCSER_NOTIFSTR + CDCSER_NOTIFSTRID /* iif */ +#else + 0 /* iif */ +#endif +}; + +/* Header functional descriptor */ + +static const struct cdc_hdr_funcdesc_s g_funchdr = +{ + SIZEOF_HDR_FUNCDESC, /* size */ + USB_DESC_TYPE_CSINTERFACE, /* type */ + CDC_DSUBTYPE_HDR, /* subtype */ + { + LSBYTE(CDC_VERSIONNO), /* LS cdc */ + MSBYTE(CDC_VERSIONNO) /* MS cdc */ + } +}; + +/* ACM functional descriptor */ + +static const struct cdc_acm_funcdesc_s g_acmfunc = +{ + SIZEOF_ACM_FUNCDESC, /* size */ + USB_DESC_TYPE_CSINTERFACE, /* type */ + CDC_DSUBTYPE_ACM, /* subtype */ + 0x06 /* caps */ +}; + +/* Union functional descriptor */ + +static const struct cdc_union_funcdesc_s g_unionfunc = +{ + SIZEOF_UNION_FUNCDESC(1), /* size */ + USB_DESC_TYPE_CSINTERFACE, /* type */ + CDC_DSUBTYPE_UNION, /* subtype */ + 0, /* master */ + {1} /* slave[0] */ +}; + +/* Interrupt IN endpoint descriptor */ + +static const struct usb_epdesc_s g_epintindesc = +{ + USB_SIZEOF_EPDESC, /* len */ + USB_DESC_TYPE_ENDPOINT, /* type */ + CDCSER_EPINTIN_ADDR, /* addr */ + CDCSER_EPINTIN_ATTR, /* attr */ + { + LSBYTE(CONFIG_CDCSER_EPINTIN_FSSIZE), /* maxpacket (full speed) */ + MSBYTE(CONFIG_CDCSER_EPINTIN_FSSIZE) + }, + 0xff /* interval */ +}; + +/* Data interface descriptor */ + +static const struct usb_ifdesc_s g_dataifdesc = +{ + USB_SIZEOF_IFDESC, /* len */ + USB_DESC_TYPE_INTERFACE, /* type */ + 1, /* ifno */ + 0, /* alt */ + 2, /* neps */ + USB_CLASS_CDC_DATA, /* class */ + CDC_DATA_SUBCLASS_NONE, /* subclass */ + CDC_DATA_PROTO_NONE, /* proto */ +#ifdef CONFIG_CDCSER_DATAIFSTR + CDCSER_DATAIFSTRID /* iif */ +#else + 0 /* iif */ +#endif +}; + +/* Bulk OUT endpoint descriptor */ + +static const struct usb_epdesc_s g_epbulkoutdesc = +{ + USB_SIZEOF_EPDESC, /* len */ + USB_DESC_TYPE_ENDPOINT, /* type */ + CDCSER_EPOUTBULK_ADDR, /* addr */ + CDCSER_EPOUTBULK_ATTR, /* attr */ + { + LSBYTE(CONFIG_CDCSER_EPBULKOUT_FSSIZE), /* maxpacket (full speed) */ + MSBYTE(CONFIG_CDCSER_EPBULKOUT_FSSIZE) + }, + 1 /* interval */ +}; + +/* Bulk IN endpoint descriptor */ + +static const struct usb_epdesc_s g_epbulkindesc = +{ + USB_SIZEOF_EPDESC, /* len */ + USB_DESC_TYPE_ENDPOINT, /* type */ + CDCSER_EPINBULK_ADDR, /* addr */ + CDCSER_EPINBULK_ATTR, /* attr */ + { + LSBYTE(CONFIG_CDCSER_EPBULKIN_FSSIZE), /* maxpacket (full speed) */ + MSBYTE(CONFIG_CDCSER_EPBULKIN_FSSIZE) + }, + 1 /* interval */ +}; + +/* The components of the the configuration descriptor are maintained as + * a collection of separate descriptor structure coordinated by the + * following array. These descriptors could have been combined into + * one larger "super" configuration descriptor structure. However, I + * have concerns about compiler-dependent alignment and packing. Since + * the individual structures consist only of byte types, alignment and + * packing is not an issue. And since the are concatentated at run time + * instead of compile time, there should no issues there either. + */ + +static const struct cfgdecsc_group_s g_cfggroup[CDCSER_CFGGROUP_SIZE] = +{ + /* Configuration Descriptor. If the serial device is used in as part + * or a composite device, then the configuration descriptor is + * provided by the composite device logic. + */ + +#ifndef CONFIG_CDCSER_COMPOSITE + { + USB_SIZEOF_CFGDESC, /* 1. Configuration descriptor */ + 0, + (FAR void *)&g_cfgdesc + }, +#endif + { + USB_SIZEOF_IFDESC, /* 2. Notification interface */ + 0, + (FAR void *)&g_notifdesc + }, + { + SIZEOF_HDR_FUNCDESC, /* 3. Header functional descriptor */ + 0, + (FAR void *)&g_funchdr + }, + { + SIZEOF_ACM_FUNCDESC, /* 4. ACM functional descriptor */ + 0, + (FAR void *)&g_acmfunc + }, + { + SIZEOF_UNION_FUNCDESC(1), /* 5. Union functional descriptor */ + 0, + (FAR void *)&g_unionfunc + }, + { + USB_SIZEOF_EPDESC, /* 6. Interrupt IN endpoint descriptor */ + CONFIG_CDCSER_EPINTIN_HSSIZE, + (FAR void *)&g_epintindesc + }, + { + USB_SIZEOF_IFDESC, /* 7. Data interface descriptor */ + 0, + (FAR void *)&g_dataifdesc + }, + { + USB_SIZEOF_EPDESC, /* 8. Bulk OUT endpoint descriptor */ + CONFIG_CDCSER_EPBULKOUT_HSSIZE, + (FAR void *)&g_epbulkoutdesc + }, + { + USB_SIZEOF_EPDESC, /* 9. Bulk OUT endpoint descriptor */ + CONFIG_CDCSER_EPBULKIN_HSSIZE, + (FAR void *)&g_epbulkindesc + } +}; + +#if !defined(CONFIG_CDCSER_COMPOSITE) && defined(CONFIG_USBDEV_DUALSPEED) +static const struct usb_qualdesc_s g_qualdesc = +{ + USB_SIZEOF_QUALDESC, /* len */ + USB_DESC_TYPE_DEVICEQUALIFIER, /* type */ + { /* usb */ + LSBYTE(0x0200), + MSBYTE(0x0200) + }, + USB_CLASS_VENDOR_SPEC, /* class */ + 0, /* subclass */ + 0, /* protocol */ + CONFIG_CDCSER_EP0MAXPACKET, /* mxpacketsize */ + CDCSER_NCONFIGS, /* nconfigs */ + 0, /* reserved */ +}; +#endif + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: cdcser_mkstrdesc + * + * Description: + * Construct a string descriptor + * + ****************************************************************************/ + +int cdcser_mkstrdesc(uint8_t id, struct usb_strdesc_s *strdesc) +{ +#if !defined(CONFIG_CDCSER_COMPOSITE) || defined(CONFIG_CDCSER_NOTIFSTR) || \ + defined(CONFIG_CDCSER_DATAIFSTR) + + const char *str; + int len; + int ndata; + int i; + + switch (id) + { +#ifndef CONFIG_CDCSER_COMPOSITE + case 0: + { + /* Descriptor 0 is the language id */ + + strdesc->len = 4; + strdesc->type = USB_DESC_TYPE_STRING; + strdesc->data[0] = LSBYTE(CDCSER_STR_LANGUAGE); + strdesc->data[1] = MSBYTE(CDCSER_STR_LANGUAGE); + return 4; + } + + case CDCSER_MANUFACTURERSTRID: + str = CONFIG_CDCSER_VENDORSTR; + break; + + case CDCSER_PRODUCTSTRID: + str = CONFIG_CDCSER_PRODUCTSTR; + break; + + case CDCSER_SERIALSTRID: + str = CONFIG_CDCSER_SERIALSTR; + break; + + case CDCSER_CONFIGSTRID: + str = CONFIG_CDCSER_CONFIGSTR; + break; +#endif + +#ifdef CONFIG_CDCSER_NOTIFSTR + case CDCSER_NOTIFSTRID: + str = CONFIG_CDCSER_NOTIFSTR; + break; +#endif + +#ifdef CONFIG_CDCSER_DATAIFSTR + case CDCSER_DATAIFSTRID: + str = CONFIG_CDCSER_DATAIFSTR; + break; +#endif + + default: + return -EINVAL; + } + + /* The string is utf16-le. The poor man's utf-8 to utf16-le + * conversion below will only handle 7-bit en-us ascii + */ + + len = strlen(str); + for (i = 0, ndata = 0; i < len; i++, ndata += 2) + { + strdesc->data[ndata] = str[i]; + strdesc->data[ndata+1] = 0; + } + + strdesc->len = ndata+2; + strdesc->type = USB_DESC_TYPE_STRING; + return strdesc->len; +#else + return -EINVAL; +#endif +} + +/**************************************************************************** + * Name: cdcser_getepdesc + * + * Description: + * Return a pointer to the raw device descriptor + * + ****************************************************************************/ + +#ifndef CONFIG_CDCSER_COMPOSITE +FAR const struct usb_devdesc_s *cdcser_getdevdesc(void) +{ + return &g_devdesc; +} +#endif + +/**************************************************************************** + * Name: cdcser_getepdesc + * + * Description: + * Return a pointer to the raw endpoint struct (used for configuring + * endpoints) + * + ****************************************************************************/ + +FAR const struct usb_epdesc_s *cdcser_getepdesc(enum cdcser_epdesc_e epid) +{ + switch (epid) + { + case CDCSER_EPINTIN: /* Interrupt IN endpoint */ + return &g_epintindesc; + + case CDCSER_EPBULKOUT: /* Bulk OUT endpoint */ + return &g_epbulkoutdesc; + + case CDCSER_EPBULKIN: /* Bulk IN endpoint */ + return &g_epbulkindesc; + + default: + return NULL; + } +} + +/**************************************************************************** + * Name: cdcser_mkepdesc + * + * Description: + * Construct the endpoint descriptor using the correct max packet size. + * + ****************************************************************************/ + +#ifdef CONFIG_USBDEV_DUALSPEED +void cdcser_mkepdesc(num cdcser_epdesc_e epid, uint16_t mxpacket, + FAR struct usb_epdesc_s *outdesc) +{ + FAR const struct usb_epdesc_s *indesc; + + /* Copy the "canned" descriptor */ + + indesc = cdcser_getepdesc(epid) + memcpy(outdesc, indesc, USB_SIZEOF_EPDESC); + + /* Then add the correct max packet size */ + + outdesc->mxpacketsize[0] = LSBYTE(mxpacket); + outdesc->mxpacketsize[1] = MSBYTE(mxpacket); +} +#endif + +/**************************************************************************** + * Name: cdcser_mkcfgdesc + * + * Description: + * Construct the configuration descriptor + * + ****************************************************************************/ + +#ifdef CONFIG_USBDEV_DUALSPEED +int16_t cdcser_mkcfgdesc(FAR uint8_t *buf, uint8_t speed, uint8_t type) +#else +int16_t cdcser_mkcfgdesc(FAR uint8_t *buf) +#endif +{ + FAR const struct cfgdecsc_group_s *group; + FAR uint8_t *dest = buf; + int i; + +#ifdef CONFIG_USBDEV_DUALSPEED + bool hispeed = (speed == USB_SPEED_HIGH); + + /* Check for switches between high and full speed */ + + if (type == USB_DESC_TYPE_OTHERSPEEDCONFIG) + { + hispeed = !hispeed; + } +#endif + + /* Copy all of the descriptors in the group */ + + for (i = 0, dest = buf; i < CDCSER_CFGGROUP_SIZE; i++) + { + group = &g_cfggroup[i]; + + /* The "canned" descriptors all have full speed endpoint maxpacket + * sizes. If high speed is selected, we will have to change the + * endpoint maxpacket size. + * + * Is there a alternative high speed maxpacket size in the table? + * If so, that is sufficient proof that the descriptor that we + * just copied is an endpoint descriptor and needs the fixup + */ + +#ifdef CONFIG_USBDEV_DUALSPEED + if (highspeed && group->hsepsize != 0) + { + cdcser_mkepdesc(group->desc, group->hsepsize, + (FAR struct usb_epdesc_s*)dest); + } + else +#endif + /* Copy the "canned" descriptor with the full speed max packet + * size + */ + + { + memcpy(dest, group->desc, group->descsize); + } + + /* Advance to the destination location for the next descriptor */ + + dest += group->descsize; + } + + return SIZEOF_CDCSER_CFGDESC; +} + +/**************************************************************************** + * Name: cdcser_getqualdesc + * + * Description: + * Return a pointer to the raw qual descriptor + * + ****************************************************************************/ + +#if !defined(CONFIG_CDCSER_COMPOSITE) && defined(CONFIG_USBDEV_DUALSPEED) +FAR const struct usb_qualdesc_s *cdcser_getqualdesc(void) +{ + return &g_qualdesc; +} +#endif diff --git a/nuttx/drivers/usbdev/composite.c b/nuttx/drivers/usbdev/composite.c new file mode 100644 index 000000000..4c9582355 --- /dev/null +++ b/nuttx/drivers/usbdev/composite.c @@ -0,0 +1,292 @@ +/**************************************************************************** + * drivers/usbdev/composite.c + * + * Copyright (C) 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 + * 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 + +#include +#include +#include +#include + +#include + +#include "composite.h" + +#ifdef CONFIG_USBDEV_COMPOSITE + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + + /**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: composite_setup + * + * Description: + * Invoked for ep0 control requests. This function probably executes + * in the context of an interrupt handler. + * + ****************************************************************************/ + + #if 0 +static int composite_setup(FAR struct usbdev_s *dev, + FAR const struct usb_ctrlreq_s *ctrl) +{ + FAR struct composite_dev_s *priv; + FAR struct usbdev_req_s *ctrlreq; + uint16_t value; + uint16_t index; + uint16_t len; + int ret = -EOPNOTSUPP; + +#ifdef CONFIG_DEBUG + if (!dev || !dev->ep0 || !ctrl) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_SETUPINVALIDARGS), 0); + return -EIO; + } +#endif + + /* Extract reference to private data */ + + usbtrace(TRACE_CLASSSETUP, ctrl->req); + priv = (FAR struct composite_dev_s *)dev->ep0->priv; + +#ifdef CONFIG_DEBUG + if (!priv || !priv->ctrlreq) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_EP0NOTBOUND2), 0); + return -ENODEV; + } +#endif + ctrlreq = priv->ctrlreq; + + /* Extract the little-endian 16-bit values to host order */ + + value = GETUINT16(ctrl->value); + index = GETUINT16(ctrl->index); + len = GETUINT16(ctrl->len); + + uvdbg("type=%02x req=%02x value=%04x index=%04x len=%04x\n", + ctrl->type, ctrl->req, value, index, len); + + if ((ctrl->type & USB_REQ_TYPE_MASK) == USB_REQ_TYPE_STANDARD) + { + /********************************************************************** + * Standard Requests + **********************************************************************/ + + switch (ctrl->req) + { + case USB_REQ_GETDESCRIPTOR: + { + /* The value field specifies the descriptor type in the MS byte and the + * descriptor index in the LS byte (order is little endian) + */ + + switch (ctrl->value[1]) + { + case USB_DESC_TYPE_DEVICE: + { + ret = USB_SIZEOF_DEVDESC; + memcpy(ctrlreq->buf, composite_getdevdesc(), ret); + } + break; + +#ifdef CONFIG_USBDEV_DUALSPEED + case USB_DESC_TYPE_DEVICEQUALIFIER: + { + ret = USB_SIZEOF_QUALDESC; + memcpy(ctrlreq->buf, composite_getqualdesc(), ret); + } + break; + + case USB_DESC_TYPE_OTHERSPEEDCONFIG: +#endif /* CONFIG_USBDEV_DUALSPEED */ + + case USB_DESC_TYPE_CONFIG: + { +#ifdef CONFIG_USBDEV_DUALSPEED + ret = composite_mkcfgdesc(ctrlreq->buf, dev->speed, ctrl->value[1]); +#else + ret = composite_mkcfgdesc(ctrlreq->buf); +#endif + } + break; + + case USB_DESC_TYPE_STRING: + { + /* index == language code. */ + + ret = composite_mkstrdesc(ctrl->value[0], (struct usb_strdesc_s *)ctrlreq->buf); + } + break; + + default: + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_GETUNKNOWNDESC), value); + } + break; + } + } + break; + + case USB_REQ_SETCONFIGURATION: + { + if (ctrl->type == 0) + { + /* Signal the worker thread to instantiate the new configuration */ + + priv->theventset |= USBSTRG_EVENT_CFGCHANGE; + priv->thvalue = value; + pthread_cond_signal(&priv->cond); + + /* Return here... the response will be provided later by the + * worker thread. + */ + + return OK; + } + } + break; + + case USB_REQ_GETCONFIGURATION: + { + if (ctrl->type == USB_DIR_IN) + { + ctrlreq->buf[0] = priv->config; + ret = 1; + } + } + break; + + case USB_REQ_SETINTERFACE: + { + if (ctrl->type == USB_REQ_RECIPIENT_INTERFACE) + { + if (priv->config == USBSTRG_CONFIGID && + index == USBSTRG_INTERFACEID && + value == USBSTRG_ALTINTERFACEID) + { + /* Signal to instantiate the interface change */ + + priv->theventset |= USBSTRG_EVENT_IFCHANGE; + pthread_cond_signal(&priv->cond); + + /* Return here... the response will be provided later by the + * worker thread. + */ + + return OK; + } + } + } + break; + + case USB_REQ_GETINTERFACE: + { + if (ctrl->type == (USB_DIR_IN|USB_REQ_RECIPIENT_INTERFACE) && + priv->config == USBSTRG_CONFIGIDNONE) + { + if (index != USBSTRG_INTERFACEID) + { + ret = -EDOM; + } + else + { + ctrlreq->buf[0] = USBSTRG_ALTINTERFACEID; + ret = 1; + } + } + } + break; + + default: + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_UNSUPPORTEDSTDREQ), ctrl->req); + break; + } + + /* Respond to the setup command if data was returned. On an error return + * value (ret < 0), the USB driver will stall EP0. + */ + + if (ret >= 0) + { + ctrlreq->len = min(len, ret); + ctrlreq->flags = USBDEV_REQFLAGS_NULLPKT; + ret = EP_SUBMIT(dev->ep0, ctrlreq); + if (ret < 0) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_EPRESPQ), (uint16_t)-ret); + ctrlreq->result = OK; + composite_ep0incomplete(dev->ep0, ctrlreq); + } + } + else + { + /********************************************************************** + * Non-Standard Class Requests + **********************************************************************/ +#warning "Missing Logic" + } + + return ret; +} +#endif + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +#endif /* CONFIG_USBDEV_COMPOSITE */ diff --git a/nuttx/drivers/usbdev/composite.h b/nuttx/drivers/usbdev/composite.h new file mode 100644 index 000000000..ed3b48eac --- /dev/null +++ b/nuttx/drivers/usbdev/composite.h @@ -0,0 +1,176 @@ +/**************************************************************************** + * drivers/usbdev/composite.h + * + * Copyright (C) 2011-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 + * 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. + * + ****************************************************************************/ + +#ifndef __DRIVERS_USBDEV_COMPOSITE_H +#define __DRIVERS_USBDEV_COMPOSITE_H 1 + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include + +#include +#include + +#ifdef CONFIG_USBDEV_COMPOSITE + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ +/* Configuration ************************************************************/ + +#ifndef CONFIG_USBDEV_TRACE_NRECORDS +# define CONFIG_USBDEV_TRACE_NRECORDS 128 +#endif + +#ifndef CONFIG_USBDEV_TRACE_INITIALIDSET +# define CONFIG_USBDEV_TRACE_INITIALIDSET 0 +#endif + +/* Packet sizes */ + +#ifndef CONFIG_COMPOSITE_EP0MAXPACKET +# define CONFIG_COMPOSITE_EP0MAXPACKET 64 +#endif + +/* Vendor and product IDs and strings */ + +#ifndef CONFIG_COMPOSITE_COMPOSITE +# ifndef CONFIG_COMPOSITE_VENDORID +# warning "CONFIG_COMPOSITE_VENDORID not defined" +# define CONFIG_COMPOSITE_VENDORID 0x584e +# endif + +# ifndef CONFIG_COMPOSITE_PRODUCTID +# warning "CONFIG_COMPOSITE_PRODUCTID not defined" +# define CONFIG_COMPOSITE_PRODUCTID 0x5342 +# endif + +# ifndef CONFIG_COMPOSITE_VERSIONNO +# define CONFIG_COMPOSITE_VERSIONNO (0x0399) +# endif + +# ifndef CONFIG_COMPOSITE_VENDORSTR +# warning "No Vendor string specified" +# define CONFIG_COMPOSITE_VENDORSTR "NuttX" +# endif + +# ifndef CONFIG_COMPOSITE_PRODUCTSTR +# warning "No Product string specified" +# define CONFIG_COMPOSITE_PRODUCTSTR "Composite Device" +# endif + +# undef CONFIG_COMPOSITE_SERIALSTR +# define CONFIG_COMPOSITE_SERIALSTR "0101" +#endif + +#undef CONFIG_COMPOSITE_CONFIGSTR +#define CONFIG_COMPOSITE_CONFIGSTR "Composite" + +/* Descriptors **************************************************************/ + +/* Descriptor strings */ + +#define COMPOSITE_MANUFACTURERSTRID (1) +#define COMPOSITE_PRODUCTSTRID (2) +#define COMPOSITE_SERIALSTRID (3) +#define COMPOSITE_CONFIGSTRID (4) + +/**************************************************************************** + * Public Types + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Name: usbstrg_mkstrdesc + * + * Description: + * Construct a string descriptor + * + ****************************************************************************/ + +int composite_mkstrdesc(uint8_t id, struct usb_strdesc_s *strdesc); + +/**************************************************************************** + * Name: composite_getepdesc + * + * Description: + * Return a pointer to the composite device descriptor + * + ****************************************************************************/ + +#ifndef CONFIG_COMPOSITE_COMPOSITE +FAR const struct usb_devdesc_s *composite_getdevdesc(void); +#endif + +/**************************************************************************** + * Name: composite_mkcfgdesc + * + * Description: + * Construct the composite configuration descriptor + * + ****************************************************************************/ + +#ifdef CONFIG_USBDEV_DUALSPEED +int16_t composite_mkcfgdesc(uint8_t *buf, uint8_t speed, uint8_t type); +#else +int16_t composite_mkcfgdesc(uint8_t *buf); +#endif + +/**************************************************************************** + * Name: composite_getqualdesc + * + * Description: + * Return a pointer to the composite qual descriptor + * + ****************************************************************************/ + +#ifdef CONFIG_USBDEV_DUALSPEED +FAR const struct usb_qualdesc_s *composite_getqualdesc(void); +#endif + +#endif /* CONFIG_USBDEV_COMPOSITE */ +#endif /* __DRIVERS_USBDEV_COMPOSITE_H */ diff --git a/nuttx/drivers/usbdev/composite_descriptors.c b/nuttx/drivers/usbdev/composite_descriptors.c new file mode 100644 index 000000000..5feed8e21 --- /dev/null +++ b/nuttx/drivers/usbdev/composite_descriptors.c @@ -0,0 +1,366 @@ +/**************************************************************************** + * drivers/usbdev/composite_descriptors.c + * + * Copyright (C) 2011-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 + * 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 + +#include + +#include +#include +#include +#include + +#include + +#include "composite.h" + +#ifdef CONFIG_CDCSER_COMPOSITE +# include "cdcacm.h" +#endif + +#ifdef CONFIG_USBSTRG_COMPOSITE +# include "msc.h" +#endif + +#ifdef CONFIG_USBDEV_COMPOSITE + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#undef DEV1_IS_CDCSERIAL +#undef DEV1_IS_USBSTRG + +#undef DEV2_IS_CDCSERIAL +#undef DEV2_IS_USBSTRG + +/* Pick the first device in the composite. At present, this may only be + * the CDC serial device or the mass storage device. + */ + +#if defined(CONFIG_CDCSER_COMPOSITE) +# define DEV1_IS_CDCSERIAL 1 +# define DEV1_MKCFGDESC cdcser_mkcfgdesc +# define DEV1_NCONFIGS CDCSER_NCONFIGS +# define DEV1_CONFIGID CDCSER_CONFIGID +# define DEV1_NINTERFACES CDCSER_NINTERFACES +# define DEV1_FIRSTSTRID CONFIG_CDCSER_STRBASE +# define DEV1_NSTRIDS (CDCSER_LASTSTRID-CONFIG_CDCSER_STRBASE) +# define DEV1_CFGDESCSIZE SIZEOF_CDCSER_CFGDESC +#elif defined(CONFIG_CDCSER_COMPOSITE) +# define DEV1_IS_USBSTRG 1 +# define DEV1_MKCFGDESC composite_mkcfgdesc +# define DEV1_NCONFIGS USBSTRG_NCONFIGS +# define DEV1_CONFIGID USBSTRG_CONFIGID +# define DEV1_NINTERFACES USBSTRG_NINTERFACES +# define DEV1_FIRSTSTRID CONFIG_CDCSER_STRBASE +# define DEV1_NSTRIDS (USBSTRG_LASTSTRID-CONFIG_USBSTRG_STRBASE) +# define DEV1_CFGDESCSIZE SIZEOF_USBSTRG_CFGDESC +#else +# error "No members of the composite defined" +#endif + +/* Pick the second device in the composite. At present, this may only be + * the CDC serial device or the mass storage device. + */ + +#if defined(CONFIG_CDCSER_COMPOSITE) && !defined(DEV1_IS_CDCSERIAL) +# define DEV2_IS_CDCSERIAL 1 +# define DEV2_MKCFGDESC cdcser_mkcfgdesc +# define DEV2_NCONFIGS CDCSER_NCONFIGS +# define DEV2_CONFIGID CDCSER_CONFIGID +# define DEV2_NINTERFACES CDCSER_NINTERFACES +# define DEV2_FIRSTSTRID CONFIG_CDCSER_STRBASE +# define DEV2_NSTRIDS (CDCSER_LASTSTRID-CONFIG_CDCSER_STRBASE) +# define DEV2_CFGDESCSIZE SIZEOF_CDCSER_CFGDESC +#elif defined(CONFIG_CDCSER_COMPOSITE) && !defined(DEV1_IS_USBSTRG) +# define DEV2_IS_USBSTRG 1 +# define DEV2_MKCFGDESC composite_mkcfgdesc +# define DEV2_NCONFIGS USBSTRG_NCONFIGS +# define DEV2_CONFIGID USBSTRG_CONFIGID +# define DEV2_NINTERFACES USBSTRG_NINTERFACES +# define DEV2_FIRSTSTRID CONFIG_CDCSER_STRBASE +# define DEV2_NSTRIDS (USBSTRG_LASTSTRID-CONFIG_USBSTRG_STRBASE) +# define DEV2_CFGDESCSIZE SIZEOF_USBSTRG_CFGDESC +#else +# error "Insufficient members of the composite defined" +#endif + +/* Total size of the configuration descriptor: */ + +#define COMPOSITE_CFGDESCSIZE (USB_SIZEOF_CFGDESC + DEV1_CFGDESCSIZE + DEV2_CFGDESCSIZE) + +/* The total number of interfaces */ + +#define COMPOSITE_NINTERFACES (DEV1_NINTERFACES + DEV2_NINTERFACES) + +/* Composite configuration ID value */ + +#if DEV1_NCONFIGS != 1 || DEV1_CONFIGID != 1 +# error "DEV1: Only a single configuration is supported" +#endif + +#if DEV2_NCONFIGS != 1 || DEV2_CONFIGID != 1 +# error "DEV2: Only a single configuration is supported" +#endif + +#define COMPOSITE_NCONFIGS 1 +#define COMPOSITE_CONFIGID 1 + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +#ifdef CONFIG_USBDEV_DUALSPEED +typedef int16_t (*mkcfgdesc)(FAR uint8_t *buf, uint8_t speed, uint8_t type); +#else +typedef int16_t (*mkcfgdesc)(FAR uint8_t *buf); +#endif + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ +/* Device Descriptor */ + +static const struct usb_devdesc_s g_devdesc = +{ + USB_SIZEOF_DEVDESC, /* len */ + USB_DESC_TYPE_DEVICE, /* type */ + { /* usb */ + LSBYTE(0x0200), + MSBYTE(0x0200) + }, + USB_CLASS_PER_INTERFACE, /* class */ + 0, /* subclass */ + 0, /* protocol */ + CONFIG_COMPOSITE_EP0MAXPACKET, /* maxpacketsize */ + { + LSBYTE(CONFIG_COMPOSITE_VENDORID), /* vendor */ + MSBYTE(CONFIG_COMPOSITE_VENDORID) + }, + { + LSBYTE(CONFIG_COMPOSITE_PRODUCTID), /* product */ + MSBYTE(CONFIG_COMPOSITE_PRODUCTID) + }, + { + LSBYTE(CONFIG_COMPOSITE_VERSIONNO), /* device */ + MSBYTE(CONFIG_COMPOSITE_VERSIONNO) + }, + COMPOSITE_MANUFACTURERSTRID, /* imfgr */ + COMPOSITE_PRODUCTSTRID, /* iproduct */ + COMPOSITE_SERIALSTRID, /* serno */ + COMPOSITE_NCONFIGS /* nconfigs */ +}; + +/* Configuration descriptor for the composite device */ + +static const struct usb_cfgdesc_s g_cfgdesc = +{ + USB_SIZEOF_CFGDESC, /* len */ + USB_DESC_TYPE_CONFIG, /* type */ + { + LSBYTE(COMPOSITE_CFGDESCSIZE), /* LS totallen */ + MSBYTE(COMPOSITE_CFGDESCSIZE) /* MS totallen */ + }, + COMPOSITE_NINTERFACES, /* ninterfaces */ + COMPOSITE_CONFIGID, /* cfgvalue */ + COMPOSITE_CONFIGSTRID, /* icfg */ + USB_CONFIG_ATTR_ONE|SELFPOWERED|REMOTEWAKEUP, /* attr */ + (CONFIG_USBDEV_MAXPOWER + 1) / 2 /* mxpower */ +}; + +#ifdef CONFIG_USBDEV_DUALSPEED +static const struct usb_qualdesc_s g_qualdesc = +{ + USB_SIZEOF_QUALDESC, /* len */ + USB_DESC_TYPE_DEVICEQUALIFIER, /* type */ + { /* usb */ + LSBYTE(0x0200), + MSBYTE(0x0200) + }, + USB_CLASS_VENDOR_SPEC, /* class */ + 0, /* subclass */ + 0, /* protocol */ + CONFIG_COMPOSITE_EP0MAXPACKET, /* mxpacketsize */ + COMPOSITE_NCONFIGS, /* nconfigs */ + 0, /* reserved */ +}; +#endif + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: usbstrg_mkstrdesc + * + * Description: + * Construct a string descriptor + * + ****************************************************************************/ + +int composite_mkstrdesc(uint8_t id, struct usb_strdesc_s *strdesc) +{ + const char *str; + int len; + int ndata; + int i; + + switch (id) + { + case 0: + { + /* Descriptor 0 is the language id */ + + strdesc->len = 4; + strdesc->type = USB_DESC_TYPE_STRING; + strdesc->data[0] = LSBYTE(USBSTRG_STR_LANGUAGE); + strdesc->data[1] = MSBYTE(USBSTRG_STR_LANGUAGE); + return 4; + } + + case COMPOSITE_MANUFACTURERSTRID: + str = g_vendorstr; + break; + + case COMPOSITE_PRODUCTSTRID: + str = g_productstr; + break; + + case COMPOSITE_SERIALSTRID: + str = g_serialstr; + break; + + case COMPOSITE_CONFIGSTRID: + str = CONFIG_USBSTRG_CONFIGSTR; + break; + + default: + return -EINVAL; + } + + /* The string is utf16-le. The poor man's utf-8 to utf16-le + * conversion below will only handle 7-bit en-us ascii + */ + + len = strlen(str); + for (i = 0, ndata = 0; i < len; i++, ndata += 2) + { + strdesc->data[ndata] = str[i]; + strdesc->data[ndata+1] = 0; + } + + strdesc->len = ndata+2; + strdesc->type = USB_DESC_TYPE_STRING; + return strdesc->len; +} + +/**************************************************************************** + * Name: composite_getepdesc + * + * Description: + * Return a pointer to the raw device descriptor + * + ****************************************************************************/ + +#ifndef CONFIG_USBSTRG_COMPOSITE +FAR const struct usb_devdesc_s *composite_getdevdesc(void) +{ + return &g_devdesc; +} +#endif + +/**************************************************************************** + * Name: composite_mkcfgdesc + * + * Description: + * Construct the configuration descriptor + * + ****************************************************************************/ + +#ifdef CONFIG_USBDEV_DUALSPEED +int16_t composite_mkcfgdesc(uint8_t *buf, uint8_t speed, uint8_t type) +#else +int16_t composite_mkcfgdesc(uint8_t *buf) +#endif +{ + FAR struct usb_cfgdesc_s *cfgdesc = (struct usb_cfgdesc_s*)buf; + + /* Configuration descriptor -- Copy the canned configuration descriptor. */ + + memcpy(cfgdesc, &g_cfgdesc, USB_SIZEOF_CFGDESC); + buf += USB_SIZEOF_CFGDESC; + + /* Copy DEV1/DEV2 configuration descriptors */ + +#ifdef CONFIG_USBDEV_DUALSPEED + buf += DEV1_MKCFGDESC(buf, speed, type); + buf += DEV2_MKCFGDESC(buf, speed, type); +#else + buf += DEV1_MKCFGDESC(buf); + buf += DEV2_MKCFGDESC(buf); +#endif + + return COMPOSITE_CFGDESCSIZE; +} + +/**************************************************************************** + * Name: composite_getqualdesc + * + * Description: + * Return a pointer to the raw qual descriptor + * + ****************************************************************************/ + +#ifdef CONFIG_USBDEV_DUALSPEED +FAR const struct usb_qualdesc_s *composite_getqualdesc(void) +{ + return &g_qualdesc; +} +#endif + +#endif /* CONFIG_USBDEV_COMPOSITE */ diff --git a/nuttx/drivers/usbdev/msc.c b/nuttx/drivers/usbdev/msc.c new file mode 100644 index 000000000..a29153b64 --- /dev/null +++ b/nuttx/drivers/usbdev/msc.c @@ -0,0 +1,1660 @@ +/**************************************************************************** + * drivers/usbdev/msc.c + * + * Copyright (C) 2008-2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Mass storage class device. Bulk-only with SCSI subclass. + * + * References: + * "Universal Serial Bus Mass Storage Class, Specification Overview," + * Revision 1.2, USB Implementer's Forum, June 23, 2003. + * + * "Universal Serial Bus Mass Storage Class, Bulk-Only Transport," + * Revision 1.0, USB Implementer's Forum, September 31, 1999. + * + * "SCSI Primary Commands - 3 (SPC-3)," American National Standard + * for Information Technology, May 4, 2005 + * + * "SCSI Primary Commands - 4 (SPC-4)," American National Standard + * for Information Technology, July 19, 2008 + * + * "SCSI Block Commands -2 (SBC-2)," American National Standard + * for Information Technology, November 13, 2004 + * + * "SCSI Multimedia Commands - 3 (MMC-3)," American National Standard + * for Information Technology, November 12, 2001 + * + * 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 + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "msc.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/* The internal version of the class driver */ + +struct usbstrg_driver_s +{ + struct usbdevclass_driver_s drvr; + FAR struct usbstrg_dev_s *dev; +}; + +/* This is what is allocated */ + +struct usbstrg_alloc_s +{ + struct usbstrg_dev_s dev; + struct usbstrg_driver_s drvr; +}; + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/* Class Driver Support *****************************************************/ + +static void usbstrg_ep0incomplete(FAR struct usbdev_ep_s *ep, + FAR struct usbdev_req_s *req); +static struct usbdev_req_s *usbstrg_allocreq(FAR struct usbdev_ep_s *ep, + uint16_t len); +static void usbstrg_freereq(FAR struct usbdev_ep_s *ep, + FAR struct usbdev_req_s *req); + +/* Class Driver Operations (most at interrupt level) ************************/ + +static int usbstrg_bind(FAR struct usbdev_s *dev, + FAR struct usbdevclass_driver_s *driver); +static void usbstrg_unbind(FAR struct usbdev_s *dev); +static int usbstrg_setup(FAR struct usbdev_s *dev, + FAR const struct usb_ctrlreq_s *ctrl); +static void usbstrg_disconnect(FAR struct usbdev_s *dev); + +/* Initialization/Uninitialization ******************************************/ + +static void usbstrg_lununinitialize(struct usbstrg_lun_s *lun); + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/* Driver operations ********************************************************/ + +static struct usbdevclass_driverops_s g_driverops = +{ + usbstrg_bind, /* bind */ + usbstrg_unbind, /* unbind */ + usbstrg_setup, /* setup */ + usbstrg_disconnect, /* disconnect */ + NULL, /* suspend */ + NULL /* resume */ +}; + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Class Driver Support + ****************************************************************************/ +/**************************************************************************** + * Name: usbstrg_ep0incomplete + * + * Description: + * Handle completion of EP0 control operations + * + ****************************************************************************/ + +static void usbstrg_ep0incomplete(FAR struct usbdev_ep_s *ep, + FAR struct usbdev_req_s *req) +{ + if (req->result || req->xfrd != req->len) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_REQRESULT), + (uint16_t)-req->result); + } +} + +/**************************************************************************** + * Name: usbstrg_allocreq + * + * Description: + * Allocate a request instance along with its buffer + * + ****************************************************************************/ + +static struct usbdev_req_s *usbstrg_allocreq(FAR struct usbdev_ep_s *ep, + uint16_t len) +{ + FAR struct usbdev_req_s *req; + + req = EP_ALLOCREQ(ep); + if (req != NULL) + { + req->len = len; + req->buf = EP_ALLOCBUFFER(ep, len); + if (!req->buf) + { + EP_FREEREQ(ep, req); + req = NULL; + } + } + return req; +} + +/**************************************************************************** + * Name: usbstrg_freereq + * + * Description: + * Free a request instance along with its buffer + * + ****************************************************************************/ + +static void usbstrg_freereq(FAR struct usbdev_ep_s *ep, struct usbdev_req_s *req) +{ + if (ep != NULL && req != NULL) + { + if (req->buf != NULL) + { + EP_FREEBUFFER(ep, req->buf); + } + EP_FREEREQ(ep, req); + } +} + +/**************************************************************************** + * Class Driver Interfaces + ****************************************************************************/ +/**************************************************************************** + * Name: usbstrg_bind + * + * Description: + * Invoked when the driver is bound to a USB device driver + * + ****************************************************************************/ + +static int usbstrg_bind(FAR struct usbdev_s *dev, FAR struct usbdevclass_driver_s *driver) +{ + FAR struct usbstrg_dev_s *priv = ((struct usbstrg_driver_s*)driver)->dev; + FAR struct usbstrg_req_s *reqcontainer; + irqstate_t flags; + int ret = OK; + int i; + + usbtrace(TRACE_CLASSBIND, 0); + + /* Bind the structures */ + + priv->usbdev = dev; + dev->ep0->priv = priv; + + /* The configured EP0 size should match the reported EP0 size. We could + * easily adapt to the reported EP0 size, but then we could not use the + * const, canned descriptors. + */ + + DEBUGASSERT(CONFIG_USBSTRG_EP0MAXPACKET == dev->ep0->maxpacket); + + /* Preallocate control request */ + + priv->ctrlreq = usbstrg_allocreq(dev->ep0, USBSTRG_MXDESCLEN); + if (priv->ctrlreq == NULL) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_ALLOCCTRLREQ), 0); + ret = -ENOMEM; + goto errout; + } + priv->ctrlreq->callback = usbstrg_ep0incomplete; + + /* Pre-allocate all endpoints... the endpoints will not be functional + * until the SET CONFIGURATION request is processed in usbstrg_setconfig. + * This is done here because there may be calls to kmalloc and the SET + * CONFIGURATION processing probably occurrs within interrupt handling + * logic where kmalloc calls will fail. + */ + + /* Pre-allocate the IN bulk endpoint */ + + priv->epbulkin = DEV_ALLOCEP(dev, USBSTRG_EPINBULK_ADDR, true, USB_EP_ATTR_XFER_BULK); + if (!priv->epbulkin) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_EPBULKINALLOCFAIL), 0); + ret = -ENODEV; + goto errout; + } + priv->epbulkin->priv = priv; + + /* Pre-allocate the OUT bulk endpoint */ + + priv->epbulkout = DEV_ALLOCEP(dev, USBSTRG_EPOUTBULK_ADDR, false, USB_EP_ATTR_XFER_BULK); + if (!priv->epbulkout) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_EPBULKOUTALLOCFAIL), 0); + ret = -ENODEV; + goto errout; + } + priv->epbulkout->priv = priv; + + /* Pre-allocate read requests */ + + for (i = 0; i < CONFIG_USBSTRG_NRDREQS; i++) + { + reqcontainer = &priv->rdreqs[i]; + reqcontainer->req = usbstrg_allocreq(priv->epbulkout, CONFIG_USBSTRG_BULKOUTREQLEN); + if (reqcontainer->req == NULL) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_RDALLOCREQ), + (uint16_t)-ret); + ret = -ENOMEM; + goto errout; + } + reqcontainer->req->priv = reqcontainer; + reqcontainer->req->callback = usbstrg_rdcomplete; + } + + /* Pre-allocate write request containers and put in a free list */ + + for (i = 0; i < CONFIG_USBSTRG_NWRREQS; i++) + { + reqcontainer = &priv->wrreqs[i]; + reqcontainer->req = usbstrg_allocreq(priv->epbulkin, CONFIG_USBSTRG_BULKINREQLEN); + if (reqcontainer->req == NULL) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_WRALLOCREQ), + (uint16_t)-ret); + ret = -ENOMEM; + goto errout; + } + reqcontainer->req->priv = reqcontainer; + reqcontainer->req->callback = usbstrg_wrcomplete; + + flags = irqsave(); + sq_addlast((sq_entry_t*)reqcontainer, &priv->wrreqlist); + irqrestore(flags); + } + + /* Report if we are selfpowered */ + +#ifdef CONFIG_USBDEV_SELFPOWERED + DEV_SETSELFPOWERED(dev); +#endif + + /* And pull-up the data line for the soft connect function */ + + DEV_CONNECT(dev); + return OK; + +errout: + usbstrg_unbind(dev); + return ret; +} + +/**************************************************************************** + * Name: usbstrg_unbind + * + * Description: + * Invoked when the driver is unbound from a USB device driver + * + ****************************************************************************/ + +static void usbstrg_unbind(FAR struct usbdev_s *dev) +{ + FAR struct usbstrg_dev_s *priv; + FAR struct usbstrg_req_s *reqcontainer; + irqstate_t flags; + int i; + + usbtrace(TRACE_CLASSUNBIND, 0); + +#ifdef CONFIG_DEBUG + if (!dev || !dev->ep0) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_UNBINDINVALIDARGS), 0); + return; + } +#endif + + /* Extract reference to private data */ + + priv = (FAR struct usbstrg_dev_s *)dev->ep0->priv; + +#ifdef CONFIG_DEBUG + if (!priv) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_EP0NOTBOUND1), 0); + return; + } +#endif + + /* The worker thread should have already been stopped by the + * driver un-initialize logic. + */ + + DEBUGASSERT(priv->thstate == USBSTRG_STATE_TERMINATED); + + /* Make sure that we are not already unbound */ + + if (priv != NULL) + { + /* Make sure that the endpoints have been unconfigured. If + * we were terminated gracefully, then the configuration should + * already have been reset. If not, then calling usbstrg_resetconfig + * should cause the endpoints to immediately terminate all + * transfers and return the requests to us (with result == -ESHUTDOWN) + */ + + usbstrg_resetconfig(priv); + up_mdelay(50); + + /* Free the pre-allocated control request */ + + if (priv->ctrlreq != NULL) + { + usbstrg_freereq(dev->ep0, priv->ctrlreq); + priv->ctrlreq = NULL; + } + + /* Free pre-allocated read requests (which should all have + * been returned to the free list at this time -- we don't check) + */ + + for (i = 0; i < CONFIG_USBSTRG_NRDREQS; i++) + { + reqcontainer = &priv->rdreqs[i]; + if (reqcontainer->req) + { + usbstrg_freereq(priv->epbulkout, reqcontainer->req); + reqcontainer->req = NULL; + } + } + + /* Free the bulk OUT endpoint */ + + if (priv->epbulkout) + { + DEV_FREEEP(dev, priv->epbulkout); + priv->epbulkout = NULL; + } + + /* Free write requests that are not in use (which should be all + * of them + */ + + flags = irqsave(); + while (!sq_empty(&priv->wrreqlist)) + { + reqcontainer = (struct usbstrg_req_s *)sq_remfirst(&priv->wrreqlist); + if (reqcontainer->req != NULL) + { + usbstrg_freereq(priv->epbulkin, reqcontainer->req); + } + } + + /* Free the bulk IN endpoint */ + + if (priv->epbulkin) + { + DEV_FREEEP(dev, priv->epbulkin); + priv->epbulkin = NULL; + } + + irqrestore(flags); + } +} + +/**************************************************************************** + * Name: usbstrg_setup + * + * Description: + * Invoked for ep0 control requests. This function probably executes + * in the context of an interrupt handler. + * + ****************************************************************************/ + +static int usbstrg_setup(FAR struct usbdev_s *dev, + FAR const struct usb_ctrlreq_s *ctrl) +{ + FAR struct usbstrg_dev_s *priv; + FAR struct usbdev_req_s *ctrlreq; + uint16_t value; + uint16_t index; + uint16_t len; + int ret = -EOPNOTSUPP; + +#ifdef CONFIG_DEBUG + if (!dev || !dev->ep0 || !ctrl) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_SETUPINVALIDARGS), 0); + return -EIO; + } +#endif + + /* Extract reference to private data */ + + usbtrace(TRACE_CLASSSETUP, ctrl->req); + priv = (FAR struct usbstrg_dev_s *)dev->ep0->priv; + +#ifdef CONFIG_DEBUG + if (!priv || !priv->ctrlreq) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_EP0NOTBOUND2), 0); + return -ENODEV; + } +#endif + ctrlreq = priv->ctrlreq; + + /* Extract the little-endian 16-bit values to host order */ + + value = GETUINT16(ctrl->value); + index = GETUINT16(ctrl->index); + len = GETUINT16(ctrl->len); + + uvdbg("type=%02x req=%02x value=%04x index=%04x len=%04x\n", + ctrl->type, ctrl->req, value, index, len); + + if ((ctrl->type & USB_REQ_TYPE_MASK) == USB_REQ_TYPE_STANDARD) + { + /********************************************************************** + * Standard Requests + **********************************************************************/ + + /* If the mass storage device is used in as part of a composite device, + * then the configuration descriptor is is handled by logic in the + * composite device logic. + */ + +#ifdef CONFIG_USBSTRG_COMPOSITE + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_UNSUPPORTEDSTDREQ), ctrl->req); +#else + switch (ctrl->req) + { + case USB_REQ_GETDESCRIPTOR: + { + /* The value field specifies the descriptor type in the MS byte and the + * descriptor index in the LS byte (order is little endian) + */ + + switch (ctrl->value[1]) + { + case USB_DESC_TYPE_DEVICE: + { + ret = USB_SIZEOF_DEVDESC; + memcpy(ctrlreq->buf, usbstrg_getdevdesc(), ret); + } + break; + +#ifdef CONFIG_USBDEV_DUALSPEED + case USB_DESC_TYPE_DEVICEQUALIFIER: + { + ret = USB_SIZEOF_QUALDESC; + memcpy(ctrlreq->buf, usbstrg_getqualdesc(), ret); + } + break; + + case USB_DESC_TYPE_OTHERSPEEDCONFIG: +#endif /* CONFIG_USBDEV_DUALSPEED */ + + case USB_DESC_TYPE_CONFIG: + { +#ifdef CONFIG_USBDEV_DUALSPEED + ret = usbstrg_mkcfgdesc(ctrlreq->buf, dev->speed, ctrl->value[1]); +#else + ret = usbstrg_mkcfgdesc(ctrlreq->buf); +#endif + } + break; + + case USB_DESC_TYPE_STRING: + { + /* index == language code. */ + + ret = usbstrg_mkstrdesc(ctrl->value[0], (struct usb_strdesc_s *)ctrlreq->buf); + } + break; + + default: + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_GETUNKNOWNDESC), value); + } + break; + } + } + break; + + case USB_REQ_SETCONFIGURATION: + { + if (ctrl->type == 0) + { + /* Signal the worker thread to instantiate the new configuration */ + + priv->theventset |= USBSTRG_EVENT_CFGCHANGE; + priv->thvalue = value; + pthread_cond_signal(&priv->cond); + + /* Return here... the response will be provided later by the + * worker thread. + */ + + return OK; + } + } + break; + + case USB_REQ_GETCONFIGURATION: + { + if (ctrl->type == USB_DIR_IN) + { + ctrlreq->buf[0] = priv->config; + ret = 1; + } + } + break; + + case USB_REQ_SETINTERFACE: + { + if (ctrl->type == USB_REQ_RECIPIENT_INTERFACE) + { + if (priv->config == USBSTRG_CONFIGID && + index == USBSTRG_INTERFACEID && + value == USBSTRG_ALTINTERFACEID) + { + /* Signal to instantiate the interface change */ + + priv->theventset |= USBSTRG_EVENT_IFCHANGE; + pthread_cond_signal(&priv->cond); + + /* Return here... the response will be provided later by the + * worker thread. + */ + + return OK; + } + } + } + break; + + case USB_REQ_GETINTERFACE: + { + if (ctrl->type == (USB_DIR_IN|USB_REQ_RECIPIENT_INTERFACE) && + priv->config == USBSTRG_CONFIGIDNONE) + { + if (index != USBSTRG_INTERFACEID) + { + ret = -EDOM; + } + else + { + ctrlreq->buf[0] = USBSTRG_ALTINTERFACEID; + ret = 1; + } + } + } + break; + + default: + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_UNSUPPORTEDSTDREQ), ctrl->req); + break; + } +#endif + } + else + { + /********************************************************************** + * Bulk-Only Mass Storage Class Requests + **********************************************************************/ + + /* Verify that we are configured */ + + if (!priv->config) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_NOTCONFIGURED), 0); + return ret; + } + + switch (ctrl->req) + { + case USBSTRG_REQ_MSRESET: /* Reset mass storage device and interface */ + { + if (ctrl->type == USBSTRG_TYPE_SETUPOUT && value == 0 && len == 0) + { + /* Only one interface is supported */ + + if (index != USBSTRG_CONFIGID) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_MSRESETNDX), index); + ret = -EDOM; + } + else + { + /* Signal to stop the current operation and reinitialize state */ + + priv->theventset |= USBSTRG_EVENT_RESET; + pthread_cond_signal(&priv->cond); + + /* Return here... the response will be provided later by the + * worker thread. + */ + + return OK; + } + } + } + break; + + case USBSTRG_REQ_GETMAXLUN: /* Return number LUNs supported */ + { + if (ctrl->type == USBSTRG_TYPE_SETUPIN && value == 0) + { + /* Only one interface is supported */ + + if (index != USBSTRG_INTERFACEID) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_GETMAXLUNNDX), index); + ret = -EDOM; + } + else + { + ctrlreq->buf[0] = priv->nluns - 1; + ret = 1; + } + } + } + break; + + default: + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_BADREQUEST), index); + break; + } + } + + /* Respond to the setup command if data was returned. On an error return + * value (ret < 0), the USB driver will stall EP0. + */ + + if (ret >= 0) + { + ctrlreq->len = min(len, ret); + ctrlreq->flags = USBDEV_REQFLAGS_NULLPKT; + ret = EP_SUBMIT(dev->ep0, ctrlreq); + if (ret < 0) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_EPRESPQ), (uint16_t)-ret); +#if 0 /* Not necessary */ + ctrlreq->result = OK; + usbstrg_ep0incomplete(dev->ep0, ctrlreq); +#endif + } + } + + return ret; +} + +/**************************************************************************** + * Name: usbstrg_disconnect + * + * Description: + * Invoked after all transfers have been stopped, when the host is + * disconnected. This function is probably called from the context of an + * interrupt handler. + * + ****************************************************************************/ + +static void usbstrg_disconnect(FAR struct usbdev_s *dev) +{ + struct usbstrg_dev_s *priv; + irqstate_t flags; + + usbtrace(TRACE_CLASSDISCONNECT, 0); + +#ifdef CONFIG_DEBUG + if (!dev || !dev->ep0) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_DISCONNECTINVALIDARGS), 0); + return; + } +#endif + + /* Extract reference to private data */ + + priv = (FAR struct usbstrg_dev_s *)dev->ep0->priv; + +#ifdef CONFIG_DEBUG + if (!priv) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_EP0NOTBOUND3), 0); + return; + } +#endif + + /* Reset the configuration */ + + flags = irqsave(); + usbstrg_resetconfig(priv); + + /* Signal the worker thread */ + + priv->theventset |= USBSTRG_EVENT_DISCONNECT; + pthread_cond_signal(&priv->cond); + irqrestore(flags); + + /* Perform the soft connect function so that we will we can be + * re-enumerated. + */ + + DEV_CONNECT(dev); +} + +/**************************************************************************** + * Initialization/Un-Initialization + ****************************************************************************/ +/**************************************************************************** + * Name: usbstrg_lununinitialize + ****************************************************************************/ + +static void usbstrg_lununinitialize(struct usbstrg_lun_s *lun) +{ + /* Has a block driver has been bound to the LUN? */ + + if (lun->inode) + { + /* Close the block driver */ + + (void)close_blockdriver(lun->inode); + } + + memset(lun, 0, sizeof(struct usbstrg_lun_s *)); +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ +/**************************************************************************** + * Internal Interfaces + ****************************************************************************/ + +/**************************************************************************** + * Name: usbstrg_setconfig + * + * Description: + * Set the device configuration by allocating and configuring endpoints and + * by allocating and queuing read and write requests. + * + ****************************************************************************/ + +int usbstrg_setconfig(FAR struct usbstrg_dev_s *priv, uint8_t config) +{ + FAR struct usbstrg_req_s *privreq; + FAR struct usbdev_req_s *req; +#ifdef CONFIG_USBDEV_DUALSPEED + FAR const struct usb_epdesc_s *epdesc; + bool hispeed = (priv->usbdev->speed == USB_SPEED_HIGH); + uint16_t bulkmxpacket; +#endif + int i; + int ret = 0; + +#if CONFIG_DEBUG + if (priv == NULL) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_SETCONFIGINVALIDARGS), 0); + return -EIO; + } +#endif + + if (config == priv->config) + { + /* Already configured -- Do nothing */ + + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_ALREADYCONFIGURED), 0); + return OK; + } + + /* Discard the previous configuration data */ + + usbstrg_resetconfig(priv); + + /* Was this a request to simply discard the current configuration? */ + + if (config == USBSTRG_CONFIGIDNONE) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_CONFIGNONE), 0); + return OK; + } + + /* We only accept one configuration */ + + if (config != USBSTRG_CONFIGID) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_CONFIGIDBAD), 0); + return -EINVAL; + } + + /* Configure the IN bulk endpoint */ + +#ifdef CONFIG_USBDEV_DUALSPEED + bulkmxpacket = USBSTRG_BULKMAXPACKET(hispeed); + epdesc = USBSTRG_EPBULKINDESC(hispeed); + ret = EP_CONFIGURE(priv->epbulkin, epdesc, false); +#else + ret = EP_CONFIGURE(priv->epbulkin, + usbstrg_getepdesc(USBSTRG_EPFSBULKIN), false); +#endif + if (ret < 0) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_EPBULKINCONFIGFAIL), 0); + goto errout; + } + + priv->epbulkin->priv = priv; + + /* Configure the OUT bulk endpoint */ + +#ifdef CONFIG_USBDEV_DUALSPEED + epdesc = USBSTRG_EPBULKOUTDESC(hispeed); + ret = EP_CONFIGURE(priv->epbulkout, epdesc, true); +#else + ret = EP_CONFIGURE(priv->epbulkout, + usbstrg_getepdesc(USBSTRG_EPFSBULKOUT), true); +#endif + if (ret < 0) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_EPBULKOUTCONFIGFAIL), 0); + goto errout; + } + + priv->epbulkout->priv = priv; + + /* Queue read requests in the bulk OUT endpoint */ + + for (i = 0; i < CONFIG_USBSTRG_NRDREQS; i++) + { + privreq = &priv->rdreqs[i]; + req = privreq->req; + req->len = CONFIG_USBSTRG_BULKOUTREQLEN; + req->priv = privreq; + req->callback = usbstrg_rdcomplete; + ret = EP_SUBMIT(priv->epbulkout, req); + if (ret < 0) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_RDSUBMIT), (uint16_t)-ret); + goto errout; + } + } + + priv->config = config; + return OK; + +errout: + usbstrg_resetconfig(priv); + return ret; +} + +/**************************************************************************** + * Name: usbstrg_resetconfig + * + * Description: + * Mark the device as not configured and disable all endpoints. + * + ****************************************************************************/ + +void usbstrg_resetconfig(FAR struct usbstrg_dev_s *priv) +{ + /* Are we configured? */ + + if (priv->config != USBSTRG_CONFIGIDNONE) + { + /* Yes.. but not anymore */ + + priv->config = USBSTRG_CONFIGIDNONE; + + /* Disable endpoints. This should force completion of all pending + * transfers. + */ + + EP_DISABLE(priv->epbulkin); + EP_DISABLE(priv->epbulkout); + } +} + +/**************************************************************************** + * Name: usbstrg_wrcomplete + * + * Description: + * Handle completion of write request. This function probably executes + * in the context of an interrupt handler. + * + ****************************************************************************/ + +void usbstrg_wrcomplete(FAR struct usbdev_ep_s *ep, FAR struct usbdev_req_s *req) +{ + FAR struct usbstrg_dev_s *priv; + FAR struct usbstrg_req_s *privreq; + irqstate_t flags; + + /* Sanity check */ + +#ifdef CONFIG_DEBUG + if (!ep || !ep->priv || !req || !req->priv) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_WRCOMPLETEINVALIDARGS), 0); + return; + } +#endif + + /* Extract references to private data */ + + priv = (FAR struct usbstrg_dev_s*)ep->priv; + privreq = (FAR struct usbstrg_req_s *)req->priv; + + /* Return the write request to the free list */ + + flags = irqsave(); + sq_addlast((sq_entry_t*)privreq, &priv->wrreqlist); + irqrestore(flags); + + /* Process the received data unless this is some unusual condition */ + + switch (req->result) + { + case OK: /* Normal completion */ + usbtrace(TRACE_CLASSWRCOMPLETE, req->xfrd); + break; + + case -ESHUTDOWN: /* Disconnection */ + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_WRSHUTDOWN), 0); + break; + + default: /* Some other error occurred */ + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_WRUNEXPECTED), + (uint16_t)-req->result); + break; + }; + + /* Inform the worker thread that a write request has been returned */ + + priv->theventset |= USBSTRG_EVENT_WRCOMPLETE; + pthread_cond_signal(&priv->cond); +} + +/**************************************************************************** + * Name: usbstrg_rdcomplete + * + * Description: + * Handle completion of read request on the bulk OUT endpoint. This + * is handled like the receipt of serial data on the "UART" + * + ****************************************************************************/ + +void usbstrg_rdcomplete(FAR struct usbdev_ep_s *ep, FAR struct usbdev_req_s *req) +{ + FAR struct usbstrg_dev_s *priv; + FAR struct usbstrg_req_s *privreq; + irqstate_t flags; + int ret; + + /* Sanity check */ + +#ifdef CONFIG_DEBUG + if (!ep || !ep->priv || !req || !req->priv) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_RDCOMPLETEINVALIDARGS), 0); + return; + } +#endif + + /* Extract references to private data */ + + priv = (FAR struct usbstrg_dev_s*)ep->priv; + privreq = (FAR struct usbstrg_req_s *)req->priv; + + /* Process the received data unless this is some unusual condition */ + + switch (req->result) + { + case 0: /* Normal completion */ + { + usbtrace(TRACE_CLASSRDCOMPLETE, req->xfrd); + + /* Add the filled read request from the rdreqlist */ + + flags = irqsave(); + sq_addlast((sq_entry_t*)privreq, &priv->rdreqlist); + irqrestore(flags); + + /* Signal the worker thread that there is received data to be processed */ + + priv->theventset |= USBSTRG_EVENT_RDCOMPLETE; + pthread_cond_signal(&priv->cond); + } + break; + + case -ESHUTDOWN: /* Disconnection */ + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_RDSHUTDOWN), 0); + + /* Drop the read request... it will be cleaned up later */ + } + break; + + default: /* Some other error occurred */ + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_RDUNEXPECTED), + (uint16_t)-req->result); + + /* Return the read request to the bulk out endpoint for re-filling */ + + req = privreq->req; + req->priv = privreq; + req->callback = usbstrg_rdcomplete; + + ret = EP_SUBMIT(priv->epbulkout, req); + if (ret != OK) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_RDCOMPLETERDSUBMIT), + (uint16_t)-ret); + } + } + break; + } +} + +/**************************************************************************** + * Name: usbstrg_deferredresponse + * + * Description: + * Some EP0 setup request cannot be responded to immediately becuase they + * require some asynchronous action from the SCSI worker thread. This + * function is provided for the SCSI thread to make that deferred response. + * The specific requests that require this deferred response are: + * + * 1. USB_REQ_SETCONFIGURATION, + * 2. USB_REQ_SETINTERFACE, or + * 3. USBSTRG_REQ_MSRESET + * + * In all cases, the success reponse is a zero-length packet; the failure + * response is an EP0 stall. + * + * Input parameters: + * priv - Private state structure for this USB storage instance + * stall - true is the action failed and a stall is required + * + ****************************************************************************/ + +void usbstrg_deferredresponse(FAR struct usbstrg_dev_s *priv, bool failed) +{ + FAR struct usbdev_s *dev; + FAR struct usbdev_req_s *ctrlreq; + int ret; + +#ifdef CONFIG_DEBUG + if (!priv || !priv->usbdev || !priv->ctrlreq) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_DEFERREDRESPINVALIDARGS), 0); + return; + } +#endif + + dev = priv->usbdev; + ctrlreq = priv->ctrlreq; + + /* If no error occurs, respond to the deferred setup command with a null + * packet. + */ + + if (!failed) + { + ctrlreq->len = 0; + ctrlreq->flags = USBDEV_REQFLAGS_NULLPKT; + ret = EP_SUBMIT(dev->ep0, ctrlreq); + if (ret < 0) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_DEFERREDRESPSUBMIT), + (uint16_t)-ret); +#if 0 /* Not necessary */ + ctrlreq->result = OK; + usbstrg_ep0incomplete(dev->ep0, ctrlreq); +#endif + } + } + else + { + /* On a failure, the USB driver will stall. */ + + usbtrace(TRACE_DEVERROR(USBSTRG_TRACEERR_DEFERREDRESPSTALLED), 0); + EP_STALL(dev->ep0); + } +} + +/**************************************************************************** + * User Interfaces + ****************************************************************************/ +/**************************************************************************** + * Name: usbstrg_configure + * + * Description: + * One-time initialization of the USB storage driver. The initialization + * sequence is as follows: + * + * 1. Call usbstrg_configure to perform one-time initialization specifying + * the number of luns. + * 2. Call usbstrg_bindlun to configure each supported LUN + * 3. Call usbstrg_exportluns when all LUNs are configured + * + * Input Parameters: + * nluns - the number of LUNs that will be registered + * handle - Location to return a handle that is used in other API calls. + * + * Returned Value: + * 0 on success; a negated errno on failure + * + ****************************************************************************/ + +int usbstrg_configure(unsigned int nluns, void **handle) +{ + FAR struct usbstrg_alloc_s *alloc; + FAR struct usbstrg_dev_s *priv; + FAR struct usbstrg_driver_s *drvr; + int ret; + +#ifdef CONFIG_DEBUG + if (nluns > 15) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_TOOMANYLUNS), 0); + return -EDOM; + } +#endif + + /* Allocate the structures needed */ + + alloc = (FAR struct usbstrg_alloc_s*)kmalloc(sizeof(struct usbstrg_alloc_s)); + if (!alloc) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_ALLOCDEVSTRUCT), 0); + return -ENOMEM; + } + + /* Initialize the USB storage driver structure */ + + priv = &alloc->dev; + memset(priv, 0, sizeof(struct usbstrg_dev_s)); + + pthread_mutex_init(&priv->mutex, NULL); + pthread_cond_init(&priv->cond, NULL); + sq_init(&priv->wrreqlist); + + priv->nluns = nluns; + + /* Allocate the LUN table */ + + priv->luntab = (struct usbstrg_lun_s*)kmalloc(priv->nluns*sizeof(struct usbstrg_lun_s)); + if (!priv->luntab) + { + ret = -ENOMEM; + goto errout; + } + memset(priv->luntab, 0, priv->nluns * sizeof(struct usbstrg_lun_s)); + + /* Initialize the USB class driver structure */ + + drvr = &alloc->drvr; +#ifdef CONFIG_USBDEV_DUALSPEED + drvr->drvr.speed = USB_SPEED_HIGH; +#else + drvr->drvr.speed = USB_SPEED_FULL; +#endif + drvr->drvr.ops = &g_driverops; + drvr->dev = priv; + + /* Return the handle and success */ + + *handle = (FAR void*)alloc; + return OK; + +errout: + usbstrg_uninitialize(alloc); + return ret; +} + +/**************************************************************************** + * Name: usbstrg_bindlun + * + * Description: + * Bind the block driver specified by drvrpath to a USB storage LUN. + * + * Input Parameters: + * handle - The handle returned by a previous call to usbstrg_configure(). + * drvrpath - the full path to the block driver + * startsector - A sector offset into the block driver to the start of the + * partition on drvrpath (0 if no partitions) + * nsectors - The number of sectors in the partition (if 0, all sectors + * to the end of the media will be exported). + * lunno - the LUN to bind to + * + * Returned Value: + * 0 on success; a negated errno on failure. + * + ****************************************************************************/ + +int usbstrg_bindlun(FAR void *handle, FAR const char *drvrpath, + unsigned int lunno, off_t startsector, size_t nsectors, + bool readonly) +{ + FAR struct usbstrg_alloc_s *alloc = (FAR struct usbstrg_alloc_s *)handle; + FAR struct usbstrg_dev_s *priv; + FAR struct usbstrg_lun_s *lun; + FAR struct inode *inode; + struct geometry geo; + int ret; + +#ifdef CONFIG_DEBUG + if (!alloc || !drvrpath || startsector < 0 || nsectors < 0) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_BINLUNINVALIDARGS1), 0); + return -EINVAL; + } +#endif + + priv = &alloc->dev; + +#ifdef CONFIG_DEBUG + if (!priv->luntab) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_INTERNALCONFUSION1), 0); + return -EIO; + } + + if (lunno > priv->nluns) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_BINDLUNINVALIDARGS2), 0); + return -EINVAL; + } +#endif + + lun = &priv->luntab[lunno]; + +#ifdef CONFIG_DEBUG + if (lun->inode != NULL) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_LUNALREADYBOUND), 0); + return -EBUSY; + } +#endif + + /* Open the block driver */ + + ret = open_blockdriver(drvrpath, 0, &inode); + if (ret < 0) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_BLKDRVEOPEN), 0); + return ret; + } + + /* Get the drive geometry */ + + if (!inode || !inode->u.i_bops || !inode->u.i_bops->geometry || + inode->u.i_bops->geometry(inode, &geo) != OK || !geo.geo_available) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_NOGEOMETRY), 0); + return -ENODEV; + } + + /* Verify that the partition parameters are valid */ + + if (startsector >= geo.geo_nsectors) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_BINDLUNINVALIDARGS3), 0); + return -EDOM; + } + else if (nsectors == 0) + { + nsectors = geo.geo_nsectors - startsector; + } + else if (startsector + nsectors >= geo.geo_nsectors) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_BINDLUNINVALIDARGS4), 0); + return -EDOM; + } + + /* Initialize the LUN structure */ + + memset(lun, 0, sizeof(struct usbstrg_lun_s *)); + + /* Allocate an I/O buffer big enough to hold one hardware sector. SCSI commands + * are processed one at a time so all LUNs may share a single I/O buffer. The + * I/O buffer will be allocated so that is it as large as the largest block + * device sector size + */ + + if (!priv->iobuffer) + { + priv->iobuffer = (uint8_t*)kmalloc(geo.geo_sectorsize); + if (!priv->iobuffer) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_ALLOCIOBUFFER), geo.geo_sectorsize); + return -ENOMEM; + } + priv->iosize = geo.geo_sectorsize; + } + else if (priv->iosize < geo.geo_sectorsize) + { + void *tmp; + tmp = (uint8_t*)realloc(priv->iobuffer, geo.geo_sectorsize); + if (!tmp) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_REALLOCIOBUFFER), geo.geo_sectorsize); + return -ENOMEM; + } + + priv->iobuffer = (uint8_t*)tmp; + priv->iosize = geo.geo_sectorsize; + } + + lun->inode = inode; + lun->startsector = startsector; + lun->nsectors = nsectors; + lun->sectorsize = geo.geo_sectorsize; + + /* If the driver does not support the write method, then this is read-only */ + + if (!inode->u.i_bops->write) + { + lun->readonly = true; + } + return OK; +} + +/**************************************************************************** + * Name: usbstrg_unbindlun + * + * Description: + * Un-bind the block driver for the specified LUN + * + * Input Parameters: + * handle - The handle returned by a previous call to usbstrg_configure(). + * lun - the LUN to unbind from + * + * Returned Value: + * 0 on success; a negated errno on failure. + * + ****************************************************************************/ + +int usbstrg_unbindlun(FAR void *handle, unsigned int lunno) +{ + FAR struct usbstrg_alloc_s *alloc = (FAR struct usbstrg_alloc_s *)handle; + FAR struct usbstrg_dev_s *priv; + FAR struct usbstrg_lun_s *lun; + int ret; + +#ifdef CONFIG_DEBUG + if (!alloc) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_UNBINDLUNINVALIDARGS1), 0); + return -EINVAL; + } +#endif + + priv = &alloc->dev; + +#ifdef CONFIG_DEBUG + if (!priv->luntab) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_INTERNALCONFUSION2), 0); + return -EIO; + } + + if (lunno > priv->nluns) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_UNBINDLUNINVALIDARGS2), 0); + return -EINVAL; + } +#endif + + lun = &priv->luntab[lunno]; + pthread_mutex_lock(&priv->mutex); + +#ifdef CONFIG_DEBUG + if (lun->inode == NULL) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_LUNNOTBOUND), 0); + ret = -EBUSY; + } + else +#endif + { + /* Close the block driver */ + + usbstrg_lununinitialize(lun); + ret = OK; + } + + pthread_mutex_unlock(&priv->mutex); + return ret; +} + +/**************************************************************************** + * Name: usbstrg_exportluns + * + * Description: + * After all of the LUNs have been bound, this function may be called + * in order to export those LUNs in the USB storage device. + * + * Input Parameters: + * handle - The handle returned by a previous call to usbstrg_configure(). + * + * Returned Value: + * 0 on success; a negated errno on failure + * + ****************************************************************************/ + +int usbstrg_exportluns(FAR void *handle) +{ + FAR struct usbstrg_alloc_s *alloc = (FAR struct usbstrg_alloc_s *)handle; + FAR struct usbstrg_dev_s *priv; + FAR struct usbstrg_driver_s *drvr; + irqstate_t flags; +#ifdef SDCC + pthread_attr_t attr; +#endif + int ret; + +#ifdef CONFIG_DEBUG + if (!alloc) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_EXPORTLUNSINVALIDARGS), 0); + return -ENXIO; + } +#endif + + priv = &alloc->dev; + drvr = &alloc->drvr; + + /* Start the worker thread */ + + pthread_mutex_lock(&priv->mutex); + priv->thstate = USBSTRG_STATE_NOTSTARTED; + priv->theventset = USBSTRG_EVENT_NOEVENTS; + +#ifdef SDCC + (void)pthread_attr_init(&attr); + ret = pthread_create(&priv->thread, &attr, usbstrg_workerthread, (pthread_addr_t)priv); +#else + ret = pthread_create(&priv->thread, NULL, usbstrg_workerthread, (pthread_addr_t)priv); +#endif + if (ret != OK) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_THREADCREATE), (uint16_t)-ret); + goto errout_with_mutex; + } + + /* Register the USB storage class driver */ + + ret = usbdev_register(&drvr->drvr); + if (ret != OK) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_DEVREGISTER), (uint16_t)-ret); + goto errout_with_mutex; + } + + /* Signal to start the thread */ + + flags = irqsave(); + priv->theventset |= USBSTRG_EVENT_READY; + pthread_cond_signal(&priv->cond); + irqrestore(flags); + +errout_with_mutex: + pthread_mutex_unlock(&priv->mutex); + return ret; +} + +/**************************************************************************** + * Name: usbstrg_uninitialize + * + * Description: + * Un-initialize the USB storage class driver + * + * Input Parameters: + * handle - The handle returned by a previous call to usbstrg_configure(). + * + * Returned Value: + * None + * + ****************************************************************************/ + +void usbstrg_uninitialize(FAR void *handle) +{ + FAR struct usbstrg_alloc_s *alloc = (FAR struct usbstrg_alloc_s *)handle; + FAR struct usbstrg_dev_s *priv; + irqstate_t flags; +#ifdef SDCC + pthread_addr_t result1, result2; + pthread_attr_t attr; +#endif + void *value; + int ret; + int i; + +#ifdef CONFIG_DEBUG + if (!handle) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_UNINITIALIZEINVALIDARGS), 0); + return; + } +#endif + priv = &alloc->dev; + + /* If the thread hasn't already exitted, tell it to exit now */ + + if (priv->thstate != USBSTRG_STATE_NOTSTARTED) + { + /* The thread was started.. Is it still running? */ + + pthread_mutex_lock(&priv->mutex); + if (priv->thstate != USBSTRG_STATE_TERMINATED) + { + /* Yes.. Ask the thread to stop */ + + flags = irqsave(); + priv->theventset |= USBSTRG_EVENT_TERMINATEREQUEST; + pthread_cond_signal(&priv->cond); + irqrestore(flags); + } + pthread_mutex_unlock(&priv->mutex); + + /* Wait for the thread to exit. This is necessary even if the + * thread has already exitted in order to collect the join + * garbage + */ + + ret = pthread_join(priv->thread, &value); + } + priv->thread = 0; + + /* Unregister the driver */ + + usbdev_unregister(&alloc->drvr.drvr); + + /* Uninitialize and release the LUNs */ + + for (i = 0; i < priv->nluns; ++i) + { + usbstrg_lununinitialize(&priv->luntab[i]); + } + kfree(priv->luntab); + + /* Release the I/O buffer */ + + if (priv->iobuffer) + { + kfree(priv->iobuffer); + } + + /* Uninitialize and release the driver structure */ + + pthread_mutex_destroy(&priv->mutex); + pthread_cond_destroy(&priv->cond); + + kfree(priv); +} diff --git a/nuttx/drivers/usbdev/msc.h b/nuttx/drivers/usbdev/msc.h new file mode 100644 index 000000000..f73d1af9c --- /dev/null +++ b/nuttx/drivers/usbdev/msc.h @@ -0,0 +1,659 @@ +/**************************************************************************** + * drivers/usbdev/msc.h + * + * Copyright (C) 2008-2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Mass storage class device. Bulk-only with SCSI subclass. + * + * 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. + * + ****************************************************************************/ + +#ifndef __DRIVERS_USBDEV_MSC_H +#define __DRIVERS_USBDEV_MSC_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ +/* Configuration ************************************************************/ +/* If the USB mass storage device is configured as part of a composite device + * then both CONFIG_USBDEV_COMPOSITE and CONFIG_USBSTRG_COMPOSITE must be + * defined. + */ + +#ifndef CONFIG_USBDEV_COMPOSITE +# undef CONFIG_USBSTRG_COMPOSITE +#endif + +#if defined(CONFIG_USBSTRG_COMPOSITE) && !defined(CONFIG_USBSTRG_STRBASE) +# define CONFIG_USBSTRG_STRBASE (4) +#endif + +/* Number of requests in the write queue */ + +#ifndef CONFIG_USBSTRG_NWRREQS +# define CONFIG_USBSTRG_NWRREQS 4 +#endif + +/* Number of requests in the read queue */ + +#ifndef CONFIG_USBSTRG_NRDREQS +# define CONFIG_USBSTRG_NRDREQS 4 +#endif + +/* Logical endpoint numbers / max packet sizes */ + +#ifndef CONFIG_USBSTRG_EPBULKOUT +# warning "EPBULKOUT not defined in the configuration" +# define CONFIG_USBSTRG_EPBULKOUT 2 +#endif + +#ifndef CONFIG_USBSTRG_EPBULKIN +# warning "EPBULKIN not defined in the configuration" +# define CONFIG_USBSTRG_EPBULKIN 3 +#endif + +/* Packet and request buffer sizes */ + +#ifndef CONFIG_CDCSER_COMPOSITE +# ifndef CONFIG_USBSTRG_EP0MAXPACKET +# define CONFIG_USBSTRG_EP0MAXPACKET 64 +# endif +#endif + +#ifndef CONFIG_USBSTRG_BULKINREQLEN +# ifdef CONFIG_USBDEV_DUALSPEED +# define CONFIG_USBSTRG_BULKINREQLEN 512 +# else +# define CONFIG_USBSTRG_BULKINREQLEN 64 +# endif +#else +# ifdef CONFIG_USBDEV_DUALSPEED +# if CONFIG_USBSTRG_BULKINREQLEN < 512 +# warning "Bulk in buffer size smaller than max packet" +# undef CONFIG_USBSTRG_BULKINREQLEN +# define CONFIG_USBSTRG_BULKINREQLEN 512 +# endif +# else +# if CONFIG_USBSTRG_BULKINREQLEN < 64 +# warning "Bulk in buffer size smaller than max packet" +# undef CONFIG_USBSTRG_BULKINREQLEN +# define CONFIG_USBSTRG_BULKINREQLEN 64 +# endif +# endif +#endif + +#ifndef CONFIG_USBSTRG_BULKOUTREQLEN +# ifdef CONFIG_USBDEV_DUALSPEED +# define CONFIG_USBSTRG_BULKOUTREQLEN 512 +# else +# define CONFIG_USBSTRG_BULKOUTREQLEN 64 +# endif +#else +# ifdef CONFIG_USBDEV_DUALSPEED +# if CONFIG_USBSTRG_BULKOUTREQLEN < 512 +# warning "Bulk in buffer size smaller than max packet" +# undef CONFIG_USBSTRG_BULKOUTREQLEN +# define CONFIG_USBSTRG_BULKOUTREQLEN 512 +# endif +# else +# if CONFIG_USBSTRG_BULKOUTREQLEN < 64 +# warning "Bulk in buffer size smaller than max packet" +# undef CONFIG_USBSTRG_BULKOUTREQLEN +# define CONFIG_USBSTRG_BULKOUTREQLEN 64 +# endif +# endif +#endif + +/* Vendor and product IDs and strings */ + +#ifndef CONFIG_USBSTRG_COMPOSITE +# ifndef CONFIG_USBSTRG_VENDORID +# warning "CONFIG_USBSTRG_VENDORID not defined" +# define CONFIG_USBSTRG_VENDORID 0x584e +# endif + +# ifndef CONFIG_USBSTRG_PRODUCTID +# warning "CONFIG_USBSTRG_PRODUCTID not defined" +# define CONFIG_USBSTRG_PRODUCTID 0x5342 +# endif + +# ifndef CONFIG_USBSTRG_VERSIONNO +# define CONFIG_USBSTRG_VERSIONNO (0x0399) +# endif + +# ifndef CONFIG_USBSTRG_VENDORSTR +# warning "No Vendor string specified" +# define CONFIG_USBSTRG_VENDORSTR "NuttX" +# endif + +# ifndef CONFIG_USBSTRG_PRODUCTSTR +# warning "No Product string specified" +# define CONFIG_USBSTRG_PRODUCTSTR "USBdev Storage" +# endif + +# undef CONFIG_USBSTRG_SERIALSTR +# define CONFIG_USBSTRG_SERIALSTR "0101" +#endif + +#undef CONFIG_USBSTRG_CONFIGSTR +#define CONFIG_USBSTRG_CONFIGSTR "Bulk" + +/* Debug -- must be consistent with include/debug.h */ + +#ifdef CONFIG_CPP_HAVE_VARARGS +# ifdef CONFIG_DEBUG +# ifdef CONFIG_ARCH_LOWPUTC +# define dbgprintf(format, arg...) lib_lowprintf(format, ##arg) +# else +# define dbgprintf(format, arg...) lib_rawprintf(format, ##arg) +# endif +# else +# define dbgprintf(x...) +# endif +#else +# ifdef CONFIG_DEBUG +# ifdef CONFIG_ARCH_LOWPUTC +# define dbgprintf lib_lowprintf +# else +# define dbgprintf lib_rawprintf +# endif +# else +# define dbgprintf (void) +# endif +#endif + +/* Packet and request buffer sizes */ + +#ifndef CONFIG_USBSTRG_EP0MAXPACKET +# define CONFIG_USBSTRG_EP0MAXPACKET 64 +#endif + +/* USB Controller */ + +#ifndef CONFIG_USBDEV_SELFPOWERED +# define SELFPOWERED USB_CONFIG_ATT_SELFPOWER +#else +# define SELFPOWERED (0) +#endif + +#ifndef CONFIG_USBDEV_REMOTEWAKEUP +# define REMOTEWAKEUP USB_CONFIG_ATTR_WAKEUP +#else +# define REMOTEWAKEUP (0) +#endif + +#ifndef CONFIG_USBDEV_MAXPOWER +# define CONFIG_USBDEV_MAXPOWER 100 +#endif + +/* Current state of the worker thread */ + +#define USBSTRG_STATE_NOTSTARTED (0) /* Thread has not yet been started */ +#define USBSTRG_STATE_STARTED (1) /* Started, but is not yet initialized */ +#define USBSTRG_STATE_IDLE (2) /* Started and waiting for commands */ +#define USBSTRG_STATE_CMDPARSE (3) /* Processing a received command */ +#define USBSTRG_STATE_CMDREAD (4) /* Processing a SCSI read command */ +#define USBSTRG_STATE_CMDWRITE (5) /* Processing a SCSI write command */ +#define USBSTRG_STATE_CMDFINISH (6) /* Finish command processing */ +#define USBSTRG_STATE_CMDSTATUS (7) /* Processing the final status of the command */ +#define USBSTRG_STATE_TERMINATED (8) /* Thread has exitted */ + +/* Event communicated to worker thread */ + +#define USBSTRG_EVENT_NOEVENTS (0) /* There are no outstanding events */ +#define USBSTRG_EVENT_READY (1 << 0) /* Initialization is complete */ +#define USBSTRG_EVENT_RDCOMPLETE (1 << 1) /* A read has completed there is data to be processed */ +#define USBSTRG_EVENT_WRCOMPLETE (1 << 2) /* A write has completed and a request is available */ +#define USBSTRG_EVENT_TERMINATEREQUEST (1 << 3) /* Shutdown requested */ +#define USBSTRG_EVENT_DISCONNECT (1 << 4) /* USB disconnect received */ +#define USBSTRG_EVENT_RESET (1 << 5) /* USB storage setup reset received */ +#define USBSTRG_EVENT_CFGCHANGE (1 << 6) /* USB setup configuration change received */ +#define USBSTRG_EVENT_IFCHANGE (1 << 7) /* USB setup interface change received */ +#define USBSTRG_EVENT_ABORTBULKOUT (1 << 8) /* SCSI receive failure */ + +/* SCSI command flags (passed to usbstrg_setupcmd()) */ + +#define USBSTRG_FLAGS_DIRMASK (0x03) /* Bits 0-1: Data direction */ +#define USBSTRG_FLAGS_DIRNONE (0x00) /* No data to send */ +#define USBSTRG_FLAGS_DIRHOST2DEVICE (0x01) /* Host-to-device */ +#define USBSTRG_FLAGS_DIRDEVICE2HOST (0x02) /* Device-to-host */ +#define USBSTRG_FLAGS_BLOCKXFR (0x04) /* Bit 2: Command is a block transfer request */ +#define USBSTRG_FLAGS_LUNNOTNEEDED (0x08) /* Bit 3: Command does not require a valid LUN */ +#define USBSTRG_FLAGS_UACOKAY (0x10) /* Bit 4: Command OK if unit attention condition */ +#define USBSTRG_FLAGS_RETAINSENSEDATA (0x20) /* Bit 5: Do not clear sense data */ + +/* Descriptors **************************************************************/ + +/* Big enough to hold our biggest descriptor */ + +#define USBSTRG_MXDESCLEN (64) + +/* String language */ + +#define USBSTRG_STR_LANGUAGE (0x0409) /* en-us */ + +/* Descriptor strings */ + +#ifndef CONFIG_USBSTRG_COMPOSITE +# define USBSTRG_MANUFACTURERSTRID (1) +# define USBSTRG_PRODUCTSTRID (2) +# define USBSTRG_SERIALSTRID (3) +# define USBSTRG_CONFIGSTRID (4) +# define USBSTRG_INTERFACESTRID USBSTRG_CONFIGSTRID + +# undef CONFIG_USBSTRG_STRBASE +# define CONFIG_USBSTRG_STRBASE (4) +#else +# define USBSTRG_INTERFACESTRID (CONFIG_USBSTRG_STRBASE+1) +#endif +#define USBSTRG_LASTSTRID USBSTRG_INTERFACESTRID + +#define USBSTRG_NCONFIGS (1) /* Number of configurations supported */ + +/* Configuration Descriptor */ + +#define USBSTRG_NINTERFACES (1) /* Number of interfaces in the configuration */ +#define USBSTRG_INTERFACEID (0) +#define USBSTRG_ALTINTERFACEID (0) +#define USBSTRG_CONFIGIDNONE (0) /* Config ID means to return to address mode */ +#define USBSTRG_CONFIGID (1) /* The only supported configuration ID */ + +/* Interface description */ + +#define USBSTRG_NENDPOINTS (2) /* Number of endpoints in the interface */ + +/* Endpoint configuration */ + +#define USBSTRG_EPOUTBULK_ADDR (CONFIG_USBSTRG_EPBULKOUT) +#define USBSTRG_EPOUTBULK_ATTR (USB_EP_ATTR_XFER_BULK) + +#define USBSTRG_EPINBULK_ADDR (USB_DIR_IN|CONFIG_USBSTRG_EPBULKIN) +#define USBSTRG_EPINBULK_ATTR (USB_EP_ATTR_XFER_BULK) + +#define USBSTRG_HSBULKMAXPACKET (512) +#define USBSTRG_HSBULKMXPKTSHIFT (9) +#define USBSTRG_HSBULKMXPKTMASK (0x000001ff) +#define USBSTRG_FSBULKMAXPACKET (64) +#define USBSTRG_FSBULKMXPKTSHIFT (6) +#define USBSTRG_FSBULKMXPKTMASK (0x0000003f) + +/* Macros for dual speed vs. full speed only operation */ + +#ifdef CONFIG_USBDEV_DUALSPEED +# define USBSTRG_EPBULKINDESC(hs) \ + usbstrg_getepdesc((hs) ? USBSTRG_EPHSBULKIN : USBSTRG_EPFSBULKIN) +# define USBSTRG_EPBULKOUTDESC(hs) \ + usbstrg_getepdesc((hs) ? USBSTRG_EPHSBULKOUT : USBSTRG_EPFSBULKOUT) +# define USBSTRG_BULKMAXPACKET(hs) \ + ((hs) ? USBSTRG_HSBULKMAXPACKET : USBSTRG_FSBULKMAXPACKET) +# define USBSTRG_BULKMXPKTSHIFT(d) \ + (((d)->speed==USB_SPEED_HIGH) ? USBSTRG_HSBULKMXPKTSHIFT : USBSTRG_FSBULKMXPKTSHIFT) +# define USBSTRG_BULKMXPKTMASK(d) \ + (((d)->speed==USB_SPEED_HIGH) ? USBSTRG_HSBULKMXPKTMASK : USBSTRG_FSBULKMXPKTMASK) +#else +# define USBSTRG_EPBULKINDESC(d) usbstrg_getepdesc(USBSTRG_EPFSBULKIN) +# define USBSTRG_EPBULKOUTDESC(d) usbstrg_getepdesc(USBSTRG_EPFSBULKOUT) +# define USBSTRG_BULKMAXPACKET(hs) USBSTRG_FSBULKMAXPACKET +# define USBSTRG_BULKMXPKTSHIFT(d) USBSTRG_FSBULKMXPKTSHIFT +# define USBSTRG_BULKMXPKTMASK(d) USBSTRG_FSBULKMXPKTMASK +#endif + +/* Configuration descriptor size */ + +#ifndef CONFIG_USBSTRG_COMPOSITE + +/* Number of individual descriptors in the configuration descriptor: + * (1) Configuration descriptor + (1) interface descriptor + (2) interface + * descriptors. + */ + +# define USBSTRG_CFGGROUP_SIZE (4) + +/* The size of the config descriptor: (9 + 9 + 2*7) = 32 */ + +# define SIZEOF_USBSTRG_CFGDESC \ + (USB_SIZEOF_CFGDESC + USB_SIZEOF_IFDESC + 2*USB_SIZEOF_EPDESC) +#else + +/* Number of individual descriptors in the configuration descriptor: + * (1) interface descriptor + (2) interface descriptors. + */ + +# define USBSTRG_CFGGROUP_SIZE (3) + +/* The size of the config descriptor: (9 + 2*7) = 23 */ + +# define SIZEOF_USBSTRG_CFGDESC (USB_SIZEOF_IFDESC + 2*USB_SIZEOF_EPDESC) +#endif + +/* Block driver helpers *****************************************************/ + +#define USBSTRG_DRVR_READ(l,b,s,n) ((l)->inode->u.i_bops->read((l)->inode,b,s,n)) +#define USBSTRG_DRVR_WRITE(l,b,s,n) ((l)->inode->u.i_bops->write((l)->inode,b,s,n)) +#define USBSTRG_DRVR_GEOMETRY(l,g) ((l)->inode->u.i_bops->geometry((l)->inode,g)) + +/* Everpresent min/max macros ***********************************************/ + +#ifndef min +# define min(a,b) ((a) < (b) ? (a) : (b)) +#endif + +#ifndef max +# define max(a,b) ((a) > (b) ? (a) : (b)) +#endif + +/**************************************************************************** + * Public Types + ****************************************************************************/ +/* Endpoint descriptors */ + +enum usbstrg_epdesc_e +{ + USBSTRG_EPFSBULKOUT = 0, /* Full speed bulk OUT endpoint descriptor */ + USBSTRG_EPFSBULKIN /* Full speed bulk IN endpoint descriptor */ +#ifdef CONFIG_USBDEV_DUALSPEED + , + USBSTRG_EPHSBULKOUT, /* High speed bulk OUT endpoint descriptor */ + USBSTRG_EPHSBULKIN /* High speed bulk IN endpoint descriptor */ +#endif +}; + +/* Container to support a list of requests */ + +struct usbstrg_req_s +{ + FAR struct usbstrg_req_s *flink; /* Implements a singly linked list */ + FAR struct usbdev_req_s *req; /* The contained request */ +}; + +/* This structure describes one LUN: */ + +struct usbstrg_lun_s +{ + struct inode *inode; /* Inode structure of open'ed block driver */ + uint8_t readonly:1; /* Media is read-only */ + uint8_t locked:1; /* Media removal is prevented */ + uint16_t sectorsize; /* The size of one sector */ + uint32_t sd; /* Sense data */ + uint32_t sdinfo; /* Sense data information */ + uint32_t uad; /* Unit needs attention data */ + off_t startsector; /* Sector offset to start of partition */ + size_t nsectors; /* Number of sectors in the partition */ +}; + +/* Describes the overall state of the driver */ + +struct usbstrg_dev_s +{ + FAR struct usbdev_s *usbdev; /* usbdev driver pointer (Non-null if registered) */ + + /* Worker thread interface */ + + pthread_t thread; /* The worker thread */ + pthread_mutex_t mutex; /* Mutually exclusive access to resources*/ + pthread_cond_t cond; /* Used to signal worker thread */ + volatile uint8_t thstate; /* State of the worker thread */ + volatile uint16_t theventset; /* Set of pending events signaled to worker thread */ + volatile uint8_t thvalue; /* Value passed with the event (must persist) */ + + /* Storage class configuration and state */ + + uint8_t nluns:4; /* Number of LUNs */ + uint8_t config; /* Configuration number */ + + /* Endpoints */ + + FAR struct usbdev_ep_s *epbulkin; /* Bulk IN endpoint structure */ + FAR struct usbdev_ep_s *epbulkout; /* Bulk OUT endpoint structure */ + FAR struct usbdev_req_s *ctrlreq; /* Control request (for ep0 setup responses) */ + + /* SCSI command processing */ + + struct usbstrg_lun_s *lun; /* Currently selected LUN */ + struct usbstrg_lun_s *luntab; /* Allocated table of all LUNs */ + uint8_t cdb[USBSTRG_MAXCDBLEN]; /* Command data (cdb[]) from CBW */ + uint8_t phaseerror:1; /* Need to send phase sensing status */ + uint8_t shortpacket:1; /* Host transmission stopped unexpectedly */ + uint8_t cbwdir:2; /* Direction from CBW. See USBSTRG_FLAGS_DIR* definitions */ + uint8_t cdblen; /* Length of cdb[] from CBW */ + uint8_t cbwlun; /* LUN from the CBW */ + uint16_t nsectbytes; /* Bytes buffered in iobuffer[] */ + uint16_t nreqbytes; /* Bytes buffered in head write requests */ + uint16_t iosize; /* Size of iobuffer[] */ + uint32_t cbwlen; /* Length of data from CBW */ + uint32_t cbwtag; /* Tag from the CBW */ + union + { + uint32_t xfrlen; /* Read/Write: Sectors remaining to be transferred */ + uint32_t alloclen; /* Other device-to-host: Host allocation length */ + } u; + uint32_t sector; /* Current sector (relative to lun->startsector) */ + uint32_t residue; /* Untransferred amount reported in the CSW */ + uint8_t *iobuffer; /* Buffer for data transfers */ + + /* Write request list */ + + struct sq_queue_s wrreqlist; /* List of empty write request containers */ + struct sq_queue_s rdreqlist; /* List of filled read request containers */ + + /* Pre-allocated write request containers. The write requests will + * be linked in a free list (wrreqlist), and used to send requests to + * EPBULKIN; Read requests will be queued in the EBULKOUT. + */ + + struct usbstrg_req_s wrreqs[CONFIG_USBSTRG_NWRREQS]; + struct usbstrg_req_s rdreqs[CONFIG_USBSTRG_NRDREQS]; +}; + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +#undef EXTERN +#if defined(__cplusplus) +# define EXTERN extern "C" +extern "C" +{ +#else +# define EXTERN extern +#endif + +/* String *******************************************************************/ + +EXTERN const char g_vendorstr[]; +EXTERN const char g_productstr[]; +EXTERN const char g_serialstr[]; + +/************************************************************************************ + * Public Function Prototypes + ************************************************************************************/ + +/************************************************************************************ + * Name: usbstrg_mkstrdesc + * + * Description: + * Construct a string descriptor + * + ************************************************************************************/ + +struct usb_strdesc_s; +int usbstrg_mkstrdesc(uint8_t id, struct usb_strdesc_s *strdesc); + +/************************************************************************************ + * Name: usbstrg_getepdesc + * + * Description: + * Return a pointer to the raw device descriptor + * + ************************************************************************************/ + +#ifndef CONFIG_USBSTRG_COMPOSITE +FAR const struct usb_devdesc_s *usbstrg_getdevdesc(void); +#endif + +/************************************************************************************ + * Name: usbstrg_getepdesc + * + * Description: + * Return a pointer to the raw endpoint descriptor (used for configuring endpoints) + * + ************************************************************************************/ + +struct usb_epdesc_s; +FAR const struct usb_epdesc_s *usbstrg_getepdesc(enum usbstrg_epdesc_e epid); + +/************************************************************************************ + * Name: usbstrg_mkcfgdesc + * + * Description: + * Construct the configuration descriptor + * + ************************************************************************************/ + +#ifdef CONFIG_USBDEV_DUALSPEED +int16_t usbstrg_mkcfgdesc(FAR uint8_t *buf, uint8_t speed, uint8_t type); +#else +int16_t usbstrg_mkcfgdesc(FAR uint8_t *buf); +#endif + +/************************************************************************************ + * Name: usbstrg_getqualdesc + * + * Description: + * Return a pointer to the raw qual descriptor + * + ************************************************************************************/ + +#if !defined(CONFIG_USBSTRG_COMPOSITE) && defined(CONFIG_USBDEV_DUALSPEED) +FAR const struct usb_qualdesc_s *usbstrg_getqualdesc(void); +#endif + +/**************************************************************************** + * Name: usbstrg_workerthread + * + * Description: + * This is the main function of the USB storage worker thread. It loops + * until USB-related events occur, then processes those events accordingly + * + ****************************************************************************/ + +EXTERN void *usbstrg_workerthread(void *arg); + +/**************************************************************************** + * Name: usbstrg_setconfig + * + * Description: + * Set the device configuration by allocating and configuring endpoints and + * by allocating and queue read and write requests. + * + ****************************************************************************/ + +EXTERN int usbstrg_setconfig(FAR struct usbstrg_dev_s *priv, uint8_t config); + +/**************************************************************************** + * Name: usbstrg_resetconfig + * + * Description: + * Mark the device as not configured and disable all endpoints. + * + ****************************************************************************/ + +EXTERN void usbstrg_resetconfig(FAR struct usbstrg_dev_s *priv); + +/**************************************************************************** + * Name: usbstrg_wrcomplete + * + * Description: + * Handle completion of write request. This function probably executes + * in the context of an interrupt handler. + * + ****************************************************************************/ + +EXTERN void usbstrg_wrcomplete(FAR struct usbdev_ep_s *ep, + FAR struct usbdev_req_s *req); + +/**************************************************************************** + * Name: usbstrg_rdcomplete + * + * Description: + * Handle completion of read request on the bulk OUT endpoint. This + * is handled like the receipt of serial data on the "UART" + * + ****************************************************************************/ + +EXTERN void usbstrg_rdcomplete(FAR struct usbdev_ep_s *ep, + FAR struct usbdev_req_s *req); + +/**************************************************************************** + * Name: usbstrg_deferredresponse + * + * Description: + * Some EP0 setup request cannot be responded to immediately becuase they + * require some asynchronous action from the SCSI worker thread. This + * function is provided for the SCSI thread to make that deferred response. + * The specific requests that require this deferred response are: + * + * 1. USB_REQ_SETCONFIGURATION, + * 2. USB_REQ_SETINTERFACE, or + * 3. USBSTRG_REQ_MSRESET + * + * In all cases, the success reponse is a zero-length packet; the failure + * response is an EP0 stall. + * + * Input parameters: + * priv - Private state structure for this USB storage instance + * stall - true is the action failed and a stall is required + * + ****************************************************************************/ + +EXTERN void usbstrg_deferredresponse(FAR struct usbstrg_dev_s *priv, bool failed); + +#undef EXTERN +#if defined(__cplusplus) +} +#endif + +#endif /* #define __DRIVERS_USBDEV_MSC_H */ diff --git a/nuttx/drivers/usbdev/msc_descriptors.c b/nuttx/drivers/usbdev/msc_descriptors.c new file mode 100644 index 000000000..7cb77d8a1 --- /dev/null +++ b/nuttx/drivers/usbdev/msc_descriptors.c @@ -0,0 +1,435 @@ +/**************************************************************************** + * drivers/usbdev/msc_descriptors.c + * + * Copyright (C) 2011-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 + * 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 + +#include +#include +#include +#include +#include + +#include +#include + +#include "msc.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ +/* Descriptors **************************************************************/ +/* Device descriptor. If the USB mass storage device is configured as part + * of a composite device, then the device descriptor will be provided by the + * composite device logic. + */ + +#ifndef CONFIG_USBSTRG_COMPOSITE +static const struct usb_devdesc_s g_devdesc = +{ + USB_SIZEOF_DEVDESC, /* len */ + USB_DESC_TYPE_DEVICE, /* type */ + {LSBYTE(0x0200), MSBYTE(0x0200)}, /* usb */ + USB_CLASS_PER_INTERFACE, /* class */ + 0, /* subclass */ + 0, /* protocol */ + CONFIG_USBSTRG_EP0MAXPACKET, /* maxpacketsize */ + { /* vendor */ + LSBYTE(CONFIG_USBSTRG_VENDORID), + MSBYTE(CONFIG_USBSTRG_VENDORID) + }, + { /* product */ + LSBYTE(CONFIG_USBSTRG_PRODUCTID), + MSBYTE(CONFIG_USBSTRG_PRODUCTID) }, + { /* device */ + LSBYTE(CONFIG_USBSTRG_VERSIONNO), + MSBYTE(CONFIG_USBSTRG_VERSIONNO) + }, + USBSTRG_MANUFACTURERSTRID, /* imfgr */ + USBSTRG_PRODUCTSTRID, /* iproduct */ + USBSTRG_SERIALSTRID, /* serno */ + USBSTRG_NCONFIGS /* nconfigs */ +}; +#endif + +/* Configuration descriptor If the USB mass storage device is configured as part + * of a composite device, then the configuration descriptor will be provided by the + * composite device logic. + */ + +#ifndef CONFIG_USBSTRG_COMPOSITE +static const struct usb_cfgdesc_s g_cfgdesc = +{ + USB_SIZEOF_CFGDESC, /* len */ + USB_DESC_TYPE_CONFIG, /* type */ + {0, 0}, /* totallen -- to be provided */ + USBSTRG_NINTERFACES, /* ninterfaces */ + USBSTRG_CONFIGID, /* cfgvalue */ + USBSTRG_CONFIGSTRID, /* icfg */ + USB_CONFIG_ATTR_ONE|SELFPOWERED|REMOTEWAKEUP, /* attr */ + (CONFIG_USBDEV_MAXPOWER + 1) / 2 /* mxpower */ +}; +#endif + +/* Single interface descriptor */ + +static const struct usb_ifdesc_s g_ifdesc = +{ + USB_SIZEOF_IFDESC, /* len */ + USB_DESC_TYPE_INTERFACE, /* type */ + USBSTRG_INTERFACEID, /* ifno */ + USBSTRG_ALTINTERFACEID, /* alt */ + USBSTRG_NENDPOINTS, /* neps */ + USB_CLASS_MASS_STORAGE, /* class */ + USBSTRG_SUBCLASS_SCSI, /* subclass */ + USBSTRG_PROTO_BULKONLY, /* protocol */ + USBSTRG_INTERFACESTRID /* iif */ +}; + +/* Endpoint descriptors */ + +static const struct usb_epdesc_s g_fsepbulkoutdesc = +{ + USB_SIZEOF_EPDESC, /* len */ + USB_DESC_TYPE_ENDPOINT, /* type */ + USBSTRG_EPOUTBULK_ADDR, /* addr */ + USBSTRG_EPOUTBULK_ATTR, /* attr */ + { /* maxpacket */ + LSBYTE(USBSTRG_FSBULKMAXPACKET), + MSBYTE(USBSTRG_FSBULKMAXPACKET) + }, + 0 /* interval */ +}; + +static const struct usb_epdesc_s g_fsepbulkindesc = +{ + USB_SIZEOF_EPDESC, /* len */ + USB_DESC_TYPE_ENDPOINT, /* type */ + USBSTRG_EPINBULK_ADDR, /* addr */ + USBSTRG_EPINBULK_ATTR, /* attr */ + { /* maxpacket */ + LSBYTE(USBSTRG_FSBULKMAXPACKET), + MSBYTE(USBSTRG_FSBULKMAXPACKET) + }, + 0 /* interval */ +}; + +#ifdef CONFIG_USBDEV_DUALSPEED +#ifndef CONFIG_USBSTRG_COMPOSITE +static const struct usb_qualdesc_s g_qualdesc = +{ + USB_SIZEOF_QUALDESC, /* len */ + USB_DESC_TYPE_DEVICEQUALIFIER, /* type */ + { /* usb */ + LSBYTE(0x0200), + MSBYTE(0x0200) + }, + USB_CLASS_PER_INTERFACE, /* class */ + 0, /* subclass */ + 0, /* protocol */ + CONFIG_USBSTRG_EP0MAXPACKET, /* mxpacketsize */ + USBSTRG_NCONFIGS, /* nconfigs */ + 0, /* reserved */ +}; +#endif + +static const struct usb_epdesc_s g_hsepbulkoutdesc = +{ + USB_SIZEOF_EPDESC, /* len */ + USB_DESC_TYPE_ENDPOINT, /* type */ + USBSTRG_EPOUTBULK_ADDR, /* addr */ + USBSTRG_EPOUTBULK_ATTR, /* attr */ + { /* maxpacket */ + LSBYTE(USBSTRG_HSBULKMAXPACKET), + MSBYTE(USBSTRG_HSBULKMAXPACKET) + }, + 0 /* interval */ +}; + +static const struct usb_epdesc_s g_hsepbulkindesc = +{ + USB_SIZEOF_EPDESC, /* len */ + USB_DESC_TYPE_ENDPOINT, /* type */ + USBSTRG_EPINBULK_ADDR, /* addr */ + USBSTRG_EPINBULK_ATTR, /* attr */ + { /* maxpacket */ + LSBYTE(USBSTRG_HSBULKMAXPACKET), + MSBYTE(USBSTRG_HSBULKMAXPACKET) + }, + 0 /* interval */ +}; +#endif + +/**************************************************************************** + * Public Data + ****************************************************************************/ +/* Strings ******************************************************************/ + +#ifndef CONFIG_USBSTRG_COMPOSITE +const char g_vendorstr[] = CONFIG_USBSTRG_VENDORSTR; +const char g_productstr[] = CONFIG_USBSTRG_PRODUCTSTR; +const char g_serialstr[] = CONFIG_USBSTRG_SERIALSTR; +#endif + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: usbstrg_mkstrdesc + * + * Description: + * Construct a string descriptor + * + ****************************************************************************/ + +int usbstrg_mkstrdesc(uint8_t id, struct usb_strdesc_s *strdesc) +{ + const char *str; + int len; + int ndata; + int i; + + switch (id) + { +#ifndef CONFIG_USBSTRG_COMPOSITE + case 0: + { + /* Descriptor 0 is the language id */ + + strdesc->len = 4; + strdesc->type = USB_DESC_TYPE_STRING; + strdesc->data[0] = LSBYTE(USBSTRG_STR_LANGUAGE); + strdesc->data[1] = MSBYTE(USBSTRG_STR_LANGUAGE); + return 4; + } + + case USBSTRG_MANUFACTURERSTRID: + str = g_vendorstr; + break; + + case USBSTRG_PRODUCTSTRID: + str = g_productstr; + break; + + case USBSTRG_SERIALSTRID: + str = g_serialstr; + break; +#endif + + /* case USBSTRG_CONFIGSTRID: */ + case USBSTRG_INTERFACESTRID: + str = CONFIG_USBSTRG_CONFIGSTR; + break; + + default: + return -EINVAL; + } + + /* The string is utf16-le. The poor man's utf-8 to utf16-le + * conversion below will only handle 7-bit en-us ascii + */ + + len = strlen(str); + for (i = 0, ndata = 0; i < len; i++, ndata += 2) + { + strdesc->data[ndata] = str[i]; + strdesc->data[ndata+1] = 0; + } + + strdesc->len = ndata+2; + strdesc->type = USB_DESC_TYPE_STRING; + return strdesc->len; +} + +/**************************************************************************** + * Name: usbstrg_getepdesc + * + * Description: + * Return a pointer to the raw device descriptor + * + ****************************************************************************/ + +#ifndef CONFIG_USBSTRG_COMPOSITE +FAR const struct usb_devdesc_s *usbstrg_getdevdesc(void) +{ + return &g_devdesc; +} +#endif + +/**************************************************************************** + * Name: usbstrg_getepdesc + * + * Description: + * Return a pointer to the raw endpoint descriptor (used for configuring + * endpoints) + * + ****************************************************************************/ + +FAR const struct usb_epdesc_s *usbstrg_getepdesc(enum usbstrg_epdesc_e epid) +{ + switch (epid) + { + case USBSTRG_EPFSBULKOUT: /* Full speed bulk OUT endpoint descriptor */ + return &g_fsepbulkoutdesc; + + case USBSTRG_EPFSBULKIN: /* Full speed bulk IN endpoint descriptor */ + return &g_fsepbulkindesc; + +#ifdef CONFIG_USBDEV_DUALSPEED + case USBSTRG_EPHSBULKOUT: /* High speed bulk OUT endpoint descriptor */ + return &g_hsepbulkoutdesc; + + case USBSTRG_EPHSBULKIN: /* High speed bulk IN endpoint descriptor */ + return &g_hsepbulkindesc; +#endif + default: + return NULL; + } +}; + +/**************************************************************************** + * Name: usbstrg_mkcfgdesc + * + * Description: + * Construct the configuration descriptor + * + ****************************************************************************/ + +#ifdef CONFIG_USBDEV_DUALSPEED +int16_t usbstrg_mkcfgdesc(uint8_t *buf, uint8_t speed, uint8_t type) +#else +int16_t usbstrg_mkcfgdesc(uint8_t *buf) +#endif +{ +#ifndef CONFIG_USBSTRG_COMPOSITE + FAR struct usb_cfgdesc_s *cfgdesc = (struct usb_cfgdesc_s*)buf; +#endif +#ifdef CONFIG_USBDEV_DUALSPEED + FAR const struct usb_epdesc_s *epdesc; + bool hispeed = (speed == USB_SPEED_HIGH); + uint16_t bulkmxpacket; +#endif + uint16_t totallen; + + /* This is the total length of the configuration (not necessarily the + * size that we will be sending now. + */ + + totallen = USB_SIZEOF_CFGDESC + USB_SIZEOF_IFDESC + USBSTRG_NENDPOINTS * USB_SIZEOF_EPDESC; + + /* Configuration descriptor -- Copy the canned descriptor and fill in the + * type (we'll also need to update the size below). If the USB mass storage + * device is configured as part of a composite device, then the configuration + * descriptor will be provided by the composite device logic. + */ + +#ifndef CONFIG_USBSTRG_COMPOSITE + memcpy(cfgdesc, &g_cfgdesc, USB_SIZEOF_CFGDESC); + buf += USB_SIZEOF_CFGDESC; +#endif + + /* Copy the canned interface descriptor */ + + memcpy(buf, &g_ifdesc, USB_SIZEOF_IFDESC); + buf += USB_SIZEOF_IFDESC; + + /* Make the two endpoint configurations */ + +#ifdef CONFIG_USBDEV_DUALSPEED + /* Check for switches between high and full speed */ + + hispeed = (speed == USB_SPEED_HIGH); + if (type == USB_DESC_TYPE_OTHERSPEEDCONFIG) + { + hispeed = !hispeed; + } + + bulkmxpacket = USBSTRG_BULKMAXPACKET(hispeed); + epdesc = USBSTRG_EPBULKINDESC(hispeed); + memcpy(buf, epdesc, USB_SIZEOF_EPDESC); + buf += USB_SIZEOF_EPDESC; + + epdesc = USBSTRG_EPBULKOUTDESC(hispeed); + memcpy(buf, epdesc, USB_SIZEOF_EPDESC); +#else + memcpy(buf, &g_fsepbulkoutdesc, USB_SIZEOF_EPDESC); + buf += USB_SIZEOF_EPDESC; + memcpy(buf, &g_fsepbulkindesc, USB_SIZEOF_EPDESC); +#endif + + /* Finally, fill in the total size of the configuration descriptor */ + +#ifndef CONFIG_USBSTRG_COMPOSITE + cfgdesc->totallen[0] = LSBYTE(totallen); + cfgdesc->totallen[1] = MSBYTE(totallen); +#endif + return totallen; +} + +/**************************************************************************** + * Name: usbstrg_getqualdesc + * + * Description: + * Return a pointer to the raw qual descriptor + * + ****************************************************************************/ + +#if !defined(CONFIG_USBSTRG_COMPOSITE) && defined(CONFIG_USBDEV_DUALSPEED) +FAR const struct usb_qualdesc_s *usbstrg_getqualdesc(void) +{ + return &g_qualdesc; +} +#endif + diff --git a/nuttx/drivers/usbdev/msc_scsi.c b/nuttx/drivers/usbdev/msc_scsi.c new file mode 100644 index 000000000..d5b0162e0 --- /dev/null +++ b/nuttx/drivers/usbdev/msc_scsi.c @@ -0,0 +1,2664 @@ +/**************************************************************************** + * drivers/usbdev/msc_scsi.c + * + * Copyright (C) 2008-2010, 2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * Mass storage class device. Bulk-only with SCSI subclass. + * + * References: + * "Universal Serial Bus Mass Storage Class, Specification Overview," + * Revision 1.2, USB Implementer's Forum, June 23, 2003. + * + * "Universal Serial Bus Mass Storage Class, Bulk-Only Transport," + * Revision 1.0, USB Implementer's Forum, September 31, 1999. + * + * "SCSI Primary Commands - 3 (SPC-3)," American National Standard + * for Information Technology, May 4, 2005 + * + * "SCSI Primary Commands - 4 (SPC-4)," American National Standard + * for Information Technology, July 19, 2008 + * + * "SCSI Block Commands -2 (SBC-2)," American National Standard + * for Information Technology, November 13, 2004 + * + * 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 + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "msc.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* Configuration ************************************************************/ + +/* Race condition workaround found by David Hewson. This race condition + * "seems to relate to stalling the endpoint when a short response is + * generated which causes a residue to exist when the CSW would be returned. + * I think there's two issues here. The first being if the transfer which + * had just been queued before the stall had not completed then it wouldn’t + * then complete once the endpoint was stalled? The second is that the + * subsequent transfer for the CSW would be dropped on the floor (by the + * epsubmit() function) if the end point was still stalled as the control + * transfer to resume it hadn't occurred." + */ + +#define CONFIG_USBSTRG_RACEWAR 1 + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/* Debug ********************************************************************/ + +#if defined(CONFIG_DEBUG_VERBOSE) && defined (CONFIG_DEBUG_USB) +static void usbstrg_dumpdata(const char *msg, const uint8_t *buf, + int buflen); +#else +# define usbstrg_dumpdata(msg, buf, len) +#endif + +/* Utility Support Functions ************************************************/ + +static uint16_t usbstrg_getbe16(uint8_t *buf); +static uint32_t usbstrg_getbe32(uint8_t *buf); +static void usbstrg_putbe16(uint8_t * buf, uint16_t val); +static void usbstrg_putbe24(uint8_t *buf, uint32_t val); +static void usbstrg_putbe32(uint8_t *buf, uint32_t val); +#if 0 /* not used */ +static uint16_t usbstrg_getle16(uint8_t *buf); +#endif +static uint32_t usbstrg_getle32(uint8_t *buf); +#if 0 /* not used */ +static void usbstrg_putle16(uint8_t * buf, uint16_t val); +#endif +static void usbstrg_putle32(uint8_t *buf, uint32_t val); + +/* SCSI Command Processing **************************************************/ + +static inline int usbstrg_cmdtestunitready(FAR struct usbstrg_dev_s *priv); +static inline int usbstrg_cmdrequestsense(FAR struct usbstrg_dev_s *priv, + FAR uint8_t *buf); +static inline int usbstrg_cmdread6(FAR struct usbstrg_dev_s *priv); +static inline int usbstrg_cmdwrite6(FAR struct usbstrg_dev_s *priv); +static inline int usbstrg_cmdinquiry(FAR struct usbstrg_dev_s *priv, + FAR uint8_t *buf); +static inline int usbstrg_cmdmodeselect6(FAR struct usbstrg_dev_s *priv); +static int usbstrg_modepage(FAR struct usbstrg_dev_s *priv, + FAR uint8_t *buf, uint8_t pcpgcode, int *mdlen); +static inline int usbstrg_cmdmodesense6(FAR struct usbstrg_dev_s *priv, + FAR uint8_t *buf); +static inline int usbstrg_cmdstartstopunit(FAR struct usbstrg_dev_s *priv); +static inline int usbstrg_cmdpreventmediumremoval(FAR struct usbstrg_dev_s *priv); +static inline int usbstrg_cmdreadformatcapacity(FAR struct usbstrg_dev_s *priv, + FAR uint8_t *buf); +static inline int usbstrg_cmdreadcapacity10(FAR struct usbstrg_dev_s *priv, + FAR uint8_t *buf); +static inline int usbstrg_cmdread10(FAR struct usbstrg_dev_s *priv); +static inline int usbstrg_cmdwrite10(FAR struct usbstrg_dev_s *priv); +static inline int usbstrg_cmdverify10(FAR struct usbstrg_dev_s *priv); +static inline int usbstrg_cmdsynchronizecache10(FAR struct usbstrg_dev_s *priv); +static inline int usbstrg_cmdmodeselect10(FAR struct usbstrg_dev_s *priv); +static inline int usbstrg_cmdmodesense10(FAR struct usbstrg_dev_s *priv, + FAR uint8_t *buf); +static inline int usbstrg_cmdread12(FAR struct usbstrg_dev_s *priv); +static inline int usbstrg_cmdwrite12(FAR struct usbstrg_dev_s *priv); +static inline int usbstrg_setupcmd(FAR struct usbstrg_dev_s *priv, + uint8_t cdblen, uint8_t flags); + +/* SCSI Worker Thread *******************************************************/ + +static int usbstrg_idlestate(FAR struct usbstrg_dev_s *priv); +static int usbstrg_cmdparsestate(FAR struct usbstrg_dev_s *priv); +static int usbstrg_cmdreadstate(FAR struct usbstrg_dev_s *priv); +static int usbstrg_cmdwritestate(FAR struct usbstrg_dev_s *priv); +static int usbstrg_cmdfinishstate(FAR struct usbstrg_dev_s *priv); +static int usbstrg_cmdstatusstate(FAR struct usbstrg_dev_s *priv); + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Debug + ****************************************************************************/ + +/**************************************************************************** + * Name: usbstrg_dumpdata + ****************************************************************************/ + +#if defined(CONFIG_DEBUG_VERBOSE) && defined (CONFIG_DEBUG_USB) +static void usbstrg_dumpdata(const char *msg, const uint8_t *buf, int buflen) +{ + int i; + + dbgprintf("%s:", msg); + for (i = 0; i < buflen; i++) + { + dbgprintf(" %02x", buf[i]); + } + dbgprintf("\n"); +} +#endif + +/**************************************************************************** + * Utility Support Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: usbstrg_getbe16 + * + * Description: + * Get a 16-bit big-endian value reference by the byte pointer + * + ****************************************************************************/ + +static uint16_t usbstrg_getbe16(uint8_t *buf) +{ + return ((uint16_t)buf[0] << 8) | ((uint16_t)buf[1]); +} + +/**************************************************************************** + * Name: usbstrg_getbe32 + * + * Description: + * Get a 32-bit big-endian value reference by the byte pointer + * + ****************************************************************************/ + +static uint32_t usbstrg_getbe32(uint8_t *buf) +{ + return ((uint32_t)buf[0] << 24) | ((uint32_t)buf[1] << 16) | + ((uint32_t)buf[2] << 8) | ((uint32_t)buf[3]); +} + +/**************************************************************************** + * Name: usbstrg_putbe16 + * + * Description: + * Store a 16-bit value in big-endian order to the location specified by + * a byte pointer + * + ****************************************************************************/ + +static void usbstrg_putbe16(uint8_t * buf, uint16_t val) +{ + buf[0] = val >> 8; + buf[1] = val; +} + +/**************************************************************************** + * Name: usbstrg_putbe24 + * + * Description: + * Store a 32-bit value in big-endian order to the location specified by + * a byte pointer + * + ****************************************************************************/ + +static void usbstrg_putbe24(uint8_t *buf, uint32_t val) +{ + buf[0] = val >> 16; + buf[1] = val >> 8; + buf[2] = val; +} + +/**************************************************************************** + * Name: usbstrg_putbe32 + * + * Description: + * Store a 32-bit value in big-endian order to the location specified by + * a byte pointer + * + ****************************************************************************/ + +static void usbstrg_putbe32(uint8_t *buf, uint32_t val) +{ + buf[0] = val >> 24; + buf[1] = val >> 16; + buf[2] = val >> 8; + buf[3] = val; +} + +/**************************************************************************** + * Name: usbstrg_getle16 + * + * Description: + * Get a 16-bit little-endian value reference by the byte pointer + * + ****************************************************************************/ + +#if 0 /* not used */ +static uint16_t usbstrg_getle16(uint8_t *buf) +{ + return ((uint16_t)buf[1] << 8) | ((uint16_t)buf[0]); +} +#endif + +/**************************************************************************** + * Name: usbstrg_getle32 + * + * Description: + * Get a 32-bit little-endian value reference by the byte pointer + * + ****************************************************************************/ + +static uint32_t usbstrg_getle32(uint8_t *buf) +{ + return ((uint32_t)buf[3] << 24) | ((uint32_t)buf[2] << 16) | + ((uint32_t)buf[1] << 8) | ((uint32_t)buf[0]); +} + +/**************************************************************************** + * Name: usbstrg_putle16 + * + * Description: + * Store a 16-bit value in little-endian order to the location specified by + * a byte pointer + * + ****************************************************************************/ + +#if 0 /* not used */ +static void usbstrg_putle16(uint8_t * buf, uint16_t val) +{ + buf[0] = val; + buf[1] = val >> 8; +} +#endif + +/**************************************************************************** + * Name: usbstrg_putle32 + * + * Description: + * Store a 32-bit value in little-endian order to the location specified by + * a byte pointer + * + ****************************************************************************/ + +static void usbstrg_putle32(uint8_t *buf, uint32_t val) +{ + buf[0] = val; + buf[1] = val >> 8; + buf[2] = val >> 16; + buf[3] = val >> 24; +} + +/**************************************************************************** + * SCSI Worker Thread + ****************************************************************************/ + +/**************************************************************************** + * Name: usbstrg_cmdtestunitready + * + * Description: + * Handle the SCSI_CMD_TESTUNITREADY command + * + ****************************************************************************/ + +static inline int usbstrg_cmdtestunitready(FAR struct usbstrg_dev_s *priv) +{ + int ret; + + priv->u.alloclen = 0; + ret = usbstrg_setupcmd(priv, 6, USBSTRG_FLAGS_DIRNONE); + return ret; +} + +/**************************************************************************** + * Name: usbstrg_cmdrequestsense + * + * Description: + * Handle the SCSI_CMD_REQUESTSENSE command + * + ****************************************************************************/ + +static inline int usbstrg_cmdrequestsense(FAR struct usbstrg_dev_s *priv, + FAR uint8_t *buf) +{ + FAR struct scsicmd_requestsense_s *request = (FAR struct scsicmd_requestsense_s *)priv->cdb; + FAR struct scsiresp_fixedsensedata_s *response = (FAR struct scsiresp_fixedsensedata_s *)buf; + FAR struct usbstrg_lun_s *lun; + uint32_t sd; + uint32_t sdinfo; + uint8_t cdblen; + int ret; + + /* Extract the host allocation length */ + + priv->u.alloclen = request->alloclen; + + /* Get the expected length of the command (with hack for MS-Windows 12-byte + * REQUEST SENSE command. + */ + + cdblen = SCSICMD_REQUESTSENSE_SIZEOF; + if (cdblen != priv->cdblen) + { + /* Try MS-Windows REQUEST SENSE with cbw->cdblen == 12 */ + + cdblen = SCSICMD_REQUESTSENSE_MSSIZEOF; + } + + ret = usbstrg_setupcmd(priv, cdblen, + USBSTRG_FLAGS_DIRDEVICE2HOST|USBSTRG_FLAGS_LUNNOTNEEDED| + USBSTRG_FLAGS_UACOKAY|USBSTRG_FLAGS_RETAINSENSEDATA); + if (ret == OK) + { + lun = priv->lun; + if (!lun) + { + sd = SCSI_KCQIR_INVALIDLUN; + sdinfo = 0; + } + else + { + /* Get the saved sense data from the LUN structure */ + + sd = lun->sd; + sdinfo = lun->sdinfo; + + /* Discard the sense data */ + + lun->sd = SCSI_KCQ_NOSENSE; + lun->sdinfo = 0; + } + + /* Create the fixed sense data response */ + + memset(response, 0, SCSIRESP_FIXEDSENSEDATA_SIZEOF); + + response->code = SCSIRESP_SENSEDATA_RESPVALID|SCSIRESP_SENSEDATA_CURRENTFIXED; + response->flags = (uint8_t)(sd >> 16); + usbstrg_putbe32(response->info, sdinfo); + response->len = SCSIRESP_FIXEDSENSEDATA_SIZEOF - 7; + response->code2 = (uint8_t)(sd >> 8); + response->qual2 = (uint8_t)sd; + + priv->nreqbytes = SCSIRESP_FIXEDSENSEDATA_SIZEOF; + ret = OK; + } + + return ret; +} + +/**************************************************************************** + * Name: usbstrg_cmdread6 + * + * Description: + * Handle the SCSI_CMD_READ6 command + * + ****************************************************************************/ + +static inline int usbstrg_cmdread6(FAR struct usbstrg_dev_s *priv) +{ + FAR struct scsicmd_read6_s *read6 = (FAR struct scsicmd_read6_s*)priv->cdb; + FAR struct usbstrg_lun_s *lun = priv->lun; + int ret; + + priv->u.xfrlen = (uint16_t)read6->xfrlen; + if (priv->u.xfrlen == 0) + { + priv->u.xfrlen = 256; + } + + ret = usbstrg_setupcmd(priv, SCSICMD_READ6_SIZEOF, USBSTRG_FLAGS_DIRDEVICE2HOST|USBSTRG_FLAGS_BLOCKXFR); + if (ret == OK) + { + /* Get the Logical Block Address (LBA) from cdb[] as the starting sector */ + + priv->sector = (uint32_t)(read6->mslba & SCSICMD_READ6_MSLBAMASK) << 16 | (uint32_t)usbstrg_getbe16(read6->lslba); + + /* Verify that a block driver has been bound to the LUN */ + + if (!lun->inode) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_READ6MEDIANOTPRESENT), 0); + lun->sd = SCSI_KCQNR_MEDIANOTPRESENT; + ret = -EINVAL; + } + + /* Verify that sector lies in the range supported by the block driver */ + + else if (priv->sector >= lun->nsectors) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_READ6LBARANGE), 0); + lun->sd = SCSI_KCQIR_LBAOUTOFRANGE; + ret = -EINVAL; + } + + /* Looks like we are good to go */ + + else + { + usbtrace(TRACE_CLASSSTATE(USBSTRG_CLASSSTATE_CMDPARSECMDREAD6), priv->cdb[0]); + priv->thstate = USBSTRG_STATE_CMDREAD; + } + } + return ret; +} + +/**************************************************************************** + * Name: usbstrg_cmdwrite6 + * + * Description: + * Handle the SCSI_CMD_WRITE6 command + * + ****************************************************************************/ + +static inline int usbstrg_cmdwrite6(FAR struct usbstrg_dev_s *priv) +{ + FAR struct scsicmd_write6_s *write6 = (FAR struct scsicmd_write6_s *)priv->cdb; + FAR struct usbstrg_lun_s *lun = priv->lun; + int ret; + + priv->u.xfrlen = (uint16_t)write6->xfrlen; + if (priv->u.xfrlen == 0) + { + priv->u.xfrlen = 256; + } + + ret = usbstrg_setupcmd(priv, SCSICMD_WRITE6_SIZEOF, USBSTRG_FLAGS_DIRHOST2DEVICE|USBSTRG_FLAGS_BLOCKXFR); + if (ret == OK) + { + /* Get the Logical Block Address (LBA) from cdb[] as the starting sector */ + + priv->sector = (uint32_t)(write6->mslba & SCSICMD_WRITE6_MSLBAMASK) << 16 | (uint32_t)usbstrg_getbe16(write6->lslba); + + /* Verify that a block driver has been bound to the LUN */ + + if (!lun->inode) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_WRITE6MEDIANOTPRESENT), 0); + lun->sd = SCSI_KCQNR_MEDIANOTPRESENT; + ret = -EINVAL; + } + + /* Check for attempts to write to a read-only device */ + + else if (lun->readonly) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_WRITE6READONLY), 0); + lun->sd = SCSI_KCQWP_COMMANDNOTALLOWED; + ret = -EINVAL; + } + + /* Verify that sector lies in the range supported by the block driver */ + + else if (priv->sector >= lun->nsectors) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_WRITE6LBARANGE), 0); + lun->sd = SCSI_KCQIR_LBAOUTOFRANGE; + ret = -EINVAL; + } + + /* Looks like we are good to go */ + + else + { + usbtrace(TRACE_CLASSSTATE(USBSTRG_CLASSSTATE_CMDPARSECMDWRITE6), priv->cdb[0]); + priv->thstate = USBSTRG_STATE_CMDWRITE; + } + } + return ret; +} + +/**************************************************************************** + * Name: usbstrg_cmdinquiry + * + * Description: + * Handle SCSI_CMD_INQUIRY command + * + ****************************************************************************/ + +static inline int usbstrg_cmdinquiry(FAR struct usbstrg_dev_s *priv, + FAR uint8_t *buf) +{ + FAR struct scscicmd_inquiry_s *inquiry = (FAR struct scscicmd_inquiry_s *)priv->cdb; + FAR struct scsiresp_inquiry_s *response = (FAR struct scsiresp_inquiry_s *)buf; + int len; + int ret; + + priv->u.alloclen = usbstrg_getbe16(inquiry->alloclen); + ret = usbstrg_setupcmd(priv, SCSICMD_INQUIRY_SIZEOF, + USBSTRG_FLAGS_DIRDEVICE2HOST|USBSTRG_FLAGS_LUNNOTNEEDED|USBSTRG_FLAGS_UACOKAY); + if (ret == OK) + { + if (!priv->lun) + { + response->qualtype = SCSIRESP_INQUIRYPQ_NOTCAPABLE|SCSIRESP_INQUIRYPD_UNKNOWN; + } + else if ((inquiry->flags != 0) || (inquiry->pagecode != 0)) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_INQUIRYFLAGS), 0); + priv->lun->sd = SCSI_KCQIR_INVALIDFIELDINCBA; + ret = -EINVAL; + } + else + { + memset(response, 0, SCSIRESP_INQUIRY_SIZEOF); + priv->nreqbytes = SCSIRESP_INQUIRY_SIZEOF; + +#ifdef CONFIG_USBSTRG_REMOVABLE + response->flags1 = SCSIRESP_INQUIRYFLAGS1_RMB; +#endif + response->version = 2; /* SCSI-2 */ + response->flags2 = 2; /* SCSI-2 INQUIRY response data format */ + response->len = SCSIRESP_INQUIRY_SIZEOF - 5; + + /* Strings */ + + memset(response->vendorid, ' ', 8+16+4); + + len = strlen(g_vendorstr); + DEBUGASSERT(len <= 8); + memcpy(response->vendorid, g_vendorstr, len); + + len = strlen(g_productstr); + DEBUGASSERT(len <= 16); + memcpy(response->productid, g_productstr, len); + + len = strlen(g_serialstr); + DEBUGASSERT(len <= 4); + memcpy(response->productid, g_serialstr, len); + } + } + + return ret; +} + +/**************************************************************************** + * Name: usbstrg_cmdmodeselect6 + * + * Description: + * Handle SCSI_CMD_MODESELECT6 command + * + ****************************************************************************/ + +static inline int usbstrg_cmdmodeselect6(FAR struct usbstrg_dev_s *priv) +{ + FAR struct scsicmd_modeselect6_s *modeselect = (FAR struct scsicmd_modeselect6_s *)priv->cdb; + + priv->u.alloclen = modeselect->plen; + (void)usbstrg_setupcmd(priv, SCSICMD_MODESELECT6_SIZEOF, USBSTRG_FLAGS_DIRHOST2DEVICE); + + /* Not supported */ + + priv->lun->sd = SCSI_KCQIR_INVALIDCOMMAND; + return -EINVAL; +} + +/**************************************************************************** + * Name: usbstrg_modepage + * + * Description: + * Common logic for usbstrg_cmdmodesense6() and usbstrg_cmdmodesense10() + * + ****************************************************************************/ + +static int usbstrg_modepage(FAR struct usbstrg_dev_s *priv, FAR uint8_t *buf, + uint8_t pcpgcode, int *mdlen) +{ + FAR struct scsiresp_cachingmodepage_s *cmp = (FAR struct scsiresp_cachingmodepage_s *)buf; + + /* Saving parms not supported */ + + if ((pcpgcode & SCSICMD_MODESENSE_PCMASK) == SCSICMD_MODESENSE_PCSAVED) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_PCSAVED), 0); + priv->lun->sd = SCSI_KCQIR_SAVINGPARMSNOTSUPPORTED; + return -EINVAL; + } + + /* Only the caching mode page is supported: */ + + if ((pcpgcode & SCSICMD_MODESENSE_PGCODEMASK) == SCSIRESP_MODESENSE_PGCCODE_CACHING || + (pcpgcode & SCSICMD_MODESENSE_PGCODEMASK) == SCSIRESP_MODESENSE_PGCCODE_RETURNALL) + { + memset(cmp, 0, 12); + cmp->pgcode = SCSIRESP_MODESENSE_PGCCODE_CACHING; + cmp->len = 10; /* n-2 */ + + /* None of the fields are changeable */ + + if (((pcpgcode & SCSICMD_MODESENSE_PCMASK) != SCSICMD_MODESENSE_PCCHANGEABLE)) + { + cmp->flags1 = SCSIRESP_CACHINGMODEPG_WCE; /* Write cache enable */ + cmp->dpflen[0] = 0xff; /* Disable prefetch transfer length = 0xffffffff */ + cmp->dpflen[1] = 0xff; + cmp->maxpf[0] = 0xff; /* Maximum pre-fetch = 0xffffffff */ + cmp->maxpf[1] = 0xff; + cmp->maxpfc[0] = 0xff; /* Maximum pref-fetch ceiling = 0xffffffff */ + cmp->maxpfc[1] = 0xff; + } + + /* Return the mode data length */ + + *mdlen = 12; /* Only the first 12-bytes of caching mode page sent */ + return OK; + } + else + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_MODEPAGEFLAGS), pcpgcode); + priv->lun->sd = SCSI_KCQIR_INVALIDFIELDINCBA; + return -EINVAL; + } +} + +/**************************************************************************** + * Name: usbstrg_cmdmodesense6 + * + * Description: + * Handle SCSI_CMD_MODESENSE6 command + * + ****************************************************************************/ + +static int inline usbstrg_cmdmodesense6(FAR struct usbstrg_dev_s *priv, + FAR uint8_t *buf) +{ + FAR struct scsicmd_modesense6_s *modesense = (FAR struct scsicmd_modesense6_s *)priv->cdb; + FAR struct scsiresp_modeparameterhdr6_s *mph = (FAR struct scsiresp_modeparameterhdr6_s *)buf; + int mdlen; + int ret; + + priv->u.alloclen = modesense->alloclen; + ret = usbstrg_setupcmd(priv, SCSICMD_MODESENSE6_SIZEOF, USBSTRG_FLAGS_DIRDEVICE2HOST); + if (ret == OK) + { + if ((modesense->flags & ~SCSICMD_MODESENSE6_DBD) != 0 || modesense->subpgcode != 0) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_MODESENSE6FLAGS), 0); + priv->lun->sd = SCSI_KCQIR_INVALIDFIELDINCBA; + ret = -EINVAL; + } + else + { + /* The response consists of: + * + * (1) A MODESENSE6-specific mode parameter header, + * (2) A variable length list of block descriptors, and + * (3) A variable length list of mode page formats + */ + + mph->type = 0; /* Medium type */ + mph->param = (priv->lun->readonly ? SCSIRESP_MODEPARMHDR_DAPARM_WP : 0x00); + mph->bdlen = 0; /* Block descriptor length */ + + /* There are no block descriptors, only the following mode page: */ + + ret = usbstrg_modepage(priv, &buf[SCSIRESP_MODEPARAMETERHDR6_SIZEOF], modesense->pcpgcode, &mdlen); + if (ret == OK) + { + /* Store the mode data length and return the total message size */ + + mph->mdlen = mdlen - 1; + priv->nreqbytes = mdlen + SCSIRESP_MODEPARAMETERHDR6_SIZEOF; + } + } + } + return ret; +} + +/**************************************************************************** + * Name: usbstrg_cmdstartstopunit + * + * Description: + * Handle SCSI_CMD_STARTSTOPUNIT command + * + ****************************************************************************/ + +static inline int usbstrg_cmdstartstopunit(FAR struct usbstrg_dev_s *priv) +{ + int ret; + + priv->u.alloclen = 0; + ret = usbstrg_setupcmd(priv, SCSICMD_STARTSTOPUNIT_SIZEOF, USBSTRG_FLAGS_DIRNONE); + if (ret == OK) + { +#ifndef CONFIG_USBSTRG_REMOVABLE + /* This command is not valid if the media is not removable */ + + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_NOTREMOVABLE), 0); + lun->sd = SCSI_KCQIR_INVALIDCOMMAND; + ret = -EINVAL; +#endif + } + return ret; +} + +/**************************************************************************** + * Name: usbstrg_cmdpreventmediumremoval + * + * Description: + * Handle SCSI_CMD_PREVENTMEDIAREMOVAL command + * + ****************************************************************************/ + +static inline int usbstrg_cmdpreventmediumremoval(FAR struct usbstrg_dev_s *priv) +{ +#ifdef CONFIG_USBSTRG_REMOVABLE + FAR struct scsicmd_preventmediumremoval_s *pmr = (FAR struct scsicmd_preventmediumremoval_s *)priv->cdb; + FAR struct usbstrg_lun_s *lun = priv->lun; +#endif + int ret; + + priv->u.alloclen = 0; + ret = usbstrg_setupcmd(priv, SCSICMD_PREVENTMEDIUMREMOVAL_SIZEOF, USBSTRG_FLAGS_DIRNONE); + if (ret == OK) + { +#ifndef CONFIG_USBSTRG_REMOVABLE + lun->sd = SCSI_KCQIR_INVALIDCOMMAND; + ret = -EINVAL; +#else + if ((pmr->prevent & ~SCSICMD_PREVENTMEDIUMREMOVAL_TRANSPORT) != 0) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_PREVENTMEDIUMREMOVALPREVENT), 0); + lun->sd = SCSI_KCQIR_INVALIDFIELDINCBA; + ret = -EINVAL; + } + + lun->locked = pmr->prevent & SCSICMD_PREVENTMEDIUMREMOVAL_TRANSPORT; +#endif + } + return ret; +} + +/**************************************************************************** + * Name: usbstrg_cmdreadformatcapacity + * + * Description: + * Handle SCSI_CMD_READFORMATCAPACITIES command (MMC) + * + ****************************************************************************/ + +static inline int usbstrg_cmdreadformatcapacity(FAR struct usbstrg_dev_s *priv, + FAR uint8_t *buf) +{ + FAR struct scsicmd_readformatcapcacities_s *rfc = (FAR struct scsicmd_readformatcapcacities_s *)priv->cdb; + FAR struct scsiresp_readformatcapacities_s *hdr; + FAR struct usbstrg_lun_s *lun = priv->lun; + int ret; + + priv->u.alloclen = usbstrg_getbe16(rfc->alloclen); + ret = usbstrg_setupcmd(priv, SCSICMD_READFORMATCAPACITIES_SIZEOF, USBSTRG_FLAGS_DIRDEVICE2HOST); + if (ret == OK) + { + hdr = (FAR struct scsiresp_readformatcapacities_s *)buf; + memset(hdr, 0, SCSIRESP_READFORMATCAPACITIES_SIZEOF); + hdr->listlen = SCSIRESP_CURRCAPACITYDESC_SIZEOF; + + /* Only the Current/Maximum Capacity Descriptor follows the header */ + + usbstrg_putbe32(hdr->nblocks, lun->nsectors); + hdr->type = SCIRESP_RDFMTCAPACITIES_FORMATED; + usbstrg_putbe24(hdr->blocklen, lun->sectorsize); + priv->nreqbytes = SCSIRESP_READFORMATCAPACITIES_SIZEOF; + } + return ret; +} + +/**************************************************************************** + * Name: usbstrg_cmdreadcapacity10 + * + * Description: + * Handle SCSI_CMD_READCAPACITY10 command + * + ****************************************************************************/ + +static int inline usbstrg_cmdreadcapacity10(FAR struct usbstrg_dev_s *priv, + FAR uint8_t *buf) +{ + FAR struct scsicmd_readcapacity10_s *rcc = (FAR struct scsicmd_readcapacity10_s *)priv->cdb; + FAR struct scsiresp_readcapacity10_s *rcr = (FAR struct scsiresp_readcapacity10_s *)buf; + FAR struct usbstrg_lun_s *lun = priv->lun; + uint32_t lba; + int ret; + + priv->u.alloclen = SCSIRESP_READCAPACITY10_SIZEOF; /* Fake the allocation length */ + ret = usbstrg_setupcmd(priv, SCSICMD_READCAPACITY10_SIZEOF, USBSTRG_FLAGS_DIRDEVICE2HOST); + if (ret == OK) + { + /* Check the PMI and LBA fields */ + + lba = usbstrg_getbe32(rcc->lba); + + if (rcc->pmi > 1 || (rcc->pmi == 0 && lba != 0)) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_READCAPACITYFLAGS), 0); + lun->sd = SCSI_KCQIR_INVALIDFIELDINCBA; + ret = -EINVAL; + } + else + { + usbstrg_putbe32(rcr->lba, lun->nsectors - 1); + usbstrg_putbe32(&buf[4], lun->sectorsize); + priv->nreqbytes = SCSIRESP_READCAPACITY10_SIZEOF; + } + } + return ret; +} + +/**************************************************************************** + * Name: usbstrg_cmdread10 + * + * Description: + * Handle SCSI_CMD_READ10 command + * + ****************************************************************************/ + +static inline int usbstrg_cmdread10(FAR struct usbstrg_dev_s *priv) +{ + struct scsicmd_read10_s *read10 = (struct scsicmd_read10_s*)priv->cdb; + FAR struct usbstrg_lun_s *lun = priv->lun; + int ret; + + priv->u.xfrlen = usbstrg_getbe16(read10->xfrlen); + ret = usbstrg_setupcmd(priv, SCSICMD_READ10_SIZEOF, USBSTRG_FLAGS_DIRDEVICE2HOST|USBSTRG_FLAGS_BLOCKXFR); + if (ret == OK) + { + /* Get the Logical Block Address (LBA) from cdb[] as the starting sector */ + + priv->sector = usbstrg_getbe32(read10->lba); + + /* Verify that we can support this read command */ + + if ((read10->flags & ~(SCSICMD_READ10FLAGS_DPO|SCSICMD_READ10FLAGS_FUA)) != 0) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_READ10FLAGS), 0); + lun->sd = SCSI_KCQIR_INVALIDFIELDINCBA; + ret = -EINVAL; + } + + /* Verify that a block driver has been bound to the LUN */ + + else if (!lun->inode) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_READ10MEDIANOTPRESENT), 0); + lun->sd = SCSI_KCQNR_MEDIANOTPRESENT; + ret = -EINVAL; + } + + /* Verify that LBA lies in the range supported by the block driver */ + + else if (priv->sector >= lun->nsectors) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_READ10LBARANGE), 0); + lun->sd = SCSI_KCQIR_LBAOUTOFRANGE; + ret = -EINVAL; + } + + /* Looks like we are good to go */ + + else + { + usbtrace(TRACE_CLASSSTATE(USBSTRG_CLASSSTATE_CMDPARSECMDREAD10), priv->cdb[0]); + priv->thstate = USBSTRG_STATE_CMDREAD; + } + } + + return ret; +} + +/**************************************************************************** + * Name: usbstrg_cmdwrite10 + * + * Description: + * Handle SCSI_CMD_WRITE10 command + * + ****************************************************************************/ + +static inline int usbstrg_cmdwrite10(FAR struct usbstrg_dev_s *priv) +{ + struct scsicmd_write10_s *write10 = (struct scsicmd_write10_s *)priv->cdb; + FAR struct usbstrg_lun_s *lun = priv->lun; + int ret; + + priv->u.xfrlen = usbstrg_getbe16(write10->xfrlen); + ret = usbstrg_setupcmd(priv, SCSICMD_WRITE10_SIZEOF, USBSTRG_FLAGS_DIRHOST2DEVICE|USBSTRG_FLAGS_BLOCKXFR); + if (ret == OK) + { + /* Get the Logical Block Address (LBA) from cdb[] as the starting sector */ + + priv->sector = usbstrg_getbe32(write10->lba); + + /* Verify that we can support this write command */ + + if ((write10->flags & ~(SCSICMD_WRITE10FLAGS_DPO|SCSICMD_WRITE10FLAGS_FUA)) != 0) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_WRITE10FLAGS), 0); + lun->sd = SCSI_KCQIR_INVALIDFIELDINCBA; + ret = -EINVAL; + } + + /* Verify that a block driver has been bound to the LUN */ + + else if (!lun->inode) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_WRITE10MEDIANOTPRESENT), 0); + lun->sd = SCSI_KCQNR_MEDIANOTPRESENT; + ret = -EINVAL; + } + + /* Check for attempts to write to a read-only device */ + + else if (lun->readonly) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_WRITE10READONLY), 0); + lun->sd = SCSI_KCQWP_COMMANDNOTALLOWED; + ret = -EINVAL; + } + + /* Verify that LBA lies in the range supported by the block driver */ + + else if (priv->sector >= lun->nsectors) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_WRITE10LBARANGE), 0); + lun->sd = SCSI_KCQIR_LBAOUTOFRANGE; + ret = -EINVAL; + } + + /* Looks like we are good to go */ + + else + { + usbtrace(TRACE_CLASSSTATE(USBSTRG_CLASSSTATE_CMDPARSECMDWRITE10), priv->cdb[0]); + priv->thstate = USBSTRG_STATE_CMDWRITE; + } + } + return ret; +} + +/**************************************************************************** + * Name: usbstrg_cmdverify10 + * + * Description: + * Handle SCSI_CMD_VERIFY10 command + * + ****************************************************************************/ + +static inline int usbstrg_cmdverify10(FAR struct usbstrg_dev_s *priv) +{ + FAR struct scsicmd_verify10_s *verf = (FAR struct scsicmd_verify10_s *)priv->cdb; + FAR struct usbstrg_lun_s *lun = priv->lun; + uint32_t lba; + uint16_t blocks; + size_t sector; + ssize_t nread; + int ret; + int i; + + priv->u.alloclen = 0; + ret = usbstrg_setupcmd(priv, SCSICMD_VERIFY10_SIZEOF, USBSTRG_FLAGS_DIRNONE); + if (ret == OK) + { + /* Verify the starting and ending LBA */ + + lba = usbstrg_getbe32(verf->lba); + blocks = usbstrg_getbe16(verf->len); + + if ((verf->flags & ~SCSICMD_VERIFY10_DPO) != 0 || verf->groupno != 0) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_VERIFY10FLAGS), 0); + lun->sd = SCSI_KCQIR_INVALIDFIELDINCBA; + ret = -EINVAL; + } + else if (blocks == 0) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_VERIFY10NOBLOCKS), 0); + ret = -EIO; /* No reply */ + } + + /* Verify that a block driver has been bound to the LUN */ + + else if (!lun->inode) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_VERIFY10MEDIANOTPRESENT), 0); + lun->sd = SCSI_KCQNR_MEDIANOTPRESENT; + ret = -EINVAL; + } + + /* Verify that LBA lies in the range supported by the block driver */ + + else if (lba >= lun->nsectors || lba + blocks > lun->nsectors) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_VERIFY10LBARANGE), 0); + lun->sd = SCSI_KCQIR_LBAOUTOFRANGE; + ret = -EINVAL; + } + else + { + /* Try to read the requested blocks */ + + for (i = 0, sector = lba + lun->startsector; i < blocks; i++, sector++) + { + nread = USBSTRG_DRVR_READ(lun, priv->iobuffer, sector, 1); + if (nread < 0) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_VERIFY10READFAIL), i); + lun->sd = SCSI_KCQME_UNRRE1; + lun->sdinfo = sector; + ret = -EIO; + break; + } + } + } + } + return ret; +} + +/**************************************************************************** + * Name: usbstrg_cmdsynchronizecache10 + * + * Description: + * Handle SCSI_CMD_SYNCHCACHE10 command + * + ****************************************************************************/ + +static inline int usbstrg_cmdsynchronizecache10(FAR struct usbstrg_dev_s *priv) +{ + int ret; + + priv->u.alloclen = 0; + + /* Verify that we have the LUN structure and the block driver has been bound */ + + if (!priv->lun->inode) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_SYNCCACHEMEDIANOTPRESENT), 0); + priv->lun->sd = SCSI_KCQNR_MEDIANOTPRESENT; + ret = -EINVAL; + } + else + { + ret = usbstrg_setupcmd(priv, SCSICMD_SYNCHRONIZECACHE10_SIZEOF, USBSTRG_FLAGS_DIRNONE); + } + return ret; +} + +/**************************************************************************** + * Name: usbstrg_cmdmodeselect10 + * + * Description: + * Handle SCSI_CMD_MODESELECT10 command + * + ****************************************************************************/ + +static inline int usbstrg_cmdmodeselect10(FAR struct usbstrg_dev_s *priv) +{ + FAR struct scsicmd_modeselect10_s *modeselect = (FAR struct scsicmd_modeselect10_s *)priv->cdb; + + priv->u.alloclen = usbstrg_getbe16(modeselect->parmlen); + (void)usbstrg_setupcmd(priv, SCSICMD_MODESELECT10_SIZEOF, USBSTRG_FLAGS_DIRHOST2DEVICE); + + /* Not supported */ + + priv->lun->sd = SCSI_KCQIR_INVALIDCOMMAND; + return -EINVAL; +} + +/**************************************************************************** + * Name: usbstrg_cmdmodesense10 + * + * Description: + * Handle SCSI_CMD_MODESENSE10 command + * + ****************************************************************************/ + +static int inline usbstrg_cmdmodesense10(FAR struct usbstrg_dev_s *priv, + FAR uint8_t *buf) +{ + FAR struct scsicmd_modesense10_s *modesense = (FAR struct scsicmd_modesense10_s *)priv->cdb; + FAR struct scsiresp_modeparameterhdr10_s *mph = (FAR struct scsiresp_modeparameterhdr10_s *)buf; + int mdlen; + int ret; + + priv->u.alloclen = usbstrg_getbe16(modesense->alloclen); + ret = usbstrg_setupcmd(priv, SCSICMD_MODESENSE10_SIZEOF, USBSTRG_FLAGS_DIRDEVICE2HOST); + if (ret == OK) + { + if ((modesense->flags & ~SCSICMD_MODESENSE10_DBD) != 0 || modesense->subpgcode != 0) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_MODESENSE10FLAGS), 0); + priv->lun->sd = SCSI_KCQIR_INVALIDFIELDINCBA; + ret = -EINVAL; + } + else + { + /* The response consists of: + * + * (1) A MODESENSE6-specific mode parameter header, + * (2) A variable length list of block descriptors, and + * (3) A variable lengtth list of mode page formats + */ + + memset(mph, 0, SCSIRESP_MODEPARAMETERHDR10_SIZEOF); + mph->param = (priv->lun->readonly ? SCSIRESP_MODEPARMHDR_DAPARM_WP : 0x00); + + /* There are no block descriptors, only the following mode page: */ + + ret = usbstrg_modepage(priv, &buf[SCSIRESP_MODEPARAMETERHDR10_SIZEOF], modesense->pcpgcode, &mdlen); + if (ret == OK) + { + /* Store the mode data length and return the total message size */ + + usbstrg_putbe16(mph->mdlen, mdlen - 2); + priv->nreqbytes = mdlen + SCSIRESP_MODEPARAMETERHDR10_SIZEOF; + } + } + } + return ret; +} + +/**************************************************************************** + * Name: usbstrg_cmdread12 + * + * Description: + * Handle SCSI_CMD_READ12 command + * + ****************************************************************************/ + +static inline int usbstrg_cmdread12(FAR struct usbstrg_dev_s *priv) +{ + struct scsicmd_read12_s *read12 = (struct scsicmd_read12_s*)priv->cdb; + FAR struct usbstrg_lun_s *lun = priv->lun; + int ret; + + priv->u.xfrlen = usbstrg_getbe32(read12->xfrlen); + ret = usbstrg_setupcmd(priv, SCSICMD_READ12_SIZEOF, USBSTRG_FLAGS_DIRDEVICE2HOST|USBSTRG_FLAGS_BLOCKXFR); + if (ret == OK) + { + /* Get the Logical Block Address (LBA) from cdb[] as the starting sector */ + + priv->sector = usbstrg_getbe32(read12->lba); + + /* Verify that we can support this read command */ + + if ((read12->flags & ~(SCSICMD_READ12FLAGS_DPO|SCSICMD_READ12FLAGS_FUA)) != 0) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_READ12FLAGS), 0); + lun->sd = SCSI_KCQIR_INVALIDFIELDINCBA; + ret = -EINVAL; + } + + /* Verify that a block driver has been bound to the LUN */ + + else if (!lun->inode) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_READ12MEDIANOTPRESENT), 0); + lun->sd = SCSI_KCQNR_MEDIANOTPRESENT; + ret = -EINVAL; + } + + /* Verify that LBA lies in the range supported by the block driver */ + + else if (priv->sector >= lun->nsectors) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_READ12LBARANGE), 0); + lun->sd = SCSI_KCQIR_LBAOUTOFRANGE; + ret = -EINVAL; + } + + /* Looks like we are good to go */ + + else + { + usbtrace(TRACE_CLASSSTATE(USBSTRG_CLASSSTATE_CMDPARSECMDREAD12), priv->cdb[0]); + priv->thstate = USBSTRG_STATE_CMDREAD; + } + } + return ret; +} + +/**************************************************************************** + * Name: usbstrg_cmdwrite12 + * + * Description: + * Handle SCSI_CMD_WRITE12 command + * + ****************************************************************************/ + +static inline int usbstrg_cmdwrite12(FAR struct usbstrg_dev_s *priv) +{ + struct scsicmd_write12_s *write12 = (struct scsicmd_write12_s *)priv->cdb; + FAR struct usbstrg_lun_s *lun = priv->lun; + int ret; + + priv->u.xfrlen = usbstrg_getbe32(write12->xfrlen); + ret = usbstrg_setupcmd(priv, SCSICMD_WRITE12_SIZEOF, USBSTRG_FLAGS_DIRHOST2DEVICE|USBSTRG_FLAGS_BLOCKXFR); + if (ret == OK) + { + /* Get the Logical Block Address (LBA) from cdb[] as the starting sector */ + + priv->sector = usbstrg_getbe32(write12->lba); + + /* Verify that we can support this write command */ + + if ((write12->flags & ~(SCSICMD_WRITE12FLAGS_DPO|SCSICMD_WRITE12FLAGS_FUA)) != 0) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_WRITE12FLAGS), 0); + lun->sd = SCSI_KCQIR_INVALIDFIELDINCBA; + } + + /* Verify that a block driver has been bound to the LUN */ + + else if (!lun->inode) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_WRITE12MEDIANOTPRESENT), 0); + lun->sd = SCSI_KCQNR_MEDIANOTPRESENT; + ret = -EINVAL; + } + + /* Check for attempts to write to a read-only device */ + + else if (lun->readonly) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_WRITE12READONLY), 0); + lun->sd = SCSI_KCQWP_COMMANDNOTALLOWED; + } + + /* Verify that LBA lies in the range supported by the block driver */ + + else if (priv->sector >= lun->nsectors) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_WRITE12LBARANGE), 0); + lun->sd = SCSI_KCQIR_LBAOUTOFRANGE; + } + + /* Looks like we are good to go */ + + else + { + usbtrace(TRACE_CLASSSTATE(USBSTRG_CLASSSTATE_CMDPARSECMDWRITE12), priv->cdb[0]); + priv->thstate = USBSTRG_STATE_CMDWRITE; + return OK; + } + } + + return ret; +} + +/**************************************************************************** + * Name: usbstrg_setupcmd + * + * Description: + * Called after each SCSI command is identified in order to perform setup + * and verification operations that are common to all SCSI commands. This + * function performs the following common setup operations: + * + * 1. Determine the direction of the response + * 2. Verify lengths + * 3. Setup and verify the LUN + * + * Includes special logic for INQUIRY and REQUESTSENSE commands + * + ****************************************************************************/ + +static int inline usbstrg_setupcmd(FAR struct usbstrg_dev_s *priv, uint8_t cdblen, uint8_t flags) +{ + FAR struct usbstrg_lun_s *lun = NULL; + uint32_t datlen; + uint8_t dir = flags & USBSTRG_FLAGS_DIRMASK; + int ret = OK; + + /* Verify the LUN and set up the current LUN reference in the + * device structure + */ + + if (priv->cbwlun < priv->nluns) + { + /* LUN number is valid in a valid range, but the LUN is not necessarily + * bound to a block driver. That will be checked as necessary in each + * individual command. + */ + + lun = &priv->luntab[priv->cbwlun]; + priv->lun = lun; + } + + /* Only a few commands may specify unsupported LUNs */ + + else if ((flags & USBSTRG_FLAGS_LUNNOTNEEDED) == 0) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_CMDBADLUN), priv->cbwlun); + ret = -EINVAL; + } + + /* Extract the direction and data transfer length */ + + dir = flags & USBSTRG_FLAGS_DIRMASK; /* Expected data direction */ + datlen = 0; + if ((flags & USBSTRG_FLAGS_BLOCKXFR) == 0) + { + /* Non-block transfer. Data length: Host allocation to receive data + * (only for device-to-host transfers. At present, alloclen is set + * to zero for all host-to-device, non-block transfers. + */ + + datlen = priv->u.alloclen; + } + else if (lun) + { + /* Block transfer: Calculate the total size of all sectors to be transferred */ + + datlen = priv->u.alloclen * lun->sectorsize; + } + + /* Check the data direction. This was set up at the end of the + * IDLE state when the CBW was parsed, but if there is no data, + * then the direction is none. + */ + + if (datlen == 0) + { + /* No data.. then direction is none */ + + dir = USBSTRG_FLAGS_DIRNONE; + } + + /* Compare the expected data length in the command to the data length + * in the CBW. + */ + + else if (priv->cbwlen < datlen) + { + /* Clip to the length in the CBW and declare a phase error */ + + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_PHASEERROR1), priv->cdb[0]); + if ((flags & USBSTRG_FLAGS_BLOCKXFR) != 0) + { + priv->u.alloclen = priv->cbwlen; + } + else + { + priv->u.xfrlen = priv->cbwlen / lun->sectorsize; + } + + priv->phaseerror = 1; + } + + /* Initialize the residue */ + + priv->residue = priv->cbwlen; + + /* Conflicting data directions is a phase error */ + + if (priv->cbwdir != dir && datlen > 0) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_PHASEERROR2), priv->cdb[0]); + priv->phaseerror = 1; + ret = -EINVAL; + } + + /* Compare the length of data in the cdb[] with the expected length + * of the command. + */ + + if (cdblen != priv->cdblen) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_PHASEERROR3), priv->cdb[0]); + priv->phaseerror = 1; + ret = -EINVAL; + } + + if (lun) + { + /* Retain the sense data for the REQUEST SENSE command */ + + if ((flags & USBSTRG_FLAGS_RETAINSENSEDATA) == 0) + { + /* Discard the sense data */ + + lun->sd = SCSI_KCQ_NOSENSE; + lun->sdinfo = 0; + } + + /* If a unit attention condition exists, then only a restricted set of + * commands is permitted. + */ + + if (lun->uad != SCSI_KCQ_NOSENSE && (flags & USBSTRG_FLAGS_UACOKAY) != 0) + { + /* Command not permitted */ + + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_CMDUNEVIOLATION), priv->cbwlun); + lun->sd = lun->uad; + lun->uad = SCSI_KCQ_NOSENSE; + ret = -EINVAL; + } + } + + /* The final, 1-byte field of every SCSI command is the Control field which + * must be zero + */ + + if (priv->cdb[cdblen-1] != 0) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_SCSICMDCONTROL), 0); + if (lun) + { + lun->sd = SCSI_KCQIR_INVALIDFIELDINCBA; + } + ret = -EINVAL; + } + return ret; +} + +/**************************************************************************** + * Name: usbstrg_idlestate + * + * Description: + * Called from the worker thread in the USBSTRG_STATE_IDLE state. Checks + * for the receipt of a bulk CBW. + * + * Returned value: + * If no new, valid CBW is available, this function returns a negated errno. + * Otherwise, when a new CBW is successfully parsed, this function sets + * priv->thstate to USBSTRG_STATE_CMDPARSE and returns OK. + * + ****************************************************************************/ + +static int usbstrg_idlestate(FAR struct usbstrg_dev_s *priv) +{ + FAR struct usbstrg_req_s *privreq; + FAR struct usbdev_req_s *req; + FAR struct usbstrg_cbw_s *cbw; + irqstate_t flags; + int ret = -EINVAL; + + /* Take a request from the rdreqlist */ + + flags = irqsave(); + privreq = (FAR struct usbstrg_req_s *)sq_remfirst(&priv->rdreqlist); + irqrestore(flags); + + /* Has anything been received? If not, just return an error. + * This will cause us to remain in the IDLE state. When a USB request is + * received, the worker thread will be awakened in the USBSTRG_STATE_IDLE + * and we will be called again. + */ + + if (!privreq) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_IDLERDREQLISTEMPTY), 0); + return -ENOMEM; + } + + req = privreq->req; + cbw = (FAR struct usbstrg_cbw_s *)req->buf; + + /* Handle the CBW */ + + usbstrg_dumpdata("SCSCI CBW", (uint8_t*)cbw, USBSTRG_CBW_SIZEOF - USBSTRG_MAXCDBLEN); + usbstrg_dumpdata(" CDB", cbw->cdb, min(cbw->cdblen, USBSTRG_MAXCDBLEN)); + + /* Check for properly formatted CBW? */ + + if (req->xfrd != USBSTRG_CBW_SIZEOF || + cbw->signature[0] != 'U' || + cbw->signature[1] != 'S' || + cbw->signature[2] != 'B' || + cbw->signature[3] != 'C') + { + /* CBS BAD: Stall the bulk endpoints. If the CBW is bad we must stall the + * bulk IN endpoint and either (1) stall the bulk OUT endpoint, or + * (2) discard data from the endpoint. + */ + + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_INVALIDCBWSIGNATURE), 0); + EP_STALL(priv->epbulkout); + EP_STALL(priv->epbulkin); + } + + /* Is the CBW meaningful? */ + + else if (cbw->lun >= priv->nluns || (cbw->flags & ~USBSTRG_CBWFLAG_IN) != 0 || + cbw->cdblen < 6 || cbw->cdblen > USBSTRG_MAXCDBLEN) + { + /* CBS BAD: Stall the bulk endpoints. If the CBW is bad we must stall the + * bulk IN endpoint and either (1) stall the bulk OUT endpoint, or + * (2) discard data from the endpoint. + */ + + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_INVALIDCBWCONTENT), 0); + EP_STALL(priv->epbulkout); + EP_STALL(priv->epbulkin); + } + + /* Save the information from the CBW */ + + else + { + /* Extract the CDB and copy the whole CBD[] for later use */ + + priv->cdblen = cbw->cdblen; + memcpy(priv->cdb, cbw->cdb, priv->cdblen); + + /* Extract the data direction */ + + if ((cbw->flags & USBSTRG_CBWFLAG_IN) != 0) + { + /* IN: Device-to-host */ + + priv->cbwdir = USBSTRG_FLAGS_DIRDEVICE2HOST; + } + else + { + /* OUT: Host-to-device */ + + priv->cbwdir = USBSTRG_FLAGS_DIRHOST2DEVICE; + } + + /* Get the max size of the data response */ + + priv->cbwlen = usbstrg_getle32(cbw->datlen); + if (priv->cbwlen == 0) + { + /* No length? Then no direction either */ + + priv->cbwdir = USBSTRG_FLAGS_DIRNONE; + } + + /* Extract and save the LUN index and TAG value */ + + priv->cbwlun = cbw->lun; + priv->cbwtag = usbstrg_getle32(cbw->tag); + + /* Return the read request to the bulk out endpoint for re-filling */ + + req = privreq->req; + req->len = CONFIG_USBSTRG_BULKOUTREQLEN; + req->priv = privreq; + req->callback = usbstrg_rdcomplete; + + if (EP_SUBMIT(priv->epbulkout, req) != OK) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_IDLERDSUBMIT), (uint16_t)-ret); + } + + /* Change to the CMDPARSE state and return success */ + + usbtrace(TRACE_CLASSSTATE(USBSTRG_CLASSSTATE_IDLECMDPARSE), priv->cdb[0]); + priv->thstate = USBSTRG_STATE_CMDPARSE; + ret = OK; + } + + return ret; +} + +/**************************************************************************** + * Name: usbstrg_cmdparsestate + * + * Description: + * Called from the worker thread in the USBSTRG_STATE_CMDPARSE state. + * This state is entered when usbstrg_idlestate obtains a valid CBW + * containing SCSI commands. This function processes those SCSI commands. + * + * Returned value: + * If no write request is available or certain other errors occur, this + * function returns a negated errno and stays in the USBSTRG_STATE_CMDPARSE + * state. Otherwise, when the new CBW is successfully process, this + * function sets priv->thstate to one of USBSTRG_STATE_CMDREAD, + * USBSTRG_STATE_CMDWRITE, or USBSTRG_STATE_CMDFINISH and returns OK. + * + ****************************************************************************/ + +static int usbstrg_cmdparsestate(FAR struct usbstrg_dev_s *priv) +{ + FAR struct usbstrg_req_s *privreq; + FAR uint8_t *buf; + int ret = -EINVAL; + + usbstrg_dumpdata("SCSCI CDB", priv->cdb, priv->cdblen); + + /* Check if there is a request in the wrreqlist that we will be able to + * use for data or status. + */ + + privreq = (FAR struct usbstrg_req_s *)sq_peek(&priv->wrreqlist); + + /* If there no request structures available, then just return an error. + * This will cause us to remain in the CMDPARSE state. When a request is + * returned, the worker thread will be awakened in the USBSTRG_STATE_CMDPARSE + * and we will be called again. + */ + + if (!privreq) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_CMDPARSEWRREQLISTEMPTY), 0); + return -ENOMEM; + } + DEBUGASSERT(privreq->req && privreq->req->buf); + buf = privreq->req->buf; + + /* Assume that no errors will be encountered */ + + priv->phaseerror = 0; + priv->shortpacket = 0; + + /* No data is buffered */ + + priv->nsectbytes = 0; + priv->nreqbytes = 0; + + /* Get exclusive access to the block driver */ + + pthread_mutex_lock(&priv->mutex); + switch (priv->cdb[0]) + { + case SCSI_CMD_TESTUNITREADY: /* 0x00 Mandatory */ + ret = usbstrg_cmdtestunitready(priv); + break; + + /* case SCSI_CMD_REZEROUNIT: * 0x01 Obsolete + * * 0x02 Vendor-specific */ + + case SCSI_CMD_REQUESTSENSE: /* 0x03 Mandatory */ + ret = usbstrg_cmdrequestsense(priv, buf); + break; + + /* case SCSI_CMD_FORMAT_UNIT: * 0x04 Mandatory, but not implemented + * * 0x05 Vendor specific + * * 0x06 Vendor specific + * case SCSI_CMD_REASSIGNBLOCKS: * 0x07 Optional */ + + case SCSI_CMD_READ6: /* 0x08 Mandatory */ + ret = usbstrg_cmdread6(priv); + break; + + /* * 0x09 Vendor specific */ + + case SCSI_CMD_WRITE6: /* 0x0a Optional */ + ret = usbstrg_cmdwrite6(priv); + break; + + /* case SCSI_CMD_SEEK6: * 0x0b Obsolete + * * 0x0c-0x10 Vendor specific + * case SCSI_CMD_SPACE6: * 0x11 Vendor specific */ + + case SCSI_CMD_INQUIRY: /* 0x12 Mandatory */ + ret = usbstrg_cmdinquiry(priv, buf); + break; + + /* * 0x13-0x14 Vendor specific */ + + case SCSI_CMD_MODESELECT6: /* 0x15 Optional */ + ret = usbstrg_cmdmodeselect6(priv); + break; + + /* case SCSI_CMD_RESERVE6: * 0x16 Obsolete + * case SCSI_CMD_RELEASE6: * 0x17 Obsolete + * case SCSI_CMD_COPY: * 0x18 Obsolete + * * 0x19 Vendor specific */ + + case SCSI_CMD_MODESENSE6: /* 0x1a Optional */ + ret = usbstrg_cmdmodesense6(priv, buf); + break; + + case SCSI_CMD_STARTSTOPUNIT: /* 0x1b Optional */ + ret = usbstrg_cmdstartstopunit(priv); + break; + + /* case SCSI_CMD_RECEIVEDIAGNOSTICRESULTS: * 0x1c Optional + * case SCSI_CMD_SENDDIAGNOSTIC: * 0x1d Mandatory, but not implemented */ + + case SCSI_CMD_PREVENTMEDIAREMOVAL: /* 0x1e Optional */ + ret = usbstrg_cmdpreventmediumremoval(priv); + break; + + /* * 0x20-22 Vendor specific */ + case SCSI_CMD_READFORMATCAPACITIES: /* 0x23 Vendor-specific (defined in MMC spec) */ + ret = usbstrg_cmdreadformatcapacity(priv, buf); + break; + /* * 0x24 Vendor specific */ + + case SCSI_CMD_READCAPACITY10: /* 0x25 Mandatory */ + ret = usbstrg_cmdreadcapacity10(priv, buf); + break; + + /* * 0x26-27 Vendor specific */ + case SCSI_CMD_READ10: /* 0x28 Mandatory */ + ret = usbstrg_cmdread10(priv); + break; + + /* * 0x29 Vendor specific */ + + case SCSI_CMD_WRITE10: /* 0x2a Optional */ + ret = usbstrg_cmdwrite10(priv); + break; + + /* case SCSI_CMD_SEEK10: * 0x2b Obsolete + * * 0x2c-2d Vendor specific + * case SCSI_CMD_WRITEANDVERIFY: * 0x2e Optional */ + + case SCSI_CMD_VERIFY10: /* 0x2f Optional, but needed my MS Windows */ + ret = usbstrg_cmdverify10(priv); + break; + + /* case SCSI_CMD_SEARCHDATAHIGH: * 0x30 Obsolete + * case SCSI_CMD_SEARCHDATAEQUAL: * 0x31 Obsolete + * case SCSI_CMD_SEARCHDATALOW: * 0x32 Obsolete + * case SCSI_CMD_SETLIMITS10: * 0x33 Obsolete + * case SCSI_CMD_PREFETCH10: * 0x34 Optional */ + + case SCSI_CMD_SYNCHCACHE10: /* 0x35 Optional */ + ret = usbstrg_cmdsynchronizecache10(priv); + break; + + /* case SCSI_CMD_LOCKCACHE: * 0x36 Obsolete + * case SCSI_CMD_READDEFECTDATA10: * 0x37 Optional + * case SCSI_CMD_COMPARE: * 0x39 Obsolete + * case SCSI_CMD_COPYANDVERIFY: * 0x3a Obsolete + * case SCSI_CMD_WRITEBUFFER: * 0x3b Optional + * case SCSI_CMD_READBUFFER: * 0x3c Optional + * case SCSI_CMD_READLONG10: * 0x3e Optional + * case SCSI_CMD_WRITELONG10: * 0x3f Optional + * case SCSI_CMD_CHANGEDEFINITION: * 0x40 Obsolete + * case SCSI_CMD_WRITESAME10: * 0x41 Optional + * case SCSI_CMD_LOGSELECT: * 0x4c Optional + * case SCSI_CMD_LOGSENSE: * 0x4d Optional + * case SCSI_CMD_XDWRITE10: * 0x50 Optional + * case SCSI_CMD_XPWRITE10: * 0x51 Optional + * case SCSI_CMD_XDREAD10: * 0x52 Optional */ + + case SCSI_CMD_MODESELECT10: /* 0x55 Optional */ + ret = usbstrg_cmdmodeselect10(priv); + break; + + /* case SCSI_CMD_RESERVE10: * 0x56 Obsolete + * case SCSI_CMD_RELEASE10: * 0x57 Obsolete */ + + case SCSI_CMD_MODESENSE10: /* 0x5a Optional */ + ret = usbstrg_cmdmodesense10(priv, buf); + break; + + /* case SCSI_CMD_PERSISTENTRESERVEIN: * 0x5e Optional + * case SCSI_CMD_PERSISTENTRESERVEOUT: * 0x5f Optional + * case SCSI_CMD_32: * 0x7f Optional + * case SCSI_CMD_XDWRITEEXTENDED: * 0x80 Obsolete + * case SCSI_CMD_REBUILD: * 0x81 Obsolete + * case SCSI_CMD_REGENERATE: * 0x82 Obsolete + * case SCSI_CMD_EXTENDEDCOPY: * 0x83 Optional + * case SCSI_CMD_COPYRESULTS: * 0x84 Optional + * case SCSI_CMD_ACCESSCONTROLIN: * 0x86 Optional + * case SCSI_CMD_ACCESSCONTROLOUT: * 0x87 Optional + * case SCSI_CMD_READ16: * 0x88 Optional + * case SCSI_CMD_WRITE16: * 0x8a Optional + * case SCSI_CMD_READATTRIBUTE: * 0x8c Optional + * case SCSI_CMD_WRITEATTRIBUTE: * 0x8d Optional + * case SCSI_CMD_WRITEANDVERIFY16: * 0x8e Optional + * case SCSI_CMD_SYNCHCACHE16: * 0x91 Optional + * case SCSI_CMD_LOCKUNLOCKACACHE: * 0x92 Optional + * case SCSI_CMD_WRITESAME16: * 0x93 Optional + * case SCSI_CMD_READCAPACITY16: * 0x9e Optional + * case SCSI_CMD_READLONG16: * 0x9e Optional + * case SCSI_CMD_WRITELONG16 * 0x9f Optional + * case SCSI_CMD_REPORTLUNS: * 0xa0 Mandatory, but not implemented + * case SCSI_CMD_MAINTENANCEIN: * 0xa3 Optional (SCCS==0) + * case SCSI_CMD_MAINTENANCEOUT: * 0xa4 Optional (SCCS==0) + * case SCSI_CMD_MOVEMEDIUM: * 0xa5 ? + * case SCSI_CMD_MOVEMEDIUMATTACHED: * 0xa7 Optional (MCHNGR==0) */ + + case SCSI_CMD_READ12: /* 0xa8 Optional */ + ret = usbstrg_cmdread12(priv); + break; + + case SCSI_CMD_WRITE12: /* 0xaa Optional */ + ret = usbstrg_cmdwrite12(priv); + break; + + /* case SCSI_CMD_READMEDIASERIALNUMBER: * 0xab Optional + * case SCSI_CMD_WRITEANDVERIFY12: * 0xae Optional + * case SCSI_CMD_VERIFY12: * 0xaf Optional + * case SCSI_CMD_SETLIMITS12 * 0xb3 Obsolete + * case SCSI_CMD_READELEMENTSTATUS: * 0xb4 Optional (MCHNGR==0) + * case SCSI_CMD_READDEFECTDATA12: * 0xb7 Optional + * case SCSI_CMD_REDUNDANCYGROUPIN: * 0xba Optional + * case SCSI_CMD_REDUNDANCYGROUPOUT: * 0xbb Optional + * case SCSI_CMD_SPAREIN: * 0xbc Optional (SCCS==0) + * case SCSI_CMD_SPAREOUT: * 0xbd Optional (SCCS==0) + * case SCSI_CMD_VOLUMESETIN: * 0xbe Optional (SCCS==0) + * case SCSI_CMD_VOLUMESETOUT: * 0xbe Optional (SCCS==0) + * * 0xc0-0xff Vendor specific */ + + default: + priv->u.alloclen = 0; + if (ret == OK) + { + priv->lun->sd = SCSI_KCQIR_INVALIDCOMMAND; + ret = -EINVAL; + } + break; + } + pthread_mutex_unlock(&priv->mutex); + + /* Is a response required? (Not for read6/10/12 and write6/10/12). */ + + if (priv->thstate == USBSTRG_STATE_CMDPARSE) + { + /* All commands come through this path (EXCEPT read6/10/12 and write6/10/12). + * For all other commands, the following setup is expected for the response + * based on data direction: + * + * For direction NONE: + * 1. priv->u.alloclen == 0 + * 2. priv->nreqbytes == 0 + * + * For direction = device-to-host: + * 1. priv->u.alloclen == allocation length; space set aside by the + * host to receive the device data. The size of the response + * cannot exceed this value. + * 2. response data is in the request currently at the head of + * the priv->wrreqlist queue. priv->nreqbytes is set to the length + * of data in the response. + * + * For direction host-to-device + * At present, there are no supported commands that should have host-to-device + * transfers (except write6/10/12 and that command logic does not take this + * path. The 'residue' is left at the full host-to-device data size. + * + * For all: + * ret set to <0 if an error occurred in parsing the commands. + */ + + /* For from device-to-hose operations, the residue is the expected size + * (the initial value of 'residue') minus the amount actually returned + * in the response: + */ + + if (priv->cbwdir == USBSTRG_FLAGS_DIRDEVICE2HOST) + { + /* The number of bytes in the response cannot exceed the host + * 'allocation length' in the command. + */ + + if (priv->nreqbytes > priv->u.alloclen) + { + priv->nreqbytes = priv->u.alloclen; + } + + /* The residue is then the number of bytes that were not sent */ + + priv->residue -= priv->nreqbytes; + } + + /* On return, we need the following: + * + * 1. Setup for CMDFINISH state if appropriate + * 2. priv->thstate set to either CMDPARSE if no buffer was available or + * CMDFINISH to send the response + * 3. Return OK to continue; <0 to wait for the next event + */ + + usbtrace(TRACE_CLASSSTATE(USBSTRG_CLASSSTATE_CMDPARSECMDFINISH), priv->cdb[0]); + priv->thstate = USBSTRG_STATE_CMDFINISH; + ret = OK; + } + return ret; +} + +/**************************************************************************** + * Name: usbstrg_cmdreadstate + * + * Description: + * Called from the worker thread in the USBSTRG_STATE_CMDREAD state. + * The USBSTRG_STATE_CMDREAD state is entered when usbstrg_cmdparsestate + * obtains a valid SCSI read command. This state is really a continuation + * of the USBSTRG_STATE_CMDPARSE state that handles extended SCSI read + * command handling. + * + * Returned value: + * If no USBDEV write request is available or certain other errors occur, this + * function returns a negated errno and stays in the USBSTRG_STATE_CMDREAD + * state. Otherwise, when the new SCSI read command is fully processed, + * this function sets priv->thstate to USBSTRG_STATE_CMDFINISH and returns OK. + * + * State variables: + * xfrlen - holds the number of sectors read to be read. + * sector - holds the sector number of the next sector to be read + * nsectbytes - holds the number of bytes buffered for the current sector + * nreqbytes - holds the number of bytes currently buffered in the request + * at the head of the wrreqlist. + * + ****************************************************************************/ + +static int usbstrg_cmdreadstate(FAR struct usbstrg_dev_s *priv) +{ + FAR struct usbstrg_lun_s *lun = priv->lun; + FAR struct usbstrg_req_s *privreq; + FAR struct usbdev_req_s *req; + irqstate_t flags; + ssize_t nread; + uint8_t *src; + uint8_t *dest; + int nbytes; + int ret; + + /* Loop transferring data until either (1) all of the data has been + * transferred, or (2) we have used up all of the write requests that we have + * available. + */ + + while (priv->u.xfrlen > 0 || priv->nsectbytes > 0) + { + usbtrace(TRACE_CLASSSTATE(USBSTRG_CLASSSTATE_CMDREAD), priv->u.xfrlen); + + /* Is the I/O buffer empty? */ + + if (priv->nsectbytes <= 0) + { + /* Yes.. read the next sector */ + + nread = USBSTRG_DRVR_READ(lun, priv->iobuffer, priv->sector, 1); + if (nread < 0) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_CMDREADREADFAIL), -nread); + lun->sd = SCSI_KCQME_UNRRE1; + lun->sdinfo = priv->sector; + break; + } + + priv->nsectbytes = lun->sectorsize; + priv->u.xfrlen--; + priv->sector++; + } + + /* Check if there is a request in the wrreqlist that we will be able to + * use for data transfer. + */ + + privreq = (FAR struct usbstrg_req_s *)sq_peek(&priv->wrreqlist); + + /* If there no request structures available, then just return an error. + * This will cause us to remain in the CMDREAD state. When a request is + * returned, the worker thread will be awakened in the USBSTRG_STATE_CMDREAD + * and we will be called again. + */ + + if (!privreq) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_CMDREADWRRQEMPTY), 0); + priv->nreqbytes = 0; + return -ENOMEM; + } + req = privreq->req; + + /* Transfer all of the data that will (1) fit into the request buffer, OR (2) + * all of the data available in the sector buffer. + */ + + src = &priv->iobuffer[lun->sectorsize - priv->nsectbytes]; + dest = &req->buf[priv->nreqbytes]; + + nbytes = min(CONFIG_USBSTRG_BULKINREQLEN - priv->nreqbytes, priv->nsectbytes); + + /* Copy the data from the sector buffer to the USB request and update counts */ + + memcpy(dest, src, nbytes); + priv->nreqbytes += nbytes; + priv->nsectbytes -= nbytes; + + /* If (1) the request buffer is full OR (2) this is the final request full of data, + * then submit the request + */ + + if (priv->nreqbytes >= CONFIG_USBSTRG_BULKINREQLEN || + (priv->u.xfrlen <= 0 && priv->nsectbytes <= 0)) + { + /* Remove the request that we just filled from wrreqlist (we've already checked + * that is it not NULL + */ + + flags = irqsave(); + privreq = (FAR struct usbstrg_req_s *)sq_remfirst(&priv->wrreqlist); + irqrestore(flags); + + /* And submit the request to the bulk IN endpoint */ + + req->len = priv->nreqbytes; + req->priv = privreq; + req->callback = usbstrg_wrcomplete; + req->flags = 0; + + ret = EP_SUBMIT(priv->epbulkin, req); + if (ret != OK) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_CMDREADSUBMIT), (uint16_t)-ret); + lun->sd = SCSI_KCQME_UNRRE1; + lun->sdinfo = priv->sector; + break; + } + + /* Assume success... residue should probably really be decremented in + * wrcomplete when we know that the transfer completed successfully. + */ + + priv->residue -= priv->nreqbytes; + priv->nreqbytes = 0; + } + } + + usbtrace(TRACE_CLASSSTATE(USBSTRG_CLASSSTATE_CMDREADCMDFINISH), priv->u.xfrlen); + priv->thstate = USBSTRG_STATE_CMDFINISH; + return OK; +} + +/**************************************************************************** + * Name: usbstrg_cmdwritestate + * + * Description: + * Called from the worker thread in the USBSTRG_STATE_CMDWRITE state. + * The USBSTRG_STATE_CMDWRITE state is entered when usbstrg_cmdparsestate + * obtains a valid SCSI write command. This state is really a continuation + * of the USBSTRG_STATE_CMDPARSE state that handles extended SCSI write + * command handling. + * + * Returned value: + * If no USBDEV write request is available or certain other errors occur, this + * function returns a negated errno and stays in the USBSTRG_STATE_CMDWRITE + * state. Otherwise, when the new SCSI write command is fully processed, + * this function sets priv->thstate to USBSTRG_STATE_CMDFINISH and returns OK. + * + * State variables: + * xfrlen - holds the number of sectors read to be written. + * sector - holds the sector number of the next sector to write + * nsectbytes - holds the number of bytes buffered for the current sector + * nreqbytes - holds the number of untransferred bytes currently in the + * request at the head of the rdreqlist. + * + ****************************************************************************/ + +static int usbstrg_cmdwritestate(FAR struct usbstrg_dev_s *priv) +{ + FAR struct usbstrg_lun_s *lun = priv->lun; + FAR struct usbstrg_req_s *privreq; + FAR struct usbdev_req_s *req; + ssize_t nwritten; + uint16_t xfrd; + uint8_t *src; + uint8_t *dest; + int nbytes; + int ret; + + /* Loop transferring data until either (1) all of the data has been + * transferred, or (2) we have written all of the data in the available + * read requests. + */ + + while (priv->u.xfrlen > 0) + { + usbtrace(TRACE_CLASSSTATE(USBSTRG_CLASSSTATE_CMDWRITE), priv->u.xfrlen); + + /* Check if there is a request in the rdreqlist containing additional + * data to be written. + */ + + privreq = (FAR struct usbstrg_req_s *)sq_remfirst(&priv->rdreqlist); + + /* If there no request data available, then just return an error. + * This will cause us to remain in the CMDWRITE state. When a filled request is + * received, the worker thread will be awakened in the USBSTRG_STATE_CMDWRITE + * and we will be called again. + */ + + if (!privreq) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_CMDWRITERDRQEMPTY), 0); + priv->nreqbytes = 0; + return -ENOMEM; + } + + req = privreq->req; + xfrd = req->xfrd; + priv->nreqbytes = xfrd; + + /* Now loop until all of the data in the read request has been tranferred + * to the block driver OR all of the request data has been transferred. + */ + + while (priv->nreqbytes > 0 && priv->u.xfrlen > 0) + { + /* Copy the data received in the read request into the sector I/O buffer */ + + src = &req->buf[xfrd - priv->nreqbytes]; + dest = &priv->iobuffer[priv->nsectbytes]; + + nbytes = min(lun->sectorsize - priv->nsectbytes, priv->nreqbytes); + + /* Copy the data from the sector buffer to the USB request and update counts */ + + memcpy(dest, src, nbytes); + priv->nsectbytes += nbytes; + priv->nreqbytes -= nbytes; + + /* Is the I/O buffer full? */ + + if (priv->nsectbytes >= lun->sectorsize) + { + /* Yes.. Write the next sector */ + + nwritten = USBSTRG_DRVR_WRITE(lun, priv->iobuffer, priv->sector, 1); + if (nwritten < 0) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_CMDWRITEWRITEFAIL), -nwritten); + lun->sd = SCSI_KCQME_WRITEFAULTAUTOREALLOCFAILED; + lun->sdinfo = priv->sector; + goto errout; + } + + priv->nsectbytes = 0; + priv->residue -= lun->sectorsize; + priv->u.xfrlen--; + priv->sector++; + } + } + + /* In either case, we are finished with this read request and can return it + * to the endpoint. Then we will go back to the top of the top and attempt + * to get the next read request. + */ + + req->len = CONFIG_USBSTRG_BULKOUTREQLEN; + req->priv = privreq; + req->callback = usbstrg_rdcomplete; + + ret = EP_SUBMIT(priv->epbulkout, req); + if (ret != OK) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_CMDWRITERDSUBMIT), (uint16_t)-ret); + } + + /* Did the host decide to stop early? */ + + if (xfrd != CONFIG_USBSTRG_BULKOUTREQLEN) + { + priv->shortpacket = 1; + goto errout; + } + } + +errout: + usbtrace(TRACE_CLASSSTATE(USBSTRG_CLASSSTATE_CMDWRITECMDFINISH), priv->u.xfrlen); + priv->thstate = USBSTRG_STATE_CMDFINISH; + return OK; +} + +/**************************************************************************** + * Name: usbstrg_cmdfinishstate + * + * Description: + * Called from the worker thread in the USBSTRG_STATE_CMDFINISH state. + * The USBSTRG_STATE_CMDFINISH state is entered when processing of a + * command has finished but before status has been returned. + * + * Returned value: + * If no USBDEV write request is available or certain other errors occur, this + * function returns a negated errno and stays in the USBSTRG_STATE_CMDFINISH + * state. Otherwise, when the command is fully processed, this function + * sets priv->thstate to USBSTRG_STATE_CMDSTATUS and returns OK. + * + ****************************************************************************/ + +static int usbstrg_cmdfinishstate(FAR struct usbstrg_dev_s *priv) +{ + FAR struct usbstrg_req_s *privreq; + irqstate_t flags; + int ret = OK; + + /* Check if there is a request in the wrreqlist that we will be able to + * use for data transfer. + */ + + privreq = (FAR struct usbstrg_req_s *)sq_peek(&priv->wrreqlist); + + /* If there no request structures available, then just return an error. + * This will cause us to remain in the CMDREAD state. When a request is + * returned, the worker thread will be awakened in the USBSTRG_STATE_CMDREAD + * and we will be called again. + */ + + if (!privreq) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_CMDFINISHRQEMPTY), 0); + return -ENOMEM; + } + + /* Finish the final stages of the reply */ + + switch (priv->cbwdir) + { + /* Device-to-host: All but the last data buffer has been sent */ + + case USBSTRG_FLAGS_DIRDEVICE2HOST: + if (priv->cbwlen > 0) + { + /* On most commands (the exception is outgoing, write commands), + * the data has not not yet been sent. + */ + + if (priv->nreqbytes > 0) + { + struct usbdev_req_s *req; + + /* Take a request from the wrreqlist (we've already checked + * that is it not NULL) + */ + + flags = irqsave(); + privreq = (FAR struct usbstrg_req_s *)sq_remfirst(&priv->wrreqlist); + irqrestore(flags); + + /* Send the write request */ + + req = privreq->req; + req->len = priv->nreqbytes; + req->callback = usbstrg_wrcomplete; + req->priv = privreq; + req->flags = USBDEV_REQFLAGS_NULLPKT; + + ret = EP_SUBMIT(priv->epbulkin, privreq->req); + if (ret < 0) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_CMDFINISHSUBMIT), (uint16_t)-ret); + } + } + + /* Stall the BULK In endpoint if there is a residue */ + + if (priv->residue > 0) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_CMDFINISHRESIDUE), (uint16_t)priv->residue); + +#ifdef CONFIG_USBSTRG_RACEWAR + /* (See description of the workaround at the top of the file). + * First, wait for the transfer to complete, then stall the endpoint + */ + + usleep (100000); + (void)EP_STALL(priv->epbulkin); + + /* now wait for stall to go away .... */ + + usleep (100000); +#else + (void)EP_STALL(priv->epbulkin); +#endif + } + } + break; + + /* Host-to-device: We have processed all we want of the host data. */ + + case USBSTRG_FLAGS_DIRHOST2DEVICE: + if (priv->residue > 0) + { + /* Did the host stop sending unexpectedly early? */ + + flags = irqsave(); + if (priv->shortpacket) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_CMDFINISHSHORTPKT), (uint16_t)priv->residue); + } + + /* Unprocessed incoming data: STALL and cancel requests. */ + + else + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_CMDFINSHSUBMIT), (uint16_t)priv->residue); + EP_STALL(priv->epbulkout); + } + + priv->theventset |= USBSTRG_EVENT_ABORTBULKOUT; + irqrestore(flags); + } + break; + + default: + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_CMDFINSHDIR), priv->cbwdir); + case USBSTRG_FLAGS_DIRNONE: /* Nothing to send */ + break; + } + + /* Return to the IDLE state */ + + usbtrace(TRACE_CLASSSTATE(USBSTRG_CLASSSTATE_CMDFINISHCMDSTATUS), 0); + priv->thstate = USBSTRG_STATE_CMDSTATUS; + return OK; +} + +/**************************************************************************** + * Name: usbstrg_cmdstatusstate + * + * Description: + * Called from the worker thread in the USBSTRG_STATE_CMDSTATUS state. + * That state is after a CBW has been fully processed. This function sends + * the concluding CSW. + * + * Returned value: + * If no write request is available or certain other errors occur, this + * function returns a negated errno and stays in the USBSTRG_STATE_CMDSTATUS + * state. Otherwise, when the SCSI statis is successfully returned, this + * function sets priv->thstate to USBSTRG_STATE_IDLE and returns OK. + * + ****************************************************************************/ + +static int usbstrg_cmdstatusstate(FAR struct usbstrg_dev_s *priv) +{ + FAR struct usbstrg_lun_s *lun = priv->lun; + FAR struct usbstrg_req_s *privreq; + FAR struct usbdev_req_s *req; + FAR struct usbstrg_csw_s *csw; + irqstate_t flags; + uint32_t sd; + uint8_t status = USBSTRG_CSWSTATUS_PASS; + int ret; + + /* Take a request from the wrreqlist */ + + flags = irqsave(); + privreq = (FAR struct usbstrg_req_s *)sq_remfirst(&priv->wrreqlist); + irqrestore(flags); + + /* If there no request structures available, then just return an error. + * This will cause us to remain in the CMDSTATUS status. When a request is + * returned, the worker thread will be awakened in the USBSTRG_STATE_CMDSTATUS + * and we will be called again. + */ + + if (!privreq) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_CMDSTATUSRDREQLISTEMPTY), 0); + return -ENOMEM; + } + + req = privreq->req; + csw = (struct usbstrg_csw_s*)req->buf; + + /* Extract the sense data from the LUN structure */ + + if (lun) + { + sd = lun->sd; + } + else + { + sd = SCSI_KCQIR_INVALIDLUN; + } + + /* Determine the CSW status: PASS, PHASEERROR, or FAIL */ + + if (priv->phaseerror) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_SNDPHERROR), 0); + status = USBSTRG_CSWSTATUS_PHASEERROR; + sd = SCSI_KCQIR_INVALIDCOMMAND; + } + else if (sd != SCSI_KCQ_NOSENSE) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_SNDCSWFAIL), 0); + status = USBSTRG_CSWSTATUS_FAIL; + } + + /* Format and submit the CSW */ + + csw->signature[0] = 'U'; + csw->signature[1] = 'S'; + csw->signature[2] = 'B'; + csw->signature[3] = 'S'; + usbstrg_putle32(csw->tag, priv->cbwtag); + usbstrg_putle32(csw->residue, priv->residue); + csw->status = status; + + usbstrg_dumpdata("SCSCI CSW", (uint8_t*)csw, USBSTRG_CSW_SIZEOF); + + req->len = USBSTRG_CSW_SIZEOF; + req->callback = usbstrg_wrcomplete; + req->priv = privreq; + req->flags = USBDEV_REQFLAGS_NULLPKT; + + ret = EP_SUBMIT(priv->epbulkin, req); + if (ret < 0) + { + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_SNDSTATUSSUBMIT), (uint16_t)-ret); + flags = irqsave(); + (void)sq_addlast((sq_entry_t*)privreq, &priv->wrreqlist); + irqrestore(flags); + } + + /* Return to the IDLE state */ + + usbtrace(TRACE_CLASSSTATE(USBSTRG_CLASSSTATE_CMDSTATUSIDLE), 0); + priv->thstate = USBSTRG_STATE_IDLE; + return OK; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: usbstrg_workerthread + * + * Description: + * This is the main function of the USB storage worker thread. It loops + * until USB-related events occur, then processes those events accordingly + * + ****************************************************************************/ + +void *usbstrg_workerthread(void *arg) +{ + struct usbstrg_dev_s *priv = (struct usbstrg_dev_s *)arg; + irqstate_t flags; + uint16_t eventset; + int ret; + + /* This thread is started before the USB storage class is fully initialized. + * wait here until we are told to begin. Start in the NOTINITIALIZED state + */ + + pthread_mutex_lock(&priv->mutex); + priv->thstate = USBSTRG_STATE_STARTED; + while ((priv->theventset & USBSTRG_EVENT_READY) != 0 && + (priv->theventset & USBSTRG_EVENT_TERMINATEREQUEST) != 0) + { + pthread_cond_wait(&priv->cond, &priv->mutex); + } + + /* Transition to the INITIALIZED/IDLE state */ + + priv->thstate = USBSTRG_STATE_IDLE; + eventset = priv->theventset; + priv->theventset = USBSTRG_EVENT_NOEVENTS; + pthread_mutex_unlock(&priv->mutex); + + /* Then loop until we are asked to terminate */ + + while ((eventset & USBSTRG_EVENT_TERMINATEREQUEST) == 0) + { + /* Wait for some interesting event. Note that we must both take the + * lock (to eliminate race conditions with other threads) and disable + * interrupts (to eliminate race conditions with USB interrupt handling. + */ + + pthread_mutex_lock(&priv->mutex); + flags = irqsave(); + if (priv->theventset == USBSTRG_EVENT_NOEVENTS) + { + pthread_cond_wait(&priv->cond, &priv->mutex); + } + + /* Sample any events before re-enabling interrupts. Any events that + * occur after re-enabling interrupts will have to be handled in the + * next time through the loop. + */ + + eventset = priv->theventset; + priv->theventset = USBSTRG_EVENT_NOEVENTS; + pthread_mutex_unlock(&priv->mutex); + + /* Were we awakened by some event that requires immediate action? + * + * - The USBSTRG_EVENT_DISCONNECT is signalled from the disconnect method + * after all transfers have been stopped, when the host is disconnected. + * + * - The CUSBSTRG_EVENT_RESET is signalled when the bulk-storage-specific + * USBSTRG_REQ_MSRESET EP0 setup received. We must stop the current + * operation and reinialize state. + * + * - The USBSTRG_EVENT_CFGCHANGE is signaled when the EP0 setup logic + * receives a valid USB_REQ_SETCONFIGURATION request + * + * - The USBSTRG_EVENT_IFCHANGE is signaled when the EP0 setup logic + * receives a valid USB_REQ_SETINTERFACE request + * + * - The USBSTRG_EVENT_ABORTBULKOUT event is signalled by the CMDFINISH + * logic when there is a residue after processing a host-to-device + * transfer. We need to discard all incoming request. + * + * All other events are just wakeup calls and are intended only + * drive the state machine. + */ + + if ((eventset & (USBSTRG_EVENT_DISCONNECT|USBSTRG_EVENT_RESET|USBSTRG_EVENT_CFGCHANGE| + USBSTRG_EVENT_IFCHANGE|USBSTRG_EVENT_ABORTBULKOUT)) != 0) + { + /* These events require that the current configuration be reset */ + + if ((eventset & USBSTRG_EVENT_IFCHANGE) != 0) + { + usbstrg_resetconfig(priv); + } + + /* These events require that a new configuration be established */ + + if ((eventset & (USBSTRG_EVENT_CFGCHANGE|USBSTRG_EVENT_IFCHANGE)) != 0) + { + usbstrg_setconfig(priv, priv->thvalue); + } + + /* These events required that we send a deferred EP0 setup response */ + + if ((eventset & (USBSTRG_EVENT_RESET|USBSTRG_EVENT_CFGCHANGE|USBSTRG_EVENT_IFCHANGE)) != 0) + { + usbstrg_deferredresponse(priv, false); + } + + /* For all of these events... terminate any transactions in progress */ + + priv->thstate = USBSTRG_STATE_IDLE; + } + irqrestore(flags); + + /* Loop processing each SCSI command state. Each state handling + * function will do the following: + * + * - If it must block for an event, it will return a negated errno value + * - If it completes the processing for that state, it will (1) set + * the next appropriate state value and (2) return OK. + * + * So the following will loop until we must block for an event in + * a particular state. When we are awakened by an event (above) we + * will resume processing in the same state. + */ + + do + { + switch (priv->thstate) + { + case USBSTRG_STATE_IDLE: /* Started and waiting for commands */ + ret = usbstrg_idlestate(priv); + break; + + case USBSTRG_STATE_CMDPARSE: /* Parsing the received a command */ + ret = usbstrg_cmdparsestate(priv); + break; + + case USBSTRG_STATE_CMDREAD: /* Continuing to process a SCSI read command */ + ret = usbstrg_cmdreadstate(priv); + break; + + case USBSTRG_STATE_CMDWRITE: /* Continuing to process a SCSI write command */ + ret = usbstrg_cmdwritestate(priv); + break; + + case USBSTRG_STATE_CMDFINISH: /* Finish command processing */ + ret = usbstrg_cmdfinishstate(priv); + break; + + case USBSTRG_STATE_CMDSTATUS: /* Processing the status phase of a command */ + ret = usbstrg_cmdstatusstate(priv); + break; + + case USBSTRG_STATE_NOTSTARTED: /* Thread has not yet been started */ + case USBSTRG_STATE_STARTED: /* Started, but is not yet initialized */ + case USBSTRG_STATE_TERMINATED: /* Thread has exitted */ + default: + usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_INVALIDSTATE), priv->thstate); + priv->thstate = USBSTRG_STATE_IDLE; + ret = OK; + break; + } + } + while (ret == OK); + } + + /* Transition to the TERMINATED state and exit */ + + priv->thstate = USBSTRG_STATE_TERMINATED; + return NULL; +} diff --git a/nuttx/drivers/usbdev/pl2303.c b/nuttx/drivers/usbdev/pl2303.c new file mode 100644 index 000000000..86e55fefb --- /dev/null +++ b/nuttx/drivers/usbdev/pl2303.c @@ -0,0 +1,2216 @@ +/**************************************************************************** + * drivers/usbdev/pl2303.c + * + * Copyright (C) 2008-2012 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * This logic emulates the Prolific PL2303 serial/USB converter + * + * 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 + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* Configuration ************************************************************/ + +/* Number of requests in the write queue */ + +#ifndef CONFIG_USBSER_NWRREQS +# define CONFIG_USBSER_NWRREQS 4 +#endif + +/* Number of requests in the read queue */ + +#ifndef CONFIG_USBSER_NRDREQS +# define CONFIG_USBSER_NRDREQS 4 +#endif + +/* Logical endpoint numbers / max packet sizes */ + +#ifndef CONFIG_USBSER_EPINTIN +# warning "EPINTIN not defined in the configuration" +# define CONFIG_USBSER_EPINTIN 1 +#endif + +#ifndef CONFIG_USBSER_EPBULKOUT +# warning "EPBULKOUT not defined in the configuration" +# define CONFIG_USBSER_EPBULKOUT 2 +#endif + +#ifndef CONFIG_USBSER_EPBULKIN +# warning "EPBULKIN not defined in the configuration" +# define CONFIG_USBSER_EPBULKIN 3 +#endif + +/* Packet and request buffer sizes */ + +#ifndef CONFIG_USBSER_EP0MAXPACKET +# define CONFIG_USBSER_EP0MAXPACKET 64 +#endif + +#undef CONFIG_USBSER_BULKREQLEN + +/* Vendor and product IDs and strings */ + +#ifndef CONFIG_USBSER_VENDORID +# define CONFIG_USBSER_VENDORID 0x067b +#endif + +#ifndef CONFIG_USBSER_PRODUCTID +# define CONFIG_USBSER_PRODUCTID 0x2303 +#endif + +#ifndef CONFIG_USBSER_VENDORSTR +# warning "No Vendor string specified" +# define CONFIG_USBSER_VENDORSTR "NuttX" +#endif + +#ifndef CONFIG_USBSER_PRODUCTSTR +# warning "No Product string specified" +# define CONFIG_USBSER_PRODUCTSTR "USBdev Serial" +#endif + +#undef CONFIG_USBSER_SERIALSTR +#define CONFIG_USBSER_SERIALSTR "0" + +#undef CONFIG_USBSER_CONFIGSTR +#define CONFIG_USBSER_CONFIGSTR "Bulk" + +/* USB Controller */ + +#ifndef CONFIG_USBDEV_SELFPOWERED +# define SELFPOWERED USB_CONFIG_ATT_SELFPOWER +#else +# define SELFPOWERED (0) +#endif + +#ifndef CONFIG_USBDEV_REMOTEWAKEUP +# define REMOTEWAKEUP USB_CONFIG_ATTR_WAKEUP +#else +# define REMOTEWAKEUP (0) +#endif + +#ifndef CONFIG_USBDEV_MAXPOWER +# define CONFIG_USBDEV_MAXPOWER 100 +#endif + +/* Descriptors ****************************************************************/ + +/* These settings are not modifiable via the NuttX configuration */ + +#define USBSER_VERSIONNO (0x0202) /* Device version number */ +#define USBSER_CONFIGIDNONE (0) /* Config ID means to return to address mode */ +#define USBSER_CONFIGID (1) /* The only supported configuration ID */ +#define USBSER_NCONFIGS (1) /* Number of configurations supported */ +#define USBSER_INTERFACEID (0) +#define USBSER_ALTINTERFACEID (0) +#define USBSER_NINTERFACES (1) /* Number of interfaces in the configuration */ +#define USBSER_NENDPOINTS (3) /* Number of endpoints in the interface */ + +/* Endpoint configuration */ + +#define USBSER_EPINTIN_ADDR (USB_DIR_IN|CONFIG_USBSER_EPINTIN) +#define USBSER_EPINTIN_ATTR (USB_EP_ATTR_XFER_INT) +#define USBSER_EPINTIN_MXPACKET (10) + +#define USBSER_EPOUTBULK_ADDR (CONFIG_USBSER_EPBULKOUT) +#define USBSER_EPOUTBULK_ATTR (USB_EP_ATTR_XFER_BULK) + +#define USBSER_EPINBULK_ADDR (USB_DIR_IN|CONFIG_USBSER_EPBULKIN) +#define USBSER_EPINBULK_ATTR (USB_EP_ATTR_XFER_BULK) + +/* String language */ + +#define USBSER_STR_LANGUAGE (0x0409) /* en-us */ + +/* Descriptor strings */ + +#define USBSER_MANUFACTURERSTRID (1) +#define USBSER_PRODUCTSTRID (2) +#define USBSER_SERIALSTRID (3) +#define USBSER_CONFIGSTRID (4) + +/* Buffer big enough for any of our descriptors */ + +#define USBSER_MXDESCLEN (64) + +/* Vender specific control requests *******************************************/ + +#define PL2303_CONTROL_TYPE (0x20) +#define PL2303_SETLINEREQUEST (0x20) /* OUT, Recipient interface */ +#define PL2303_GETLINEREQUEST (0x21) /* IN, Recipient interface */ +#define PL2303_SETCONTROLREQUEST (0x22) /* OUT, Recipient interface */ +#define PL2303_BREAKREQUEST (0x23) /* OUT, Recipient interface */ + +/* Vendor read/write */ + +#define PL2303_RWREQUEST_TYPE (0x40) +#define PL2303_RWREQUEST (0x01) /* IN/OUT, Recipient device */ + +/* Misc Macros ****************************************************************/ + +/* min/max macros */ + +#ifndef min +# define min(a,b) ((a)<(b)?(a):(b)) +#endif + +#ifndef max +# define max(a,b) ((a)>(b)?(a):(b)) +#endif + +/* Trace values *************************************************************/ + +#define USBSER_CLASSAPI_SETUP TRACE_EVENT(TRACE_CLASSAPI_ID, USBSER_TRACECLASSAPI_SETUP) +#define USBSER_CLASSAPI_SHUTDOWN TRACE_EVENT(TRACE_CLASSAPI_ID, USBSER_TRACECLASSAPI_SHUTDOWN) +#define USBSER_CLASSAPI_ATTACH TRACE_EVENT(TRACE_CLASSAPI_ID, USBSER_TRACECLASSAPI_ATTACH) +#define USBSER_CLASSAPI_DETACH TRACE_EVENT(TRACE_CLASSAPI_ID, USBSER_TRACECLASSAPI_DETACH) +#define USBSER_CLASSAPI_IOCTL TRACE_EVENT(TRACE_CLASSAPI_ID, USBSER_TRACECLASSAPI_IOCTL) +#define USBSER_CLASSAPI_RECEIVE TRACE_EVENT(TRACE_CLASSAPI_ID, USBSER_TRACECLASSAPI_RECEIVE) +#define USBSER_CLASSAPI_RXINT TRACE_EVENT(TRACE_CLASSAPI_ID, USBSER_TRACECLASSAPI_RXINT) +#define USBSER_CLASSAPI_RXAVAILABLE TRACE_EVENT(TRACE_CLASSAPI_ID, USBSER_TRACECLASSAPI_RXAVAILABLE) +#define USBSER_CLASSAPI_SEND TRACE_EVENT(TRACE_CLASSAPI_ID, USBSER_TRACECLASSAPI_SEND) +#define USBSER_CLASSAPI_TXINT TRACE_EVENT(TRACE_CLASSAPI_ID, USBSER_TRACECLASSAPI_TXINT) +#define USBSER_CLASSAPI_TXREADY TRACE_EVENT(TRACE_CLASSAPI_ID, USBSER_TRACECLASSAPI_TXREADY) +#define USBSER_CLASSAPI_TXEMPTY TRACE_EVENT(TRACE_CLASSAPI_ID, USBSER_TRACECLASSAPI_TXEMPTY) + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/* Container to support a list of requests */ + +struct usbser_req_s +{ + FAR struct usbser_req_s *flink; /* Implements a singly linked list */ + FAR struct usbdev_req_s *req; /* The contained request */ +}; + +/* This structure describes the internal state of the driver */ + +struct usbser_dev_s +{ + FAR struct uart_dev_s serdev; /* Serial device structure */ + FAR struct usbdev_s *usbdev; /* usbdev driver pointer */ + + uint8_t config; /* Configuration number */ + uint8_t nwrq; /* Number of queue write requests (in reqlist)*/ + uint8_t nrdq; /* Number of queue read requests (in epbulkout) */ + bool rxenabled; /* true: UART RX "interrupts" enabled */ + uint8_t linest[7]; /* Fake line status */ + int16_t rxhead; /* Working head; used when rx int disabled */ + + FAR struct usbdev_ep_s *epintin; /* Interrupt IN endpoint structure */ + FAR struct usbdev_ep_s *epbulkin; /* Bulk IN endpoint structure */ + FAR struct usbdev_ep_s *epbulkout; /* Bulk OUT endpoint structure */ + FAR struct usbdev_req_s *ctrlreq; /* Control request */ + struct sq_queue_s reqlist; /* List of write request containers */ + + /* Pre-allocated write request containers. The write requests will + * be linked in a free list (reqlist), and used to send requests to + * EPBULKIN; Read requests will be queued in the EBULKOUT. + */ + + struct usbser_req_s wrreqs[CONFIG_USBSER_NWRREQS]; + struct usbser_req_s rdreqs[CONFIG_USBSER_NWRREQS]; + + /* Serial I/O buffers */ + + char rxbuffer[CONFIG_USBSER_RXBUFSIZE]; + char txbuffer[CONFIG_USBSER_TXBUFSIZE]; +}; + +/* The internal version of the class driver */ + +struct usbser_driver_s +{ + struct usbdevclass_driver_s drvr; + FAR struct usbser_dev_s *dev; +}; + +/* This is what is allocated */ + +struct usbser_alloc_s +{ + struct usbser_dev_s dev; + struct usbser_driver_s drvr; +}; + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/* Transfer helpers *********************************************************/ + +static uint16_t usbclass_fillrequest(FAR struct usbser_dev_s *priv, + uint8_t *reqbuf, uint16_t reqlen); +static int usbclass_sndpacket(FAR struct usbser_dev_s *priv); +static inline int usbclass_recvpacket(FAR struct usbser_dev_s *priv, + uint8_t *reqbuf, uint16_t reqlen); + +/* Request helpers *********************************************************/ + +static struct usbdev_req_s *usbclass_allocreq(FAR struct usbdev_ep_s *ep, + uint16_t len); +static void usbclass_freereq(FAR struct usbdev_ep_s *ep, + FAR struct usbdev_req_s *req); + +/* Configuration ***********************************************************/ + +static int usbclass_mkstrdesc(uint8_t id, struct usb_strdesc_s *strdesc); +#ifdef CONFIG_USBDEV_DUALSPEED +static void usbclass_mkepbulkdesc(const struct usb_epdesc_s *indesc, + uint16_t mxpacket, struct usb_epdesc_s *outdesc); +static int16_t usbclass_mkcfgdesc(uint8_t *buf, uint8_t speed, uint8_t type); +#else +static int16_t usbclass_mkcfgdesc(uint8_t *buf); +#endif +static void usbclass_resetconfig(FAR struct usbser_dev_s *priv); +static int usbclass_setconfig(FAR struct usbser_dev_s *priv, + uint8_t config); + +/* Completion event handlers ***********************************************/ + +static void usbclass_ep0incomplete(FAR struct usbdev_ep_s *ep, + FAR struct usbdev_req_s *req); +static void usbclass_rdcomplete(FAR struct usbdev_ep_s *ep, + FAR struct usbdev_req_s *req); +static void usbclass_wrcomplete(FAR struct usbdev_ep_s *ep, + FAR struct usbdev_req_s *req); + +/* USB class device ********************************************************/ + +static int usbclass_bind(FAR struct usbdev_s *dev, + FAR struct usbdevclass_driver_s *driver); +static void usbclass_unbind(FAR struct usbdev_s *dev); +static int usbclass_setup(FAR struct usbdev_s *dev, + const struct usb_ctrlreq_s *ctrl); +static void usbclass_disconnect(FAR struct usbdev_s *dev); + +/* Serial port *************************************************************/ + +static int usbser_setup(FAR struct uart_dev_s *dev); +static void usbser_shutdown(FAR struct uart_dev_s *dev); +static int usbser_attach(FAR struct uart_dev_s *dev); +static void usbser_detach(FAR struct uart_dev_s *dev); +static void usbser_rxint(FAR struct uart_dev_s *dev, bool enable); +static void usbser_txint(FAR struct uart_dev_s *dev, bool enable); +static bool usbser_txempty(FAR struct uart_dev_s *dev); + +/**************************************************************************** + * Private Variables + ****************************************************************************/ + +/* USB class device ********************************************************/ + +static const struct usbdevclass_driverops_s g_driverops = +{ + usbclass_bind, /* bind */ + usbclass_unbind, /* unbind */ + usbclass_setup, /* setup */ + usbclass_disconnect, /* disconnect */ + NULL, /* suspend */ + NULL, /* resume */ +}; + +/* Serial port *************************************************************/ + +static const struct uart_ops_s g_uartops = +{ + usbser_setup, /* setup */ + usbser_shutdown, /* shutdown */ + usbser_attach, /* attach */ + usbser_detach, /* detach */ + NULL, /* ioctl */ + NULL, /* receive */ + usbser_rxint, /* rxinit */ + NULL, /* rxavailable */ + NULL, /* send */ + usbser_txint, /* txinit */ + NULL, /* txready */ + usbser_txempty /* txempty */ +}; + +/* USB descriptor templates these will be copied and modified **************/ + +static const struct usb_devdesc_s g_devdesc = +{ + USB_SIZEOF_DEVDESC, /* len */ + USB_DESC_TYPE_DEVICE, /* type */ + {LSBYTE(0x0200), MSBYTE(0x0200)}, /* usb */ + USB_CLASS_PER_INTERFACE, /* class */ + 0, /* subclass */ + 0, /* protocol */ + CONFIG_USBSER_EP0MAXPACKET, /* maxpacketsize */ + { LSBYTE(CONFIG_USBSER_VENDORID), /* vendor */ + MSBYTE(CONFIG_USBSER_VENDORID) }, + { LSBYTE(CONFIG_USBSER_PRODUCTID), /* product */ + MSBYTE(CONFIG_USBSER_PRODUCTID) }, + { LSBYTE(USBSER_VERSIONNO), /* device */ + MSBYTE(USBSER_VERSIONNO) }, + USBSER_MANUFACTURERSTRID, /* imfgr */ + USBSER_PRODUCTSTRID, /* iproduct */ + USBSER_SERIALSTRID, /* serno */ + USBSER_NCONFIGS /* nconfigs */ +}; + +static const struct usb_cfgdesc_s g_cfgdesc = +{ + USB_SIZEOF_CFGDESC, /* len */ + USB_DESC_TYPE_CONFIG, /* type */ + {0, 0}, /* totallen -- to be provided */ + USBSER_NINTERFACES, /* ninterfaces */ + USBSER_CONFIGID, /* cfgvalue */ + USBSER_CONFIGSTRID, /* icfg */ + USB_CONFIG_ATTR_ONE|SELFPOWERED|REMOTEWAKEUP, /* attr */ + (CONFIG_USBDEV_MAXPOWER + 1) / 2 /* mxpower */ +}; + +static const struct usb_ifdesc_s g_ifdesc = +{ + USB_SIZEOF_IFDESC, /* len */ + USB_DESC_TYPE_INTERFACE, /* type */ + 0, /* ifno */ + 0, /* alt */ + USBSER_NENDPOINTS, /* neps */ + USB_CLASS_VENDOR_SPEC, /* class */ + 0, /* subclass */ + 0, /* protocol */ + USBSER_CONFIGSTRID /* iif */ +}; + +static const struct usb_epdesc_s g_epintindesc = +{ + USB_SIZEOF_EPDESC, /* len */ + USB_DESC_TYPE_ENDPOINT, /* type */ + USBSER_EPINTIN_ADDR, /* addr */ + USBSER_EPINTIN_ATTR, /* attr */ + { LSBYTE(USBSER_EPINTIN_MXPACKET), /* maxpacket */ + MSBYTE(USBSER_EPINTIN_MXPACKET) }, + 1 /* interval */ +}; + +static const struct usb_epdesc_s g_epbulkoutdesc = +{ + USB_SIZEOF_EPDESC, /* len */ + USB_DESC_TYPE_ENDPOINT, /* type */ + USBSER_EPOUTBULK_ADDR, /* addr */ + USBSER_EPOUTBULK_ATTR, /* attr */ + { LSBYTE(64), MSBYTE(64) }, /* maxpacket -- might change to 512*/ + 0 /* interval */ +}; + +static const struct usb_epdesc_s g_epbulkindesc = +{ + USB_SIZEOF_EPDESC, /* len */ + USB_DESC_TYPE_ENDPOINT, /* type */ + USBSER_EPINBULK_ADDR, /* addr */ + USBSER_EPINBULK_ATTR, /* attr */ + { LSBYTE(64), MSBYTE(64) }, /* maxpacket -- might change to 512*/ + 0 /* interval */ +}; + +#ifdef CONFIG_USBDEV_DUALSPEED +static const struct usb_qualdesc_s g_qualdesc = +{ + USB_SIZEOF_QUALDESC, /* len */ + USB_DESC_TYPE_DEVICEQUALIFIER, /* type */ + {LSBYTE(0x0200), MSBYTE(0x0200) }, /* USB */ + USB_CLASS_VENDOR_SPEC, /* class */ + 0, /* subclass */ + 0, /* protocol */ + CONFIG_USBSER_EP0MAXPACKET, /* mxpacketsize */ + USBSER_NCONFIGS, /* nconfigs */ + 0, /* reserved */ +}; +#endif + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/************************************************************************************ + * Name: usbclass_fillrequest + * + * Description: + * If there is data to send it is copied to the given buffer. Called either + * to initiate the first write operation, or from the completion interrupt handler + * service consecutive write operations. + * + * NOTE: The USB serial driver does not use the serial drivers uart_xmitchars() + * API. That logic is essentially duplicated here because unlike UART hardware, + * we need to be able to handle writes not byte-by-byte, but packet-by-packet. + * Unfortunately, that decision also exposes some internals of the serial driver + * in the following. + * + ************************************************************************************/ + +static uint16_t usbclass_fillrequest(FAR struct usbser_dev_s *priv, uint8_t *reqbuf, + uint16_t reqlen) +{ + FAR uart_dev_t *serdev = &priv->serdev; + FAR struct uart_buffer_s *xmit = &serdev->xmit; + irqstate_t flags; + uint16_t nbytes = 0; + + /* Disable interrupts */ + + flags = irqsave(); + + /* Transfer bytes while we have bytes available and there is room in the request */ + + while (xmit->head != xmit->tail && nbytes < reqlen) + { + *reqbuf++ = xmit->buffer[xmit->tail]; + nbytes++; + + /* Increment the tail pointer */ + + if (++(xmit->tail) >= xmit->size) + { + xmit->tail = 0; + } + } + + /* When all of the characters have been sent from the buffer + * disable the "TX interrupt". + */ + + if (xmit->head == xmit->tail) + { + uart_disabletxint(serdev); + } + + /* If any bytes were removed from the buffer, inform any waiters + * there there is space available. + */ + + if (nbytes) + { + uart_datasent(serdev); + } + + irqrestore(flags); + return nbytes; +} + +/************************************************************************************ + * Name: usbclass_sndpacket + * + * Description: + * This function obtains write requests, transfers the TX data into the request, + * and submits the requests to the USB controller. This continues untils either + * (1) there are no further packets available, or (2) thre is not further data + * to send. + * + ************************************************************************************/ + +static int usbclass_sndpacket(FAR struct usbser_dev_s *priv) +{ + FAR struct usbdev_ep_s *ep; + FAR struct usbdev_req_s *req; + FAR struct usbser_req_s *reqcontainer; + irqstate_t flags; + int len; + int ret = OK; + +#ifdef CONFIG_DEBUG + if (priv == NULL) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_INVALIDARG), 0); + return -ENODEV; + } +#endif + + flags = irqsave(); + + /* Use our IN endpoint for the transfer */ + + ep = priv->epbulkin; + + /* Loop until either (1) we run out or write requests, or (2) usbclass_fillrequest() + * is unable to fill the request with data (i.e., untilthere is no more data + * to be sent). + */ + + uvdbg("head=%d tail=%d nwrq=%d empty=%d\n", + priv->serdev.xmit.head, priv->serdev.xmit.tail, + priv->nwrq, sq_empty(&priv->reqlist)); + + while (!sq_empty(&priv->reqlist)) + { + /* Peek at the request in the container at the head of the list */ + + reqcontainer = (struct usbser_req_s *)sq_peek(&priv->reqlist); + req = reqcontainer->req; + + /* Fill the request with serial TX data */ + + len = usbclass_fillrequest(priv, req->buf, req->len); + if (len > 0) + { + /* Remove the empty container from the request list */ + + (void)sq_remfirst(&priv->reqlist); + priv->nwrq--; + + /* Then submit the request to the endpoint */ + + req->len = len; + req->priv = reqcontainer; + req->flags = USBDEV_REQFLAGS_NULLPKT; + ret = EP_SUBMIT(ep, req); + if (ret != OK) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_SUBMITFAIL), (uint16_t)-ret); + break; + } + } + else + { + break; + } + } + + irqrestore(flags); + return ret; +} + +/************************************************************************************ + * Name: usbclass_recvpacket + * + * Description: + * A normal completion event was received by the read completion handler at the + * interrupt level (with interrupts disabled). This function handles the USB packet + * and provides the received data to the uart RX buffer. + * + * Assumptions: + * Called from the USB interrupt handler with interrupts disabled. + * + ************************************************************************************/ + +static inline int usbclass_recvpacket(FAR struct usbser_dev_s *priv, + uint8_t *reqbuf, uint16_t reqlen) +{ + FAR uart_dev_t *serdev = &priv->serdev; + FAR struct uart_buffer_s *recv = &serdev->recv; + uint16_t currhead; + uint16_t nexthead; + uint16_t nbytes = 0; + + /* Get the next head index. During the time that RX interrupts are disabled, the + * the serial driver will be extracting data from the circular buffer and modifying + * recv.tail. During this time, we should avoid modifying recv.head; Instead we will + * use a shadow copy of the index. When interrupts are restored, the real recv.head + * will be updated with this indes. + */ + + if (priv->rxenabled) + { + currhead = recv->head; + } + else + { + currhead = priv->rxhead; + } + + /* Pre-calculate the head index and check for wrap around. We need to do this + * so that we can determine if the circular buffer will overrun BEFORE we + * overrun the buffer! + */ + + nexthead = currhead + 1; + if (nexthead >= recv->size) + { + nexthead = 0; + } + + /* Then copy data into the RX buffer until either: (1) all of the data has been + * copied, or (2) the RX buffer is full. NOTE: If the RX buffer becomes full, + * then we have overrun the serial driver and data will be lost. + */ + + while (nexthead != recv->tail && nbytes < reqlen) + { + /* Copy one byte to the head of the circular RX buffer */ + + recv->buffer[currhead] = *reqbuf++; + + /* Update counts and indices */ + + currhead = nexthead; + nbytes++; + + /* Increment the head index and check for wrap around */ + + nexthead = currhead + 1; + if (nexthead >= recv->size) + { + nexthead = 0; + } + } + + /* Write back the head pointer using the shadow index if RX "interrupts" + * are disabled. + */ + + if (priv->rxenabled) + { + recv->head = currhead; + } + else + { + priv->rxhead = currhead; + } + + /* If data was added to the incoming serial buffer, then wake up any + * threads is waiting for incoming data. If we are running in an interrupt + * handler, then the serial driver will not run until the interrupt handler + * returns. + */ + + if (priv->rxenabled && nbytes > 0) + { + uart_datareceived(serdev); + } + + /* Return an error if the entire packet could not be transferred */ + + if (nbytes < reqlen) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_RXOVERRUN), 0); + return -ENOSPC; + } + return OK; +} + +/**************************************************************************** + * Name: usbclass_allocreq + * + * Description: + * Allocate a request instance along with its buffer + * + ****************************************************************************/ + +static struct usbdev_req_s *usbclass_allocreq(FAR struct usbdev_ep_s *ep, + uint16_t len) +{ + FAR struct usbdev_req_s *req; + + req = EP_ALLOCREQ(ep); + if (req != NULL) + { + req->len = len; + req->buf = EP_ALLOCBUFFER(ep, len); + if (!req->buf) + { + EP_FREEREQ(ep, req); + req = NULL; + } + } + return req; +} + +/**************************************************************************** + * Name: usbclass_freereq + * + * Description: + * Free a request instance along with its buffer + * + ****************************************************************************/ + +static void usbclass_freereq(FAR struct usbdev_ep_s *ep, + FAR struct usbdev_req_s *req) +{ + if (ep != NULL && req != NULL) + { + if (req->buf != NULL) + { + EP_FREEBUFFER(ep, req->buf); + } + EP_FREEREQ(ep, req); + } +} + +/**************************************************************************** + * Name: usbclass_mkstrdesc + * + * Description: + * Construct a string descriptor + * + ****************************************************************************/ + +static int usbclass_mkstrdesc(uint8_t id, struct usb_strdesc_s *strdesc) +{ + const char *str; + int len; + int ndata; + int i; + + switch (id) + { + case 0: + { + /* Descriptor 0 is the language id */ + + strdesc->len = 4; + strdesc->type = USB_DESC_TYPE_STRING; + strdesc->data[0] = LSBYTE(USBSER_STR_LANGUAGE); + strdesc->data[1] = MSBYTE(USBSER_STR_LANGUAGE); + return 4; + } + + case USBSER_MANUFACTURERSTRID: + str = CONFIG_USBSER_VENDORSTR; + break; + + case USBSER_PRODUCTSTRID: + str = CONFIG_USBSER_PRODUCTSTR; + break; + + case USBSER_SERIALSTRID: + str = CONFIG_USBSER_SERIALSTR; + break; + + case USBSER_CONFIGSTRID: + str = CONFIG_USBSER_CONFIGSTR; + break; + + default: + return -EINVAL; + } + + /* The string is utf16-le. The poor man's utf-8 to utf16-le + * conversion below will only handle 7-bit en-us ascii + */ + + len = strlen(str); + for (i = 0, ndata = 0; i < len; i++, ndata += 2) + { + strdesc->data[ndata] = str[i]; + strdesc->data[ndata+1] = 0; + } + + strdesc->len = ndata+2; + strdesc->type = USB_DESC_TYPE_STRING; + return strdesc->len; +} + +/**************************************************************************** + * Name: usbclass_mkepbulkdesc + * + * Description: + * Construct the endpoint descriptor + * + ****************************************************************************/ + +#ifdef CONFIG_USBDEV_DUALSPEED +static inline void usbclass_mkepbulkdesc(const FAR struct usb_epdesc_s *indesc, + uint16_t mxpacket, + FAR struct usb_epdesc_s *outdesc) +{ + /* Copy the canned descriptor */ + + memcpy(outdesc, indesc, USB_SIZEOF_EPDESC); + + /* Then add the correct max packet size */ + + outdesc->mxpacketsize[0] = LSBYTE(mxpacket); + outdesc->mxpacketsize[1] = MSBYTE(mxpacket); +} +#endif + +/**************************************************************************** + * Name: usbclass_mkcfgdesc + * + * Description: + * Construct the configuration descriptor + * + ****************************************************************************/ + +#ifdef CONFIG_USBDEV_DUALSPEED +static int16_t usbclass_mkcfgdesc(uint8_t *buf, uint8_t speed, uint8_t type) +#else +static int16_t usbclass_mkcfgdesc(uint8_t *buf) +#endif +{ + FAR struct usb_cfgdesc_s *cfgdesc = (struct usb_cfgdesc_s*)buf; +#ifdef CONFIG_USBDEV_DUALSPEED + bool hispeed = (speed == USB_SPEED_HIGH); + uint16_t bulkmxpacket; +#endif + uint16_t totallen; + + /* This is the total length of the configuration (not necessarily the + * size that we will be sending now. + */ + + totallen = USB_SIZEOF_CFGDESC + USB_SIZEOF_IFDESC + USBSER_NENDPOINTS * USB_SIZEOF_EPDESC; + + /* Configuration descriptor -- Copy the canned descriptor and fill in the + * type (we'll also need to update the size below + */ + + memcpy(cfgdesc, &g_cfgdesc, USB_SIZEOF_CFGDESC); + buf += USB_SIZEOF_CFGDESC; + + /* Copy the canned interface descriptor */ + + memcpy(buf, &g_ifdesc, USB_SIZEOF_IFDESC); + buf += USB_SIZEOF_IFDESC; + + /* Make the three endpoint configurations. First, check for switches + * between high and full speed + */ + +#ifdef CONFIG_USBDEV_DUALSPEED + if (type == USB_DESC_TYPE_OTHERSPEEDCONFIG) + { + hispeed = !hispeed; + } +#endif + + memcpy(buf, &g_epintindesc, USB_SIZEOF_EPDESC); + buf += USB_SIZEOF_EPDESC; + +#ifdef CONFIG_USBDEV_DUALSPEED + if (hispeed) + { + bulkmxpacket = 512; + } + else + { + bulkmxpacket = 64; + } + + usbclass_mkepbulkdesc(&g_epbulkoutdesc, bulkmxpacket, (struct usb_epdesc_s*)buf); + buf += USB_SIZEOF_EPDESC; + usbclass_mkepbulkdesc(&g_epbulkindesc, bulkmxpacket, (struct usb_epdesc_s*)buf); +#else + memcpy(buf, &g_epbulkoutdesc, USB_SIZEOF_EPDESC); + buf += USB_SIZEOF_EPDESC; + memcpy(buf, &g_epbulkindesc, USB_SIZEOF_EPDESC); +#endif + + /* Finally, fill in the total size of the configuration descriptor */ + + cfgdesc->totallen[0] = LSBYTE(totallen); + cfgdesc->totallen[1] = MSBYTE(totallen); + return totallen; +} + +/**************************************************************************** + * Name: usbclass_resetconfig + * + * Description: + * Mark the device as not configured and disable all endpoints. + * + ****************************************************************************/ + +static void usbclass_resetconfig(FAR struct usbser_dev_s *priv) +{ + /* Are we configured? */ + + if (priv->config != USBSER_CONFIGIDNONE) + { + /* Yes.. but not anymore */ + + priv->config = USBSER_CONFIGIDNONE; + + /* Disable endpoints. This should force completion of all pending + * transfers. + */ + + EP_DISABLE(priv->epintin); + EP_DISABLE(priv->epbulkin); + EP_DISABLE(priv->epbulkout); + } +} + +/**************************************************************************** + * Name: usbclass_setconfig + * + * Description: + * Set the device configuration by allocating and configuring endpoints and + * by allocating and queue read and write requests. + * + ****************************************************************************/ + +static int usbclass_setconfig(FAR struct usbser_dev_s *priv, uint8_t config) +{ + FAR struct usbdev_req_s *req; +#ifdef CONFIG_USBDEV_DUALSPEED + struct usb_epdesc_s epdesc; + uint16_t bulkmxpacket; +#endif + int i; + int ret = 0; + +#if CONFIG_DEBUG + if (priv == NULL) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_INVALIDARG), 0); + return -EIO; + } +#endif + + if (config == priv->config) + { + /* Already configured -- Do nothing */ + + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_ALREADYCONFIGURED), 0); + return 0; + } + + /* Discard the previous configuration data */ + + usbclass_resetconfig(priv); + + /* Was this a request to simply discard the current configuration? */ + + if (config == USBSER_CONFIGIDNONE) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_CONFIGNONE), 0); + return 0; + } + + /* We only accept one configuration */ + + if (config != USBSER_CONFIGID) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_CONFIGIDBAD), 0); + return -EINVAL; + } + + /* Configure the IN interrupt endpoint */ + + ret = EP_CONFIGURE(priv->epintin, &g_epintindesc, false); + if (ret < 0) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_EPINTINCONFIGFAIL), 0); + goto errout; + } + priv->epintin->priv = priv; + + /* Configure the IN bulk endpoint */ + +#ifdef CONFIG_USBDEV_DUALSPEED + if (priv->usbdev->speed == USB_SPEED_HIGH) + { + bulkmxpacket = 512; + } + else + { + bulkmxpacket = 64; + } + + usbclass_mkepbulkdesc(&g_epbulkindesc, bulkmxpacket, &epdesc); + ret = EP_CONFIGURE(priv->epbulkin, &epdesc, false); +#else + ret = EP_CONFIGURE(priv->epbulkin, &g_epbulkindesc, false); +#endif + if (ret < 0) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_EPBULKINCONFIGFAIL), 0); + goto errout; + } + + priv->epbulkin->priv = priv; + + /* Configure the OUT bulk endpoint */ + +#ifdef CONFIG_USBDEV_DUALSPEED + usbclass_mkepbulkdesc(&g_epbulkoutdesc, bulkmxpacket, &epdesc); + ret = EP_CONFIGURE(priv->epbulkout, &epdesc, true); +#else + ret = EP_CONFIGURE(priv->epbulkout, &g_epbulkoutdesc, true); +#endif + if (ret < 0) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_EPBULKOUTCONFIGFAIL), 0); + goto errout; + } + + priv->epbulkout->priv = priv; + + /* Queue read requests in the bulk OUT endpoint */ + + DEBUGASSERT(priv->nrdq == 0); + for (i = 0; i < CONFIG_USBSER_NRDREQS; i++) + { + req = priv->rdreqs[i].req; + req->callback = usbclass_rdcomplete; + ret = EP_SUBMIT(priv->epbulkout, req); + if (ret != OK) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_RDSUBMIT), (uint16_t)-ret); + goto errout; + } + priv->nrdq++; + } + + priv->config = config; + return OK; + +errout: + usbclass_resetconfig(priv); + return ret; +} + +/**************************************************************************** + * Name: usbclass_ep0incomplete + * + * Description: + * Handle completion of EP0 control operations + * + ****************************************************************************/ + +static void usbclass_ep0incomplete(FAR struct usbdev_ep_s *ep, + FAR struct usbdev_req_s *req) +{ + if (req->result || req->xfrd != req->len) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_REQRESULT), (uint16_t)-req->result); + } +} + +/**************************************************************************** + * Name: usbclass_rdcomplete + * + * Description: + * Handle completion of read request on the bulk OUT endpoint. This + * is handled like the receipt of serial data on the "UART" + * + ****************************************************************************/ + +static void usbclass_rdcomplete(FAR struct usbdev_ep_s *ep, + FAR struct usbdev_req_s *req) +{ + FAR struct usbser_dev_s *priv; + irqstate_t flags; + int ret; + + /* Sanity check */ + +#ifdef CONFIG_DEBUG + if (!ep || !ep->priv || !req) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_INVALIDARG), 0); + return; + } +#endif + + /* Extract references to private data */ + + priv = (FAR struct usbser_dev_s*)ep->priv; + + /* Process the received data unless this is some unusual condition */ + + flags = irqsave(); + switch (req->result) + { + case 0: /* Normal completion */ + usbtrace(TRACE_CLASSRDCOMPLETE, priv->nrdq); + usbclass_recvpacket(priv, req->buf, req->xfrd); + break; + + case -ESHUTDOWN: /* Disconnection */ + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_RDSHUTDOWN), 0); + priv->nrdq--; + irqrestore(flags); + return; + + default: /* Some other error occurred */ + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_RDUNEXPECTED), (uint16_t)-req->result); + break; + }; + + /* Requeue the read request */ + +#ifdef CONFIG_USBSER_BULKREQLEN + req->len = max(CONFIG_USBSER_BULKREQLEN, ep->maxpacket); +#else + req->len = ep->maxpacket; +#endif + + ret = EP_SUBMIT(ep, req); + if (ret != OK) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_RDSUBMIT), (uint16_t)-req->result); + } + irqrestore(flags); +} + +/**************************************************************************** + * Name: usbclass_wrcomplete + * + * Description: + * Handle completion of write request. This function probably executes + * in the context of an interrupt handler. + * + ****************************************************************************/ + +static void usbclass_wrcomplete(FAR struct usbdev_ep_s *ep, + FAR struct usbdev_req_s *req) +{ + FAR struct usbser_dev_s *priv; + FAR struct usbser_req_s *reqcontainer; + irqstate_t flags; + + /* Sanity check */ + +#ifdef CONFIG_DEBUG + if (!ep || !ep->priv || !req || !req->priv) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_INVALIDARG), 0); + return; + } +#endif + + /* Extract references to our private data */ + + priv = (FAR struct usbser_dev_s *)ep->priv; + reqcontainer = (FAR struct usbser_req_s *)req->priv; + + /* Return the write request to the free list */ + + flags = irqsave(); + sq_addlast((sq_entry_t*)reqcontainer, &priv->reqlist); + priv->nwrq++; + irqrestore(flags); + + /* Send the next packet unless this was some unusual termination + * condition + */ + + switch (req->result) + { + case OK: /* Normal completion */ + usbtrace(TRACE_CLASSWRCOMPLETE, priv->nwrq); + usbclass_sndpacket(priv); + break; + + case -ESHUTDOWN: /* Disconnection */ + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_WRSHUTDOWN), priv->nwrq); + break; + + default: /* Some other error occurred */ + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_WRUNEXPECTED), (uint16_t)-req->result); + break; + } +} + +/**************************************************************************** + * USB Class Driver Methods + ****************************************************************************/ + +/**************************************************************************** + * Name: usbclass_bind + * + * Description: + * Invoked when the driver is bound to a USB device driver + * + ****************************************************************************/ + +static int usbclass_bind(FAR struct usbdev_s *dev, FAR struct usbdevclass_driver_s *driver) +{ + FAR struct usbser_dev_s *priv = ((struct usbser_driver_s*)driver)->dev; + FAR struct usbser_req_s *reqcontainer; + irqstate_t flags; + uint16_t reqlen; + int ret; + int i; + + usbtrace(TRACE_CLASSBIND, 0); + + /* Bind the structures */ + + priv->usbdev = dev; + dev->ep0->priv = priv; + + /* Preallocate control request */ + + priv->ctrlreq = usbclass_allocreq(dev->ep0, USBSER_MXDESCLEN); + if (priv->ctrlreq == NULL) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_ALLOCCTRLREQ), 0); + ret = -ENOMEM; + goto errout; + } + priv->ctrlreq->callback = usbclass_ep0incomplete; + + /* Pre-allocate all endpoints... the endpoints will not be functional + * until the SET CONFIGURATION request is processed in usbclass_setconfig. + * This is done here because there may be calls to kmalloc and the SET + * CONFIGURATION processing probably occurrs within interrupt handling + * logic where kmalloc calls will fail. + */ + + /* Pre-allocate the IN interrupt endpoint */ + + priv->epintin = DEV_ALLOCEP(dev, USBSER_EPINTIN_ADDR, true, USB_EP_ATTR_XFER_INT); + if (!priv->epintin) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_EPINTINALLOCFAIL), 0); + ret = -ENODEV; + goto errout; + } + priv->epintin->priv = priv; + + /* Pre-allocate the IN bulk endpoint */ + + priv->epbulkin = DEV_ALLOCEP(dev, USBSER_EPINBULK_ADDR, true, USB_EP_ATTR_XFER_BULK); + if (!priv->epbulkin) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_EPBULKINALLOCFAIL), 0); + ret = -ENODEV; + goto errout; + } + priv->epbulkin->priv = priv; + + /* Pre-allocate the OUT bulk endpoint */ + + priv->epbulkout = DEV_ALLOCEP(dev, USBSER_EPOUTBULK_ADDR, false, USB_EP_ATTR_XFER_BULK); + if (!priv->epbulkout) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_EPBULKOUTALLOCFAIL), 0); + ret = -ENODEV; + goto errout; + } + priv->epbulkout->priv = priv; + + /* Pre-allocate read requests */ + +#ifdef CONFIG_USBSER_BULKREQLEN + reqlen = max(CONFIG_USBSER_BULKREQLEN, priv->epbulkout->maxpacket); +#else + reqlen = priv->epbulkout->maxpacket; +#endif + + for (i = 0; i < CONFIG_USBSER_NRDREQS; i++) + { + reqcontainer = &priv->rdreqs[i]; + reqcontainer->req = usbclass_allocreq(priv->epbulkout, reqlen); + if (reqcontainer->req == NULL) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_RDALLOCREQ), -ENOMEM); + ret = -ENOMEM; + goto errout; + } + reqcontainer->req->priv = reqcontainer; + reqcontainer->req->callback = usbclass_rdcomplete; + } + + /* Pre-allocate write request containers and put in a free list */ + +#ifdef CONFIG_USBSER_BULKREQLEN + reqlen = max(CONFIG_USBSER_BULKREQLEN, priv->epbulkin->maxpacket); +#else + reqlen = priv->epbulkin->maxpacket; +#endif + + for (i = 0; i < CONFIG_USBSER_NWRREQS; i++) + { + reqcontainer = &priv->wrreqs[i]; + reqcontainer->req = usbclass_allocreq(priv->epbulkin, reqlen); + if (reqcontainer->req == NULL) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_WRALLOCREQ), -ENOMEM); + ret = -ENOMEM; + goto errout; + } + reqcontainer->req->priv = reqcontainer; + reqcontainer->req->callback = usbclass_wrcomplete; + + flags = irqsave(); + sq_addlast((sq_entry_t*)reqcontainer, &priv->reqlist); + priv->nwrq++; /* Count of write requests available */ + irqrestore(flags); + } + + /* Report if we are selfpowered */ + +#ifdef CONFIG_USBDEV_SELFPOWERED + DEV_SETSELFPOWERED(dev); +#endif + + /* And pull-up the data line for the soft connect function */ + + DEV_CONNECT(dev); + return OK; + +errout: + usbclass_unbind(dev); + return ret; +} + +/**************************************************************************** + * Name: usbclass_unbind + * + * Description: + * Invoked when the driver is unbound from a USB device driver + * + ****************************************************************************/ + +static void usbclass_unbind(FAR struct usbdev_s *dev) +{ + FAR struct usbser_dev_s *priv; + FAR struct usbser_req_s *reqcontainer; + irqstate_t flags; + int i; + + usbtrace(TRACE_CLASSUNBIND, 0); + +#ifdef CONFIG_DEBUG + if (!dev || !dev->ep0) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_INVALIDARG), 0); + return; + } +#endif + + /* Extract reference to private data */ + + priv = (FAR struct usbser_dev_s *)dev->ep0->priv; + +#ifdef CONFIG_DEBUG + if (!priv) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_EP0NOTBOUND), 0); + return; + } +#endif + + /* Make sure that we are not already unbound */ + + if (priv != NULL) + { + /* Make sure that the endpoints have been unconfigured. If + * we were terminated gracefully, then the configuration should + * already have been reset. If not, then calling usbclass_resetconfig + * should cause the endpoints to immediately terminate all + * transfers and return the requests to us (with result == -ESHUTDOWN) + */ + + usbclass_resetconfig(priv); + up_mdelay(50); + + /* Free the interrupt IN endpoint */ + + if (priv->epintin) + { + DEV_FREEEP(dev, priv->epintin); + priv->epintin = NULL; + } + + /* Free the bulk IN endpoint */ + + if (priv->epbulkin) + { + DEV_FREEEP(dev, priv->epbulkin); + priv->epbulkin = NULL; + } + + /* Free the pre-allocated control request */ + + if (priv->ctrlreq != NULL) + { + usbclass_freereq(dev->ep0, priv->ctrlreq); + priv->ctrlreq = NULL; + } + + /* Free pre-allocated read requests (which should all have + * been returned to the free list at this time -- we don't check) + */ + + DEBUGASSERT(priv->nrdq == 0); + for (i = 0; i < CONFIG_USBSER_NRDREQS; i++) + { + reqcontainer = &priv->rdreqs[i]; + if (reqcontainer->req) + { + usbclass_freereq(priv->epbulkout, reqcontainer->req); + reqcontainer->req = NULL; + } + } + + /* Free the bulk OUT endpoint */ + + if (priv->epbulkout) + { + DEV_FREEEP(dev, priv->epbulkout); + priv->epbulkout = NULL; + } + + /* Free write requests that are not in use (which should be all + * of them + */ + + flags = irqsave(); + DEBUGASSERT(priv->nwrq == CONFIG_USBSER_NWRREQS); + while (!sq_empty(&priv->reqlist)) + { + reqcontainer = (struct usbser_req_s *)sq_remfirst(&priv->reqlist); + if (reqcontainer->req != NULL) + { + usbclass_freereq(priv->epbulkin, reqcontainer->req); + priv->nwrq--; /* Number of write requests queued */ + } + } + DEBUGASSERT(priv->nwrq == 0); + irqrestore(flags); + } + + /* Clear out all data in the circular buffer */ + + priv->serdev.xmit.head = 0; + priv->serdev.xmit.tail = 0; +} + +/**************************************************************************** + * Name: usbclass_setup + * + * Description: + * Invoked for ep0 control requests. This function probably executes + * in the context of an interrupt handler. + * + ****************************************************************************/ + +static int usbclass_setup(FAR struct usbdev_s *dev, const struct usb_ctrlreq_s *ctrl) +{ + FAR struct usbser_dev_s *priv; + FAR struct usbdev_req_s *ctrlreq; + uint16_t value; + uint16_t index; + uint16_t len; + int ret = -EOPNOTSUPP; + +#ifdef CONFIG_DEBUG + if (!dev || !dev->ep0 || !ctrl) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_INVALIDARG), 0); + return -EIO; + } +#endif + + /* Extract reference to private data */ + + usbtrace(TRACE_CLASSSETUP, ctrl->req); + priv = (FAR struct usbser_dev_s *)dev->ep0->priv; + +#ifdef CONFIG_DEBUG + if (!priv || !priv->ctrlreq) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_EP0NOTBOUND), 0); + return -ENODEV; + } +#endif + ctrlreq = priv->ctrlreq; + + /* Extract the little-endian 16-bit values to host order */ + + value = GETUINT16(ctrl->value); + index = GETUINT16(ctrl->index); + len = GETUINT16(ctrl->len); + + uvdbg("type=%02x req=%02x value=%04x index=%04x len=%04x\n", + ctrl->type, ctrl->req, value, index, len); + + switch (ctrl->type & USB_REQ_TYPE_MASK) + { + /*********************************************************************** + * Standard Requests + ***********************************************************************/ + + case USB_REQ_TYPE_STANDARD: + { + switch (ctrl->req) + { + case USB_REQ_GETDESCRIPTOR: + { + /* The value field specifies the descriptor type in the MS byte and the + * descriptor index in the LS byte (order is little endian) + */ + + switch (ctrl->value[1]) + { + case USB_DESC_TYPE_DEVICE: + { + ret = USB_SIZEOF_DEVDESC; + memcpy(ctrlreq->buf, &g_devdesc, ret); + } + break; + +#ifdef CONFIG_USBDEV_DUALSPEED + case USB_DESC_TYPE_DEVICEQUALIFIER: + { + ret = USB_SIZEOF_QUALDESC; + memcpy(ctrlreq->buf, &g_qualdesc, ret); + } + break; + + case USB_DESC_TYPE_OTHERSPEEDCONFIG: +#endif /* CONFIG_USBDEV_DUALSPEED */ + + case USB_DESC_TYPE_CONFIG: + { +#ifdef CONFIG_USBDEV_DUALSPEED + ret = usbclass_mkcfgdesc(ctrlreq->buf, dev->speed, ctrl->req); +#else + ret = usbclass_mkcfgdesc(ctrlreq->buf); +#endif + } + break; + + case USB_DESC_TYPE_STRING: + { + /* index == language code. */ + + ret = usbclass_mkstrdesc(ctrl->value[0], (struct usb_strdesc_s *)ctrlreq->buf); + } + break; + + default: + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_GETUNKNOWNDESC), value); + } + break; + } + } + break; + + case USB_REQ_SETCONFIGURATION: + { + if (ctrl->type == 0) + { + ret = usbclass_setconfig(priv, value); + } + } + break; + + case USB_REQ_GETCONFIGURATION: + { + if (ctrl->type == USB_DIR_IN) + { + *(uint8_t*)ctrlreq->buf = priv->config; + ret = 1; + } + } + break; + + case USB_REQ_SETINTERFACE: + { + if (ctrl->type == USB_REQ_RECIPIENT_INTERFACE) + { + if (priv->config == USBSER_CONFIGID && + index == USBSER_INTERFACEID && + value == USBSER_ALTINTERFACEID) + { + usbclass_resetconfig(priv); + usbclass_setconfig(priv, priv->config); + ret = 0; + } + } + } + break; + + case USB_REQ_GETINTERFACE: + { + if (ctrl->type == (USB_DIR_IN|USB_REQ_RECIPIENT_INTERFACE) && + priv->config == USBSER_CONFIGIDNONE) + { + if (index != USBSER_INTERFACEID) + { + ret = -EDOM; + } + else + { + *(uint8_t*) ctrlreq->buf = USBSER_ALTINTERFACEID; + ret = 1; + } + } + } + break; + + default: + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_UNSUPPORTEDSTDREQ), ctrl->req); + break; + } + } + break; + + /*********************************************************************** + * PL2303 Vendor-Specific Requests + ***********************************************************************/ + + case PL2303_CONTROL_TYPE: + { + if ((ctrl->type & USB_REQ_RECIPIENT_MASK) == USB_REQ_RECIPIENT_INTERFACE) + { + switch (ctrl->req) + { + case PL2303_SETLINEREQUEST: + { + memcpy(priv->linest, ctrlreq->buf, min(len, 7)); + ret = 0; + } + break; + + + case PL2303_GETLINEREQUEST: + { + memcpy(ctrlreq->buf, priv->linest, 7); + ret = 7; + } + break; + + case PL2303_SETCONTROLREQUEST: + case PL2303_BREAKREQUEST: + { + ret = 0; + } + break; + + default: + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_UNSUPPORTEDCLASSREQ), ctrl->type); + break; + } + } + } + break; + + case PL2303_RWREQUEST_TYPE: + { + if ((ctrl->type & USB_REQ_RECIPIENT_MASK) == USB_REQ_RECIPIENT_DEVICE) + { + if (ctrl->req == PL2303_RWREQUEST) + { + if ((ctrl->type & USB_DIR_IN) != 0) + { + *(uint32_t*)ctrlreq->buf = 0xdeadbeef; + ret = 4; + } + else + { + ret = 0; + } + } + else + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_UNSUPPORTEDCLASSREQ), ctrl->type); + } + } + } + break; + + default: + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_UNSUPPORTEDTYPE), ctrl->type); + break; + } + + /* Respond to the setup command if data was returned. On an error return + * value (ret < 0), the USB driver will stall. + */ + + if (ret >= 0) + { + ctrlreq->len = min(len, ret); + ctrlreq->flags = USBDEV_REQFLAGS_NULLPKT; + ret = EP_SUBMIT(dev->ep0, ctrlreq); + if (ret < 0) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_EPRESPQ), (uint16_t)-ret); + ctrlreq->result = OK; + usbclass_ep0incomplete(dev->ep0, ctrlreq); + } + } + + return ret; +} + +/**************************************************************************** + * Name: usbclass_disconnect + * + * Description: + * Invoked after all transfers have been stopped, when the host is + * disconnected. This function is probably called from the context of an + * interrupt handler. + * + ****************************************************************************/ + +static void usbclass_disconnect(FAR struct usbdev_s *dev) +{ + FAR struct usbser_dev_s *priv; + irqstate_t flags; + + usbtrace(TRACE_CLASSDISCONNECT, 0); + +#ifdef CONFIG_DEBUG + if (!dev || !dev->ep0) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_INVALIDARG), 0); + return; + } +#endif + + /* Extract reference to private data */ + + priv = (FAR struct usbser_dev_s *)dev->ep0->priv; + +#ifdef CONFIG_DEBUG + if (!priv) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_EP0NOTBOUND), 0); + return; + } +#endif + + /* Reset the configuration */ + + flags = irqsave(); + usbclass_resetconfig(priv); + + /* Clear out all data in the circular buffer */ + + priv->serdev.xmit.head = 0; + priv->serdev.xmit.tail = 0; + irqrestore(flags); + + /* Perform the soft connect function so that we will we can be + * re-enumerated. + */ + + DEV_CONNECT(dev); +} + +/**************************************************************************** + * Serial Device Methods + ****************************************************************************/ + +/**************************************************************************** + * Name: usbser_setup + * + * Description: + * This method is called the first time that the serial port is opened. + * + ****************************************************************************/ + +static int usbser_setup(FAR struct uart_dev_s *dev) +{ + FAR struct usbser_dev_s *priv; + + usbtrace(USBSER_CLASSAPI_SETUP, 0); + + /* Sanity check */ + +#if CONFIG_DEBUG + if (!dev || !dev->priv) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_INVALIDARG), 0); + return -EIO; + } +#endif + + /* Extract reference to private data */ + + priv = (FAR struct usbser_dev_s*)dev->priv; + + /* Check if we have been configured */ + + if (priv->config == USBSER_CONFIGIDNONE) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_SETUPNOTCONNECTED), 0); + return -ENOTCONN; + } + + return OK; +} + +/**************************************************************************** + * Name: usbser_shutdown + * + * Description: + * This method is called when the serial port is closed. This operation + * is very simple for the USB serial backend because the serial driver + * has already assured that the TX data has full drained -- it calls + * usbser_txempty() until that function returns true before calling this + * function. + * + ****************************************************************************/ + +static void usbser_shutdown(FAR struct uart_dev_s *dev) +{ + usbtrace(USBSER_CLASSAPI_SHUTDOWN, 0); + + /* Sanity check */ + +#if CONFIG_DEBUG + if (!dev || !dev->priv) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_INVALIDARG), 0); + } +#endif +} + +/**************************************************************************** + * Name: usbser_attach + * + * Description: + * Does not apply to the USB serial class device + * + ****************************************************************************/ + +static int usbser_attach(FAR struct uart_dev_s *dev) +{ + usbtrace(USBSER_CLASSAPI_ATTACH, 0); + return OK; +} + +/**************************************************************************** + * Name: usbser_detach + * + * Description: +* Does not apply to the USB serial class device + * + ****************************************************************************/ + +static void usbser_detach(FAR struct uart_dev_s *dev) +{ + usbtrace(USBSER_CLASSAPI_DETACH, 0); +} + +/**************************************************************************** + * Name: usbser_rxint + * + * Description: + * Called by the serial driver to enable or disable RX interrupts. We, of + * course, have no RX interrupts but must behave consistently. This method + * is called under the conditions: + * + * 1. With enable==true when the port is opened (just after usbser_setup + * and usbser_attach are called called) + * 2. With enable==false while transferring data from the RX buffer + * 2. With enable==true while waiting for more incoming data + * 3. With enable==false when the port is closed (just before usbser_detach + * and usbser_shutdown are called). + * + ****************************************************************************/ + +static void usbser_rxint(FAR struct uart_dev_s *dev, bool enable) +{ + FAR struct usbser_dev_s *priv; + FAR uart_dev_t *serdev; + irqstate_t flags; + + usbtrace(USBSER_CLASSAPI_RXINT, (uint16_t)enable); + + /* Sanity check */ + +#if CONFIG_DEBUG + if (!dev || !dev->priv) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_INVALIDARG), 0); + return; + } +#endif + + /* Extract reference to private data */ + + priv = (FAR struct usbser_dev_s*)dev->priv; + serdev = &priv->serdev; + + /* We need exclusive access to the RX buffer and private structure + * in the following. + */ + + flags = irqsave(); + if (enable) + { + /* RX "interrupts" are enabled. Is this a transition from disabled + * to enabled state? + */ + + if (!priv->rxenabled) + { + /* Yes. During the time that RX interrupts are disabled, the + * the serial driver will be extracting data from the circular + * buffer and modifying recv.tail. During this time, we + * should avoid modifying recv.head; When interrupts are restored, + * we can update the head pointer for all of the data that we + * put into cicular buffer while "interrupts" were disabled. + */ + + if (priv->rxhead != serdev->recv.head) + { + serdev->recv.head = priv->rxhead; + + /* Yes... signal the availability of new data */ + + uart_datareceived(serdev); + } + + /* RX "interrupts are no longer disabled */ + + priv->rxenabled = true; + } + } + + /* RX "interrupts" are disabled. Is this a transition from enabled + * to disabled state? + */ + + else if (priv->rxenabled) + { + /* Yes. During the time that RX interrupts are disabled, the + * the serial driver will be extracting data from the circular + * buffer and modifying recv.tail. During this time, we + * should avoid modifying recv.head; When interrupts are disabled, + * we use a shadow index and continue adding data to the circular + * buffer. + */ + + priv->rxhead = serdev->recv.head; + priv->rxenabled = false; + } + irqrestore(flags); +} + +/**************************************************************************** + * Name: usbser_txint + * + * Description: + * Called by the serial driver to enable or disable TX interrupts. We, of + * course, have no TX interrupts but must behave consistently. Initially, + * TX interrupts are disabled. This method is called under the conditions: + * + * 1. With enable==false while transferring data into the TX buffer + * 2. With enable==true when data may be taken from the buffer. + * 3. With enable==false when the TX buffer is empty + * + ****************************************************************************/ + +static void usbser_txint(FAR struct uart_dev_s *dev, bool enable) +{ + FAR struct usbser_dev_s *priv; + + usbtrace(USBSER_CLASSAPI_TXINT, (uint16_t)enable); + + /* Sanity checks */ + +#if CONFIG_DEBUG + if (!dev || !dev->priv) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_INVALIDARG), 0); + return; + } +#endif + + /* Extract references to private data */ + + priv = (FAR struct usbser_dev_s*)dev->priv; + + /* If the new state is enabled and if there is data in the XMIT buffer, + * send the next packet now. + */ + + uvdbg("enable=%d head=%d tail=%d\n", + enable, priv->serdev.xmit.head, priv->serdev.xmit.tail); + + if (enable && priv->serdev.xmit.head != priv->serdev.xmit.tail) + { + usbclass_sndpacket(priv); + } +} + +/**************************************************************************** + * Name: usbser_txempty + * + * Description: + * Return true when all data has been sent. This is called from the + * serial driver when the driver is closed. It will call this API + * periodically until it reports true. NOTE that the serial driver takes all + * responsibility for flushing TX data through the hardware so we can be + * a bit sloppy about that. + * + ****************************************************************************/ + +static bool usbser_txempty(FAR struct uart_dev_s *dev) +{ + FAR struct usbser_dev_s *priv = (FAR struct usbser_dev_s*)dev->priv; + + usbtrace(USBSER_CLASSAPI_TXEMPTY, 0); + +#if CONFIG_DEBUG + if (!priv) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_INVALIDARG), 0); + return true; + } +#endif + + /* When all of the allocated write requests have been returned to the + * reqlist, then there is no longer any TX data in flight. + */ + + return priv->nwrq >= CONFIG_USBSER_NWRREQS; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: usbdev_serialinitialize + * + * Description: + * Register USB serial port (and USB serial console if so configured). + * + ****************************************************************************/ + +int usbdev_serialinitialize(int minor) +{ + FAR struct usbser_alloc_s *alloc; + FAR struct usbser_dev_s *priv; + FAR struct usbser_driver_s *drvr; + char devname[16]; + int ret; + + /* Allocate the structures needed */ + + alloc = (FAR struct usbser_alloc_s*)kmalloc(sizeof(struct usbser_alloc_s)); + if (!alloc) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_ALLOCDEVSTRUCT), 0); + return -ENOMEM; + } + + /* Convenience pointers into the allocated blob */ + + priv = &alloc->dev; + drvr = &alloc->drvr; + + /* Initialize the USB serial driver structure */ + + memset(priv, 0, sizeof(struct usbser_dev_s)); + sq_init(&priv->reqlist); + + /* Fake line status */ + + priv->linest[0] = (115200) & 0xff; /* Baud=115200 */ + priv->linest[1] = (115200 >> 8) & 0xff; + priv->linest[2] = (115200 >> 16) & 0xff; + priv->linest[3] = (115200 >> 24) & 0xff; + priv->linest[4] = 0; /* One stop bit */ + priv->linest[5] = 0; /* No parity */ + priv->linest[6] = 8; /*8 data bits */ + + /* Initialize the serial driver sub-structure */ + + priv->serdev.recv.size = CONFIG_USBSER_RXBUFSIZE; + priv->serdev.recv.buffer = priv->rxbuffer; + priv->serdev.xmit.size = CONFIG_USBSER_TXBUFSIZE; + priv->serdev.xmit.buffer = priv->txbuffer; + priv->serdev.ops = &g_uartops; + priv->serdev.priv = priv; + + /* Initialize the USB class driver structure */ + +#ifdef CONFIG_USBDEV_DUALSPEED + drvr->drvr.speed = USB_SPEED_HIGH; +#else + drvr->drvr.speed = USB_SPEED_FULL; +#endif + drvr->drvr.ops = &g_driverops; + drvr->dev = priv; + + /* Register the USB serial class driver */ + + ret = usbdev_register(&drvr->drvr); + if (ret) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_DEVREGISTER), (uint16_t)-ret); + goto errout_with_alloc; + } + + /* Register the USB serial console */ + +#ifdef CONFIG_USBSER_CONSOLE + g_usbserialport.isconsole = true; + ret = uart_register("/dev/console", &pri->serdev); + if (ret < 0) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_CONSOLEREGISTER), (uint16_t)-ret); + goto errout_with_class; + } +#endif + + /* Register the single port supported by this implementation */ + + sprintf(devname, "/dev/ttyUSB%d", minor); + ret = uart_register(devname, &priv->serdev); + if (ret) + { + usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_UARTREGISTER), (uint16_t)-ret); + goto errout_with_class; + } + return OK; + +errout_with_class: + usbdev_unregister(&drvr->drvr); +errout_with_alloc: + kfree(alloc); + return ret; +} diff --git a/nuttx/drivers/usbdev/usbdev_composite.c b/nuttx/drivers/usbdev/usbdev_composite.c deleted file mode 100644 index de531b8a4..000000000 --- a/nuttx/drivers/usbdev/usbdev_composite.c +++ /dev/null @@ -1,85 +0,0 @@ -/**************************************************************************** - * drivers/usbdev/usbdev_composite.c - * - * Copyright (C) 2011 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 - * 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 - -#include -#include -#include -#include - -#include - -#include "usbdev_composite.h" - -/**************************************************************************** - * Definitions - ****************************************************************************/ - -/**************************************************************************** - * Private Types - ****************************************************************************/ - -/**************************************************************************** - * Private Function Prototypes - ****************************************************************************/ - -/**************************************************************************** - * Private Data - ****************************************************************************/ - -/**************************************************************************** - * Private Functions - ****************************************************************************/ - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -/******************************************************************************* - * Name: - * - * Description: - * - * Input Parameters: - * - * Returned Value: - * - *******************************************************************************/ - diff --git a/nuttx/drivers/usbdev/usbdev_composite.h b/nuttx/drivers/usbdev/usbdev_composite.h deleted file mode 100644 index 472f523b2..000000000 --- a/nuttx/drivers/usbdev/usbdev_composite.h +++ /dev/null @@ -1,86 +0,0 @@ -/**************************************************************************** - * drivers/usbdev/usbdev_composite.h - * - * Copyright (C) 2011 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 - * 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. - * - ****************************************************************************/ - -#ifndef __DRIVERS_USBDEV_USBDEV_COMPOSITE_H -#define __DRIVERS_USBDEV_USBDEV_COMPOSITE_H 1 - -/**************************************************************************** - * Included Files - ****************************************************************************/ - -#include - -#include -#include - -#include - -/**************************************************************************** - * Definitions - ****************************************************************************/ -/* Configuration ************************************************************/ - -#ifndef CONFIG_USBDEV_TRACE_NRECORDS -# define CONFIG_USBDEV_TRACE_NRECORDS 128 -#endif - -#ifndef CONFIG_USBDEV_TRACE_INITIALIDSET -# define CONFIG_USBDEV_TRACE_INITIALIDSET 0 -#endif - -/**************************************************************************** - * Public Types - ****************************************************************************/ - -/**************************************************************************** - * Public Data - ****************************************************************************/ - -/**************************************************************************** - * Public Function Prototypes - ****************************************************************************/ - -/******************************************************************************* - * Name: - * - * Description: - * - * Input Parameters: - * - * Returned Value: - * - *******************************************************************************/ - -#endif /* __DRIVERS_USBDEV_USBDEV_COMPOSITE_H */ diff --git a/nuttx/drivers/usbdev/usbdev_scsi.c b/nuttx/drivers/usbdev/usbdev_scsi.c deleted file mode 100644 index a0bbf8c2a..000000000 --- a/nuttx/drivers/usbdev/usbdev_scsi.c +++ /dev/null @@ -1,2664 +0,0 @@ -/**************************************************************************** - * drivers/usbdev/usbdev_storage.c - * - * Copyright (C) 2008-2010 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * Mass storage class device. Bulk-only with SCSI subclass. - * - * References: - * "Universal Serial Bus Mass Storage Class, Specification Overview," - * Revision 1.2, USB Implementer's Forum, June 23, 2003. - * - * "Universal Serial Bus Mass Storage Class, Bulk-Only Transport," - * Revision 1.0, USB Implementer's Forum, September 31, 1999. - * - * "SCSI Primary Commands - 3 (SPC-3)," American National Standard - * for Information Technology, May 4, 2005 - * - * "SCSI Primary Commands - 4 (SPC-4)," American National Standard - * for Information Technology, July 19, 2008 - * - * "SCSI Block Commands -2 (SBC-2)," American National Standard - * for Information Technology, November 13, 2004 - * - * 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 - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include "usbdev_storage.h" - -/**************************************************************************** - * Pre-processor Definitions - ****************************************************************************/ - -/* Configuration ************************************************************/ - -/* Race condition workaround found by David Hewson. This race condition - * "seems to relate to stalling the endpoint when a short response is - * generated which causes a residue to exist when the CSW would be returned. - * I think there's two issues here. The first being if the transfer which - * had just been queued before the stall had not completed then it wouldn’t - * then complete once the endpoint was stalled? The second is that the - * subsequent transfer for the CSW would be dropped on the floor (by the - * epsubmit() function) if the end point was still stalled as the control - * transfer to resume it hadn't occurred." - */ - -#define CONFIG_USBSTRG_RACEWAR 1 - -/**************************************************************************** - * Private Types - ****************************************************************************/ - -/**************************************************************************** - * Private Function Prototypes - ****************************************************************************/ - -/* Debug ********************************************************************/ - -#if defined(CONFIG_DEBUG_VERBOSE) && defined (CONFIG_DEBUG_USB) -static void usbstrg_dumpdata(const char *msg, const uint8_t *buf, - int buflen); -#else -# define usbstrg_dumpdata(msg, buf, len) -#endif - -/* Utility Support Functions ************************************************/ - -static uint16_t usbstrg_getbe16(uint8_t *buf); -static uint32_t usbstrg_getbe32(uint8_t *buf); -static void usbstrg_putbe16(uint8_t * buf, uint16_t val); -static void usbstrg_putbe24(uint8_t *buf, uint32_t val); -static void usbstrg_putbe32(uint8_t *buf, uint32_t val); -#if 0 /* not used */ -static uint16_t usbstrg_getle16(uint8_t *buf); -#endif -static uint32_t usbstrg_getle32(uint8_t *buf); -#if 0 /* not used */ -static void usbstrg_putle16(uint8_t * buf, uint16_t val); -#endif -static void usbstrg_putle32(uint8_t *buf, uint32_t val); - -/* SCSI Command Processing **************************************************/ - -static inline int usbstrg_cmdtestunitready(FAR struct usbstrg_dev_s *priv); -static inline int usbstrg_cmdrequestsense(FAR struct usbstrg_dev_s *priv, - FAR uint8_t *buf); -static inline int usbstrg_cmdread6(FAR struct usbstrg_dev_s *priv); -static inline int usbstrg_cmdwrite6(FAR struct usbstrg_dev_s *priv); -static inline int usbstrg_cmdinquiry(FAR struct usbstrg_dev_s *priv, - FAR uint8_t *buf); -static inline int usbstrg_cmdmodeselect6(FAR struct usbstrg_dev_s *priv); -static int usbstrg_modepage(FAR struct usbstrg_dev_s *priv, - FAR uint8_t *buf, uint8_t pcpgcode, int *mdlen); -static inline int usbstrg_cmdmodesense6(FAR struct usbstrg_dev_s *priv, - FAR uint8_t *buf); -static inline int usbstrg_cmdstartstopunit(FAR struct usbstrg_dev_s *priv); -static inline int usbstrg_cmdpreventmediumremoval(FAR struct usbstrg_dev_s *priv); -static inline int usbstrg_cmdreadformatcapacity(FAR struct usbstrg_dev_s *priv, - FAR uint8_t *buf); -static inline int usbstrg_cmdreadcapacity10(FAR struct usbstrg_dev_s *priv, - FAR uint8_t *buf); -static inline int usbstrg_cmdread10(FAR struct usbstrg_dev_s *priv); -static inline int usbstrg_cmdwrite10(FAR struct usbstrg_dev_s *priv); -static inline int usbstrg_cmdverify10(FAR struct usbstrg_dev_s *priv); -static inline int usbstrg_cmdsynchronizecache10(FAR struct usbstrg_dev_s *priv); -static inline int usbstrg_cmdmodeselect10(FAR struct usbstrg_dev_s *priv); -static inline int usbstrg_cmdmodesense10(FAR struct usbstrg_dev_s *priv, - FAR uint8_t *buf); -static inline int usbstrg_cmdread12(FAR struct usbstrg_dev_s *priv); -static inline int usbstrg_cmdwrite12(FAR struct usbstrg_dev_s *priv); -static inline int usbstrg_setupcmd(FAR struct usbstrg_dev_s *priv, - uint8_t cdblen, uint8_t flags); - -/* SCSI Worker Thread *******************************************************/ - -static int usbstrg_idlestate(FAR struct usbstrg_dev_s *priv); -static int usbstrg_cmdparsestate(FAR struct usbstrg_dev_s *priv); -static int usbstrg_cmdreadstate(FAR struct usbstrg_dev_s *priv); -static int usbstrg_cmdwritestate(FAR struct usbstrg_dev_s *priv); -static int usbstrg_cmdfinishstate(FAR struct usbstrg_dev_s *priv); -static int usbstrg_cmdstatusstate(FAR struct usbstrg_dev_s *priv); - -/**************************************************************************** - * Private Data - ****************************************************************************/ - -/**************************************************************************** - * Private Functions - ****************************************************************************/ - -/**************************************************************************** - * Debug - ****************************************************************************/ - -/**************************************************************************** - * Name: usbstrg_dumpdata - ****************************************************************************/ - -#if defined(CONFIG_DEBUG_VERBOSE) && defined (CONFIG_DEBUG_USB) -static void usbstrg_dumpdata(const char *msg, const uint8_t *buf, int buflen) -{ - int i; - - dbgprintf("%s:", msg); - for (i = 0; i < buflen; i++) - { - dbgprintf(" %02x", buf[i]); - } - dbgprintf("\n"); -} -#endif - -/**************************************************************************** - * Utility Support Functions - ****************************************************************************/ - -/**************************************************************************** - * Name: usbstrg_getbe16 - * - * Description: - * Get a 16-bit big-endian value reference by the byte pointer - * - ****************************************************************************/ - -static uint16_t usbstrg_getbe16(uint8_t *buf) -{ - return ((uint16_t)buf[0] << 8) | ((uint16_t)buf[1]); -} - -/**************************************************************************** - * Name: usbstrg_getbe32 - * - * Description: - * Get a 32-bit big-endian value reference by the byte pointer - * - ****************************************************************************/ - -static uint32_t usbstrg_getbe32(uint8_t *buf) -{ - return ((uint32_t)buf[0] << 24) | ((uint32_t)buf[1] << 16) | - ((uint32_t)buf[2] << 8) | ((uint32_t)buf[3]); -} - -/**************************************************************************** - * Name: usbstrg_putbe16 - * - * Description: - * Store a 16-bit value in big-endian order to the location specified by - * a byte pointer - * - ****************************************************************************/ - -static void usbstrg_putbe16(uint8_t * buf, uint16_t val) -{ - buf[0] = val >> 8; - buf[1] = val; -} - -/**************************************************************************** - * Name: usbstrg_putbe24 - * - * Description: - * Store a 32-bit value in big-endian order to the location specified by - * a byte pointer - * - ****************************************************************************/ - -static void usbstrg_putbe24(uint8_t *buf, uint32_t val) -{ - buf[0] = val >> 16; - buf[1] = val >> 8; - buf[2] = val; -} - -/**************************************************************************** - * Name: usbstrg_putbe32 - * - * Description: - * Store a 32-bit value in big-endian order to the location specified by - * a byte pointer - * - ****************************************************************************/ - -static void usbstrg_putbe32(uint8_t *buf, uint32_t val) -{ - buf[0] = val >> 24; - buf[1] = val >> 16; - buf[2] = val >> 8; - buf[3] = val; -} - -/**************************************************************************** - * Name: usbstrg_getle16 - * - * Description: - * Get a 16-bit little-endian value reference by the byte pointer - * - ****************************************************************************/ - -#if 0 /* not used */ -static uint16_t usbstrg_getle16(uint8_t *buf) -{ - return ((uint16_t)buf[1] << 8) | ((uint16_t)buf[0]); -} -#endif - -/**************************************************************************** - * Name: usbstrg_getle32 - * - * Description: - * Get a 32-bit little-endian value reference by the byte pointer - * - ****************************************************************************/ - -static uint32_t usbstrg_getle32(uint8_t *buf) -{ - return ((uint32_t)buf[3] << 24) | ((uint32_t)buf[2] << 16) | - ((uint32_t)buf[1] << 8) | ((uint32_t)buf[0]); -} - -/**************************************************************************** - * Name: usbstrg_putle16 - * - * Description: - * Store a 16-bit value in little-endian order to the location specified by - * a byte pointer - * - ****************************************************************************/ - -#if 0 /* not used */ -static void usbstrg_putle16(uint8_t * buf, uint16_t val) -{ - buf[0] = val; - buf[1] = val >> 8; -} -#endif - -/**************************************************************************** - * Name: usbstrg_putle32 - * - * Description: - * Store a 32-bit value in little-endian order to the location specified by - * a byte pointer - * - ****************************************************************************/ - -static void usbstrg_putle32(uint8_t *buf, uint32_t val) -{ - buf[0] = val; - buf[1] = val >> 8; - buf[2] = val >> 16; - buf[3] = val >> 24; -} - -/**************************************************************************** - * SCSI Worker Thread - ****************************************************************************/ - -/**************************************************************************** - * Name: usbstrg_cmdtestunitready - * - * Description: - * Handle the SCSI_CMD_TESTUNITREADY command - * - ****************************************************************************/ - -static inline int usbstrg_cmdtestunitready(FAR struct usbstrg_dev_s *priv) -{ - int ret; - - priv->u.alloclen = 0; - ret = usbstrg_setupcmd(priv, 6, USBSTRG_FLAGS_DIRNONE); - return ret; -} - -/**************************************************************************** - * Name: usbstrg_cmdrequestsense - * - * Description: - * Handle the SCSI_CMD_REQUESTSENSE command - * - ****************************************************************************/ - -static inline int usbstrg_cmdrequestsense(FAR struct usbstrg_dev_s *priv, - FAR uint8_t *buf) -{ - FAR struct scsicmd_requestsense_s *request = (FAR struct scsicmd_requestsense_s *)priv->cdb; - FAR struct scsiresp_fixedsensedata_s *response = (FAR struct scsiresp_fixedsensedata_s *)buf; - FAR struct usbstrg_lun_s *lun; - uint32_t sd; - uint32_t sdinfo; - uint8_t cdblen; - int ret; - - /* Extract the host allocation length */ - - priv->u.alloclen = request->alloclen; - - /* Get the expected length of the command (with hack for MS-Windows 12-byte - * REQUEST SENSE command. - */ - - cdblen = SCSICMD_REQUESTSENSE_SIZEOF; - if (cdblen != priv->cdblen) - { - /* Try MS-Windows REQUEST SENSE with cbw->cdblen == 12 */ - - cdblen = SCSICMD_REQUESTSENSE_MSSIZEOF; - } - - ret = usbstrg_setupcmd(priv, cdblen, - USBSTRG_FLAGS_DIRDEVICE2HOST|USBSTRG_FLAGS_LUNNOTNEEDED| - USBSTRG_FLAGS_UACOKAY|USBSTRG_FLAGS_RETAINSENSEDATA); - if (ret == OK) - { - lun = priv->lun; - if (!lun) - { - sd = SCSI_KCQIR_INVALIDLUN; - sdinfo = 0; - } - else - { - /* Get the saved sense data from the LUN structure */ - - sd = lun->sd; - sdinfo = lun->sdinfo; - - /* Discard the sense data */ - - lun->sd = SCSI_KCQ_NOSENSE; - lun->sdinfo = 0; - } - - /* Create the fixed sense data response */ - - memset(response, 0, SCSIRESP_FIXEDSENSEDATA_SIZEOF); - - response->code = SCSIRESP_SENSEDATA_RESPVALID|SCSIRESP_SENSEDATA_CURRENTFIXED; - response->flags = (uint8_t)(sd >> 16); - usbstrg_putbe32(response->info, sdinfo); - response->len = SCSIRESP_FIXEDSENSEDATA_SIZEOF - 7; - response->code2 = (uint8_t)(sd >> 8); - response->qual2 = (uint8_t)sd; - - priv->nreqbytes = SCSIRESP_FIXEDSENSEDATA_SIZEOF; - ret = OK; - } - - return ret; -} - -/**************************************************************************** - * Name: usbstrg_cmdread6 - * - * Description: - * Handle the SCSI_CMD_READ6 command - * - ****************************************************************************/ - -static inline int usbstrg_cmdread6(FAR struct usbstrg_dev_s *priv) -{ - FAR struct scsicmd_read6_s *read6 = (FAR struct scsicmd_read6_s*)priv->cdb; - FAR struct usbstrg_lun_s *lun = priv->lun; - int ret; - - priv->u.xfrlen = (uint16_t)read6->xfrlen; - if (priv->u.xfrlen == 0) - { - priv->u.xfrlen = 256; - } - - ret = usbstrg_setupcmd(priv, SCSICMD_READ6_SIZEOF, USBSTRG_FLAGS_DIRDEVICE2HOST|USBSTRG_FLAGS_BLOCKXFR); - if (ret == OK) - { - /* Get the Logical Block Address (LBA) from cdb[] as the starting sector */ - - priv->sector = (uint32_t)(read6->mslba & SCSICMD_READ6_MSLBAMASK) << 16 | (uint32_t)usbstrg_getbe16(read6->lslba); - - /* Verify that a block driver has been bound to the LUN */ - - if (!lun->inode) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_READ6MEDIANOTPRESENT), 0); - lun->sd = SCSI_KCQNR_MEDIANOTPRESENT; - ret = -EINVAL; - } - - /* Verify that sector lies in the range supported by the block driver */ - - else if (priv->sector >= lun->nsectors) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_READ6LBARANGE), 0); - lun->sd = SCSI_KCQIR_LBAOUTOFRANGE; - ret = -EINVAL; - } - - /* Looks like we are good to go */ - - else - { - usbtrace(TRACE_CLASSSTATE(USBSTRG_CLASSSTATE_CMDPARSECMDREAD6), priv->cdb[0]); - priv->thstate = USBSTRG_STATE_CMDREAD; - } - } - return ret; -} - -/**************************************************************************** - * Name: usbstrg_cmdwrite6 - * - * Description: - * Handle the SCSI_CMD_WRITE6 command - * - ****************************************************************************/ - -static inline int usbstrg_cmdwrite6(FAR struct usbstrg_dev_s *priv) -{ - FAR struct scsicmd_write6_s *write6 = (FAR struct scsicmd_write6_s *)priv->cdb; - FAR struct usbstrg_lun_s *lun = priv->lun; - int ret; - - priv->u.xfrlen = (uint16_t)write6->xfrlen; - if (priv->u.xfrlen == 0) - { - priv->u.xfrlen = 256; - } - - ret = usbstrg_setupcmd(priv, SCSICMD_WRITE6_SIZEOF, USBSTRG_FLAGS_DIRHOST2DEVICE|USBSTRG_FLAGS_BLOCKXFR); - if (ret == OK) - { - /* Get the Logical Block Address (LBA) from cdb[] as the starting sector */ - - priv->sector = (uint32_t)(write6->mslba & SCSICMD_WRITE6_MSLBAMASK) << 16 | (uint32_t)usbstrg_getbe16(write6->lslba); - - /* Verify that a block driver has been bound to the LUN */ - - if (!lun->inode) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_WRITE6MEDIANOTPRESENT), 0); - lun->sd = SCSI_KCQNR_MEDIANOTPRESENT; - ret = -EINVAL; - } - - /* Check for attempts to write to a read-only device */ - - else if (lun->readonly) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_WRITE6READONLY), 0); - lun->sd = SCSI_KCQWP_COMMANDNOTALLOWED; - ret = -EINVAL; - } - - /* Verify that sector lies in the range supported by the block driver */ - - else if (priv->sector >= lun->nsectors) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_WRITE6LBARANGE), 0); - lun->sd = SCSI_KCQIR_LBAOUTOFRANGE; - ret = -EINVAL; - } - - /* Looks like we are good to go */ - - else - { - usbtrace(TRACE_CLASSSTATE(USBSTRG_CLASSSTATE_CMDPARSECMDWRITE6), priv->cdb[0]); - priv->thstate = USBSTRG_STATE_CMDWRITE; - } - } - return ret; -} - -/**************************************************************************** - * Name: usbstrg_cmdinquiry - * - * Description: - * Handle SCSI_CMD_INQUIRY command - * - ****************************************************************************/ - -static inline int usbstrg_cmdinquiry(FAR struct usbstrg_dev_s *priv, - FAR uint8_t *buf) -{ - FAR struct scscicmd_inquiry_s *inquiry = (FAR struct scscicmd_inquiry_s *)priv->cdb; - FAR struct scsiresp_inquiry_s *response = (FAR struct scsiresp_inquiry_s *)buf; - int len; - int ret; - - priv->u.alloclen = usbstrg_getbe16(inquiry->alloclen); - ret = usbstrg_setupcmd(priv, SCSICMD_INQUIRY_SIZEOF, - USBSTRG_FLAGS_DIRDEVICE2HOST|USBSTRG_FLAGS_LUNNOTNEEDED|USBSTRG_FLAGS_UACOKAY); - if (ret == OK) - { - if (!priv->lun) - { - response->qualtype = SCSIRESP_INQUIRYPQ_NOTCAPABLE|SCSIRESP_INQUIRYPD_UNKNOWN; - } - else if ((inquiry->flags != 0) || (inquiry->pagecode != 0)) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_INQUIRYFLAGS), 0); - priv->lun->sd = SCSI_KCQIR_INVALIDFIELDINCBA; - ret = -EINVAL; - } - else - { - memset(response, 0, SCSIRESP_INQUIRY_SIZEOF); - priv->nreqbytes = SCSIRESP_INQUIRY_SIZEOF; - -#ifdef CONFIG_USBSTRG_REMOVABLE - response->flags1 = SCSIRESP_INQUIRYFLAGS1_RMB; -#endif - response->version = 2; /* SCSI-2 */ - response->flags2 = 2; /* SCSI-2 INQUIRY response data format */ - response->len = SCSIRESP_INQUIRY_SIZEOF - 5; - - /* Strings */ - - memset(response->vendorid, ' ', 8+16+4); - - len = strlen(g_vendorstr); - DEBUGASSERT(len <= 8); - memcpy(response->vendorid, g_vendorstr, len); - - len = strlen(g_productstr); - DEBUGASSERT(len <= 16); - memcpy(response->productid, g_productstr, len); - - len = strlen(g_serialstr); - DEBUGASSERT(len <= 4); - memcpy(response->productid, g_serialstr, len); - } - } - - return ret; -} - -/**************************************************************************** - * Name: usbstrg_cmdmodeselect6 - * - * Description: - * Handle SCSI_CMD_MODESELECT6 command - * - ****************************************************************************/ - -static inline int usbstrg_cmdmodeselect6(FAR struct usbstrg_dev_s *priv) -{ - FAR struct scsicmd_modeselect6_s *modeselect = (FAR struct scsicmd_modeselect6_s *)priv->cdb; - - priv->u.alloclen = modeselect->plen; - (void)usbstrg_setupcmd(priv, SCSICMD_MODESELECT6_SIZEOF, USBSTRG_FLAGS_DIRHOST2DEVICE); - - /* Not supported */ - - priv->lun->sd = SCSI_KCQIR_INVALIDCOMMAND; - return -EINVAL; -} - -/**************************************************************************** - * Name: usbstrg_modepage - * - * Description: - * Common logic for usbstrg_cmdmodesense6() and usbstrg_cmdmodesense10() - * - ****************************************************************************/ - -static int usbstrg_modepage(FAR struct usbstrg_dev_s *priv, FAR uint8_t *buf, - uint8_t pcpgcode, int *mdlen) -{ - FAR struct scsiresp_cachingmodepage_s *cmp = (FAR struct scsiresp_cachingmodepage_s *)buf; - - /* Saving parms not supported */ - - if ((pcpgcode & SCSICMD_MODESENSE_PCMASK) == SCSICMD_MODESENSE_PCSAVED) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_PCSAVED), 0); - priv->lun->sd = SCSI_KCQIR_SAVINGPARMSNOTSUPPORTED; - return -EINVAL; - } - - /* Only the caching mode page is supported: */ - - if ((pcpgcode & SCSICMD_MODESENSE_PGCODEMASK) == SCSIRESP_MODESENSE_PGCCODE_CACHING || - (pcpgcode & SCSICMD_MODESENSE_PGCODEMASK) == SCSIRESP_MODESENSE_PGCCODE_RETURNALL) - { - memset(cmp, 0, 12); - cmp->pgcode = SCSIRESP_MODESENSE_PGCCODE_CACHING; - cmp->len = 10; /* n-2 */ - - /* None of the fields are changeable */ - - if (((pcpgcode & SCSICMD_MODESENSE_PCMASK) != SCSICMD_MODESENSE_PCCHANGEABLE)) - { - cmp->flags1 = SCSIRESP_CACHINGMODEPG_WCE; /* Write cache enable */ - cmp->dpflen[0] = 0xff; /* Disable prefetch transfer length = 0xffffffff */ - cmp->dpflen[1] = 0xff; - cmp->maxpf[0] = 0xff; /* Maximum pre-fetch = 0xffffffff */ - cmp->maxpf[1] = 0xff; - cmp->maxpfc[0] = 0xff; /* Maximum pref-fetch ceiling = 0xffffffff */ - cmp->maxpfc[1] = 0xff; - } - - /* Return the mode data length */ - - *mdlen = 12; /* Only the first 12-bytes of caching mode page sent */ - return OK; - } - else - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_MODEPAGEFLAGS), pcpgcode); - priv->lun->sd = SCSI_KCQIR_INVALIDFIELDINCBA; - return -EINVAL; - } -} - -/**************************************************************************** - * Name: usbstrg_cmdmodesense6 - * - * Description: - * Handle SCSI_CMD_MODESENSE6 command - * - ****************************************************************************/ - -static int inline usbstrg_cmdmodesense6(FAR struct usbstrg_dev_s *priv, - FAR uint8_t *buf) -{ - FAR struct scsicmd_modesense6_s *modesense = (FAR struct scsicmd_modesense6_s *)priv->cdb; - FAR struct scsiresp_modeparameterhdr6_s *mph = (FAR struct scsiresp_modeparameterhdr6_s *)buf; - int mdlen; - int ret; - - priv->u.alloclen = modesense->alloclen; - ret = usbstrg_setupcmd(priv, SCSICMD_MODESENSE6_SIZEOF, USBSTRG_FLAGS_DIRDEVICE2HOST); - if (ret == OK) - { - if ((modesense->flags & ~SCSICMD_MODESENSE6_DBD) != 0 || modesense->subpgcode != 0) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_MODESENSE6FLAGS), 0); - priv->lun->sd = SCSI_KCQIR_INVALIDFIELDINCBA; - ret = -EINVAL; - } - else - { - /* The response consists of: - * - * (1) A MODESENSE6-specific mode parameter header, - * (2) A variable length list of block descriptors, and - * (3) A variable length list of mode page formats - */ - - mph->type = 0; /* Medium type */ - mph->param = (priv->lun->readonly ? SCSIRESP_MODEPARMHDR_DAPARM_WP : 0x00); - mph->bdlen = 0; /* Block descriptor length */ - - /* There are no block descriptors, only the following mode page: */ - - ret = usbstrg_modepage(priv, &buf[SCSIRESP_MODEPARAMETERHDR6_SIZEOF], modesense->pcpgcode, &mdlen); - if (ret == OK) - { - /* Store the mode data length and return the total message size */ - - mph->mdlen = mdlen - 1; - priv->nreqbytes = mdlen + SCSIRESP_MODEPARAMETERHDR6_SIZEOF; - } - } - } - return ret; -} - -/**************************************************************************** - * Name: usbstrg_cmdstartstopunit - * - * Description: - * Handle SCSI_CMD_STARTSTOPUNIT command - * - ****************************************************************************/ - -static inline int usbstrg_cmdstartstopunit(FAR struct usbstrg_dev_s *priv) -{ - int ret; - - priv->u.alloclen = 0; - ret = usbstrg_setupcmd(priv, SCSICMD_STARTSTOPUNIT_SIZEOF, USBSTRG_FLAGS_DIRNONE); - if (ret == OK) - { -#ifndef CONFIG_USBSTRG_REMOVABLE - /* This command is not valid if the media is not removable */ - - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_NOTREMOVABLE), 0); - lun->sd = SCSI_KCQIR_INVALIDCOMMAND; - ret = -EINVAL; -#endif - } - return ret; -} - -/**************************************************************************** - * Name: usbstrg_cmdpreventmediumremoval - * - * Description: - * Handle SCSI_CMD_PREVENTMEDIAREMOVAL command - * - ****************************************************************************/ - -static inline int usbstrg_cmdpreventmediumremoval(FAR struct usbstrg_dev_s *priv) -{ -#ifdef CONFIG_USBSTRG_REMOVABLE - FAR struct scsicmd_preventmediumremoval_s *pmr = (FAR struct scsicmd_preventmediumremoval_s *)priv->cdb; - FAR struct usbstrg_lun_s *lun = priv->lun; -#endif - int ret; - - priv->u.alloclen = 0; - ret = usbstrg_setupcmd(priv, SCSICMD_PREVENTMEDIUMREMOVAL_SIZEOF, USBSTRG_FLAGS_DIRNONE); - if (ret == OK) - { -#ifndef CONFIG_USBSTRG_REMOVABLE - lun->sd = SCSI_KCQIR_INVALIDCOMMAND; - ret = -EINVAL; -#else - if ((pmr->prevent & ~SCSICMD_PREVENTMEDIUMREMOVAL_TRANSPORT) != 0) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_PREVENTMEDIUMREMOVALPREVENT), 0); - lun->sd = SCSI_KCQIR_INVALIDFIELDINCBA; - ret = -EINVAL; - } - - lun->locked = pmr->prevent & SCSICMD_PREVENTMEDIUMREMOVAL_TRANSPORT; -#endif - } - return ret; -} - -/**************************************************************************** - * Name: usbstrg_cmdreadformatcapacity - * - * Description: - * Handle SCSI_CMD_READFORMATCAPACITIES command (MMC) - * - ****************************************************************************/ - -static inline int usbstrg_cmdreadformatcapacity(FAR struct usbstrg_dev_s *priv, - FAR uint8_t *buf) -{ - FAR struct scsicmd_readformatcapcacities_s *rfc = (FAR struct scsicmd_readformatcapcacities_s *)priv->cdb; - FAR struct scsiresp_readformatcapacities_s *hdr; - FAR struct usbstrg_lun_s *lun = priv->lun; - int ret; - - priv->u.alloclen = usbstrg_getbe16(rfc->alloclen); - ret = usbstrg_setupcmd(priv, SCSICMD_READFORMATCAPACITIES_SIZEOF, USBSTRG_FLAGS_DIRDEVICE2HOST); - if (ret == OK) - { - hdr = (FAR struct scsiresp_readformatcapacities_s *)buf; - memset(hdr, 0, SCSIRESP_READFORMATCAPACITIES_SIZEOF); - hdr->listlen = SCSIRESP_CURRCAPACITYDESC_SIZEOF; - - /* Only the Current/Maximum Capacity Descriptor follows the header */ - - usbstrg_putbe32(hdr->nblocks, lun->nsectors); - hdr->type = SCIRESP_RDFMTCAPACITIES_FORMATED; - usbstrg_putbe24(hdr->blocklen, lun->sectorsize); - priv->nreqbytes = SCSIRESP_READFORMATCAPACITIES_SIZEOF; - } - return ret; -} - -/**************************************************************************** - * Name: usbstrg_cmdreadcapacity10 - * - * Description: - * Handle SCSI_CMD_READCAPACITY10 command - * - ****************************************************************************/ - -static int inline usbstrg_cmdreadcapacity10(FAR struct usbstrg_dev_s *priv, - FAR uint8_t *buf) -{ - FAR struct scsicmd_readcapacity10_s *rcc = (FAR struct scsicmd_readcapacity10_s *)priv->cdb; - FAR struct scsiresp_readcapacity10_s *rcr = (FAR struct scsiresp_readcapacity10_s *)buf; - FAR struct usbstrg_lun_s *lun = priv->lun; - uint32_t lba; - int ret; - - priv->u.alloclen = SCSIRESP_READCAPACITY10_SIZEOF; /* Fake the allocation length */ - ret = usbstrg_setupcmd(priv, SCSICMD_READCAPACITY10_SIZEOF, USBSTRG_FLAGS_DIRDEVICE2HOST); - if (ret == OK) - { - /* Check the PMI and LBA fields */ - - lba = usbstrg_getbe32(rcc->lba); - - if (rcc->pmi > 1 || (rcc->pmi == 0 && lba != 0)) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_READCAPACITYFLAGS), 0); - lun->sd = SCSI_KCQIR_INVALIDFIELDINCBA; - ret = -EINVAL; - } - else - { - usbstrg_putbe32(rcr->lba, lun->nsectors - 1); - usbstrg_putbe32(&buf[4], lun->sectorsize); - priv->nreqbytes = SCSIRESP_READCAPACITY10_SIZEOF; - } - } - return ret; -} - -/**************************************************************************** - * Name: usbstrg_cmdread10 - * - * Description: - * Handle SCSI_CMD_READ10 command - * - ****************************************************************************/ - -static inline int usbstrg_cmdread10(FAR struct usbstrg_dev_s *priv) -{ - struct scsicmd_read10_s *read10 = (struct scsicmd_read10_s*)priv->cdb; - FAR struct usbstrg_lun_s *lun = priv->lun; - int ret; - - priv->u.xfrlen = usbstrg_getbe16(read10->xfrlen); - ret = usbstrg_setupcmd(priv, SCSICMD_READ10_SIZEOF, USBSTRG_FLAGS_DIRDEVICE2HOST|USBSTRG_FLAGS_BLOCKXFR); - if (ret == OK) - { - /* Get the Logical Block Address (LBA) from cdb[] as the starting sector */ - - priv->sector = usbstrg_getbe32(read10->lba); - - /* Verify that we can support this read command */ - - if ((read10->flags & ~(SCSICMD_READ10FLAGS_DPO|SCSICMD_READ10FLAGS_FUA)) != 0) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_READ10FLAGS), 0); - lun->sd = SCSI_KCQIR_INVALIDFIELDINCBA; - ret = -EINVAL; - } - - /* Verify that a block driver has been bound to the LUN */ - - else if (!lun->inode) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_READ10MEDIANOTPRESENT), 0); - lun->sd = SCSI_KCQNR_MEDIANOTPRESENT; - ret = -EINVAL; - } - - /* Verify that LBA lies in the range supported by the block driver */ - - else if (priv->sector >= lun->nsectors) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_READ10LBARANGE), 0); - lun->sd = SCSI_KCQIR_LBAOUTOFRANGE; - ret = -EINVAL; - } - - /* Looks like we are good to go */ - - else - { - usbtrace(TRACE_CLASSSTATE(USBSTRG_CLASSSTATE_CMDPARSECMDREAD10), priv->cdb[0]); - priv->thstate = USBSTRG_STATE_CMDREAD; - } - } - - return ret; -} - -/**************************************************************************** - * Name: usbstrg_cmdwrite10 - * - * Description: - * Handle SCSI_CMD_WRITE10 command - * - ****************************************************************************/ - -static inline int usbstrg_cmdwrite10(FAR struct usbstrg_dev_s *priv) -{ - struct scsicmd_write10_s *write10 = (struct scsicmd_write10_s *)priv->cdb; - FAR struct usbstrg_lun_s *lun = priv->lun; - int ret; - - priv->u.xfrlen = usbstrg_getbe16(write10->xfrlen); - ret = usbstrg_setupcmd(priv, SCSICMD_WRITE10_SIZEOF, USBSTRG_FLAGS_DIRHOST2DEVICE|USBSTRG_FLAGS_BLOCKXFR); - if (ret == OK) - { - /* Get the Logical Block Address (LBA) from cdb[] as the starting sector */ - - priv->sector = usbstrg_getbe32(write10->lba); - - /* Verify that we can support this write command */ - - if ((write10->flags & ~(SCSICMD_WRITE10FLAGS_DPO|SCSICMD_WRITE10FLAGS_FUA)) != 0) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_WRITE10FLAGS), 0); - lun->sd = SCSI_KCQIR_INVALIDFIELDINCBA; - ret = -EINVAL; - } - - /* Verify that a block driver has been bound to the LUN */ - - else if (!lun->inode) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_WRITE10MEDIANOTPRESENT), 0); - lun->sd = SCSI_KCQNR_MEDIANOTPRESENT; - ret = -EINVAL; - } - - /* Check for attempts to write to a read-only device */ - - else if (lun->readonly) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_WRITE10READONLY), 0); - lun->sd = SCSI_KCQWP_COMMANDNOTALLOWED; - ret = -EINVAL; - } - - /* Verify that LBA lies in the range supported by the block driver */ - - else if (priv->sector >= lun->nsectors) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_WRITE10LBARANGE), 0); - lun->sd = SCSI_KCQIR_LBAOUTOFRANGE; - ret = -EINVAL; - } - - /* Looks like we are good to go */ - - else - { - usbtrace(TRACE_CLASSSTATE(USBSTRG_CLASSSTATE_CMDPARSECMDWRITE10), priv->cdb[0]); - priv->thstate = USBSTRG_STATE_CMDWRITE; - } - } - return ret; -} - -/**************************************************************************** - * Name: usbstrg_cmdverify10 - * - * Description: - * Handle SCSI_CMD_VERIFY10 command - * - ****************************************************************************/ - -static inline int usbstrg_cmdverify10(FAR struct usbstrg_dev_s *priv) -{ - FAR struct scsicmd_verify10_s *verf = (FAR struct scsicmd_verify10_s *)priv->cdb; - FAR struct usbstrg_lun_s *lun = priv->lun; - uint32_t lba; - uint16_t blocks; - size_t sector; - ssize_t nread; - int ret; - int i; - - priv->u.alloclen = 0; - ret = usbstrg_setupcmd(priv, SCSICMD_VERIFY10_SIZEOF, USBSTRG_FLAGS_DIRNONE); - if (ret == OK) - { - /* Verify the starting and ending LBA */ - - lba = usbstrg_getbe32(verf->lba); - blocks = usbstrg_getbe16(verf->len); - - if ((verf->flags & ~SCSICMD_VERIFY10_DPO) != 0 || verf->groupno != 0) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_VERIFY10FLAGS), 0); - lun->sd = SCSI_KCQIR_INVALIDFIELDINCBA; - ret = -EINVAL; - } - else if (blocks == 0) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_VERIFY10NOBLOCKS), 0); - ret = -EIO; /* No reply */ - } - - /* Verify that a block driver has been bound to the LUN */ - - else if (!lun->inode) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_VERIFY10MEDIANOTPRESENT), 0); - lun->sd = SCSI_KCQNR_MEDIANOTPRESENT; - ret = -EINVAL; - } - - /* Verify that LBA lies in the range supported by the block driver */ - - else if (lba >= lun->nsectors || lba + blocks > lun->nsectors) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_VERIFY10LBARANGE), 0); - lun->sd = SCSI_KCQIR_LBAOUTOFRANGE; - ret = -EINVAL; - } - else - { - /* Try to read the requested blocks */ - - for (i = 0, sector = lba + lun->startsector; i < blocks; i++, sector++) - { - nread = USBSTRG_DRVR_READ(lun, priv->iobuffer, sector, 1); - if (nread < 0) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_VERIFY10READFAIL), i); - lun->sd = SCSI_KCQME_UNRRE1; - lun->sdinfo = sector; - ret = -EIO; - break; - } - } - } - } - return ret; -} - -/**************************************************************************** - * Name: usbstrg_cmdsynchronizecache10 - * - * Description: - * Handle SCSI_CMD_SYNCHCACHE10 command - * - ****************************************************************************/ - -static inline int usbstrg_cmdsynchronizecache10(FAR struct usbstrg_dev_s *priv) -{ - int ret; - - priv->u.alloclen = 0; - - /* Verify that we have the LUN structure and the block driver has been bound */ - - if (!priv->lun->inode) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_SYNCCACHEMEDIANOTPRESENT), 0); - priv->lun->sd = SCSI_KCQNR_MEDIANOTPRESENT; - ret = -EINVAL; - } - else - { - ret = usbstrg_setupcmd(priv, SCSICMD_SYNCHRONIZECACHE10_SIZEOF, USBSTRG_FLAGS_DIRNONE); - } - return ret; -} - -/**************************************************************************** - * Name: usbstrg_cmdmodeselect10 - * - * Description: - * Handle SCSI_CMD_MODESELECT10 command - * - ****************************************************************************/ - -static inline int usbstrg_cmdmodeselect10(FAR struct usbstrg_dev_s *priv) -{ - FAR struct scsicmd_modeselect10_s *modeselect = (FAR struct scsicmd_modeselect10_s *)priv->cdb; - - priv->u.alloclen = usbstrg_getbe16(modeselect->parmlen); - (void)usbstrg_setupcmd(priv, SCSICMD_MODESELECT10_SIZEOF, USBSTRG_FLAGS_DIRHOST2DEVICE); - - /* Not supported */ - - priv->lun->sd = SCSI_KCQIR_INVALIDCOMMAND; - return -EINVAL; -} - -/**************************************************************************** - * Name: usbstrg_cmdmodesense10 - * - * Description: - * Handle SCSI_CMD_MODESENSE10 command - * - ****************************************************************************/ - -static int inline usbstrg_cmdmodesense10(FAR struct usbstrg_dev_s *priv, - FAR uint8_t *buf) -{ - FAR struct scsicmd_modesense10_s *modesense = (FAR struct scsicmd_modesense10_s *)priv->cdb; - FAR struct scsiresp_modeparameterhdr10_s *mph = (FAR struct scsiresp_modeparameterhdr10_s *)buf; - int mdlen; - int ret; - - priv->u.alloclen = usbstrg_getbe16(modesense->alloclen); - ret = usbstrg_setupcmd(priv, SCSICMD_MODESENSE10_SIZEOF, USBSTRG_FLAGS_DIRDEVICE2HOST); - if (ret == OK) - { - if ((modesense->flags & ~SCSICMD_MODESENSE10_DBD) != 0 || modesense->subpgcode != 0) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_MODESENSE10FLAGS), 0); - priv->lun->sd = SCSI_KCQIR_INVALIDFIELDINCBA; - ret = -EINVAL; - } - else - { - /* The response consists of: - * - * (1) A MODESENSE6-specific mode parameter header, - * (2) A variable length list of block descriptors, and - * (3) A variable lengtth list of mode page formats - */ - - memset(mph, 0, SCSIRESP_MODEPARAMETERHDR10_SIZEOF); - mph->param = (priv->lun->readonly ? SCSIRESP_MODEPARMHDR_DAPARM_WP : 0x00); - - /* There are no block descriptors, only the following mode page: */ - - ret = usbstrg_modepage(priv, &buf[SCSIRESP_MODEPARAMETERHDR10_SIZEOF], modesense->pcpgcode, &mdlen); - if (ret == OK) - { - /* Store the mode data length and return the total message size */ - - usbstrg_putbe16(mph->mdlen, mdlen - 2); - priv->nreqbytes = mdlen + SCSIRESP_MODEPARAMETERHDR10_SIZEOF; - } - } - } - return ret; -} - -/**************************************************************************** - * Name: usbstrg_cmdread12 - * - * Description: - * Handle SCSI_CMD_READ12 command - * - ****************************************************************************/ - -static inline int usbstrg_cmdread12(FAR struct usbstrg_dev_s *priv) -{ - struct scsicmd_read12_s *read12 = (struct scsicmd_read12_s*)priv->cdb; - FAR struct usbstrg_lun_s *lun = priv->lun; - int ret; - - priv->u.xfrlen = usbstrg_getbe32(read12->xfrlen); - ret = usbstrg_setupcmd(priv, SCSICMD_READ12_SIZEOF, USBSTRG_FLAGS_DIRDEVICE2HOST|USBSTRG_FLAGS_BLOCKXFR); - if (ret == OK) - { - /* Get the Logical Block Address (LBA) from cdb[] as the starting sector */ - - priv->sector = usbstrg_getbe32(read12->lba); - - /* Verify that we can support this read command */ - - if ((read12->flags & ~(SCSICMD_READ12FLAGS_DPO|SCSICMD_READ12FLAGS_FUA)) != 0) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_READ12FLAGS), 0); - lun->sd = SCSI_KCQIR_INVALIDFIELDINCBA; - ret = -EINVAL; - } - - /* Verify that a block driver has been bound to the LUN */ - - else if (!lun->inode) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_READ12MEDIANOTPRESENT), 0); - lun->sd = SCSI_KCQNR_MEDIANOTPRESENT; - ret = -EINVAL; - } - - /* Verify that LBA lies in the range supported by the block driver */ - - else if (priv->sector >= lun->nsectors) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_READ12LBARANGE), 0); - lun->sd = SCSI_KCQIR_LBAOUTOFRANGE; - ret = -EINVAL; - } - - /* Looks like we are good to go */ - - else - { - usbtrace(TRACE_CLASSSTATE(USBSTRG_CLASSSTATE_CMDPARSECMDREAD12), priv->cdb[0]); - priv->thstate = USBSTRG_STATE_CMDREAD; - } - } - return ret; -} - -/**************************************************************************** - * Name: usbstrg_cmdwrite12 - * - * Description: - * Handle SCSI_CMD_WRITE12 command - * - ****************************************************************************/ - -static inline int usbstrg_cmdwrite12(FAR struct usbstrg_dev_s *priv) -{ - struct scsicmd_write12_s *write12 = (struct scsicmd_write12_s *)priv->cdb; - FAR struct usbstrg_lun_s *lun = priv->lun; - int ret; - - priv->u.xfrlen = usbstrg_getbe32(write12->xfrlen); - ret = usbstrg_setupcmd(priv, SCSICMD_WRITE12_SIZEOF, USBSTRG_FLAGS_DIRHOST2DEVICE|USBSTRG_FLAGS_BLOCKXFR); - if (ret == OK) - { - /* Get the Logical Block Address (LBA) from cdb[] as the starting sector */ - - priv->sector = usbstrg_getbe32(write12->lba); - - /* Verify that we can support this write command */ - - if ((write12->flags & ~(SCSICMD_WRITE12FLAGS_DPO|SCSICMD_WRITE12FLAGS_FUA)) != 0) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_WRITE12FLAGS), 0); - lun->sd = SCSI_KCQIR_INVALIDFIELDINCBA; - } - - /* Verify that a block driver has been bound to the LUN */ - - else if (!lun->inode) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_WRITE12MEDIANOTPRESENT), 0); - lun->sd = SCSI_KCQNR_MEDIANOTPRESENT; - ret = -EINVAL; - } - - /* Check for attempts to write to a read-only device */ - - else if (lun->readonly) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_WRITE12READONLY), 0); - lun->sd = SCSI_KCQWP_COMMANDNOTALLOWED; - } - - /* Verify that LBA lies in the range supported by the block driver */ - - else if (priv->sector >= lun->nsectors) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_WRITE12LBARANGE), 0); - lun->sd = SCSI_KCQIR_LBAOUTOFRANGE; - } - - /* Looks like we are good to go */ - - else - { - usbtrace(TRACE_CLASSSTATE(USBSTRG_CLASSSTATE_CMDPARSECMDWRITE12), priv->cdb[0]); - priv->thstate = USBSTRG_STATE_CMDWRITE; - return OK; - } - } - - return ret; -} - -/**************************************************************************** - * Name: usbstrg_setupcmd - * - * Description: - * Called after each SCSI command is identified in order to perform setup - * and verification operations that are common to all SCSI commands. This - * function performs the following common setup operations: - * - * 1. Determine the direction of the response - * 2. Verify lengths - * 3. Setup and verify the LUN - * - * Includes special logic for INQUIRY and REQUESTSENSE commands - * - ****************************************************************************/ - -static int inline usbstrg_setupcmd(FAR struct usbstrg_dev_s *priv, uint8_t cdblen, uint8_t flags) -{ - FAR struct usbstrg_lun_s *lun = NULL; - uint32_t datlen; - uint8_t dir = flags & USBSTRG_FLAGS_DIRMASK; - int ret = OK; - - /* Verify the LUN and set up the current LUN reference in the - * device structure - */ - - if (priv->cbwlun < priv->nluns) - { - /* LUN number is valid in a valid range, but the LUN is not necessarily - * bound to a block driver. That will be checked as necessary in each - * individual command. - */ - - lun = &priv->luntab[priv->cbwlun]; - priv->lun = lun; - } - - /* Only a few commands may specify unsupported LUNs */ - - else if ((flags & USBSTRG_FLAGS_LUNNOTNEEDED) == 0) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_CMDBADLUN), priv->cbwlun); - ret = -EINVAL; - } - - /* Extract the direction and data transfer length */ - - dir = flags & USBSTRG_FLAGS_DIRMASK; /* Expected data direction */ - datlen = 0; - if ((flags & USBSTRG_FLAGS_BLOCKXFR) == 0) - { - /* Non-block transfer. Data length: Host allocation to receive data - * (only for device-to-host transfers. At present, alloclen is set - * to zero for all host-to-device, non-block transfers. - */ - - datlen = priv->u.alloclen; - } - else if (lun) - { - /* Block transfer: Calculate the total size of all sectors to be transferred */ - - datlen = priv->u.alloclen * lun->sectorsize; - } - - /* Check the data direction. This was set up at the end of the - * IDLE state when the CBW was parsed, but if there is no data, - * then the direction is none. - */ - - if (datlen == 0) - { - /* No data.. then direction is none */ - - dir = USBSTRG_FLAGS_DIRNONE; - } - - /* Compare the expected data length in the command to the data length - * in the CBW. - */ - - else if (priv->cbwlen < datlen) - { - /* Clip to the length in the CBW and declare a phase error */ - - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_PHASEERROR1), priv->cdb[0]); - if ((flags & USBSTRG_FLAGS_BLOCKXFR) != 0) - { - priv->u.alloclen = priv->cbwlen; - } - else - { - priv->u.xfrlen = priv->cbwlen / lun->sectorsize; - } - - priv->phaseerror = 1; - } - - /* Initialize the residue */ - - priv->residue = priv->cbwlen; - - /* Conflicting data directions is a phase error */ - - if (priv->cbwdir != dir && datlen > 0) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_PHASEERROR2), priv->cdb[0]); - priv->phaseerror = 1; - ret = -EINVAL; - } - - /* Compare the length of data in the cdb[] with the expected length - * of the command. - */ - - if (cdblen != priv->cdblen) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_PHASEERROR3), priv->cdb[0]); - priv->phaseerror = 1; - ret = -EINVAL; - } - - if (lun) - { - /* Retain the sense data for the REQUEST SENSE command */ - - if ((flags & USBSTRG_FLAGS_RETAINSENSEDATA) == 0) - { - /* Discard the sense data */ - - lun->sd = SCSI_KCQ_NOSENSE; - lun->sdinfo = 0; - } - - /* If a unit attention condition exists, then only a restricted set of - * commands is permitted. - */ - - if (lun->uad != SCSI_KCQ_NOSENSE && (flags & USBSTRG_FLAGS_UACOKAY) != 0) - { - /* Command not permitted */ - - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_CMDUNEVIOLATION), priv->cbwlun); - lun->sd = lun->uad; - lun->uad = SCSI_KCQ_NOSENSE; - ret = -EINVAL; - } - } - - /* The final, 1-byte field of every SCSI command is the Control field which - * must be zero - */ - - if (priv->cdb[cdblen-1] != 0) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_SCSICMDCONTROL), 0); - if (lun) - { - lun->sd = SCSI_KCQIR_INVALIDFIELDINCBA; - } - ret = -EINVAL; - } - return ret; -} - -/**************************************************************************** - * Name: usbstrg_idlestate - * - * Description: - * Called from the worker thread in the USBSTRG_STATE_IDLE state. Checks - * for the receipt of a bulk CBW. - * - * Returned value: - * If no new, valid CBW is available, this function returns a negated errno. - * Otherwise, when a new CBW is successfully parsed, this function sets - * priv->thstate to USBSTRG_STATE_CMDPARSE and returns OK. - * - ****************************************************************************/ - -static int usbstrg_idlestate(FAR struct usbstrg_dev_s *priv) -{ - FAR struct usbstrg_req_s *privreq; - FAR struct usbdev_req_s *req; - FAR struct usbstrg_cbw_s *cbw; - irqstate_t flags; - int ret = -EINVAL; - - /* Take a request from the rdreqlist */ - - flags = irqsave(); - privreq = (FAR struct usbstrg_req_s *)sq_remfirst(&priv->rdreqlist); - irqrestore(flags); - - /* Has anything been received? If not, just return an error. - * This will cause us to remain in the IDLE state. When a USB request is - * received, the worker thread will be awakened in the USBSTRG_STATE_IDLE - * and we will be called again. - */ - - if (!privreq) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_IDLERDREQLISTEMPTY), 0); - return -ENOMEM; - } - - req = privreq->req; - cbw = (FAR struct usbstrg_cbw_s *)req->buf; - - /* Handle the CBW */ - - usbstrg_dumpdata("SCSCI CBW", (uint8_t*)cbw, USBSTRG_CBW_SIZEOF - USBSTRG_MAXCDBLEN); - usbstrg_dumpdata(" CDB", cbw->cdb, min(cbw->cdblen, USBSTRG_MAXCDBLEN)); - - /* Check for properly formatted CBW? */ - - if (req->xfrd != USBSTRG_CBW_SIZEOF || - cbw->signature[0] != 'U' || - cbw->signature[1] != 'S' || - cbw->signature[2] != 'B' || - cbw->signature[3] != 'C') - { - /* CBS BAD: Stall the bulk endpoints. If the CBW is bad we must stall the - * bulk IN endpoint and either (1) stall the bulk OUT endpoint, or - * (2) discard data from the endpoint. - */ - - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_INVALIDCBWSIGNATURE), 0); - EP_STALL(priv->epbulkout); - EP_STALL(priv->epbulkin); - } - - /* Is the CBW meaningful? */ - - else if (cbw->lun >= priv->nluns || (cbw->flags & ~USBSTRG_CBWFLAG_IN) != 0 || - cbw->cdblen < 6 || cbw->cdblen > USBSTRG_MAXCDBLEN) - { - /* CBS BAD: Stall the bulk endpoints. If the CBW is bad we must stall the - * bulk IN endpoint and either (1) stall the bulk OUT endpoint, or - * (2) discard data from the endpoint. - */ - - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_INVALIDCBWCONTENT), 0); - EP_STALL(priv->epbulkout); - EP_STALL(priv->epbulkin); - } - - /* Save the information from the CBW */ - - else - { - /* Extract the CDB and copy the whole CBD[] for later use */ - - priv->cdblen = cbw->cdblen; - memcpy(priv->cdb, cbw->cdb, priv->cdblen); - - /* Extract the data direction */ - - if ((cbw->flags & USBSTRG_CBWFLAG_IN) != 0) - { - /* IN: Device-to-host */ - - priv->cbwdir = USBSTRG_FLAGS_DIRDEVICE2HOST; - } - else - { - /* OUT: Host-to-device */ - - priv->cbwdir = USBSTRG_FLAGS_DIRHOST2DEVICE; - } - - /* Get the max size of the data response */ - - priv->cbwlen = usbstrg_getle32(cbw->datlen); - if (priv->cbwlen == 0) - { - /* No length? Then no direction either */ - - priv->cbwdir = USBSTRG_FLAGS_DIRNONE; - } - - /* Extract and save the LUN index and TAG value */ - - priv->cbwlun = cbw->lun; - priv->cbwtag = usbstrg_getle32(cbw->tag); - - /* Return the read request to the bulk out endpoint for re-filling */ - - req = privreq->req; - req->len = CONFIG_USBSTRG_BULKOUTREQLEN; - req->priv = privreq; - req->callback = usbstrg_rdcomplete; - - if (EP_SUBMIT(priv->epbulkout, req) != OK) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_IDLERDSUBMIT), (uint16_t)-ret); - } - - /* Change to the CMDPARSE state and return success */ - - usbtrace(TRACE_CLASSSTATE(USBSTRG_CLASSSTATE_IDLECMDPARSE), priv->cdb[0]); - priv->thstate = USBSTRG_STATE_CMDPARSE; - ret = OK; - } - - return ret; -} - -/**************************************************************************** - * Name: usbstrg_cmdparsestate - * - * Description: - * Called from the worker thread in the USBSTRG_STATE_CMDPARSE state. - * This state is entered when usbstrg_idlestate obtains a valid CBW - * containing SCSI commands. This function processes those SCSI commands. - * - * Returned value: - * If no write request is available or certain other errors occur, this - * function returns a negated errno and stays in the USBSTRG_STATE_CMDPARSE - * state. Otherwise, when the new CBW is successfully process, this - * function sets priv->thstate to one of USBSTRG_STATE_CMDREAD, - * USBSTRG_STATE_CMDWRITE, or USBSTRG_STATE_CMDFINISH and returns OK. - * - ****************************************************************************/ - -static int usbstrg_cmdparsestate(FAR struct usbstrg_dev_s *priv) -{ - FAR struct usbstrg_req_s *privreq; - FAR uint8_t *buf; - int ret = -EINVAL; - - usbstrg_dumpdata("SCSCI CDB", priv->cdb, priv->cdblen); - - /* Check if there is a request in the wrreqlist that we will be able to - * use for data or status. - */ - - privreq = (FAR struct usbstrg_req_s *)sq_peek(&priv->wrreqlist); - - /* If there no request structures available, then just return an error. - * This will cause us to remain in the CMDPARSE state. When a request is - * returned, the worker thread will be awakened in the USBSTRG_STATE_CMDPARSE - * and we will be called again. - */ - - if (!privreq) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_CMDPARSEWRREQLISTEMPTY), 0); - return -ENOMEM; - } - DEBUGASSERT(privreq->req && privreq->req->buf); - buf = privreq->req->buf; - - /* Assume that no errors will be encountered */ - - priv->phaseerror = 0; - priv->shortpacket = 0; - - /* No data is buffered */ - - priv->nsectbytes = 0; - priv->nreqbytes = 0; - - /* Get exclusive access to the block driver */ - - pthread_mutex_lock(&priv->mutex); - switch (priv->cdb[0]) - { - case SCSI_CMD_TESTUNITREADY: /* 0x00 Mandatory */ - ret = usbstrg_cmdtestunitready(priv); - break; - - /* case SCSI_CMD_REZEROUNIT: * 0x01 Obsolete - * * 0x02 Vendor-specific */ - - case SCSI_CMD_REQUESTSENSE: /* 0x03 Mandatory */ - ret = usbstrg_cmdrequestsense(priv, buf); - break; - - /* case SCSI_CMD_FORMAT_UNIT: * 0x04 Mandatory, but not implemented - * * 0x05 Vendor specific - * * 0x06 Vendor specific - * case SCSI_CMD_REASSIGNBLOCKS: * 0x07 Optional */ - - case SCSI_CMD_READ6: /* 0x08 Mandatory */ - ret = usbstrg_cmdread6(priv); - break; - - /* * 0x09 Vendor specific */ - - case SCSI_CMD_WRITE6: /* 0x0a Optional */ - ret = usbstrg_cmdwrite6(priv); - break; - - /* case SCSI_CMD_SEEK6: * 0x0b Obsolete - * * 0x0c-0x10 Vendor specific - * case SCSI_CMD_SPACE6: * 0x11 Vendor specific */ - - case SCSI_CMD_INQUIRY: /* 0x12 Mandatory */ - ret = usbstrg_cmdinquiry(priv, buf); - break; - - /* * 0x13-0x14 Vendor specific */ - - case SCSI_CMD_MODESELECT6: /* 0x15 Optional */ - ret = usbstrg_cmdmodeselect6(priv); - break; - - /* case SCSI_CMD_RESERVE6: * 0x16 Obsolete - * case SCSI_CMD_RELEASE6: * 0x17 Obsolete - * case SCSI_CMD_COPY: * 0x18 Obsolete - * * 0x19 Vendor specific */ - - case SCSI_CMD_MODESENSE6: /* 0x1a Optional */ - ret = usbstrg_cmdmodesense6(priv, buf); - break; - - case SCSI_CMD_STARTSTOPUNIT: /* 0x1b Optional */ - ret = usbstrg_cmdstartstopunit(priv); - break; - - /* case SCSI_CMD_RECEIVEDIAGNOSTICRESULTS: * 0x1c Optional - * case SCSI_CMD_SENDDIAGNOSTIC: * 0x1d Mandatory, but not implemented */ - - case SCSI_CMD_PREVENTMEDIAREMOVAL: /* 0x1e Optional */ - ret = usbstrg_cmdpreventmediumremoval(priv); - break; - - /* * 0x20-22 Vendor specific */ - case SCSI_CMD_READFORMATCAPACITIES: /* 0x23 Vendor-specific (defined in MMC spec) */ - ret = usbstrg_cmdreadformatcapacity(priv, buf); - break; - /* * 0x24 Vendor specific */ - - case SCSI_CMD_READCAPACITY10: /* 0x25 Mandatory */ - ret = usbstrg_cmdreadcapacity10(priv, buf); - break; - - /* * 0x26-27 Vendor specific */ - case SCSI_CMD_READ10: /* 0x28 Mandatory */ - ret = usbstrg_cmdread10(priv); - break; - - /* * 0x29 Vendor specific */ - - case SCSI_CMD_WRITE10: /* 0x2a Optional */ - ret = usbstrg_cmdwrite10(priv); - break; - - /* case SCSI_CMD_SEEK10: * 0x2b Obsolete - * * 0x2c-2d Vendor specific - * case SCSI_CMD_WRITEANDVERIFY: * 0x2e Optional */ - - case SCSI_CMD_VERIFY10: /* 0x2f Optional, but needed my MS Windows */ - ret = usbstrg_cmdverify10(priv); - break; - - /* case SCSI_CMD_SEARCHDATAHIGH: * 0x30 Obsolete - * case SCSI_CMD_SEARCHDATAEQUAL: * 0x31 Obsolete - * case SCSI_CMD_SEARCHDATALOW: * 0x32 Obsolete - * case SCSI_CMD_SETLIMITS10: * 0x33 Obsolete - * case SCSI_CMD_PREFETCH10: * 0x34 Optional */ - - case SCSI_CMD_SYNCHCACHE10: /* 0x35 Optional */ - ret = usbstrg_cmdsynchronizecache10(priv); - break; - - /* case SCSI_CMD_LOCKCACHE: * 0x36 Obsolete - * case SCSI_CMD_READDEFECTDATA10: * 0x37 Optional - * case SCSI_CMD_COMPARE: * 0x39 Obsolete - * case SCSI_CMD_COPYANDVERIFY: * 0x3a Obsolete - * case SCSI_CMD_WRITEBUFFER: * 0x3b Optional - * case SCSI_CMD_READBUFFER: * 0x3c Optional - * case SCSI_CMD_READLONG10: * 0x3e Optional - * case SCSI_CMD_WRITELONG10: * 0x3f Optional - * case SCSI_CMD_CHANGEDEFINITION: * 0x40 Obsolete - * case SCSI_CMD_WRITESAME10: * 0x41 Optional - * case SCSI_CMD_LOGSELECT: * 0x4c Optional - * case SCSI_CMD_LOGSENSE: * 0x4d Optional - * case SCSI_CMD_XDWRITE10: * 0x50 Optional - * case SCSI_CMD_XPWRITE10: * 0x51 Optional - * case SCSI_CMD_XDREAD10: * 0x52 Optional */ - - case SCSI_CMD_MODESELECT10: /* 0x55 Optional */ - ret = usbstrg_cmdmodeselect10(priv); - break; - - /* case SCSI_CMD_RESERVE10: * 0x56 Obsolete - * case SCSI_CMD_RELEASE10: * 0x57 Obsolete */ - - case SCSI_CMD_MODESENSE10: /* 0x5a Optional */ - ret = usbstrg_cmdmodesense10(priv, buf); - break; - - /* case SCSI_CMD_PERSISTENTRESERVEIN: * 0x5e Optional - * case SCSI_CMD_PERSISTENTRESERVEOUT: * 0x5f Optional - * case SCSI_CMD_32: * 0x7f Optional - * case SCSI_CMD_XDWRITEEXTENDED: * 0x80 Obsolete - * case SCSI_CMD_REBUILD: * 0x81 Obsolete - * case SCSI_CMD_REGENERATE: * 0x82 Obsolete - * case SCSI_CMD_EXTENDEDCOPY: * 0x83 Optional - * case SCSI_CMD_COPYRESULTS: * 0x84 Optional - * case SCSI_CMD_ACCESSCONTROLIN: * 0x86 Optional - * case SCSI_CMD_ACCESSCONTROLOUT: * 0x87 Optional - * case SCSI_CMD_READ16: * 0x88 Optional - * case SCSI_CMD_WRITE16: * 0x8a Optional - * case SCSI_CMD_READATTRIBUTE: * 0x8c Optional - * case SCSI_CMD_WRITEATTRIBUTE: * 0x8d Optional - * case SCSI_CMD_WRITEANDVERIFY16: * 0x8e Optional - * case SCSI_CMD_SYNCHCACHE16: * 0x91 Optional - * case SCSI_CMD_LOCKUNLOCKACACHE: * 0x92 Optional - * case SCSI_CMD_WRITESAME16: * 0x93 Optional - * case SCSI_CMD_READCAPACITY16: * 0x9e Optional - * case SCSI_CMD_READLONG16: * 0x9e Optional - * case SCSI_CMD_WRITELONG16 * 0x9f Optional - * case SCSI_CMD_REPORTLUNS: * 0xa0 Mandatory, but not implemented - * case SCSI_CMD_MAINTENANCEIN: * 0xa3 Optional (SCCS==0) - * case SCSI_CMD_MAINTENANCEOUT: * 0xa4 Optional (SCCS==0) - * case SCSI_CMD_MOVEMEDIUM: * 0xa5 ? - * case SCSI_CMD_MOVEMEDIUMATTACHED: * 0xa7 Optional (MCHNGR==0) */ - - case SCSI_CMD_READ12: /* 0xa8 Optional */ - ret = usbstrg_cmdread12(priv); - break; - - case SCSI_CMD_WRITE12: /* 0xaa Optional */ - ret = usbstrg_cmdwrite12(priv); - break; - - /* case SCSI_CMD_READMEDIASERIALNUMBER: * 0xab Optional - * case SCSI_CMD_WRITEANDVERIFY12: * 0xae Optional - * case SCSI_CMD_VERIFY12: * 0xaf Optional - * case SCSI_CMD_SETLIMITS12 * 0xb3 Obsolete - * case SCSI_CMD_READELEMENTSTATUS: * 0xb4 Optional (MCHNGR==0) - * case SCSI_CMD_READDEFECTDATA12: * 0xb7 Optional - * case SCSI_CMD_REDUNDANCYGROUPIN: * 0xba Optional - * case SCSI_CMD_REDUNDANCYGROUPOUT: * 0xbb Optional - * case SCSI_CMD_SPAREIN: * 0xbc Optional (SCCS==0) - * case SCSI_CMD_SPAREOUT: * 0xbd Optional (SCCS==0) - * case SCSI_CMD_VOLUMESETIN: * 0xbe Optional (SCCS==0) - * case SCSI_CMD_VOLUMESETOUT: * 0xbe Optional (SCCS==0) - * * 0xc0-0xff Vendor specific */ - - default: - priv->u.alloclen = 0; - if (ret == OK) - { - priv->lun->sd = SCSI_KCQIR_INVALIDCOMMAND; - ret = -EINVAL; - } - break; - } - pthread_mutex_unlock(&priv->mutex); - - /* Is a response required? (Not for read6/10/12 and write6/10/12). */ - - if (priv->thstate == USBSTRG_STATE_CMDPARSE) - { - /* All commands come through this path (EXCEPT read6/10/12 and write6/10/12). - * For all other commands, the following setup is expected for the response - * based on data direction: - * - * For direction NONE: - * 1. priv->u.alloclen == 0 - * 2. priv->nreqbytes == 0 - * - * For direction = device-to-host: - * 1. priv->u.alloclen == allocation length; space set aside by the - * host to receive the device data. The size of the response - * cannot exceed this value. - * 2. response data is in the request currently at the head of - * the priv->wrreqlist queue. priv->nreqbytes is set to the length - * of data in the response. - * - * For direction host-to-device - * At present, there are no supported commands that should have host-to-device - * transfers (except write6/10/12 and that command logic does not take this - * path. The 'residue' is left at the full host-to-device data size. - * - * For all: - * ret set to <0 if an error occurred in parsing the commands. - */ - - /* For from device-to-hose operations, the residue is the expected size - * (the initial value of 'residue') minus the amount actually returned - * in the response: - */ - - if (priv->cbwdir == USBSTRG_FLAGS_DIRDEVICE2HOST) - { - /* The number of bytes in the response cannot exceed the host - * 'allocation length' in the command. - */ - - if (priv->nreqbytes > priv->u.alloclen) - { - priv->nreqbytes = priv->u.alloclen; - } - - /* The residue is then the number of bytes that were not sent */ - - priv->residue -= priv->nreqbytes; - } - - /* On return, we need the following: - * - * 1. Setup for CMDFINISH state if appropriate - * 2. priv->thstate set to either CMDPARSE if no buffer was available or - * CMDFINISH to send the response - * 3. Return OK to continue; <0 to wait for the next event - */ - - usbtrace(TRACE_CLASSSTATE(USBSTRG_CLASSSTATE_CMDPARSECMDFINISH), priv->cdb[0]); - priv->thstate = USBSTRG_STATE_CMDFINISH; - ret = OK; - } - return ret; -} - -/**************************************************************************** - * Name: usbstrg_cmdreadstate - * - * Description: - * Called from the worker thread in the USBSTRG_STATE_CMDREAD state. - * The USBSTRG_STATE_CMDREAD state is entered when usbstrg_cmdparsestate - * obtains a valid SCSI read command. This state is really a continuation - * of the USBSTRG_STATE_CMDPARSE state that handles extended SCSI read - * command handling. - * - * Returned value: - * If no USBDEV write request is available or certain other errors occur, this - * function returns a negated errno and stays in the USBSTRG_STATE_CMDREAD - * state. Otherwise, when the new SCSI read command is fully processed, - * this function sets priv->thstate to USBSTRG_STATE_CMDFINISH and returns OK. - * - * State variables: - * xfrlen - holds the number of sectors read to be read. - * sector - holds the sector number of the next sector to be read - * nsectbytes - holds the number of bytes buffered for the current sector - * nreqbytes - holds the number of bytes currently buffered in the request - * at the head of the wrreqlist. - * - ****************************************************************************/ - -static int usbstrg_cmdreadstate(FAR struct usbstrg_dev_s *priv) -{ - FAR struct usbstrg_lun_s *lun = priv->lun; - FAR struct usbstrg_req_s *privreq; - FAR struct usbdev_req_s *req; - irqstate_t flags; - ssize_t nread; - uint8_t *src; - uint8_t *dest; - int nbytes; - int ret; - - /* Loop transferring data until either (1) all of the data has been - * transferred, or (2) we have used up all of the write requests that we have - * available. - */ - - while (priv->u.xfrlen > 0 || priv->nsectbytes > 0) - { - usbtrace(TRACE_CLASSSTATE(USBSTRG_CLASSSTATE_CMDREAD), priv->u.xfrlen); - - /* Is the I/O buffer empty? */ - - if (priv->nsectbytes <= 0) - { - /* Yes.. read the next sector */ - - nread = USBSTRG_DRVR_READ(lun, priv->iobuffer, priv->sector, 1); - if (nread < 0) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_CMDREADREADFAIL), -nread); - lun->sd = SCSI_KCQME_UNRRE1; - lun->sdinfo = priv->sector; - break; - } - - priv->nsectbytes = lun->sectorsize; - priv->u.xfrlen--; - priv->sector++; - } - - /* Check if there is a request in the wrreqlist that we will be able to - * use for data transfer. - */ - - privreq = (FAR struct usbstrg_req_s *)sq_peek(&priv->wrreqlist); - - /* If there no request structures available, then just return an error. - * This will cause us to remain in the CMDREAD state. When a request is - * returned, the worker thread will be awakened in the USBSTRG_STATE_CMDREAD - * and we will be called again. - */ - - if (!privreq) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_CMDREADWRRQEMPTY), 0); - priv->nreqbytes = 0; - return -ENOMEM; - } - req = privreq->req; - - /* Transfer all of the data that will (1) fit into the request buffer, OR (2) - * all of the data available in the sector buffer. - */ - - src = &priv->iobuffer[lun->sectorsize - priv->nsectbytes]; - dest = &req->buf[priv->nreqbytes]; - - nbytes = min(CONFIG_USBSTRG_BULKINREQLEN - priv->nreqbytes, priv->nsectbytes); - - /* Copy the data from the sector buffer to the USB request and update counts */ - - memcpy(dest, src, nbytes); - priv->nreqbytes += nbytes; - priv->nsectbytes -= nbytes; - - /* If (1) the request buffer is full OR (2) this is the final request full of data, - * then submit the request - */ - - if (priv->nreqbytes >= CONFIG_USBSTRG_BULKINREQLEN || - (priv->u.xfrlen <= 0 && priv->nsectbytes <= 0)) - { - /* Remove the request that we just filled from wrreqlist (we've already checked - * that is it not NULL - */ - - flags = irqsave(); - privreq = (FAR struct usbstrg_req_s *)sq_remfirst(&priv->wrreqlist); - irqrestore(flags); - - /* And submit the request to the bulk IN endpoint */ - - req->len = priv->nreqbytes; - req->priv = privreq; - req->callback = usbstrg_wrcomplete; - req->flags = 0; - - ret = EP_SUBMIT(priv->epbulkin, req); - if (ret != OK) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_CMDREADSUBMIT), (uint16_t)-ret); - lun->sd = SCSI_KCQME_UNRRE1; - lun->sdinfo = priv->sector; - break; - } - - /* Assume success... residue should probably really be decremented in - * wrcomplete when we know that the transfer completed successfully. - */ - - priv->residue -= priv->nreqbytes; - priv->nreqbytes = 0; - } - } - - usbtrace(TRACE_CLASSSTATE(USBSTRG_CLASSSTATE_CMDREADCMDFINISH), priv->u.xfrlen); - priv->thstate = USBSTRG_STATE_CMDFINISH; - return OK; -} - -/**************************************************************************** - * Name: usbstrg_cmdwritestate - * - * Description: - * Called from the worker thread in the USBSTRG_STATE_CMDWRITE state. - * The USBSTRG_STATE_CMDWRITE state is entered when usbstrg_cmdparsestate - * obtains a valid SCSI write command. This state is really a continuation - * of the USBSTRG_STATE_CMDPARSE state that handles extended SCSI write - * command handling. - * - * Returned value: - * If no USBDEV write request is available or certain other errors occur, this - * function returns a negated errno and stays in the USBSTRG_STATE_CMDWRITE - * state. Otherwise, when the new SCSI write command is fully processed, - * this function sets priv->thstate to USBSTRG_STATE_CMDFINISH and returns OK. - * - * State variables: - * xfrlen - holds the number of sectors read to be written. - * sector - holds the sector number of the next sector to write - * nsectbytes - holds the number of bytes buffered for the current sector - * nreqbytes - holds the number of untransferred bytes currently in the - * request at the head of the rdreqlist. - * - ****************************************************************************/ - -static int usbstrg_cmdwritestate(FAR struct usbstrg_dev_s *priv) -{ - FAR struct usbstrg_lun_s *lun = priv->lun; - FAR struct usbstrg_req_s *privreq; - FAR struct usbdev_req_s *req; - ssize_t nwritten; - uint16_t xfrd; - uint8_t *src; - uint8_t *dest; - int nbytes; - int ret; - - /* Loop transferring data until either (1) all of the data has been - * transferred, or (2) we have written all of the data in the available - * read requests. - */ - - while (priv->u.xfrlen > 0) - { - usbtrace(TRACE_CLASSSTATE(USBSTRG_CLASSSTATE_CMDWRITE), priv->u.xfrlen); - - /* Check if there is a request in the rdreqlist containing additional - * data to be written. - */ - - privreq = (FAR struct usbstrg_req_s *)sq_remfirst(&priv->rdreqlist); - - /* If there no request data available, then just return an error. - * This will cause us to remain in the CMDWRITE state. When a filled request is - * received, the worker thread will be awakened in the USBSTRG_STATE_CMDWRITE - * and we will be called again. - */ - - if (!privreq) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_CMDWRITERDRQEMPTY), 0); - priv->nreqbytes = 0; - return -ENOMEM; - } - - req = privreq->req; - xfrd = req->xfrd; - priv->nreqbytes = xfrd; - - /* Now loop until all of the data in the read request has been tranferred - * to the block driver OR all of the request data has been transferred. - */ - - while (priv->nreqbytes > 0 && priv->u.xfrlen > 0) - { - /* Copy the data received in the read request into the sector I/O buffer */ - - src = &req->buf[xfrd - priv->nreqbytes]; - dest = &priv->iobuffer[priv->nsectbytes]; - - nbytes = min(lun->sectorsize - priv->nsectbytes, priv->nreqbytes); - - /* Copy the data from the sector buffer to the USB request and update counts */ - - memcpy(dest, src, nbytes); - priv->nsectbytes += nbytes; - priv->nreqbytes -= nbytes; - - /* Is the I/O buffer full? */ - - if (priv->nsectbytes >= lun->sectorsize) - { - /* Yes.. Write the next sector */ - - nwritten = USBSTRG_DRVR_WRITE(lun, priv->iobuffer, priv->sector, 1); - if (nwritten < 0) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_CMDWRITEWRITEFAIL), -nwritten); - lun->sd = SCSI_KCQME_WRITEFAULTAUTOREALLOCFAILED; - lun->sdinfo = priv->sector; - goto errout; - } - - priv->nsectbytes = 0; - priv->residue -= lun->sectorsize; - priv->u.xfrlen--; - priv->sector++; - } - } - - /* In either case, we are finished with this read request and can return it - * to the endpoint. Then we will go back to the top of the top and attempt - * to get the next read request. - */ - - req->len = CONFIG_USBSTRG_BULKOUTREQLEN; - req->priv = privreq; - req->callback = usbstrg_rdcomplete; - - ret = EP_SUBMIT(priv->epbulkout, req); - if (ret != OK) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_CMDWRITERDSUBMIT), (uint16_t)-ret); - } - - /* Did the host decide to stop early? */ - - if (xfrd != CONFIG_USBSTRG_BULKOUTREQLEN) - { - priv->shortpacket = 1; - goto errout; - } - } - -errout: - usbtrace(TRACE_CLASSSTATE(USBSTRG_CLASSSTATE_CMDWRITECMDFINISH), priv->u.xfrlen); - priv->thstate = USBSTRG_STATE_CMDFINISH; - return OK; -} - -/**************************************************************************** - * Name: usbstrg_cmdfinishstate - * - * Description: - * Called from the worker thread in the USBSTRG_STATE_CMDFINISH state. - * The USBSTRG_STATE_CMDFINISH state is entered when processing of a - * command has finished but before status has been returned. - * - * Returned value: - * If no USBDEV write request is available or certain other errors occur, this - * function returns a negated errno and stays in the USBSTRG_STATE_CMDFINISH - * state. Otherwise, when the command is fully processed, this function - * sets priv->thstate to USBSTRG_STATE_CMDSTATUS and returns OK. - * - ****************************************************************************/ - -static int usbstrg_cmdfinishstate(FAR struct usbstrg_dev_s *priv) -{ - FAR struct usbstrg_req_s *privreq; - irqstate_t flags; - int ret = OK; - - /* Check if there is a request in the wrreqlist that we will be able to - * use for data transfer. - */ - - privreq = (FAR struct usbstrg_req_s *)sq_peek(&priv->wrreqlist); - - /* If there no request structures available, then just return an error. - * This will cause us to remain in the CMDREAD state. When a request is - * returned, the worker thread will be awakened in the USBSTRG_STATE_CMDREAD - * and we will be called again. - */ - - if (!privreq) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_CMDFINISHRQEMPTY), 0); - return -ENOMEM; - } - - /* Finish the final stages of the reply */ - - switch (priv->cbwdir) - { - /* Device-to-host: All but the last data buffer has been sent */ - - case USBSTRG_FLAGS_DIRDEVICE2HOST: - if (priv->cbwlen > 0) - { - /* On most commands (the exception is outgoing, write commands), - * the data has not not yet been sent. - */ - - if (priv->nreqbytes > 0) - { - struct usbdev_req_s *req; - - /* Take a request from the wrreqlist (we've already checked - * that is it not NULL) - */ - - flags = irqsave(); - privreq = (FAR struct usbstrg_req_s *)sq_remfirst(&priv->wrreqlist); - irqrestore(flags); - - /* Send the write request */ - - req = privreq->req; - req->len = priv->nreqbytes; - req->callback = usbstrg_wrcomplete; - req->priv = privreq; - req->flags = USBDEV_REQFLAGS_NULLPKT; - - ret = EP_SUBMIT(priv->epbulkin, privreq->req); - if (ret < 0) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_CMDFINISHSUBMIT), (uint16_t)-ret); - } - } - - /* Stall the BULK In endpoint if there is a residue */ - - if (priv->residue > 0) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_CMDFINISHRESIDUE), (uint16_t)priv->residue); - -#ifdef CONFIG_USBSTRG_RACEWAR - /* (See description of the workaround at the top of the file). - * First, wait for the transfer to complete, then stall the endpoint - */ - - usleep (100000); - (void)EP_STALL(priv->epbulkin); - - /* now wait for stall to go away .... */ - - usleep (100000); -#else - (void)EP_STALL(priv->epbulkin); -#endif - } - } - break; - - /* Host-to-device: We have processed all we want of the host data. */ - - case USBSTRG_FLAGS_DIRHOST2DEVICE: - if (priv->residue > 0) - { - /* Did the host stop sending unexpectedly early? */ - - flags = irqsave(); - if (priv->shortpacket) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_CMDFINISHSHORTPKT), (uint16_t)priv->residue); - } - - /* Unprocessed incoming data: STALL and cancel requests. */ - - else - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_CMDFINSHSUBMIT), (uint16_t)priv->residue); - EP_STALL(priv->epbulkout); - } - - priv->theventset |= USBSTRG_EVENT_ABORTBULKOUT; - irqrestore(flags); - } - break; - - default: - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_CMDFINSHDIR), priv->cbwdir); - case USBSTRG_FLAGS_DIRNONE: /* Nothing to send */ - break; - } - - /* Return to the IDLE state */ - - usbtrace(TRACE_CLASSSTATE(USBSTRG_CLASSSTATE_CMDFINISHCMDSTATUS), 0); - priv->thstate = USBSTRG_STATE_CMDSTATUS; - return OK; -} - -/**************************************************************************** - * Name: usbstrg_cmdstatusstate - * - * Description: - * Called from the worker thread in the USBSTRG_STATE_CMDSTATUS state. - * That state is after a CBW has been fully processed. This function sends - * the concluding CSW. - * - * Returned value: - * If no write request is available or certain other errors occur, this - * function returns a negated errno and stays in the USBSTRG_STATE_CMDSTATUS - * state. Otherwise, when the SCSI statis is successfully returned, this - * function sets priv->thstate to USBSTRG_STATE_IDLE and returns OK. - * - ****************************************************************************/ - -static int usbstrg_cmdstatusstate(FAR struct usbstrg_dev_s *priv) -{ - FAR struct usbstrg_lun_s *lun = priv->lun; - FAR struct usbstrg_req_s *privreq; - FAR struct usbdev_req_s *req; - FAR struct usbstrg_csw_s *csw; - irqstate_t flags; - uint32_t sd; - uint8_t status = USBSTRG_CSWSTATUS_PASS; - int ret; - - /* Take a request from the wrreqlist */ - - flags = irqsave(); - privreq = (FAR struct usbstrg_req_s *)sq_remfirst(&priv->wrreqlist); - irqrestore(flags); - - /* If there no request structures available, then just return an error. - * This will cause us to remain in the CMDSTATUS status. When a request is - * returned, the worker thread will be awakened in the USBSTRG_STATE_CMDSTATUS - * and we will be called again. - */ - - if (!privreq) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_CMDSTATUSRDREQLISTEMPTY), 0); - return -ENOMEM; - } - - req = privreq->req; - csw = (struct usbstrg_csw_s*)req->buf; - - /* Extract the sense data from the LUN structure */ - - if (lun) - { - sd = lun->sd; - } - else - { - sd = SCSI_KCQIR_INVALIDLUN; - } - - /* Determine the CSW status: PASS, PHASEERROR, or FAIL */ - - if (priv->phaseerror) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_SNDPHERROR), 0); - status = USBSTRG_CSWSTATUS_PHASEERROR; - sd = SCSI_KCQIR_INVALIDCOMMAND; - } - else if (sd != SCSI_KCQ_NOSENSE) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_SNDCSWFAIL), 0); - status = USBSTRG_CSWSTATUS_FAIL; - } - - /* Format and submit the CSW */ - - csw->signature[0] = 'U'; - csw->signature[1] = 'S'; - csw->signature[2] = 'B'; - csw->signature[3] = 'S'; - usbstrg_putle32(csw->tag, priv->cbwtag); - usbstrg_putle32(csw->residue, priv->residue); - csw->status = status; - - usbstrg_dumpdata("SCSCI CSW", (uint8_t*)csw, USBSTRG_CSW_SIZEOF); - - req->len = USBSTRG_CSW_SIZEOF; - req->callback = usbstrg_wrcomplete; - req->priv = privreq; - req->flags = USBDEV_REQFLAGS_NULLPKT; - - ret = EP_SUBMIT(priv->epbulkin, req); - if (ret < 0) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_SNDSTATUSSUBMIT), (uint16_t)-ret); - flags = irqsave(); - (void)sq_addlast((sq_entry_t*)privreq, &priv->wrreqlist); - irqrestore(flags); - } - - /* Return to the IDLE state */ - - usbtrace(TRACE_CLASSSTATE(USBSTRG_CLASSSTATE_CMDSTATUSIDLE), 0); - priv->thstate = USBSTRG_STATE_IDLE; - return OK; -} - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -/**************************************************************************** - * Name: usbstrg_workerthread - * - * Description: - * This is the main function of the USB storage worker thread. It loops - * until USB-related events occur, then processes those events accordingly - * - ****************************************************************************/ - -void *usbstrg_workerthread(void *arg) -{ - struct usbstrg_dev_s *priv = (struct usbstrg_dev_s *)arg; - irqstate_t flags; - uint16_t eventset; - int ret; - - /* This thread is started before the USB storage class is fully initialized. - * wait here until we are told to begin. Start in the NOTINITIALIZED state - */ - - pthread_mutex_lock(&priv->mutex); - priv->thstate = USBSTRG_STATE_STARTED; - while ((priv->theventset & USBSTRG_EVENT_READY) != 0 && - (priv->theventset & USBSTRG_EVENT_TERMINATEREQUEST) != 0) - { - pthread_cond_wait(&priv->cond, &priv->mutex); - } - - /* Transition to the INITIALIZED/IDLE state */ - - priv->thstate = USBSTRG_STATE_IDLE; - eventset = priv->theventset; - priv->theventset = USBSTRG_EVENT_NOEVENTS; - pthread_mutex_unlock(&priv->mutex); - - /* Then loop until we are asked to terminate */ - - while ((eventset & USBSTRG_EVENT_TERMINATEREQUEST) == 0) - { - /* Wait for some interesting event. Note that we must both take the - * lock (to eliminate race conditions with other threads) and disable - * interrupts (to eliminate race conditions with USB interrupt handling. - */ - - pthread_mutex_lock(&priv->mutex); - flags = irqsave(); - if (priv->theventset == USBSTRG_EVENT_NOEVENTS) - { - pthread_cond_wait(&priv->cond, &priv->mutex); - } - - /* Sample any events before re-enabling interrupts. Any events that - * occur after re-enabling interrupts will have to be handled in the - * next time through the loop. - */ - - eventset = priv->theventset; - priv->theventset = USBSTRG_EVENT_NOEVENTS; - pthread_mutex_unlock(&priv->mutex); - - /* Were we awakened by some event that requires immediate action? - * - * - The USBSTRG_EVENT_DISCONNECT is signalled from the disconnect method - * after all transfers have been stopped, when the host is disconnected. - * - * - The CUSBSTRG_EVENT_RESET is signalled when the bulk-storage-specific - * USBSTRG_REQ_MSRESET EP0 setup received. We must stop the current - * operation and reinialize state. - * - * - The USBSTRG_EVENT_CFGCHANGE is signaled when the EP0 setup logic - * receives a valid USB_REQ_SETCONFIGURATION request - * - * - The USBSTRG_EVENT_IFCHANGE is signaled when the EP0 setup logic - * receives a valid USB_REQ_SETINTERFACE request - * - * - The USBSTRG_EVENT_ABORTBULKOUT event is signalled by the CMDFINISH - * logic when there is a residue after processing a host-to-device - * transfer. We need to discard all incoming request. - * - * All other events are just wakeup calls and are intended only - * drive the state machine. - */ - - if ((eventset & (USBSTRG_EVENT_DISCONNECT|USBSTRG_EVENT_RESET|USBSTRG_EVENT_CFGCHANGE| - USBSTRG_EVENT_IFCHANGE|USBSTRG_EVENT_ABORTBULKOUT)) != 0) - { - /* These events require that the current configuration be reset */ - - if ((eventset & USBSTRG_EVENT_IFCHANGE) != 0) - { - usbstrg_resetconfig(priv); - } - - /* These events require that a new configuration be established */ - - if ((eventset & (USBSTRG_EVENT_CFGCHANGE|USBSTRG_EVENT_IFCHANGE)) != 0) - { - usbstrg_setconfig(priv, priv->thvalue); - } - - /* These events required that we send a deferred EP0 setup response */ - - if ((eventset & (USBSTRG_EVENT_RESET|USBSTRG_EVENT_CFGCHANGE|USBSTRG_EVENT_IFCHANGE)) != 0) - { - usbstrg_deferredresponse(priv, false); - } - - /* For all of these events... terminate any transactions in progress */ - - priv->thstate = USBSTRG_STATE_IDLE; - } - irqrestore(flags); - - /* Loop processing each SCSI command state. Each state handling - * function will do the following: - * - * - If it must block for an event, it will return a negated errno value - * - If it completes the processing for that state, it will (1) set - * the next appropriate state value and (2) return OK. - * - * So the following will loop until we must block for an event in - * a particular state. When we are awakened by an event (above) we - * will resume processing in the same state. - */ - - do - { - switch (priv->thstate) - { - case USBSTRG_STATE_IDLE: /* Started and waiting for commands */ - ret = usbstrg_idlestate(priv); - break; - - case USBSTRG_STATE_CMDPARSE: /* Parsing the received a command */ - ret = usbstrg_cmdparsestate(priv); - break; - - case USBSTRG_STATE_CMDREAD: /* Continuing to process a SCSI read command */ - ret = usbstrg_cmdreadstate(priv); - break; - - case USBSTRG_STATE_CMDWRITE: /* Continuing to process a SCSI write command */ - ret = usbstrg_cmdwritestate(priv); - break; - - case USBSTRG_STATE_CMDFINISH: /* Finish command processing */ - ret = usbstrg_cmdfinishstate(priv); - break; - - case USBSTRG_STATE_CMDSTATUS: /* Processing the status phase of a command */ - ret = usbstrg_cmdstatusstate(priv); - break; - - case USBSTRG_STATE_NOTSTARTED: /* Thread has not yet been started */ - case USBSTRG_STATE_STARTED: /* Started, but is not yet initialized */ - case USBSTRG_STATE_TERMINATED: /* Thread has exitted */ - default: - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_INVALIDSTATE), priv->thstate); - priv->thstate = USBSTRG_STATE_IDLE; - ret = OK; - break; - } - } - while (ret == OK); - } - - /* Transition to the TERMINATED state and exit */ - - priv->thstate = USBSTRG_STATE_TERMINATED; - return NULL; -} diff --git a/nuttx/drivers/usbdev/usbdev_serial.c b/nuttx/drivers/usbdev/usbdev_serial.c deleted file mode 100644 index bef02ed73..000000000 --- a/nuttx/drivers/usbdev/usbdev_serial.c +++ /dev/null @@ -1,2216 +0,0 @@ -/**************************************************************************** - * drivers/usbdev/usbdev_serial.c - * - * Copyright (C) 2008-2011 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * This logic emulates the Prolific PL2303 serial/USB converter - * - * 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 - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -/**************************************************************************** - * Pre-processor Definitions - ****************************************************************************/ - -/* Configuration ************************************************************/ - -/* Number of requests in the write queue */ - -#ifndef CONFIG_USBSER_NWRREQS -# define CONFIG_USBSER_NWRREQS 4 -#endif - -/* Number of requests in the read queue */ - -#ifndef CONFIG_USBSER_NRDREQS -# define CONFIG_USBSER_NRDREQS 4 -#endif - -/* Logical endpoint numbers / max packet sizes */ - -#ifndef CONFIG_USBSER_EPINTIN -# warning "EPINTIN not defined in the configuration" -# define CONFIG_USBSER_EPINTIN 1 -#endif - -#ifndef CONFIG_USBSER_EPBULKOUT -# warning "EPBULKOUT not defined in the configuration" -# define CONFIG_USBSER_EPBULKOUT 2 -#endif - -#ifndef CONFIG_USBSER_EPBULKIN -# warning "EPBULKIN not defined in the configuration" -# define CONFIG_USBSER_EPBULKIN 3 -#endif - -/* Packet and request buffer sizes */ - -#ifndef CONFIG_USBSER_EP0MAXPACKET -# define CONFIG_USBSER_EP0MAXPACKET 64 -#endif - -#undef CONFIG_USBSER_BULKREQLEN - -/* Vendor and product IDs and strings */ - -#ifndef CONFIG_USBSER_VENDORID -# define CONFIG_USBSER_VENDORID 0x067b -#endif - -#ifndef CONFIG_USBSER_PRODUCTID -# define CONFIG_USBSER_PRODUCTID 0x2303 -#endif - -#ifndef CONFIG_USBSER_VENDORSTR -# warning "No Vendor string specified" -# define CONFIG_USBSER_VENDORSTR "NuttX" -#endif - -#ifndef CONFIG_USBSER_PRODUCTSTR -# warning "No Product string specified" -# define CONFIG_USBSER_PRODUCTSTR "USBdev Serial" -#endif - -#undef CONFIG_USBSER_SERIALSTR -#define CONFIG_USBSER_SERIALSTR "0" - -#undef CONFIG_USBSER_CONFIGSTR -#define CONFIG_USBSER_CONFIGSTR "Bulk" - -/* USB Controller */ - -#ifndef CONFIG_USBDEV_SELFPOWERED -# define SELFPOWERED USB_CONFIG_ATT_SELFPOWER -#else -# define SELFPOWERED (0) -#endif - -#ifndef CONFIG_USBDEV_REMOTEWAKEUP -# define REMOTEWAKEUP USB_CONFIG_ATTR_WAKEUP -#else -# define REMOTEWAKEUP (0) -#endif - -#ifndef CONFIG_USBDEV_MAXPOWER -# define CONFIG_USBDEV_MAXPOWER 100 -#endif - -/* Descriptors ****************************************************************/ - -/* These settings are not modifiable via the NuttX configuration */ - -#define USBSER_VERSIONNO (0x0202) /* Device version number */ -#define USBSER_CONFIGIDNONE (0) /* Config ID means to return to address mode */ -#define USBSER_CONFIGID (1) /* The only supported configuration ID */ -#define USBSER_NCONFIGS (1) /* Number of configurations supported */ -#define USBSER_INTERFACEID (0) -#define USBSER_ALTINTERFACEID (0) -#define USBSER_NINTERFACES (1) /* Number of interfaces in the configuration */ -#define USBSER_NENDPOINTS (3) /* Number of endpoints in the interface */ - -/* Endpoint configuration */ - -#define USBSER_EPINTIN_ADDR (USB_DIR_IN|CONFIG_USBSER_EPINTIN) -#define USBSER_EPINTIN_ATTR (USB_EP_ATTR_XFER_INT) -#define USBSER_EPINTIN_MXPACKET (10) - -#define USBSER_EPOUTBULK_ADDR (CONFIG_USBSER_EPBULKOUT) -#define USBSER_EPOUTBULK_ATTR (USB_EP_ATTR_XFER_BULK) - -#define USBSER_EPINBULK_ADDR (USB_DIR_IN|CONFIG_USBSER_EPBULKIN) -#define USBSER_EPINBULK_ATTR (USB_EP_ATTR_XFER_BULK) - -/* String language */ - -#define USBSER_STR_LANGUAGE (0x0409) /* en-us */ - -/* Descriptor strings */ - -#define USBSER_MANUFACTURERSTRID (1) -#define USBSER_PRODUCTSTRID (2) -#define USBSER_SERIALSTRID (3) -#define USBSER_CONFIGSTRID (4) - -/* Buffer big enough for any of our descriptors */ - -#define USBSER_MXDESCLEN (64) - -/* Vender specific control requests *******************************************/ - -#define PL2303_CONTROL_TYPE (0x20) -#define PL2303_SETLINEREQUEST (0x20) /* OUT, Recipient interface */ -#define PL2303_GETLINEREQUEST (0x21) /* IN, Recipient interface */ -#define PL2303_SETCONTROLREQUEST (0x22) /* OUT, Recipient interface */ -#define PL2303_BREAKREQUEST (0x23) /* OUT, Recipient interface */ - -/* Vendor read/write */ - -#define PL2303_RWREQUEST_TYPE (0x40) -#define PL2303_RWREQUEST (0x01) /* IN/OUT, Recipient device */ - -/* Misc Macros ****************************************************************/ - -/* min/max macros */ - -#ifndef min -# define min(a,b) ((a)<(b)?(a):(b)) -#endif - -#ifndef max -# define max(a,b) ((a)>(b)?(a):(b)) -#endif - -/* Trace values *************************************************************/ - -#define USBSER_CLASSAPI_SETUP TRACE_EVENT(TRACE_CLASSAPI_ID, USBSER_TRACECLASSAPI_SETUP) -#define USBSER_CLASSAPI_SHUTDOWN TRACE_EVENT(TRACE_CLASSAPI_ID, USBSER_TRACECLASSAPI_SHUTDOWN) -#define USBSER_CLASSAPI_ATTACH TRACE_EVENT(TRACE_CLASSAPI_ID, USBSER_TRACECLASSAPI_ATTACH) -#define USBSER_CLASSAPI_DETACH TRACE_EVENT(TRACE_CLASSAPI_ID, USBSER_TRACECLASSAPI_DETACH) -#define USBSER_CLASSAPI_IOCTL TRACE_EVENT(TRACE_CLASSAPI_ID, USBSER_TRACECLASSAPI_IOCTL) -#define USBSER_CLASSAPI_RECEIVE TRACE_EVENT(TRACE_CLASSAPI_ID, USBSER_TRACECLASSAPI_RECEIVE) -#define USBSER_CLASSAPI_RXINT TRACE_EVENT(TRACE_CLASSAPI_ID, USBSER_TRACECLASSAPI_RXINT) -#define USBSER_CLASSAPI_RXAVAILABLE TRACE_EVENT(TRACE_CLASSAPI_ID, USBSER_TRACECLASSAPI_RXAVAILABLE) -#define USBSER_CLASSAPI_SEND TRACE_EVENT(TRACE_CLASSAPI_ID, USBSER_TRACECLASSAPI_SEND) -#define USBSER_CLASSAPI_TXINT TRACE_EVENT(TRACE_CLASSAPI_ID, USBSER_TRACECLASSAPI_TXINT) -#define USBSER_CLASSAPI_TXREADY TRACE_EVENT(TRACE_CLASSAPI_ID, USBSER_TRACECLASSAPI_TXREADY) -#define USBSER_CLASSAPI_TXEMPTY TRACE_EVENT(TRACE_CLASSAPI_ID, USBSER_TRACECLASSAPI_TXEMPTY) - -/**************************************************************************** - * Private Types - ****************************************************************************/ - -/* Container to support a list of requests */ - -struct usbser_req_s -{ - FAR struct usbser_req_s *flink; /* Implements a singly linked list */ - FAR struct usbdev_req_s *req; /* The contained request */ -}; - -/* This structure describes the internal state of the driver */ - -struct usbser_dev_s -{ - FAR struct uart_dev_s serdev; /* Serial device structure */ - FAR struct usbdev_s *usbdev; /* usbdev driver pointer */ - - uint8_t config; /* Configuration number */ - uint8_t nwrq; /* Number of queue write requests (in reqlist)*/ - uint8_t nrdq; /* Number of queue read requests (in epbulkout) */ - bool rxenabled; /* true: UART RX "interrupts" enabled */ - uint8_t linest[7]; /* Fake line status */ - int16_t rxhead; /* Working head; used when rx int disabled */ - - FAR struct usbdev_ep_s *epintin; /* Interrupt IN endpoint structure */ - FAR struct usbdev_ep_s *epbulkin; /* Bulk IN endpoint structure */ - FAR struct usbdev_ep_s *epbulkout; /* Bulk OUT endpoint structure */ - FAR struct usbdev_req_s *ctrlreq; /* Control request */ - struct sq_queue_s reqlist; /* List of write request containers */ - - /* Pre-allocated write request containers. The write requests will - * be linked in a free list (reqlist), and used to send requests to - * EPBULKIN; Read requests will be queued in the EBULKOUT. - */ - - struct usbser_req_s wrreqs[CONFIG_USBSER_NWRREQS]; - struct usbser_req_s rdreqs[CONFIG_USBSER_NWRREQS]; - - /* Serial I/O buffers */ - - char rxbuffer[CONFIG_USBSER_RXBUFSIZE]; - char txbuffer[CONFIG_USBSER_TXBUFSIZE]; -}; - -/* The internal version of the class driver */ - -struct usbser_driver_s -{ - struct usbdevclass_driver_s drvr; - FAR struct usbser_dev_s *dev; -}; - -/* This is what is allocated */ - -struct usbser_alloc_s -{ - struct usbser_dev_s dev; - struct usbser_driver_s drvr; -}; - -/**************************************************************************** - * Private Function Prototypes - ****************************************************************************/ - -/* Transfer helpers *********************************************************/ - -static uint16_t usbclass_fillrequest(FAR struct usbser_dev_s *priv, - uint8_t *reqbuf, uint16_t reqlen); -static int usbclass_sndpacket(FAR struct usbser_dev_s *priv); -static inline int usbclass_recvpacket(FAR struct usbser_dev_s *priv, - uint8_t *reqbuf, uint16_t reqlen); - -/* Request helpers *********************************************************/ - -static struct usbdev_req_s *usbclass_allocreq(FAR struct usbdev_ep_s *ep, - uint16_t len); -static void usbclass_freereq(FAR struct usbdev_ep_s *ep, - FAR struct usbdev_req_s *req); - -/* Configuration ***********************************************************/ - -static int usbclass_mkstrdesc(uint8_t id, struct usb_strdesc_s *strdesc); -#ifdef CONFIG_USBDEV_DUALSPEED -static void usbclass_mkepbulkdesc(const struct usb_epdesc_s *indesc, - uint16_t mxpacket, struct usb_epdesc_s *outdesc); -static int16_t usbclass_mkcfgdesc(uint8_t *buf, uint8_t speed, uint8_t type); -#else -static int16_t usbclass_mkcfgdesc(uint8_t *buf); -#endif -static void usbclass_resetconfig(FAR struct usbser_dev_s *priv); -static int usbclass_setconfig(FAR struct usbser_dev_s *priv, - uint8_t config); - -/* Completion event handlers ***********************************************/ - -static void usbclass_ep0incomplete(FAR struct usbdev_ep_s *ep, - FAR struct usbdev_req_s *req); -static void usbclass_rdcomplete(FAR struct usbdev_ep_s *ep, - FAR struct usbdev_req_s *req); -static void usbclass_wrcomplete(FAR struct usbdev_ep_s *ep, - FAR struct usbdev_req_s *req); - -/* USB class device ********************************************************/ - -static int usbclass_bind(FAR struct usbdev_s *dev, - FAR struct usbdevclass_driver_s *driver); -static void usbclass_unbind(FAR struct usbdev_s *dev); -static int usbclass_setup(FAR struct usbdev_s *dev, - const struct usb_ctrlreq_s *ctrl); -static void usbclass_disconnect(FAR struct usbdev_s *dev); - -/* Serial port *************************************************************/ - -static int usbser_setup(FAR struct uart_dev_s *dev); -static void usbser_shutdown(FAR struct uart_dev_s *dev); -static int usbser_attach(FAR struct uart_dev_s *dev); -static void usbser_detach(FAR struct uart_dev_s *dev); -static void usbser_rxint(FAR struct uart_dev_s *dev, bool enable); -static void usbser_txint(FAR struct uart_dev_s *dev, bool enable); -static bool usbser_txempty(FAR struct uart_dev_s *dev); - -/**************************************************************************** - * Private Variables - ****************************************************************************/ - -/* USB class device ********************************************************/ - -static const struct usbdevclass_driverops_s g_driverops = -{ - usbclass_bind, /* bind */ - usbclass_unbind, /* unbind */ - usbclass_setup, /* setup */ - usbclass_disconnect, /* disconnect */ - NULL, /* suspend */ - NULL, /* resume */ -}; - -/* Serial port *************************************************************/ - -static const struct uart_ops_s g_uartops = -{ - usbser_setup, /* setup */ - usbser_shutdown, /* shutdown */ - usbser_attach, /* attach */ - usbser_detach, /* detach */ - NULL, /* ioctl */ - NULL, /* receive */ - usbser_rxint, /* rxinit */ - NULL, /* rxavailable */ - NULL, /* send */ - usbser_txint, /* txinit */ - NULL, /* txready */ - usbser_txempty /* txempty */ -}; - -/* USB descriptor templates these will be copied and modified **************/ - -static const struct usb_devdesc_s g_devdesc = -{ - USB_SIZEOF_DEVDESC, /* len */ - USB_DESC_TYPE_DEVICE, /* type */ - {LSBYTE(0x0200), MSBYTE(0x0200)}, /* usb */ - USB_CLASS_PER_INTERFACE, /* class */ - 0, /* subclass */ - 0, /* protocol */ - CONFIG_USBSER_EP0MAXPACKET, /* maxpacketsize */ - { LSBYTE(CONFIG_USBSER_VENDORID), /* vendor */ - MSBYTE(CONFIG_USBSER_VENDORID) }, - { LSBYTE(CONFIG_USBSER_PRODUCTID), /* product */ - MSBYTE(CONFIG_USBSER_PRODUCTID) }, - { LSBYTE(USBSER_VERSIONNO), /* device */ - MSBYTE(USBSER_VERSIONNO) }, - USBSER_MANUFACTURERSTRID, /* imfgr */ - USBSER_PRODUCTSTRID, /* iproduct */ - USBSER_SERIALSTRID, /* serno */ - USBSER_NCONFIGS /* nconfigs */ -}; - -static const struct usb_cfgdesc_s g_cfgdesc = -{ - USB_SIZEOF_CFGDESC, /* len */ - USB_DESC_TYPE_CONFIG, /* type */ - {0, 0}, /* totallen -- to be provided */ - USBSER_NINTERFACES, /* ninterfaces */ - USBSER_CONFIGID, /* cfgvalue */ - USBSER_CONFIGSTRID, /* icfg */ - USB_CONFIG_ATTR_ONE|SELFPOWERED|REMOTEWAKEUP, /* attr */ - (CONFIG_USBDEV_MAXPOWER + 1) / 2 /* mxpower */ -}; - -static const struct usb_ifdesc_s g_ifdesc = -{ - USB_SIZEOF_IFDESC, /* len */ - USB_DESC_TYPE_INTERFACE, /* type */ - 0, /* ifno */ - 0, /* alt */ - USBSER_NENDPOINTS, /* neps */ - USB_CLASS_VENDOR_SPEC, /* class */ - 0, /* subclass */ - 0, /* protocol */ - USBSER_CONFIGSTRID /* iif */ -}; - -static const struct usb_epdesc_s g_epintindesc = -{ - USB_SIZEOF_EPDESC, /* len */ - USB_DESC_TYPE_ENDPOINT, /* type */ - USBSER_EPINTIN_ADDR, /* addr */ - USBSER_EPINTIN_ATTR, /* attr */ - { LSBYTE(USBSER_EPINTIN_MXPACKET), /* maxpacket */ - MSBYTE(USBSER_EPINTIN_MXPACKET) }, - 1 /* interval */ -}; - -static const struct usb_epdesc_s g_epbulkoutdesc = -{ - USB_SIZEOF_EPDESC, /* len */ - USB_DESC_TYPE_ENDPOINT, /* type */ - USBSER_EPOUTBULK_ADDR, /* addr */ - USBSER_EPOUTBULK_ATTR, /* attr */ - { LSBYTE(64), MSBYTE(64) }, /* maxpacket -- might change to 512*/ - 0 /* interval */ -}; - -static const struct usb_epdesc_s g_epbulkindesc = -{ - USB_SIZEOF_EPDESC, /* len */ - USB_DESC_TYPE_ENDPOINT, /* type */ - USBSER_EPINBULK_ADDR, /* addr */ - USBSER_EPINBULK_ATTR, /* attr */ - { LSBYTE(64), MSBYTE(64) }, /* maxpacket -- might change to 512*/ - 0 /* interval */ -}; - -#ifdef CONFIG_USBDEV_DUALSPEED -static const struct usb_qualdesc_s g_qualdesc = -{ - USB_SIZEOF_QUALDESC, /* len */ - USB_DESC_TYPE_DEVICEQUALIFIER, /* type */ - {LSBYTE(0x0200), MSBYTE(0x0200) }, /* USB */ - USB_CLASS_VENDOR_SPEC, /* class */ - 0, /* subclass */ - 0, /* protocol */ - CONFIG_USBSER_EP0MAXPACKET, /* mxpacketsize */ - USBSER_NCONFIGS, /* nconfigs */ - 0, /* reserved */ -}; -#endif - -/**************************************************************************** - * Private Functions - ****************************************************************************/ - -/************************************************************************************ - * Name: usbclass_fillrequest - * - * Description: - * If there is data to send it is copied to the given buffer. Called either - * to initiate the first write operation, or from the completion interrupt handler - * service consecutive write operations. - * - * NOTE: The USB serial driver does not use the serial drivers uart_xmitchars() - * API. That logic is essentially duplicated here because unlike UART hardware, - * we need to be able to handle writes not byte-by-byte, but packet-by-packet. - * Unfortunately, that decision also exposes some internals of the serial driver - * in the following. - * - ************************************************************************************/ - -static uint16_t usbclass_fillrequest(FAR struct usbser_dev_s *priv, uint8_t *reqbuf, - uint16_t reqlen) -{ - FAR uart_dev_t *serdev = &priv->serdev; - FAR struct uart_buffer_s *xmit = &serdev->xmit; - irqstate_t flags; - uint16_t nbytes = 0; - - /* Disable interrupts */ - - flags = irqsave(); - - /* Transfer bytes while we have bytes available and there is room in the request */ - - while (xmit->head != xmit->tail && nbytes < reqlen) - { - *reqbuf++ = xmit->buffer[xmit->tail]; - nbytes++; - - /* Increment the tail pointer */ - - if (++(xmit->tail) >= xmit->size) - { - xmit->tail = 0; - } - } - - /* When all of the characters have been sent from the buffer - * disable the "TX interrupt". - */ - - if (xmit->head == xmit->tail) - { - uart_disabletxint(serdev); - } - - /* If any bytes were removed from the buffer, inform any waiters - * there there is space available. - */ - - if (nbytes) - { - uart_datasent(serdev); - } - - irqrestore(flags); - return nbytes; -} - -/************************************************************************************ - * Name: usbclass_sndpacket - * - * Description: - * This function obtains write requests, transfers the TX data into the request, - * and submits the requests to the USB controller. This continues untils either - * (1) there are no further packets available, or (2) thre is not further data - * to send. - * - ************************************************************************************/ - -static int usbclass_sndpacket(FAR struct usbser_dev_s *priv) -{ - FAR struct usbdev_ep_s *ep; - FAR struct usbdev_req_s *req; - FAR struct usbser_req_s *reqcontainer; - irqstate_t flags; - int len; - int ret = OK; - -#ifdef CONFIG_DEBUG - if (priv == NULL) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_INVALIDARG), 0); - return -ENODEV; - } -#endif - - flags = irqsave(); - - /* Use our IN endpoint for the transfer */ - - ep = priv->epbulkin; - - /* Loop until either (1) we run out or write requests, or (2) usbclass_fillrequest() - * is unable to fill the request with data (i.e., untilthere is no more data - * to be sent). - */ - - uvdbg("head=%d tail=%d nwrq=%d empty=%d\n", - priv->serdev.xmit.head, priv->serdev.xmit.tail, - priv->nwrq, sq_empty(&priv->reqlist)); - - while (!sq_empty(&priv->reqlist)) - { - /* Peek at the request in the container at the head of the list */ - - reqcontainer = (struct usbser_req_s *)sq_peek(&priv->reqlist); - req = reqcontainer->req; - - /* Fill the request with serial TX data */ - - len = usbclass_fillrequest(priv, req->buf, req->len); - if (len > 0) - { - /* Remove the empty container from the request list */ - - (void)sq_remfirst(&priv->reqlist); - priv->nwrq--; - - /* Then submit the request to the endpoint */ - - req->len = len; - req->priv = reqcontainer; - req->flags = USBDEV_REQFLAGS_NULLPKT; - ret = EP_SUBMIT(ep, req); - if (ret != OK) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_SUBMITFAIL), (uint16_t)-ret); - break; - } - } - else - { - break; - } - } - - irqrestore(flags); - return ret; -} - -/************************************************************************************ - * Name: usbclass_recvpacket - * - * Description: - * A normal completion event was received by the read completion handler at the - * interrupt level (with interrupts disabled). This function handles the USB packet - * and provides the received data to the uart RX buffer. - * - * Assumptions: - * Called from the USB interrupt handler with interrupts disabled. - * - ************************************************************************************/ - -static inline int usbclass_recvpacket(FAR struct usbser_dev_s *priv, - uint8_t *reqbuf, uint16_t reqlen) -{ - FAR uart_dev_t *serdev = &priv->serdev; - FAR struct uart_buffer_s *recv = &serdev->recv; - uint16_t currhead; - uint16_t nexthead; - uint16_t nbytes = 0; - - /* Get the next head index. During the time that RX interrupts are disabled, the - * the serial driver will be extracting data from the circular buffer and modifying - * recv.tail. During this time, we should avoid modifying recv.head; Instead we will - * use a shadow copy of the index. When interrupts are restored, the real recv.head - * will be updated with this indes. - */ - - if (priv->rxenabled) - { - currhead = recv->head; - } - else - { - currhead = priv->rxhead; - } - - /* Pre-calculate the head index and check for wrap around. We need to do this - * so that we can determine if the circular buffer will overrun BEFORE we - * overrun the buffer! - */ - - nexthead = currhead + 1; - if (nexthead >= recv->size) - { - nexthead = 0; - } - - /* Then copy data into the RX buffer until either: (1) all of the data has been - * copied, or (2) the RX buffer is full. NOTE: If the RX buffer becomes full, - * then we have overrun the serial driver and data will be lost. - */ - - while (nexthead != recv->tail && nbytes < reqlen) - { - /* Copy one byte to the head of the circular RX buffer */ - - recv->buffer[currhead] = *reqbuf++; - - /* Update counts and indices */ - - currhead = nexthead; - nbytes++; - - /* Increment the head index and check for wrap around */ - - nexthead = currhead + 1; - if (nexthead >= recv->size) - { - nexthead = 0; - } - } - - /* Write back the head pointer using the shadow index if RX "interrupts" - * are disabled. - */ - - if (priv->rxenabled) - { - recv->head = currhead; - } - else - { - priv->rxhead = currhead; - } - - /* If data was added to the incoming serial buffer, then wake up any - * threads is waiting for incoming data. If we are running in an interrupt - * handler, then the serial driver will not run until the interrupt handler - * returns. - */ - - if (priv->rxenabled && nbytes > 0) - { - uart_datareceived(serdev); - } - - /* Return an error if the entire packet could not be transferred */ - - if (nbytes < reqlen) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_RXOVERRUN), 0); - return -ENOSPC; - } - return OK; -} - -/**************************************************************************** - * Name: usbclass_allocreq - * - * Description: - * Allocate a request instance along with its buffer - * - ****************************************************************************/ - -static struct usbdev_req_s *usbclass_allocreq(FAR struct usbdev_ep_s *ep, - uint16_t len) -{ - FAR struct usbdev_req_s *req; - - req = EP_ALLOCREQ(ep); - if (req != NULL) - { - req->len = len; - req->buf = EP_ALLOCBUFFER(ep, len); - if (!req->buf) - { - EP_FREEREQ(ep, req); - req = NULL; - } - } - return req; -} - -/**************************************************************************** - * Name: usbclass_freereq - * - * Description: - * Free a request instance along with its buffer - * - ****************************************************************************/ - -static void usbclass_freereq(FAR struct usbdev_ep_s *ep, - FAR struct usbdev_req_s *req) -{ - if (ep != NULL && req != NULL) - { - if (req->buf != NULL) - { - EP_FREEBUFFER(ep, req->buf); - } - EP_FREEREQ(ep, req); - } -} - -/**************************************************************************** - * Name: usbclass_mkstrdesc - * - * Description: - * Construct a string descriptor - * - ****************************************************************************/ - -static int usbclass_mkstrdesc(uint8_t id, struct usb_strdesc_s *strdesc) -{ - const char *str; - int len; - int ndata; - int i; - - switch (id) - { - case 0: - { - /* Descriptor 0 is the language id */ - - strdesc->len = 4; - strdesc->type = USB_DESC_TYPE_STRING; - strdesc->data[0] = LSBYTE(USBSER_STR_LANGUAGE); - strdesc->data[1] = MSBYTE(USBSER_STR_LANGUAGE); - return 4; - } - - case USBSER_MANUFACTURERSTRID: - str = CONFIG_USBSER_VENDORSTR; - break; - - case USBSER_PRODUCTSTRID: - str = CONFIG_USBSER_PRODUCTSTR; - break; - - case USBSER_SERIALSTRID: - str = CONFIG_USBSER_SERIALSTR; - break; - - case USBSER_CONFIGSTRID: - str = CONFIG_USBSER_CONFIGSTR; - break; - - default: - return -EINVAL; - } - - /* The string is utf16-le. The poor man's utf-8 to utf16-le - * conversion below will only handle 7-bit en-us ascii - */ - - len = strlen(str); - for (i = 0, ndata = 0; i < len; i++, ndata += 2) - { - strdesc->data[ndata] = str[i]; - strdesc->data[ndata+1] = 0; - } - - strdesc->len = ndata+2; - strdesc->type = USB_DESC_TYPE_STRING; - return strdesc->len; -} - -/**************************************************************************** - * Name: usbclass_mkepbulkdesc - * - * Description: - * Construct the endpoint descriptor - * - ****************************************************************************/ - -#ifdef CONFIG_USBDEV_DUALSPEED -static inline void usbclass_mkepbulkdesc(const FAR struct usb_epdesc_s *indesc, - uint16_t mxpacket, - FAR struct usb_epdesc_s *outdesc) -{ - /* Copy the canned descriptor */ - - memcpy(outdesc, indesc, USB_SIZEOF_EPDESC); - - /* Then add the correct max packet size */ - - outdesc->mxpacketsize[0] = LSBYTE(mxpacket); - outdesc->mxpacketsize[1] = MSBYTE(mxpacket); -} -#endif - -/**************************************************************************** - * Name: usbclass_mkcfgdesc - * - * Description: - * Construct the configuration descriptor - * - ****************************************************************************/ - -#ifdef CONFIG_USBDEV_DUALSPEED -static int16_t usbclass_mkcfgdesc(uint8_t *buf, uint8_t speed, uint8_t type) -#else -static int16_t usbclass_mkcfgdesc(uint8_t *buf) -#endif -{ - FAR struct usb_cfgdesc_s *cfgdesc = (struct usb_cfgdesc_s*)buf; -#ifdef CONFIG_USBDEV_DUALSPEED - bool hispeed = (speed == USB_SPEED_HIGH); - uint16_t bulkmxpacket; -#endif - uint16_t totallen; - - /* This is the total length of the configuration (not necessarily the - * size that we will be sending now. - */ - - totallen = USB_SIZEOF_CFGDESC + USB_SIZEOF_IFDESC + USBSER_NENDPOINTS * USB_SIZEOF_EPDESC; - - /* Configuration descriptor -- Copy the canned descriptor and fill in the - * type (we'll also need to update the size below - */ - - memcpy(cfgdesc, &g_cfgdesc, USB_SIZEOF_CFGDESC); - buf += USB_SIZEOF_CFGDESC; - - /* Copy the canned interface descriptor */ - - memcpy(buf, &g_ifdesc, USB_SIZEOF_IFDESC); - buf += USB_SIZEOF_IFDESC; - - /* Make the three endpoint configurations. First, check for switches - * between high and full speed - */ - -#ifdef CONFIG_USBDEV_DUALSPEED - if (type == USB_DESC_TYPE_OTHERSPEEDCONFIG) - { - hispeed = !hispeed; - } -#endif - - memcpy(buf, &g_epintindesc, USB_SIZEOF_EPDESC); - buf += USB_SIZEOF_EPDESC; - -#ifdef CONFIG_USBDEV_DUALSPEED - if (hispeed) - { - bulkmxpacket = 512; - } - else - { - bulkmxpacket = 64; - } - - usbclass_mkepbulkdesc(&g_epbulkoutdesc, bulkmxpacket, (struct usb_epdesc_s*)buf); - buf += USB_SIZEOF_EPDESC; - usbclass_mkepbulkdesc(&g_epbulkindesc, bulkmxpacket, (struct usb_epdesc_s*)buf); -#else - memcpy(buf, &g_epbulkoutdesc, USB_SIZEOF_EPDESC); - buf += USB_SIZEOF_EPDESC; - memcpy(buf, &g_epbulkindesc, USB_SIZEOF_EPDESC); -#endif - - /* Finally, fill in the total size of the configuration descriptor */ - - cfgdesc->totallen[0] = LSBYTE(totallen); - cfgdesc->totallen[1] = MSBYTE(totallen); - return totallen; -} - -/**************************************************************************** - * Name: usbclass_resetconfig - * - * Description: - * Mark the device as not configured and disable all endpoints. - * - ****************************************************************************/ - -static void usbclass_resetconfig(FAR struct usbser_dev_s *priv) -{ - /* Are we configured? */ - - if (priv->config != USBSER_CONFIGIDNONE) - { - /* Yes.. but not anymore */ - - priv->config = USBSER_CONFIGIDNONE; - - /* Disable endpoints. This should force completion of all pending - * transfers. - */ - - EP_DISABLE(priv->epintin); - EP_DISABLE(priv->epbulkin); - EP_DISABLE(priv->epbulkout); - } -} - -/**************************************************************************** - * Name: usbclass_setconfig - * - * Description: - * Set the device configuration by allocating and configuring endpoints and - * by allocating and queue read and write requests. - * - ****************************************************************************/ - -static int usbclass_setconfig(FAR struct usbser_dev_s *priv, uint8_t config) -{ - FAR struct usbdev_req_s *req; -#ifdef CONFIG_USBDEV_DUALSPEED - struct usb_epdesc_s epdesc; - uint16_t bulkmxpacket; -#endif - int i; - int ret = 0; - -#if CONFIG_DEBUG - if (priv == NULL) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_INVALIDARG), 0); - return -EIO; - } -#endif - - if (config == priv->config) - { - /* Already configured -- Do nothing */ - - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_ALREADYCONFIGURED), 0); - return 0; - } - - /* Discard the previous configuration data */ - - usbclass_resetconfig(priv); - - /* Was this a request to simply discard the current configuration? */ - - if (config == USBSER_CONFIGIDNONE) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_CONFIGNONE), 0); - return 0; - } - - /* We only accept one configuration */ - - if (config != USBSER_CONFIGID) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_CONFIGIDBAD), 0); - return -EINVAL; - } - - /* Configure the IN interrupt endpoint */ - - ret = EP_CONFIGURE(priv->epintin, &g_epintindesc, false); - if (ret < 0) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_EPINTINCONFIGFAIL), 0); - goto errout; - } - priv->epintin->priv = priv; - - /* Configure the IN bulk endpoint */ - -#ifdef CONFIG_USBDEV_DUALSPEED - if (priv->usbdev->speed == USB_SPEED_HIGH) - { - bulkmxpacket = 512; - } - else - { - bulkmxpacket = 64; - } - - usbclass_mkepbulkdesc(&g_epbulkindesc, bulkmxpacket, &epdesc); - ret = EP_CONFIGURE(priv->epbulkin, &epdesc, false); -#else - ret = EP_CONFIGURE(priv->epbulkin, &g_epbulkindesc, false); -#endif - if (ret < 0) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_EPBULKINCONFIGFAIL), 0); - goto errout; - } - - priv->epbulkin->priv = priv; - - /* Configure the OUT bulk endpoint */ - -#ifdef CONFIG_USBDEV_DUALSPEED - usbclass_mkepbulkdesc(&g_epbulkoutdesc, bulkmxpacket, &epdesc); - ret = EP_CONFIGURE(priv->epbulkout, &epdesc, true); -#else - ret = EP_CONFIGURE(priv->epbulkout, &g_epbulkoutdesc, true); -#endif - if (ret < 0) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_EPBULKOUTCONFIGFAIL), 0); - goto errout; - } - - priv->epbulkout->priv = priv; - - /* Queue read requests in the bulk OUT endpoint */ - - DEBUGASSERT(priv->nrdq == 0); - for (i = 0; i < CONFIG_USBSER_NRDREQS; i++) - { - req = priv->rdreqs[i].req; - req->callback = usbclass_rdcomplete; - ret = EP_SUBMIT(priv->epbulkout, req); - if (ret != OK) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_RDSUBMIT), (uint16_t)-ret); - goto errout; - } - priv->nrdq++; - } - - priv->config = config; - return OK; - -errout: - usbclass_resetconfig(priv); - return ret; -} - -/**************************************************************************** - * Name: usbclass_ep0incomplete - * - * Description: - * Handle completion of EP0 control operations - * - ****************************************************************************/ - -static void usbclass_ep0incomplete(FAR struct usbdev_ep_s *ep, - FAR struct usbdev_req_s *req) -{ - if (req->result || req->xfrd != req->len) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_REQRESULT), (uint16_t)-req->result); - } -} - -/**************************************************************************** - * Name: usbclass_rdcomplete - * - * Description: - * Handle completion of read request on the bulk OUT endpoint. This - * is handled like the receipt of serial data on the "UART" - * - ****************************************************************************/ - -static void usbclass_rdcomplete(FAR struct usbdev_ep_s *ep, - FAR struct usbdev_req_s *req) -{ - FAR struct usbser_dev_s *priv; - irqstate_t flags; - int ret; - - /* Sanity check */ - -#ifdef CONFIG_DEBUG - if (!ep || !ep->priv || !req) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_INVALIDARG), 0); - return; - } -#endif - - /* Extract references to private data */ - - priv = (FAR struct usbser_dev_s*)ep->priv; - - /* Process the received data unless this is some unusual condition */ - - flags = irqsave(); - switch (req->result) - { - case 0: /* Normal completion */ - usbtrace(TRACE_CLASSRDCOMPLETE, priv->nrdq); - usbclass_recvpacket(priv, req->buf, req->xfrd); - break; - - case -ESHUTDOWN: /* Disconnection */ - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_RDSHUTDOWN), 0); - priv->nrdq--; - irqrestore(flags); - return; - - default: /* Some other error occurred */ - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_RDUNEXPECTED), (uint16_t)-req->result); - break; - }; - - /* Requeue the read request */ - -#ifdef CONFIG_USBSER_BULKREQLEN - req->len = max(CONFIG_USBSER_BULKREQLEN, ep->maxpacket); -#else - req->len = ep->maxpacket; -#endif - - ret = EP_SUBMIT(ep, req); - if (ret != OK) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_RDSUBMIT), (uint16_t)-req->result); - } - irqrestore(flags); -} - -/**************************************************************************** - * Name: usbclass_wrcomplete - * - * Description: - * Handle completion of write request. This function probably executes - * in the context of an interrupt handler. - * - ****************************************************************************/ - -static void usbclass_wrcomplete(FAR struct usbdev_ep_s *ep, - FAR struct usbdev_req_s *req) -{ - FAR struct usbser_dev_s *priv; - FAR struct usbser_req_s *reqcontainer; - irqstate_t flags; - - /* Sanity check */ - -#ifdef CONFIG_DEBUG - if (!ep || !ep->priv || !req || !req->priv) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_INVALIDARG), 0); - return; - } -#endif - - /* Extract references to our private data */ - - priv = (FAR struct usbser_dev_s *)ep->priv; - reqcontainer = (FAR struct usbser_req_s *)req->priv; - - /* Return the write request to the free list */ - - flags = irqsave(); - sq_addlast((sq_entry_t*)reqcontainer, &priv->reqlist); - priv->nwrq++; - irqrestore(flags); - - /* Send the next packet unless this was some unusual termination - * condition - */ - - switch (req->result) - { - case OK: /* Normal completion */ - usbtrace(TRACE_CLASSWRCOMPLETE, priv->nwrq); - usbclass_sndpacket(priv); - break; - - case -ESHUTDOWN: /* Disconnection */ - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_WRSHUTDOWN), priv->nwrq); - break; - - default: /* Some other error occurred */ - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_WRUNEXPECTED), (uint16_t)-req->result); - break; - } -} - -/**************************************************************************** - * USB Class Driver Methods - ****************************************************************************/ - -/**************************************************************************** - * Name: usbclass_bind - * - * Description: - * Invoked when the driver is bound to a USB device driver - * - ****************************************************************************/ - -static int usbclass_bind(FAR struct usbdev_s *dev, FAR struct usbdevclass_driver_s *driver) -{ - FAR struct usbser_dev_s *priv = ((struct usbser_driver_s*)driver)->dev; - FAR struct usbser_req_s *reqcontainer; - irqstate_t flags; - uint16_t reqlen; - int ret; - int i; - - usbtrace(TRACE_CLASSBIND, 0); - - /* Bind the structures */ - - priv->usbdev = dev; - dev->ep0->priv = priv; - - /* Preallocate control request */ - - priv->ctrlreq = usbclass_allocreq(dev->ep0, USBSER_MXDESCLEN); - if (priv->ctrlreq == NULL) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_ALLOCCTRLREQ), 0); - ret = -ENOMEM; - goto errout; - } - priv->ctrlreq->callback = usbclass_ep0incomplete; - - /* Pre-allocate all endpoints... the endpoints will not be functional - * until the SET CONFIGURATION request is processed in usbclass_setconfig. - * This is done here because there may be calls to kmalloc and the SET - * CONFIGURATION processing probably occurrs within interrupt handling - * logic where kmalloc calls will fail. - */ - - /* Pre-allocate the IN interrupt endpoint */ - - priv->epintin = DEV_ALLOCEP(dev, USBSER_EPINTIN_ADDR, true, USB_EP_ATTR_XFER_INT); - if (!priv->epintin) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_EPINTINALLOCFAIL), 0); - ret = -ENODEV; - goto errout; - } - priv->epintin->priv = priv; - - /* Pre-allocate the IN bulk endpoint */ - - priv->epbulkin = DEV_ALLOCEP(dev, USBSER_EPINBULK_ADDR, true, USB_EP_ATTR_XFER_BULK); - if (!priv->epbulkin) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_EPBULKINALLOCFAIL), 0); - ret = -ENODEV; - goto errout; - } - priv->epbulkin->priv = priv; - - /* Pre-allocate the OUT bulk endpoint */ - - priv->epbulkout = DEV_ALLOCEP(dev, USBSER_EPOUTBULK_ADDR, false, USB_EP_ATTR_XFER_BULK); - if (!priv->epbulkout) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_EPBULKOUTALLOCFAIL), 0); - ret = -ENODEV; - goto errout; - } - priv->epbulkout->priv = priv; - - /* Pre-allocate read requests */ - -#ifdef CONFIG_USBSER_BULKREQLEN - reqlen = max(CONFIG_USBSER_BULKREQLEN, priv->epbulkout->maxpacket); -#else - reqlen = priv->epbulkout->maxpacket; -#endif - - for (i = 0; i < CONFIG_USBSER_NRDREQS; i++) - { - reqcontainer = &priv->rdreqs[i]; - reqcontainer->req = usbclass_allocreq(priv->epbulkout, reqlen); - if (reqcontainer->req == NULL) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_RDALLOCREQ), -ENOMEM); - ret = -ENOMEM; - goto errout; - } - reqcontainer->req->priv = reqcontainer; - reqcontainer->req->callback = usbclass_rdcomplete; - } - - /* Pre-allocate write request containers and put in a free list */ - -#ifdef CONFIG_USBSER_BULKREQLEN - reqlen = max(CONFIG_USBSER_BULKREQLEN, priv->epbulkin->maxpacket); -#else - reqlen = priv->epbulkin->maxpacket; -#endif - - for (i = 0; i < CONFIG_USBSER_NWRREQS; i++) - { - reqcontainer = &priv->wrreqs[i]; - reqcontainer->req = usbclass_allocreq(priv->epbulkin, reqlen); - if (reqcontainer->req == NULL) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_WRALLOCREQ), -ENOMEM); - ret = -ENOMEM; - goto errout; - } - reqcontainer->req->priv = reqcontainer; - reqcontainer->req->callback = usbclass_wrcomplete; - - flags = irqsave(); - sq_addlast((sq_entry_t*)reqcontainer, &priv->reqlist); - priv->nwrq++; /* Count of write requests available */ - irqrestore(flags); - } - - /* Report if we are selfpowered */ - -#ifdef CONFIG_USBDEV_SELFPOWERED - DEV_SETSELFPOWERED(dev); -#endif - - /* And pull-up the data line for the soft connect function */ - - DEV_CONNECT(dev); - return OK; - -errout: - usbclass_unbind(dev); - return ret; -} - -/**************************************************************************** - * Name: usbclass_unbind - * - * Description: - * Invoked when the driver is unbound from a USB device driver - * - ****************************************************************************/ - -static void usbclass_unbind(FAR struct usbdev_s *dev) -{ - FAR struct usbser_dev_s *priv; - FAR struct usbser_req_s *reqcontainer; - irqstate_t flags; - int i; - - usbtrace(TRACE_CLASSUNBIND, 0); - -#ifdef CONFIG_DEBUG - if (!dev || !dev->ep0) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_INVALIDARG), 0); - return; - } -#endif - - /* Extract reference to private data */ - - priv = (FAR struct usbser_dev_s *)dev->ep0->priv; - -#ifdef CONFIG_DEBUG - if (!priv) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_EP0NOTBOUND), 0); - return; - } -#endif - - /* Make sure that we are not already unbound */ - - if (priv != NULL) - { - /* Make sure that the endpoints have been unconfigured. If - * we were terminated gracefully, then the configuration should - * already have been reset. If not, then calling usbclass_resetconfig - * should cause the endpoints to immediately terminate all - * transfers and return the requests to us (with result == -ESHUTDOWN) - */ - - usbclass_resetconfig(priv); - up_mdelay(50); - - /* Free the interrupt IN endpoint */ - - if (priv->epintin) - { - DEV_FREEEP(dev, priv->epintin); - priv->epintin = NULL; - } - - /* Free the bulk IN endpoint */ - - if (priv->epbulkin) - { - DEV_FREEEP(dev, priv->epbulkin); - priv->epbulkin = NULL; - } - - /* Free the pre-allocated control request */ - - if (priv->ctrlreq != NULL) - { - usbclass_freereq(dev->ep0, priv->ctrlreq); - priv->ctrlreq = NULL; - } - - /* Free pre-allocated read requests (which should all have - * been returned to the free list at this time -- we don't check) - */ - - DEBUGASSERT(priv->nrdq == 0); - for (i = 0; i < CONFIG_USBSER_NRDREQS; i++) - { - reqcontainer = &priv->rdreqs[i]; - if (reqcontainer->req) - { - usbclass_freereq(priv->epbulkout, reqcontainer->req); - reqcontainer->req = NULL; - } - } - - /* Free the bulk OUT endpoint */ - - if (priv->epbulkout) - { - DEV_FREEEP(dev, priv->epbulkout); - priv->epbulkout = NULL; - } - - /* Free write requests that are not in use (which should be all - * of them - */ - - flags = irqsave(); - DEBUGASSERT(priv->nwrq == CONFIG_USBSER_NWRREQS); - while (!sq_empty(&priv->reqlist)) - { - reqcontainer = (struct usbser_req_s *)sq_remfirst(&priv->reqlist); - if (reqcontainer->req != NULL) - { - usbclass_freereq(priv->epbulkin, reqcontainer->req); - priv->nwrq--; /* Number of write requests queued */ - } - } - DEBUGASSERT(priv->nwrq == 0); - irqrestore(flags); - } - - /* Clear out all data in the circular buffer */ - - priv->serdev.xmit.head = 0; - priv->serdev.xmit.tail = 0; -} - -/**************************************************************************** - * Name: usbclass_setup - * - * Description: - * Invoked for ep0 control requests. This function probably executes - * in the context of an interrupt handler. - * - ****************************************************************************/ - -static int usbclass_setup(FAR struct usbdev_s *dev, const struct usb_ctrlreq_s *ctrl) -{ - FAR struct usbser_dev_s *priv; - FAR struct usbdev_req_s *ctrlreq; - uint16_t value; - uint16_t index; - uint16_t len; - int ret = -EOPNOTSUPP; - -#ifdef CONFIG_DEBUG - if (!dev || !dev->ep0 || !ctrl) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_INVALIDARG), 0); - return -EIO; - } -#endif - - /* Extract reference to private data */ - - usbtrace(TRACE_CLASSSETUP, ctrl->req); - priv = (FAR struct usbser_dev_s *)dev->ep0->priv; - -#ifdef CONFIG_DEBUG - if (!priv || !priv->ctrlreq) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_EP0NOTBOUND), 0); - return -ENODEV; - } -#endif - ctrlreq = priv->ctrlreq; - - /* Extract the little-endian 16-bit values to host order */ - - value = GETUINT16(ctrl->value); - index = GETUINT16(ctrl->index); - len = GETUINT16(ctrl->len); - - uvdbg("type=%02x req=%02x value=%04x index=%04x len=%04x\n", - ctrl->type, ctrl->req, value, index, len); - - switch (ctrl->type & USB_REQ_TYPE_MASK) - { - /*********************************************************************** - * Standard Requests - ***********************************************************************/ - - case USB_REQ_TYPE_STANDARD: - { - switch (ctrl->req) - { - case USB_REQ_GETDESCRIPTOR: - { - /* The value field specifies the descriptor type in the MS byte and the - * descriptor index in the LS byte (order is little endian) - */ - - switch (ctrl->value[1]) - { - case USB_DESC_TYPE_DEVICE: - { - ret = USB_SIZEOF_DEVDESC; - memcpy(ctrlreq->buf, &g_devdesc, ret); - } - break; - -#ifdef CONFIG_USBDEV_DUALSPEED - case USB_DESC_TYPE_DEVICEQUALIFIER: - { - ret = USB_SIZEOF_QUALDESC; - memcpy(ctrlreq->buf, &g_qualdesc, ret); - } - break; - - case USB_DESC_TYPE_OTHERSPEEDCONFIG: -#endif /* CONFIG_USBDEV_DUALSPEED */ - - case USB_DESC_TYPE_CONFIG: - { -#ifdef CONFIG_USBDEV_DUALSPEED - ret = usbclass_mkcfgdesc(ctrlreq->buf, dev->speed, ctrl->req); -#else - ret = usbclass_mkcfgdesc(ctrlreq->buf); -#endif - } - break; - - case USB_DESC_TYPE_STRING: - { - /* index == language code. */ - - ret = usbclass_mkstrdesc(ctrl->value[0], (struct usb_strdesc_s *)ctrlreq->buf); - } - break; - - default: - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_GETUNKNOWNDESC), value); - } - break; - } - } - break; - - case USB_REQ_SETCONFIGURATION: - { - if (ctrl->type == 0) - { - ret = usbclass_setconfig(priv, value); - } - } - break; - - case USB_REQ_GETCONFIGURATION: - { - if (ctrl->type == USB_DIR_IN) - { - *(uint8_t*)ctrlreq->buf = priv->config; - ret = 1; - } - } - break; - - case USB_REQ_SETINTERFACE: - { - if (ctrl->type == USB_REQ_RECIPIENT_INTERFACE) - { - if (priv->config == USBSER_CONFIGID && - index == USBSER_INTERFACEID && - value == USBSER_ALTINTERFACEID) - { - usbclass_resetconfig(priv); - usbclass_setconfig(priv, priv->config); - ret = 0; - } - } - } - break; - - case USB_REQ_GETINTERFACE: - { - if (ctrl->type == (USB_DIR_IN|USB_REQ_RECIPIENT_INTERFACE) && - priv->config == USBSER_CONFIGIDNONE) - { - if (index != USBSER_INTERFACEID) - { - ret = -EDOM; - } - else - { - *(uint8_t*) ctrlreq->buf = USBSER_ALTINTERFACEID; - ret = 1; - } - } - } - break; - - default: - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_UNSUPPORTEDSTDREQ), ctrl->req); - break; - } - } - break; - - /*********************************************************************** - * PL2303 Vendor-Specific Requests - ***********************************************************************/ - - case PL2303_CONTROL_TYPE: - { - if ((ctrl->type & USB_REQ_RECIPIENT_MASK) == USB_REQ_RECIPIENT_INTERFACE) - { - switch (ctrl->req) - { - case PL2303_SETLINEREQUEST: - { - memcpy(priv->linest, ctrlreq->buf, min(len, 7)); - ret = 0; - } - break; - - - case PL2303_GETLINEREQUEST: - { - memcpy(ctrlreq->buf, priv->linest, 7); - ret = 7; - } - break; - - case PL2303_SETCONTROLREQUEST: - case PL2303_BREAKREQUEST: - { - ret = 0; - } - break; - - default: - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_UNSUPPORTEDCLASSREQ), ctrl->type); - break; - } - } - } - break; - - case PL2303_RWREQUEST_TYPE: - { - if ((ctrl->type & USB_REQ_RECIPIENT_MASK) == USB_REQ_RECIPIENT_DEVICE) - { - if (ctrl->req == PL2303_RWREQUEST) - { - if ((ctrl->type & USB_DIR_IN) != 0) - { - *(uint32_t*)ctrlreq->buf = 0xdeadbeef; - ret = 4; - } - else - { - ret = 0; - } - } - else - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_UNSUPPORTEDCLASSREQ), ctrl->type); - } - } - } - break; - - default: - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_UNSUPPORTEDTYPE), ctrl->type); - break; - } - - /* Respond to the setup command if data was returned. On an error return - * value (ret < 0), the USB driver will stall. - */ - - if (ret >= 0) - { - ctrlreq->len = min(len, ret); - ctrlreq->flags = USBDEV_REQFLAGS_NULLPKT; - ret = EP_SUBMIT(dev->ep0, ctrlreq); - if (ret < 0) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_EPRESPQ), (uint16_t)-ret); - ctrlreq->result = OK; - usbclass_ep0incomplete(dev->ep0, ctrlreq); - } - } - - return ret; -} - -/**************************************************************************** - * Name: usbclass_disconnect - * - * Description: - * Invoked after all transfers have been stopped, when the host is - * disconnected. This function is probably called from the context of an - * interrupt handler. - * - ****************************************************************************/ - -static void usbclass_disconnect(FAR struct usbdev_s *dev) -{ - FAR struct usbser_dev_s *priv; - irqstate_t flags; - - usbtrace(TRACE_CLASSDISCONNECT, 0); - -#ifdef CONFIG_DEBUG - if (!dev || !dev->ep0) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_INVALIDARG), 0); - return; - } -#endif - - /* Extract reference to private data */ - - priv = (FAR struct usbser_dev_s *)dev->ep0->priv; - -#ifdef CONFIG_DEBUG - if (!priv) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_EP0NOTBOUND), 0); - return; - } -#endif - - /* Reset the configuration */ - - flags = irqsave(); - usbclass_resetconfig(priv); - - /* Clear out all data in the circular buffer */ - - priv->serdev.xmit.head = 0; - priv->serdev.xmit.tail = 0; - irqrestore(flags); - - /* Perform the soft connect function so that we will we can be - * re-enumerated. - */ - - DEV_CONNECT(dev); -} - -/**************************************************************************** - * Serial Device Methods - ****************************************************************************/ - -/**************************************************************************** - * Name: usbser_setup - * - * Description: - * This method is called the first time that the serial port is opened. - * - ****************************************************************************/ - -static int usbser_setup(FAR struct uart_dev_s *dev) -{ - FAR struct usbser_dev_s *priv; - - usbtrace(USBSER_CLASSAPI_SETUP, 0); - - /* Sanity check */ - -#if CONFIG_DEBUG - if (!dev || !dev->priv) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_INVALIDARG), 0); - return -EIO; - } -#endif - - /* Extract reference to private data */ - - priv = (FAR struct usbser_dev_s*)dev->priv; - - /* Check if we have been configured */ - - if (priv->config == USBSER_CONFIGIDNONE) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_SETUPNOTCONNECTED), 0); - return -ENOTCONN; - } - - return OK; -} - -/**************************************************************************** - * Name: usbser_shutdown - * - * Description: - * This method is called when the serial port is closed. This operation - * is very simple for the USB serial backend because the serial driver - * has already assured that the TX data has full drained -- it calls - * usbser_txempty() until that function returns true before calling this - * function. - * - ****************************************************************************/ - -static void usbser_shutdown(FAR struct uart_dev_s *dev) -{ - usbtrace(USBSER_CLASSAPI_SHUTDOWN, 0); - - /* Sanity check */ - -#if CONFIG_DEBUG - if (!dev || !dev->priv) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_INVALIDARG), 0); - } -#endif -} - -/**************************************************************************** - * Name: usbser_attach - * - * Description: - * Does not apply to the USB serial class device - * - ****************************************************************************/ - -static int usbser_attach(FAR struct uart_dev_s *dev) -{ - usbtrace(USBSER_CLASSAPI_ATTACH, 0); - return OK; -} - -/**************************************************************************** - * Name: usbser_detach - * - * Description: -* Does not apply to the USB serial class device - * - ****************************************************************************/ - -static void usbser_detach(FAR struct uart_dev_s *dev) -{ - usbtrace(USBSER_CLASSAPI_DETACH, 0); -} - -/**************************************************************************** - * Name: usbser_rxint - * - * Description: - * Called by the serial driver to enable or disable RX interrupts. We, of - * course, have no RX interrupts but must behave consistently. This method - * is called under the conditions: - * - * 1. With enable==true when the port is opened (just after usbser_setup - * and usbser_attach are called called) - * 2. With enable==false while transferring data from the RX buffer - * 2. With enable==true while waiting for more incoming data - * 3. With enable==false when the port is closed (just before usbser_detach - * and usbser_shutdown are called). - * - ****************************************************************************/ - -static void usbser_rxint(FAR struct uart_dev_s *dev, bool enable) -{ - FAR struct usbser_dev_s *priv; - FAR uart_dev_t *serdev; - irqstate_t flags; - - usbtrace(USBSER_CLASSAPI_RXINT, (uint16_t)enable); - - /* Sanity check */ - -#if CONFIG_DEBUG - if (!dev || !dev->priv) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_INVALIDARG), 0); - return; - } -#endif - - /* Extract reference to private data */ - - priv = (FAR struct usbser_dev_s*)dev->priv; - serdev = &priv->serdev; - - /* We need exclusive access to the RX buffer and private structure - * in the following. - */ - - flags = irqsave(); - if (enable) - { - /* RX "interrupts" are enabled. Is this a transition from disabled - * to enabled state? - */ - - if (!priv->rxenabled) - { - /* Yes. During the time that RX interrupts are disabled, the - * the serial driver will be extracting data from the circular - * buffer and modifying recv.tail. During this time, we - * should avoid modifying recv.head; When interrupts are restored, - * we can update the head pointer for all of the data that we - * put into cicular buffer while "interrupts" were disabled. - */ - - if (priv->rxhead != serdev->recv.head) - { - serdev->recv.head = priv->rxhead; - - /* Yes... signal the availability of new data */ - - uart_datareceived(serdev); - } - - /* RX "interrupts are no longer disabled */ - - priv->rxenabled = true; - } - } - - /* RX "interrupts" are disabled. Is this a transition from enabled - * to disabled state? - */ - - else if (priv->rxenabled) - { - /* Yes. During the time that RX interrupts are disabled, the - * the serial driver will be extracting data from the circular - * buffer and modifying recv.tail. During this time, we - * should avoid modifying recv.head; When interrupts are disabled, - * we use a shadow index and continue adding data to the circular - * buffer. - */ - - priv->rxhead = serdev->recv.head; - priv->rxenabled = false; - } - irqrestore(flags); -} - -/**************************************************************************** - * Name: usbser_txint - * - * Description: - * Called by the serial driver to enable or disable TX interrupts. We, of - * course, have no TX interrupts but must behave consistently. Initially, - * TX interrupts are disabled. This method is called under the conditions: - * - * 1. With enable==false while transferring data into the TX buffer - * 2. With enable==true when data may be taken from the buffer. - * 3. With enable==false when the TX buffer is empty - * - ****************************************************************************/ - -static void usbser_txint(FAR struct uart_dev_s *dev, bool enable) -{ - FAR struct usbser_dev_s *priv; - - usbtrace(USBSER_CLASSAPI_TXINT, (uint16_t)enable); - - /* Sanity checks */ - -#if CONFIG_DEBUG - if (!dev || !dev->priv) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_INVALIDARG), 0); - return; - } -#endif - - /* Extract references to private data */ - - priv = (FAR struct usbser_dev_s*)dev->priv; - - /* If the new state is enabled and if there is data in the XMIT buffer, - * send the next packet now. - */ - - uvdbg("enable=%d head=%d tail=%d\n", - enable, priv->serdev.xmit.head, priv->serdev.xmit.tail); - - if (enable && priv->serdev.xmit.head != priv->serdev.xmit.tail) - { - usbclass_sndpacket(priv); - } -} - -/**************************************************************************** - * Name: usbser_txempty - * - * Description: - * Return true when all data has been sent. This is called from the - * serial driver when the driver is closed. It will call this API - * periodically until it reports true. NOTE that the serial driver takes all - * responsibility for flushing TX data through the hardware so we can be - * a bit sloppy about that. - * - ****************************************************************************/ - -static bool usbser_txempty(FAR struct uart_dev_s *dev) -{ - FAR struct usbser_dev_s *priv = (FAR struct usbser_dev_s*)dev->priv; - - usbtrace(USBSER_CLASSAPI_TXEMPTY, 0); - -#if CONFIG_DEBUG - if (!priv) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_INVALIDARG), 0); - return true; - } -#endif - - /* When all of the allocated write requests have been returned to the - * reqlist, then there is no longer any TX data in flight. - */ - - return priv->nwrq >= CONFIG_USBSER_NWRREQS; -} - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -/**************************************************************************** - * Name: usbdev_serialinitialize - * - * Description: - * Register USB serial port (and USB serial console if so configured). - * - ****************************************************************************/ - -int usbdev_serialinitialize(int minor) -{ - FAR struct usbser_alloc_s *alloc; - FAR struct usbser_dev_s *priv; - FAR struct usbser_driver_s *drvr; - char devname[16]; - int ret; - - /* Allocate the structures needed */ - - alloc = (FAR struct usbser_alloc_s*)kmalloc(sizeof(struct usbser_alloc_s)); - if (!alloc) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_ALLOCDEVSTRUCT), 0); - return -ENOMEM; - } - - /* Convenience pointers into the allocated blob */ - - priv = &alloc->dev; - drvr = &alloc->drvr; - - /* Initialize the USB serial driver structure */ - - memset(priv, 0, sizeof(struct usbser_dev_s)); - sq_init(&priv->reqlist); - - /* Fake line status */ - - priv->linest[0] = (115200) & 0xff; /* Baud=115200 */ - priv->linest[1] = (115200 >> 8) & 0xff; - priv->linest[2] = (115200 >> 16) & 0xff; - priv->linest[3] = (115200 >> 24) & 0xff; - priv->linest[4] = 0; /* One stop bit */ - priv->linest[5] = 0; /* No parity */ - priv->linest[6] = 8; /*8 data bits */ - - /* Initialize the serial driver sub-structure */ - - priv->serdev.recv.size = CONFIG_USBSER_RXBUFSIZE; - priv->serdev.recv.buffer = priv->rxbuffer; - priv->serdev.xmit.size = CONFIG_USBSER_TXBUFSIZE; - priv->serdev.xmit.buffer = priv->txbuffer; - priv->serdev.ops = &g_uartops; - priv->serdev.priv = priv; - - /* Initialize the USB class driver structure */ - -#ifdef CONFIG_USBDEV_DUALSPEED - drvr->drvr.speed = USB_SPEED_HIGH; -#else - drvr->drvr.speed = USB_SPEED_FULL; -#endif - drvr->drvr.ops = &g_driverops; - drvr->dev = priv; - - /* Register the USB serial class driver */ - - ret = usbdev_register(&drvr->drvr); - if (ret) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_DEVREGISTER), (uint16_t)-ret); - goto errout_with_alloc; - } - - /* Register the USB serial console */ - -#ifdef CONFIG_USBSER_CONSOLE - g_usbserialport.isconsole = true; - ret = uart_register("/dev/console", &pri->serdev); - if (ret < 0) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_CONSOLEREGISTER), (uint16_t)-ret); - goto errout_with_class; - } -#endif - - /* Register the single port supported by this implementation */ - - sprintf(devname, "/dev/ttyUSB%d", minor); - ret = uart_register(devname, &priv->serdev); - if (ret) - { - usbtrace(TRACE_CLSERROR(USBSER_TRACEERR_UARTREGISTER), (uint16_t)-ret); - goto errout_with_class; - } - return OK; - -errout_with_class: - usbdev_unregister(&drvr->drvr); -errout_with_alloc: - kfree(alloc); - return ret; -} diff --git a/nuttx/drivers/usbdev/usbdev_storage.c b/nuttx/drivers/usbdev/usbdev_storage.c deleted file mode 100644 index 360756b12..000000000 --- a/nuttx/drivers/usbdev/usbdev_storage.c +++ /dev/null @@ -1,1651 +0,0 @@ -/**************************************************************************** - * drivers/usbdev/usbdev_storage.c - * - * Copyright (C) 2008-2011 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * Mass storage class device. Bulk-only with SCSI subclass. - * - * References: - * "Universal Serial Bus Mass Storage Class, Specification Overview," - * Revision 1.2, USB Implementer's Forum, June 23, 2003. - * - * "Universal Serial Bus Mass Storage Class, Bulk-Only Transport," - * Revision 1.0, USB Implementer's Forum, September 31, 1999. - * - * "SCSI Primary Commands - 3 (SPC-3)," American National Standard - * for Information Technology, May 4, 2005 - * - * "SCSI Primary Commands - 4 (SPC-4)," American National Standard - * for Information Technology, July 19, 2008 - * - * "SCSI Block Commands -2 (SBC-2)," American National Standard - * for Information Technology, November 13, 2004 - * - * "SCSI Multimedia Commands - 3 (MMC-3)," American National Standard - * for Information Technology, November 12, 2001 - * - * 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 - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#include "usbdev_storage.h" - -/**************************************************************************** - * Pre-processor Definitions - ****************************************************************************/ - -/**************************************************************************** - * Private Types - ****************************************************************************/ - -/* The internal version of the class driver */ - -struct usbstrg_driver_s -{ - struct usbdevclass_driver_s drvr; - FAR struct usbstrg_dev_s *dev; -}; - -/* This is what is allocated */ - -struct usbstrg_alloc_s -{ - struct usbstrg_dev_s dev; - struct usbstrg_driver_s drvr; -}; - -/**************************************************************************** - * Private Function Prototypes - ****************************************************************************/ - -/* Class Driver Support *****************************************************/ - -static void usbstrg_ep0incomplete(FAR struct usbdev_ep_s *ep, - FAR struct usbdev_req_s *req); -static struct usbdev_req_s *usbstrg_allocreq(FAR struct usbdev_ep_s *ep, - uint16_t len); -static void usbstrg_freereq(FAR struct usbdev_ep_s *ep, - FAR struct usbdev_req_s *req); - -/* Class Driver Operations (most at interrupt level) ************************/ - -static int usbstrg_bind(FAR struct usbdev_s *dev, - FAR struct usbdevclass_driver_s *driver); -static void usbstrg_unbind(FAR struct usbdev_s *dev); -static int usbstrg_setup(FAR struct usbdev_s *dev, - FAR const struct usb_ctrlreq_s *ctrl); -static void usbstrg_disconnect(FAR struct usbdev_s *dev); - -/* Initialization/Uninitialization ******************************************/ - -static void usbstrg_lununinitialize(struct usbstrg_lun_s *lun); - -/**************************************************************************** - * Private Data - ****************************************************************************/ - -/* Driver operations ********************************************************/ - -static struct usbdevclass_driverops_s g_driverops = -{ - usbstrg_bind, /* bind */ - usbstrg_unbind, /* unbind */ - usbstrg_setup, /* setup */ - usbstrg_disconnect, /* disconnect */ - NULL, /* suspend */ - NULL /* resume */ -}; - -/**************************************************************************** - * Public Data - ****************************************************************************/ - -/**************************************************************************** - * Private Functions - ****************************************************************************/ - -/**************************************************************************** - * Class Driver Support - ****************************************************************************/ -/**************************************************************************** - * Name: usbstrg_ep0incomplete - * - * Description: - * Handle completion of EP0 control operations - * - ****************************************************************************/ - -static void usbstrg_ep0incomplete(FAR struct usbdev_ep_s *ep, - FAR struct usbdev_req_s *req) -{ - if (req->result || req->xfrd != req->len) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_REQRESULT), - (uint16_t)-req->result); - } -} - -/**************************************************************************** - * Name: usbstrg_allocreq - * - * Description: - * Allocate a request instance along with its buffer - * - ****************************************************************************/ - -static struct usbdev_req_s *usbstrg_allocreq(FAR struct usbdev_ep_s *ep, - uint16_t len) -{ - FAR struct usbdev_req_s *req; - - req = EP_ALLOCREQ(ep); - if (req != NULL) - { - req->len = len; - req->buf = EP_ALLOCBUFFER(ep, len); - if (!req->buf) - { - EP_FREEREQ(ep, req); - req = NULL; - } - } - return req; -} - -/**************************************************************************** - * Name: usbstrg_freereq - * - * Description: - * Free a request instance along with its buffer - * - ****************************************************************************/ - -static void usbstrg_freereq(FAR struct usbdev_ep_s *ep, struct usbdev_req_s *req) -{ - if (ep != NULL && req != NULL) - { - if (req->buf != NULL) - { - EP_FREEBUFFER(ep, req->buf); - } - EP_FREEREQ(ep, req); - } -} - -/**************************************************************************** - * Class Driver Interfaces - ****************************************************************************/ -/**************************************************************************** - * Name: usbstrg_bind - * - * Description: - * Invoked when the driver is bound to a USB device driver - * - ****************************************************************************/ - -static int usbstrg_bind(FAR struct usbdev_s *dev, FAR struct usbdevclass_driver_s *driver) -{ - FAR struct usbstrg_dev_s *priv = ((struct usbstrg_driver_s*)driver)->dev; - FAR struct usbstrg_req_s *reqcontainer; - irqstate_t flags; - int ret = OK; - int i; - - usbtrace(TRACE_CLASSBIND, 0); - - /* Bind the structures */ - - priv->usbdev = dev; - dev->ep0->priv = priv; - - /* The configured EP0 size should match the reported EP0 size. We could - * easily adapt to the reported EP0 size, but then we could not use the - * const, canned descriptors. - */ - - DEBUGASSERT(CONFIG_USBSTRG_EP0MAXPACKET == dev->ep0->maxpacket); - - /* Preallocate control request */ - - priv->ctrlreq = usbstrg_allocreq(dev->ep0, USBSTRG_MXDESCLEN); - if (priv->ctrlreq == NULL) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_ALLOCCTRLREQ), 0); - ret = -ENOMEM; - goto errout; - } - priv->ctrlreq->callback = usbstrg_ep0incomplete; - - /* Pre-allocate all endpoints... the endpoints will not be functional - * until the SET CONFIGURATION request is processed in usbstrg_setconfig. - * This is done here because there may be calls to kmalloc and the SET - * CONFIGURATION processing probably occurrs within interrupt handling - * logic where kmalloc calls will fail. - */ - - /* Pre-allocate the IN bulk endpoint */ - - priv->epbulkin = DEV_ALLOCEP(dev, USBSTRG_EPINBULK_ADDR, true, USB_EP_ATTR_XFER_BULK); - if (!priv->epbulkin) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_EPBULKINALLOCFAIL), 0); - ret = -ENODEV; - goto errout; - } - priv->epbulkin->priv = priv; - - /* Pre-allocate the OUT bulk endpoint */ - - priv->epbulkout = DEV_ALLOCEP(dev, USBSTRG_EPOUTBULK_ADDR, false, USB_EP_ATTR_XFER_BULK); - if (!priv->epbulkout) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_EPBULKOUTALLOCFAIL), 0); - ret = -ENODEV; - goto errout; - } - priv->epbulkout->priv = priv; - - /* Pre-allocate read requests */ - - for (i = 0; i < CONFIG_USBSTRG_NRDREQS; i++) - { - reqcontainer = &priv->rdreqs[i]; - reqcontainer->req = usbstrg_allocreq(priv->epbulkout, CONFIG_USBSTRG_BULKOUTREQLEN); - if (reqcontainer->req == NULL) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_RDALLOCREQ), - (uint16_t)-ret); - ret = -ENOMEM; - goto errout; - } - reqcontainer->req->priv = reqcontainer; - reqcontainer->req->callback = usbstrg_rdcomplete; - } - - /* Pre-allocate write request containers and put in a free list */ - - for (i = 0; i < CONFIG_USBSTRG_NWRREQS; i++) - { - reqcontainer = &priv->wrreqs[i]; - reqcontainer->req = usbstrg_allocreq(priv->epbulkin, CONFIG_USBSTRG_BULKINREQLEN); - if (reqcontainer->req == NULL) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_WRALLOCREQ), - (uint16_t)-ret); - ret = -ENOMEM; - goto errout; - } - reqcontainer->req->priv = reqcontainer; - reqcontainer->req->callback = usbstrg_wrcomplete; - - flags = irqsave(); - sq_addlast((sq_entry_t*)reqcontainer, &priv->wrreqlist); - irqrestore(flags); - } - - /* Report if we are selfpowered */ - -#ifdef CONFIG_USBDEV_SELFPOWERED - DEV_SETSELFPOWERED(dev); -#endif - - /* And pull-up the data line for the soft connect function */ - - DEV_CONNECT(dev); - return OK; - -errout: - usbstrg_unbind(dev); - return ret; -} - -/**************************************************************************** - * Name: usbstrg_unbind - * - * Description: - * Invoked when the driver is unbound from a USB device driver - * - ****************************************************************************/ - -static void usbstrg_unbind(FAR struct usbdev_s *dev) -{ - FAR struct usbstrg_dev_s *priv; - FAR struct usbstrg_req_s *reqcontainer; - irqstate_t flags; - int i; - - usbtrace(TRACE_CLASSUNBIND, 0); - -#ifdef CONFIG_DEBUG - if (!dev || !dev->ep0) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_UNBINDINVALIDARGS), 0); - return; - } -#endif - - /* Extract reference to private data */ - - priv = (FAR struct usbstrg_dev_s *)dev->ep0->priv; - -#ifdef CONFIG_DEBUG - if (!priv) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_EP0NOTBOUND1), 0); - return; - } -#endif - - /* The worker thread should have already been stopped by the - * driver un-initialize logic. - */ - - DEBUGASSERT(priv->thstate == USBSTRG_STATE_TERMINATED); - - /* Make sure that we are not already unbound */ - - if (priv != NULL) - { - /* Make sure that the endpoints have been unconfigured. If - * we were terminated gracefully, then the configuration should - * already have been reset. If not, then calling usbstrg_resetconfig - * should cause the endpoints to immediately terminate all - * transfers and return the requests to us (with result == -ESHUTDOWN) - */ - - usbstrg_resetconfig(priv); - up_mdelay(50); - - /* Free the pre-allocated control request */ - - if (priv->ctrlreq != NULL) - { - usbstrg_freereq(dev->ep0, priv->ctrlreq); - priv->ctrlreq = NULL; - } - - /* Free pre-allocated read requests (which should all have - * been returned to the free list at this time -- we don't check) - */ - - for (i = 0; i < CONFIG_USBSTRG_NRDREQS; i++) - { - reqcontainer = &priv->rdreqs[i]; - if (reqcontainer->req) - { - usbstrg_freereq(priv->epbulkout, reqcontainer->req); - reqcontainer->req = NULL; - } - } - - /* Free the bulk OUT endpoint */ - - if (priv->epbulkout) - { - DEV_FREEEP(dev, priv->epbulkout); - priv->epbulkout = NULL; - } - - /* Free write requests that are not in use (which should be all - * of them - */ - - flags = irqsave(); - while (!sq_empty(&priv->wrreqlist)) - { - reqcontainer = (struct usbstrg_req_s *)sq_remfirst(&priv->wrreqlist); - if (reqcontainer->req != NULL) - { - usbstrg_freereq(priv->epbulkin, reqcontainer->req); - } - } - - /* Free the bulk IN endpoint */ - - if (priv->epbulkin) - { - DEV_FREEEP(dev, priv->epbulkin); - priv->epbulkin = NULL; - } - - irqrestore(flags); - } -} - -/**************************************************************************** - * Name: usbstrg_setup - * - * Description: - * Invoked for ep0 control requests. This function probably executes - * in the context of an interrupt handler. - * - ****************************************************************************/ - -static int usbstrg_setup(FAR struct usbdev_s *dev, - FAR const struct usb_ctrlreq_s *ctrl) -{ - FAR struct usbstrg_dev_s *priv; - FAR struct usbdev_req_s *ctrlreq; - uint16_t value; - uint16_t index; - uint16_t len; - int ret = -EOPNOTSUPP; - -#ifdef CONFIG_DEBUG - if (!dev || !dev->ep0 || !ctrl) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_SETUPINVALIDARGS), 0); - return -EIO; - } -#endif - - /* Extract reference to private data */ - - usbtrace(TRACE_CLASSSETUP, ctrl->req); - priv = (FAR struct usbstrg_dev_s *)dev->ep0->priv; - -#ifdef CONFIG_DEBUG - if (!priv || !priv->ctrlreq) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_EP0NOTBOUND2), 0); - return -ENODEV; - } -#endif - ctrlreq = priv->ctrlreq; - - /* Extract the little-endian 16-bit values to host order */ - - value = GETUINT16(ctrl->value); - index = GETUINT16(ctrl->index); - len = GETUINT16(ctrl->len); - - uvdbg("type=%02x req=%02x value=%04x index=%04x len=%04x\n", - ctrl->type, ctrl->req, value, index, len); - - if ((ctrl->type & USB_REQ_TYPE_MASK) == USB_REQ_TYPE_STANDARD) - { - /********************************************************************** - * Standard Requests - **********************************************************************/ - - switch (ctrl->req) - { - case USB_REQ_GETDESCRIPTOR: - { - /* The value field specifies the descriptor type in the MS byte and the - * descriptor index in the LS byte (order is little endian) - */ - - switch (ctrl->value[1]) - { - case USB_DESC_TYPE_DEVICE: - { - ret = USB_SIZEOF_DEVDESC; - memcpy(ctrlreq->buf, usbstrg_getdevdesc(), ret); - } - break; - -#ifdef CONFIG_USBDEV_DUALSPEED - case USB_DESC_TYPE_DEVICEQUALIFIER: - { - ret = USB_SIZEOF_QUALDESC; - memcpy(ctrlreq->buf, usbstrg_getqualdesc(), ret); - } - break; - - case USB_DESC_TYPE_OTHERSPEEDCONFIG: -#endif /* CONFIG_USBDEV_DUALSPEED */ - - case USB_DESC_TYPE_CONFIG: - { -#ifdef CONFIG_USBDEV_DUALSPEED - ret = usbstrg_mkcfgdesc(ctrlreq->buf, dev->speed, ctrl->value[1]); -#else - ret = usbstrg_mkcfgdesc(ctrlreq->buf); -#endif - } - break; - - case USB_DESC_TYPE_STRING: - { - /* index == language code. */ - - ret = usbstrg_mkstrdesc(ctrl->value[0], (struct usb_strdesc_s *)ctrlreq->buf); - } - break; - - default: - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_GETUNKNOWNDESC), value); - } - break; - } - } - break; - - case USB_REQ_SETCONFIGURATION: - { - if (ctrl->type == 0) - { - /* Signal the worker thread to instantiate the new configuration */ - - priv->theventset |= USBSTRG_EVENT_CFGCHANGE; - priv->thvalue = value; - pthread_cond_signal(&priv->cond); - - /* Return here... the response will be provided later by the - * worker thread. - */ - - return OK; - } - } - break; - - case USB_REQ_GETCONFIGURATION: - { - if (ctrl->type == USB_DIR_IN) - { - ctrlreq->buf[0] = priv->config; - ret = 1; - } - } - break; - - case USB_REQ_SETINTERFACE: - { - if (ctrl->type == USB_REQ_RECIPIENT_INTERFACE) - { - if (priv->config == USBSTRG_CONFIGID && - index == USBSTRG_INTERFACEID && - value == USBSTRG_ALTINTERFACEID) - { - /* Signal to instantiate the interface change */ - - priv->theventset |= USBSTRG_EVENT_IFCHANGE; - pthread_cond_signal(&priv->cond); - - /* Return here... the response will be provided later by the - * worker thread. - */ - - return OK; - } - } - } - break; - - case USB_REQ_GETINTERFACE: - { - if (ctrl->type == (USB_DIR_IN|USB_REQ_RECIPIENT_INTERFACE) && - priv->config == USBSTRG_CONFIGIDNONE) - { - if (index != USBSTRG_INTERFACEID) - { - ret = -EDOM; - } - else - { - ctrlreq->buf[0] = USBSTRG_ALTINTERFACEID; - ret = 1; - } - } - } - break; - - default: - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_UNSUPPORTEDSTDREQ), ctrl->req); - break; - } - } - else - { - /********************************************************************** - * Bulk-Only Mass Storage Class Requests - **********************************************************************/ - - /* Verify that we are configured */ - - if (!priv->config) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_NOTCONFIGURED), 0); - return ret; - } - - switch (ctrl->req) - { - case USBSTRG_REQ_MSRESET: /* Reset mass storage device and interface */ - { - if (ctrl->type == USBSTRG_TYPE_SETUPOUT && value == 0 && len == 0) - { - /* Only one interface is supported */ - - if (index != USBSTRG_CONFIGID) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_MSRESETNDX), index); - ret = -EDOM; - } - else - { - /* Signal to stop the current operation and reinitialize state */ - - priv->theventset |= USBSTRG_EVENT_RESET; - pthread_cond_signal(&priv->cond); - - /* Return here... the response will be provided later by the - * worker thread. - */ - - return OK; - } - } - } - break; - - case USBSTRG_REQ_GETMAXLUN: /* Return number LUNs supported */ - { - if (ctrl->type == USBSTRG_TYPE_SETUPIN && value == 0) - { - /* Only one interface is supported */ - - if (index != USBSTRG_INTERFACEID) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_GETMAXLUNNDX), index); - ret = -EDOM; - } - else - { - ctrlreq->buf[0] = priv->nluns - 1; - ret = 1; - } - } - } - break; - - default: - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_BADREQUEST), index); - break; - } - } - - /* Respond to the setup command if data was returned. On an error return - * value (ret < 0), the USB driver will stall EP0. - */ - - if (ret >= 0) - { - ctrlreq->len = min(len, ret); - ctrlreq->flags = USBDEV_REQFLAGS_NULLPKT; - ret = EP_SUBMIT(dev->ep0, ctrlreq); - if (ret < 0) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_EPRESPQ), (uint16_t)-ret); -#if 0 /* Not necessary */ - ctrlreq->result = OK; - usbstrg_ep0incomplete(dev->ep0, ctrlreq); -#endif - } - } - - return ret; -} - -/**************************************************************************** - * Name: usbstrg_disconnect - * - * Description: - * Invoked after all transfers have been stopped, when the host is - * disconnected. This function is probably called from the context of an - * interrupt handler. - * - ****************************************************************************/ - -static void usbstrg_disconnect(FAR struct usbdev_s *dev) -{ - struct usbstrg_dev_s *priv; - irqstate_t flags; - - usbtrace(TRACE_CLASSDISCONNECT, 0); - -#ifdef CONFIG_DEBUG - if (!dev || !dev->ep0) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_DISCONNECTINVALIDARGS), 0); - return; - } -#endif - - /* Extract reference to private data */ - - priv = (FAR struct usbstrg_dev_s *)dev->ep0->priv; - -#ifdef CONFIG_DEBUG - if (!priv) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_EP0NOTBOUND3), 0); - return; - } -#endif - - /* Reset the configuration */ - - flags = irqsave(); - usbstrg_resetconfig(priv); - - /* Signal the worker thread */ - - priv->theventset |= USBSTRG_EVENT_DISCONNECT; - pthread_cond_signal(&priv->cond); - irqrestore(flags); - - /* Perform the soft connect function so that we will we can be - * re-enumerated. - */ - - DEV_CONNECT(dev); -} - -/**************************************************************************** - * Initialization/Un-Initialization - ****************************************************************************/ -/**************************************************************************** - * Name: usbstrg_lununinitialize - ****************************************************************************/ - -static void usbstrg_lununinitialize(struct usbstrg_lun_s *lun) -{ - /* Has a block driver has been bound to the LUN? */ - - if (lun->inode) - { - /* Close the block driver */ - - (void)close_blockdriver(lun->inode); - } - - memset(lun, 0, sizeof(struct usbstrg_lun_s *)); -} - -/**************************************************************************** - * Public Functions - ****************************************************************************/ -/**************************************************************************** - * Internal Interfaces - ****************************************************************************/ - -/**************************************************************************** - * Name: usbstrg_setconfig - * - * Description: - * Set the device configuration by allocating and configuring endpoints and - * by allocating and queuing read and write requests. - * - ****************************************************************************/ - -int usbstrg_setconfig(FAR struct usbstrg_dev_s *priv, uint8_t config) -{ - FAR struct usbstrg_req_s *privreq; - FAR struct usbdev_req_s *req; -#ifdef CONFIG_USBDEV_DUALSPEED - FAR const struct usb_epdesc_s *epdesc; - bool hispeed = (priv->usbdev->speed == USB_SPEED_HIGH); - uint16_t bulkmxpacket; -#endif - int i; - int ret = 0; - -#if CONFIG_DEBUG - if (priv == NULL) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_SETCONFIGINVALIDARGS), 0); - return -EIO; - } -#endif - - if (config == priv->config) - { - /* Already configured -- Do nothing */ - - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_ALREADYCONFIGURED), 0); - return OK; - } - - /* Discard the previous configuration data */ - - usbstrg_resetconfig(priv); - - /* Was this a request to simply discard the current configuration? */ - - if (config == USBSTRG_CONFIGIDNONE) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_CONFIGNONE), 0); - return OK; - } - - /* We only accept one configuration */ - - if (config != USBSTRG_CONFIGID) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_CONFIGIDBAD), 0); - return -EINVAL; - } - - /* Configure the IN bulk endpoint */ - -#ifdef CONFIG_USBDEV_DUALSPEED - bulkmxpacket = USBSTRG_BULKMAXPACKET(hispeed); - epdesc = USBSTRG_EPBULKINDESC(hispeed); - ret = EP_CONFIGURE(priv->epbulkin, epdesc, false); -#else - ret = EP_CONFIGURE(priv->epbulkin, - usbstrg_getepdesc(USBSTRG_EPFSBULKIN), false); -#endif - if (ret < 0) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_EPBULKINCONFIGFAIL), 0); - goto errout; - } - - priv->epbulkin->priv = priv; - - /* Configure the OUT bulk endpoint */ - -#ifdef CONFIG_USBDEV_DUALSPEED - epdesc = USBSTRG_EPBULKOUTDESC(hispeed); - ret = EP_CONFIGURE(priv->epbulkout, epdesc, true); -#else - ret = EP_CONFIGURE(priv->epbulkout, - usbstrg_getepdesc(USBSTRG_EPFSBULKOUT), true); -#endif - if (ret < 0) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_EPBULKOUTCONFIGFAIL), 0); - goto errout; - } - - priv->epbulkout->priv = priv; - - /* Queue read requests in the bulk OUT endpoint */ - - for (i = 0; i < CONFIG_USBSTRG_NRDREQS; i++) - { - privreq = &priv->rdreqs[i]; - req = privreq->req; - req->len = CONFIG_USBSTRG_BULKOUTREQLEN; - req->priv = privreq; - req->callback = usbstrg_rdcomplete; - ret = EP_SUBMIT(priv->epbulkout, req); - if (ret < 0) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_RDSUBMIT), (uint16_t)-ret); - goto errout; - } - } - - priv->config = config; - return OK; - -errout: - usbstrg_resetconfig(priv); - return ret; -} - -/**************************************************************************** - * Name: usbstrg_resetconfig - * - * Description: - * Mark the device as not configured and disable all endpoints. - * - ****************************************************************************/ - -void usbstrg_resetconfig(FAR struct usbstrg_dev_s *priv) -{ - /* Are we configured? */ - - if (priv->config != USBSTRG_CONFIGIDNONE) - { - /* Yes.. but not anymore */ - - priv->config = USBSTRG_CONFIGIDNONE; - - /* Disable endpoints. This should force completion of all pending - * transfers. - */ - - EP_DISABLE(priv->epbulkin); - EP_DISABLE(priv->epbulkout); - } -} - -/**************************************************************************** - * Name: usbstrg_wrcomplete - * - * Description: - * Handle completion of write request. This function probably executes - * in the context of an interrupt handler. - * - ****************************************************************************/ - -void usbstrg_wrcomplete(FAR struct usbdev_ep_s *ep, FAR struct usbdev_req_s *req) -{ - FAR struct usbstrg_dev_s *priv; - FAR struct usbstrg_req_s *privreq; - irqstate_t flags; - - /* Sanity check */ - -#ifdef CONFIG_DEBUG - if (!ep || !ep->priv || !req || !req->priv) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_WRCOMPLETEINVALIDARGS), 0); - return; - } -#endif - - /* Extract references to private data */ - - priv = (FAR struct usbstrg_dev_s*)ep->priv; - privreq = (FAR struct usbstrg_req_s *)req->priv; - - /* Return the write request to the free list */ - - flags = irqsave(); - sq_addlast((sq_entry_t*)privreq, &priv->wrreqlist); - irqrestore(flags); - - /* Process the received data unless this is some unusual condition */ - - switch (req->result) - { - case OK: /* Normal completion */ - usbtrace(TRACE_CLASSWRCOMPLETE, req->xfrd); - break; - - case -ESHUTDOWN: /* Disconnection */ - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_WRSHUTDOWN), 0); - break; - - default: /* Some other error occurred */ - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_WRUNEXPECTED), - (uint16_t)-req->result); - break; - }; - - /* Inform the worker thread that a write request has been returned */ - - priv->theventset |= USBSTRG_EVENT_WRCOMPLETE; - pthread_cond_signal(&priv->cond); -} - -/**************************************************************************** - * Name: usbstrg_rdcomplete - * - * Description: - * Handle completion of read request on the bulk OUT endpoint. This - * is handled like the receipt of serial data on the "UART" - * - ****************************************************************************/ - -void usbstrg_rdcomplete(FAR struct usbdev_ep_s *ep, FAR struct usbdev_req_s *req) -{ - FAR struct usbstrg_dev_s *priv; - FAR struct usbstrg_req_s *privreq; - irqstate_t flags; - int ret; - - /* Sanity check */ - -#ifdef CONFIG_DEBUG - if (!ep || !ep->priv || !req || !req->priv) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_RDCOMPLETEINVALIDARGS), 0); - return; - } -#endif - - /* Extract references to private data */ - - priv = (FAR struct usbstrg_dev_s*)ep->priv; - privreq = (FAR struct usbstrg_req_s *)req->priv; - - /* Process the received data unless this is some unusual condition */ - - switch (req->result) - { - case 0: /* Normal completion */ - { - usbtrace(TRACE_CLASSRDCOMPLETE, req->xfrd); - - /* Add the filled read request from the rdreqlist */ - - flags = irqsave(); - sq_addlast((sq_entry_t*)privreq, &priv->rdreqlist); - irqrestore(flags); - - /* Signal the worker thread that there is received data to be processed */ - - priv->theventset |= USBSTRG_EVENT_RDCOMPLETE; - pthread_cond_signal(&priv->cond); - } - break; - - case -ESHUTDOWN: /* Disconnection */ - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_RDSHUTDOWN), 0); - - /* Drop the read request... it will be cleaned up later */ - } - break; - - default: /* Some other error occurred */ - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_RDUNEXPECTED), - (uint16_t)-req->result); - - /* Return the read request to the bulk out endpoint for re-filling */ - - req = privreq->req; - req->priv = privreq; - req->callback = usbstrg_rdcomplete; - - ret = EP_SUBMIT(priv->epbulkout, req); - if (ret != OK) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_RDCOMPLETERDSUBMIT), - (uint16_t)-ret); - } - } - break; - } -} - -/**************************************************************************** - * Name: usbstrg_deferredresponse - * - * Description: - * Some EP0 setup request cannot be responded to immediately becuase they - * require some asynchronous action from the SCSI worker thread. This - * function is provided for the SCSI thread to make that deferred response. - * The specific requests that require this deferred response are: - * - * 1. USB_REQ_SETCONFIGURATION, - * 2. USB_REQ_SETINTERFACE, or - * 3. USBSTRG_REQ_MSRESET - * - * In all cases, the success reponse is a zero-length packet; the failure - * response is an EP0 stall. - * - * Input parameters: - * priv - Private state structure for this USB storage instance - * stall - true is the action failed and a stall is required - * - ****************************************************************************/ - -void usbstrg_deferredresponse(FAR struct usbstrg_dev_s *priv, bool failed) -{ - FAR struct usbdev_s *dev; - FAR struct usbdev_req_s *ctrlreq; - int ret; - -#ifdef CONFIG_DEBUG - if (!priv || !priv->usbdev || !priv->ctrlreq) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_DEFERREDRESPINVALIDARGS), 0); - return; - } -#endif - - dev = priv->usbdev; - ctrlreq = priv->ctrlreq; - - /* If no error occurs, respond to the deferred setup command with a null - * packet. - */ - - if (!failed) - { - ctrlreq->len = 0; - ctrlreq->flags = USBDEV_REQFLAGS_NULLPKT; - ret = EP_SUBMIT(dev->ep0, ctrlreq); - if (ret < 0) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_DEFERREDRESPSUBMIT), - (uint16_t)-ret); -#if 0 /* Not necessary */ - ctrlreq->result = OK; - usbstrg_ep0incomplete(dev->ep0, ctrlreq); -#endif - } - } - else - { - /* On a failure, the USB driver will stall. */ - - usbtrace(TRACE_DEVERROR(USBSTRG_TRACEERR_DEFERREDRESPSTALLED), 0); - EP_STALL(dev->ep0); - } -} - -/**************************************************************************** - * User Interfaces - ****************************************************************************/ -/**************************************************************************** - * Name: usbstrg_configure - * - * Description: - * One-time initialization of the USB storage driver. The initialization - * sequence is as follows: - * - * 1. Call usbstrg_configure to perform one-time initialization specifying - * the number of luns. - * 2. Call usbstrg_bindlun to configure each supported LUN - * 3. Call usbstrg_exportluns when all LUNs are configured - * - * Input Parameters: - * nluns - the number of LUNs that will be registered - * handle - Location to return a handle that is used in other API calls. - * - * Returned Value: - * 0 on success; a negated errno on failure - * - ****************************************************************************/ - -int usbstrg_configure(unsigned int nluns, void **handle) -{ - FAR struct usbstrg_alloc_s *alloc; - FAR struct usbstrg_dev_s *priv; - FAR struct usbstrg_driver_s *drvr; - int ret; - -#ifdef CONFIG_DEBUG - if (nluns > 15) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_TOOMANYLUNS), 0); - return -EDOM; - } -#endif - - /* Allocate the structures needed */ - - alloc = (FAR struct usbstrg_alloc_s*)kmalloc(sizeof(struct usbstrg_alloc_s)); - if (!alloc) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_ALLOCDEVSTRUCT), 0); - return -ENOMEM; - } - - /* Initialize the USB storage driver structure */ - - priv = &alloc->dev; - memset(priv, 0, sizeof(struct usbstrg_dev_s)); - - pthread_mutex_init(&priv->mutex, NULL); - pthread_cond_init(&priv->cond, NULL); - sq_init(&priv->wrreqlist); - - priv->nluns = nluns; - - /* Allocate the LUN table */ - - priv->luntab = (struct usbstrg_lun_s*)kmalloc(priv->nluns*sizeof(struct usbstrg_lun_s)); - if (!priv->luntab) - { - ret = -ENOMEM; - goto errout; - } - memset(priv->luntab, 0, priv->nluns * sizeof(struct usbstrg_lun_s)); - - /* Initialize the USB class driver structure */ - - drvr = &alloc->drvr; -#ifdef CONFIG_USBDEV_DUALSPEED - drvr->drvr.speed = USB_SPEED_HIGH; -#else - drvr->drvr.speed = USB_SPEED_FULL; -#endif - drvr->drvr.ops = &g_driverops; - drvr->dev = priv; - - /* Return the handle and success */ - - *handle = (FAR void*)alloc; - return OK; - -errout: - usbstrg_uninitialize(alloc); - return ret; -} - -/**************************************************************************** - * Name: usbstrg_bindlun - * - * Description: - * Bind the block driver specified by drvrpath to a USB storage LUN. - * - * Input Parameters: - * handle - The handle returned by a previous call to usbstrg_configure(). - * drvrpath - the full path to the block driver - * startsector - A sector offset into the block driver to the start of the - * partition on drvrpath (0 if no partitions) - * nsectors - The number of sectors in the partition (if 0, all sectors - * to the end of the media will be exported). - * lunno - the LUN to bind to - * - * Returned Value: - * 0 on success; a negated errno on failure. - * - ****************************************************************************/ - -int usbstrg_bindlun(FAR void *handle, FAR const char *drvrpath, - unsigned int lunno, off_t startsector, size_t nsectors, - bool readonly) -{ - FAR struct usbstrg_alloc_s *alloc = (FAR struct usbstrg_alloc_s *)handle; - FAR struct usbstrg_dev_s *priv; - FAR struct usbstrg_lun_s *lun; - FAR struct inode *inode; - struct geometry geo; - int ret; - -#ifdef CONFIG_DEBUG - if (!alloc || !drvrpath || startsector < 0 || nsectors < 0) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_BINLUNINVALIDARGS1), 0); - return -EINVAL; - } -#endif - - priv = &alloc->dev; - -#ifdef CONFIG_DEBUG - if (!priv->luntab) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_INTERNALCONFUSION1), 0); - return -EIO; - } - - if (lunno > priv->nluns) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_BINDLUNINVALIDARGS2), 0); - return -EINVAL; - } -#endif - - lun = &priv->luntab[lunno]; - -#ifdef CONFIG_DEBUG - if (lun->inode != NULL) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_LUNALREADYBOUND), 0); - return -EBUSY; - } -#endif - - /* Open the block driver */ - - ret = open_blockdriver(drvrpath, 0, &inode); - if (ret < 0) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_BLKDRVEOPEN), 0); - return ret; - } - - /* Get the drive geometry */ - - if (!inode || !inode->u.i_bops || !inode->u.i_bops->geometry || - inode->u.i_bops->geometry(inode, &geo) != OK || !geo.geo_available) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_NOGEOMETRY), 0); - return -ENODEV; - } - - /* Verify that the partition parameters are valid */ - - if (startsector >= geo.geo_nsectors) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_BINDLUNINVALIDARGS3), 0); - return -EDOM; - } - else if (nsectors == 0) - { - nsectors = geo.geo_nsectors - startsector; - } - else if (startsector + nsectors >= geo.geo_nsectors) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_BINDLUNINVALIDARGS4), 0); - return -EDOM; - } - - /* Initialize the LUN structure */ - - memset(lun, 0, sizeof(struct usbstrg_lun_s *)); - - /* Allocate an I/O buffer big enough to hold one hardware sector. SCSI commands - * are processed one at a time so all LUNs may share a single I/O buffer. The - * I/O buffer will be allocated so that is it as large as the largest block - * device sector size - */ - - if (!priv->iobuffer) - { - priv->iobuffer = (uint8_t*)kmalloc(geo.geo_sectorsize); - if (!priv->iobuffer) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_ALLOCIOBUFFER), geo.geo_sectorsize); - return -ENOMEM; - } - priv->iosize = geo.geo_sectorsize; - } - else if (priv->iosize < geo.geo_sectorsize) - { - void *tmp; - tmp = (uint8_t*)realloc(priv->iobuffer, geo.geo_sectorsize); - if (!tmp) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_REALLOCIOBUFFER), geo.geo_sectorsize); - return -ENOMEM; - } - - priv->iobuffer = (uint8_t*)tmp; - priv->iosize = geo.geo_sectorsize; - } - - lun->inode = inode; - lun->startsector = startsector; - lun->nsectors = nsectors; - lun->sectorsize = geo.geo_sectorsize; - - /* If the driver does not support the write method, then this is read-only */ - - if (!inode->u.i_bops->write) - { - lun->readonly = true; - } - return OK; -} - -/**************************************************************************** - * Name: usbstrg_unbindlun - * - * Description: - * Un-bind the block driver for the specified LUN - * - * Input Parameters: - * handle - The handle returned by a previous call to usbstrg_configure(). - * lun - the LUN to unbind from - * - * Returned Value: - * 0 on success; a negated errno on failure. - * - ****************************************************************************/ - -int usbstrg_unbindlun(FAR void *handle, unsigned int lunno) -{ - FAR struct usbstrg_alloc_s *alloc = (FAR struct usbstrg_alloc_s *)handle; - FAR struct usbstrg_dev_s *priv; - FAR struct usbstrg_lun_s *lun; - int ret; - -#ifdef CONFIG_DEBUG - if (!alloc) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_UNBINDLUNINVALIDARGS1), 0); - return -EINVAL; - } -#endif - - priv = &alloc->dev; - -#ifdef CONFIG_DEBUG - if (!priv->luntab) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_INTERNALCONFUSION2), 0); - return -EIO; - } - - if (lunno > priv->nluns) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_UNBINDLUNINVALIDARGS2), 0); - return -EINVAL; - } -#endif - - lun = &priv->luntab[lunno]; - pthread_mutex_lock(&priv->mutex); - -#ifdef CONFIG_DEBUG - if (lun->inode == NULL) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_LUNNOTBOUND), 0); - ret = -EBUSY; - } - else -#endif - { - /* Close the block driver */ - - usbstrg_lununinitialize(lun); - ret = OK; - } - - pthread_mutex_unlock(&priv->mutex); - return ret; -} - -/**************************************************************************** - * Name: usbstrg_exportluns - * - * Description: - * After all of the LUNs have been bound, this function may be called - * in order to export those LUNs in the USB storage device. - * - * Input Parameters: - * handle - The handle returned by a previous call to usbstrg_configure(). - * - * Returned Value: - * 0 on success; a negated errno on failure - * - ****************************************************************************/ - -int usbstrg_exportluns(FAR void *handle) -{ - FAR struct usbstrg_alloc_s *alloc = (FAR struct usbstrg_alloc_s *)handle; - FAR struct usbstrg_dev_s *priv; - FAR struct usbstrg_driver_s *drvr; - irqstate_t flags; -#ifdef SDCC - pthread_attr_t attr; -#endif - int ret; - -#ifdef CONFIG_DEBUG - if (!alloc) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_EXPORTLUNSINVALIDARGS), 0); - return -ENXIO; - } -#endif - - priv = &alloc->dev; - drvr = &alloc->drvr; - - /* Start the worker thread */ - - pthread_mutex_lock(&priv->mutex); - priv->thstate = USBSTRG_STATE_NOTSTARTED; - priv->theventset = USBSTRG_EVENT_NOEVENTS; - -#ifdef SDCC - (void)pthread_attr_init(&attr); - ret = pthread_create(&priv->thread, &attr, usbstrg_workerthread, (pthread_addr_t)priv); -#else - ret = pthread_create(&priv->thread, NULL, usbstrg_workerthread, (pthread_addr_t)priv); -#endif - if (ret != OK) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_THREADCREATE), (uint16_t)-ret); - goto errout_with_mutex; - } - - /* Register the USB storage class driver */ - - ret = usbdev_register(&drvr->drvr); - if (ret != OK) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_DEVREGISTER), (uint16_t)-ret); - goto errout_with_mutex; - } - - /* Signal to start the thread */ - - flags = irqsave(); - priv->theventset |= USBSTRG_EVENT_READY; - pthread_cond_signal(&priv->cond); - irqrestore(flags); - -errout_with_mutex: - pthread_mutex_unlock(&priv->mutex); - return ret; -} - -/**************************************************************************** - * Name: usbstrg_uninitialize - * - * Description: - * Un-initialize the USB storage class driver - * - * Input Parameters: - * handle - The handle returned by a previous call to usbstrg_configure(). - * - * Returned Value: - * None - * - ****************************************************************************/ - -void usbstrg_uninitialize(FAR void *handle) -{ - FAR struct usbstrg_alloc_s *alloc = (FAR struct usbstrg_alloc_s *)handle; - FAR struct usbstrg_dev_s *priv; - irqstate_t flags; -#ifdef SDCC - pthread_addr_t result1, result2; - pthread_attr_t attr; -#endif - void *value; - int ret; - int i; - -#ifdef CONFIG_DEBUG - if (!handle) - { - usbtrace(TRACE_CLSERROR(USBSTRG_TRACEERR_UNINITIALIZEINVALIDARGS), 0); - return; - } -#endif - priv = &alloc->dev; - - /* If the thread hasn't already exitted, tell it to exit now */ - - if (priv->thstate != USBSTRG_STATE_NOTSTARTED) - { - /* The thread was started.. Is it still running? */ - - pthread_mutex_lock(&priv->mutex); - if (priv->thstate != USBSTRG_STATE_TERMINATED) - { - /* Yes.. Ask the thread to stop */ - - flags = irqsave(); - priv->theventset |= USBSTRG_EVENT_TERMINATEREQUEST; - pthread_cond_signal(&priv->cond); - irqrestore(flags); - } - pthread_mutex_unlock(&priv->mutex); - - /* Wait for the thread to exit. This is necessary even if the - * thread has already exitted in order to collect the join - * garbage - */ - - ret = pthread_join(priv->thread, &value); - } - priv->thread = 0; - - /* Unregister the driver */ - - usbdev_unregister(&alloc->drvr.drvr); - - /* Uninitialize and release the LUNs */ - - for (i = 0; i < priv->nluns; ++i) - { - usbstrg_lununinitialize(&priv->luntab[i]); - } - kfree(priv->luntab); - - /* Release the I/O buffer */ - - if (priv->iobuffer) - { - kfree(priv->iobuffer); - } - - /* Uninitialize and release the driver structure */ - - pthread_mutex_destroy(&priv->mutex); - pthread_cond_destroy(&priv->cond); - - kfree(priv); -} diff --git a/nuttx/drivers/usbdev/usbdev_storage.h b/nuttx/drivers/usbdev/usbdev_storage.h deleted file mode 100644 index 77cc0e53c..000000000 --- a/nuttx/drivers/usbdev/usbdev_storage.h +++ /dev/null @@ -1,608 +0,0 @@ -/**************************************************************************** - * drivers/usbdev/usbdev_storage.h - * - * Copyright (C) 2008-2011 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * Mass storage class device. Bulk-only with SCSI subclass. - * - * 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. - * - ****************************************************************************/ - -#ifndef __DRIVERS_USBDEV_USBDEV_STORAGE_H -#define __DRIVERS_USBDEV_USBDEV_STORAGE_H - -/**************************************************************************** - * Included Files - ****************************************************************************/ - -#include - -#include -#include -#include -#include -#include - -#include -#include -#include - -/**************************************************************************** - * Definitions - ****************************************************************************/ -/* Configuration ************************************************************/ -/* Number of requests in the write queue */ - -#ifndef CONFIG_USBSTRG_NWRREQS -# define CONFIG_USBSTRG_NWRREQS 4 -#endif - -/* Number of requests in the read queue */ - -#ifndef CONFIG_USBSTRG_NRDREQS -# define CONFIG_USBSTRG_NRDREQS 4 -#endif - -/* Logical endpoint numbers / max packet sizes */ - -#ifndef CONFIG_USBSTRG_EPBULKOUT -# warning "EPBULKOUT not defined in the configuration" -# define CONFIG_USBSTRG_EPBULKOUT 2 -#endif - -#ifndef CONFIG_USBSTRG_EPBULKIN -# warning "EPBULKIN not defined in the configuration" -# define CONFIG_USBSTRG_EPBULKIN 3 -#endif - -/* Packet and request buffer sizes */ - -#ifndef CONFIG_USBSTRG_EP0MAXPACKET -# define CONFIG_USBSTRG_EP0MAXPACKET 64 -#endif - -#ifndef CONFIG_USBSTRG_BULKINREQLEN -# ifdef CONFIG_USBDEV_DUALSPEED -# define CONFIG_USBSTRG_BULKINREQLEN 512 -# else -# define CONFIG_USBSTRG_BULKINREQLEN 64 -# endif -#else -# ifdef CONFIG_USBDEV_DUALSPEED -# if CONFIG_USBSTRG_BULKINREQLEN < 512 -# warning "Bulk in buffer size smaller than max packet" -# undef CONFIG_USBSTRG_BULKINREQLEN -# define CONFIG_USBSTRG_BULKINREQLEN 512 -# endif -# else -# if CONFIG_USBSTRG_BULKINREQLEN < 64 -# warning "Bulk in buffer size smaller than max packet" -# undef CONFIG_USBSTRG_BULKINREQLEN -# define CONFIG_USBSTRG_BULKINREQLEN 64 -# endif -# endif -#endif - -#ifndef CONFIG_USBSTRG_BULKOUTREQLEN -# ifdef CONFIG_USBDEV_DUALSPEED -# define CONFIG_USBSTRG_BULKOUTREQLEN 512 -# else -# define CONFIG_USBSTRG_BULKOUTREQLEN 64 -# endif -#else -# ifdef CONFIG_USBDEV_DUALSPEED -# if CONFIG_USBSTRG_BULKOUTREQLEN < 512 -# warning "Bulk in buffer size smaller than max packet" -# undef CONFIG_USBSTRG_BULKOUTREQLEN -# define CONFIG_USBSTRG_BULKOUTREQLEN 512 -# endif -# else -# if CONFIG_USBSTRG_BULKOUTREQLEN < 64 -# warning "Bulk in buffer size smaller than max packet" -# undef CONFIG_USBSTRG_BULKOUTREQLEN -# define CONFIG_USBSTRG_BULKOUTREQLEN 64 -# endif -# endif -#endif - -/* Vendor and product IDs and strings */ - -#ifndef CONFIG_USBSTRG_VENDORID -# warning "CONFIG_USBSTRG_VENDORID not defined" -# define CONFIG_USBSTRG_VENDORID 0x584e -#endif - -#ifndef CONFIG_USBSTRG_PRODUCTID -# warning "CONFIG_USBSTRG_PRODUCTID not defined" -# define CONFIG_USBSTRG_PRODUCTID 0x5342 -#endif - -#ifndef CONFIG_USBSTRG_VERSIONNO -# define CONFIG_USBSTRG_VERSIONNO (0x0399) -#endif - -#ifndef CONFIG_USBSTRG_VENDORSTR -# warning "No Vendor string specified" -# define CONFIG_USBSTRG_VENDORSTR "NuttX" -#endif - -#ifndef CONFIG_USBSTRG_PRODUCTSTR -# warning "No Product string specified" -# define CONFIG_USBSTRG_PRODUCTSTR "USBdev Storage" -#endif - -#undef CONFIG_USBSTRG_SERIALSTR -#define CONFIG_USBSTRG_SERIALSTR "0101" - -#undef CONFIG_USBSTRG_CONFIGSTR -#define CONFIG_USBSTRG_CONFIGSTR "Bulk" - -/* Debug -- must be consistent with include/debug.h */ - -#ifdef CONFIG_CPP_HAVE_VARARGS -# ifdef CONFIG_DEBUG -# ifdef CONFIG_ARCH_LOWPUTC -# define dbgprintf(format, arg...) lib_lowprintf(format, ##arg) -# else -# define dbgprintf(format, arg...) lib_rawprintf(format, ##arg) -# endif -# else -# define dbgprintf(x...) -# endif -#else -# ifdef CONFIG_DEBUG -# ifdef CONFIG_ARCH_LOWPUTC -# define dbgprintf lib_lowprintf -# else -# define dbgprintf lib_rawprintf -# endif -# else -# define dbgprintf (void) -# endif -#endif - -/* Packet and request buffer sizes */ - -#ifndef CONFIG_USBSTRG_EP0MAXPACKET -# define CONFIG_USBSTRG_EP0MAXPACKET 64 -#endif - -/* USB Controller */ - -#ifndef CONFIG_USBDEV_SELFPOWERED -# define SELFPOWERED USB_CONFIG_ATT_SELFPOWER -#else -# define SELFPOWERED (0) -#endif - -#ifndef CONFIG_USBDEV_REMOTEWAKEUP -# define REMOTEWAKEUP USB_CONFIG_ATTR_WAKEUP -#else -# define REMOTEWAKEUP (0) -#endif - -#ifndef CONFIG_USBDEV_MAXPOWER -# define CONFIG_USBDEV_MAXPOWER 100 -#endif - -/* Current state of the worker thread */ - -#define USBSTRG_STATE_NOTSTARTED (0) /* Thread has not yet been started */ -#define USBSTRG_STATE_STARTED (1) /* Started, but is not yet initialized */ -#define USBSTRG_STATE_IDLE (2) /* Started and waiting for commands */ -#define USBSTRG_STATE_CMDPARSE (3) /* Processing a received command */ -#define USBSTRG_STATE_CMDREAD (4) /* Processing a SCSI read command */ -#define USBSTRG_STATE_CMDWRITE (5) /* Processing a SCSI write command */ -#define USBSTRG_STATE_CMDFINISH (6) /* Finish command processing */ -#define USBSTRG_STATE_CMDSTATUS (7) /* Processing the final status of the command */ -#define USBSTRG_STATE_TERMINATED (8) /* Thread has exitted */ - -/* Event communicated to worker thread */ - -#define USBSTRG_EVENT_NOEVENTS (0) /* There are no outstanding events */ -#define USBSTRG_EVENT_READY (1 << 0) /* Initialization is complete */ -#define USBSTRG_EVENT_RDCOMPLETE (1 << 1) /* A read has completed there is data to be processed */ -#define USBSTRG_EVENT_WRCOMPLETE (1 << 2) /* A write has completed and a request is available */ -#define USBSTRG_EVENT_TERMINATEREQUEST (1 << 3) /* Shutdown requested */ -#define USBSTRG_EVENT_DISCONNECT (1 << 4) /* USB disconnect received */ -#define USBSTRG_EVENT_RESET (1 << 5) /* USB storage setup reset received */ -#define USBSTRG_EVENT_CFGCHANGE (1 << 6) /* USB setup configuration change received */ -#define USBSTRG_EVENT_IFCHANGE (1 << 7) /* USB setup interface change received */ -#define USBSTRG_EVENT_ABORTBULKOUT (1 << 8) /* SCSI receive failure */ - -/* SCSI command flags (passed to usbstrg_setupcmd()) */ - -#define USBSTRG_FLAGS_DIRMASK (0x03) /* Bits 0-1: Data direction */ -#define USBSTRG_FLAGS_DIRNONE (0x00) /* No data to send */ -#define USBSTRG_FLAGS_DIRHOST2DEVICE (0x01) /* Host-to-device */ -#define USBSTRG_FLAGS_DIRDEVICE2HOST (0x02) /* Device-to-host */ -#define USBSTRG_FLAGS_BLOCKXFR (0x04) /* Bit 2: Command is a block transfer request */ -#define USBSTRG_FLAGS_LUNNOTNEEDED (0x08) /* Bit 3: Command does not require a valid LUN */ -#define USBSTRG_FLAGS_UACOKAY (0x10) /* Bit 4: Command OK if unit attention condition */ -#define USBSTRG_FLAGS_RETAINSENSEDATA (0x20) /* Bit 5: Do not clear sense data */ - -/* Descriptors **************************************************************/ - -/* Big enough to hold our biggest descriptor */ - -#define USBSTRG_MXDESCLEN (64) - -/* String language */ - -#define USBSTRG_STR_LANGUAGE (0x0409) /* en-us */ - -/* Descriptor strings */ - -#define USBSTRG_MANUFACTURERSTRID (1) -#define USBSTRG_PRODUCTSTRID (2) -#define USBSTRG_SERIALSTRID (3) -#define USBSTRG_CONFIGSTRID (4) -#define USBSTRG_NCONFIGS (1) /* Number of configurations supported */ - -/* Configuration Descriptor */ - -#define USBSTRG_NINTERFACES (1) /* Number of interfaces in the configuration */ -#define USBSTRG_INTERFACEID (0) -#define USBSTRG_ALTINTERFACEID (0) -#define USBSTRG_CONFIGIDNONE (0) /* Config ID means to return to address mode */ -#define USBSTRG_CONFIGID (1) /* The only supported configuration ID */ - -#define STRING_MANUFACTURER (1) -#define STRING_PRODUCT (2) -#define STRING_SERIAL (3) - -#define USBSTRG_CONFIGID (1) /* There is only one configuration. */ - -/* Interface description */ - -#define USBSTRG_NENDPOINTS (2) /* Number of endpoints in the interface */ - -/* Endpoint configuration */ - -#define USBSTRG_EPOUTBULK_ADDR (CONFIG_USBSTRG_EPBULKOUT) -#define USBSTRG_EPOUTBULK_ATTR (USB_EP_ATTR_XFER_BULK) - -#define USBSTRG_EPINBULK_ADDR (USB_DIR_IN|CONFIG_USBSTRG_EPBULKIN) -#define USBSTRG_EPINBULK_ATTR (USB_EP_ATTR_XFER_BULK) - -#define USBSTRG_HSBULKMAXPACKET (512) -#define USBSTRG_HSBULKMXPKTSHIFT (9) -#define USBSTRG_HSBULKMXPKTMASK (0x000001ff) -#define USBSTRG_FSBULKMAXPACKET (64) -#define USBSTRG_FSBULKMXPKTSHIFT (6) -#define USBSTRG_FSBULKMXPKTMASK (0x0000003f) - -/* Macros for dual speed vs. full speed only operation */ - -#ifdef CONFIG_USBDEV_DUALSPEED -# define USBSTRG_EPBULKINDESC(hs) \ - usbstrg_getepdesc((hs) ? USBSTRG_EPHSBULKIN : USBSTRG_EPFSBULKIN) -# define USBSTRG_EPBULKOUTDESC(hs) \ - usbstrg_getepdesc((hs) ? USBSTRG_EPHSBULKOUT : USBSTRG_EPFSBULKOUT) -# define USBSTRG_BULKMAXPACKET(hs) \ - ((hs) ? USBSTRG_HSBULKMAXPACKET : USBSTRG_FSBULKMAXPACKET) -# define USBSTRG_BULKMXPKTSHIFT(d) \ - (((d)->speed==USB_SPEED_HIGH) ? USBSTRG_HSBULKMXPKTSHIFT : USBSTRG_FSBULKMXPKTSHIFT) -# define USBSTRG_BULKMXPKTMASK(d) \ - (((d)->speed==USB_SPEED_HIGH) ? USBSTRG_HSBULKMXPKTMASK : USBSTRG_FSBULKMXPKTMASK) -#else -# define USBSTRG_EPBULKINDESC(d) usbstrg_getepdesc(USBSTRG_EPFSBULKIN) -# define USBSTRG_EPBULKOUTDESC(d) usbstrg_getepdesc(USBSTRG_EPFSBULKOUT) -# define USBSTRG_BULKMAXPACKET(hs) USBSTRG_FSBULKMAXPACKET -# define USBSTRG_BULKMXPKTSHIFT(d) USBSTRG_FSBULKMXPKTSHIFT -# define USBSTRG_BULKMXPKTMASK(d) USBSTRG_FSBULKMXPKTMASK -#endif - -/* Block driver helpers *****************************************************/ - -#define USBSTRG_DRVR_READ(l,b,s,n) ((l)->inode->u.i_bops->read((l)->inode,b,s,n)) -#define USBSTRG_DRVR_WRITE(l,b,s,n) ((l)->inode->u.i_bops->write((l)->inode,b,s,n)) -#define USBSTRG_DRVR_GEOMETRY(l,g) ((l)->inode->u.i_bops->geometry((l)->inode,g)) - -/* Everpresent min/max macros ***********************************************/ - -#ifndef min -# define min(a,b) ((a) < (b) ? (a) : (b)) -#endif - -#ifndef max -# define max(a,b) ((a) > (b) ? (a) : (b)) -#endif - -/**************************************************************************** - * Public Types - ****************************************************************************/ -/* Endpoint descriptors */ - -enum usbstrg_epdesc_e -{ - USBSTRG_EPFSBULKOUT = 0, /* Full speed bulk OUT endpoint descriptor */ - USBSTRG_EPFSBULKIN /* Full speed bulk IN endpoint descriptor */ -#ifdef CONFIG_USBDEV_DUALSPEED - , - USBSTRG_EPHSBULKOUT, /* High speed bulk OUT endpoint descriptor */ - USBSTRG_EPHSBULKIN /* High speed bulk IN endpoint descriptor */ -#endif -}; - -/* Container to support a list of requests */ - -struct usbstrg_req_s -{ - FAR struct usbstrg_req_s *flink; /* Implements a singly linked list */ - FAR struct usbdev_req_s *req; /* The contained request */ -}; - -/* This structure describes one LUN: */ - -struct usbstrg_lun_s -{ - struct inode *inode; /* Inode structure of open'ed block driver */ - uint8_t readonly:1; /* Media is read-only */ - uint8_t locked:1; /* Media removal is prevented */ - uint16_t sectorsize; /* The size of one sector */ - uint32_t sd; /* Sense data */ - uint32_t sdinfo; /* Sense data information */ - uint32_t uad; /* Unit needs attention data */ - off_t startsector; /* Sector offset to start of partition */ - size_t nsectors; /* Number of sectors in the partition */ -}; - -/* Describes the overall state of the driver */ - -struct usbstrg_dev_s -{ - FAR struct usbdev_s *usbdev; /* usbdev driver pointer (Non-null if registered) */ - - /* Worker thread interface */ - - pthread_t thread; /* The worker thread */ - pthread_mutex_t mutex; /* Mutually exclusive access to resources*/ - pthread_cond_t cond; /* Used to signal worker thread */ - volatile uint8_t thstate; /* State of the worker thread */ - volatile uint16_t theventset; /* Set of pending events signaled to worker thread */ - volatile uint8_t thvalue; /* Value passed with the event (must persist) */ - - /* Storage class configuration and state */ - - uint8_t nluns:4; /* Number of LUNs */ - uint8_t config; /* Configuration number */ - - /* Endpoints */ - - FAR struct usbdev_ep_s *epbulkin; /* Bulk IN endpoint structure */ - FAR struct usbdev_ep_s *epbulkout; /* Bulk OUT endpoint structure */ - FAR struct usbdev_req_s *ctrlreq; /* Control request (for ep0 setup responses) */ - - /* SCSI command processing */ - - struct usbstrg_lun_s *lun; /* Currently selected LUN */ - struct usbstrg_lun_s *luntab; /* Allocated table of all LUNs */ - uint8_t cdb[USBSTRG_MAXCDBLEN]; /* Command data (cdb[]) from CBW */ - uint8_t phaseerror:1; /* Need to send phase sensing status */ - uint8_t shortpacket:1; /* Host transmission stopped unexpectedly */ - uint8_t cbwdir:2; /* Direction from CBW. See USBSTRG_FLAGS_DIR* definitions */ - uint8_t cdblen; /* Length of cdb[] from CBW */ - uint8_t cbwlun; /* LUN from the CBW */ - uint16_t nsectbytes; /* Bytes buffered in iobuffer[] */ - uint16_t nreqbytes; /* Bytes buffered in head write requests */ - uint16_t iosize; /* Size of iobuffer[] */ - uint32_t cbwlen; /* Length of data from CBW */ - uint32_t cbwtag; /* Tag from the CBW */ - union - { - uint32_t xfrlen; /* Read/Write: Sectors remaining to be transferred */ - uint32_t alloclen; /* Other device-to-host: Host allocation length */ - } u; - uint32_t sector; /* Current sector (relative to lun->startsector) */ - uint32_t residue; /* Untransferred amount reported in the CSW */ - uint8_t *iobuffer; /* Buffer for data transfers */ - - /* Write request list */ - - struct sq_queue_s wrreqlist; /* List of empty write request containers */ - struct sq_queue_s rdreqlist; /* List of filled read request containers */ - - /* Pre-allocated write request containers. The write requests will - * be linked in a free list (wrreqlist), and used to send requests to - * EPBULKIN; Read requests will be queued in the EBULKOUT. - */ - - struct usbstrg_req_s wrreqs[CONFIG_USBSTRG_NWRREQS]; - struct usbstrg_req_s rdreqs[CONFIG_USBSTRG_NRDREQS]; -}; - -/**************************************************************************** - * Public Data - ****************************************************************************/ - -#undef EXTERN -#if defined(__cplusplus) -# define EXTERN extern "C" -extern "C" -{ -#else -# define EXTERN extern -#endif - -/* String *******************************************************************/ - -EXTERN const char g_vendorstr[]; -EXTERN const char g_productstr[]; -EXTERN const char g_serialstr[]; - -/************************************************************************************ - * Public Function Prototypes - ************************************************************************************/ - -/************************************************************************************ - * Name: usbstrg_mkstrdesc - * - * Description: - * Construct a string descriptor - * - ************************************************************************************/ - -struct usb_strdesc_s; -int usbstrg_mkstrdesc(uint8_t id, struct usb_strdesc_s *strdesc); - -/************************************************************************************ - * Name: usbstrg_getepdesc - * - * Description: - * Return a pointer to the raw device descriptor - * - ************************************************************************************/ - -FAR const struct usb_devdesc_s *usbstrg_getdevdesc(void); - -/************************************************************************************ - * Name: usbstrg_getepdesc - * - * Description: - * Return a pointer to the raw endpoint descriptor (used for configuring endpoints) - * - ************************************************************************************/ - -struct usb_epdesc_s; -FAR const struct usb_epdesc_s *usbstrg_getepdesc(enum usbstrg_epdesc_e epid); - -/************************************************************************************ - * Name: usbstrg_mkcfgdesc - * - * Description: - * Construct the configuration descriptor - * - ************************************************************************************/ - -#ifdef CONFIG_USBDEV_DUALSPEED -int16_t usbstrg_mkcfgdesc(FAR uint8_t *buf, uint8_t speed, uint8_t type); -#else -int16_t usbstrg_mkcfgdesc(FAR uint8_t *buf); -#endif - -/************************************************************************************ - * Name: usbstrg_getqualdesc - * - * Description: - * Return a pointer to the raw qual descriptor - * - ************************************************************************************/ - -#ifdef CONFIG_USBDEV_DUALSPEED -FAR const struct usb_qualdesc_s *usbstrg_getqualdesc(void); -#endif - -/**************************************************************************** - * Name: usbstrg_workerthread - * - * Description: - * This is the main function of the USB storage worker thread. It loops - * until USB-related events occur, then processes those events accordingly - * - ****************************************************************************/ - -EXTERN void *usbstrg_workerthread(void *arg); - -/**************************************************************************** - * Name: usbstrg_setconfig - * - * Description: - * Set the device configuration by allocating and configuring endpoints and - * by allocating and queue read and write requests. - * - ****************************************************************************/ - -EXTERN int usbstrg_setconfig(FAR struct usbstrg_dev_s *priv, uint8_t config); - -/**************************************************************************** - * Name: usbstrg_resetconfig - * - * Description: - * Mark the device as not configured and disable all endpoints. - * - ****************************************************************************/ - -EXTERN void usbstrg_resetconfig(FAR struct usbstrg_dev_s *priv); - -/**************************************************************************** - * Name: usbstrg_wrcomplete - * - * Description: - * Handle completion of write request. This function probably executes - * in the context of an interrupt handler. - * - ****************************************************************************/ - -EXTERN void usbstrg_wrcomplete(FAR struct usbdev_ep_s *ep, - FAR struct usbdev_req_s *req); - -/**************************************************************************** - * Name: usbstrg_rdcomplete - * - * Description: - * Handle completion of read request on the bulk OUT endpoint. This - * is handled like the receipt of serial data on the "UART" - * - ****************************************************************************/ - -EXTERN void usbstrg_rdcomplete(FAR struct usbdev_ep_s *ep, - FAR struct usbdev_req_s *req); - -/**************************************************************************** - * Name: usbstrg_deferredresponse - * - * Description: - * Some EP0 setup request cannot be responded to immediately becuase they - * require some asynchronous action from the SCSI worker thread. This - * function is provided for the SCSI thread to make that deferred response. - * The specific requests that require this deferred response are: - * - * 1. USB_REQ_SETCONFIGURATION, - * 2. USB_REQ_SETINTERFACE, or - * 3. USBSTRG_REQ_MSRESET - * - * In all cases, the success reponse is a zero-length packet; the failure - * response is an EP0 stall. - * - * Input parameters: - * priv - Private state structure for this USB storage instance - * stall - true is the action failed and a stall is required - * - ****************************************************************************/ - -EXTERN void usbstrg_deferredresponse(FAR struct usbstrg_dev_s *priv, bool failed); - -#undef EXTERN -#if defined(__cplusplus) -} -#endif - -#endif /* #define __DRIVERS_USBDEV_USBDEV_STORAGE_H */ diff --git a/nuttx/drivers/usbdev/usbdev_stordesc.c b/nuttx/drivers/usbdev/usbdev_stordesc.c deleted file mode 100644 index 68dffa96c..000000000 --- a/nuttx/drivers/usbdev/usbdev_stordesc.c +++ /dev/null @@ -1,408 +0,0 @@ -/**************************************************************************** - * drivers/usbdev/usbdev_stordesc.c - * - * Copyright (C) 2011 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 - * 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 - -#include -#include -#include -#include -#include - -#include -#include - -#include "usbdev_storage.h" - -/**************************************************************************** - * Definitions - ****************************************************************************/ - -/**************************************************************************** - * Private Types - ****************************************************************************/ - -/**************************************************************************** - * Private Function Prototypes - ****************************************************************************/ - -/**************************************************************************** - * Private Data - ****************************************************************************/ -/* Descriptors **************************************************************/ -/* Device descriptor */ - -static const struct usb_devdesc_s g_devdesc = -{ - USB_SIZEOF_DEVDESC, /* len */ - USB_DESC_TYPE_DEVICE, /* type */ - {LSBYTE(0x0200), MSBYTE(0x0200)}, /* usb */ - USB_CLASS_PER_INTERFACE, /* class */ - 0, /* subclass */ - 0, /* protocol */ - CONFIG_USBSTRG_EP0MAXPACKET, /* maxpacketsize */ - { /* vendor */ - LSBYTE(CONFIG_USBSTRG_VENDORID), - MSBYTE(CONFIG_USBSTRG_VENDORID) - }, - { /* product */ - LSBYTE(CONFIG_USBSTRG_PRODUCTID), - MSBYTE(CONFIG_USBSTRG_PRODUCTID) }, - { /* device */ - LSBYTE(CONFIG_USBSTRG_VERSIONNO), - MSBYTE(CONFIG_USBSTRG_VERSIONNO) - }, - USBSTRG_MANUFACTURERSTRID, /* imfgr */ - USBSTRG_PRODUCTSTRID, /* iproduct */ - USBSTRG_SERIALSTRID, /* serno */ - USBSTRG_NCONFIGS /* nconfigs */ -}; - -/* Configuration descriptor */ - -static const struct usb_cfgdesc_s g_cfgdesc = -{ - USB_SIZEOF_CFGDESC, /* len */ - USB_DESC_TYPE_CONFIG, /* type */ - {0, 0}, /* totallen -- to be provided */ - USBSTRG_NINTERFACES, /* ninterfaces */ - USBSTRG_CONFIGID, /* cfgvalue */ - USBSTRG_CONFIGSTRID, /* icfg */ - USB_CONFIG_ATTR_ONE|SELFPOWERED|REMOTEWAKEUP, /* attr */ - (CONFIG_USBDEV_MAXPOWER + 1) / 2 /* mxpower */ -}; - -/* Single interface descriptor */ - -static const struct usb_ifdesc_s g_ifdesc = -{ - USB_SIZEOF_IFDESC, /* len */ - USB_DESC_TYPE_INTERFACE, /* type */ - USBSTRG_INTERFACEID, /* ifno */ - USBSTRG_ALTINTERFACEID, /* alt */ - USBSTRG_NENDPOINTS, /* neps */ - USB_CLASS_MASS_STORAGE, /* class */ - USBSTRG_SUBCLASS_SCSI, /* subclass */ - USBSTRG_PROTO_BULKONLY, /* protocol */ - USBSTRG_CONFIGSTRID /* iif */ -}; - -/* Endpoint descriptors */ - -static const struct usb_epdesc_s g_fsepbulkoutdesc = -{ - USB_SIZEOF_EPDESC, /* len */ - USB_DESC_TYPE_ENDPOINT, /* type */ - USBSTRG_EPOUTBULK_ADDR, /* addr */ - USBSTRG_EPOUTBULK_ATTR, /* attr */ - { /* maxpacket */ - LSBYTE(USBSTRG_FSBULKMAXPACKET), - MSBYTE(USBSTRG_FSBULKMAXPACKET) - }, - 0 /* interval */ -}; - -static const struct usb_epdesc_s g_fsepbulkindesc = -{ - USB_SIZEOF_EPDESC, /* len */ - USB_DESC_TYPE_ENDPOINT, /* type */ - USBSTRG_EPINBULK_ADDR, /* addr */ - USBSTRG_EPINBULK_ATTR, /* attr */ - { /* maxpacket */ - LSBYTE(USBSTRG_FSBULKMAXPACKET), - MSBYTE(USBSTRG_FSBULKMAXPACKET) - }, - 0 /* interval */ -}; - -#ifdef CONFIG_USBDEV_DUALSPEED -static const struct usb_qualdesc_s g_qualdesc = -{ - USB_SIZEOF_QUALDESC, /* len */ - USB_DESC_TYPE_DEVICEQUALIFIER, /* type */ - { /* usb */ - LSBYTE(0x0200), - MSBYTE(0x0200) - }, - USB_CLASS_PER_INTERFACE, /* class */ - 0, /* subclass */ - 0, /* protocol */ - CONFIG_USBSTRG_EP0MAXPACKET, /* mxpacketsize */ - USBSTRG_NCONFIGS, /* nconfigs */ - 0, /* reserved */ -}; - -static const struct usb_epdesc_s g_hsepbulkoutdesc = -{ - USB_SIZEOF_EPDESC, /* len */ - USB_DESC_TYPE_ENDPOINT, /* type */ - USBSTRG_EPOUTBULK_ADDR, /* addr */ - USBSTRG_EPOUTBULK_ATTR, /* attr */ - { /* maxpacket */ - LSBYTE(USBSTRG_HSBULKMAXPACKET), - MSBYTE(USBSTRG_HSBULKMAXPACKET) - }, - 0 /* interval */ -}; - -static const struct usb_epdesc_s g_hsepbulkindesc = -{ - USB_SIZEOF_EPDESC, /* len */ - USB_DESC_TYPE_ENDPOINT, /* type */ - USBSTRG_EPINBULK_ADDR, /* addr */ - USBSTRG_EPINBULK_ATTR, /* attr */ - { /* maxpacket */ - LSBYTE(USBSTRG_HSBULKMAXPACKET), - MSBYTE(USBSTRG_HSBULKMAXPACKET) - }, - 0 /* interval */ -}; -#endif - -/**************************************************************************** - * Public Data - ****************************************************************************/ -/* Strings ******************************************************************/ - -const char g_vendorstr[] = CONFIG_USBSTRG_VENDORSTR; -const char g_productstr[] = CONFIG_USBSTRG_PRODUCTSTR; -const char g_serialstr[] = CONFIG_USBSTRG_SERIALSTR; - -/**************************************************************************** - * Private Functions - ****************************************************************************/ - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -/**************************************************************************** - * Name: usbstrg_mkstrdesc - * - * Description: - * Construct a string descriptor - * - ****************************************************************************/ - -int usbstrg_mkstrdesc(uint8_t id, struct usb_strdesc_s *strdesc) -{ - const char *str; - int len; - int ndata; - int i; - - switch (id) - { - case 0: - { - /* Descriptor 0 is the language id */ - - strdesc->len = 4; - strdesc->type = USB_DESC_TYPE_STRING; - strdesc->data[0] = LSBYTE(USBSTRG_STR_LANGUAGE); - strdesc->data[1] = MSBYTE(USBSTRG_STR_LANGUAGE); - return 4; - } - - case USBSTRG_MANUFACTURERSTRID: - str = g_vendorstr; - break; - - case USBSTRG_PRODUCTSTRID: - str = g_productstr; - break; - - case USBSTRG_SERIALSTRID: - str = g_serialstr; - break; - - case USBSTRG_CONFIGSTRID: - str = CONFIG_USBSTRG_CONFIGSTR; - break; - - default: - return -EINVAL; - } - - /* The string is utf16-le. The poor man's utf-8 to utf16-le - * conversion below will only handle 7-bit en-us ascii - */ - - len = strlen(str); - for (i = 0, ndata = 0; i < len; i++, ndata += 2) - { - strdesc->data[ndata] = str[i]; - strdesc->data[ndata+1] = 0; - } - - strdesc->len = ndata+2; - strdesc->type = USB_DESC_TYPE_STRING; - return strdesc->len; -} - -/**************************************************************************** - * Name: usbstrg_getepdesc - * - * Description: - * Return a pointer to the raw device descriptor - * - ****************************************************************************/ - -FAR const struct usb_devdesc_s *usbstrg_getdevdesc(void) -{ - return &g_devdesc; -} - -/**************************************************************************** - * Name: usbstrg_getepdesc - * - * Description: - * Return a pointer to the raw endpoint descriptor (used for configuring - * endpoints) - * - ****************************************************************************/ - -FAR const struct usb_epdesc_s *usbstrg_getepdesc(enum usbstrg_epdesc_e epid) -{ - switch (epid) - { - case USBSTRG_EPFSBULKOUT: /* Full speed bulk OUT endpoint descriptor */ - return &g_fsepbulkoutdesc; - - case USBSTRG_EPFSBULKIN: /* Full speed bulk IN endpoint descriptor */ - return &g_fsepbulkindesc; - -#ifdef CONFIG_USBDEV_DUALSPEED - case USBSTRG_EPHSBULKOUT: /* High speed bulk OUT endpoint descriptor */ - return &g_hsepbulkoutdesc; - - case USBSTRG_EPHSBULKIN: /* High speed bulk IN endpoint descriptor */ - return &g_hsepbulkindesc; -#endif - default: - return NULL; - } -}; - -/**************************************************************************** - * Name: usbstrg_mkcfgdesc - * - * Description: - * Construct the configuration descriptor - * - ****************************************************************************/ - -#ifdef CONFIG_USBDEV_DUALSPEED -int16_t usbstrg_mkcfgdesc(uint8_t *buf, uint8_t speed, uint8_t type) -#else -int16_t usbstrg_mkcfgdesc(uint8_t *buf) -#endif -{ - FAR struct usb_cfgdesc_s *cfgdesc = (struct usb_cfgdesc_s*)buf; -#ifdef CONFIG_USBDEV_DUALSPEED - FAR const struct usb_epdesc_s *epdesc; - bool hispeed = (speed == USB_SPEED_HIGH); - uint16_t bulkmxpacket; -#endif - uint16_t totallen; - - /* This is the total length of the configuration (not necessarily the - * size that we will be sending now. - */ - - totallen = USB_SIZEOF_CFGDESC + USB_SIZEOF_IFDESC + USBSTRG_NENDPOINTS * USB_SIZEOF_EPDESC; - - /* Configuration descriptor -- Copy the canned descriptor and fill in the - * type (we'll also need to update the size below - */ - - memcpy(cfgdesc, &g_cfgdesc, USB_SIZEOF_CFGDESC); - buf += USB_SIZEOF_CFGDESC; - - /* Copy the canned interface descriptor */ - - memcpy(buf, &g_ifdesc, USB_SIZEOF_IFDESC); - buf += USB_SIZEOF_IFDESC; - - /* Make the two endpoint configurations */ - -#ifdef CONFIG_USBDEV_DUALSPEED - /* Check for switches between high and full speed */ - - hispeed = (speed == USB_SPEED_HIGH); - if (type == USB_DESC_TYPE_OTHERSPEEDCONFIG) - { - hispeed = !hispeed; - } - - bulkmxpacket = USBSTRG_BULKMAXPACKET(hispeed); - epdesc = USBSTRG_EPBULKINDESC(hispeed); - memcpy(buf, epdesc, USB_SIZEOF_EPDESC); - buf += USB_SIZEOF_EPDESC; - - epdesc = USBSTRG_EPBULKOUTDESC(hispeed); - memcpy(buf, epdesc, USB_SIZEOF_EPDESC); -#else - memcpy(buf, &g_fsepbulkoutdesc, USB_SIZEOF_EPDESC); - buf += USB_SIZEOF_EPDESC; - memcpy(buf, &g_fsepbulkindesc, USB_SIZEOF_EPDESC); -#endif - - /* Finally, fill in the total size of the configuration descriptor */ - - cfgdesc->totallen[0] = LSBYTE(totallen); - cfgdesc->totallen[1] = MSBYTE(totallen); - return totallen; -} - -/**************************************************************************** - * Name: usbstrg_getqualdesc - * - * Description: - * Return a pointer to the raw qual descriptor - * - ****************************************************************************/ - -#ifdef CONFIG_USBDEV_DUALSPEED -FAR const struct usb_qualdesc_s *usbstrg_getqualdesc(void) -{ - return &g_qualdesc; -} -#endif - diff --git a/nuttx/drivers/usbdev/usbdev_trace.c b/nuttx/drivers/usbdev/usbdev_trace.c index 7b258b1cb..c8cc09292 100644 --- a/nuttx/drivers/usbdev/usbdev_trace.c +++ b/nuttx/drivers/usbdev/usbdev_trace.c @@ -1,8 +1,8 @@ /**************************************************************************** * drivers/usbdev/usbdev_trace.c * - * Copyright (C) 2008-2010 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt + * Copyright (C) 2008-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 diff --git a/nuttx/drivers/usbdev/usbdev_trprintf.c b/nuttx/drivers/usbdev/usbdev_trprintf.c index 55c517b2e..2a9921f98 100644 --- a/nuttx/drivers/usbdev/usbdev_trprintf.c +++ b/nuttx/drivers/usbdev/usbdev_trprintf.c @@ -1,8 +1,8 @@ /**************************************************************************** * drivers/usbdev/usbdev_trprintf.c * - * Copyright (C) 2008-2010 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt + * Copyright (C) 2008-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 -- cgit v1.2.3