libgfamininet/libmininet/gfamininetmst.c

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <malloc.h>
#include <fcntl.h>
#include <errno.h>
#include <termios.h>
#include <unistd.h>
#include <ctype.h>
#include <sys/file.h>
#include "gfamininetmst.h"

/////////////////////////////////////////////////////////////////////////////
// https://www.racom.eu/eng/support/prot/mininet/index.html
/////////////////////////////////////////////////////////////////////////////

#define STX							((uint8_t)0x02)
#define ACK							((uint8_t)0x06)
#define SLAVE_IND					((uint8_t)0x80)

#define START_INDEX					0x40
#define MIN_INDEX					0x3F
#define MAX_INDEX					0x7F
#define RESET_INDEX					MIN_INDEX
#define STATUS_INDEX_RESET_DONE		0xC0
#define STATUS_INDEX_ERROR			0xC1
#define STATUS_INDEX_CMD_ERROR		0xC2
#define STATUS_INDEX_INVALID_PARAM	0xC3
#define STATUS_INDEX_UNKNOWN_CMD	0xC4
#define STATUS_INDEX_CMD_ALREADY_EX	0xC8

#define MAX_DATA_PAYLOAD_LENGTH		250

#define NODE_IS_BROADCAST(n)		((n) == 0)
#define NODE_IS_GROUPCAST(n)		((((n) & 0xF0) != 0) && (((n) & 0x0F) == 0))

/////////////////////////////////////////////////////////////////////////////

typedef enum _GfaMininetRxStates
{
	GfaRxMNS_Stx,
	GfaRxMNS_Len,
	GfaRxMNS_Node,
	GfaRxMNS_Index,
	GfaRxMNS_Data,
	GfaRxMNS_Zero,
	GfaRxMNS_Check
}GfaMininetRxStates, *LPGfaMininetRxStates;

/////////////////////////////////////////////////////////////////////////////

typedef struct _GFA_MININET_MASTER
{
	HMINETDEV hDev;
	struct timeval tvRXSave;
	struct timeval tvTXSave;
	uint8_t nIndexTable[256];
}GFA_MININET_MASTER, *LPGFA_MININET_MASTER;
typedef const GFA_MININET_MASTER *LPCGFA_MININET_MASTER;

/////////////////////////////////////////////////////////////////////////////

static bool _IsValidIndex(uint8_t nIndex)
{
	return ((nIndex >= MIN_INDEX) && (nIndex <= MAX_INDEX));
}

static uint8_t _GetNextIndex(HGFAMINEMST hMst, uint8_t nNode)
{
	if(hMst)
	{
		LPGFA_MININET_MASTER pMst = (LPGFA_MININET_MASTER)hMst;
		uint8_t idx = ++pMst->nIndexTable[nNode];
		if(idx > MAX_INDEX)
		{
			pMst->nIndexTable[nNode] = START_INDEX;
			idx = pMst->nIndexTable[nNode];
		}
		return idx;
	}
	return 0xFF;
}

static uint8_t _GetCurIndex(HGFAMINEMST hMst, uint8_t nNode)
{
	if(hMst)
	{
		LPGFA_MININET_MASTER pMst = (LPGFA_MININET_MASTER)hMst;
		return pMst->nIndexTable[nNode];
	}
	return 0xFF;
}

/////////////////////////////////////////////////////////////////////////////

HGFAMINEMST GfaMininetMasterOpen(LPGFA_MININET_MST_CFG_PARAMS pmmcp)
{
	if(pmmcp)
	{
		HMINETDEV hDev = GfaMininetDeviceOpen(&pmmcp->devcfg);

		if(hDev)
		{
			LPGFA_MININET_MASTER pMst = malloc(sizeof(GFA_MININET_MASTER));
			memset(pMst, 0, sizeof(GFA_MININET_MASTER));
			memset(pMst->nIndexTable, START_INDEX, sizeof(pMst->nIndexTable));
			pMst->hDev = hDev;
			return (HGFAMINEMST)pMst;
		}

		return NULL;
	}

	errno = EINVAL;
	return NULL;
}

/////////////////////////////////////////////////////////////////////////////

void GfaMininetMasterClose(HGFAMINEMST hMst)
{
	if(hMst)
	{
		LPGFA_MININET_MASTER pMst = (LPGFA_MININET_MASTER)hMst;
		GfaMininetDeviceClose(pMst->hDev);
		free(pMst);
	}
}

/////////////////////////////////////////////////////////////////////////////

bool GfaMininetMasterGetTimeouts(HGFAMINEMST hMst, struct timeval *ptvRX, struct timeval *ptvTX)
{
	if(hMst)
	{
		LPGFA_MININET_MASTER pMst = (LPGFA_MININET_MASTER)hMst;
		return GfaMininetDeviceGetTimeouts(pMst->hDev, ptvRX, ptvTX);
	}

	errno = EINVAL;
	return false;
}

/////////////////////////////////////////////////////////////////////////////

bool GfaMininetMasterSetTimeouts(HGFAMINEMST hMst, const struct timeval *ptvRX, const struct timeval *ptvTX)
{
	if(hMst)
	{
		LPGFA_MININET_MASTER pMst = (LPGFA_MININET_MASTER)hMst;
		return GfaMininetDeviceSetTimeouts(pMst->hDev, ptvRX, ptvTX);
	}

	errno = EINVAL;
	return false;
}

/////////////////////////////////////////////////////////////////////////////

bool GfaMininetMasterSaveTimeouts(HGFAMINEMST hMst)
{
	if(hMst)
	{
		LPGFA_MININET_MASTER pMst = (LPGFA_MININET_MASTER)hMst;
		return GfaMininetMasterGetTimeouts(hMst, &pMst->tvRXSave, &pMst->tvTXSave);
	}

	errno = EINVAL;
	return false;
}

/////////////////////////////////////////////////////////////////////////////

bool GfaMininetMasterRestoreTimeouts(HGFAMINEMST hMst)
{
	if(hMst)
	{
		LPGFA_MININET_MASTER pMst = (LPGFA_MININET_MASTER)hMst;
		return GfaMininetMasterSetTimeouts(hMst, &pMst->tvRXSave, &pMst->tvTXSave);
	}

	errno = EINVAL;
	return false;
}

/////////////////////////////////////////////////////////////////////////////

ssize_t	GfaMininetMasterGetConfigParams(HGFAMINEMST hMst, void *pDevParams, size_t nSizeDevParams)
{
	if(hMst)
	{
		LPGFA_MININET_MASTER pMst = (LPGFA_MININET_MASTER)hMst;
		return GfaMininetDeviceGetConfigParams(pMst->hDev, pDevParams, nSizeDevParams);
	}

	errno = EINVAL;
	return false;
}

/////////////////////////////////////////////////////////////////////////////

int GfaMininetMasterSetConfigParams(HGFAMINEMST hMst, const void *pDevParams, size_t nSizeDevParams)
{
	if(hMst)
	{
		LPGFA_MININET_MASTER pMst = (LPGFA_MININET_MASTER)hMst;
		return GfaMininetDeviceSetConfigParams(pMst->hDev, pDevParams, nSizeDevParams);
	}

	errno = EINVAL;
	return false;
}

/////////////////////////////////////////////////////////////////////////////

uint8_t GfaMininetMasterCalcChk(const void *pData, size_t nCbData)
{
	size_t i = 0;
	uint16_t chk = 0;
	uint8_t cc, lc = 0;
	const uint8_t *pszData = (const uint8_t*)pData;

	for(i = 0; i < nCbData; ++i)
	{
		cc = *pszData++;

		if(cc || (lc != STX))
		{
			chk <<= 1;

			if(chk & 0x0100)
				chk = (chk + 1) & 0x00FF;

			chk += cc;

			if(chk > 0x00FF)
				chk = (chk + 1) & 0x00FF;
		}

		if(i > 4)
			lc = cc;
	}

	if((uint8_t)chk == STX)
		chk = ~chk;
	return (uint8_t)chk;
}

/////////////////////////////////////////////////////////////////////////////

size_t GfaMininetMasterBuildFrame(HGFAMINEMST hMst, uint8_t nNode, uint8_t nIndex, const void *pDataPayload, size_t nCbDataPayload, void *pFrameBuffer, size_t nCbFrameBuffer)
{
	if(hMst && pFrameBuffer && (nCbFrameBuffer >= 5) && (nCbDataPayload <= MAX_DATA_PAYLOAD_LENGTH))
	{
		int i = 0;
		uint8_t b, c;
		size_t nLen = 4;
		const uint8_t *pszData = (const uint8_t*)pDataPayload;
		LPGFA_MININET_FRAME pf = (LPGFA_MININET_FRAME)pFrameBuffer;

		pf->stx		= STX;
		pf->len		= 5 + nCbDataPayload;
		pf->node	= nNode;
		pf->index	= _IsValidIndex(nIndex) ? nIndex : _GetNextIndex(hMst, nNode);

		if(pszData && nCbDataPayload)
		{
			while(nCbDataPayload--)
			{
				b = *pszData++;

				do
				{
					if(++nLen == nCbFrameBuffer)
					{
						errno = ENOMEM;
						return 0;
					}

					pf->data.by[i++] = b;
					c = b;
					b = 0;
				}
				while(c == STX);
			}
		}

		pf->data.by[i++] = GfaMininetMasterCalcChk(pFrameBuffer, nLen);
		++nLen;
		return nLen;
	}

	errno = EINVAL;
	return 0;
}

/////////////////////////////////////////////////////////////////////////////

ssize_t GfaMininetMasterResetSlaveIndex(HGFAMINEMST hMst, uint8_t nNode)
{
	if(hMst)
	{
		ssize_t nRet;
		uint8_t nIndex;
		uint8_t txb[16];
		uint8_t rxb[16];

		if(	(nRet = GfaMininetMasterBuildFrame(hMst, nNode, RESET_INDEX, "\x1b\x52", 2, txb, sizeof(txb))) > 0 &&
			(nRet = GfaMininetMasterTransmitFrame(hMst, txb, nRet)))
		{
			if(!NODE_IS_BROADCAST(nNode) && !NODE_IS_GROUPCAST(nNode))
			{
				// we expect a response
				if(	(nRet = GfaMininetMasterReceiveFrame(hMst, rxb, sizeof(rxb), false)) > 0 &&
					(nRet = GfaMininetMasterEvaluateSlaveResponse(hMst, nNode, rxb, nRet, false, &nIndex)) == MINET_SLAVE_RESPONSE_INDEX_IS_STATUS_CODE)
				{
					if(nIndex == STATUS_INDEX_RESET_DONE)
						return GfaMininetMasterResetLocalIndex(hMst, nNode);
					else
					{
						errno = nIndex;
						return -1;
					}
				}
			}
			else
			{
				// we don't expect a response. so let's just hope that all slaves have received the request and have performed an index reset!
				return GfaMininetMasterResetLocalIndex(hMst, nNode);
			}
		}

		return nRet;
	}

	errno = EINVAL;
	return -1;
}

/////////////////////////////////////////////////////////////////////////////

ssize_t GfaMininetMasterResetLocalIndex(HGFAMINEMST hMst, uint8_t nNode)
{
	if(hMst)
	{
		LPGFA_MININET_MASTER pMst = (LPGFA_MININET_MASTER)hMst;

		if(NODE_IS_BROADCAST(nNode))
			memset(pMst->nIndexTable, START_INDEX, sizeof(pMst->nIndexTable));
		else if(NODE_IS_GROUPCAST(nNode))
			memset(&pMst->nIndexTable[nNode], START_INDEX, 10);
		else
			pMst->nIndexTable[nNode] = START_INDEX;

		return 0;
	}

	errno = EINVAL;
	return -1;
}

/////////////////////////////////////////////////////////////////////////////

int GfaMininetMasterEvaluateSlaveResponse(HGFAMINEMST hMst, uint8_t nNode, const void *pFrame, size_t nCbFrame, bool bAckPossible, uint8_t *pbIndex)
{
	if(hMst && pFrame && nCbFrame > 0)
	{
		bool bIsStatusIndex = false;
		LPCGFA_MININET_FRAME pf = (LPCGFA_MININET_FRAME)pFrame;

		/////////////////////////////////////////////////////////////////////

		if((nCbFrame < 6))
		{
			if(bAckPossible && (pf->stx == ACK))
			{
				return MINET_SLAVE_RESPONSE_ACK;
			}
			else
			{
				errno = MINET_SLAVE_RESPONSE_ERROR_INVALID_LENGTH;
				return -1;
			}
		}
		else
		{
			/////////////////////////////////////////////////////////////////
			// STX / ACK

			if(bAckPossible && (pf->stx == ACK))
				return MINET_SLAVE_RESPONSE_ACK;
			else if(pf->stx != STX)
			{
				errno =  MINET_SLAVE_RESPONSE_ERROR_STX_ERROR;
				return -1;
			}

			/////////////////////////////////////////////////////////////////
			// Length

			if(pf->len > nCbFrame)
			{
				errno = MINET_SLAVE_RESPONSE_ERROR_INCOMPLETE_DATA;
				return -1;
			}

			/////////////////////////////////////////////////////////////////
			// Node address

			if(pf->node != (nNode & 0xF0))
			{
				errno = MINET_SLAVE_RESPONSE_ERROR_INVALID_NODE_ADDRESS;
				return -1;
			}

			/////////////////////////////////////////////////////////////////
			// Index range / match / status code

			if(pbIndex)
				*pbIndex = pf->index;

			if(_IsValidIndex(pf->index))
			{
				uint8_t nIndex = _GetCurIndex(hMst, nNode);
				if(pf->index != nIndex)
				{
					errno = MINET_SLAVE_RESPONSE_ERROR_INDEX_NO_MATCH;
					return -1;
				}
			}
			else if(pf->index < MIN_INDEX)
			{
				errno = MINET_SLAVE_RESPONSE_ERROR_INDEX_OUT_OF_RANGE;
				return -1;
			}
			else // pf->index > MAX_INDEX
			{
				switch(pf->index)
				{
				case STATUS_INDEX_RESET_DONE:
				case STATUS_INDEX_ERROR:
				case STATUS_INDEX_CMD_ERROR:
				case STATUS_INDEX_INVALID_PARAM:
				case STATUS_INDEX_UNKNOWN_CMD:
				case STATUS_INDEX_CMD_ALREADY_EX:
					bIsStatusIndex = true;
					break;
				default:
					errno = MINET_SLAVE_RESPONSE_ERROR_INDEX_OUT_OF_RANGE;
					return -1;
				}
			}

			/////////////////////////////////////////////////////////////////
			// Data / Slave indicator

			if(pf->data.by[0] != SLAVE_IND)
			{
				errno = MINET_SLAVE_RESPONSE_ERROR_INVALID_SLAVE_INDICATOR;
				return -1;
			}

			/////////////////////////////////////////////////////////////////
			// Checksum

			if(pf->data.by[pf->len - 5] != GfaMininetMasterCalcChk(pf, pf->len - 1))
			{
				errno = MINET_SLAVE_RESPONSE_ERROR_INVALID_CHECKSUM;
				return -1;
			}
			else
			{
				return bIsStatusIndex ? MINET_SLAVE_RESPONSE_INDEX_IS_STATUS_CODE : MINET_SLAVE_RESPONSE_SUCCESS;
			}
		}
	}

	errno = MINET_SLAVE_RESPONSE_ERROR_INVALID_ARGUMENT;
	return -1;
}

/////////////////////////////////////////////////////////////////////////////

ssize_t GfaMininetMasterTransmitFrame(HGFAMINEMST hMst, const void *pData, size_t nCbData)
{
	if(hMst && pData && nCbData)
	{
		LPGFA_MININET_MASTER pMst = (LPGFA_MININET_MASTER)hMst;
		return GfaMininetDeviceTransmit(pMst->hDev, pData, nCbData);
	}

	errno = EINVAL;
	return -1;
}

/////////////////////////////////////////////////////////////////////////////

ssize_t GfaMininetMasterReceiveFrame(HGFAMINEMST hMst, void *pBuffer, size_t nCbBuffer, bool bAckPossible)
{
	if(hMst && pBuffer && nCbBuffer)
	{
		uint8_t b, c, buf[256] = {0};
		bool bLoop = true;
		GfaMininetRxStates nState = GfaRxMNS_Stx;
		ssize_t nRet, nCbDataPayloadExp = 0, nCbDataPayloadRcv = 0;
		LPGFA_MININET_MASTER pMst = (LPGFA_MININET_MASTER)hMst;
		LPGFA_MININET_FRAME pFrameRx = (LPGFA_MININET_FRAME)buf;

		if(!pBuffer || !nCbBuffer)
		{
			errno = EINVAL;
			return -1;
		}

		do
		{
			switch(nState)
			{
			////////////////////////////////////////////////////////////////////////
			// handle STX
			case GfaRxMNS_Stx:
				if((nRet = GfaMininetDevicePop(pMst->hDev, &b)) != 1)
				{
					bLoop = false;
					break;
				}
				if(b != STX)
				{
					if(bAckPossible && (b == ACK))
					{
						*(uint8_t*)pBuffer = b;
                        nRet = 1;
                        bLoop = false;
                        break;
                    }
					continue;
				}
				pFrameRx->stx = b;
				++nState;
				// fall through

			////////////////////////////////////////////////////////////////////////
			// handle length
			case GfaRxMNS_Len:
				if((nRet = GfaMininetDevicePop(pMst->hDev, &b)) != 1)
				{
					bLoop = false;
					break;
				}
				if(b < 5)
				{
					if(b == STX)
						continue;
					else
					{
						nState = GfaRxMNS_Stx;
						continue;
					}
				}
				pFrameRx->len = b;
				nCbDataPayloadExp = b - 5;
				++nState;
				// fall through

			////////////////////////////////////////////////////////////////////////
			// handle node
			case GfaRxMNS_Node:
				if((nRet = GfaMininetDevicePop(pMst->hDev, &b)) != 1)
				{
					bLoop = false;
					break;
				}
				if(b == STX)
				{
					nState = GfaRxMNS_Len;
					continue;
				}
				pFrameRx->node = b;
				++nState;
				// fall through

			////////////////////////////////////////////////////////////////////////
			// handle index
			case GfaRxMNS_Index:
				if((nRet = GfaMininetDevicePop(pMst->hDev, &b)) != 1)
				{
					bLoop = false;
					break;
				}
				if(b == STX)
				{
					nState = GfaRxMNS_Len;
					continue;
				}
				else if(b < MIN_INDEX)
				{
					nState = GfaRxMNS_Stx;
					continue;
				}
				pFrameRx->index = b;
				++nState;
				// fall through

			////////////////////////////////////////////////////////////////////////
			// handle data, if any
			case GfaRxMNS_Data:
				if(nCbDataPayloadExp > nCbDataPayloadRcv)
				{
					if((nRet = GfaMininetDevicePop(pMst->hDev, &b)) != 1)
					{
						bLoop = false;
						break;
					}
					if(b == STX)
						++nState;
					pFrameRx->data.by[nCbDataPayloadRcv++] = b;
					continue;
				}
				else
				{
					nState = GfaRxMNS_Check;
					continue;
				}

			////////////////////////////////////////////////////////////////////////
			// handle 0
			case GfaRxMNS_Zero:
				if((nRet = GfaMininetDevicePop(pMst->hDev, &b)) != 1)
				{
					bLoop = false;
					break;
				}
				if(b == 0)
				{
					nState = GfaRxMNS_Data;
					continue;
				}
				else if(b == STX)
				{
					nCbDataPayloadRcv = nCbDataPayloadExp = 0;
					nState = GfaRxMNS_Len;
					continue;
				}
				else if(b >= 5)
				{
					pFrameRx->len = b;
					nCbDataPayloadRcv = 0;
					nCbDataPayloadExp = b - 5;
					nState = GfaRxMNS_Node;
					continue;
				}
				else
				{
					nCbDataPayloadRcv = nCbDataPayloadExp = 0;
					nState = GfaRxMNS_Stx;
					continue;
				}

			////////////////////////////////////////////////////////////////////////
			// handle Checksum
			case GfaRxMNS_Check:
				if((nRet = GfaMininetDevicePop(pMst->hDev, &b)) != 1)
				{
					bLoop = false;
					break;
				}
				if(b == STX)
				{
					nCbDataPayloadRcv = nCbDataPayloadExp = 0;
					nState = GfaRxMNS_Len;
					continue;
				}
				c = GfaMininetMasterCalcChk(pFrameRx, pFrameRx->len - 1);
				if(b == c)
				{
					if((size_t)pFrameRx->len <= nCbBuffer)
					{
						pFrameRx->data.by[nCbDataPayloadRcv] = b;
						nRet = pFrameRx->len;
						memcpy(pBuffer, pFrameRx, nRet);
					}
					else
					{
						errno = ENOMEM;
						nRet = -1;
					}
				}
				else
				{
					errno = EPROTO;
					nRet = -1;
				}
				bLoop = false;
				break;

			////////////////////////////////////////////////////////////////////////
			default:
				nRet = -1;
				bLoop = false;
				break;
			}
		}
		while(bLoop);

		return nRet;
	}

	errno = EINVAL;
	return -1;
}

/////////////////////////////////////////////////////////////////////////////

void GfaMininetMasterDumpFrame(FILE *pf, LPCGFA_MININET_FRAME pFrame)
{
	if(pf)
	{
		if(pFrame && pFrame->len >= 5)
		{
			int i, nCbData = pFrame->len - 5;

			fprintf(pf, "\nSTX:    %02hhX (%hhu)\n", pFrame->stx, pFrame->stx);
			fprintf(pf, "Length: %02hhX (%hhu)\n", pFrame->len, pFrame->len);
			fprintf(pf, "Node:   %02hhX (%hhu)\n", pFrame->node, pFrame->node);
			fprintf(pf, "Index:  %02hhX (%hhu)\n", pFrame->index, pFrame->index);
			fprintf(pf, "Data:   ");

			for(i = 0; i < nCbData; i++)
			{
				if(pFrame->data.by[i] >= 0x41 && pFrame->data.by[i] <= 0x5A)
					fprintf(pf, "<%c>", (char)pFrame->data.by[i]);
				else
					fprintf(pf, "[%02hhX]", (char)pFrame->data.by[i]);
			}

			fprintf(pf, "\nCRC:    %02hhX (%hhu)\n\n", pFrame->data.by[i], pFrame->data.by[i]);
		}
		else
		{
			fprintf(pf, "\nInvalid Mininet-Frame!\n");
		}
		
		fflush(pf);
	}
}