#include "semihost.h"

// stolen from https://tools.ietf.org/html/rfc1055
/* SLIP special character codes
 */
#define END				0xC0	/* indicates end of packet */
#define ESC				0xDB	/* indicates byte stuffing */
#define ESC_END			0xDC	/* ESC ESC_END means END data byte */
#define ESC_ESC			0xDD	/* ESC ESC_ESC means ESC data byte */

static inline void send_char(uint8_t ch)
{
	uart_write_byte(SEMI_HOST_UART, ch);
}

/* SEND_PACKET: sends a packet of length "len", starting at
 * location "p".
 */
void send_packet(uint8_t *p, int len)
{
	/* send an initial END character to flush out any data that may
	 * have accumulated in the receiver due to line noise
	 */
	//send_char(END);

	/* for each byte in the packet, send the appropriate character
	 * sequence
	 */
	while (len--) {
		switch (*p) {
		/* if it's the same code as an END character, we send a
		 * special two character code so as not to make the
		 * receiver think we sent an END
		 */
		case END:
			send_char(ESC);
			send_char(ESC_END);
			break;

		/* if it's the same code as an ESC character,
		 * we send a special two character code so as not
		 * to make the receiver think we sent an ESC
		 */
		case ESC:
			send_char(ESC);
			send_char(ESC_ESC);
			break;

		/* otherwise, we just send the character
		 */
		default:
			send_char(*p);
		}

		p++;
	}

	/* tell the receiver that we're done sending the packet
	 */
	send_char(END);
}

/* RECV_PACKET: receives a packet into the buffer located at "p".
 *		If more than len bytes are received, the packet will
 *		be truncated.
 *		Returns the number of bytes stored in the buffer.
 * 1: received a complete packet
 * 0: others
 */
int recv_packet(struct semi_host_env *sh)
{
	uint8_t c;
	int received = 0;

	/* sit in a loop reading bytes until we put together
	 * a whole packet.
	 * Make sure not to copy them into the packet if we
	 * run out of room.
	 */
	while (1) {
		/* get a character to process
		 */
		int len = bk_uart_read_bytes(sh->port, &c, 1, BEKEN_WAIT_FOREVER);
		if (len < 0) {
			break;
		}

		if (sh->slip_state != SLIP_ST_ESC) {
			/* handle bytestuffing if necessary
			 */
			switch (c) {

			/* if it's an END character then we're done with
			 * the packet
			 */
			case END:
				/* a minor optimization: if there is no
				 * data in the packet, ignore it. This is
				 * meant to avoid bothering IP with all
				 * the empty packets generated by the
				 * duplicate END characters which are in

				* turn sent to try to detect line noise.
				*/
				if (received)
					return 1;
				break;

			/* if it's the same code as an ESC character, wait
			 * and get another character and then figure out
			 * what to store in the packet based on that.
			 */
			case ESC:
				sh->slip_state = SLIP_ST_ESC;
				continue;

			/* here we fall into the default handler and let
			 * it store the character for us
			 */
			default:
				if (received < sizeof(sh->buf) - sh->received) {
					received++;
					sh->buf[sh->received++] = c;
				} else {
					return 2;
				}
			}

		} else {
			sh->slip_state = 0;
			// ESC handler
			/* if "c" is not one of these two, then we
			 * have a protocol violation.  The best bet
			 * seems to be to leave the byte alone and
			 * just stuff it into the packet
			 */
			switch (c) {
			case ESC_END:
				c = END;
				break;
			case ESC_ESC:
				c = ESC;
				break;
			}

			if (received < sizeof(sh->buf) - sh->received) {
				received++;
				sh->buf[sh->received++] = c;
			} else {
				return 2;
			}
		}
	}

	return 0;
}