Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #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;
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement