libgfamininet/libmininet/gfamininetmst.c

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <malloc.h>
#include <time.h>
#include <fcntl.h>
#include <errno.h>
#include <termios.h>
#include <unistd.h>
#include <ctype.h>
#include <sys/file.h>
#include "gfamininetmst.h"
#include "dbghlp.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 MAX_DATA_PAYLOAD_LENGTH		250
#define MAX_SAVE_TIMOUT_DEPTH		8

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

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[MAX_SAVE_TIMOUT_DEPTH];
	struct timeval tvTXSave[MAX_SAVE_TIMOUT_DEPTH];
	struct timespec tsStart;
	int nVerbosity;
	int nSaveToIndex;
	FILE *pDumpCtx;
	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(LPCGFA_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;
			pMst->pDumpCtx		= stdout;
			clock_gettime(CLOCK_MONOTONIC, &pMst->tsStart);
			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;
		if(pMst->nSaveToIndex < MAX_SAVE_TIMOUT_DEPTH)
		{
			int nIndex = pMst->nSaveToIndex++;
			return GfaMininetMasterGetTimeouts(hMst, &pMst->tvRXSave[nIndex], &pMst->tvTXSave[nIndex]);
		}
		else
		{
			errno = EPERM;
			return false;
		}
	}

	errno = EINVAL;
	return false;
}

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

bool GfaMininetMasterRestoreTimeouts(HGFAMINEMST hMst)
{
	if(hMst)
	{
		LPGFA_MININET_MASTER pMst = (LPGFA_MININET_MASTER)hMst;
		if(pMst->nSaveToIndex > 0)
		{
			int nIndex = --pMst->nSaveToIndex;
			return GfaMininetMasterSetTimeouts(hMst, &pMst->tvRXSave[nIndex], &pMst->tvTXSave[nIndex]);
		}
		else
		{
			errno = EPERM;
			return false;
		}
	}

	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;
}

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

bool GfaMininetMasterIsValidBaudrate(HGFAMINEMST hMst, uint32_t nBaudrate)
{
	if(hMst)
	{
		LPGFA_MININET_MASTER pMst = (LPGFA_MININET_MASTER)hMst;
		return GfaMininetDeviceIsValidBaudrate(pMst->hDev, nBaudrate);
	}

	errno = EINVAL;
	return false;
}

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

int GfaMininetMasterSetBaudrate(HGFAMINEMST hMst, uint32_t nBaudrate)
{
	if(hMst)
	{
		LPGFA_MININET_MASTER pMst = (LPGFA_MININET_MASTER)hMst;
		return GfaMininetDeviceSetBaudrate(pMst->hDev, nBaudrate);
	}

	errno = EINVAL;
	return false;
}

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

HMINETDEV GfaMininetMasterGetDeviceHandle(HGFAMINEMST hMst)
{
	if(hMst)
	{
		LPGFA_MININET_MASTER pMst = (LPGFA_MININET_MASTER)hMst;
		return pMst->hDev;
	}

	errno = EINVAL;
	return NULL;
}

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

uint8_t GfaMininetMasterCalcChk(const void *pData, size_t nCbData, bool bProcessSTX)
{
	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(bProcessSTX && (i > 3))
			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, true);
		++nLen;
		return nLen;
	}

	errno = EINVAL;
	return 0;
}

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

int GfaMininetMasterResetSlaveIndex(HGFAMINEMST hMst, uint8_t nNode)
{
	if(hMst)
	{
		LPGFA_MININET_MASTER pMst = (LPGFA_MININET_MASTER)hMst;
		ssize_t nLen, nRet;
		uint8_t nIndex;
		uint8_t txb[16];
		uint8_t rxb[16];

		nLen = GfaMininetMasterBuildFrame(hMst, nNode, RESET_INDEX, "\x1b\x52", 2, txb, sizeof(txb));

		if((nRet = GfaMininetMasterTransmitFrame(hMst, txb, nLen)) != nLen)
		{
			GfaMininetDevicePurgeRXBuffer(pMst->hDev);
			return -1;
		}

		if(!NODE_IS_BROADCAST(nNode) && !NODE_IS_GROUPCAST(nNode))
		{
			// we expect a response
			if((nRet = GfaMininetMasterReceiveFrame(hMst, rxb, sizeof(rxb), false)) <= 0)
				return -1;

			if((nRet = GfaMininetMasterEvaluateSlaveResponse(hMst, nNode, rxb, nRet, false, &nIndex)) == MINET_SLAVE_RESPONSE_INDEX_IS_STATUS_CODE)
			{
				if(nIndex == MINET_SLAVE_STATUS_INDEX_RESET_DONE)
					return GfaMininetMasterResetLocalIndex(hMst, nNode);
				else
				{
					errno = nIndex;
					nRet = -1;
				}
			}
			else
			{
				errno = EPROTO;
				nRet = -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;
}

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

int 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	GfaMininetMasterPurgeDeviceRXBuffer(HGFAMINEMST hMst)
{
	if(hMst)
	{
		LPGFA_MININET_MASTER pMst = (LPGFA_MININET_MASTER)hMst;
		return GfaMininetDevicePurgeRXBuffer(pMst->hDev);
	}

	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 MINET_SLAVE_STATUS_INDEX_RESET_DONE:
				case MINET_SLAVE_STATUS_INDEX_ERROR:
				case MINET_SLAVE_STATUS_INDEX_CMD_ERROR:
				case MINET_SLAVE_STATUS_INDEX_INVALID_PARAM:
				case MINET_SLAVE_STATUS_INDEX_UNKNOWN_CMD:
				case MINET_SLAVE_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, false))
			{
				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;
		GfaMininetMasterDumpFrame(hMst, pMst->pDumpCtx, (LPCGFA_MININET_FRAME)pData, true, NULL);
		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;
						GfaMininetMasterDumpFrame(hMst, pMst->pDumpCtx, (LPCGFA_MININET_FRAME)pBuffer, false, NULL);
                        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, false);
				if(b == c)
				{
					if((size_t)pFrameRx->len <= nCbBuffer)
					{
						pFrameRx->data.by[nCbDataPayloadRcv] = b;
						nRet = pFrameRx->len;
						memcpy(pBuffer, pFrameRx, nRet);
						GfaMininetMasterDumpFrame(hMst, pMst->pDumpCtx, (LPCGFA_MININET_FRAME)pFrameRx, false, NULL);
					}
					else
					{
						errno = ENOMEM;
						nRet = -1;
					}
				}
				else
				{
					pFrameRx->data.by[nCbDataPayloadRcv] = b;
					GfaMininetMasterDumpFrame(hMst, pMst->pDumpCtx, (LPCGFA_MININET_FRAME)pFrameRx, false, "CRC32!!!");
					errno = EPROTO;
					nRet = -1;
				}
				bLoop = false;
				break;

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

		return nRet;
	}

	errno = EINVAL;
	return -1;
}

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

ssize_t	GfaMininetMasterGetDataFromFrame(const void *pFrame, size_t nCbFrame, void *pData, size_t nCbData)
{
	if(pFrame && (nCbFrame >= 5) && pData && (nCbData > 0))
	{
		LPCGFA_MININET_FRAME pf = (LPCGFA_MININET_FRAME)pFrame;

		if(pf->len == 5)
			return 0; // no data
		else if(pf->len > 5)
		{
			if(nCbFrame >= pf->len)
			{
				size_t nDatalen = (size_t)pf->len - 5;

				if(nDatalen <= nCbData)
				{
					memcpy(pData, pf->data.by, nDatalen);
					return (ssize_t)nDatalen;
				}
				else
				{
					errno = ENOMEM;
					return -1;
				}
			}
		}
		else
		{
			errno = EPROTO;
			return -1;
		}
	}

	errno = EINVAL;
	return -1;
}

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

ssize_t	GfaMininetMasterGetDataFromSlaveFrame(const void *pFrame, size_t nCbFrame, void *pData, size_t nCbData)
{
	uint8_t data[256];
	ssize_t nRet = GfaMininetMasterGetDataFromFrame(pFrame, nCbFrame, data, sizeof(data));

	if(nRet > 0)
	{
		if(data[0] == SLAVE_IND)
		{
			if(--nRet <= (ssize_t)nCbData)
			{
				if(nRet > 0)
					memcpy(pData, &data[1], nRet);
			}
			else
			{
				errno = ENOMEM;
				nRet = -1;
			}
		}
		else
		{
			errno = EPROTO;
			nRet = -1;
		}
	}

	return nRet;
}

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

void GfaMininetMasterDumpFrame(HGFAMINEMST hMst, FILE *pf, LPCGFA_MININET_FRAME pFrame, bool bTX, const char *pszAnnotation)
{
	if(hMst && pf)
	{
		LPGFA_MININET_MASTER pMst = (LPGFA_MININET_MASTER)hMst;

		if(pMst->nVerbosity >= 4)
		{
			struct timespec tsCur, tsIntv;
			clock_gettime(CLOCK_MONOTONIC, &tsCur);
			uint64_t nInterval = TimespecDiff(&tsCur, &pMst->tsStart);
			Ns2Timespec(nInterval, &tsIntv);

			if(pFrame)
			{
				if(pFrame->stx == ACK)
				{
					fprintf(pf, "\n///////////////////////////////////////\n");
					fprintf(pf, "// %ld.%03ld\n", tsIntv.tv_sec, tsIntv.tv_nsec / 1000000);
					fprintf(pf, "// %s:\n", bTX ? "TX" : "RX");
					fprintf(pf, "ACK:    06 (6)\n");
				}
				else if(pFrame->len >= 5)
				{
					int i, nCbData = pFrame->len - 5;

					fprintf(pf, "\n///////////////////////////////////////\n");
					fprintf(pf, "// %ld.%03ld\n", tsIntv.tv_sec, tsIntv.tv_nsec / 1000000);
					fprintf(pf, "// %s:\n", bTX ? "TX" : "RX");
					fprintf(pf, "STX:    %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", pFrame->data.by[i], pFrame->data.by[i]);
					if(pszAnnotation)
						fprintf(pf, "Annot.: %s\n", pszAnnotation);
				}
			}
			else
			{
				fprintf(pf, "\n///////////////////////////////////////\n");
				fprintf(pf, "// %ld:%03ld\n", tsIntv.tv_sec, tsIntv.tv_nsec / 1000000);
				fprintf(pf, "// %s:\n", bTX ? "TX" : "RX");
				fprintf(pf, "Invalid Mininet-Frame!\n");
			}

			fprintf(pf, "\n");
			fflush(pf);
		}
	}
}

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

int GfaMininetMasterSetVerbosity(HGFAMINEMST hMst, int nVerbosity)
{
	if(hMst)
	{
		LPGFA_MININET_MASTER pMst = (LPGFA_MININET_MASTER)hMst;
		if(nVerbosity < 0)
			nVerbosity = 0;
		else if(nVerbosity > 4)
			nVerbosity = 4;
		pMst->nVerbosity = nVerbosity;
		return 0;
	}

	errno = EINVAL;
	return -1;
}

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

const char* GfaMininetMasterStrError(int nErrorCode)
{
	switch(nErrorCode)
	{
	case MINET_SLAVE_RESPONSE_ERROR_STX_ERROR:
		return "First byte in Mininet Frame is neither STX nor ACK";
	case MINET_SLAVE_RESPONSE_ERROR_INVALID_ARGUMENT:
		return "An invalid argument was passed to GfaMininetMasterEvaluateSlaveResponse";
	case MINET_SLAVE_RESPONSE_ERROR_INVALID_LENGTH:
		return "Invalid Mininet Frame length";
	case MINET_SLAVE_RESPONSE_ERROR_INVALID_SLAVE_INDICATOR:
		return "Invalid Mininet slave indicator";
	case MINET_SLAVE_RESPONSE_ERROR_INCOMPLETE_DATA:
		return "Not enough data for Mininet Frame";
	case MINET_SLAVE_RESPONSE_ERROR_INVALID_NODE_ADDRESS:
		return "Invalid Mininet Node address";
	case MINET_SLAVE_RESPONSE_ERROR_INDEX_OUT_OF_RANGE:
		return "Mininet Index out of range";
	case MINET_SLAVE_RESPONSE_ERROR_INDEX_NO_MATCH:
		return "Mininet Index no match";
	case MINET_SLAVE_RESPONSE_ERROR_INVALID_CHECKSUM:
		return "Mininet Checksum error";
	case -MINET_SLAVE_STATUS_INDEX_RESET_DONE:
		return "Mininet index reset done";
	case -MINET_SLAVE_STATUS_INDEX_ERROR:
		return "Mininet index error";
	case -MINET_SLAVE_STATUS_INDEX_CMD_ERROR:
		return "Mininet command error";
	case -MINET_SLAVE_STATUS_INDEX_INVALID_PARAM:
		return "Mininet invalid parameter";
	case -MINET_SLAVE_STATUS_INDEX_UNKNOWN_CMD:
		return "Mininet unknown command";
	case -MINET_SLAVE_STATUS_INDEX_CMD_ALREADY_EX:
		return "Mininet command alearx executing";
	default:
		return strerror(nErrorCode);
	}
}