Example dwc2 microframe schedule test code for v3

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

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

#define ENOSPC 28
typedef unsigned short u16;

#define dwc2_sch_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 EARLY_FRAME_USEC		100
#define TOTAL_PERIODIC_USEC		(6 * EARLY_FRAME_USEC + 30)

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[] = {
	EARLY_FRAME_USEC, EARLY_FRAME_USEC, EARLY_FRAME_USEC,
	EARLY_FRAME_USEC, EARLY_FRAME_USEC, EARLY_FRAME_USEC,
	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,
						   frame_start + frame_time,
						   frame_start, utime, 0);

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

		frame_start += frame_time;
	}

	dwc2_sch_dbg(hsotg, "%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) {
		dwc2_sch_dbg(hsotg, "%s: failed to assign %d us\n",
			     __func__, qh->usecs);
		return -ENOSPC;
	}

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

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

	return start / EARLY_FRAME_USEC;
}

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 = 0;
	const int test_new_single = 1;

	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)
						bitmap_to_old(&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;
}