/*
 *******************************************************************************
 * Authors:
 *     Maxime Villard, 2017
 *     Jankovic Marco, 01/07/2014
 *******************************************************************************
 */

#include "common.h"

typedef enum fsm_state_t {
	FSM_INIT_COM,
	FSM_IDLE,
	FSM_READ_TTY,
	FSM_WRITE_TTY,
	FSM_READ_DEVICE,
	FSM_WRITE_DEVICE,
	FSM_READ_FIFO_RX,
	FSM_READ_FIFO_TX,
	FSM_WRITE_FIFO_RX,
	FSM_WRITE_FIFO_TX,
} fsm_state;

typedef enum flux_control_state_t {
	DONE,
	SEND_XON,
	SEND_XOFF
} flux_control_state;

typedef enum tx_state_t {
	ENABLE,
	DISABLE,
} tx_state;

/*
 * Variables used for thread communication.
 */
static flux_control_state flux_control = DONE;	// used for xon/xoff
static tx_state TX_STATE = ENABLE;				// used to ENABLE/DISABLE thread write

/*
 * This thread reads the fifo_rx for data, then writes those data to the correct
 * tty with the id contained in the packet. Then it checks if a tty is readable.
 * If there is one, write N packet from one tty to the fifo_tx then return to
 * idle state.
 */
void *thread_mux_demux(void *a)
{
	UNUSED(a);
	printf("[+] thread_mux launched\n");

	fd_set readfs;
	char packet_buffer[2 * READ_LEN];
	char tty_raw_buffer[READ_LEN];
	char *pkt;

	int res;
	int i,j;
	int tty_id;
	int tty_size_read = 0;
	int fifo_size_read = 0;
	fsm_state mux_state;

	sigset_t ens;
	sigfillset(&ens);
	sigdelset(&ens, SIGTERM);
	pthread_sigmask(SIG_SETMASK, &ens, NULL);

	struct timeval timeout;

	mux_state = FSM_IDLE;

	while (1)
	{
		switch (mux_state) {
			/*
			 * check if the fifo_rx is readable -> FSM_READ_FIFO_RX
			 * if not check if there is readable data in target_fd -> FSM_READ_TTY
			 */
			case FSM_IDLE:
				FD_ZERO(&readfs);
				FD_SET(fifo_rx_fd, &readfs);

				/*
				 * check if fifo_rx is readable
				 */
				timeout.tv_sec = TIMEOUT_DELAY_MUX_S;
				timeout.tv_usec = TIMEOUT_DELAY_MUX_US;
				res = select(FD_SETSIZE, &readfs, NULL, NULL, &timeout);
				if (res < 0) {
					err(1, "select");
				} else if (res == 0) {
					/* nothing to read, try to read a tty */
					mux_state = FSM_READ_TTY;
					break;
				}

				/* readable */
				mux_state = FSM_READ_FIFO_RX;
				break;

			/*
			 * Read fifo_rx.
			 */
			case FSM_READ_FIFO_RX:
				fifo_size_read = read(fifo_rx_fd, packet_buffer, READ_LEN);

				if (!fifo_size_read) {
					perror("read");
					mux_state = FSM_IDLE;
					break;
				}

				mux_state = FSM_WRITE_TTY;
				break;

			/*
			 * Write the data to the proper tty, then -> FSM_READ_TTY.
			 */
			case FSM_WRITE_TTY:
				j = 0;

				while (j < fifo_size_read) {
					pkt = packet_buffer + j;
					write(slave_fd[(uint8_t)pkt[PKT_TTY]], &pkt[PKT_DAT], 1);
#if DEBUG_MUX
					printf("[MUX::FSM_WRITE_TTY] tty_id=%x | data=%c\n",
					    pkt[PKT_TTY], pkt[PKT_DAT]);
#endif
					j += PACKET_SIZE;
				}
				mux_state = FSM_READ_TTY;
				break;

			/*
			 * Check if there is a readable tty. Return to idle if there is no
			 * tty ready, otherwise -> FSM_WRITE_FIFO_TX.
			 */
			case FSM_READ_TTY:
				timeout.tv_sec = TIMEOUT_DELAY_MUX_S;
				timeout.tv_usec = TIMEOUT_DELAY_MUX_US;

				FD_ZERO(&readfs);
				for (i = 0; i < NB_CHANNELS; i++) {
					FD_SET(slave_fd[i], &readfs);
				}

				res = select(FD_SETSIZE, &readfs, NULL, NULL, &timeout);
				if (res < 0) {
					err(1, "select");
				} else if (res == 0) {
					/* no readable tty */
					mux_state = FSM_IDLE;
					break;
				}

				/* one tty is readable, find it */
				tty_id = 0;
				while (tty_id < NB_CHANNELS && !(res = FD_ISSET(slave_fd[tty_id], &readfs))) {
					tty_id++;
				}

				/* read it */
				tty_size_read = read(slave_fd[tty_id], tty_raw_buffer, READ_LEN);

				if (tty_size_read == 0) {
					mux_state = FSM_IDLE;
					printf("thread MUX FSM_READ_TTY: error while reading tty_%d\n", tty_id);
					perror("read");
					break;
				}
#if DEBUG_MUX
				printf("[MUX::FSM_READ_TTY] read from tty_id=%d | %d bytes\n",
				    tty_id, tty_size_read);
#endif
				i = 0;
				j = 0;
				while (i < tty_size_read) {
					pkt = packet_buffer + j;
					pkt[PKT_TTY] = tty_id | 0x80;
					pkt[PKT_DAT] = tty_raw_buffer[i];
					i++;
					j += PACKET_SIZE;
				}

				mux_state = FSM_WRITE_FIFO_TX;
				break;

			/*
			 * Write packet_buffer into fifo_tx.
			 */
			case FSM_WRITE_FIFO_TX:
				write(fifo_tx_fd, packet_buffer, tty_size_read * PACKET_SIZE);
				mux_state = FSM_IDLE;
				break;

			default:
				err(1, "unreachable");
				break;
		}
	}
	return (void*)NULL;
}

/*
 * This thread initiates communication with the device by sending XON
 * -> read the RX_BUFFER
 * -> check packet correctness
 * -> check fifo_rx size
 * -> software flux control
 * -> then write to the fifo_rx
 */
void *thread_read_rx(void *a)
{
	UNUSED(a);
	printf("[+] thread_read_rx launched\n");
	int size_read = 0;
	int bytes_avail = 0;
	int id_ok = 0;
	int res = 0;
	int x_off = 0;
	int device_raw_offset = 0;
	fd_set readfs;
	int fd;

#if TEST_BENCH
	fd = test_rx_fd;
#else
	fd = device_fd;
#endif

	unsigned char device_raw_data[READ_LEN];
	unsigned char packet_buffer[2];

	struct timeval timeout;

#if TEST_BENCH
	fsm_state read_state = FSM_IDLE;
#else
	fsm_state read_state = FSM_INIT_COM;
#endif

	while (1)
	{
		switch (read_state)
		{
			/*
			 * This state is used to initiate communication with the device
			 * by sending periodically XON while the RX_BUFFER is empty.
			 * Then we reach FSM_IDLE, and never to this state afterwards.
			 */
			case FSM_INIT_COM:
				FD_ZERO(&readfs);
				FD_SET(fd, &readfs);

				timeout.tv_sec = TIMEOUT_DELAY_READ_S;
				timeout.tv_usec = TIMEOUT_DELAY_READ_US;

				res = select(FD_SETSIZE, &readfs, NULL, NULL, &timeout);
				if (res > 0) {
					read_state = FSM_IDLE;
					flux_control = DONE;
					break;
				} else if (res < 0) {
					err(1, "select");
				} else {
					/* nothing to read */
				}

				if (flux_control == DONE)
					flux_control = SEND_XON;
				break;

			/*
			 * This state first checks the number of bytes available in fifo_rx.
			 * If FIFO_LIMIT_SIZE is reached a XOFF is sent. After checking the
			 * fifo, check if the device is ready for i/o -> FSM_READ_DEVICE.
			 */
			case FSM_IDLE:
				FD_ZERO(&readfs);
				FD_SET(fd, &readfs);

				if (ioctl(fifo_rx_fd, FIONREAD, &bytes_avail) < 0) {
					err(1, "ioctl");
				}

				if (bytes_avail > FIFO_LIMIT_SIZE && !x_off) {
					if (flux_control == DONE) {
						flux_control = SEND_XOFF;
						x_off = 1;
					}
				} else if ((bytes_avail < FIFO_LIMIT_SIZE) && x_off) {
					if (flux_control == DONE) {
						flux_control = SEND_XON;
						x_off = 0;
					}
				}

				/*
				 * check if the device is readable
				 */
				timeout.tv_sec = TIMEOUT_DELAY_READ_S;
				timeout.tv_usec = TIMEOUT_DELAY_READ_US;
				res = select(FD_SETSIZE, &readfs, NULL, NULL, &timeout);
				if (res > 0) {
					read_state = FSM_READ_DEVICE;
					break;
				} else if (res < 0) {
					err(1, "select");
				} else {
					/* nothing to read */
				}

				break;

				/*
				 * Read the device, then -> FSM_WRITE_FIFO_RX.
				 */
			case FSM_READ_DEVICE:
				size_read = read(fd, device_raw_data, READ_LEN);
				if (size_read == 0) {
					read_state = FSM_IDLE;
					break;
				}

				read_state = FSM_WRITE_FIFO_RX;
				break;

				/*
				 * Control packet correctness
				 * if a data is XON -> ENABLE thread_write
				 * if a data is XOFF -> DISABLE thread_write
				 * non correct packet are dropped
				 * then back to idle
				 */
			case FSM_WRITE_FIFO_RX:
				device_raw_offset = 0;
				while (device_raw_offset < size_read)
				{
#if DEBUG_READ
					printf("-> got packet from device: data=%x\n", device_raw_data[device_raw_offset]);
#endif
					if (device_raw_data[device_raw_offset] == XOFF) {
						printf("XOFF received\n");
						TX_STATE = DISABLE;
						device_raw_offset++;
						continue;
					} else if (device_raw_data[device_raw_offset] == XON) {
						printf("XON received\n");
						TX_STATE = ENABLE;
						device_raw_offset++;
						continue;
					} else if (device_raw_data[device_raw_offset] & 0x80) {
						id_ok = 1;
						packet_buffer[PKT_TTY] = device_raw_data[device_raw_offset] & 0x7F; /* XXX: should be in the demux thread */

						device_raw_offset++;
#if DEBUG_READ
						printf("--> detected ID in packet: id=%x\n", packet_buffer[PKT_TTY]);
#endif
						continue;
					}
					/* check if data is not ID*/
					else if (id_ok && !(device_raw_data[device_raw_offset] & 0x80))
					{
						if (device_raw_data[device_raw_offset] == XOFF) {
#if DEBUG_READ
							printf("--> detected DATA in packet: XOFF\n");
#endif
							device_raw_offset++;
							TX_STATE = DISABLE;
							continue;
						} else if (device_raw_data[device_raw_offset] == XON) {
#if DEBUG_READ
							printf("--> detected DATA in packet: XON\n");
#endif
							device_raw_offset++;
							TX_STATE = ENABLE;
							continue;
						}
#if DEBUG_READ
						printf("--> detected DATA in packet: '%c'\n", device_raw_data[device_raw_offset]);
#endif
						packet_buffer[PKT_DAT] = device_raw_data[device_raw_offset];
						id_ok = 0;

						write(fifo_rx_fd, packet_buffer, PACKET_SIZE);
						device_raw_offset++;
						continue;
					}
					printf("-> dropping data=%x\n", device_raw_data[device_raw_offset]);
					device_raw_offset++;
				}

				read_state = FSM_IDLE;
				break;

			default:
				err(1, "unreachable");
				break;
		}
	}
	return NULL;
}

/*
 * This thread writes packets from the fifo_tx to the TX_BUFFER. This thread is
 * controlled with TX_STATE and flux_control.
 */
void *thread_write_tx(void *a)
{
	UNUSED(a);
#if DEBUG_WRITE
	int i,j;
#endif

	int res = 0;
	int size_read = 0;
	int fd;

	unsigned char xon = XON;
	unsigned char xoff = XOFF;
	fd_set readfs;

#if TEST_BENCH
	fd = test_tx_fd;
#else
	fd = device_fd;
#endif

	unsigned char device_buf[READ_LEN];

	struct timeval timeout;

	fsm_state write_state = FSM_IDLE;

	printf("[+] thread_write_tx launched\n");

	while (1)
	{
		switch (write_state)
		{
			/*
			 * Depending on TX_STATE and flux_control, send a xon or xoff or
			 * yield if DISABLE. Otherwise, check if the fifo_tx is readable
			 * -> FSM_READ_FIFO_TX.
			 */
			case FSM_IDLE:
				/* software flux control section */
				if (TX_STATE == DISABLE && flux_control != DONE) {
					sched_yield();
					break;
				} else if (flux_control == SEND_XOFF) {
					write(fd, &xoff, 1);
					flux_control = DONE;
					printf("XOFF sent\n");
					break;
				} else if (flux_control == SEND_XON) {
					write(fd, &xon, 1);
					flux_control = DONE;
					printf("XON sent\n");
					break;
				}
				/**********************************/

				/*
				 * check if the fifo is readable
				 */
				FD_ZERO(&readfs);
				FD_SET(fifo_tx_fd, &readfs);
				timeout.tv_sec = TIMEOUT_DELAY_WRITE_S;
				timeout.tv_usec = TIMEOUT_DELAY_WRITE_US;
				res = select(FD_SETSIZE, &readfs, NULL, NULL, &timeout);
				if (res > 0) {
					if (TX_STATE != DISABLE)
						write_state = FSM_READ_FIFO_TX;
					break;
				} else if (res < 0) {
					err(1, "select");
				} else {
					/* nothing to read */
				}

				break;

			/*
			 * Read from fifo_tx, then -> FSM_WRITE_DEVICE.
			 */
			case FSM_READ_FIFO_TX:
				size_read = read(fifo_tx_fd, device_buf, READ_LEN);

#if DEBUG_WRITE
				i = 0;
				j = 0;
				while (i < size_read) {
					printf("thread WRITE id=%x data='%c'\n", device_buf[j + PKT_TTY],device_buf[j + PKT_DAT]);
					i += PACKET_SIZE;
					j += PACKET_SIZE;
				}
#endif

				if (size_read == 0) {
					write_state = FSM_IDLE;
					break;
				}

				write_state = FSM_WRITE_DEVICE;
				break;

				/*
				 * Write to the device, then return to idle.
				 */
			case FSM_WRITE_DEVICE:
				size_read = write(fd, device_buf, size_read);
				write_state = FSM_IDLE;
				break;

			default:
				err(1, "unreachable");
				break;
		}
	}
	return NULL;
}