/******************************************************************
	Example for reading modbus registers over TCP in Pico-C
	Not perfect but working

	Check how your modbus device behaves when more than one client is connecting...
	
	Andrej Konkow, 01/2026
 
	...Pico-C is in somekind very special in syntax and possibilities...
******************************************************************/

/********** Main configuration block ****************
*/
// Do we configure this code from inside ourself or is a
// Textgenerator-component connected. Either keep the line or
// outcomment the next line like this:    // #define CODE...
#define CODECONFIG_BY_TEXTGENERATOR

// If not using the textgenerator specify your data here
#ifndef CODECONFIG_BY_TEXTGENERATOR
	#define MODBUS_INPUT_IP_PORT     "192.168.13.xx:1502"
	#define MODBUS_INPUT_ADDRESS_HEX "9D0E"
	#define MODBUS_INPUT_RESULTS_DT  "20x2"
	#define MODBUS_INPUT_BIGENDIAN   1
#endif
/*
****************************************************/

// supported modbus datatypes
enum registerDatatype
{
	DT_NONE  = 0,
	UINT16   = 1,
	INT16    = 2,
	UINT32   = 3,
	INT32    = 4,
	FLOAT16  = 5,
	FLOAT32  = 6,
	STRING16 = 7,
	STRING32 = 8,
	COIL     = 9
};

// Shall we run in debugmode and print some message on the outputchannels?  0 or 1
#define DEBUGMODE 0

// Inputchannels when using the textgenerator component
#ifdef CODECONFIG_BY_TEXTGENERATOR
	#define MODBUS_INPUT_IP_PORT     getinputtext(0)
	#define MODBUS_INPUT_ADDRESS_HEX getinputtext(1)
	#define MODBUS_INPUT_RESULTS_DT  getinputtext(2)
	#define MODBUS_INPUT_BIGENDIAN   getinput(0)
#endif

// Supported modbus functioncodes
#define MODBUS_FC_READCOILS            0x01
#define MODBUS_FC_READHOLDINGREGISTERS 0x03

// byte-size of message being sent to modbus-device
#define MODBUS_MSG_SIZE 12

// Payload bytes which can be ignored when getting the values from the response
#define MODBUS_RESPONSE_OFFSET 9

// Mostly a default value but configurable
#define MODBUS_DEVICE_ID 1

// Our own transaction id, as we like but unique
#define MODBUS_TRANSACTIONID 140

// Size of char-array holding the connectionstring for target device
#define MODBUS_CONNECTIONSTRING_SIZE 40

// Time spent in reading the responsestream/waiting for data, timeout in ms
// This timeout is a critical one. Redeployments on the miniserver during streamreading
// results in a miniserver crash. To my knowledge there is no workaround but minimizing 
// this value to a minimum, but high enough dealing with your device. The stream reading
// implementation is provided by Loxone itself. As this error is existing for over 10 years
// lets hope the best for the next 10 years. :-)
#define MODBUS_READTIME_MS 1000

// Blocksize upper limit for reading response data
#define RD_BLOCK_SIZE 300

// Define how many Output channels are available before handing over
// the remaining results. In Loxone programs currently can address
// 13 output channels for number-values in maximum
#define MAX_OUTPUT_CHANNELS 13

// Maximum of displayable results over all components
#define MAX_OUTPUT_CHANNELS_TOTAL 2*MAX_OUTPUT_CHANNELS

// Sleeptime before repeating request in milliseconds
#define REPEATER_SLEEP_MS 10000

// Definition of delimiters for datainput, defining
// datatypes and the number of results
#define DELIMITER_DT_BLOCKS ","
#define DELIMITER_DT_NUMBER "x"

/*
	Convert characters/bytes to readable hex-string
	Caution: Don't use strlen or other string-oriented functions as
	byte-characters don't behave like normal alphabetical characters
*/
void convertToReadableHex(char *bytes, int byteLen, char *dest)
{
	int i;
	
	if (byteLen > 0)
	{
		if (strlen(dest) == 0)
			sprintf(dest,"%02x", bytes[0]);
		else
			sprintf(dest,"%s %02x",dest, bytes[0]);
	}
	
	for (i=1; i<byteLen; i++)
		sprintf(dest,"%s %02x",dest,bytes[i]);
}

/*
	Convert hex-String to bytes, hex-characters are handled 4bit-wise
	Be aware: For example two string-characters like 1C have to result
	in one 8bit byte-character
	
	To avoid free()-statements organize the memory allocation outside
	of this function and hand over "resultBytes"
	For the ease of use errorhandling is not priority...
	
	Returns the length of the characters created
*/
int convertHexToBytes(char *hexString, char *resultBytes)
{
	int hexlen, resultcounter, i;
    char bit4_1, bit4_2; 

	hexlen = strlen(hexString);
	resultcounter = 0;
	i = 0;
	
	do
	{
        resultBytes[resultcounter] = 0;
		
		// Unhandled errorcase
		if (i+1 > hexlen)
			break;
		
		bit4_1 = hexString[i]; 
        bit4_2 = hexString[i+1]; 

		// Ignore whitespaces
		if (bit4_1 == ' ')
		{
			i++;
			continue;
		}

		// Do the transformation: 1 Byte is 2x 4bits to be converted
		// Read pairwise
		if      (bit4_1 >= '0' && bit4_1 <= '9') bit4_1 = bit4_1 - '0';
		else if (bit4_1 >= 'a' && bit4_1 <='f')  bit4_1 = bit4_1 - 'a' + 10;
		else if (bit4_1 >= 'A' && bit4_1 <='F')  bit4_1 = bit4_1 - 'A' + 10;    

		if      (bit4_2 >= '0' && bit4_2 <= '9') bit4_2 = bit4_2 - '0';
		else if (bit4_2 >= 'a' && bit4_2 <='f')  bit4_2 = bit4_2 - 'a' + 10;
		else if (bit4_2 >= 'A' && bit4_2 <='F')  bit4_2 = bit4_2 - 'A' + 10;    
		
		// stick together the two 4bit values 
		resultBytes[resultcounter] = (bit4_1 << 4) | (bit4_2 & 0xF);

		i = i+2;
		resultcounter++;
    }
	while (i < hexlen);

	return resultcounter;
}

/*
	The bytes represent ascii-encoded values. Decode them...
*/
void decodeBytes2Ascii(char *src, char *dest, int len)
{
	char hlp[32];
	init(hlp, 32, 0);
	
	int tmpLen = strlen(src);
	if (tmpLen < len)
		len = tmpLen;

	if (len == 0)
		return;
	
	strncpy(hlp, src, len);
	if (strlen(dest) > 0)
		sprintf(dest,"%s, %s", dest, hlp);
	else
		sprintf(dest,"%s", hlp);
}

/*
	PicoC works little endian
	Supported values are arraylengths of 2 and 4
*/
void swapBytes(char *bytes, int result_type, int littleendian)
{
	// In these cases return unmodified
	if (littleendian             ||
		result_type == STRING16  || 
		result_type == STRING32  || 
		result_type == COIL)
		return;
		
	// Maximum size possible
	char tmpBytes[4];
	int  len = 0;
	int  i=0;

	len = getBytesizeByType(result_type);
	
	for (i=0; i < len; i++) 
		tmpBytes[i] = bytes[i];
	
	if (len == 2)
	{
		bytes[0] = tmpBytes[1];
		bytes[1] = tmpBytes[0];		
	}
	else if (len == 4)
	{
		bytes[0] = tmpBytes[3];
		bytes[1] = tmpBytes[2];
		bytes[2] = tmpBytes[1];
		bytes[3] = tmpBytes[0];
	}
}

/*
	Helper function for retrieving bytesizes
*/
int getBytesizeByType(int type)
{
		if (type == UINT16      || type == INT16 || type == FLOAT16)
			return 2;
		else if (type == UINT32 || type == INT32 || type == FLOAT32)
			return 4;		
		else if (type == STRING16)
			return 8;		
		else if (type == STRING32)
			return 16;		
		else if (type == COIL)
			return 1;
		
		return 0;	// Unknown	
}

/*
	Helper function for retrieving bytesizes
*/
int getBytesizeByList(int *types, int sizeOfList)
{
	int bytesize=0, 
		i;
	
	for (i=0; i<sizeOfList; i++)
		bytesize += getBytesizeByType(types[i]);

	// Bytesize of Coils has to be handled different
	if (types[0] == COIL)
		bytesize = (bytesize + 7) / 8;
	
	return bytesize;
}

/*
	calc float16 value
*/
float calc_float16(char *flt16_bits)
{ 
    char sign_bit;
    char exponent;
    unsigned int  significand;

    // IEEE 754 half precision
    sign_bit    =  (flt16_bits[0] & 0x80) >> 7;
    exponent    = ((flt16_bits[0] & 0x7C) >> 2);
    significand = ((flt16_bits[0] & 0x03) << 8) | (flt16_bits[1]);

	return (float)(pow(-1,sign_bit) * (1 + significand/pow(2,10)) * pow(2,(exponent - 31)));
}

/*
	calc float32 value
*/
float calc_float32(char *flt32_bits)
{ 
    char sign_bit;
    char exponent;
    unsigned int  significand;

    // IEEE 754 single precision
    sign_bit    =   (flt32_bits[1] & 0x80) >> 7;
    exponent    =  ((flt32_bits[1] & 0x7F) << 1)  | ((flt32_bits[0] & 0x80) >> 7);
    significand = (((flt32_bits[0] & 0x7F) << 16) |  (flt32_bits[3] << 8) | (flt32_bits[2]));

	return (float)(pow(-1,sign_bit) * (1 + significand/pow(2,23)) * pow(2,(exponent - 127)));
}

/*
	Helperfunction for initialization
*/
char* init(char *array, int size, int value)
{
	int x;
	
	for (x=0; x<size; x++)
		array[x] = value;
	
	return array;
}

/*
	Build connection string from an ip address including the port.
	No errorhandling is done as we assume a correct address
*/
void buildConnectionString(char *targetaddress, char *connectionString)
{
	char ip[25], port[10];
	int  colon;
	
	colon = strfind(targetaddress,":",0);

	strncpy(ip,   targetaddress, colon);
	strncpy(port, &targetaddress[colon+1], strlen(targetaddress)-colon-1);
	sprintf(connectionString,"/dev/tcp/%s/%s", ip, port);

	debugMsg(connectionString);
}

/*
The structure of a Modbus TCP message is:
Transaction Id(2 bytes), Protocol(2 bytes), Length(2 bytes), Unit Address(1 byte), Message(N bytes)

Where:
    The Transaction Id field identifies the transaction.
    The Protocol field is zero to indicate Modbus protocol.
    The Length field is the number of following bytes.
    The Unit Address field is the PLC Address encoded as single byte.
    The Message field is a Modbus PDU. The maximum length of the Message field is is 253 bytes.
    The maximum Modbus TCP message length is 260 bytes.
*/
void buildModbusRequest(char *modbusMsg, int modbus_readregister_count, int functioncode)
{
	modbusMsg[0] = 0x00;
	modbusMsg[1] = MODBUS_TRANSACTIONID;

	modbusMsg[2] = 0x00;
	modbusMsg[3] = 0x00;

	modbusMsg[4] = 0x00;  // Bytelength of Unit-Id and PDU-Message hi
	modbusMsg[5] = 0x06;  // Bytelength of Unit-Id and PDU-Message lo

	// Beginning of PDU-Message
	modbusMsg[6] = MODBUS_DEVICE_ID;    // Unit address, see documentation of device, good guess if unknown: 1
	modbusMsg[7] = functioncode;

	// Set register startaddresses hi an low hex
	// modbusMsg[8] and [9] are filled up
	convertHexToBytes(MODBUS_INPUT_ADDRESS_HEX, &modbusMsg[8]);

	modbusMsg[10] = 0x00;
	modbusMsg[11] = modbus_readregister_count;
}

/*
	Execute Modbusrequest and read the results
*/
int doModbusRequest(char *connectionString, char *modbusRequest, int sizeModbusReq, char *modbusResp, int sizeModbusResp)
{
	int bytesReceived;
	int bytesWritten;
			
	// Request
	debugMsg("Start creating stream");
	STREAM*	pTcpStream = stream_create(connectionString,0,0); // create tcp stream
	
	if (pTcpStream == NULL)
	{
		debugMsg("Error in creating stream");
		return 0;
	}

	debugMsg("Finished creating stream, now writing");
	bytesWritten = stream_write(pTcpStream, modbusRequest, sizeModbusReq); // write to stream
	
	if (bytesWritten != sizeModbusReq)
	{
		debugMsgParam("Writing stream not successful. Written bytes: ", bytesWritten);
		stream_close(pTcpStream);
		return 0;
	}

	debugMsg("Finished writing stream, now flushing");
	stream_flush(pTcpStream); // flush output stream buffer
	/*
		char hexHelper[RD_BLOCK_SIZE];		
		init(hexHelper, RD_BLOCK_SIZE, 0);
		convertToReadableHex(modbusRequest, sizeModbusReq, hexHelper);
		setoutputtext(0, hexHelper);
	*/
	
	/* 
	   Response
	   Following line is the critical part: Redeployments on the miniserver during this action
	   results in a miniserver crash. To minimize risk: keep the MODBUS_READTIME_MS very low but
	   sufficient for your own device. Consult your device documentation.
	*/
	debugMsg("Start reading stream");
	bytesReceived = stream_read(pTcpStream,modbusResp,sizeModbusResp,MODBUS_READTIME_MS);
	debugMsgParam("Finished reading stream. Received bytes: ", bytesReceived);

	stream_close(pTcpStream);
	/*
		// Print results before bytes are swapped big/little endian
		char hexHelper[RD_BLOCK_SIZE];		
		init(hexHelper, RD_BLOCK_SIZE, 0);
		convertToReadableHex(modbusResp, bytesReceived, hexHelper);
		setoutputtext(2, hexHelper); 
	*/

	return bytesReceived;
}

/*
	Check for errors in receiving the modbus response
*/
int checkForErrors(int bytesReceived, char *modbusResp, int expected_bytesize)
{
	char errorMsg[100], error_code;

	// Check for Modbus-Error message. Means: First bit of byte 8 is set.
	// In that case the following byte holds the exception code
	error_code = bytesReceived > 8 && modbusResp[7] >= 129 && modbusResp[7] <= 134;
		
	if ( bytesReceived - MODBUS_RESPONSE_OFFSET < 0 || 
		 bytesReceived != expected_bytesize || 
		 error_code == 1 )
	{
		if (error_code == 0)
			sprintf(errorMsg,"%s", "Reading nok or no connection to device.");
		else
			sprintf(errorMsg,"%s Exceptioncode hex: %02x", "Reading nok.", modbusResp[8]);

		setoutputtext(0, errorMsg);
		setoutputtext(1, "");
		return 1;
	}
	
	return 0;
}

/*
	Get the datatype per outputchannel and store it in an array
	Keep your data clean as errorhandling is not good
	
	Returns the total number of results (not bytes) to read
*/
int parseDatatypes(char *dtString, int *listOfDatatypes, int listSize)
{
	int  delim_block=0, delim_number=0,
		 dtSum=0, i, 
		 noResults, datatype;
	char szResults[3], szDatatype[2];

	
	for (i=0; i<listSize; i++)
		listOfDatatypes[i] = DT_NONE;

	do
	{
		delim_number = strfind(&dtString[delim_block], DELIMITER_DT_NUMBER, 0);
		strncpy(szResults,  &dtString[delim_block], delim_number);
		strncpy(szDatatype, &dtString[delim_block+delim_number+1], 1);

		noResults = batoi(szResults);
		datatype  = batoi(szDatatype);

		for (i=0; i<noResults; i++)
		{
			listOfDatatypes[dtSum] = datatype;
			dtSum++;			
			
			// Have we reached the maximum number of displayable Results?
			// Avoid an overflow
			if (dtSum == listSize)
				break;
		}

		delim_block = strfind(dtString, DELIMITER_DT_BLOCKS, delim_block+1);

		// Have we reached the last block?
		if (delim_block == -1)
			break;

		delim_block++;
	}
	while (TRUE);
	
	return dtSum;
}

/*
	Print status and readable information
*/
void printReadingStatus(int clearError, char *status, char *results)
{
	sprintf(status,"%s", "Reading ok");
	if (strlen(results) > 0)
		sprintf(status,"%s, %s", status, results);

	// Remove existing errormsg/refresh output channel
	if (clearError == 1)
		setoutputtext(0, "");	
	setoutputtext(1, status);
}

/*
	Hexify message for handover
*/
void msgMarshalling(char *hexOutput, int hexOutputSize, int endian, int *szListOfDatatypes, char *modbusResponse, int baseIndex)
{
	int  i, result_type, result_bytesize;
	char szResultHelper[2];
	
	// Handover the remaining results as hex-string
	init(hexOutput, hexOutputSize, 0);
	init(szResultHelper, 2, 0);

	// Hexify info about endian
	szResultHelper[0] = (char)(!endian);
	convertToReadableHex(szResultHelper, 1, hexOutput);

	// Hexify the remaining result datatypes
	for (i=0; i<MAX_OUTPUT_CHANNELS; i++)
	{
		// Downcasting in sizes is ok here
		szResultHelper[0] = (char)szListOfDatatypes[MAX_OUTPUT_CHANNELS+i];
		convertToReadableHex(szResultHelper, 1, hexOutput);
	}

	// Hexify the remaining results themselves
	for (i=0; i<MAX_OUTPUT_CHANNELS; i++)
	{
		result_type     = szListOfDatatypes[MAX_OUTPUT_CHANNELS+i];
		result_bytesize = getBytesizeByType(result_type);

		convertToReadableHex(&modbusResponse[baseIndex], result_bytesize, hexOutput);
		baseIndex += result_bytesize;
	}
}

/*
	In case of debugmode print a message and have a short break to
	assure that the message can be read
*/
void debugMsg(char *msg) { debugMsgParam(msg, -9999); }
void debugMsgParam(char *msg, int val)
{
	if (!DEBUGMODE)
		return;
	
	char debugmsg[150];

	if (val == -9999)
		sprintf(debugmsg,"[DEBUG] %s", msg);
	else
		sprintf(debugmsg,"[DEBUG] %s %d", msg, val);
	
	setoutputtext(0, debugmsg);
	sleep(1000);
}

/************************************************************************
	
	Now lets do the main work

	Mainfunctionality encapsulated in a function
	Due to picoC issues a little awkward
*/
void main()
{
	char  modbusRequest[MODBUS_MSG_SIZE], modbusResponse[RD_BLOCK_SIZE],
		  connectionString[MODBUS_CONNECTIONSTRING_SIZE],
		  okString[RD_BLOCK_SIZE],
		  modbusStrings[RD_BLOCK_SIZE], hexOutput[RD_BLOCK_SIZE];
	int   baseIndex, nBytesReceived, errorInLastIteration,
		  no_resultvalues, no_printValues,
		  result_type, result_bytesize,
		  modbus_resultregister_type_bytesize, 
		  channel, i, bReadCoils=0,
		  bLittle_endian,
		  szListOfDatatypes[MAX_OUTPUT_CHANNELS_TOTAL];
	
	unsigned short us_resultValue;
	signed   short ss_resultValue;
	unsigned int   ui_resultValue;
	signed   int   si_resultValue;

	bLittle_endian  = ! MODBUS_INPUT_BIGENDIAN;
	no_resultvalues = parseDatatypes(MODBUS_INPUT_RESULTS_DT, szListOfDatatypes, MAX_OUTPUT_CHANNELS_TOTAL);
		
	if (no_resultvalues > 0 && szListOfDatatypes[0] == COIL)
		bReadCoils = 1;
	
	modbus_resultregister_type_bytesize = getBytesizeByList(szListOfDatatypes, MAX_OUTPUT_CHANNELS_TOTAL);

	// Coils are handled different
	if (bReadCoils)
	{
		buildModbusRequest(modbusRequest, no_resultvalues, MODBUS_FC_READCOILS);
		no_printValues = modbus_resultregister_type_bytesize;
	}
	else
	{
		buildModbusRequest(modbusRequest, modbus_resultregister_type_bytesize/2, MODBUS_FC_READHOLDINGREGISTERS);
		no_printValues = no_resultvalues;	
	}

	// if we have more than MAX_OUTPUT_CHANNELS results (limited outputchannels...) 
	// then hand over results after MAX_OUTPUT_CHANNELS-1 outputs
	if (!bReadCoils && no_printValues > MAX_OUTPUT_CHANNELS)
		no_printValues = MAX_OUTPUT_CHANNELS;

	buildConnectionString(MODBUS_INPUT_IP_PORT, connectionString);
	
	// Keep polling
	while (TRUE)
	{
		init(modbusStrings, RD_BLOCK_SIZE, 0);
		init(okString, 		RD_BLOCK_SIZE, 0);
		baseIndex = MODBUS_RESPONSE_OFFSET;
		
		setoutputtext(1, "Reading Modbus-TCP. Do not interrupt!");
		nBytesReceived = doModbusRequest(connectionString, modbusRequest, MODBUS_MSG_SIZE, modbusResponse, RD_BLOCK_SIZE);
		setoutputtext(1, "Finished reading Modbus-TCP");
				
		if (checkForErrors(nBytesReceived, modbusResponse, MODBUS_RESPONSE_OFFSET+modbus_resultregister_type_bytesize))
		{
			errorInLastIteration = 1;
			sleep(REPEATER_SLEEP_MS);
			continue;
		}

		// Resulthandling
		// Buffers are big enough. So take the bytes from the responsestream and put them
		// into the result types/values	
		for (i=0; i<no_printValues; i++)
		{
			result_type     = szListOfDatatypes[i];
			result_bytesize = getBytesizeByType(result_type);
			
			swapBytes(&modbusResponse[baseIndex], result_type, bLittle_endian);
			
			// Decode Bits/Bytes to values
			switch (result_type)
			{
				case UINT16:	// sizeof(short) == 2 == 16bit
					us_resultValue = (modbusResponse[baseIndex]) | (modbusResponse[baseIndex + 1] << 8);	
					setoutput(i, us_resultValue);
					break;

				case INT16:		// sizeof(short) == 2 == 16bit
					ss_resultValue = (modbusResponse[baseIndex]) | (modbusResponse[baseIndex + 1] << 8);
					setoutput(i, ss_resultValue);
					break;

				case UINT32:	// sizeof(int)   == 4 == 32bit
					ui_resultValue = (modbusResponse[baseIndex] << 16) | (modbusResponse[baseIndex + 1] << 24) | (modbusResponse[baseIndex + 2]) | (modbusResponse[baseIndex + 3]  << 8);
					setoutput(i, ui_resultValue);
					break;

				case INT32:		// sizeof(int)   == 4 == 32bit
					si_resultValue = (modbusResponse[baseIndex] << 16) | (modbusResponse[baseIndex + 1] << 24) | (modbusResponse[baseIndex + 2]) | (modbusResponse[baseIndex + 3]  << 8);
					setoutput(i, si_resultValue);
					break;
				
				case FLOAT16:	// sizeof(float) == 8 == 64bit
					setoutput(i, calc_float16(&modbusResponse[baseIndex]));
					break;
				
				case FLOAT32:	// sizeof(float) == 8 == 64bit
					setoutput(i, calc_float32(&modbusResponse[baseIndex]));
					break;

				case STRING16:
				case STRING32:
					decodeBytes2Ascii(&modbusResponse[baseIndex], modbusStrings, result_bytesize);
					break;

				case COIL:	// In case of coils the number of results are coded in bits. Means: 8 coils  == 1 Byte
					setoutput(i, modbusResponse[baseIndex]);	// sizeof(char)  == 1 == 8bit
					break;
				
				default:
					setoutput(i, -11111111);	// Unknown
			}
			
			baseIndex += result_bytesize;
		}
		
		printReadingStatus(errorInLastIteration, okString, modbusStrings);
		
		// Handover remaining results
		if (no_resultvalues > MAX_OUTPUT_CHANNELS && szListOfDatatypes[0] != COIL)
		{
			msgMarshalling(hexOutput, RD_BLOCK_SIZE, bLittle_endian, szListOfDatatypes, modbusResponse, baseIndex);
			
			setoutputtext(0, hexOutput);
		}

		errorInLastIteration = 0;	
		sleep(REPEATER_SLEEP_MS);
	}
}

main();