#include <pic.h>
#include "config.h"
#include "serial.h"

volatile uint8_t stq[STQ_SIZE], stq_start, stq_len;
volatile uint8_t bank1 srq[SRQ_SIZE], srq_start, srq_len;

/*
 * Initialization
 */

void serial_init(void)
{
	/* Set up serial transmit queue and serial receive queue */
	stq_start = stq_len = 0;
	srq_start = srq_len = 0;

	/* Initialize serial port with baudrate specified in config.h */
	TRISRX = 1;
	TRISTX = 1;

	BRGH = 1;
#define BRG (((FOSC + (8 * BAUDRATE - 1)) /(16 * BAUDRATE)) - 1)
#if (BRG < 1) || (BRG > 255)
#error Cannot achieve baudrate
#endif
#define ACT_BR (FOSC / (16 * (BRG + 1)))
#if ((ACT_BR * 100 / BAUDRATE) > 105) || ((ACT_BR * 100 / BAUDRATE) < 94)
#error Actual baudrate is too far from requested baudrate.  Ratio is
#error FOSC/(16*floor((FOSC+(8*BAUDRATE-1))/(16*BAUDRATE)))/BAUDRATE
#endif

	SPBRG = BRG;
	SYNC = 0;
	SPEN = 1;
	CREN = 1;
	SREN = 0;
	TX9 = 0;
	RX9 = 0;
	TXEN = 1;

	/* Enable serial port interrupts.  To avoid being interrupted
	   all the time, TXIE is kept off until we have data to
	   transmit. */
	TXIE = 0;
	RCIE = 1;
	PEIE = 1;
}

/*
 * Interrupt handling
 */

/* Get a byte from the transmit queue and send it */
void serial_interrupt_tx(void)
{
	if(stq_len != 0) {
		TXREG = stq[stq_start];
		stq_start = (stq_start + 1) & STQ_MASK;
		stq_len--;
	}

	/* If queue is empty, disable interrupt */
	if(stq_len == 0)
		TXIE = 0;
}

/* Receive a byte and write it to the receive queue. */
void serial_interrupt_rx(void)
{
	if(FERR) {
		/* Framing error: discard byte */
		(void) RCREG;
		return;
	}

	if(OERR) {
		/* Overflow error, clear it */
		CREN = 0;
		CREN = 1;
		return;
	}
	
	if(!(srq_len & SRQ_SIZE)) {
		srq[(srq_start + srq_len) & SRQ_MASK] = RCREG;
		srq_len++;
	} else {
		/* Queue is full; discard data */
		(void) RCREG;
	}
}

/*
 * Userspace interface
 */

/* Send byte to PC.  Blocks iff transmit queue is full. */
void serial_put(uint8_t x)
{
	while(!serial_can_put()) 
		continue;

	GIE = 0;
	stq[(stq_start + stq_len) & STQ_MASK] = x;
	stq_len++;
	TXIE = 1;
	GIE = 1;
}

/* Get byte from PC.  Blocks iff receive queue is empty. */
uint8_t serial_get(void)
{
	uint8_t x;

	while(!serial_can_get())
		continue;

	x = srq[srq_start];
	GIE = 0;
	srq_start = (srq_start + 1) & SRQ_MASK;
	srq_len--;
	GIE = 1;
	return x;
}

/*
 * I/O helpers
 */

void serial_put_string(const uint8_t *s)
{
	while(s && *s)
		serial_put(*s++);
}

const uint8_t hex[16]={'0','1','2','3','4','5','6','7',
		       '8','9','a','b','c','d','e','f'};
void serial_put_hex(uint8_t x)
{
	serial_put(hex[x >> 4]);
	serial_put(hex[x & 15]);
}

void serial_put_hex32(uint32_t x)
{
	serial_put_hex((x >> 24) & 0xFF);
	serial_put_hex((x >> 16) & 0xFF);
	serial_put_hex((x >> 8) & 0xFF);
	serial_put_hex((x) & 0xFF);
}

void serial_crlf(void)
{
	serial_put('\r');
	serial_put('\n');
}