1 #include <stdint.h> 2 #include <string.h> 3 #include <stdio.h> 4 5 #define L2CAP_FIXED_CHANNEL_TABLE_SIZE 3 6 #define FIXED_CHANNEL_FIFO_INVALID_INDEX 0xff 7 8 typedef struct l2cap_fixed_channel { 9 uint8_t waiting_for_can_send_now; 10 uint8_t next_request; 11 } l2cap_fixed_channel_t; 12 13 static l2cap_fixed_channel_t fixed_channels[L2CAP_FIXED_CHANNEL_TABLE_SIZE]; 14 static uint8_t fixed_channel_head_index = FIXED_CHANNEL_FIFO_INVALID_INDEX; 15 static uint8_t fixed_channel_tail_index = FIXED_CHANNEL_FIFO_INVALID_INDEX; 16 fifo_init(void)17static void fifo_init(void){ 18 printf("\ninit\n"); 19 memset(fixed_channels, 0, sizeof(fixed_channels)); 20 int i; 21 for (i=0;i<L2CAP_FIXED_CHANNEL_TABLE_SIZE;i++){ 22 fixed_channels[i].next_request = FIXED_CHANNEL_FIFO_INVALID_INDEX; 23 } 24 } 25 fifo_add(int index)26static void fifo_add(int index){ 27 printf("add index %u\n", index); 28 // insert into queue 29 if (fixed_channel_tail_index == FIXED_CHANNEL_FIFO_INVALID_INDEX){ 30 fixed_channel_head_index = index; 31 } else { 32 fixed_channels[fixed_channel_tail_index].next_request = index; 33 } 34 fixed_channel_tail_index = index; 35 fixed_channels[index].next_request = FIXED_CHANNEL_FIFO_INVALID_INDEX; 36 fixed_channels[index].waiting_for_can_send_now = 1; 37 } 38 fifo_get_next(void)39static int fifo_get_next(void){ 40 if (fixed_channel_head_index == FIXED_CHANNEL_FIFO_INVALID_INDEX) return -1; 41 42 int can_send = 0; 43 uint8_t i = fixed_channel_head_index; 44 if (fixed_channels[i].waiting_for_can_send_now){ 45 can_send = 1; 46 } 47 fixed_channels[i].waiting_for_can_send_now = 0; 48 fixed_channel_head_index = fixed_channels[i].next_request; 49 fixed_channels[i].next_request = FIXED_CHANNEL_FIFO_INVALID_INDEX; 50 if (fixed_channel_head_index == FIXED_CHANNEL_FIFO_INVALID_INDEX){ 51 fixed_channel_tail_index = FIXED_CHANNEL_FIFO_INVALID_INDEX; 52 } 53 if (can_send) { 54 return i; 55 } 56 return -1; 57 } 58 fifo_dump(void)59static void fifo_dump(void){ 60 while (1){ 61 int i = fifo_get_next(); 62 if (i < 0) break; 63 printf("got index %u\n", i); 64 } 65 } 66 test1(void)67static void test1(void){ 68 fifo_init(); 69 fifo_add(1); 70 fifo_dump(); 71 } 72 test2(void)73static void test2(void){ 74 fifo_init(); 75 fifo_add(1); 76 fifo_add(2); 77 fifo_dump(); 78 } 79 test3(void)80static void test3(void){ 81 fifo_init(); 82 fifo_add(1); 83 int index; 84 index = fifo_get_next(); 85 printf("got %u\n", index); 86 fifo_add(2); 87 fifo_add(1); 88 fifo_add(0); 89 fifo_dump(); 90 } 91 main(int argc,const char ** argv)92int main(int argc, const char ** argv){ 93 (void) argc; 94 (void) argv; 95 test1(); 96 test2(); 97 test3(); 98 } 99