Revision 12:ea4badc78215, committed 2013-03-20

Comitter:

va009039

Date:

Wed Mar 20 14:28:39 2013 +0000

Parent:

11:6a8eef89eb22

Commit message:

support detach

Changed in this revision

--- a/USBHost.lib	Mon Mar 18 12:34:47 2013 +0000
+++ b/USBHost.lib	Wed Mar 20 14:28:39 2013 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/va009039/code/USBHost/#5cc7bad4f28e
+http://mbed.org/users/va009039/code/USBHost/#a338d6a681fb

--- a/USBHostC270/BaseUvc.cpp	Mon Mar 18 12:34:47 2013 +0000
+++ b/USBHostC270/BaseUvc.cpp	Wed Mar 20 14:28:39 2013 +0000
@@ -1,19 +1,9 @@
 // BaseUvc.cpp
-
 #include "USBHostConf.h"
 #include "USBHost.h"
-
+#include "USBIsochronous.h"
 #include "BaseUvc.h"
 
-//#define ISO_DEBUG 1
-#ifdef ISO_DEBUG
-#define ISO_DBG(x, ...) std::printf("[%s:%d]"x"\r\n", __PRETTY_FUNCTION__, __LINE__, ##__VA_ARGS__);
-#else
-#define ISO_DBG(...)  while(0);
-#endif
-
-#define TEST_ASSERT(A) while(!(A)){fprintf(stderr,"\n\n%s@%d %s ASSERT!\n\n",__PRETTY_FUNCTION__,__LINE__,#A);exit(1);};
-
 void BaseUvc::poll(int millisec)
 {
     HCITD* itd = m_isoEp->isochronousReceive(millisec);
@@ -82,168 +72,3 @@
 }
 
 
-HCITD::HCITD(BaseEp* obj, uint16_t FrameNumber, int FrameCount, uint16_t PacketSize) {
-    Control = 0xe0000000           | // CC ConditionCode NOT ACCESSED
-             ((FrameCount-1) << 24)| // FC FrameCount
-                  TD_DELAY_INT(0)  | // DI DelayInterrupt
-                 FrameNumber;        // SF StartingFrame
-    BufferPage0 = const_cast<uint8_t*>(buf);
-    BufferEnd = const_cast<uint8_t*>(buf) + PacketSize * FrameCount - 1;
-    Next = NULL; 
-    ep = obj;
-    uint32_t addr = reinterpret_cast<uint32_t>(buf);
-    for(int i = 0; i < FrameCount; i++) {
-        uint16_t offset = addr & 0x0fff;
-        if ((addr&0xfffff000) == (reinterpret_cast<uint32_t>(BufferEnd)&0xfffff000)) {
-            offset |= 0x1000;
-        }
-        OffsetPSW[i] = 0xe000|offset;
-        addr += PacketSize;
-    }
-}
-
-void IsochronousEp::init(int addr, uint8_t ep, uint16_t size)
-{
-    //ISO_DBG("%p FA:%d EP:%02X MPS:%d\n", this, addr, ep, size);
-    BaseEp::init(addr, ep, size);
-    TEST_ASSERT(m_pED);
-    
-    m_pED->setFormat(); // F Format ITD
-
-    TEST_ASSERT(size >= 128 && size <= 1023);
-    m_PacketSize = size;
-    m_FrameCount = 4; // 1-8
-    TEST_ASSERT(m_FrameCount >= 1 && m_FrameCount <= 8);
-    m_itd_queue_count = 0;
-    reset();
-    HCITD* itd = new_HCITD(this);
-    m_pED->init_queue<HCITD>(itd); 
-    TEST_ASSERT(itd);
-    if (itd == NULL) {
-        return;
-    }
-    _HCCA* hcca = reinterpret_cast<_HCCA*>(LPC_USB->HcHCCA);
-    TEST_ASSERT(hcca);
-    if (hcca == NULL) {
-        return;
-    }
-    hcca->enqueue(m_pED);
-}
-
-void IsochronousEp::reset(int delay_ms)
-{
-    m_FrameNumber = LPC_USB->HcFmNumber + delay_ms;
-}
-
-HCITD* IsochronousEp::new_HCITD(BaseEp* obj)
-{
-    HCITD* itd = new(m_PacketSize*m_FrameCount)HCITD(obj, m_FrameNumber, m_FrameCount, m_PacketSize);
-    if (itd == NULL) {
-        return NULL;
-    } 
-    m_FrameNumber += m_FrameCount;
-    return itd;
-}
-
-HCITD* IsochronousEp::isochronousReceive(int millisec)
-{
-    TEST_ASSERT(m_itd_queue_count >= 0);
-    while(m_itd_queue_count < 3 && m_itd_queue_count < HCTD_QUEUE_SIZE) {
-        TEST_ASSERT(m_pED);
-        if (m_pED->Skip()) {
-            break;
-        }
-        HCITD* blank_itd = new_HCITD(this);
-        TEST_ASSERT(blank_itd);
-        if (m_pED->enqueue<HCITD>(blank_itd)) {
-            m_itd_queue_count++;
-        }
-        enable(); // Enable Periodic
-    }
-    
-    HCITD* itd = get_queue_HCITD(millisec);
-    if (itd) {
-        m_itd_queue_count--;
-    }
-    return itd;
-}
-
-HCITD* IsochronousEp::get_queue_HCITD(int millisec)
-{
-    for(int i = 0; i < 16; i++) {
-        osEvent evt = m_queue.get(millisec);
-        if (evt.status == osEventMessage) {
-            HCITD* itd = reinterpret_cast<HCITD*>(evt.value.p);
-            TEST_ASSERT(itd);
-            return itd;
-        } else if (evt.status == osOK) {
-            continue;
-        } else if (evt.status == osEventTimeout) {
-            return NULL;
-        } else {
-            ISO_DBG("evt.status: %02x\n", evt.status);
-            TEST_ASSERT(evt.status == osEventMessage);
-            return NULL;
-        }
-    }
-    return NULL;
-}
-
-void IsochronousEp::enable()
-{
-    LPC_USB->HcControl |= OR_CONTROL_PLE;
-}
-
-void IsochronousEp::disconnect()
-{
-    m_pED->setSkip();
-    Thread::wait(50);
-    ISO_DBG("rtos-queue: %d", m_itd_queue_count);
-    int queue_count = m_itd_queue_count;
-    Timer t;
-    t.reset();
-    t.start();
-    while(queue_count > 0 && t.read_ms() < 50) {
-        HCITD* itd = get_queue_HCITD(10);
-        if (itd) {
-            ISO_DBG("delete ITD:%p from rtos-queue %d ms", itd, t.read_ms());
-            delete itd;
-            queue_count--;
-            t.reset();
-        }
-    }
-    ISO_DBG("rtos-queue: %d, %d ms", queue_count, t.read_ms());
-    while(1) {
-        HCITD* itd = m_pED->dequeue<HCITD>();
-        if (itd == NULL) {
-            break;
-        }
-        ISO_DBG("delete ITD:%p from ED(%p)-queue", itd, m_pED);        
-        delete itd;
-        TEST_ASSERT(queue_count > 0);
-        queue_count--;
-    }            
-    TEST_ASSERT(queue_count == 0);
-    HCITD* tail = reinterpret_cast<HCITD*>(m_pED->TailTd);
-    ISO_DBG("delete ITD:%p from ED(%p)-tail", tail, m_pED);
-    TEST_ASSERT(tail);
-    delete tail;
-    m_pED->init_queue<HCITD>(NULL);
-    
-    _HCCA* hcca = reinterpret_cast<_HCCA*>(LPC_USB->HcHCCA);
-    TEST_ASSERT(hcca);
-    hcca->dequeue(m_pED);
-    ISO_DBG("delete ED:%p", m_pED);
-    delete m_pED;
-    m_pED = NULL;
-}
-
-void BaseEp::init(int addr, uint8_t ep, uint16_t size, int lowSpeed)
-{
-    ISO_DBG("%p FA=%d EN=%02x MPS=%d S=%d\n", this, addr, ep, size, lowSpeed);
-    TEST_ASSERT(size >= 8 && size <= 1023);    
-    TEST_ASSERT(lowSpeed == 0 || lowSpeed == 1);
-    m_pED  = new _HCED(addr, ep, size, lowSpeed);
-    TEST_ASSERT(m_pED);
-    m_td_queue_count = 0;
-}

--- a/USBHostC270/BaseUvc.h	Mon Mar 18 12:34:47 2013 +0000
+++ b/USBHostC270/BaseUvc.h	Wed Mar 20 14:28:39 2013 +0000
@@ -1,5 +1,5 @@
 // BaseUvc.h
-
+#include "USBIsochronous.h"
 #pragma once
 
 // --- UVC --------------------------------------------------
@@ -23,230 +23,11 @@
 #define VS_PROBE_CONTROL  0x01
 #define VS_COMMIT_CONTROL 0x02
 
-class BaseEp;
-struct HCITD {    // HostController Isochronous Transfer Descriptor
-    __IO uint32_t Control;      // +0 Transfer descriptor control
-    uint8_t*  BufferPage0;      // +4 Buffer Page 0
-    HCITD* Next;                // +8 Physical pointer to next Isochronous Transfer Descriptor
-    uint8_t*  BufferEnd;        // +12 buffer End
-    __IO uint16_t OffsetPSW[8]; // +16 Offset/PSW
-    BaseEp* ep;                 // +32 endpoint object
-    __IO uint8_t buf[0];        // +36 buffer
-                                // +36
-    HCITD(BaseEp* obj, uint16_t FrameNumber, int FrameCount, uint16_t PacketSize);
-    inline void* operator new(size_t size, int buf_size) {
-        void* p;
-        if (posix_memalign(&p, 32, size+buf_size) == 0) {
-            return p;
-        }
-        return NULL;
-    }
-
-    inline void operator delete(void* p) {
-        free(p);
-    }
-
-    inline uint16_t StartingFrame() {
-        return Control & 0xffff;
-    }
-
-    inline uint8_t FrameCount() {
-        return ((Control>>24)&7)+1;
-    }    
-
-    inline uint8_t ConditionCode() {
-        return Control>>28;
-    }
-};
-
-struct _HCED {    // HostController EndPoint Descriptor
-    __IO uint32_t Control; // +0 Endpoint descriptor control
-    HCTD* TailTd;          // +4 Physical address of tail in Transfer descriptor list
-    __IO HCTD* HeadTd;     // +8 Physcial address of head in Transfer descriptor list
-    _HCED* Next;           // +12 Physical address of next Endpoint descriptor
-                           // +16
-    _HCED(int addr, uint8_t ep, uint16_t size, int lowSpeed) {
-        Control =  addr            | /* USB address */
-        ((ep & 0x7F) << 7)         | /* Endpoint address */
-        (ep!=0?(((ep&0x80)?2:1) << 11):0)| /* direction : Out = 1, 2 = In */
-        ((lowSpeed?1:0) << 13)     | /* speed full=0 low=1 */
-        (size << 16);                /* MaxPkt Size */
-        Next = NULL;
-    }
-
-    inline void* operator new(size_t size) {
-        void* p;
-        if (posix_memalign(&p, 16, size) == 0) {
-            return p;
-        }
-        return NULL;
-    }
-
-    inline void operator delete(void* p) {
-        free(p);
-    }
-
-    inline uint8_t FunctionAddress() {
-        return Control & 0x7f;
-    }
-
-    inline int Speed() {
-        return (Control>>13)&1;
-    }
-
-    inline void setFunctionAddress(int addr) {
-        Control &= ~0x7f;
-        Control |= addr;
-    }
-
-    inline void setMaxPacketSize(uint16_t size) {
-        Control &= ~0xffff0000;
-        Control |= size<<16;
-    }
-
-    int Skip() {
-        return (Control>>14) & 1;
-    }
-
-    void setSkip() {
-        Control |= (1<<14);
-    }
-
-    void setFormat() {
-        Control |= (1<<15);
-    }
-
-    template<typename T>
-    inline bool enqueue(T* td) {
-        if (td) {
-            T* tail = reinterpret_cast<T*>(TailTd);
-            if (tail) {
-                tail->Next = td;
-                TailTd = reinterpret_cast<HCTD*>(td);
-                return true;
-            }
-        }
-        return false;
-    }
-
-    template<typename T>
-    inline T* dequeue() {
-        T* head = reinterpret_cast<T*>(reinterpret_cast<uint32_t>(HeadTd)&~3); // delete Halted and Toggle Carry bit
-        T* tail = reinterpret_cast<T*>(TailTd);
-        if (head == NULL || tail == NULL || head == tail) {
-            return NULL;
-        }
-        HeadTd = reinterpret_cast<HCTD*>(head->Next);
-        return head;
-    }
-    template<typename T>
-    void init_queue(T* td) {
-        TailTd = reinterpret_cast<HCTD*>(td);
-        HeadTd = reinterpret_cast<HCTD*>(td); 
-    }
-};
-
-struct _HCCA {    // Host Controller Communication Area
-    _HCED* InterruptTable[32]; // +0 Interrupt Table
-    __IO uint16_t FrameNumber;// +128 Frame Number
-    __IO uint16_t Pad1;       // +130
-    __IO HCTD* DoneHead;      // +132 Done Head
-    uint8_t Reserved[116];    // +136 Reserved for future use
-    uint8_t Unknown[4];       // +252 Unused
-                              // +256
-    inline void* operator new(size_t size) {
-        void* p;
-        if (posix_memalign(&p, 256, size) == 0) {
-            return p;
-        }
-        return NULL;
-    }
-
-    inline void operator delete(void* p) {
-        free(p);
-    }
-    
-    inline void enqueue(_HCED* ed) {
-        for(int i = 0; i < 32; i++) {
-            if (InterruptTable[i] == NULL) {
-                InterruptTable[i] = ed;
-            } else {
-                _HCED* nextEd = InterruptTable[i];
-                while(nextEd->Next && nextEd->Next != ed) {
-                    nextEd = nextEd->Next;
-                }
-                nextEd->Next = ed;
-            }
-        }
-    }
-    
-    inline void dequeue(_HCED* ed) {
-         for(int i = 0; i < 32; i++) {
-            if (InterruptTable[i] == ed) {
-                InterruptTable[i] = ed->Next;
-            } else if (InterruptTable[i]) {
-                _HCED* nextEd = InterruptTable[i];
-                while(nextEd) {
-                    if (nextEd->Next == ed) {
-                        nextEd->Next = ed->Next;
-                        break;
-                    }
-                    nextEd = nextEd->Next;
-                }
-            }
-         }
-    }
-};
-
-#define HCTD_QUEUE_SIZE 3
-
-class BaseEp { // endpoint
-public:
-    void init(int addr, uint8_t ep = 0, uint16_t size = 8, int lowSpeed = 0);
-    int GetAddr();
-    int GetLowSpeed();
-    void update_FunctionAddress(int addr);
-    void update_MaxPacketSize(uint16_t size);
-    int transfer(uint8_t* buf, int len);
-    int status(uint32_t millisec=osWaitForever);
-    //
-    virtual void enable() = 0;
-    virtual void irqWdhHandler(HCTD* td) {m_queue.put(td);} // WDH
-    int wait_queue_HCTD(HCTD* wait_td, uint32_t millisec=osWaitForever);
-    // report
-    uint8_t m_ConditionCode;
-    int m_report_queue_error;
-protected:
-    int send_receive(uint8_t* buf, int len, int millisec);
-    HCTD* get_queue_HCTD(uint32_t millisec=osWaitForever);
-    _HCED* m_pED;
-    Queue<HCTD, HCTD_QUEUE_SIZE> m_queue; // TD done queue
-    int m_td_queue_count;
-};
-
-class IsochronousEp : public BaseEp {
-public:
-    void init(int addr, uint8_t ep, uint16_t size);
-    void reset(int delay_ms = 100);
-    HCITD* isochronousReceive(int millisec=osWaitForever);
-    int isochronousSend(uint8_t* buf, int len, int millisec=osWaitForever);
-    HCITD* get_queue_HCITD(int millisec);
-    uint16_t m_PacketSize;
-    void disconnect();
-private:
-    HCITD* new_HCITD(BaseEp* obj);
-    int m_itd_queue_count;
-    uint16_t m_FrameNumber;
-    int m_FrameCount; // 1-8
-    virtual void enable();
-};
-
 class BaseUvc {
 public:
-    void poll(int millisec=osWaitForever);
+    void poll(int millisec = 0);
     USB_TYPE Control(int req, int cs, int index, uint8_t* buf, int size);
     USB_TYPE setInterfaceAlternate(uint8_t intf, uint8_t alt);
-    //ControlEp* m_ctlEp;
     IsochronousEp* m_isoEp;
     uint32_t report_cc_count[16];  // ConditionCode
     uint32_t report_ps_cc_count[16]; // Packt Status ConditionCode

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/USBHostC270/USBisochronous/USBIsochronous.cpp	Wed Mar 20 14:28:39 2013 +0000
@@ -0,0 +1,212 @@
+// USBIsochronous.cpp
+#include "USBHostConf.h"
+#include "USBHost.h"
+#include "USBIsochronous.h"
+
+//#define ISO_DEBUG 1
+#ifdef ISO_DEBUG
+#define ISO_DBG(x, ...) std::printf("[%s:%d]"x"\r\n", __PRETTY_FUNCTION__, __LINE__, ##__VA_ARGS__);
+#else
+#define ISO_DBG(...)  while(0);
+#endif
+
+#define TEST_ASSERT(A) while(!(A)){fprintf(stderr,"\n\n%s@%d %s ASSERT!\n\n",__PRETTY_FUNCTION__,__LINE__,#A);exit(1);};
+
+HCITD::HCITD(IsochronousEp* obj, uint16_t FrameNumber, int FrameCount, uint16_t PacketSize) {
+    Control = 0xe0000000           | // CC ConditionCode NOT ACCESSED
+             ((FrameCount-1) << 24)| // FC FrameCount
+                  TD_DELAY_INT(0)  | // DI DelayInterrupt
+                 FrameNumber;        // SF StartingFrame
+    BufferPage0 = const_cast<uint8_t*>(buf);
+    BufferEnd = const_cast<uint8_t*>(buf) + PacketSize * FrameCount - 1;
+    Next = NULL; 
+    ep = obj;
+    uint32_t addr = reinterpret_cast<uint32_t>(buf);
+    for(int i = 0; i < FrameCount; i++) {
+        uint16_t offset = addr & 0x0fff;
+        if ((addr&0xfffff000) == (reinterpret_cast<uint32_t>(BufferEnd)&0xfffff000)) {
+            offset |= 0x1000;
+        }
+        OffsetPSW[i] = 0xe000|offset;
+        addr += PacketSize;
+    }
+}
+
+void IsochronousEp::init(int addr, uint8_t ep, uint16_t size, uint8_t frameCount, uint8_t queueLimit)
+{
+    //ISO_DBG("%p FA:%d EP:%02X MPS:%d\n", this, addr, ep, size);
+    TEST_ASSERT(addr >= 1);    
+    TEST_ASSERT(size >= 8 && size <= 1023);    
+    m_pED  = new _HCED(addr, ep, size);
+    TEST_ASSERT(m_pED);
+    
+    m_pED->setFormat(); // F Format ITD
+
+    TEST_ASSERT(size >= 128 && size <= 1023);
+    m_PacketSize = size;
+    TEST_ASSERT(frameCount >= 1 && frameCount <= 8);
+    m_FrameCount = frameCount;
+    TEST_ASSERT(queueLimit >= 1 && queueLimit <= HCITD_QUEUE_SIZE);
+    m_itd_queue_limit = queueLimit;
+    
+    m_itd_queue_count = 0;
+    reset();
+    HCITD* itd = new_HCITD(this);
+    m_pED->init_queue<HCITD>(itd); 
+    TEST_ASSERT(itd);
+    if (itd == NULL) {
+        return;
+    }
+    _HCCA* hcca = reinterpret_cast<_HCCA*>(LPC_USB->HcHCCA);
+    TEST_ASSERT(hcca);
+    if (hcca == NULL) {
+        return;
+    }
+    hcca->enqueue(m_pED);
+}
+
+void IsochronousEp::reset(int delay_ms)
+{
+    m_FrameNumber = LPC_USB->HcFmNumber + delay_ms;
+}
+
+HCITD* IsochronousEp::new_HCITD(IsochronousEp* obj)
+{
+    HCITD* itd = new(m_PacketSize*m_FrameCount)HCITD(obj, m_FrameNumber, m_FrameCount, m_PacketSize);
+    if (itd == NULL) {
+        return NULL;
+    } 
+    m_FrameNumber += m_FrameCount;
+    return itd;
+}
+
+HCITD* IsochronousEp::isochronousReceive(int timeout_ms)
+{
+    TEST_ASSERT(m_itd_queue_count >= 0);
+    while(m_itd_queue_count < m_itd_queue_limit) {
+        if (m_pED == NULL) {
+            ISO_DBG("m_pED is NULL");
+            break;
+        }
+        if (m_pED->Skip()) {
+            break;
+        }
+        HCITD* blank_itd = new_HCITD(this);
+        TEST_ASSERT(blank_itd);
+        if (m_pED->enqueue<HCITD>(blank_itd)) {
+            m_itd_queue_count++;
+        }
+        enable(); // Enable Periodic
+    }
+    
+    HCITD* itd = get_queue_HCITD(timeout_ms);
+    if (itd) {
+        m_itd_queue_count--;
+    }
+    return itd;
+}
+
+int IsochronousEp::isochronousSend(uint8_t* buf, int len, int timeout_ms)
+{
+    //ISO_DBG("buf: %p, len: %d", buf, len);
+    HCITD* itd = get_queue_HCITD(timeout_ms);
+    if (itd) {
+        delete itd;
+        m_itd_queue_count--;
+        TEST_ASSERT(m_itd_queue_count >= 0);
+    }
+    TEST_ASSERT(m_itd_queue_count >= 0);
+    if(m_itd_queue_count < m_itd_queue_limit) {
+        if (m_pED == NULL) {
+            ISO_DBG("m_pED is NULL");
+            return 0;
+        }
+        if (m_pED->Skip()) {
+            return 0;
+        }
+        itd = new_HCITD(this);
+        TEST_ASSERT(itd);
+        //ISO_DBG("m_pED: %p itd: %p", m_pED, itd);
+        memcpy(const_cast<uint8_t*>(itd->buf), buf, len);
+        if (m_pED->enqueue<HCITD>(itd)) {
+            m_itd_queue_count++;
+        }
+        enable(); // Enable Periodic
+        //ISO_DBG("m_itd_queue_count: %d", m_itd_queue_count);
+        return len;
+    }
+    return 0;
+}
+
+HCITD* IsochronousEp::get_queue_HCITD(int timeout_ms)
+{
+    Timer t;
+    t.reset();
+    t.start();
+    do {
+        osEvent evt = m_queue.get(0);
+        if (evt.status == osEventMessage) {
+            HCITD* itd = reinterpret_cast<HCITD*>(evt.value.p);
+            TEST_ASSERT(itd);
+            return itd;
+        } else if (evt.status == osOK) {
+            ;
+        } else if (evt.status == osEventTimeout) {
+            break;
+        } else {
+            ISO_DBG("evt.status: %02x\n", evt.status);
+            TEST_ASSERT(evt.status == osEventMessage);
+            break;
+        }
+    } while(t.read_ms() < timeout_ms);
+    return NULL;
+}
+
+void IsochronousEp::enable()
+{
+    LPC_USB->HcControl |= OR_CONTROL_PLE;
+}
+
+void IsochronousEp::disconnect()
+{
+    m_pED->setSkip(); // skip bit on
+    ISO_DBG("rtos-queue: %d", m_itd_queue_count);
+    int queue_count = m_itd_queue_count;
+    Timer t;
+    t.reset();
+    t.start();
+    do {
+        HCITD* itd = get_queue_HCITD(10);
+        if (itd) {
+            ISO_DBG("delete ITD:%p from rtos-queue %d ms", itd, t.read_ms());
+            delete itd;
+            queue_count--;
+            t.reset();
+        }
+    } while(t.read_ms() < 50);
+    ISO_DBG("rtos-queue: %d, %d ms", queue_count, t.read_ms());
+    TEST_ASSERT(queue_count >= 0);
+    while(1) {
+        HCITD* itd = m_pED->dequeue<HCITD>();
+        if (itd == NULL) {
+            break;
+        }
+        ISO_DBG("delete ITD:%p from ED(%p)-queue", itd, m_pED);        
+        delete itd;
+        TEST_ASSERT(queue_count > 0);
+        queue_count--;
+    }            
+    TEST_ASSERT(queue_count == 0);
+    HCITD* tail = reinterpret_cast<HCITD*>(m_pED->TailTd);
+    ISO_DBG("delete ITD:%p from ED(%p)-tail", tail, m_pED);
+    TEST_ASSERT(tail);
+    delete tail;
+    m_pED->init_queue<HCITD>(NULL);
+    
+    _HCCA* hcca = reinterpret_cast<_HCCA*>(LPC_USB->HcHCCA);
+    TEST_ASSERT(hcca);
+    hcca->dequeue(m_pED);
+    ISO_DBG("delete ED:%p", m_pED);
+    delete m_pED;
+    m_pED = NULL;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/USBHostC270/USBisochronous/USBIsochronous.h	Wed Mar 20 14:28:39 2013 +0000
@@ -0,0 +1,200 @@
+// USBIsochronous.h
+#pragma once
+
+class IsochronousEp;
+struct HCITD {    // HostController Isochronous Transfer Descriptor
+    __IO uint32_t Control;      // +0 Transfer descriptor control
+    uint8_t*  BufferPage0;      // +4 Buffer Page 0
+    HCITD* Next;                // +8 Physical pointer to next Isochronous Transfer Descriptor
+    uint8_t*  BufferEnd;        // +12 buffer End
+    __IO uint16_t OffsetPSW[8]; // +16 Offset/PSW
+    IsochronousEp* ep;          // +32 endpoint object
+    __IO uint8_t buf[0];        // +36 buffer
+                                // +36
+    HCITD(IsochronousEp* obj, uint16_t FrameNumber, int FrameCount, uint16_t PacketSize);
+    inline void* operator new(size_t size, int buf_size) {
+        void* p;
+        if (posix_memalign(&p, 32, size+buf_size) == 0) {
+            return p;
+        }
+        return NULL;
+    }
+
+    inline void operator delete(void* p) {
+        free(p);
+    }
+
+    inline uint16_t StartingFrame() {
+        return Control & 0xffff;
+    }
+
+    inline uint8_t FrameCount() {
+        return ((Control>>24)&7)+1;
+    }    
+
+    inline uint8_t ConditionCode() {
+        return Control>>28;
+    }
+};
+
+struct _HCED {    // HostController EndPoint Descriptor
+    __IO uint32_t Control; // +0 Endpoint descriptor control
+    HCTD* TailTd;          // +4 Physical address of tail in Transfer descriptor list
+    __IO HCTD* HeadTd;     // +8 Physcial address of head in Transfer descriptor list
+    _HCED* Next;           // +12 Physical address of next Endpoint descriptor
+                           // +16
+    _HCED(int addr, uint8_t ep, uint16_t size, int lowSpeed = 0) {
+        Control =  addr            | /* USB address */
+        ((ep & 0x7F) << 7)         | /* Endpoint address */
+        (ep!=0?(((ep&0x80)?2:1) << 11):0)| /* direction : Out = 1, 2 = In */
+        ((lowSpeed?1:0) << 13)     | /* speed full=0 low=1 */
+        (size << 16);                /* MaxPkt Size */
+        Next = NULL;
+    }
+
+    inline void* operator new(size_t size) {
+        void* p;
+        if (posix_memalign(&p, 16, size) == 0) {
+            return p;
+        }
+        return NULL;
+    }
+
+    inline void operator delete(void* p) {
+        free(p);
+    }
+
+    inline uint8_t FunctionAddress() {
+        return Control & 0x7f;
+    }
+
+    inline int Speed() {
+        return (Control>>13)&1;
+    }
+
+    inline void setFunctionAddress(int addr) {
+        Control &= ~0x7f;
+        Control |= addr;
+    }
+
+    inline void setMaxPacketSize(uint16_t size) {
+        Control &= ~0xffff0000;
+        Control |= size<<16;
+    }
+
+    int Skip() {
+        return (Control>>14) & 1;
+    }
+
+    void setSkip() {
+        Control |= (1<<14);
+    }
+
+    void setFormat() {
+        Control |= (1<<15);
+    }
+
+    template<typename T>
+    inline bool enqueue(T* td) {
+        if (td) {
+            T* tail = reinterpret_cast<T*>(TailTd);
+            if (tail) {
+                tail->Next = td;
+                TailTd = reinterpret_cast<HCTD*>(td);
+                return true;
+            }
+        }
+        return false;
+    }
+
+    template<typename T>
+    inline T* dequeue() {
+        T* head = reinterpret_cast<T*>(reinterpret_cast<uint32_t>(HeadTd)&~3); // delete Halted and Toggle Carry bit
+        T* tail = reinterpret_cast<T*>(TailTd);
+        if (head == NULL || tail == NULL || head == tail) {
+            return NULL;
+        }
+        HeadTd = reinterpret_cast<HCTD*>(head->Next);
+        return head;
+    }
+    template<typename T>
+    void init_queue(T* td) {
+        TailTd = reinterpret_cast<HCTD*>(td);
+        HeadTd = reinterpret_cast<HCTD*>(td); 
+    }
+};
+
+struct _HCCA {    // Host Controller Communication Area
+    _HCED* InterruptTable[32]; // +0 Interrupt Table
+    __IO uint16_t FrameNumber;// +128 Frame Number
+    __IO uint16_t Pad1;       // +130
+    __IO HCTD* DoneHead;      // +132 Done Head
+    uint8_t Reserved[116];    // +136 Reserved for future use
+    uint8_t Unknown[4];       // +252 Unused
+                              // +256
+    inline void* operator new(size_t size) {
+        void* p;
+        if (posix_memalign(&p, 256, size) == 0) {
+            return p;
+        }
+        return NULL;
+    }
+
+    inline void operator delete(void* p) {
+        free(p);
+    }
+    
+    inline void enqueue(_HCED* ed) {
+        for(int i = 0; i < 32; i++) {
+            if (InterruptTable[i] == NULL) {
+                InterruptTable[i] = ed;
+            } else {
+                _HCED* nextEd = InterruptTable[i];
+                while(nextEd->Next && nextEd->Next != ed) {
+                    nextEd = nextEd->Next;
+                }
+                nextEd->Next = ed;
+            }
+        }
+    }
+    
+    inline void dequeue(_HCED* ed) {
+         for(int i = 0; i < 32; i++) {
+            if (InterruptTable[i] == ed) {
+                InterruptTable[i] = ed->Next;
+            } else if (InterruptTable[i]) {
+                _HCED* nextEd = InterruptTable[i];
+                while(nextEd) {
+                    if (nextEd->Next == ed) {
+                        nextEd->Next = ed->Next;
+                        break;
+                    }
+                    nextEd = nextEd->Next;
+                }
+            }
+         }
+    }
+};
+
+#define HCITD_QUEUE_SIZE 3
+
+class IsochronousEp {
+public:
+    void init(int addr, uint8_t ep, uint16_t size, uint8_t frameCount = 4, uint8_t queueLimit = HCITD_QUEUE_SIZE);
+    void reset(int delay_ms = 100);
+    HCITD* isochronousReceive(int timeout_ms);
+    int isochronousSend(uint8_t* buf, int len, int timeout_ms);
+    HCITD* get_queue_HCITD(int timeout_ms);
+    uint16_t m_PacketSize;
+    void disconnect();
+    void irqWdhHandler(HCITD* itd) {m_queue.put(itd);} // WDH
+private:
+    HCITD* new_HCITD(IsochronousEp* obj);
+    Queue<HCITD, HCITD_QUEUE_SIZE> m_queue; // ITD done queue
+    int m_itd_queue_count;
+    int m_itd_queue_limit;
+    uint16_t m_FrameNumber;
+    int m_FrameCount; // 1-8
+    void enable();
+    _HCED* m_pED;
+};

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/USBHostCam/CamInfo.cpp	Wed Mar 20 14:28:39 2013 +0000
@@ -0,0 +1,193 @@
+// CamInfo.cpp
+#include "USBHostCam.h"
+
+// Logitech C270
+#define C270_VID 0x046d
+#define C270_PID 0x0825
+#define C270_160x120 2
+#define C270_176x144 3
+#define C270_320x176 4
+#define C270_320x240 5
+#define C270_352x288 6
+#define C270_432x240 7
+#define C270_640x480 1
+#define C270_544x288 8
+#define C270_640x360 9
+#define C270_752x416 10
+#define C270_800x448 11
+#define C270_800x600 12
+
+#define C270_MJPEG 2
+#define C270_YUV2  1
+
+#define C270_EN  0x81
+#define C270_MPS  192
+#define C270_IF_ALT_192 1
+#define C270_IF_ALT(A) C270_IF_ALT_##A
+
+#define C270_INFO(SIZE) {C270_VID, C270_PID, _##SIZE, 0, \
+    "C270", \
+    C270_MJPEG, \
+    C270_##SIZE, \
+    _5FPS, \
+    C270_EN, \
+    192, \
+    C270_IF_ALT(192), \
+    4, \
+    3}
+
+#define C210_PID 0x819
+#define C210_INFO(SIZE) {C270_VID, C210_PID, _##SIZE, 0, \
+    "C270", \
+    C270_MJPEG, \
+    C270_##SIZE, \
+    _5FPS, \
+    C270_EN, \
+    192, \
+    C270_IF_ALT(192), \
+    4, \
+    3}
+
+// Logitech Qcam Orbit AF QCAM-200R
+#define Q200R_VID 0x046d
+#define Q200R_PID 0x0994
+#define Q200R_160x120 1
+#define Q200R_176x144 2
+#define Q200R_320x240 3
+#define Q200R_352x288 4
+#define Q200R_640x480 5
+#define Q200R_800x600 6
+
+#define Q200R_MJPEG 1
+#define Q200R_YUV2  2
+
+#define Q200R_EN  0x81
+#define Q200R_MPS  192
+#define Q200R_IF_ALT_192 1
+#define Q200R_IF_ALT_384 2
+#define Q200R_IF_ALT_512 3
+#define Q200R_IF_ALT_640 4
+#define Q200R_IF_ALT_800 5
+#define Q200R_IF_ALT_944 6
+#define Q200R_IF_ALT(A) Q200R_IF_ALT_##A
+#define Q200R_INFO(SIZE) {Q200R_VID, Q200R_PID, _##SIZE, 0, \
+    "Q200R", \
+    Q200R_MJPEG, \
+    Q200R_##SIZE, \
+    _5FPS, \
+    Q200R_EN, \
+    192, \
+    Q200R_IF_ALT(192), \
+    4, \
+    3}
+
+//LifeCam VX700 / VX500
+#define VX700_VID 0x045e
+#define VX700_PID 0x074a
+
+#define VX700_160x120 5
+#define VX700_176x144 4
+#define VX700_320x240 3
+#define VX700_352x288 2
+#define VX700_640x480 1
+
+#define VX700_MJPEG 1
+
+#define VX700_EN  0x81
+#define VX700_MPS  128
+#define VX700_IF_ALT_128 1 
+#define VX700_IF_ALT(A) VX700_IF_ALT_##A
+#define VX700_INFO(SIZE) {VX700_VID, VX700_PID, _##SIZE, 0, \
+    "VX700", \
+    VX700_MJPEG, \
+    VX700_##SIZE, \
+    _5FPS, \
+    VX700_EN, \
+    128, \
+    VX700_IF_ALT(128), \
+    4, \
+    3}
+
+//Sonix USB 2.0 Camera
+#define SONIX_160x120 5
+#define SONIX_176x144 4
+#define SONIX_320x240 3
+#define SONIX_352x288 2
+#define SONIX_640x480 1
+
+#define SONIX_IF_ALT_128 1
+#define SONIX_IF_ALT_256 2
+#define SONIX_IF_ALT_512 3
+#define SONIX_IF_ALT_600 4
+#define SONIX_IF_ALT_800 5
+#define SONIX_IF_ALT_956 6
+#define SONIX_IF_ALT(A) SONIX_IF_ALT_##A
+#define SONIX_INFO(SIZE) {0x0c45, 0x62c0, _##SIZE, 0, \
+    "SONIX", \
+    1, \
+    SONIX_##SIZE, \
+    _5FPS, \
+    0x81, \
+    128, \
+    SONIX_IF_ALT(128), \
+    4, \
+    3}
+
+static const CamInfo CamInfoList[] = {
+// Logitech C270
+C270_INFO(160x120),
+C270_INFO(176x144),
+C270_INFO(320x176),
+C270_INFO(320x240),
+C270_INFO(352x288),
+C270_INFO(432x240),
+C270_INFO(640x480),
+C270_INFO(544x288),
+C270_INFO(640x360),
+C270_INFO(752x416),
+C270_INFO(800x448),
+C270_INFO(800x600),
+
+// Logitech C210
+C210_INFO(160x120),
+C210_INFO(176x144),
+C210_INFO(320x176),
+C210_INFO(320x240),
+C210_INFO(352x288),
+C210_INFO(432x240),
+C210_INFO(640x480),
+C210_INFO(544x288),
+C210_INFO(640x360),
+C210_INFO(752x416),
+C210_INFO(800x448),
+C210_INFO(800x600),
+
+// Logitech Qcam Orbit AF QCAM-200R
+Q200R_INFO(160x120),
+Q200R_INFO(176x144),
+Q200R_INFO(320x240),
+Q200R_INFO(352x288),
+Q200R_INFO(640x480),
+Q200R_INFO(800x600),
+
+// LifeCam VX700
+VX700_INFO(160x120),
+VX700_INFO(176x144),
+VX700_INFO(320x240),
+VX700_INFO(352x288),
+VX700_INFO(640x480),
+
+// Sonix USB 2.0 Camera
+SONIX_INFO(160x120),
+SONIX_INFO(176x144),
+SONIX_INFO(320x240),
+SONIX_INFO(352x288),
+SONIX_INFO(640x480),
+
+// Not found
+{0,0,0,0},
+};
+
+CamInfo* getCamInfoList() {
+    return const_cast<CamInfo*>(CamInfoList);
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/USBHostCam/USBHostCam.cpp	Wed Mar 20 14:28:39 2013 +0000
@@ -0,0 +1,176 @@
+// USBHostCam.cpp
+#include "USBHostCam.h"
+#include "dbg.h"
+
+//#define CAM_DEBUG 1
+#ifdef CAM_DEBUG
+#define CAM_DBG(x, ...) std::printf("[%s:%d]"x"\r\n", __PRETTY_FUNCTION__, __LINE__, ##__VA_ARGS__);
+#else
+#define CAM_DBG(...)  while(0);
+#endif
+
+// ------------------ HcControl Register ---------------------
+#define  OR_CONTROL_IE                  0x00000008
+
+CamInfo* getCamInfoList(); // CamInfo.cpp
+
+USBHostCam::USBHostCam(uint8_t size, uint8_t option, CamInfo* user_caminfo)
+{
+    CAM_DBG("size: %d, option: %d", size, option);
+    _caminfo_size = size;
+    _caminfo_option = option;
+    if (user_caminfo) {
+        CamInfoList = user_caminfo;
+    } else {
+        CamInfoList = getCamInfoList();
+    }
+    clearOnResult();
+    host = USBHost::getHostInst();
+    m_isoEp = new IsochronousEp;
+    init();
+}
+
+void USBHostCam::init()
+{
+    CAM_DBG("");
+    dev_connected = false;
+    dev = NULL;
+    cam_intf = -1;
+    device_found = false;
+    caminfo_found = false;
+}
+
+bool USBHostCam::connected()
+{
+    return dev_connected;
+}
+
+bool USBHostCam::connect()
+{
+    if (dev_connected) {
+        return true;
+    }
+
+    for (uint8_t i = 0; i < MAX_DEVICE_CONNECTED; i++) {
+        if ((dev = host->getDevice(i)) != NULL) {
+            
+            CAM_DBG("Trying to connect Cam device\r\n");
+            
+            if(host->enumerate(dev, this)) {
+                break;
+            }
+            if (device_found) {
+                USB_INFO("New Cam: %s device: VID:%04x PID:%04x [dev: %p - intf: %d]", caminfo->name, dev->getVid(), dev->getPid(), dev, cam_intf);
+                dev->setName(caminfo->name, cam_intf);
+                host->registerDriver(dev, cam_intf, this, &USBHostCam::onDisconnect);
+                int addr = dev->getAddress();
+                m_isoEp->init(addr, caminfo->en, caminfo->mps, caminfo->frameCount, caminfo->queueLimit);
+                uint8_t buf[26];
+                memset(buf, 0, sizeof(buf));
+                buf[2] = caminfo->formatIndex;
+                buf[3] = caminfo->frameIndex;
+                *reinterpret_cast<uint32_t*>(buf+4) = caminfo->interval;
+                USB_TYPE res = Control(SET_CUR, VS_COMMIT_CONTROL, 1, buf, sizeof(buf));
+                if (res != USB_TYPE_OK) {
+                    CAM_DBG("SET_CUR VS_COMMIT_CONTROL FAILED");
+                }
+                res = setInterfaceAlternate(1, caminfo->if_alt);
+                if (res != USB_TYPE_OK) {
+                    CAM_DBG("SET_INTERFACE FAILED");
+                }
+                for(int i = 0; i < 16; i++) {
+                    report_cc_count[i] = 0;
+                    report_ps_cc_count[i] = 0;
+                }
+                LPC_USB->HcControl |= OR_CONTROL_PLE; // PeriodicListEnable
+                LPC_USB->HcControl |= OR_CONTROL_IE;  // IsochronousEnable
+
+                dev_connected = true;
+                return true;
+            }
+        }
+    }
+    init();
+    return false;
+}
+
+void USBHostCam::onDisconnect()
+{
+    CAM_DBG("dev_connected: %d", dev_connected);
+    if (dev_connected) {
+        m_isoEp->disconnect();
+        init();
+    }
+}
+
+/*virtual*/ void USBHostCam::setVidPid(uint16_t vid, uint16_t pid)
+{
+    CAM_DBG("vid:%04x,pid:%04x", vid, pid);
+    caminfo = CamInfoList;
+    while(caminfo->vid != 0) {
+        if (caminfo->vid == vid && caminfo->pid == pid && 
+            caminfo->size == _caminfo_size && caminfo->option == _caminfo_option) {
+            caminfo_found = true;
+            break;
+        }
+        caminfo++;
+    }
+}
+
+/*virtual*/ bool USBHostCam::parseInterface(uint8_t intf_nb, uint8_t intf_class, uint8_t intf_subclass, uint8_t intf_protocol) //Must return true if the interface should be parsed
+{
+    CAM_DBG("intf_nb=%d,intf_class=%02X,intf_subclass=%d,intf_protocol=%d", intf_nb, intf_class, intf_subclass, intf_protocol);
+    if ((cam_intf == -1) && caminfo_found) {
+        cam_intf = intf_nb;
+        device_found = true;
+        return true;
+    }
+    return false;
+}
+
+/*virtual*/ bool USBHostCam::useEndpoint(uint8_t intf_nb, ENDPOINT_TYPE type, ENDPOINT_DIRECTION dir) //Must return true if the endpoint will be used
+{
+    CAM_DBG("intf_nb:%d,type:%d,dir:%d",intf_nb, type, dir);
+    return false;
+}
+
+#define SEQ_READ_IDOL 0
+#define SEQ_READ_EXEC 1
+#define SEQ_READ_DONE 2
+
+int USBHostCam::readJPEG(uint8_t* buf, int size, int timeout_ms) {
+    _buf = buf;
+    _pos = 0;
+    _size = size;
+    _seq = SEQ_READ_IDOL;
+    setOnResult(this, &USBHostCam::callback_motion_jpeg);
+    Timer timeout_t;
+    timeout_t.reset();
+    timeout_t.start();
+    while(timeout_t.read_ms() < timeout_ms && _seq != SEQ_READ_DONE && connected()) {
+        poll();
+        Thread::wait(1);
+    } 
+    return _pos;
+}
+
+/* virtual */ void USBHostCam::outputJPEG(uint8_t c, int status) { // from decodeMJPEG
+    if (_seq == SEQ_READ_IDOL) {
+        if (status == JPEG_START) {
+            _pos = 0;
+            _seq = SEQ_READ_EXEC;
+        }
+    }
+    if (_seq == SEQ_READ_EXEC) {
+        if (_pos < _size) {
+            _buf[_pos++] = c;  
+        }  
+        if (status == JPEG_END) {
+            _seq = SEQ_READ_DONE;
+        }
+    }
+}
+
+void USBHostCam::callback_motion_jpeg(uint16_t frame, uint8_t* buf, int len) {
+        inputPacket(buf, len);
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/USBHostCam/USBHostCam.h	Wed Mar 20 14:28:39 2013 +0000
@@ -0,0 +1,101 @@
+// USBHostCam.h
+#include "USBHostConf.h"
+#include "USBHost.h"
+#include "USBIsochronous.h"
+#include "BaseUvc.h"
+#include "decodeMJPEG.h"
+#pragma once
+
+#define _160x120 2
+#define _176x144 3
+#define _320x176 4
+#define _320x240 5
+#define _352x288 6
+#define _432x240 7
+#define _640x480 1
+#define _544x288 8
+#define _640x360 9
+#define _752x416 10
+#define _800x448 11
+#define _800x600 12
+
+#define TEST_ASSERT(A) while(!(A)){fprintf(stderr,"\n\n%s@%d %s ASSERT!\n\n",__PRETTY_FUNCTION__,__LINE__,#A);exit(1);};
+
+struct CamInfo {
+    uint16_t vid;
+    uint16_t pid;
+    uint8_t size;
+    uint8_t option;
+//
+    const char* name;
+    uint8_t formatIndex;
+    uint8_t frameIndex;
+    uint32_t interval;
+    uint8_t en;
+    uint8_t mps;
+    uint8_t if_alt;
+    uint8_t frameCount; // ITD frame count 1-8
+    uint8_t queueLimit; // ITD queue limit 1-3
+};
+
+/** 
+ * A class to communicate a Cam
+ */
+class USBHostCam : public IUSBEnumerator, public BaseUvc, public decodeMJPEG {
+public:
+    /**
+    * Constructor
+    *
+    */
+    USBHostCam(uint8_t size = _160x120, uint8_t option = 0, CamInfo* user_caminfo = NULL);
+
+    /**
+    * Check if a Cam device is connected
+    *
+    * @return true if a Cam device is connected
+    */
+    bool connected();
+
+    /**
+     * Try to connect to a Cam device
+     *
+     * @return true if connection was successful
+     */
+    bool connect();
+
+    /**
+     * read jpeg image
+     *
+     * @param buf read buffer 
+     * @param size buffer size 
+     * @param timeout_ms timeout default 15sec
+     * @return jpeg size if read success else -1
+     */
+    int readJPEG(uint8_t* buf, int size, int timeout_ms = 15*1000);
+
+protected:
+    //From IUSBEnumerator
+    virtual void setVidPid(uint16_t vid, uint16_t pid);
+    virtual bool parseInterface(uint8_t intf_nb, uint8_t intf_class, uint8_t intf_subclass, uint8_t intf_protocol); //Must return true if the interface should be parsed
+    virtual bool useEndpoint(uint8_t intf_nb, ENDPOINT_TYPE type, ENDPOINT_DIRECTION dir); //Must return true if the endpoint will be used
+
+private:
+    bool dev_connected;
+
+    int cam_intf;
+    bool device_found;
+    bool caminfo_found;
+    uint8_t _seq;
+    uint8_t* _buf;
+    int _pos;
+    int _size;
+    CamInfo* CamInfoList;
+    CamInfo* caminfo;
+    uint8_t _caminfo_size;
+    uint8_t _caminfo_option;
+
+    virtual void outputJPEG(uint8_t c, int status); // from decodeMJPEG
+    void callback_motion_jpeg(uint16_t frame, uint8_t* buf, int len);
+    void init();
+    void onDisconnect();
+};