#include "common.h"
#include "bus/usart.h"

#include <avr/io.h>
#include <avr/interrupt.h>

#define USART_BAUDRATE    9600
#define USART_RXBUF_SIZE  512
#define USART_TXBUF_SIZE  512

#define USART_RXBUF_MASK (USART_RXBUF_SIZE - 1)
#define USART_TXBUF_MASK (USART_TXBUF_SIZE - 1)
#define USART_BAUD_PRESCALE ((((F_CPU / 16) + \
        (USART_BAUDRATE / 2)) / (USART_BAUDRATE)) - 1)

static volatile char rxbuf[USART_RXBUF_SIZE]; // RX ring buffer
static volatile char txbuf[USART_TXBUF_SIZE]; // TX ring buffer
static volatile short rxhead, txhead; // Current write position
static volatile short rxtail, txtail; // Current read position

int USART_Init(void)
{
	rxhead = 0;
	rxtail = 0;
	txhead = 0;
	txtail = 0;

	UCSRB = BIT(RXCIE); // Handle RXC interrupts
	UCSRB |= BIT(RXEN) | BIT(TXEN); // Enable RX and TX circuitry
	UCSRC = BIT(URSEL) | BIT(UCSZ0) | BIT(UCSZ1); // Using 8-bit chars
	UBRRH = (USART_BAUD_PRESCALE >> 8); // Set baud rate upper byte
	UBRRL = USART_BAUD_PRESCALE; // Set baud rate lower byte

	return 0;
}

char USART_Getc(void)
{
	if (rxhead == rxtail) {
		return -1;
	}

	rxtail = (rxtail + 1) & USART_RXBUF_MASK;
	return rxbuf[rxtail];
}

void USART_Putc(char ch)
{
	short head;

	head = (txhead + 1) & USART_TXBUF_MASK;
	while (head == txtail); // Wait for space

	txbuf[head] = ch;
	txhead = head;

	// Enable interrupt
	UCSRB |= BIT(UDRIE);
}

// INT: Rx complete
ISR(USART_RXC_vect)
{
	short head;

	// Next byte ready to read from serial
	head = (rxhead + 1) & USART_RXBUF_MASK;
	// Free space in RX buffer?
	if (head != rxtail) {
		// Copy from register
		rxbuf[head] = UDR;
		rxhead = head;
	} else {
		// XXX: Discard overflow
	}
}

// INT: Data register empty
ISR(USART_UDRE_vect)
{
	short tail;

	// Anything in TX buffer?
	if (txhead != txtail) {
		// Write next byte to data register
		tail = (txtail + 1) & USART_TXBUF_MASK;
		UDR = txbuf[tail];
		txtail = tail;
	} else {
		// Disable interrupt
		UCSRB &= ~BIT(UDRIE);
	}
}
