/************************************************************************/
/* Copyright 2009-2016 MBARI											*/
/************************************************************************/
/* Summary	: ModBus Functions for Oasis5 controller					*/
/* Filename : modbus.c													*/
/* Author	: Robert Herlien (rah)										*/
/* Project	: Oasis5 Mooring Controller									*/
/* Revision: 1.0														*/
/* Created	: 08/31/2016												*/
/*																			*/
/* MBARI provides this documentation and code "as is", with no warranty,	*/
/* express or implied, of its quality or consistency. It is provided without*/
/* support and without obligation on the part of the Monterey Bay Aquarium	*/
/* Research Institute to assist in its use, correction, modification, or	*/
/* enhancement. This information should not be published or distributed to	*/
/* third parties without specific written permission from MBARI.			*/
/*																			*/
/************************************************************************/
/* Modification History:												*/
/* 07apr2009 rah - created Oasis4 version								*/
/* 31aug2016 rah - created Oasis5 version from Oasis4 version			*/
/************************************************************************/

#include <mbariTypes.h>					/* MBARI type definitions		*/
#include <oasis.h>						/* OASIS controller definitions */
#include <custom.h>						/* Custom def's for deployment	*/
#include <dig_io.h>						/* OASIS I/O definitions		*/
#include <olist.h>						/* OASIS Linked List library	*/
#include <sem.h>						/* OASIS Semaphore library		*/
#include <serial.h>						/* OASIS Serial I/O				*/		
#include <modbus.h>						/* Modbus protocol library		*/
#include <modbusio.h>					/* Modbus I/O					*/


/************************************************************************/
/* Function	   : buildModBusQuery										*/
/* Purpose	   : Build Query Header										*/
/* Inputs	   : Buffer, slave, func, start addr, number				*/
/* Outputs	   : Errno (see modbus.h)									*/
/************************************************************************/
MLocal Nat16 buildModBusQuery(volatile Byte *buf, Nat16 slave, Nat16 func,
							  Nat16 startAddr, Nat16 num)
{
	volatile Byte *p = buf;

	*p++ = slave;
	*p++ = func;
	startAddr -= 1;				//Coil/reg 1 transferred as 0
	*p++ = (startAddr >> 8);
	*p++ = (startAddr & 0x00ff);
	*p++ = (num >> 8);
	*p++ = (num & 0x00ff);

	return(BASE_QUERY_LEN);

} /* buildModBusQuery() */


/************************************************************************/
/* Function	   : readBits												*/
/* Purpose	   : Handles Read Coils/Inputs functions of ModBus (01, 02) */
/* Inputs	   : Handle, slave, func start addr, number of bits, result ptr*/
/* Outputs	   : Errno (see modbus.h)									*/
/************************************************************************/
MLocal Errno readBits(ModBusHandle mh, RemID *rp, Nat16  func,
					  Nat16 startAddr, Nat16 numBits, Byte *result)
{
	Nat16		sndLen;
	Errno		rtn;

	if (numBits > MAX_BITS)
		return(DATA_SIZE_ERROR);

	if ((rtn = getModbusPort(mh)) != OK)
		return(rtn);
	
	sndLen = buildModBusQuery(mh->ioBuf->sndBuf, rp->slaveID, func, startAddr, numBits);

	if ((rtn = modBusIO(mh, rp, sndLen)) == OK)
		memcpy(result, (char *)mh->ioBuf->rcvBuf+3, (size_t)mh->ioBuf->rcvBuf[2]);

	releaseModbusPort(mh);
	return(rtn);

} /* readBits() */


/************************************************************************/
/* Function	   : readCoils												*/
/* Purpose	   : Implements Read Coils Function (01) of ModBus spec		*/
/* Inputs	   : Handle, slave, coil start addr, number of coils, result*/
/* Outputs	   : Errno (see modbus.h)									*/
/************************************************************************/
Errno readCoils(ModBusHandle mh, RemID *rp, Nat16 startAddr,
				Nat16 numCoils, Byte *result)
{
	return(readBits(mh, rp, FC_READ_COILS, startAddr, numCoils, result));

} /* readCoils() */


/************************************************************************/
/* Function	   : readInputs												*/
/* Purpose	   : Implements Read Inputs Function (02) of ModBus spec	*/
/* Inputs	   : Handle, slave, input start addr, number of coils, result*/
/* Outputs	   : Errno													*/
/************************************************************************/
Errno readInputs(ModBusHandle mh, RemID *rp, Nat16 startAddr,
				 Nat16 numInputs, Byte *result)
{
	return(readBits(mh, rp, FC_READ_INPUTS, startAddr, numInputs, result));

} /* readInputs() */


/************************************************************************/
/* Function	   : readRegs												*/
/* Purpose	   : Handles Read Holding/Input Regs Functions (03,04) of spec*/
/* Inputs	   : Handle, slave, reg start addr, number of regs, result	*/
/* Outputs	   : Errno													*/
/************************************************************************/
MLocal Errno readRegs(ModBusHandle mh, RemID *rp, Nat16 func,
					  Nat16 startAddr, Nat16 numRegs, Nat16 *result)
{
	Nat16		sndLen, i;
	Errno		rtn;
	Byte		*p;

	if (numRegs > MAX_REGS)
		return(DATA_SIZE_ERROR);

	if ((rtn = getModbusPort(mh)) != OK)
		return(rtn);
	
	sndLen = buildModBusQuery((Byte *)mh->ioBuf->sndBuf, rp->slaveID,
							  func, startAddr, numRegs);

	if ((rtn = modBusIO(mh, rp, sndLen)) == OK)
		for (i = 0, p = (Byte *)mh->ioBuf->rcvBuf+3; i < numRegs; i++)
		{
			result[i] = (p[0] << 8) + p[1];
			p += 2;
		}

	releaseModbusPort(mh);
	return(rtn);

} /* readRegs() */


/************************************************************************/
/* Function	   : readHoldingRegs										*/
/* Purpose	   : Implements Read Holding Regs Function (03) of spec		*/
/* Inputs	   : Handle, slave, reg start addr, number of regs, result	*/
/* Outputs	   : Errno													*/
/************************************************************************/
Errno readHoldingRegs(ModBusHandle mh, RemID *rp, Nat16 startAddr,
						Nat16 numRegs, Nat16 *result)
{
	return(readRegs(mh, rp, FC_READ_HOLDING_REGS, startAddr,
					numRegs, result));

} /* readHoldingRegs() */


/************************************************************************/
/* Function	   : readInputRegs											*/
/* Purpose	   : Implements Read Input Regs Function (04) of ModBus spec */
/* Inputs	   : Handle, slave, coil start addr, number of coils, result*/
/* Outputs	   : Errno													*/
/************************************************************************/
Errno readInputRegs(ModBusHandle mh, RemID *rp, Nat16 startAddr,
					Nat16 numRegs, Nat16 *result)
{
	return(readRegs(mh, rp, FC_READ_INPUT_REGS, startAddr,
					numRegs, result));

} /* readInputRegs() */


/************************************************************************/
/* Function	   : writeOneCoil											*/
/* Purpose	   : Implements Write One Coil Function (05) of ModBus		*/
/* Inputs	   : Handle, slave, coil start addr, number of coils, value */
/* Outputs	   : Errno													*/
/************************************************************************/
Errno writeOneCoil(ModBusHandle mh, RemID *rp, Nat16 coilAddr, MBool on)
{
	Errno		rtn;

	if ((rtn = getModbusPort(mh)) != OK)
		return(rtn);
	
	rtn = modBusIO(mh, rp, 
				   buildModBusQuery(mh->ioBuf->sndBuf, rp->slaveID,
									FC_WRITE_ONE_COIL,
									coilAddr, (on ? 0xFF00 : 0)));

	releaseModbusPort(mh);
	return(rtn);

} /* writeOneCoil() */


/************************************************************************/
/* Function	   : writeOneReg											*/
/* Purpose	   : Implements Write One Reg Function (06) of ModBus		*/
/* Inputs	   : Handle, slave, coil start addr, number of coils, value */
/* Outputs	   : Errno													*/
/************************************************************************/
Errno writeOneReg(ModBusHandle mh, RemID *rp, Nat16 regAddr, Nat16 value)
{
	Errno		rtn;

	if ((rtn = getModbusPort(mh)) != OK)
		return(rtn);
	
	rtn = modBusIO(mh, rp, 
				   buildModBusQuery(mh->ioBuf->sndBuf, rp->slaveID,
									FC_WRITE_ONE_REG, regAddr, value));
	releaseModbusPort(mh);
	return(rtn);

} /* writeOneReg() */


/************************************************************************/
/* Function	   : writeCoils												*/
/* Purpose	   : Implements Write Multiple Coils Function (15) of ModBus*/
/* Inputs	   : Handle, slave, coil start addr, number of coils, data	*/
/* Outputs	   : Errno													*/
/************************************************************************/
Errno writeCoils(ModBusHandle mh, RemID *rp, Nat16 startAddr,
				 Nat16 numCoils, const Byte *data)
{
	Nat16		sndLen, i, numBytes;
	Byte		*p = (Byte *)mh->ioBuf->sndBuf;
	Errno		rtn;

	if (numCoils > MAX_BITS)
		return(DATA_SIZE_ERROR);

	if ((rtn = getModbusPort(mh)) != OK)
		return(rtn);
	
	sndLen = buildModBusQuery(p, rp->slaveID, FC_WRITE_MULT_COILS,
							  startAddr, numCoils);

	numBytes = (numCoils+7)/8;
	p += sndLen;
	*p++ = numBytes;
	sndLen = 7 + numBytes;

	for (i = 0; i < numBytes; i++)
		*p++ = data[i];

	rtn = modBusIO(mh, rp, sndLen);
	releaseModbusPort(mh);
	return(rtn);

} /* writeCoils() */


/************************************************************************/
/* Function	   : writeRegs												*/
/* Purpose	   : Implements Write Multiple Regs Function (16) of ModBus */
/* Inputs	   : Handle, slave, reg start addr, number of regs, data	*/
/* Outputs	   : Errno													*/
/************************************************************************/
Errno writeRegs(ModBusHandle mh, RemID *rp, Nat16 startAddr,
				Nat16 numRegs, const Nat16 *data)
{
	Nat16		sndLen, i;
	Byte		*p = (Byte *)mh->ioBuf->sndBuf;
	Errno		rtn;

	if (numRegs > MAX_REGS)
		return(DATA_SIZE_ERROR);

	if ((rtn = getModbusPort(mh)) != OK)
		return(rtn);
	
	sndLen = buildModBusQuery(p, rp->slaveID, FC_WRITE_MULT_REGS,
							  startAddr, numRegs);

	p += sndLen;
	*p++ = (2 * numRegs);
	sndLen = 7 + (2*numRegs);

	for (i = 0; i < numRegs; i++)
	{
		*p++ = (data[i] >> 8);
		*p++ = (data[i] & 0xff);
	}

	rtn = modBusIO(mh, rp, sndLen);
	releaseModbusPort(mh);
	return(rtn);

} /* writeRegs() */


/************************************************************************/
/* Function	   : reportSlaveId											*/
/* Purpose	   : Implements Report Slave ID Function (17) of ModBus		*/
/* Inputs	   : Handle, slave, coil start addr, number of coils, result*/
/* Outputs	   : Errno													*/
/************************************************************************/
Errno reportSlaveId(ModBusHandle mh, RemID *rp, Byte *result)
{
	Errno		rtn;

	if ((rtn = getModbusPort(mh)) != OK)
		return(rtn);
	
	mh->ioBuf->sndBuf[0] = rp->slaveID;
	mh->ioBuf->sndBuf[1] = FC_REPORT_SLAVE_ID;

	if ((rtn = modBusIO(mh, rp, 2)) == OK)
		memcpy(result,(Byte *) mh->ioBuf->rcvBuf+3, mh->ioBuf->rcvBuf[2]);

	releaseModbusPort(mh);
	return(rtn);

} /* reportSlaveId() */


/************************************************************************/
/* Function	   : writeReadRegs											*/
/* Purpose	   : Implements Read/Write Mult Regs Function (23) of ModBus*/
/* Inputs	   : Handle, slave, reg start addr, number of write regs,	*/
/*				 write data, number of read regs, ptr to read data		*/
/* Outputs	   : Errno													*/
/************************************************************************/
Errno writeReadRegs(ModBusHandle mh, RemID *rp, Nat16 readAddr,
					Nat16 numReads, Nat16 writeAddr, Nat16 numWrites,
					const Nat16 *writeData, Nat16 *readData)
{
	Nat16		sndLen, i;
	Errno		rtn;
	Byte		*p = (Byte *)mh->ioBuf->sndBuf;

	if ((numReads > MAX_REGS) || (numWrites > MAX_REGS-4))
		return(DATA_SIZE_ERROR);

	if ((rtn = getModbusPort(mh)) != OK)
		return(rtn);
	
	sndLen = buildModBusQuery(p, rp->slaveID, FC_WRITE_READ_MULT_REGS,
							  readAddr, numReads);
	p += sndLen;
	writeAddr -= 1;						/* Reg addresses start at 0		*/
	*p++ = (writeAddr >> 8);
	*p++ = (writeAddr & 0x00ff);
	*p++ = (numWrites >> 8);
	*p++ = (numWrites & 0x00ff);
	*p++ = (2 * numWrites);

	sndLen = 11 + (2*numWrites);

	for (i = 0; i < numWrites; i++)
	{
		*p++ = (writeData[i] >> 8);
		*p++ = (writeData[i] & 0xff);
	}

	rtn = modBusIO(mh, rp, sndLen);

	if (rtn == OK)
		for (i = 0, p = (Byte *)mh->ioBuf->rcvBuf+3; i < numReads; i++)
		{
			readData[i] = (p[0] << 8) + p[1];
			p += 2;
		}

	releaseModbusPort(mh);
	return(rtn);

} /* writeReadRegs() */


/************************************************************************/
/* Function	   : writeReadSpecial										*/
/* Purpose	   : Special impl of Read/Write Mult allowing chunks and align*/
/* Inputs	   : Handle, slave, reg start addr, first num of write regs,*/
/*				 first write data, 2nd num write regs, 2nd write ptr,	*/
/*				 number of read regs, ptr to read data					*/
/* Outputs	   : Errno													*/
/* Comments	   : 1st and 2nd chunks of write data are to contiguous		*/
/*				 register addrs.  Impl allows byte alignment of data.	*/
/*				 This impl is to facilitate remSerWrite()				*/
/************************************************************************/
Errno writeReadSpecial(ModBusHandle mh, RemID *rp, Nat16 readAddr,
					   Nat16 numReads, Nat16 writeAddr, Nat16 numWrites1,
					   Byte *writeData1, Nat16 numWrites2, const Byte *writeData2,
					   Nat16 *readData)
{
	Nat16		sndLen, i, numWrites;
	Errno		rtn;
	Byte		*p = (Byte *)mh->ioBuf->sndBuf;
	const Byte	*q;

	numWrites = numWrites1 + numWrites2;

	if ((numReads > MAX_REGS) || (numWrites > MAX_REGS-4))
		return(DATA_SIZE_ERROR);

	if ((rtn = getModbusPort(mh)) != OK)
		return(rtn);
	
	sndLen = buildModBusQuery(p, rp->slaveID, FC_WRITE_READ_MULT_REGS,
							  readAddr, numReads);
	p += sndLen;
	writeAddr -= 1;						/* Bus reg addr = addr - 1		*/
	*p++ = (writeAddr >> 8);
	*p++ = (writeAddr & 0x00ff);
	*p++ = (numWrites >> 8);
	*p++ = (numWrites & 0x00ff);
	*p++ = (2 * numWrites);

	sndLen = 11 + (2*numWrites);

	for (i = 0, q = writeData1; i < numWrites1; i++)
	{
		*p++ = *q++;
		*p++ = *q++;
	}

	for (i = 0, q = writeData2; i < numWrites2; i++)
	{
		*p++ = *q++;
		*p++ = *q++;
	}

	rtn = modBusIO(mh, rp, sndLen);

	if (rtn == OK)
		for (i = 0, p = (Byte *)mh->ioBuf->rcvBuf+3; i < numReads; i++)
		{
			readData[i] = (p[0] << 8) + p[1];
			p += 2;
		}

	releaseModbusPort(mh);
	return(rtn);

} /* writeReadSpecial() */