/************************************************************************/
/* Copyright 2009 MBARI													*/
/************************************************************************/
/* Summary	: ModBus Functions for Respirometer controller (O4)			*/
/* Filename : modbus.c													*/
/* Author	: Robert Herlien (rah)										*/
/* Project	: Respirometers												*/
/* Revision: 1.0														*/
/* Created	: 03/03/2009												*/
/*																			*/
/* 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:												*/
/* 3mar2009 rah - created												*/
/************************************************************************/

#include <mbariTypes.h>					/* MBARI type definitions		*/
#include <mbariConst.h>					/* MBARI constants				*/
#include <oasis.h>						/* OASIS controller definitions */
#include <custom.h>						/* Custom def's for deployment	*/
#include <io.h>							/* OASIS I/O definitions		*/
#include <list.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 <cfxpico.h>					/* Persistor PicoDOS definitions*/
#include <stdio.h>
#include <stdlib.h>
#include <string.h>


/************************************************************************/
/* Function	   : buildModBusQuery										*/
/* Purpose	   : Build Query Header										*/
/* Inputs	   : Buffer, slave, func, start addr, number				*/
/* Outputs	   : Errno (see modbus.h)									*/
/************************************************************************/
MLocal Nat16 buildModBusQuery(Byte *buf, Nat16 slave, Nat16 func,
							  Nat16 startAddr, Nat16 num)
{
	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, Nat16 slave, Nat16  func,
					  Nat16 startAddr, Nat16 numBits, Byte *result)
{
	Nat16		sndLen;
	Errno		rtn;

	if (numBits > MAX_BITS)
		return(DATA_SIZE_ERROR);

	getModbusPort(mh->serport);

	sndLen = buildModBusQuery(mh->sndBuf, slave, func, startAddr, numBits);

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

	releaseModbusPort(mh->serport);
	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, Nat16 slave, Nat16 startAddr,
				Nat16 numCoils, Byte *result)
{
	return(readBits(mh, slave, 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, Nat16 slave, Nat16 startAddr,
				 Nat16 numInputs, Byte *result)
{
	return(readBits(mh, slave, 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, Nat16 slave, Nat16 func,
					  Nat16 startAddr, Nat16 numRegs, Nat16 *result)
{
	Nat16		sndLen, i;
	Errno		rtn;
	Byte		*p;

	if (numRegs > MAX_REGS)
		return(DATA_SIZE_ERROR);

	getModbusPort(mh->serport);

	sndLen = buildModBusQuery(mh->sndBuf, slave, func, startAddr, numRegs);

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

	releaseModbusPort(mh->serport);
	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, Nat16 slave, Nat16 startAddr,
						Nat16 numRegs, Nat16 *result)
{
	return(readRegs(mh, slave, 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, Nat16 slave, Nat16 startAddr,
					Nat16 numRegs, Nat16 *result)
{
	return(readRegs(mh, slave, 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, Nat16 slave, Nat16 coilAddr, MBool on)
{
	Errno		rtn;

	getModbusPort(mh->serport);

	rtn = modBusIO(mh, buildModBusQuery(mh->sndBuf, slave, FC_WRITE_ONE_COIL,
										coilAddr, (on ? 0xFF00 : 0)));

	releaseModbusPort(mh->serport);
	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, Nat16 slave, Nat16 regAddr, Nat16 value)
{
	Errno		rtn;

	getModbusPort(mh->serport);

	rtn = modBusIO(mh, buildModBusQuery(mh->sndBuf, slave, FC_WRITE_ONE_REG,
										regAddr, value));
	releaseModbusPort(mh->serport);
	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, Nat16 slave, Nat16 startAddr,
				 Nat16 numCoils, const Byte *data)
{
	Nat16		sndLen, i, numBytes;
	Byte		*p = mh->sndBuf;
	Errno		rtn;

	if (numCoils > MAX_BITS)
		return(DATA_SIZE_ERROR);

	getModbusPort(mh->serport);

	sndLen = buildModBusQuery(p, slave, 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, sndLen);
	releaseModbusPort(mh->serport);
	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, Nat16 slave, Nat16 startAddr,
				Nat16 numRegs, const Nat16 *data)
{
	Nat16		sndLen, i;
	Byte		*p = mh->sndBuf;
	Errno		rtn;

	if (numRegs > MAX_REGS)
		return(DATA_SIZE_ERROR);

	getModbusPort(mh->serport);

	sndLen = buildModBusQuery(p, slave, 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, sndLen);
	releaseModbusPort(mh->serport);
	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, Nat16 slave, Byte *result)
{
	Errno		rtn;

	getModbusPort(mh->serport);

	mh->sndBuf[0] = slave;
	mh->sndBuf[1] = FC_REPORT_SLAVE_ID;

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

	releaseModbusPort(mh->serport);
	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, Nat16 slave, Nat16 readAddr,
					Nat16 numReads, Nat16 writeAddr, Nat16 numWrites,
					const Nat16 *writeData, Nat16 *readData)
{
	Nat16		sndLen, i;
	Errno		rtn;
	Byte		*p = mh->sndBuf;

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

	getModbusPort(mh->serport);

	sndLen = buildModBusQuery(p, slave, 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, sndLen);

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

	releaseModbusPort(mh->serport);
	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, Nat16 slave, Nat16 readAddr,
					   Nat16 numReads, Nat16 writeAddr, Nat16 numWrites1,
					   Byte *writeData1, Nat16 numWrites2, Byte *writeData2,
					   Nat16 *readData)
{
	Nat16		sndLen, i, numWrites;
	Errno		rtn;
	Byte		*p = mh->sndBuf, *q;

	numWrites = numWrites1 + numWrites2;

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

	getModbusPort(mh->serport);

	sndLen = buildModBusQuery(p, slave, 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, sndLen);

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

	releaseModbusPort(mh->serport);
	return(rtn);

} /* writeReadSpecial() */
