File content as of revision 41:5c3ebf7372ee:

// USBIsochronous.cpp
#include "USBHostConf.h"
#include "USBHost.h"
#include "USBIsochronous.h"
#if defined(TARGET_RZ_A1H)
#include "ohci_wrapp_RZ_A1.h"
#endif

#define  OR_CONTROL_PLE                 0x00000004
#define  OR_CONTROL_IE                  0x00000008

//#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

    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;
    }
#if defined(TARGET_LPC1768)
    _HCCA* hcca = reinterpret_cast<_HCCA*>(LPC_USB->HcHCCA);
#elif defined(TARGET_RZ_A1H)
    _HCCA* hcca = reinterpret_cast<_HCCA*>(ohciwrapp_reg_r(OHCI_REG_HCCA));
#endif
    TEST_ASSERT(hcca);
    if (hcca == NULL) {
        return;
    }
    hcca->enqueue(m_pED);
}

void IsochronousEp::reset(int delay_ms)
{
#if defined(TARGET_LPC1768)
    m_FrameNumber = LPC_USB->HcFmNumber + delay_ms;
#elif defined(TARGET_RZ_A1H)
    m_FrameNumber = ohciwrapp_reg_r(OHCI_REG_FMNUMBER) + delay_ms;
#endif
}

HCITD* IsochronousEp::new_HCITD(IsochronousEp* obj) {
    HCITD* itd = new(m_PacketSize*m_FrameCount)HCITD(obj, 0, m_FrameCount, m_PacketSize);
    if (itd == NULL) {
        return NULL;
    } 
    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);
        ((HCITD *)m_pED->TailTd)->SetStartingFrame(m_FrameNumber);
        m_FrameNumber += m_FrameCount;
        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;

    if (m_itd_queue_count >= m_itd_queue_limit) {
        itd = get_queue_HCITD(timeout_ms);
    } else {
        itd = get_queue_HCITD(0);
    }
    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);
        ((HCITD *)m_pED->TailTd)->SetStartingFrame(m_FrameNumber);
        m_FrameNumber += m_FrameCount;
        memcpy(const_cast<uint8_t*>(((HCITD *)m_pED->TailTd)->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) {
    osEvent evt = m_queue.get(timeout_ms);
    if (evt.status == osEventMessage) {
        HCITD* itd = reinterpret_cast<HCITD*>(evt.value.p);
        TEST_ASSERT(itd);
        return itd;
    }
    return NULL;
}

void IsochronousEp::enable() {
#if defined(TARGET_LPC1768)
    LPC_USB->HcControl |= (OR_CONTROL_PLE | OR_CONTROL_IE);
#elif defined(TARGET_RZ_A1H)
    uint32_t data;

    data = ohciwrapp_reg_r(OHCI_REG_CONTROL) | OR_CONTROL_PLE | OR_CONTROL_IE;
    ohciwrapp_reg_w(OHCI_REG_CONTROL, data);
#endif
}

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);
    
#if defined(TARGET_LPC1768)
    _HCCA* hcca = reinterpret_cast<_HCCA*>(LPC_USB->HcHCCA);
#elif defined(TARGET_RZ_A1H)
    _HCCA* hcca = reinterpret_cast<_HCCA*>(ohciwrapp_reg_r(OHCI_REG_HCCA));
#endif
    TEST_ASSERT(hcca);
    hcca->dequeue(m_pED);
    ISO_DBG("delete ED:%p", m_pED);
    delete m_pED;
    m_pED = NULL;
}