Context Navigation

← Previous Revision
Latest Revision
Next Revision →
Normal
Revision Log

threads.c @ 446

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

Rev	Line
[231]	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 @ 446

Download in other formats: