/*
 * Loosely modelled after AVR-RS485 by Yoshinori Kohyama (http://algobit.jp/),
 * available at https://github.com/kohyama/AVR-RS485/
 *
 * All bugs by Jan "Yenya" Kasprzak <kas@fi.muni.cz> :-)
 */

#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/atomic.h>
#include <util/delay.h>

#define BUFSIZE 128	// must be a power of two

// configure the control pin
#define ctl_pin_setup()	do { DDRD  |=  _BV(PD2); } while (0)
#define ctl_pin_on()	do { PORTD |=  _BV(PD2); } while (0)
#define ctl_pin_off()	do { PORTD &= ~_BV(PD2); } while (0)

#define BUFMASK (BUFSIZE-1)
#if (BUFSIZE & BUFMASK)
#error BUFSIZE must be a power of two
#endif

#if BUFSIZE > 255
typedef uint16_t bufptr_t;
#else
typedef uint8_t  bufptr_t;
#endif

#define bufptr_inc(x)	((x + 1) & BUFMASK)

static volatile bufptr_t rx_bytes, tx_head, tx_tail;
static volatile uint8_t rxbuf[BUFSIZE], txbuf[BUFSIZE];
static volatile uint16_t last_rx;
static volatile uint8_t unit_id;

#define UART_BAUD	9600
#define UBRR_VAL        ((F_CPU + 8UL * UART_BAUD) / (16UL*UART_BAUD) - 1)
#define wait_one_byte()	_delay_us(10*1000000/UART_BAUD)

#define get_clock()	(TCNT1)
#define CLOCK_SPEED	(F_CPU/1024)
/*
 * According to Wikipedia, it is indeed 28 bits = 3.5 bytes without
 * start- and stopbits.
 */
#define TIMEOUT		(28*CLOCK_SPEED/UART_BAUD)

void modbus_init()
{
	rx_bytes = 0;
	tx_head = tx_tail = 0;

	unit_id = 42;

	ctl_pin_off();
	ctl_pin_setup();

	UBRR0 = UBRR_VAL;
	UCSR0A = 0;
	UCSR0B = _BV(RXCIE0)|_BV(RXEN0)|_BV(TXEN0);
	UCSR0C = _BV(UCSZ01)|_BV(UCSZ00);
}

void rs485_send(char *p)
{
	bufptr_t next;

	if (*p == '\0')
		return;

	ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
		next = bufptr_inc(tx_head);
		while (next != tx_tail && *p != '\0') {
			txbuf[tx_head] = *p++;
			tx_head = next;
			next = bufptr_inc(tx_head);
		}
		ctl_pin_on();
		UCSR0B |= _BV(UDRIE0);
	}
}

static uint16_t compute_crc(volatile uint8_t *buf, bufptr_t len)
{
	bufptr_t i;
	uint16_t crc = 0xFFFF;

	for (i = 0; i < len; i++) {
		uint8_t j;
		crc ^= (uint16_t)(buf[i]);
		for(j = 0; j < 8; j++) {
			if (crc & 0x0001) {
				crc >>= 1;
				crc ^= 0xA001;
			} else {
				crc >>= 1;
			}
		}
	}

	return crc;
}

static void make_exception(uint8_t func, uint8_t code)
{
	txbuf[tx_head++] = unit_id;
	txbuf[tx_head++] = func | 0x80;
	txbuf[tx_head++] = code;
}

void modbus_poll()
{
	bufptr_t packet_len;
	uint16_t crc;

	ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
		if (rx_bytes == 0) // nothing received yet
			return;

		if (get_clock() - last_rx < TIMEOUT) // still receiving
			return;

		if (rx_bytes < 4) { // too short
			rx_bytes = 0;
			return;
		}

		if (rxbuf[0] != unit_id) { // not for myself
			rx_bytes = 0;
			return;
		}

		if (tx_tail) { // still sending?
			rx_bytes = 0;
			return;
		}

		packet_len = rx_bytes; // make a copy
	}

	crc = compute_crc(rxbuf, packet_len - 2);

	if ((crc & 0xFF) != rxbuf[packet_len-2]
		|| (crc >> 8) != rxbuf[packet_len-1]) // bad crc
		goto out;

	tx_head = 0;

	switch (rxbuf[1]) { // function
	default:
		make_exception(rxbuf[1], 1); // illegal function
	}

send:
	if (tx_head) {
		crc = compute_crc(txbuf, tx_head);
		txbuf[tx_head++] = crc & 0xFF;
		txbuf[tx_head++] = crc >> 8;

		tx_tail = 0;

		ctl_pin_on();
		UCSR0B |= _BV(UDRIE0);
	}
out:
	ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
		rx_bytes = 0;
	}
}

ISR(USART_RX_vect)
{
	ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
		rxbuf[rx_bytes] = UDR0;
		if (rx_bytes + 1 < BUFSIZE) // ignore overruns
			rx_bytes++;
		last_rx = get_clock();
	}
}

ISR(USART_UDRE_vect)
{
	ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
		if (tx_head == tx_tail) {
			UCSR0B &= ~_BV(UDRIE0);
			tx_tail = tx_head = 0;
			wait_one_byte(); // FIXME: too long busy-wait
			ctl_pin_off();
		} else {
			UDR0 = txbuf[tx_tail];
			tx_tail = bufptr_inc(tx_tail);
		}
	}
}