/*
 * ul_modbus.c
 *
 *  Created on: Mar 26, 2012
 *      Author: qwer1
 */

#include "ul_modbus.h"
#include "ul_crc.h"

void modbus_reset(struct modbus_struct *mb) {
	mb->buf_cnt = 0; //reset input buffer
	mb->timeout = 0; //reset timeout counter
	modbus_reset_crc(mb);
}

void modbus_reset_crc(struct modbus_struct *mb) {
	mb->crc = CRC16_MODBUS_INIT; //reset CRC
}

U16 modbus_crc(struct modbus_struct *mb, U8 size) {
	return crc16_modbus_buf(&(mb->buf[0]), size, CRC16_MODBUS_INIT);
}

U8 modbus_slave_receive(struct modbus_struct *mb, U8 data) {
	mb->timeout = 0; //reset timeout counter
	if (mb->buf_cnt < MODBUS_BUF_SIZE) {
		mb->buf[mb->buf_cnt] = data;
		if (mb->buf_cnt > 1) {
			inc_crc16_modbus_upd(mb->buf[mb->buf_cnt - 2], mb->crc);
			//CRC is right ?
			if ((mb->crc == modbus_get_U16(&(mb->buf[mb->buf_cnt - 1])))&&
				(mb->buf[0] == mb->addr)&& //address is right ?
				//command is right ?
				((mb->buf[1] == MODBUS_CMD_READ_HOLDING_REGISTERS)||
				 (mb->buf[1] == MODBUS_CMD_WRITE_SINGLE_REGISTER))
			) {
//log_led1_flip();
				return 1;
			}
		}
		mb->buf_cnt++;
	}
	return 0;
}

//create response packet, returns number of bytes
U8 modbus_slave_respond(struct modbus_struct *mb) {
	U8 *buf = &(mb->buf[0]);
	if ((*buf == mb->addr)&&(*(buf + 1) == MODBUS_CMD_READ_HOLDING_REGISTERS)) {
		buf += 2;
		U16 regs_addr = modbus_get_U16(buf); //address of first register
		U16 regs_num = modbus_get_U16(buf + 2);  //number of registers
		if ((regs_num > 0)&&
			(regs_addr >= MODBUS_REGS_ADDR)&&
			(regs_addr <= (MODBUS_REGS_ADDR + (MODBUS_REGS_NUM - 1) * 2))&&
			((regs_addr + (regs_num - 1) * 2) <= (MODBUS_REGS_ADDR + (MODBUS_REGS_NUM - 1) * 2))
		) {
			*buf++ = regs_num << 1; //number of bytes in response
			//add register values
			for (U8 i=0; i<regs_num; i++) {
				U16 val = modbus_reg_get(regs_addr);
				*buf++ = val >> 8;
				*buf++ = val;
				regs_addr++;
			}
			U16 crc = modbus_crc(mb, (regs_num << 1) + 3);
			*buf++ = crc >> 8;
			*buf = crc;
			return (regs_num << 1) + 5; //return number of bytes in packet
		}
	}
	return 0;
}

S16 modbus_reg_rd(struct modbus_reg_struct *reg, U16 reg_num) {
	if (reg != NULL) {
		reg += reg_num;
		switch (reg->type) {
			case MODBUS_REG_TY_BIT:
				return *((U8 *)(reg->buf));
				break;
			case MODBUS_REG_TY_U8:
				return *((U8 *)(reg->buf));
				break;
			case MODBUS_REG_TY_S8:
				return *((S8 *)(reg->buf));
				break;
			case MODBUS_REG_TY_U16:
				return *((U16 *)(reg->buf));
				break;
			case MODBUS_REG_TY_S16:
				return *((S16 *)(reg->buf));
				break;
			case MODBUS_REG_TY_U32L:
				return (*((U32 *)(reg->buf))) & 0xFFFF;
				break;
			case MODBUS_REG_TY_U32H:
				return (*((U32 *)(reg->buf))) >> 16;
				break;
			case MODBUS_REG_TY_S32L:
				return (*((U32 *)(reg->buf))) & 0xFFFF;
				break;
			case MODBUS_REG_TY_S32H:
				return (*((U32 *)(reg->buf))) >> 16;
				break;
			default:
				return 0;
		}
	}
	return 0;
}

void modbus_reg_wr(struct modbus_reg_struct *reg, U16 reg_num, S16 reg_val) {
	if (reg != NULL) {
		reg += reg_num;
		if (reg->wr) switch (reg->type) {
			case MODBUS_REG_TY_BIT:
				*((U8 *)(reg->buf)) = ((U8)reg_val & 1);
				break;
			case MODBUS_REG_TY_U8:
				*((U8 *)(reg->buf)) = (U8)reg_val;
				break;
			case MODBUS_REG_TY_S8:
				*((S8 *)(reg->buf)) = (S8)reg_val;
				break;
			case MODBUS_REG_TY_U16:
				*((U16 *)(reg->buf)) = (U16)reg_val;
				break;
			case MODBUS_REG_TY_S16:
				*((S16 *)(reg->buf)) = (S16)reg_val;
				break;
			case MODBUS_REG_TY_U32L:
				(*((U32 *)(reg->buf))) = ((*((U32 *)(reg->buf))) & 0xFFFF0000) | (U16)reg_val;
				break;
			case MODBUS_REG_TY_U32H:
				(*((U32 *)(reg->buf))) = ((*((U32 *)(reg->buf))) & 0x0000FFFF) | ((U32)((U16)reg_val) << 16);
				break;
			case MODBUS_REG_TY_S32L:
				(*((U32 *)(reg->buf))) = ((*((U32 *)(reg->buf))) & 0xFFFF0000) | (U16)reg_val;
				break;
			case MODBUS_REG_TY_S32H:
				(*((U32 *)(reg->buf))) = ((*((U32 *)(reg->buf))) & 0x0000FFFF) | ((U32)((U16)reg_val) << 16);
				break;
		}
	}
}
