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