Context Navigation

← Previous Revision
Latest Revision
Next Revision →
Blame
Revision Log

threads.c @ 566

Last change on this file since 566 was 231, checked in by max@…, 8 years ago
Add a serial port multiplexer, usable to communicate with the ALMOS-MKH kernel over a serial port.
File size: 11.5 KB

Line
1	/*
2	*******************************************************************************
3	* Authors:
4	* Maxime Villard, 2017
5	* Jankovic Marco, 01/07/2014
6	*******************************************************************************
7	*/
8
9	#include "common.h"
10
11	typedef enum fsm_state_t {
12	FSM_INIT_COM,
13	FSM_IDLE,
14	FSM_READ_TTY,
15	FSM_WRITE_TTY,
16	FSM_READ_DEVICE,
17	FSM_WRITE_DEVICE,
18	FSM_READ_FIFO_RX,
19	FSM_READ_FIFO_TX,
20	FSM_WRITE_FIFO_RX,
21	FSM_WRITE_FIFO_TX,
22	} fsm_state;
23
24	typedef enum flux_control_state_t {
25	DONE,
26	SEND_XON,
27	SEND_XOFF
28	} flux_control_state;
29
30	typedef enum tx_state_t {
31	ENABLE,
32	DISABLE,
33	} tx_state;
34
35	/*
36	* Variables used for thread communication.
37	*/
38	static flux_control_state flux_control = DONE; // used for xon/xoff
39	static tx_state TX_STATE = ENABLE; // used to ENABLE/DISABLE thread write
40
41	/*
42	* This thread reads the fifo_rx for data, then writes those data to the correct
43	* tty with the id contained in the packet. Then it checks if a tty is readable.
44	* If there is one, write N packet from one tty to the fifo_tx then return to
45	* idle state.
46	*/
47	void thread_mux_demux(void a)
48	{
49	UNUSED(a);
50	printf("[+] thread_mux launched\n");
51
52	fd_set readfs;
53	char packet_buffer[2 * READ_LEN];
54	char tty_raw_buffer[READ_LEN];
55	char *pkt;
56
57	int res;
58	int i,j;
59	int tty_id;
60	int tty_size_read = 0;
61	int fifo_size_read = 0;
62	fsm_state mux_state;
63
64	sigset_t ens;
65	sigfillset(&ens);
66	sigdelset(&ens, SIGTERM);
67	pthread_sigmask(SIG_SETMASK, &ens, NULL);
68
69	struct timeval timeout;
70
71	mux_state = FSM_IDLE;
72
73	while (1)
74	{
75	switch (mux_state) {
76	/*
77	* check if the fifo_rx is readable -> FSM_READ_FIFO_RX
78	* if not check if there is readable data in target_fd -> FSM_READ_TTY
79	*/
80	case FSM_IDLE:
81	FD_ZERO(&readfs);
82	FD_SET(fifo_rx_fd, &readfs);
83
84	/*
85	* check if fifo_rx is readable
86	*/
87	timeout.tv_sec = TIMEOUT_DELAY_MUX_S;
88	timeout.tv_usec = TIMEOUT_DELAY_MUX_US;
89	res = select(FD_SETSIZE, &readfs, NULL, NULL, &timeout);
90	if (res < 0) {
91	err(1, "select");
92	} else if (res == 0) {
93	/* nothing to read, try to read a tty */
94	mux_state = FSM_READ_TTY;
95	break;
96	}
97
98	/* readable */
99	mux_state = FSM_READ_FIFO_RX;
100	break;
101
102	/*
103	* Read fifo_rx.
104	*/
105	case FSM_READ_FIFO_RX:
106	fifo_size_read = read(fifo_rx_fd, packet_buffer, READ_LEN);
107
108	if (!fifo_size_read) {
109	perror("read");
110	mux_state = FSM_IDLE;
111	break;
112	}
113
114	mux_state = FSM_WRITE_TTY;
115	break;
116
117	/*
118	* Write the data to the proper tty, then -> FSM_READ_TTY.
119	*/
120	case FSM_WRITE_TTY:
121	j = 0;
122
123	while (j < fifo_size_read) {
124	pkt = packet_buffer + j;
125	write(slave_fd[(uint8_t)pkt[PKT_TTY]], &pkt[PKT_DAT], 1);
126	#if DEBUG_MUX
127	printf("[MUX::FSM_WRITE_TTY] tty_id=%x \| data=%c\n",
128	pkt[PKT_TTY], pkt[PKT_DAT]);
129	#endif
130	j += PACKET_SIZE;
131	}
132	mux_state = FSM_READ_TTY;
133	break;
134
135	/*
136	* Check if there is a readable tty. Return to idle if there is no
137	* tty ready, otherwise -> FSM_WRITE_FIFO_TX.
138	*/
139	case FSM_READ_TTY:
140	timeout.tv_sec = TIMEOUT_DELAY_MUX_S;
141	timeout.tv_usec = TIMEOUT_DELAY_MUX_US;
142
143	FD_ZERO(&readfs);
144	for (i = 0; i < NB_CHANNELS; i++) {
145	FD_SET(slave_fd[i], &readfs);
146	}
147
148	res = select(FD_SETSIZE, &readfs, NULL, NULL, &timeout);
149	if (res < 0) {
150	err(1, "select");
151	} else if (res == 0) {
152	/* no readable tty */
153	mux_state = FSM_IDLE;
154	break;
155	}
156
157	/* one tty is readable, find it */
158	tty_id = 0;
159	while (tty_id < NB_CHANNELS && !(res = FD_ISSET(slave_fd[tty_id], &readfs))) {
160	tty_id++;
161	}
162
163	/* read it */
164	tty_size_read = read(slave_fd[tty_id], tty_raw_buffer, READ_LEN);
165
166	if (tty_size_read == 0) {
167	mux_state = FSM_IDLE;
168	printf("thread MUX FSM_READ_TTY: error while reading tty_%d\n", tty_id);
169	perror("read");
170	break;
171	}
172	#if DEBUG_MUX
173	printf("[MUX::FSM_READ_TTY] read from tty_id=%d \| %d bytes\n",
174	tty_id, tty_size_read);
175	#endif
176	i = 0;
177	j = 0;
178	while (i < tty_size_read) {
179	pkt = packet_buffer + j;
180	pkt[PKT_TTY] = tty_id \| 0x80;
181	pkt[PKT_DAT] = tty_raw_buffer[i];
182	i++;
183	j += PACKET_SIZE;
184	}
185
186	mux_state = FSM_WRITE_FIFO_TX;
187	break;
188
189	/*
190	* Write packet_buffer into fifo_tx.
191	*/
192	case FSM_WRITE_FIFO_TX:
193	write(fifo_tx_fd, packet_buffer, tty_size_read * PACKET_SIZE);
194	mux_state = FSM_IDLE;
195	break;
196
197	default:
198	err(1, "unreachable");
199	break;
200	}
201	}
202	return (void*)NULL;
203	}
204
205	/*
206	* This thread initiates communication with the device by sending XON
207	* -> read the RX_BUFFER
208	* -> check packet correctness
209	* -> check fifo_rx size
210	* -> software flux control
211	* -> then write to the fifo_rx
212	*/
213	void thread_read_rx(void a)
214	{
215	UNUSED(a);
216	printf("[+] thread_read_rx launched\n");
217	int size_read = 0;
218	int bytes_avail = 0;
219	int id_ok = 0;
220	int res = 0;
221	int x_off = 0;
222	int device_raw_offset = 0;
223	fd_set readfs;
224	int fd;
225
226	#if TEST_BENCH
227	fd = test_rx_fd;
228	#else
229	fd = device_fd;
230	#endif
231
232	unsigned char device_raw_data[READ_LEN];
233	unsigned char packet_buffer[2];
234
235	struct timeval timeout;
236
237	#if TEST_BENCH
238	fsm_state read_state = FSM_IDLE;
239	#else
240	fsm_state read_state = FSM_INIT_COM;
241	#endif
242
243	while (1)
244	{
245	switch (read_state)
246	{
247	/*
248	* This state is used to initiate communication with the device
249	* by sending periodically XON while the RX_BUFFER is empty.
250	* Then we reach FSM_IDLE, and never to this state afterwards.
251	*/
252	case FSM_INIT_COM:
253	FD_ZERO(&readfs);
254	FD_SET(fd, &readfs);
255
256	timeout.tv_sec = TIMEOUT_DELAY_READ_S;
257	timeout.tv_usec = TIMEOUT_DELAY_READ_US;
258
259	res = select(FD_SETSIZE, &readfs, NULL, NULL, &timeout);
260	if (res > 0) {
261	read_state = FSM_IDLE;
262	flux_control = DONE;
263	break;
264	} else if (res < 0) {
265	err(1, "select");
266	} else {
267	/* nothing to read */
268	}
269
270	if (flux_control == DONE)
271	flux_control = SEND_XON;
272	break;
273
274	/*
275	* This state first checks the number of bytes available in fifo_rx.
276	* If FIFO_LIMIT_SIZE is reached a XOFF is sent. After checking the
277	* fifo, check if the device is ready for i/o -> FSM_READ_DEVICE.
278	*/
279	case FSM_IDLE:
280	FD_ZERO(&readfs);
281	FD_SET(fd, &readfs);
282
283	if (ioctl(fifo_rx_fd, FIONREAD, &bytes_avail) < 0) {
284	err(1, "ioctl");
285	}
286
287	if (bytes_avail > FIFO_LIMIT_SIZE && !x_off) {
288	if (flux_control == DONE) {
289	flux_control = SEND_XOFF;
290	x_off = 1;
291	}
292	} else if ((bytes_avail < FIFO_LIMIT_SIZE) && x_off) {
293	if (flux_control == DONE) {
294	flux_control = SEND_XON;
295	x_off = 0;
296	}
297	}
298
299	/*
300	* check if the device is readable
301	*/
302	timeout.tv_sec = TIMEOUT_DELAY_READ_S;
303	timeout.tv_usec = TIMEOUT_DELAY_READ_US;
304	res = select(FD_SETSIZE, &readfs, NULL, NULL, &timeout);
305	if (res > 0) {
306	read_state = FSM_READ_DEVICE;
307	break;
308	} else if (res < 0) {
309	err(1, "select");
310	} else {
311	/* nothing to read */
312	}
313
314	break;
315
316	/*
317	* Read the device, then -> FSM_WRITE_FIFO_RX.
318	*/
319	case FSM_READ_DEVICE:
320	size_read = read(fd, device_raw_data, READ_LEN);
321	if (size_read == 0) {
322	read_state = FSM_IDLE;
323	break;
324	}
325
326	read_state = FSM_WRITE_FIFO_RX;
327	break;
328
329	/*
330	* Control packet correctness
331	* if a data is XON -> ENABLE thread_write
332	* if a data is XOFF -> DISABLE thread_write
333	* non correct packet are dropped
334	* then back to idle
335	*/
336	case FSM_WRITE_FIFO_RX:
337	device_raw_offset = 0;
338	while (device_raw_offset < size_read)
339	{
340	#if DEBUG_READ
341	printf("-> got packet from device: data=%x\n", device_raw_data[device_raw_offset]);
342	#endif
343	if (device_raw_data[device_raw_offset] == XOFF) {
344	printf("XOFF received\n");
345	TX_STATE = DISABLE;
346	device_raw_offset++;
347	continue;
348	} else if (device_raw_data[device_raw_offset] == XON) {
349	printf("XON received\n");
350	TX_STATE = ENABLE;
351	device_raw_offset++;
352	continue;
353	} else if (device_raw_data[device_raw_offset] & 0x80) {
354	id_ok = 1;
355	packet_buffer[PKT_TTY] = device_raw_data[device_raw_offset] & 0x7F; /* XXX: should be in the demux thread */
356
357	device_raw_offset++;
358	#if DEBUG_READ
359	printf("--> detected ID in packet: id=%x\n", packet_buffer[PKT_TTY]);
360	#endif
361	continue;
362	}
363	/* check if data is not ID*/
364	else if (id_ok && !(device_raw_data[device_raw_offset] & 0x80))
365	{
366	if (device_raw_data[device_raw_offset] == XOFF) {
367	#if DEBUG_READ
368	printf("--> detected DATA in packet: XOFF\n");
369	#endif
370	device_raw_offset++;
371	TX_STATE = DISABLE;
372	continue;
373	} else if (device_raw_data[device_raw_offset] == XON) {
374	#if DEBUG_READ
375	printf("--> detected DATA in packet: XON\n");
376	#endif
377	device_raw_offset++;
378	TX_STATE = ENABLE;
379	continue;
380	}
381	#if DEBUG_READ
382	printf("--> detected DATA in packet: '%c'\n", device_raw_data[device_raw_offset]);
383	#endif
384	packet_buffer[PKT_DAT] = device_raw_data[device_raw_offset];
385	id_ok = 0;
386
387	write(fifo_rx_fd, packet_buffer, PACKET_SIZE);
388	device_raw_offset++;
389	continue;
390	}
391	printf("-> dropping data=%x\n", device_raw_data[device_raw_offset]);
392	device_raw_offset++;
393	}
394
395	read_state = FSM_IDLE;
396	break;
397
398	default:
399	err(1, "unreachable");
400	break;
401	}
402	}
403	return NULL;
404	}
405
406	/*
407	* This thread writes packets from the fifo_tx to the TX_BUFFER. This thread is
408	* controlled with TX_STATE and flux_control.
409	*/
410	void thread_write_tx(void a)
411	{
412	UNUSED(a);
413	#if DEBUG_WRITE
414	int i,j;
415	#endif
416
417	int res = 0;
418	int size_read = 0;
419	int fd;
420
421	unsigned char xon = XON;
422	unsigned char xoff = XOFF;
423	fd_set readfs;
424
425	#if TEST_BENCH
426	fd = test_tx_fd;
427	#else
428	fd = device_fd;
429	#endif
430
431	unsigned char device_buf[READ_LEN];
432
433	struct timeval timeout;
434
435	fsm_state write_state = FSM_IDLE;
436
437	printf("[+] thread_write_tx launched\n");
438
439	while (1)
440	{
441	switch (write_state)
442	{
443	/*
444	* Depending on TX_STATE and flux_control, send a xon or xoff or
445	* yield if DISABLE. Otherwise, check if the fifo_tx is readable
446	* -> FSM_READ_FIFO_TX.
447	*/
448	case FSM_IDLE:
449	/* software flux control section */
450	if (TX_STATE == DISABLE && flux_control != DONE) {
451	sched_yield();
452	break;
453	} else if (flux_control == SEND_XOFF) {
454	write(fd, &xoff, 1);
455	flux_control = DONE;
456	printf("XOFF sent\n");
457	break;
458	} else if (flux_control == SEND_XON) {
459	write(fd, &xon, 1);
460	flux_control = DONE;
461	printf("XON sent\n");
462	break;
463	}
464	/**********************************/
465
466	/*
467	* check if the fifo is readable
468	*/
469	FD_ZERO(&readfs);
470	FD_SET(fifo_tx_fd, &readfs);
471	timeout.tv_sec = TIMEOUT_DELAY_WRITE_S;
472	timeout.tv_usec = TIMEOUT_DELAY_WRITE_US;
473	res = select(FD_SETSIZE, &readfs, NULL, NULL, &timeout);
474	if (res > 0) {
475	if (TX_STATE != DISABLE)
476	write_state = FSM_READ_FIFO_TX;
477	break;
478	} else if (res < 0) {
479	err(1, "select");
480	} else {
481	/* nothing to read */
482	}
483
484	break;
485
486	/*
487	* Read from fifo_tx, then -> FSM_WRITE_DEVICE.
488	*/
489	case FSM_READ_FIFO_TX:
490	size_read = read(fifo_tx_fd, device_buf, READ_LEN);
491
492	#if DEBUG_WRITE
493	i = 0;
494	j = 0;
495	while (i < size_read) {
496	printf("thread WRITE id=%x data='%c'\n", device_buf[j + PKT_TTY],device_buf[j + PKT_DAT]);
497	i += PACKET_SIZE;
498	j += PACKET_SIZE;
499	}
500	#endif
501
502	if (size_read == 0) {
503	write_state = FSM_IDLE;
504	break;
505	}
506
507	write_state = FSM_WRITE_DEVICE;
508	break;
509
510	/*
511	* Write to the device, then return to idle.
512	*/
513	case FSM_WRITE_DEVICE:
514	size_read = write(fd, device_buf, size_read);
515	write_state = FSM_IDLE;
516	break;
517
518	default:
519	err(1, "unreachable");
520	break;
521	}
522	}
523	return NULL;
524	}

Note: See TracBrowser for help on using the repository browser.

Context Navigation

source: trunk/tools/alcons/threads.c @ 566

Download in other formats: