/*******************************************************************************
 * (c) Copyright 2015 Microsemi Corporation.  All rights reserved.
 *
 *
 *
 * SVN $Revision: $
 * SVN $Date: $
 */

#include <api_oam_sdk/common/error_codes/inc/mr_error_codes.h>
#include <api_oam_sdk/common/types/inc/mr_api_types.h>
#include <api_oam_sdk/metroam_hal/inc/mr_hal.h>
#include <api_oam_sdk/service_evc/inc/svc_evc.h>
#include <api_oam_sdk/table_access/inc/table_access.h>
#include <api_oam_sdk/table_config/inc/table_config.h>
#include <api_oam_sdk/tunnel/inc/tunnel.h>
#include <stdlib.h>
#include <string.h>
#include "sys/time.h"

#include "osal.h"
#include "egl_assert.h"
#include "egl_log.h"
#ifdef FREERTOS
#include "rtc.h"
#endif

#define MAX_LIST_LEN			MR_MAX_VLANS		// The max number of value that the mapList can hold

#if 0
extern	mrDeviceInfo_t		mrDeviceInfo;
#endif

const char *maIntervalStr[8] = {"Invalid","3.3ms","10ms","100ms","1sec","10sec","1min","10min"};

uint16_t vidPrioDeToTag(uint16_t vlan, uint8_t prio, bool de)
{
	uint16_t	tagVal;
	tagVal = (((vlan) << MR_TAG_VLAN_OFFSET) & MR_TAG_VLAN_MASK) |	 // Ctag VID -  cTagVID[0]={prio,de,vlan[11:8]},cTagVID[1]=vlan[7:0]
			((((uint16_t)de) << MR_TAG_DE_OFFSET) & MR_TAG_DE_MASK) |
			((((uint16_t)prio) << MR_TAG_PRIO_OFFSET) & MR_TAG_PRIO_MASK);
	return(tagVal);
}

void getInterfaceMAC(iFace_t iFace, macAddress_t macAddress)
{
	readPortMacRam(iFace, macAddress);
#if 0
	switch(iFace)
	{
		case wanPort:
		{
			memcpy(macAddress,mrDeviceInfo.wanMAC,6);
			break;
		}
		case lanPort:
		{

			memcpy(macAddress,mrDeviceInfo.lanMAC,6);
			break;
		}
		case mgntPort:
		{
			memcpy(macAddress,mrDeviceInfo.mgntMAC,6);
			break;
		}
		default:
		{
			memset(macAddress,0,6);
		}
	}
#endif
}

uint16_t getPortDefaultLan(iFace_t iFace)
{
	pVidEntry_t pVidEntry;
	readPVidTable((mrPort_t)iFace, &pVidEntry);

	return(pVidEntry.vlan);
}

volatile uint16_t	g_portStatus		= 0x22;//((psUp << 4) & psUp);		// Port Status for both Interfaces encoded as: iface1[7:4],iface0[3:0]
volatile uint16_t	g_interfaceStatus	= 0x11;//((isUp << 4) & isUp);		// Interface Status for both Interfaces encoded as: iface1[7:4],iface0[3:0]

void getPortDefaultVid(iFace_t iFace, pVidEntry_t *pVidEntry)
{
	readPVidTable((mrPort_t)iFace, pVidEntry);
}

psValue_t sysGetPortStatus(iFace_t iFace)
{
	if (iFace == wanPort)
	{
		return ((psValue_t)(g_portStatus & 0x3));
	}
	else
	{
		return ((psValue_t)((g_portStatus >> 4) & 0x3));
	}
}

isValue_t sysGetInterfaceStatus(iFace_t iFace)
{
	if (iFace == wanPort)
	{
		return ((isValue_t)(g_interfaceStatus & 0x7));
	}
	else
	{
		return ((isValue_t)((g_interfaceStatus >> 4) & 0x7));
	}
}

/*
 * @brief This should be called when ever the Port Status value on an interface changes
 *
 * It updates the PS TLV for CCMs on all MEPs that are sending a PS TLV on that interface.
 */
void sysPortStatusChangeCB(iFace_t iFace, psValue_t psValue)
{
	mdIndex_t mdIndex;

	// If the Interface status changes on an interface update all affected MEPs
	mdIndex = mdReadList(0);
	while(mdIndex != 0)
	{
		_mdUpdateCCMTLVs(mdIndex, iFace, portStatusTlvType, 1, &psValue);
		mdIndex = mdReadList(mdIndex);
	}
}

/*
 * @brief This should be called when ever the Interface Status value on an interface changes
 *
 * It updates the IS TLV for CCMs on all MEPs that are sending a IS TLV on that interface.
 */
void sysInterfaceStatusChangeCB(iFace_t iFace, isValue_t isValue)
{
	mdIndex_t mdIndex;

	// If the Interface status changes on an interface update all affected MEPs
	mdIndex = mdReadList(0);
	while(mdIndex != 0)
	{
		_mdUpdateCCMTLVs(mdIndex, iFace, interfaceStatusTlvType, 1, &isValue);
		mdIndex = mdReadList(mdIndex);
	}
}

bool mepIdIsValid(mepId_t mepId)
{
	if ((mepId < 1) || (mepId > 8191))
	{
		return (false);
	}
	return (true);
}

/*
 *
 */
void generatePacketTags(
		tagEntry_t	*innerTag,
		tagEntry_t	*outerTag,
		uint16_t 	vLan,
		prio_t		prio,
		bool		de,
		tagType_t	tagType,
		iFace_t		iFace)
{
	vidEntry_t	tunnelVid;

	outerTag->present = false;
	innerTag->present = false;

	switch(tagType)
	{
		case VLAN_NONE:
		{
			if (isTunnelModeActive(iFace) && (iFace == wanPort))
			{
				getTunnelVid(iFace, &tunnelVid);
				innerTag->present	= true;
				innerTag->tagId 	= getTunnelTagId(iFace);
				innerTag->tagVal	= vidPrioDeToTag(tunnelVid.vlan,tunnelVid.prio,tunnelVid.de);
				//innerTag->tagVal	= mr_htons(innerTag->tagVal);	// Put the tag value in network byte order
			}
			break;
#if 0		// TODO: confirm this code is no longer required and remove
			// If un-tagged, Stick the Port default vlan, prio, de in the outer tag...other services (e.g. LB will look here for this info)
			pVidEntry_t pVidEntry;
			getPortDefaultVid(iFace,&pVidEntry);
			outerTag->tagId 	= 0;
			outerTag->tagVal	= vidPrioDeToTag(pVidEntry.vlan,pVidEntry.prio,pVidEntry.de);
			//outerTag->tagVal	= mr_htons(outerTag->tagVal);	// Put the tag value in network byte order
#endif
		}
		case CTAG_TYPE:
		case STAG_TYPE:
		{
			// There should be a Ctag on the CCM
			// Set the tunnel tag as the outer tag
			if (isTunnelModeActive(iFace) && (iFace == wanPort))
			{
				getTunnelVid(iFace,&tunnelVid);
				outerTag->present	= true;
				outerTag->tagId 	= getTunnelTagId(iFace);
				outerTag->tagVal	= vidPrioDeToTag(tunnelVid.vlan,tunnelVid.prio,tunnelVid.de);
				//outerTag->tagVal	= mr_htons(outerTag->tagVal);	// Put the tag value in network byte order
			}
			// Configure the inner tag as the Ctag
			innerTag->present 		= true;
			if (tagType == CTAG_TYPE)
			{
				innerTag->tagId 	= getCTagId();
			}
			else
			{
				innerTag->tagId 	= getSTagId();
			}
			innerTag->tagVal		= vidPrioDeToTag(vLan,prio,de);
			//innerTag->tagVal		= mr_htons(innerTag->tagVal);	// Put the tag value in network byte order
			break;
		}
	}
}


/*
 * @brief Populate common fields of a txTemplateEntry_t struct
 * @param[in] lMepNum
 * @return NO_ERR if no error occurred, a non-zero error code otherwise
 *
 * This function is used to populate a txTemplateEntry_t struct
 * based on current values for the local MEP and some updated
 * fields such as priority and tags values.
 */
void generateBasicCfmTemplate(lMepNum_t lMepNum,iFace_t iFace,uint16_t pktSize,tagEntry_t *innerTag,tagEntry_t *outerTag,txTemplateEntry_t *txTemplateEntry)
{
	uint32_t		cfmType;

	// Set the MAC SA to the Port MAC for this lMep
	getInterfaceMAC(iFace,txTemplateEntry->SA);		// TODO: change this to get correct MAC for iFace

	txTemplateEntry->cTagPresent	= innerTag->present;
	if (innerTag->present)
	{
		txTemplateEntry->cTagTPID[0]	= (uint8_t)((innerTag->tagId >> MR_BYTE1_OFFSET) & MR_BYTE0_MASK);						  // Ctag Type field - typically cTagTPID[0]=0x81,cTagTPID[1]=0x00
		txTemplateEntry->cTagTPID[1]	= (uint8_t)(innerTag->tagId & MR_BYTE0_MASK);
		// expect to find the VLan tag value in network byte order
		txTemplateEntry->cTagVID[0]		= (uint8_t)((innerTag->tagVal >> MR_BYTE1_OFFSET) & MR_BYTE0_MASK);
		txTemplateEntry->cTagVID[1]		= (uint8_t)(innerTag->tagVal & MR_BYTE0_MASK);
	}

	txTemplateEntry->sTagPresent	= outerTag->present;
	if (outerTag->present)
	{
		txTemplateEntry->sTagTPID[0]	= (uint8_t)((outerTag->tagId >> MR_BYTE1_OFFSET) & MR_BYTE0_MASK);						  // Ctag Type field - typically cTagTPID[0]=0x81,cTagTPID[1]=0x00
		txTemplateEntry->sTagTPID[1]	= (uint8_t)(outerTag->tagId & MR_BYTE0_MASK);
		// expect to find the VLan tag value in network byte order
		txTemplateEntry->sTagVID[0]		= (uint8_t)((outerTag->tagVal >> MR_BYTE1_OFFSET) & MR_BYTE0_MASK);
		txTemplateEntry->sTagVID[1]		= (uint8_t)(outerTag->tagVal & MR_BYTE0_MASK);
	}

	cfmType = getCfmTypeId();
	txTemplateEntry->eType[0] = (uint8_t)((cfmType >> MR_BYTE1_OFFSET) & MR_BYTE0_MASK);	// ether Type field - typically eType[0]=0x89,eType[1]=0x02
	txTemplateEntry->eType[1] = (uint8_t)(cfmType & MR_BYTE0_MASK);

	txTemplateEntry->proHdrLen = 0;	    // proHeader Length in Dwords = 0 for metroProbe or MetroHaul
	//memcpy(txTemplateEntry->proHdr,mrLMepList[lMepNum].proHdr,sizeof(mrLMepList[lMepNum].proHdr));	 // The user specified proHeader Bytes
	txTemplateEntry->comCfmHdr[0] = 0;				// These will be set by the specific service function
	txTemplateEntry->comCfmHdr[1] = 0;				// These will be set by the specific service function
	txTemplateEntry->comCfmHdr[2] = 0;				// These will be set by the specific service function
	txTemplateEntry->comCfmHdr[3] = 0;				// These will be set by the specific service function
	txTemplateEntry->seqNum = 1;
	txTemplateEntry->cfmTLVLen = 0;
	txTemplateEntry->mepID = 0;				// This must be set by the calling function
	txTemplateEntry->eTLV = 0x00;

	txTemplateEntry->pktLen = pktSize;		// This should be the packet size excluding CRC (i.e. 4 less than desired packet size)
}

prio_t	sysGetLowestFrameLossPrio(void)
{
	// TODO : update code
	return prio0;
}


void cfmTxEnable(bool enable)
{
	uint32_t	data;
	#ifndef EAGLE_SW_TEST
		uint32_t	tOutCnt = MR_MAX_ITERS;
	#endif

	//DO_LOG_INFO(log_handle, "%s CFM Tx\n",enable ? "Enabled" : "Disabled");

	mrHalRegRead(TX_INIT_REG_ADDR, &data);
	if (enable)
		data |= TX_INIT_REG_TX_EN_MASK;			// turn on bit 0
	else
		data &= ~TX_INIT_REG_TX_EN_MASK;		// turn off bit 0
	// Write the New value to the register
	mrHalRegWrite(TX_INIT_REG_ADDR, data);

	if (!enable)
	{
		// If Tx is being disabled then Stop all LB channels and ensure
		// all pending Tx Carrier Pkts are sent before proceeding.
		// This prevents unintentional runt pkts being sent to the MAC.

		// Disable all LB channels
		mrHalRegWrite(LB_RESET_REG_ADDR, LB_RESET_REG_ALL_CHNL_RESET_MASK);

		// Read the init reg and see if Idle is set...
		mrHalRegRead(TX_INIT_REG_ADDR, &data);

		#ifndef EAGLE_SW_TEST
			// poll the idle bit...and wait for it to be set.
			// This ensures that pending Tx packets are gracefully flushed
			while ((!(data & TX_INIT_REG_TX_IDLE_MASK)) && ((tOutCnt) > 0))
			{
				osSleepMS(2);
				mrHalRegRead(TX_INIT_REG_ADDR, &data);
				tOutCnt--;
			}

			// check if the loop timed out before Idle was set.
			//ASSERT(tOutCnt != 0, ERR_POLL_FLAG_TIMEOUT); //mutyam
		#endif
	}
}

void msaSetLOS(void)
{
	uint32_t	data;
	mrHalRegRead(I2C_PIN_STATS_REG_ADDR,&data);
	data |= I2C_PIN_STATS_LOS_MASK;
	mrHalRegWrite(I2C_PIN_STATS_REG_ADDR,data);
}

void msaClrLOS(void)
{
	uint32_t	data;
	mrHalRegRead(I2C_PIN_STATS_REG_ADDR,&data);
	data &= ~I2C_PIN_STATS_LOS_MASK;
	mrHalRegWrite(I2C_PIN_STATS_REG_ADDR,data);
}

void msaEnableOptics(void)
{
	uint32_t	data;
	mrHalRegRead(I2C_CTRL_REG_ADDR,&data);
	data |= I2C_CTRL_ENABLE_OPTICS_MASK;
	mrHalRegWrite(I2C_CTRL_REG_ADDR,data);

	// Note the Optics controller CPU will clear this bit one the Optics are enabled
}

void msaDisableOptics(void)
{
	uint32_t	data;
	int		 	tOut = 100;
	mrHalRegRead(I2C_CTRL_REG_ADDR,&data);
	data |= I2C_CTRL_DISABLE_OPTICS_MASK;
	mrHalRegWrite(I2C_CTRL_REG_ADDR,data);

	// Wait for the Optics controller CPU to clear this bit...
	while(( data | I2C_CTRL_DISABLE_OPTICS_MASK) && (tOut-- != 0))
	{
		mrHalRegRead(I2C_CTRL_REG_ADDR,&data);
		osSleepMS(1);			// play nice with other threads while waiting!
	}
}

/*
 * @brief This will parse the ceVlanMap string and extract all vlans into a list. Returns false if parsing fails.
 *
 * strMap is a pointer to the ceVlanMap string
 * listPtr is a pointer to an array of MR_MAX_VLANS uint16_t values OR NULL. If NULL the string is parsed but the
 * are not returned. If listPtr is not NULL then it is populated with all vLan values described in the MAP.
 * The list is terminated with a 0 value since this is an invalid value for a vLan.
 */
bool getListFromStringMap(char *strMap,uint16_t *listPtr, uint16_t minVal, uint16_t maxVal, uint16_t *listLen)
{
	uint16_t	vCount		= 0;
	int16_t		startValue;
	int16_t		endValue;
	uint16_t	k;
	char		*cmdPtr;
	char		*valuePtr;
	char		strMapCpy[256];
	char		*srcStrPtr = strMapCpy;

	*listLen = 0;
	// Example inputs:
	//		"1:4095"
	//		"1,4,8"
	//		"1,10:20"
	//		"1,10:20,100:110"
	//		"1,5,100:200,205"
	//      "1'2,2'3" => "1,1,2,2,2"  i.e. "a'b" = replicate value a, b times.
	if (strlen(strMap) == 0)
	{
		// If the string is empty...
		if (listPtr != NULL) {
			listPtr[vCount] = 0;
		}
		//DO_LOG_DEBUG(log_handle,"Parsing StringMap identified 0 entries - empty string!");
		return(true);		// true because it is parsed correctly and an empty string may be valid in some instances
	}
	else
	{
		strcpy(srcStrPtr,strMap);					// copy the string first since parsing with strtok() will modify the string!

		// The string is not empty, start parsing...
		valuePtr = strtok(srcStrPtr,",");			// Split the string up to the first comma - if there is no ',' the full string is returned
		while (valuePtr != NULL)
		{
			// Return an error if any non numeric characters are detected when a number was expected
			if ((*valuePtr < '0') || (*valuePtr > '9'))
			{
				if (*valuePtr != 'x' && *valuePtr != 'X')
				{
					// Not a number so exit
					return(false);
				}
			}
			// There may be more than one vlan or vlan list (i.e. "x:y" or "x'y")...
			cmdPtr = strchr(valuePtr,':');			// See if there is a ':' in this part of the string
			if (cmdPtr != NULL)
			{
				// There is a colon so valuePtr points to a string with the format "x:y"
				startValue	= atoi(valuePtr);
				endValue	= atoi(cmdPtr+1);
				if ((startValue < minVal) || (startValue > maxVal) || (endValue < minVal) || (endValue > maxVal))
				{
					//DO_LOG_DEBUG(log_handle,"Parsing StringMap identified invalid values: start=%d,end=%d",startValue,endValue);
					return(false);
				}
				// Handle incrementing or decrementing the value...
				if (startValue < endValue)
				{
					//DO_LOG_DEBUG(log_handle,"Parsing StringMap identified invalid values: start=%d,end=%d",startValue,endValue);
					// Count the value in the list and possibly Add the Vlans to the provided listPtr
					for (;startValue <= endValue;startValue++)
					{
						if (listPtr != NULL)
						{
							listPtr[vCount] = (uint16_t)startValue;
						}
						vCount++;
						if (vCount == MAX_LIST_LEN + 1)
						{
							return(false);	// List is too long!
						}
					}
				}
				else
				{
					// Count the value in the list and possibly Add the Vlans to the provided listPtr
					for (;startValue >= endValue;startValue--)
					{
						if (listPtr != NULL)
						{
							listPtr[vCount] = (uint16_t)startValue;
						}
						vCount++;
						if (vCount == MAX_LIST_LEN + 1)
						{
							return(false);	// List is too long!
						}
					}
				}

			}
			else
			{
				cmdPtr = strchr(valuePtr,'\'');			// See if there is a 'replicate' command in this part of the string
				if (cmdPtr != NULL)
				{
					// There is a replicate command  so valuePtr points to a string with the format "x'y"
					// If the value of x is 'x' or 'X' this indicates that that COS should be dropped
					if (*valuePtr == 'x' || *valuePtr == 'X')
					{
						startValue = 0xFFFF;		// Special value used to specify 'discard' for COS Map
					}
					else
					{
						startValue	= atoi(valuePtr);
						if ((startValue < minVal) || (startValue > maxVal))
						{
							//DO_LOG_DEBUG(log_handle,"Parsing StringMap identified invalid values: start=%d,end=%d",startValue,endValue);
							return(false);
						}
					}
					endValue	= atoi(cmdPtr+1);
					// Count the value in the list and possibly Add the Vlans to the provided listPtr
					for (k=0;k < endValue;k++)
					{
						if (listPtr != NULL)
						{
							listPtr[vCount] = (uint16_t)startValue;
						}
						vCount++;
						if (vCount == MAX_LIST_LEN + 1)
						{
							return(false);	// List is too long!
						}
					}
				}
				else
				{
					// There was no colon or replicate command so this is a single value
					// If the value of is 'x' or 'X' this indicates that that COS should be dropped
					if (*valuePtr == 'x' || *valuePtr == 'X')
					{
						startValue = 0xFFFF;		// Special value used to specify 'discard' for COS Map
					}
					else
					{
						startValue	= atoi(valuePtr);
						if ((startValue < minVal) || (startValue > maxVal))
						{
							//DO_LOG_DEBUG(log_handle,"Parsing StringMap identified invalid values: start=%d,end=%d",startValue,endValue);
							return(false);
						}
					}
					// Count the value in the list and possibly Add the Vlan to the provided listPtr
					if (listPtr != NULL)
					{
						listPtr[vCount] = (uint16_t)startValue;
					}
					vCount++;
					if (vCount == MAX_LIST_LEN + 1)
					{
						return(false);	// List is too long!
					}

				}
			}
			valuePtr = strtok(NULL,",");				// Extract the next segment of the string, if any remain.
		} // while

		//DO_LOG_DEBUG(log_handle,"Parsing StringMap identified %d vLans",vCount);
	}
	*listLen = vCount;
	return (true);
}

int enableMgntVlan(uint16_t mgntVlan)
{
	uint32_t	data;
	OS_ARCH_DECL_PROTECT(prv);

	OS_ARCH_PROTECT(prv);

	if (mgntVlan >= MR_MAX_VLANS)
	{
		return(1);		// Operation Rejected due to invalid Vlan
	}

	// This operation must be protected because Ethernet device drivers can also access this register
	OS_ARCH_PROTECT(prv);
	// Set the Vlan to the new value and set the 'match Vlan' control bit
	mrHalRegRead(CPU_MGNT_VLAN_CTRL_REG_ADDR, &data);
	data &= ~CPU_MGNT_VLAN_CTRL_VLAN_MASK;
	data |= CPU_MGNT_VLAN_CTRL_VLAN_MASK + CPU_MGNT_VLAN_CTRL_MATCH_VLAN_MASK;
	mrHalRegWrite(CPU_MGNT_VLAN_CTRL_REG_ADDR, data);
	OS_ARCH_UNPROTECT(prv);

	//Ensure this Vlan in both ports is set as un-blocked
	readRawVlanTbl(mgntVlan, &data, wanPort);
	data &= ~VLAN_INFO_RAM_VLAN_FILT_MASK;
	writeRawVlanTbl(mgntVlan, data, wanPort);
	readRawVlanTbl(mgntVlan, &data, lanPort);
	data &= ~VLAN_INFO_RAM_VLAN_FILT_MASK;
	writeRawVlanTbl(mgntVlan, data, lanPort);
	return (0);
}

void disableMgntVlan(void)
{
	uint32_t		data;
	uint32_t		dataCpy;
	uint16_t		mgntVlan;
	uint32_t		rdData;
	OS_ARCH_DECL_PROTECT(prv);

	// This operation must be protected because Ethernet device drivers can also access this register
	OS_ARCH_PROTECT(prv);
	// clear the 'match Vlan' control bit
	mrHalRegRead(CPU_MGNT_VLAN_CTRL_REG_ADDR, &data);
	dataCpy = data;
	data &= ~CPU_MGNT_VLAN_CTRL_MATCH_VLAN_MASK;
	mrHalRegWrite(CPU_MGNT_VLAN_CTRL_REG_ADDR, data);
	OS_ARCH_UNPROTECT(prv);

	if (dataCpy & CPU_MGNT_VLAN_CTRL_MATCH_VLAN_MASK)
	{
		mgntVlan = (uint16_t)((dataCpy & CPU_MGNT_VLAN_CTRL_VLAN_MASK) >> CPU_MGNT_VLAN_CTRL_VLAN_OFFSET);
		// If the mgnt Vlan had been enabled we must take further action...
		if (svcEvcReadList(0) == 0)
		{
			//If there are no EVCs configured then this Vlan in both ports should be set as un-blocked
			readRawVlanTbl(mgntVlan, &rdData, wanPort);
			rdData &= ~VLAN_INFO_RAM_VLAN_FILT_MASK;
			writeRawVlanTbl(mgntVlan, rdData, wanPort);
			readRawVlanTbl(mgntVlan, &data, lanPort);
			data &= ~VLAN_INFO_RAM_VLAN_FILT_MASK;
			writeRawVlanTbl(mgntVlan, data, lanPort);
		}
		else
		{
			//If there are one or more EVCs configured then this Vlan in both ports should be set as blocked
			readRawVlanTbl(mgntVlan, &rdData, wanPort);
			rdData |= VLAN_INFO_RAM_VLAN_FILT_MASK;
			writeRawVlanTbl(mgntVlan, rdData, wanPort);
			readRawVlanTbl(mgntVlan, &rdData, lanPort);
			rdData |= VLAN_INFO_RAM_VLAN_FILT_MASK;
			writeRawVlanTbl(mgntVlan, rdData, lanPort);
		}
	}
}

uint16_t getMgntVlan(void)
{
	uint32_t	data;

	mrHalRegRead(CPU_MGNT_VLAN_CTRL_REG_ADDR, &data);
	if (data & CPU_MGNT_VLAN_CTRL_MATCH_VLAN_MASK)
	{
		// The mgnt Vlan is enabled, extract and return the value
		return((uint16_t)((data & CPU_MGNT_VLAN_CTRL_VLAN_MASK) >> CPU_MGNT_VLAN_CTRL_VLAN_OFFSET));
	}
	return(0);		// return 0 if the mgnt Vlan is not enabled
}

int clock_time (struct timespec *tp)
{
#ifdef FREERTOS
	//getTimeOfDay (tp);
#else
	clock_gettime(CLOCK_REALTIME,tp);
#endif
	return 0;
}


int clock_gettime_plus_ms_delay (struct timespec *tp, uint32_t delay)
{
#ifdef FREERTOS
    //VR missing function
	//getTimeOfDay (tp);
#else
	clock_gettime(CLOCK_REALTIME,tp);
#endif
#ifdef DEBUG_OSAL_MQUEUES
	fprintf(stderr, "n=%lu.%lu\n",tp->tv_sec,tp->tv_nsec);		//TODO: ND: remove after debug
#endif
	tp->tv_sec += delay/1000;
	tp->tv_nsec += (delay % 1000) * 1000000;
	if (tp->tv_nsec >= 1000000000)
	{
		tp->tv_sec += 1;
		tp->tv_nsec -= 1000000000;
	}

	return 0;
}

void time_plus_ms_delay (struct timespec *tp, uint32_t delay)
{
	tp->tv_sec += delay/1000;
	tp->tv_nsec += (delay % 1000) * 1000000;
	if (tp->tv_nsec >= 1000000000)
	{
		tp->tv_sec += 1;
		tp->tv_nsec -= 1000000000;
	}

}

bool timeAisAfterTimeB(struct timespec *timeA,struct timespec *timeB)
{
	// Time A = A.a, B = B.b
	if (timeA->tv_sec > timeB->tv_sec)
	{
		// A > A
		return true;
	}
	else if (timeA->tv_sec < timeB->tv_sec)
	{
		//A < A
		return false;
	}
	else
	{
		// A == B e.g. 123.x 123.y
		if (timeA->tv_nsec > timeB->tv_nsec)
		{
			// a > b e.g. 123.6 > 123.5
			return true;
		}
		else if (timeA->tv_nsec < timeB->tv_nsec)
		{
			// a < b  e.g. 123.5 < 123.6
			return false;
		}
	}
	// they are equal A.a == B.b
	return false;
}

int64_t	deltaFromNowInMs(struct timespec *tm)
{
	struct timespec tnow;
	int64_t	nowMS;
	int64_t	tmMs;

#ifdef FREERTOS
	//getTimeOfDay(&tnow);
#else
	clock_gettime(CLOCK_REALTIME,&tnow);
#endif
	nowMS = (((int64_t)tnow.tv_sec) * 1000) + (int64_t)(tnow.tv_nsec/1000000);

	tmMs = (((int64_t)tm->tv_sec) * 1000) + (int64_t)(tm->tv_nsec/1000000);

	return (tmMs - nowMS);
}

