// USBIsochronous.h
#pragma once
#if !defined (__CC_ARM) && (!defined (_POSIX_C_SOURCE) || (_POSIX_C_SOURCE < 200112L))
#if defined(__ICCARM__)
#include <iar_dlmalloc.h>
#define memalign   __iar_dlmemalign
#else
#include <malloc.h>
#endif
#endif

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
    uint8_t*  buf;              // +36 buffer
                                // +40
    HCITD(IsochronousEp* obj, uint16_t FrameNumber, int FrameCount, uint16_t PacketSize);
    inline void* operator new(size_t size, int buf_size) {
        void* p;
        struct HCITD * wk_hcitd;
        void* q;
#if !defined (__CC_ARM) && (!defined (_POSIX_C_SOURCE) || (_POSIX_C_SOURCE < 200112L))
        p = memalign(0x20, size);
        if (p != NULL) {
            q = memalign(0x1000, buf_size);
            if (q != NULL) {
                wk_hcitd = (struct HCITD *)p;
                wk_hcitd->buf = (uint8_t*)q;
            } else {
                free(p);
                p = NULL;
            }
        }
        return p;
#else
        if (posix_memalign(&p, 0x20, size) == 0) {
            if (posix_memalign(&q, 0x1000, buf_size) == 0) {
                wk_hcitd = (struct HCITD *)p;
                wk_hcitd->buf = (uint8_t*)q;
            } else {
                free(p);
                p = NULL;
            }
            return p;
        }
        return NULL;
#endif
    }

    inline void operator delete(void* p) {
        struct HCITD * wk_hcitd = (struct HCITD *)p;
        free((void *)wk_hcitd->buf);
        free(p);
    }

    inline uint16_t StartingFrame() {
        return Control & 0xffff;
    }

    inline void SetStartingFrame(uint16_t FrameNumber) {
        Control = (Control & 0xffff0000) | FrameNumber;
    }

    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 !defined (__CC_ARM) && (!defined (_POSIX_C_SOURCE) || (_POSIX_C_SOURCE < 200112L))
        p = memalign(16, size);
        return p;
#else
        if (posix_memalign(&p, 16, size) == 0) {
            return p;
        }
        return NULL;
#endif
    }

    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 !defined (__CC_ARM) && (!defined (_POSIX_C_SOURCE) || (_POSIX_C_SOURCE < 200112L))
        p = memalign(256, size);
        return p;
#else
        if (posix_memalign(&p, 256, size) == 0) {
            return p;
        }
        return NULL;
#endif
    }

    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 24

class IsochronousEp {
public:
    void init(int addr, uint8_t ep, uint16_t size, uint8_t frameCount = 4, uint8_t queueLimit = 3);
    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
    int getQueueNum() {return m_itd_queue_count;}
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;
};
