1 /* Author: Magnus Ivarsson <magnus.ivarsson@volvo.com> */
2
3 /* ---------------------------------------------- */
4 /* --- fifo 4 unix ------------------------------ */
5 /* ---------------------------------------------- */
6 #include "lwip/err.h"
7 #include "netif/fifo.h"
8 #include "lwip/debug.h"
9 #include "lwip/def.h"
10 #include "lwip/sys.h"
11 #include "lwip/arch.h"
12 #include <unistd.h>
13
14 #ifndef TRUE
15 #define TRUE 1
16 #endif
17 #ifndef FALSE
18 #define FALSE 0
19 #endif
20
21 #ifndef SIO_FIFO_DEBUG
22 #define SIO_FIFO_DEBUG LWIP_DBG_OFF
23 #endif
24
fifoGet(fifo_t * fifo)25 u8_t fifoGet(fifo_t * fifo)
26 {
27 u8_t c;
28
29 sys_sem_wait(&fifo->sem); /* enter critical section */
30
31 if (fifo->dataslot == fifo->emptyslot)
32 {
33 fifo->getWaiting = TRUE; /* tell putFifo to signal us when data is available */
34 sys_sem_signal(&fifo->sem); /* leave critical section (allow input from serial port..) */
35 sys_sem_wait(&fifo->getSem); /* wait 4 data */
36 sys_sem_wait(&fifo->sem); /* reenter critical section */
37 }
38
39 c = fifo->data[fifo->dataslot++];
40 fifo->len--;
41
42 if (fifo->dataslot == FIFOSIZE)
43 {
44 fifo->dataslot = 0;
45 }
46 sys_sem_signal(&fifo->sem); /* leave critical section */
47 return c;
48 }
49
50
fifoGetNonBlock(fifo_t * fifo)51 s16_t fifoGetNonBlock(fifo_t * fifo)
52 {
53 u16_t c;
54
55 sys_sem_wait(&fifo->sem); /* enter critical section */
56
57 if (fifo->dataslot == fifo->emptyslot)
58 {
59 /* empty fifo */
60 c = -1;
61 }
62 else
63 {
64 c = fifo->data[fifo->dataslot++];
65 fifo->len--;
66
67 if (fifo->dataslot == FIFOSIZE)
68 {
69 fifo->dataslot = 0;
70 }
71 }
72 sys_sem_signal(&fifo->sem); /* leave critical section */
73 return c;
74 }
75
76
fifoPut(fifo_t * fifo,int fd)77 void fifoPut(fifo_t * fifo, int fd)
78 {
79 /* FIXME: mutex around struct data.. */
80 int cnt=0;
81
82 sys_sem_wait(&fifo->sem ); /* enter critical */
83
84 LWIP_DEBUGF( SIO_FIFO_DEBUG,("fifoput: len%d dat%d empt%d --> ", fifo->len, fifo->dataslot, fifo->emptyslot ) );
85
86 if ( fifo->emptyslot < fifo->dataslot )
87 {
88 cnt = read( fd, &fifo->data[fifo->emptyslot], fifo->dataslot - fifo->emptyslot );
89 }
90 else
91 {
92 cnt = read( fd, &fifo->data[fifo->emptyslot], FIFOSIZE-fifo->emptyslot );
93 }
94 fifo->emptyslot += cnt;
95 fifo->len += cnt;
96
97 LWIP_DEBUGF( SIO_FIFO_DEBUG,("len%d dat%d empt%d\n", fifo->len, fifo->dataslot, fifo->emptyslot ) );
98
99 if ( fifo->len > FIFOSIZE )
100 {
101 printf( "ERROR: fifo overrun detected len=%d, flushing\n", fifo->len );
102 fifo->dataslot = 0;
103 fifo->emptyslot = 0;
104 fifo->len = 0;
105 }
106
107 if ( fifo->emptyslot == FIFOSIZE )
108 {
109 fifo->emptyslot = 0;
110 LWIP_DEBUGF( SIO_FIFO_DEBUG, ("(WRAP) ") );
111
112 sys_sem_signal(&fifo->sem ); /* leave critical */
113 fifoPut( fifo, fd );
114 return;
115 }
116 if ( fifo->getWaiting )
117 {
118 fifo->getWaiting = FALSE;
119 sys_sem_signal(&fifo->getSem );
120 }
121
122 sys_sem_signal(&fifo->sem ); /* leave critical */
123 return;
124 }
125
126
fifoInit(fifo_t * fifo)127 void fifoInit(fifo_t * fifo)
128 {
129 fifo->dataslot = 0;
130 fifo->emptyslot = 0;
131 fifo->len = 0;
132 if(sys_sem_new(&fifo->sem, 1) != ERR_OK) { /* critical section 1=free to enter */
133 LWIP_ASSERT("Failed to create semaphore", 0);
134 }
135 if(sys_sem_new(&fifo->getSem, 0) != ERR_OK) { /* 0 = no one waiting */
136 LWIP_ASSERT("Failed to create semaphore", 0);
137 }
138 fifo->getWaiting = FALSE;
139 }
140