/*
 * Modbus_holding_registers.c
 *
 *  Created on: 8. 3. 2020
 *      Author: Ondrej Vitoslavsky
 */


#include "main.h"
#include "Modbus.h"
#include "Modbus_RTU.h"
#include "Modbus_functions.h"
#include "Modbus_registers.h"

volatile uint16_t   holding_register_Buffer[HOLDING_REGISTERS];
extern uint8_t  Transmit_Buffer[MODBUS_ADU_SIZE_MAX];
extern uint16_t Modbus_Address;
uint8_t Settings_change=0;


ModbusException Modbus_Function_Write_Holding_Register(uint8_t* Frame,
	uint16_t* Length)
{
	uint16_t Address;
	uint8_t* PDUFrameResponse;

	Transmit_Buffer[MODBUS_ADU_ADDR_OFF] = Modbus_Address;
	PDUFrameResponse = &Transmit_Buffer[MODBUS_ADU_PDU_OFF];
	PDUFrameResponse[MODBUS_PDU_FUNC_OFF] = MODBUS_FUNC_WRITE_REGISTER;



	Address = Modbus_Byte_to_Word(Frame, MODBUS_FUNC_WRITE_ADDR_OFF);
	//check length of frame
	if (*Length != (MODBUS_FUNC_WRITE_SIZE + MODBUS_ADU_PDU_OFF))
	{
		return EXCEPTION_ILLEGAL_DATA_VALUE;
	}
	else if (Address + 1 >= HOLDING_REGISTERS) {
		return EXCEPTION_ILLEGAL_DATA_ADDRESS;
	}
	else
	{
		//copy registers from frame to holding_register_Buffer
		Modbus_Holding_Register_Write(
			&Frame[MODBUS_FUNC_WRITE_VALUE_OFF], Address, 1);


		//build response frame
		*Length = 2;	//already has ID and function code
		PDUFrameResponse[MODBUS_FUNC_WRITE_ADDR_OFF] = (Address >> 8);
		PDUFrameResponse[MODBUS_FUNC_WRITE_ADDR_OFF + 1] = Address & 0xFF;
		*Length += 2; //plus register address
		//copy registers from holding_register_Buffer to frame
		Modbus_Holding_Register_Read(
			&PDUFrameResponse[MODBUS_FUNC_WRITE_VALUE_OFF], Address, 1);
		*Length += 2; //plus data

		return EXCEPTION_NONE;
	}
}

ModbusException Modbus_Function_Write_Multiple_Holding_Register(uint8_t* Frame,
	uint16_t* Length)
{
	uint16_t Address;
	uint16_t Count;
	uint16_t ByteCount;

	uint8_t* PDUFrameResponse;

	Transmit_Buffer[MODBUS_ADU_ADDR_OFF] = Modbus_Address;
	PDUFrameResponse = &Transmit_Buffer[MODBUS_ADU_PDU_OFF];
	PDUFrameResponse[MODBUS_PDU_FUNC_OFF] = MODBUS_FUNC_WRITE_MULTIPLE_REGISTERS;
	//check length of frame
	if (*Length < (MODBUS_FUNC_WRITE_MUL_SIZE_MIN + MODBUS_ADU_PDU_OFF))
	{
		// incorrect length
		return EXCEPTION_ILLEGAL_DATA_VALUE;
	}
	else
	{
		Address = Modbus_Byte_to_Word(Frame, MODBUS_FUNC_WRITE_MUL_ADDR_OFF);
		Count = Modbus_Byte_to_Word(Frame, MODBUS_FUNC_WRITE_MUL_REGCNT_OFF);
		ByteCount = Frame[MODBUS_FUNC_WRITE_MUL_BYTECNT_OFF];

		//valid data check
		if ((ByteCount != 2 * Count) || (Count == 0))
		{
			return EXCEPTION_ILLEGAL_DATA_VALUE;

		}
		else if (Address + Count >= HOLDING_REGISTERS) {
			return EXCEPTION_ILLEGAL_DATA_ADDRESS;
		}
		else
		{
			//copy registers from frame to holding_register_Buffer
			Modbus_Holding_Register_Write(
				&Frame[MODBUS_FUNC_WRITE_MUL_VALUES_OFF], Address, Count);

			//build response frame
			*Length = 2;	//already has ID and function code
			PDUFrameResponse[MODBUS_FUNC_WRITE_MUL_ADDR_OFF] = (Address >> 8);
			PDUFrameResponse[MODBUS_FUNC_WRITE_MUL_ADDR_OFF + 1] = Address & 0xFF;
			*Length += 2; //plus register address
			PDUFrameResponse[MODBUS_FUNC_WRITE_MUL_REGCNT_OFF] = (Count >> 8);
			PDUFrameResponse[MODBUS_FUNC_WRITE_MUL_REGCNT_OFF + 1] = Count & 0xFF;
			*Length += 2; //plus Register Count

			return EXCEPTION_NONE;
		}
	}
}



ModbusException Modbus_Function_Read_Holding_Register(uint8_t* Frame,
	uint16_t* Length)
{
	uint16_t Address;
	uint16_t Count;

	uint8_t* PDUFrameResponse;

	Transmit_Buffer[MODBUS_ADU_ADDR_OFF] = Modbus_Address;
	PDUFrameResponse = &Transmit_Buffer[MODBUS_ADU_PDU_OFF];
	PDUFrameResponse[MODBUS_PDU_FUNC_OFF] = MODBUS_FUNC_READ_HOLDING_REGISTER;

	//check length of frame
	if (*Length != (MODBUS_FUNC_READ_SIZE + MODBUS_ADU_PDU_OFF))
	{
		//length of frame is incorrect
		return EXCEPTION_ILLEGAL_DATA_VALUE;
	}
	else
	{
		Address = Modbus_Byte_to_Word(Frame, MODBUS_FUNC_READ_ADDR_OFF);
		Count = Modbus_Byte_to_Word(Frame, MODBUS_FUNC_READ_REGCNT_OFF);

		//If number of registers to read is not valid return illegal data exception.
		if (Count == 0)
		{
			return EXCEPTION_ILLEGAL_DATA_VALUE;
		}
		else if (Address + Count >= HOLDING_REGISTERS) {
			return EXCEPTION_ILLEGAL_DATA_ADDRESS;
		}
		else
		{
			//build response frame
			*Length = 2;	//already has ID and function code
			PDUFrameResponse[MODBUS_FUNC_READ_BYTECNT_OFF] = (uint8_t)(Count * 2);
			*Length += 1; //plus Byte Count
			//copy data from holding_register_Buffer to Frame
			Modbus_Holding_Register_Read(&PDUFrameResponse[MODBUS_FUNC_READ_VALUES_OFF],
				Address, Count);
			*Length += 2 * Count; //plus Register Count

			return EXCEPTION_NONE;

		}
	}
}



ModbusException Modbus_Function_Read_Write_Multiple_Holding_Register(uint8_t* Frame,
	uint16_t* Length)
{
	uint16_t ReadAddress;
	uint16_t ReadCount;
	uint16_t WriteAddress;
	uint16_t WriteCount;
	uint8_t WriteByteCount;

	uint8_t* PDUFrameResponse;

	Transmit_Buffer[MODBUS_ADU_ADDR_OFF] = Modbus_Address;
	PDUFrameResponse = &Transmit_Buffer[MODBUS_ADU_PDU_OFF];
	PDUFrameResponse[MODBUS_PDU_FUNC_OFF] = MODBUS_FUNC_READWRITE_MULTIPLE_REGISTERS;

	if (*Length < (MODBUS_FUNC_READWRITE_SIZE_MIN + MODBUS_ADU_PDU_OFF))
	{
		return EXCEPTION_ILLEGAL_DATA_VALUE;

	}
	else {

		ReadAddress = Modbus_Byte_to_Word(Frame, MODBUS_FUNC_READWRITE_READ_ADDR_OFF);
		ReadCount = Modbus_Byte_to_Word(Frame, MODBUS_FUNC_READWRITE_READ_REGCNT_OFF);
		WriteAddress = Modbus_Byte_to_Word(Frame, MODBUS_FUNC_READWRITE_WRITE_ADDR_OFF);
		WriteCount = Modbus_Byte_to_Word(Frame, MODBUS_FUNC_READWRITE_WRITE_REGCNT_OFF);
		WriteByteCount = Frame[MODBUS_FUNC_READWRITE_WRITE_BYTECNT_OFF];

		if ((ReadCount == 0) || (WriteCount == 0) ||
			((2 * WriteCount) != WriteByteCount))
		{
			return EXCEPTION_ILLEGAL_DATA_VALUE;
		}
		else if (ReadAddress + ReadCount >= HOLDING_REGISTERS) {
			return EXCEPTION_ILLEGAL_DATA_ADDRESS;
		}
		else if (WriteAddress + WriteCount >= HOLDING_REGISTERS) {
			return EXCEPTION_ILLEGAL_DATA_ADDRESS;
		}
		else
		{
			//copy data from Frame to holding_register_Buffer
			Modbus_Holding_Register_Write(&Frame[MODBUS_FUNC_READWRITE_WRITE_VALUES_OFF],
				WriteAddress, WriteCount);
			//build response frame
			*Length = 2;	//already has ID and function code
			PDUFrameResponse[MODBUS_FUNC_READWRITE_READ_BYTECNT_OFF] = (uint8_t)(ReadCount * 2);
			*Length += 1; //plus Byte Count
			//copy data from holding_register_Buffer to Frame
			Modbus_Holding_Register_Read(&PDUFrameResponse[MODBUS_FUNC_READWRITE_READ_VALUES_OFF],
				ReadAddress, ReadCount);
			*Length += 2 * ReadCount; //plus Register Count

			return EXCEPTION_NONE;

		}
	}
}


void Modbus_Holding_Register_Write(uint8_t* Frame,
	uint16_t RegisterAddress, uint16_t RegCount)
{

	while (RegCount > 0)
	{
		holding_register_Buffer[RegisterAddress] = *(Frame++) << 8;
		holding_register_Buffer[RegisterAddress++] |= *(Frame++);
		RegCount--;
	}

}


void Modbus_Holding_Register_Read(uint8_t* Frame,
	uint16_t RegisterAddress, uint16_t RegCount)
{
	while (RegCount > 0)
	{
		*Frame++ = (uint8_t)(holding_register_Buffer[RegisterAddress] >> 8);
		*Frame++ = (uint8_t)(holding_register_Buffer[RegisterAddress++] & 0xFF);
		RegCount--;
	}
}





uint16_t Modbus_Byte_to_Word(uint8_t* Frame, uint16_t Address) {
	uint16_t Word = (Frame[Address] << 8);
	Word |= (Frame[Address + 1]);
	return Word;
}

