USBIsochronous.cpp

00001 // USBIsochronous.cpp
00002 #include "USBHostConf.h"
00003 #include "USBHost.h"
00004 #include "USBIsochronous.h"
00005 
00006 //#define ISO_DEBUG 1
00007 #ifdef ISO_DEBUG
00008 #define ISO_DBG(x, ...) std::printf("[%s:%d]"x"\r\n", __PRETTY_FUNCTION__, __LINE__, ##__VA_ARGS__);
00009 #else
00010 #define ISO_DBG(...)  while(0);
00011 #endif
00012 
00013 #define TEST_ASSERT(A) while(!(A)){fprintf(stderr,"\n\n%s@%d %s ASSERT!\n\n",__PRETTY_FUNCTION__,__LINE__,#A);exit(1);};
00014 
00015 HCITD::HCITD(IsochronousEp* obj, uint16_t FrameNumber, int FrameCount, uint16_t PacketSize) {
00016     Control = 0xe0000000           | // CC ConditionCode NOT ACCESSED
00017              ((FrameCount-1) << 24)| // FC FrameCount
00018                   TD_DELAY_INT(0)  | // DI DelayInterrupt
00019                  FrameNumber;        // SF StartingFrame
00020     BufferPage0 = const_cast<uint8_t*>(buf);
00021     BufferEnd = const_cast<uint8_t*>(buf) + PacketSize * FrameCount - 1;
00022     Next = NULL; 
00023     ep = obj;
00024     uint32_t addr = reinterpret_cast<uint32_t>(buf);
00025     for(int i = 0; i < FrameCount; i++) {
00026         uint16_t offset = addr & 0x0fff;
00027         if ((addr&0xfffff000) == (reinterpret_cast<uint32_t>(BufferEnd)&0xfffff000)) {
00028             offset |= 0x1000;
00029         }
00030         OffsetPSW[i] = 0xe000|offset;
00031         addr += PacketSize;
00032     }
00033 }
00034 
00035 void IsochronousEp::init(int addr, uint8_t ep, uint16_t size, uint8_t frameCount, uint8_t queueLimit)
00036 {
00037     //ISO_DBG("%p FA:%d EP:%02X MPS:%d\n", this, addr, ep, size);
00038     TEST_ASSERT(addr >= 1);    
00039     TEST_ASSERT(size >= 8 && size <= 1023);    
00040     m_pED  = new _HCED(addr, ep, size);
00041     TEST_ASSERT(m_pED);
00042     
00043     m_pED->setFormat(); // F Format ITD
00044 
00045     TEST_ASSERT(size >= 128 && size <= 1023);
00046     m_PacketSize = size;
00047     TEST_ASSERT(frameCount >= 1 && frameCount <= 8);
00048     m_FrameCount = frameCount;
00049     TEST_ASSERT(queueLimit >= 1 && queueLimit <= HCITD_QUEUE_SIZE);
00050     m_itd_queue_limit = queueLimit;
00051     
00052     m_itd_queue_count = 0;
00053     reset();
00054     HCITD* itd = new_HCITD(this);
00055     m_pED->init_queue<HCITD>(itd); 
00056     TEST_ASSERT(itd);
00057     if (itd == NULL) {
00058         return;
00059     }
00060     _HCCA* hcca = reinterpret_cast<_HCCA*>(LPC_USB->HcHCCA);
00061     TEST_ASSERT(hcca);
00062     if (hcca == NULL) {
00063         return;
00064     }
00065     hcca->enqueue(m_pED);
00066 }
00067 
00068 void IsochronousEp::reset(int delay_ms)
00069 {
00070     m_FrameNumber = LPC_USB->HcFmNumber + delay_ms;
00071 }
00072 
00073 HCITD* IsochronousEp::new_HCITD(IsochronousEp* obj)
00074 {
00075     HCITD* itd = new(m_PacketSize*m_FrameCount)HCITD(obj, m_FrameNumber, m_FrameCount, m_PacketSize);
00076     if (itd == NULL) {
00077         return NULL;
00078     } 
00079     m_FrameNumber += m_FrameCount;
00080     return itd;
00081 }
00082 
00083 HCITD* IsochronousEp::isochronousReceive(int timeout_ms)
00084 {
00085     TEST_ASSERT(m_itd_queue_count >= 0);
00086     while(m_itd_queue_count < m_itd_queue_limit) {
00087         if (m_pED == NULL) {
00088             ISO_DBG("m_pED is NULL");
00089             break;
00090         }
00091         if (m_pED->Skip()) {
00092             break;
00093         }
00094         HCITD* blank_itd = new_HCITD(this);
00095         TEST_ASSERT(blank_itd);
00096         if (m_pED->enqueue<HCITD>(blank_itd)) {
00097             m_itd_queue_count++;
00098         }
00099         enable(); // Enable Periodic
00100     }
00101     
00102     HCITD* itd = get_queue_HCITD(timeout_ms);
00103     if (itd) {
00104         m_itd_queue_count--;
00105     }
00106     return itd;
00107 }
00108 
00109 int IsochronousEp::isochronousSend(uint8_t* buf, int len, int timeout_ms)
00110 {
00111     //ISO_DBG("buf: %p, len: %d", buf, len);
00112     HCITD* itd = get_queue_HCITD(timeout_ms);
00113     if (itd) {
00114         delete itd;
00115         m_itd_queue_count--;
00116         TEST_ASSERT(m_itd_queue_count >= 0);
00117     }
00118     TEST_ASSERT(m_itd_queue_count >= 0);
00119     if(m_itd_queue_count < m_itd_queue_limit) {
00120         if (m_pED == NULL) {
00121             ISO_DBG("m_pED is NULL");
00122             return 0;
00123         }
00124         if (m_pED->Skip()) {
00125             return 0;
00126         }
00127         itd = new_HCITD(this);
00128         TEST_ASSERT(itd);
00129         //ISO_DBG("m_pED: %p itd: %p", m_pED, itd);
00130         memcpy(const_cast<uint8_t*>(itd->buf), buf, len);
00131         if (m_pED->enqueue<HCITD>(itd)) {
00132             m_itd_queue_count++;
00133         }
00134         enable(); // Enable Periodic
00135         //ISO_DBG("m_itd_queue_count: %d", m_itd_queue_count);
00136         return len;
00137     }
00138     return 0;
00139 }
00140 
00141 HCITD* IsochronousEp::get_queue_HCITD(int timeout_ms)
00142 {
00143     Timer t;
00144     t.reset();
00145     t.start();
00146     do {
00147         osEvent evt = m_queue.get(0);
00148         if (evt.status == osEventMessage) {
00149             HCITD* itd = reinterpret_cast<HCITD*>(evt.value.p);
00150             TEST_ASSERT(itd);
00151             return itd;
00152         } else if (evt.status == osOK) {
00153             ;
00154         } else if (evt.status == osEventTimeout) {
00155             break;
00156         } else {
00157             ISO_DBG("evt.status: %02x\n", evt.status);
00158             TEST_ASSERT(evt.status == osEventMessage);
00159             break;
00160         }
00161     } while(t.read_ms() < timeout_ms);
00162     return NULL;
00163 }
00164 
00165 void IsochronousEp::enable()
00166 {
00167     LPC_USB->HcControl |= OR_CONTROL_PLE;
00168 }
00169 
00170 void IsochronousEp::disconnect()
00171 {
00172     m_pED->setSkip(); // skip bit on
00173     ISO_DBG("rtos-queue: %d", m_itd_queue_count);
00174     int queue_count = m_itd_queue_count;
00175     Timer t;
00176     t.reset();
00177     t.start();
00178     do {
00179         HCITD* itd = get_queue_HCITD(10);
00180         if (itd) {
00181             ISO_DBG("delete ITD:%p from rtos-queue %d ms", itd, t.read_ms());
00182             delete itd;
00183             queue_count--;
00184             t.reset();
00185         }
00186     } while(t.read_ms() < 50);
00187     ISO_DBG("rtos-queue: %d, %d ms", queue_count, t.read_ms());
00188     TEST_ASSERT(queue_count >= 0);
00189     while(1) {
00190         HCITD* itd = m_pED->dequeue<HCITD>();
00191         if (itd == NULL) {
00192             break;
00193         }
00194         ISO_DBG("delete ITD:%p from ED(%p)-queue", itd, m_pED);        
00195         delete itd;
00196         TEST_ASSERT(queue_count > 0);
00197         queue_count--;
00198     }            
00199     TEST_ASSERT(queue_count == 0);
00200     HCITD* tail = reinterpret_cast<HCITD*>(m_pED->TailTd);
00201     ISO_DBG("delete ITD:%p from ED(%p)-tail", tail, m_pED);
00202     TEST_ASSERT(tail);
00203     delete tail;
00204     m_pED->init_queue<HCITD>(NULL);
00205     
00206     _HCCA* hcca = reinterpret_cast<_HCCA*>(LPC_USB->HcHCCA);
00207     TEST_ASSERT(hcca);
00208     hcca->dequeue(m_pED);
00209     ISO_DBG("delete ED:%p", m_pED);
00210     delete m_pED;
00211     m_pED = NULL;
00212 }