void test3() {
struct fifo p1;
int buf1[256];
int output_buf[128];
int zero[128];
int buf_val[128];
size_t i;
for(i = 0; i < 128; i++) {
buf_val[i] = 0xaabbccdd;
}
fifo_init(&p1, buf1, sizeof(int), 128);
ok1(fifo_amt(&p1) == 0);
ok1(fifo_avail(&p1) == 128);
ok1(p1.read == 0);
fifo_write(&p1, buf_val, 128);
for(i = 0; i < 128; i++) {
buf1[128+i] = 0x01234567;
}
memset(zero, 0, 128*sizeof(int));
ok1(fifo_amt(&p1) == 128);
ok1(fifo_avail(&p1) == 0);
ok1(p1.read == 0);
memset(output_buf, 0, 128);
fifo_peek(&p1, output_buf, 128);
ok1(memcmp(output_buf, buf_val, 128*sizeof(int)) == 0);
memset(output_buf, 0, 128*sizeof(int));
fifo_consume(&p1, output_buf, 96);
ok1(memcmp(output_buf, buf_val, 96*sizeof(int)) == 0);
ok1(memcmp(&output_buf[96], zero, 32*sizeof(int)) == 0);
ok1(fifo_amt(&p1) == 32);
ok1(fifo_avail(&p1) == 96);
ok1(p1.read == 96);
fifo_write(&p1, buf_val, 32);
ok1(fifo_amt(&p1) == 64);
ok1(fifo_avail(&p1) == 64);
ok1(p1.read == 96);
memset(output_buf, 0, 128*sizeof(int));
fifo_peek(&p1, output_buf, 64);
ok1(memcmp(output_buf, buf_val, 64*sizeof(int)) == 0);
#define TEST3AMT 3 + 3 + 1 + 5 + 3 + 1
diag("----test3----\n#");
}
uint16_t AsebaGetBuffer(AsebaVMState *vm, uint8_t * data, uint16_t maxLength, uint16_t* source) {
int flags;
uint16_t ret = 0;
size_t u;
// Touching the FIFO, mask the interrupt ...
USBMaskInterrupts(flags);
u = get_used(&AsebaUsb.rx);
/* Minium packet size == len + src + msg_type == 6 bytes */
if(u >= 6) {
int len;
fifo_peek((unsigned char *) &len, &AsebaUsb.rx, 2);
if (u >= len + 6) {
memcpy_out_fifo((unsigned char *) &len, &AsebaUsb.rx, 2);
memcpy_out_fifo((unsigned char *) source, &AsebaUsb.rx, 2);
// msg_type is not in the len but is always present
len = len + 2;
/* Yay ! We have a complete packet ! */
if(len > maxLength)
len = maxLength;
memcpy_out_fifo(data, &AsebaUsb.rx, len);
ret = len;
}
}
if(usb_uart_serial_port_open())
USBCDCKickRx();
else
fifo_reset(&AsebaUsb.rx);
USBUnmaskInterrupts(flags);
return ret;
}
uint32_t app_fifo_peek(app_fifo_t * p_fifo, uint16_t index, uint8_t * p_byte)
{
if (FIFO_LENGTH() > index)
{
fifo_peek(p_fifo, index, p_byte);
return NRF_SUCCESS;
}
return NRF_ERROR_NOT_FOUND;
}
// TODO doc
// ATx\r : shell command, where x is a char which maps to a command.
// List of supported commands:
// - R: reboot device
// AT$<command handler id> : command to be handled by the command handler specified. The command handler id is a byte < 65 (non ASCII)
// The handlers are passed the command fifo (including the header) and are responsible for pop()-ing the bytes which are processed by the handler.
// When the fifo does not yet contain a full command which can be processed by the specific handler nothing should be popped and the handler will
// called again later when more data is received.
static void process_cmd_fifo()
{
if(fifo_get_size(&cmd_fifo) >= SHELL_CMD_HEADER_SIZE)
{
uint8_t cmd_header[SHELL_CMD_HEADER_SIZE];
fifo_peek(&cmd_fifo, cmd_header, 0, SHELL_CMD_HEADER_SIZE);
if(cmd_header[0] != 'A' || cmd_header[1] != 'T')
{
// unexpected data, pop and return
// TODO log?
fifo_pop(&cmd_fifo, cmd_header, 1);
sched_post_task(&process_cmd_fifo);
return;
}
if(cmd_header[2] != '$')
{
process_shell_cmd(cmd_header[2]);
fifo_pop(&cmd_fifo, cmd_header, SHELL_CMD_HEADER_SIZE);
}
else
{
get_cmd_handler_callback(cmd_header[3])(&cmd_fifo);
}
sched_post_task(&process_cmd_fifo);
} else if(fifo_get_size(&cmd_fifo) >= 3) {
// AT[\r|\n]
uint8_t cmd_header[3];
fifo_peek(&cmd_fifo, cmd_header, 0, 3);
if( cmd_header[0] == 'A' && cmd_header[1] == 'T'
&& ( cmd_header[2] == '\r' || cmd_header[2] == '\n' ) )
{
console_print("OK\r\n");
fifo_pop(&cmd_fifo, cmd_header, 3);
}
}
}
void test5() {
struct fifo p1;
int buf1[128];
int elem;
int output_buf[128];
int test_arr[512];
size_t i;
srand(time(NULL));
for(i = 0; i < ARRAY_SIZE(test_arr); i++) {
test_arr[i] = rand();
}
diag("++++test5++++");
fifo_init(&p1, buf1, sizeof(int), ARRAY_SIZE(buf1) );
ok1(fifo_amt(&p1) == 0);
ok1(fifo_avail(&p1) == ARRAY_SIZE(buf1));
fifo_write(&p1, test_arr, 128);
ok1(fifo_amt(&p1) == 128);
ok1(fifo_avail(&p1) == 0);
fifo_peek(&p1, output_buf, 128);
ok1(memcmp(output_buf, test_arr, 128*sizeof(int)) == 0);
for(i = 0; i < 256; i++) {
fifo_pop(&p1, &elem);
ok1(elem == test_arr[i] && fifo_amt(&p1) == 127 \
&& fifo_avail(&p1) == 1);
fifo_push(&p1, test_arr+128+i);
ok1(fifo_amt(&p1) == 128 && fifo_avail(&p1) == 0);
memset(output_buf, 0, 128*sizeof(int));
fifo_peek(&p1, output_buf, 128);
ok1(memcmp(output_buf, test_arr+i, 128*sizeof(int)));
}
#define TEST5AMT 2 + 3 + 256*3
diag("----test5----\n#");
}
int AsebaUsbRecvBufferEmpty(void) {
// We are called with interrupt disabled ! Check if rx contain something meaningfull
int u;
u = get_used(&AsebaUsb.rx);
if(u > 6) {
int len;
fifo_peek((unsigned char *) &len, &AsebaUsb.rx, 2);
if (u >= len + 6)
return 0;
}
return 1;
}
// in: linda_ud key
int keepercall_get( lua_State* L)
{
keeper_fifo* fifo;
push_table( L, 1); // ud key fifos
lua_replace( L, 1); // fifos key
lua_rawget( L, 1); // fifos fifo
fifo = prepare_fifo_access( L, -1); // fifos fifo
if( fifo != NULL && fifo->count > 0)
{
lua_remove( L, 1); // fifo
// read one value off the fifo
fifo_peek( L, fifo, 1); // fifo ...
return 1;
}
// no fifo was ever registered for this key, or it is empty
return 0;
}
static void process_rx_fifo(void *arg) {
if(!parsed_header) {
// <sync byte (0xC0)><version (0x00)><length of ALP command (1 byte)><ALP command> // TODO CRC
if(fifo_get_size(&rx_fifo) > SERIAL_ALP_FRAME_HEADER_SIZE) {
uint8_t header[SERIAL_ALP_FRAME_HEADER_SIZE];
fifo_peek(&rx_fifo, header, 0, SERIAL_ALP_FRAME_HEADER_SIZE);
DPRINT_DATA(header, 3); // TODO tmp
if(header[0] != SERIAL_ALP_FRAME_SYNC_BYTE || header[1] != SERIAL_ALP_FRAME_VERSION) {
fifo_skip(&rx_fifo, 1);
DPRINT("skip");
parsed_header = false;
payload_len = 0;
if(fifo_get_size(&rx_fifo) > SERIAL_ALP_FRAME_HEADER_SIZE)
sched_post_task(&process_rx_fifo);
return;
}
parsed_header = true;
fifo_skip(&rx_fifo, SERIAL_ALP_FRAME_HEADER_SIZE);
payload_len = header[2];
DPRINT("found header, payload size = %i", payload_len);
sched_post_task(&process_rx_fifo);
}
} else {
if(fifo_get_size(&rx_fifo) < payload_len) {
DPRINT("payload not complete yet");
return;
}
// payload complete, start parsing
// rx_fifo can be bigger than the current serial packet, init a subview fifo
// which is restricted to payload_len so we can't parse past this packet.
fifo_t payload_fifo;
fifo_init_subview(&payload_fifo, &rx_fifo, 0, payload_len);
process_serial_frame(&payload_fifo);
// pop parsed bytes from original fifo
fifo_skip(&rx_fifo, payload_len - fifo_get_size(&payload_fifo));
parsed_header = false;
}
}
static inline void tx_free_descriptors(struct fm10k_tx_queue *q)
{
uint16_t next_rs, count = 0;
next_rs = fifo_peek(&q->rs_tracker);
if (!(q->hw_ring[next_rs].flags & FM10K_TXD_FLAG_DONE))
return;
/* the DONE flag is set on this descriptor so remove the ID
* from the RS bit tracker and free the buffers */
fifo_remove(&q->rs_tracker);
/* wrap around? if so, free buffers from last_free up to but NOT
* including nb_desc */
if (q->last_free > next_rs) {
count = q->nb_desc - q->last_free;
while (q->last_free < q->nb_desc) {
rte_pktmbuf_free_seg(q->sw_ring[q->last_free]);
q->sw_ring[q->last_free] = NULL;
++q->last_free;
}
q->last_free = 0;
}
/* adjust free descriptor count before the next loop */
q->nb_free += count + (next_rs + 1 - q->last_free);
/* free buffers from last_free, up to and including next_rs */
while (q->last_free <= next_rs) {
rte_pktmbuf_free_seg(q->sw_ring[q->last_free]);
q->sw_ring[q->last_free] = NULL;
++q->last_free;
}
if (q->last_free == q->nb_desc)
q->last_free = 0;
}
// in: linda_ud key [count]
// out: at most <count> values
int keepercall_get( lua_State* L)
{
keeper_fifo* fifo;
int count = 1;
if( lua_gettop( L) == 3) // ud key count
{
count = lua_tointeger( L, 3);
lua_pop( L, 1); // ud key
}
push_table( L, 1); // ud key fifos
lua_replace( L, 1); // fifos key
lua_rawget( L, 1); // fifos fifo
fifo = prepare_fifo_access( L, -1); // fifos fifo
if( fifo != NULL && fifo->count > 0)
{
lua_remove( L, 1); // fifo
count = __min( count, fifo->count);
// read <count> value off the fifo
fifo_peek( L, fifo, count); // fifo ...
return count;
}
// no fifo was ever registered for this key, or it is empty
return 0;
}
int main() {
Fifo fifo;
fifo_init(&fifo);
printf("Size (0): %i\n", fifo_size(&fifo));
char c;
// put a char
fifo_put(&fifo, 'a');
printf("%i %i\n",fifo.in, fifo.out);
printf("Size (1): %i\n", fifo_size(&fifo));
fifo_pop(&fifo, &c);
printf("Pop a: %c\n",c);
// Pop when zero elements
printf("Pop (0,a): %i, %c\n", fifo_pop(&fifo, &c), c);
// Print the size
fifo_put(&fifo, 'a');
printf("Size (1): %i\n", fifo_size(&fifo));
fifo_pop(&fifo, &c);
// put several chars
fifo_put(&fifo, 'a');
fifo_put(&fifo, 'b');
fifo_put(&fifo, 'c');
printf("Size (3): %i\n", fifo_size(&fifo));
fifo_pop(&fifo, &c);
printf("Pop a: %c\n",c);
fifo_pop(&fifo, &c);
printf("Pop b: %c\n",c);
fifo_pop(&fifo, &c);
printf("Pop c: %c\n",c);
// Print int 1
fifo_put(&fifo, 1);
fifo_pop(&fifo, &c);
printf("Pop 1: %i\n",c);
// put negative number
fifo_put(&fifo, -127);
fifo_pop(&fifo, &c);
printf("Pop -127: %i\n",c);
// Check peek
fifo_put(&fifo, 34);
fifo_peek(&fifo, &c);
printf("Peek 34: %i\n",c);
printf("Size 1: %i\n", fifo_size(&fifo));
fifo_pop(&fifo,&c);
// Check peek 2
fifo_put(&fifo, 75);
fifo_put(&fifo, 32);
fifo_peek_at(&fifo, &c, 1);
printf("Peek 32: %i\n",c);
printf("Size 2: %i\n", fifo_size(&fifo));
// overflowtest (does not add any more elements)
printf("Size (2): %i\n", fifo_size(&fifo));
int j;
for(j = 0; j < 130; j++) {
//printf("Size: %i (%i,%i)\n", fifo_size(&fifo), fifo.in, fifo.out);
fifo_put(&fifo, j);
}
printf("Size (127): %i\n", fifo_size(&fifo));
//printf("in: %i, out: %i\n", fifo.in, fifo.out);
fifo_pop(&fifo, &c);
printf("Pop 75: %i\n", c);
fifo_pop(&fifo, &c);
printf("Pop 32: %i\n", c);
printf("Size (125): %i\n", fifo_size(&fifo));
// Second FIFO
Fifo fifo2;
fifo_init(&fifo);
fifo_put(&fifo2, 'b');
printf("Size: %i\n", fifo_size(&fifo2));
fifo_pop(&fifo2, &c);
printf("Pop b: %c\n",c);
return 1;
}
int tty_read(struct inode* inode, void* ptr, off_t pos, size_t len) {
if(unlikely(!inode || !ptr)) {
errno = EINVAL;
return -1;
}
if(unlikely(!inode->userdata)) {
errno = EINVAL;
return -1;
}
if(unlikely(!len))
return 0;
struct tty_context* tio = (struct tty_context*) inode->userdata;
uint8_t* buf = (uint8_t*) ptr;
int p = 0;
if(tio->pgrp != current_task->pgid)
sys_exit((1 << 31) | W_STOPCODE(SIGTTIN));
int fd = sys_open(TTY_DEFAULT_INPUT_DEVICE, O_RDONLY, 0);
if(fd < 0) {
errno = EIO;
return -1;
}
if(fifo_available(&tio->in)) {
char tmp[BUFSIZ];
for(int j = 0; (j = fifo_read(&tio->in, tmp, sizeof(tmp))) > 0;)
fifo_write(&tio->uin, tmp, j);
}
while(p < len) {
uint8_t ch;
if(fifo_available(&tio->uin))
fifo_read(&tio->uin, &ch, sizeof(ch));
else
ch = process_keyboard(inode, tio, fd, &p);
switch(ch) {
case 0:
continue;
case '\b':
case '\x7f':
if(tio->ios.c_lflag & ICANON) {
if(p > 0) {
fifo_peek(&tio->in, 1);
p--;
if(tio->ios.c_lflag & ECHOE)
ch = '\b';
else
ch = '\x7f';
if(tio->ios.c_lflag & ECHO)
tty_output_write(inode, &ch, 0, 1);
}
continue;
}
/* No processing */
ch = '\b';
break;
default:
break;
}
if(unlikely(ch < 32)) {
for(int i = 0; i < NCCS; i++) {
if(ch != tio->ios.c_cc[i])
continue;
fifo_write(&tio->in, &ch, 1);
p++;
ch = 0;
break;
}
if(unlikely(!ch))
continue;
}
if(!(tio->ios.c_iflag & IGNCR)) {
if(ch == '\n')
break;
}
char utf8[UTF8_MAX_LENGTH];
size_t utf8len = ucs2_to_utf8((int32_t) ch, utf8);
fifo_write(&tio->in, utf8, utf8len);
p += utf8len;
if(tio->ios.c_lflag & ECHO)
tty_output_write(inode, utf8, 0, utf8len);
}
if(p < len) {
if(!(tio->ios.c_iflag & IGNCR)) {
char ch = '\n';
fifo_write(&tio->in, &ch, 1);
p++;
if((tio->ios.c_lflag & ECHO) || (tio->ios.c_lflag & ICANON && tio->ios.c_lflag & ECHONL))
tty_output_write(inode, &ch, 0, 1);
}
} else
p = len;
if(fifo_available(&tio->in))
fifo_read(&tio->in, buf, p);
sys_close(fd);
return p;
}
/**@brief Get one byte from the FIFO. */
static __INLINE void fifo_get(app_fifo_t * p_fifo, uint8_t * p_byte)
{
fifo_peek(p_fifo, 0, p_byte);
p_fifo->read_pos++;
}
static int
kgetch(EVENTLIST_0th(_nc_eventlist * evl))
{
struct tries *ptr;
int ch = 0;
int timeleft = ESCDELAY;
TR(TRACE_IEVENT, ("kgetch() called"));
ptr = SP->_keytry;
for (;;) {
if (cooked_key_in_fifo() && SP->_fifo[head] >= KEY_MIN) {
break;
} else if (!raw_key_in_fifo()) {
ch = fifo_push(EVENTLIST_1st(evl));
if (ch == ERR) {
peek = head; /* the keys stay uninterpreted */
return ERR;
}
#ifdef NCURSES_WGETCH_EVENTS
else if (ch == KEY_EVENT) {
peek = head; /* the keys stay uninterpreted */
return fifo_pull(); /* Remove KEY_EVENT from the queue */
}
#endif
}
ch = fifo_peek();
if (ch >= KEY_MIN) {
/* If not first in queue, somebody put this key there on purpose in
* emergency. Consider it higher priority than the unfinished
* keysequence we are parsing.
*/
peek = head;
/* assume the key is the last in fifo */
t_dec(); /* remove the key */
return ch;
}
TR(TRACE_IEVENT, ("ch: %s", _tracechar((unsigned char) ch)));
while ((ptr != NULL) && (ptr->ch != (unsigned char) ch))
ptr = ptr->sibling;
if (ptr == NULL) {
TR(TRACE_IEVENT, ("ptr is null"));
break;
}
TR(TRACE_IEVENT, ("ptr=%p, ch=%d, value=%d",
ptr, ptr->ch, ptr->value));
if (ptr->value != 0) { /* sequence terminated */
TR(TRACE_IEVENT, ("end of sequence"));
if (peek == tail)
fifo_clear();
else
head = peek;
return (ptr->value);
}
ptr = ptr->child;
if (!raw_key_in_fifo()) {
int rc;
TR(TRACE_IEVENT, ("waiting for rest of sequence"));
rc = check_mouse_activity(timeleft EVENTLIST_2nd(evl));
#ifdef NCURSES_WGETCH_EVENTS
if (rc & 4) {
TR(TRACE_IEVENT, ("interrupted by a user event"));
/* FIXME Should have preserved remainder timeleft for reusal... */
peek = head; /* Restart interpreting later */
return KEY_EVENT;
}
#endif
if (!rc) {
TR(TRACE_IEVENT, ("ran out of time"));
break;
}
}
}
ch = fifo_pull();
peek = head;
return ch;
}
int main(void)
{
fifo f;
size_t i, nelem;
status_t rc;
char dummydata[1000] = "Hello";
clock_t stop, start;
double duration;
nelem = 1*1000*1000;
f = fifo_new(nelem);
#if 1
/* Fill the fifo completely */
start = clock();
for (i = 0; i < nelem; i++) {
rc = fifo_add(f, dummydata);
assert(rc != 0);
}
stop = clock();
duration = (stop - start) * 1.0 / CLOCKS_PER_SEC;
printf("%s: Added %lu elements in %f seconds\n", __FILE__, (unsigned long)nelem, duration);
assert(fifo_nelem(f) == nelem);
/* Test fifo_peek() */
for (i = 0; i < nelem; i++) {
const char *s = fifo_peek(f, i);
/* Stupid workaround for gcc 4.0.2 optimization in conjunction with assert() */
int xi;
xi = (s != NULL); assert(xi);
xi = (strcmp(s, dummydata) == 0); assert(xi);
}
/* Add a new one, this should fail */
assert(fifo_add(f, dummydata) == 0);
/* Now read two and then add one. That should be possible */
assert(fifo_get(f));
assert(fifo_get(f));
assert(fifo_nelem(f) == nelem - 2);
assert(fifo_add(f, dummydata) != 0);
assert(fifo_nelem(f) == nelem - 1);
/* Test fifo_peek() */
for (i = 0; i < fifo_nelem(f); i++) {
const char *s = fifo_peek(f, i);
int xi;
xi = (s != NULL); assert(xi);
xi = (strcmp(s, dummydata) == 0); assert(xi);
}
/* The first should be OK, the next must fail */
assert(fifo_add(f, dummydata) != 0);
assert(fifo_nelem(f) == nelem);
assert(fifo_add(f, dummydata) == 0);
assert(fifo_nelem(f) == nelem);
start = clock();
for (i = 0; i < nelem; i++) {
char *x = fifo_get(f);
(void)x;
}
stop = clock();
duration = (stop - start) * 1.0 / CLOCKS_PER_SEC;
printf("%s: Got %lu elements in %f seconds\n", __FILE__, (unsigned long)nelem, duration);
#endif
/* Now check signalling and wait for data.
* We start two threads, a reader and a writer.
* The writer writes n times to the fifo and the reader
* prints the data.
* This thread joins the writer and calls fifo_wake() when
* the writer is done, to stop the reader thread in a controlled
* manner.
*/
{
pthread_t w, r;
pthread_create(&r, NULL, reader, f);
pthread_create(&w, NULL, writer, f);
pthread_join(w, NULL);
if (!fifo_wake(f))
exit(EXIT_FAILURE);
pthread_join(r, NULL);
}
fifo_free(f, free);
return 0;
}
