/*
 * 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/eeprom.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/atomic.h>
#include <util/delay.h>

#include "clock.h"
#include "modbus.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;
#ifndef mb_unit_id
static uint8_t mb_unit_id;
#endif

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

/*
 * According to Wikipedia, it is indeed 28 bits = 3.5 bytes without
 * start- and stopbits.
 */
#define REQ_TIMEOUT		(28*CLOCK_HZ/UART_BAUD)

uint16_t hold_regs[MB_N_HOLD_REGS];

#if MB_N_HOLD_REGS_EEPROM > 0
static uint16_t hold_regs_ee[MB_N_HOLD_REGS_EEPROM] EEMEM = {
	42, 
	0, 0, 0, 30, 30, 30, 30, 0, 0, 0, 0, 30,
	(1 << 4) | (1 << 11), // LED 1
	0, // LED 2
};

#endif

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

	if (unit)
		mb_unit_id = unit;
#if MB_N_HOLD_REGS_EEPROM > 0
	do {
		int i;
		for (i = 0; i < MB_N_HOLD_REGS_EEPROM; i++)
			hold_regs[i] = eeprom_read_word(&hold_regs_ee[i]);
	} while (0);
#endif

	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(mb_exception code)
{
	txbuf[1] |= 0x80;
	txbuf[2] = code;
	tx_head = 3;
}

#define get_word(ptr, off) (((uint16_t)ptr[off] << 8) | ptr[off+1])
void put_byte(uint8_t byte)
{
	txbuf[tx_head++] = byte;
}

void put_word(uint16_t word)
{
	txbuf[tx_head++] = word >> 8;
	txbuf[tx_head++] = word & 0xFF;
}

static mb_exception read_holding_regs(uint16_t start, uint16_t len)
{
	if (len > BUFSIZE/2 - 3)
		return MB_ILLEGAL_ADDR;

	if (start < MB_HOLD_REGS_BASE
		|| start + len > MB_HOLD_REGS_BASE + MB_N_HOLD_REGS)
		return MB_ILLEGAL_ADDR;

	put_byte(2*len);

	start -= MB_HOLD_REGS_BASE;
	while(len--)
		put_word(hold_regs[start++]);

	return MB_OK;
}

static mb_exception write_single_reg(uint16_t reg, uint16_t val)
{
	if (reg < MB_HOLD_REGS_BASE
		|| reg >= MB_HOLD_REGS_BASE + MB_N_HOLD_REGS)
		return MB_ILLEGAL_ADDR;

	if (!hold_reg_is_valid(reg, val))
		return MB_ILLEGAL_VAL;

	reg -= MB_HOLD_REGS_BASE;
	hold_regs[reg] = val;
#if MB_N_HOLD_REGS_EEPROM > 0
	if (reg < MB_N_HOLD_REGS_EEPROM)
		eeprom_write_word(&hold_regs_ee[reg], val);
#endif
	put_word(reg + MB_HOLD_REGS_BASE);
	put_word(val);

	return MB_OK;
}

void modbus_poll()
{
	bufptr_t packet_len;
	uint16_t crc;
	uint8_t rv;

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

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

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

		if (rxbuf[0] != mb_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;

	txbuf[0] = rxbuf[0]; // not mb_unit_id in case it gets changed
	txbuf[1] = rxbuf[1];
	tx_head = 2;

	rv = MB_OK;
	switch (rxbuf[1]) { // function
	case 3:
		rv = read_holding_regs(get_word(rxbuf, 2), get_word(rxbuf, 4));
		break;
	case 6:
		rv = write_single_reg(get_word(rxbuf, 2), get_word(rxbuf, 4));
		break;
	default:
		make_exception(MB_ILLEGAL_FUNC); // illegal function
	}
	
	if (rv)
		make_exception(rv);
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_TX_vect)
{
	UCSR0B &= ~_BV(TXCIE0); // disable further IRQs
	ctl_pin_off();
}

ISR(USART_UDRE_vect)
{
	ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
		if (tx_head == tx_tail) {
			UCSR0B |= _BV(TXCIE0); // enable xmit complete irq
			UCSR0B &= ~_BV(UDRIE0);
			tx_tail = tx_head = 0;
		} else {
			UDR0 = txbuf[tx_tail];
			tx_tail = bufptr_inc(tx_tail);
		}
	}
}