Example dwc2 microframe schedule test code

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

/* ****
 *
 * RANDOM CRAP FROM LINUX
 *
 * ****/

#define ENOSPC 28
typedef unsigned short u16;

#define dev_dbg(x, ...) do { if (0) printf(__VA_ARGS__); } while (0)

#define min_t(type, x, y) ({            \
type __min1 = (x);          \
type __min2 = (y);          \
__min1 < __min2 ? __min1: __min2; })

#define DIV_ROUND_UP(n,d) (((n) + (d) - 1) / (d))
#define ARRAY_SIZE(arr) (sizeof(arr) / sizeof((arr)[0]))


/* ****
 *
 * BITMAP CODE
 *
 * ****/

//#define BITS_PER_LONG (sizeof (unsigned long) * 8)
#define BITS_PER_LONG 64
#define BITOP_WORD(nr)      ((nr) / BITS_PER_LONG)
#define BIT_WORD(nr)        ((nr) / BITS_PER_LONG)

#define BITMAP_FIRST_WORD_MASK(start) (~0UL << ((start) % BITS_PER_LONG))
#define BITMAP_LAST_WORD_MASK(nbits)                    \
(                                   \
((nbits) % BITS_PER_LONG) ?                 \
(1UL<<((nbits) % BITS_PER_LONG))-1 : ~0UL       \
)


#define __ALIGN_KERNEL_MASK(x, mask)        (((x) + (mask)) & ~(mask))
#define __ALIGN_MASK(x, mask)    __ALIGN_KERNEL_MASK((x), (mask))

#define ffz(x)  __ffs(~(x))
static __always_inline unsigned long __ffs(unsigned long word)
{
    int num = 0;

    #if BITS_PER_LONG == 64
    if ((word & 0xffffffff) == 0) {
        num += 32;
        word >>= 32;
    }
    #endif
    if ((word & 0xffff) == 0) {
        num += 16;
        word >>= 16;
    }
    if ((word & 0xff) == 0) {
        num += 8;
        word >>= 8;
    }
    if ((word & 0xf) == 0) {
        num += 4;
        word >>= 4;
    }
    if ((word & 0x3) == 0) {
        num += 2;
        word >>= 2;
    }
    if ((word & 0x1) == 0)
        num += 1;
    return num;
}

unsigned long find_next_zero_bit(const unsigned long *addr, unsigned long size,
                 unsigned long offset)
{
    const unsigned long *p = addr + BITOP_WORD(offset);
    unsigned long result = offset & ~(BITS_PER_LONG-1);
    unsigned long tmp;

    if (offset >= size)
        return size;
    size -= result;
    offset %= BITS_PER_LONG;
    if (offset) {
        tmp = *(p++);
        tmp |= ~0UL >> (BITS_PER_LONG - offset);
        if (size < BITS_PER_LONG)
            goto found_first;
        if (~tmp)
            goto found_middle;
        size -= BITS_PER_LONG;
        result += BITS_PER_LONG;
    }
    while (size & ~(BITS_PER_LONG-1)) {
        if (~(tmp = *(p++)))
            goto found_middle;
        result += BITS_PER_LONG;
        size -= BITS_PER_LONG;
    }
    if (!size)
        return result;
    tmp = *p;

    found_first:
    tmp |= ~0UL << size;
    if (tmp == ~0UL)    /* Are any bits zero? */
        return result + size;   /* Nope. */
        found_middle:
        return result + ffz(tmp);
}

unsigned long find_next_bit(const unsigned long *addr, unsigned long size,
                unsigned long offset)
{
    const unsigned long *p = addr + BITOP_WORD(offset);
    unsigned long result = offset & ~(BITS_PER_LONG-1);
    unsigned long tmp;

    if (offset >= size)
        return size;
    size -= result;
    offset %= BITS_PER_LONG;
    if (offset) {
        tmp = *(p++);
        tmp &= (~0UL << offset);
        if (size < BITS_PER_LONG)
            goto found_first;
        if (tmp)
            goto found_middle;
        size -= BITS_PER_LONG;
        result += BITS_PER_LONG;
    }
    while (size & ~(BITS_PER_LONG-1)) {
        if ((tmp = *(p++)))
            goto found_middle;
        result += BITS_PER_LONG;
        size -= BITS_PER_LONG;
    }
    if (!size)
        return result;
    tmp = *p;

    found_first:
    tmp &= (~0UL >> (BITS_PER_LONG - size));
    if (tmp == 0UL)     /* Are any bits set? */
        return result + size;   /* Nope. */
        found_middle:
        return result + __ffs(tmp);
}

unsigned long bitmap_find_next_zero_area(unsigned long *map,
                     unsigned long size,
                     unsigned long start,
                     unsigned int nr,
                     unsigned long align_mask)
{
    unsigned long index, end, i;
    again:
    index = find_next_zero_bit(map, size, start);

    /* Align allocation */
    index = __ALIGN_MASK(index, align_mask);

    end = index + nr;
    if (end > size)
        return end;
    i = find_next_bit(map, end, index);
    if (i < end) {
        start = i + 1;
        goto again;
    }
    return index;
}

void bitmap_set(unsigned long *map, int start, int nr)
{
    unsigned long *p = map + BIT_WORD(start);
    const int size = start + nr;
    int bits_to_set = BITS_PER_LONG - (start % BITS_PER_LONG);
    unsigned long mask_to_set = BITMAP_FIRST_WORD_MASK(start);

    while (nr - bits_to_set >= 0) {
        *p |= mask_to_set;
        nr -= bits_to_set;
        bits_to_set = BITS_PER_LONG;
        mask_to_set = ~0UL;
        p++;
    }
    if (nr) {
        mask_to_set &= BITMAP_LAST_WORD_MASK(size);
        *p |= mask_to_set;
    }
}

void bitmap_clear(unsigned long *map, int start, int nr)
{
    unsigned long *p = map + BIT_WORD(start);
    const int size = start + nr;
    int bits_to_clear = BITS_PER_LONG - (start % BITS_PER_LONG);
    unsigned long mask_to_clear = BITMAP_FIRST_WORD_MASK(start);

    while (nr - bits_to_clear >= 0) {
        *p &= ~mask_to_clear;
        nr -= bits_to_clear;
        bits_to_clear = BITS_PER_LONG;
        mask_to_clear = ~0UL;
        p++;
    }
    if (nr) {
        mask_to_clear &= BITMAP_LAST_WORD_MASK(size);
        *p &= ~mask_to_clear;
    }
}

/* ****
 *
 * END BITMAP CODE
 *
 * ****/


#define TOTAL_PERIODIC_USEC 630

struct dwc2_qh {
    unsigned short frame_usecs[8];
    int usecs;

    /* For bitmap */
    int start_usecs;
};

struct dwc2_hsotg {
    unsigned short frame_usecs[8];

    unsigned long periodic_bitmap[DIV_ROUND_UP(TOTAL_PERIODIC_USEC,
                           BITS_PER_LONG)];
};

static const unsigned short max_uframe_usecs[8] = {
    100, 100, 100, 100, 100, 100, 30, 0
};

static void bitmap_to_old(struct dwc2_hsotg *hsotg, struct dwc2_qh *qh)
{
    unsigned long start = qh->start_usecs / 100;
    int j;
    int utime;

    for (utime = qh->usecs, j = start; utime > 0 && j < 8; j++) {
        qh->frame_usecs[j] = min_t(u16, utime,
                        hsotg->frame_usecs[j]);
        utime -= qh->frame_usecs[j];
        hsotg->frame_usecs[j] -= qh->frame_usecs[j];
    }
}

static int find_single_uframe_bitmap(struct dwc2_hsotg *hsotg, struct dwc2_qh *qh)
{
    unsigned short frame_start = 0;
    unsigned short utime = qh->usecs;
    int i;

    for (i = 0; i < ARRAY_SIZE(max_uframe_usecs); i++) {
        unsigned short frame_time = max_uframe_usecs[i];
        unsigned long start;

        /* Look for a chunk starting at begin of this frame */
        start = bitmap_find_next_zero_area(hsotg->periodic_bitmap,
                           TOTAL_PERIODIC_USEC,
                           frame_start, utime, 0);

        /* The chunk has to totally fit in this frame */
        if (start - frame_start + utime <= frame_time) {
            bitmap_set(hsotg->periodic_bitmap, start, utime);
            qh->start_usecs = start;
            bitmap_to_old(hsotg, qh);
            dev_dbg(hsotg->dev, "%s: assigned %d us @ %d us\n",
                __func__, qh->usecs, qh->start_usecs);
            return i;
        }

        frame_start += frame_time;
    }
    dev_dbg(hsotg->dev, "%s: failed to assign %d us\n",
        __func__, qh->usecs);
    return -ENOSPC;
}

static int find_multi_uframe_bitmap(struct dwc2_hsotg *hsotg, struct dwc2_qh *qh)
{
    unsigned short utime = qh->usecs;
    unsigned long start;

    start = bitmap_find_next_zero_area(hsotg->periodic_bitmap,
                       TOTAL_PERIODIC_USEC, 0, utime, 0);
    if (start >= TOTAL_PERIODIC_USEC) {
        dev_dbg(hsotg->dev, "%s: failed to assign %d us\n",
            __func__, qh->usecs);
        return -ENOSPC;
    }

    bitmap_set(hsotg->periodic_bitmap, start, qh->usecs);
    qh->start_usecs = start;
    bitmap_to_old(hsotg, qh);

    dev_dbg(hsotg->dev, "%s: assigned %d us @ %d us\n",
        __func__, qh->usecs, qh->start_usecs);

    return start / 100;
}

static int find_multi_uframe_old(struct dwc2_hsotg *hsotg, struct dwc2_qh *qh)
{
    unsigned short utime = qh->usecs;
    unsigned short xtime;
    int t_left;
    int i;
    int j;
    int k;
    for (i = 0; i < 8; i++) {
        if (hsotg->frame_usecs[i] <= 0)
            continue;
        /*
         * we need n consecutive slots so use j as a start slot
         * j plus j+1 must be enough time (for now)
         */
        xtime = hsotg->frame_usecs[i];
        for (j = i + 1; j < 8; j++) {
            /*
             * if we add this frame remaining time to xtime we may
             * be OK, if not we need to test j for a complete frame
             */
            if (xtime + hsotg->frame_usecs[j] < utime) {
                if (hsotg->frame_usecs[j] <
                    max_uframe_usecs[j])
                    continue;
            }
            if (xtime >= utime) {
                t_left = utime;
                for (k = i; k < 8; k++) {
                    t_left -= hsotg->frame_usecs[k];
                    if (t_left <= 0) {
                        qh->frame_usecs[k] +=
                        hsotg->frame_usecs[k]
                        + t_left;
                        hsotg->frame_usecs[k] = -t_left;
                        return i;
                    } else {
                        qh->frame_usecs[k] +=
                        hsotg->frame_usecs[k];
                        hsotg->frame_usecs[k] = 0;
                    }
                }
            }
            /* add the frame time to x time */
            xtime += hsotg->frame_usecs[j];
            /* we must have a fully available next frame or break */
            if (xtime < utime &&
                hsotg->frame_usecs[j] == max_uframe_usecs[j])
                continue;
        }
    }
    return -ENOSPC;
}


static void unschedule(struct dwc2_hsotg *hsotg, struct dwc2_qh *qh)
{
    int i;

    for (i = 0; i < 8; i++) {
        hsotg->frame_usecs[i] += qh->frame_usecs[i];
        qh->frame_usecs[i] = 0;
    }

    bitmap_clear(hsotg->periodic_bitmap, qh->start_usecs, qh->usecs);
}

#define NUM_QH 26

void print_one(char result[], int *char_num, int usecs, int qh_num)
{
    int count = usecs / 10;

    while (count && result[*char_num]) {
        //    printf("%s %c %d %d %d\n", result, 'A' + *char_num, usecs, qh_num, count);

        if (result[*char_num] == ' ') {
            result[*char_num] = 'A' + qh_num;
            count--;
        }
        (*char_num)++;
    }

    if (count)
        printf("ERROR\n");
}

void print_qhs(struct dwc2_qh qhs[])
{
    char result[] =
    "          |          |          |          |          |          |   |";
        int qh_num;
        int uframe;

        for (uframe = 0; uframe < 8; uframe++) {
            int char_num = uframe * 11;
            int first = -1;
            int last = -1;

            for (qh_num = 0; qh_num < NUM_QH; qh_num++) {
                struct dwc2_qh *qh = qhs + qh_num;
                if (uframe != 0 && qh->frame_usecs[uframe] && qh->frame_usecs[uframe-1]) {
                    first = qh_num;
                    continue;
                }
                if (uframe != 7 && qh->frame_usecs[uframe] && qh->frame_usecs[uframe+1])
                    last = qh_num;
            }

            if (first != -1)
                print_one(result, &char_num, qhs[first].frame_usecs[uframe], first);

            for (qh_num = 0; qh_num < NUM_QH; qh_num++) {
                if (qh_num == first || qh_num == last)
                    continue;

                print_one(result, &char_num, qhs[qh_num].frame_usecs[uframe], qh_num);
            }

            if (last != -1) {
                char_num = (uframe + 1) * 11 - 1 - (qhs[last].frame_usecs[uframe] / 10);
                print_one(result, &char_num, qhs[last].frame_usecs[uframe], last);
            }
        }

        puts(result);
}

int main (int argc, char *argv[])
{
    const int test_old = 1;
    const int test_new_single = 0;

    struct dwc2_hsotg hsotg = { };
    struct dwc2_qh qhs[NUM_QH] = { };
    int i;

    memcpy(hsotg.frame_usecs, max_uframe_usecs, sizeof(max_uframe_usecs));

    for (i = 0; i < 1000; i++) {
        if ((rand() % 4) == 0) {
            /* Delete */
            int to_delete = rand() % NUM_QH;

            if (qhs[to_delete].usecs) {
                printf("Delete %c (%3d us):       ", 'A' + to_delete, qhs[to_delete].usecs);
                unschedule(&hsotg, qhs + to_delete);
                qhs[to_delete].usecs = 0;
                print_qhs(qhs);
            }
        } else {
            int to_add = rand() % NUM_QH;

            if (!qhs[to_add].usecs) {
                int usecs = (rand() % 30) * 10 + 10;
                int single = 0;
                int ret;

                qhs[to_add].usecs = usecs;
                if (test_old)
                    ret = find_multi_uframe_old(&hsotg, qhs + to_add);
                else {
                    if (test_new_single && usecs < 100 && ((rand() % 3) == 0)) {
                        single = 1;
                        ret = find_single_uframe_bitmap(&hsotg, qhs + to_add);
                    } else {
                        ret = find_multi_uframe_bitmap(&hsotg, qhs + to_add);
                    }
                }

                if (ret >= 0) {
                    printf("Add %c (%3d us - %d) OK:   ", 'A' + to_add, usecs, single);
                    print_qhs(qhs);
                } else {
                    printf("Add %c (%3d us - %d) fail: \n", 'A' + to_add, usecs, single);
                    //print_qhs(qhs);
                    qhs[to_add].usecs = 0;
                }
            }
        }
    }

    return 0;
}