USBIsochronous.cpp

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