/*
 * 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 180 // maximum request size

// 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)

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

static volatile bufptr_t buf_len, tx_ptr;
static volatile uint8_t buffer[BUFSIZE], transmitting;
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 = {
	99,
	//0   1    2   3   4    5    6    7   8   9    10   11
	//1   2    3   4   5    6    7    8   9   10   11   12
	220, 220, 220, 220, 220, 220, 220, 220, 220, 220, 0, 0,
	0x3E0, // LED 1
	0x01F, // LED 2
};

#endif

void modbus_init(uint8_t unit)
{
	buf_len = 0;
	transmitting = 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();

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

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)
{
	buffer[1] |= 0x80;
	buffer[2] = code;
	buf_len = 3;
}

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

void put_word(uint16_t word)
{
	buffer[buf_len++] = word >> 8;
	buffer[buf_len++] = 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;
}

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

	ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
		if (transmitting)
			return 0;

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

		if (buf_len < 4) { // too short (or not for us)
			buf_len = 0;
			return 0;
		}

		transmitting = 1; // disable further reads
		packet_len = buf_len;
	}

	crc = compute_crc(buffer, packet_len - 2);

	if ((crc & 0xFF) != buffer[packet_len-2]
		|| (crc >> 8) != buffer[packet_len-1]) { // bad CRC
		ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
			transmitting = 0;
			buf_len = 0;
		}
		return 1;
	}

	packet_len -= 2; // strip the CRC
	buf_len = 2; // keep the first two bytes (unit ID and function) for TX

	rv = MB_ILLEGAL_FUNC;

	switch (buffer[1]) { // function
	case 3:
		if (packet_len == 6)
			rv = read_holding_regs(
				get_word(buffer, 2),
				get_word(buffer, 4)
			);
		break;
	case 6:
		if (packet_len == 6)
			rv = write_single_reg(
				get_word(buffer, 2),
				get_word(buffer, 4)
			);
		break;
	}
	
	if (rv)
		make_exception(rv);

	// append the CRC
	crc = compute_crc(buffer, buf_len);
	put_byte(crc & 0xFF);
	put_byte(crc >> 8);

	// send out the reply
	tx_ptr = 0;
	ctl_pin_on();
	UCSR0B |= _BV(UDRIE0);

	return 1;
}

ISR(USART_RX_vect)
{
	uint8_t rx_byte = UDR0;
	clock_t now = get_clock();

	if (transmitting) // how did we get here? discard it
		goto out;

	if (buf_len && buffer[0] != mb_unit_id) // not for us
		goto out;

	if (buf_len == BUFSIZE) { // overrun - discard the packet
		buffer[0] = 0xFF;
		buf_len = 1;
		goto out;
	}

	if (now - last_rx >= REQ_TIMEOUT) { // new packet; start over
		buf_len = 0;
	}

	// TODO: we can probably calculate the CRC here as well
	buffer[buf_len++] = rx_byte;
out:
	last_rx = now;
}

ISR(USART_TX_vect)
{
	UCSR0B &= ~_BV(TXCIE0); // disable further IRQs
	ctl_pin_off();
	buf_len = 0;
	transmitting = 0; // enable receiving
}

ISR(USART_UDRE_vect)
{
	if (tx_ptr >= buf_len) {
		UCSR0A |= _BV(TXC0); // clear the pending TXC flag
		UCSR0B |= _BV(TXCIE0); // enable xmit complete irq
		UCSR0B &= ~_BV(UDRIE0); // disable ourselves
	} else {
		UDR0 = buffer[tx_ptr++];
	}
}