/*******************************************************************************
 * (c) Copyright 2015 Microsemi Corporation.  All rights reserved.
 *
 *
 *
 * SVN $Revision: $
 * SVN $Date: $
 */
#include <api_oam_sdk/common/types/inc/linked_list.h>
#include <api_oam_sdk/metroam_hal/inc/mr_hal.h>
#include <api_oam_sdk/service_interface/src/svc_iface_types.h>
#include <api_oam_sdk/service_l2cp/src/svc_l2cp_types.h>
#include "stdio.h"
#include "string.h"
#include "egl_types.h"
#include "egl_log.h"
#include "egl_assert.h"
#include "osal.h"

static l2cpGrpEntry_t	l2cpGrpArray[MAX_NUM_L2CP_GRPS];
static l2cpEntry_t		l2cpArray[MAX_NUM_L2CP];
static uint16_t			log_handle				= 0;		// handle used to log messages for this module
static osMutex_t		l2cpListLock			= NULL;
static bool				l2cpInitialised			= false;
static l2cpGrpIndex_t	l2cpGrpNextIndex 		= 1;
static l2cpNum_t		l2cpGrpCount;
static l2cpNum_t		l2cpCount;
static TAILQ_HEAD(l2cpGrpHeadEntry_s,struct l2cpGrpEntry_s,)	llL2cpGrpHead;

#define LOCK_L2CP_LIST(timeout)	osRecursiveMutexTake(l2cpListLock,timeout)
#define UNLOCK_L2CP_LIST()		osRecursiveMutexGive(l2cpListLock)

/*
 * This function is used to change the lowest level severity to log for
 * this module. Values of eLOG_LEVEL_DEBUG (lowest) to eLOG_LEVEL_ERROR
 * can be selected. Levels with greater severity than eLOG_LEVEL_ERROR
 * will default to eLOG_LEVEL_ERROR since severity of eLOG_LEVEL_ERROR
 * and higher are non maskable.
 */
void setL2cpLogLevel(log_level_t logLevel)
{
	setModuleLogLevel(log_handle,logLevel);
}

/*
 * @brief	This sets the l2cpEntry Group values to their default
 *
 * In most cases this should only be done with l2cpLock enabled
 * to ensure thread safe operation
 */
static void setL2cpGrpDefaults(l2cpGrpEntry_t *l2cpGrpEntry)
{
	memset(l2cpGrpEntry,0,sizeof(l2cpGrpEntry_t));
	l2cpGrpEntry->l2cpNextIndex = 1;
	TAILQ_INIT(&l2cpGrpEntry->llL2cpHead);
}

/*
 * @brief	This sets the l2cpEntry values to their default
 *
 * In most cases this should only be done with l2cpLock enabled
 * to ensure thread safe operation
 */
static void setL2cpDefaults(l2cpEntry_t *l2cpEntry)
{
	memset(l2cpEntry,0,sizeof(l2cpEntry_t));
	l2cpEntry->l2cpCtxt.l2cpNum 	= INVALID_L2CP_NUM;
	l2cpEntry->cfg.l2cpType			= l2cp_tunnel;
}

/*
 * @brief Clears the L2CP list
 *
 * This should be called to initialise the L2CP list.
 * Any L2CPs on the list will be lost.
 */
static void clearL2cpList(void)
{
	uint16_t	k;

	l2cpGrpNextIndex	= 1;
	l2cpGrpCount		= 0;
	l2cpCount			= 0;

	TAILQ_INIT(&llL2cpGrpHead);

	// Initialise all entries in the L2CP group store to defaults
	for (k = 0; k < MAX_NUM_L2CP_GRPS; k++)
	{
		setL2cpGrpDefaults(&l2cpGrpArray[k]);
	}
	// Initialise all entries in the L2CP store to defaults
	for (k = 0; k < MAX_NUM_L2CP; k++)
	{
		setL2cpDefaults(&l2cpArray[k]);
	}
	DO_LOG_DEBUG(log_handle,"Initialised the L2CP List");
}

/*
 * @brief	Called at device startup to initialise L2CP module
 *
 * Safe to call repeatedly.
 */
void l2cpInit(void)
{
	log_init();	// Initialises the logging system if it's not already initialised

	// Get a log handle if we don't already have one
	if (log_handle == 0)
		log_handle = log_register("L2CP");

	// Create a mutex to protect the MD list if we don't already have one
	if (l2cpListLock == NULL)
	{
		l2cpListLock = osRecursiveMutexCreate();
		ASSERT_NOT_NULL(l2cpListLock);
	}

	// If not already initialised clear the L2CP Group list for first use.
	if (!l2cpInitialised)
	{
		// Set the default logging level to NOTICE
		setModuleLogLevel(log_handle, eLOG_LEVEL_NOTICE);

		DO_LOG_DEBUG(log_handle,"Preparing the L2CP List for first use");
		LOCK_L2CP_LIST(0);
		clearL2cpList();
		UNLOCK_L2CP_LIST();
		l2cpInitialised = true;
	}
}

/*
 * @brief Convert an l2cpNum into a MD MIB index
 */
l2cpIndex_t _l2cpNumToMibIndex(l2cpNum_t l2cpNum)
{
	l2cpIndex_t l2cpIndex  = 0;

	LOCK_L2CP_LIST(0);
	if (l2cpArray[l2cpNum].l2cpCtxt.inUse)
	{
		l2cpIndex = l2cpArray[l2cpNum].l2cpCtxt.l2cpIndex;
	}
	UNLOCK_L2CP_LIST();
	return (l2cpIndex);
}

static l2cpGrpEntry_t *getL2cpGrpWithIndex(l2cpGrpIndex_t l2cpGrpIndex)
{
	l2cpGrpEntry_t		*l2cpGrpEntryPtr;

	// Start searching valid L2CP Groups at the head of the list
	l2cpGrpEntryPtr = TAILQ_FIRST(&llL2cpGrpHead);
	while (l2cpGrpEntryPtr != NULL)
	{
		if (l2cpGrpEntryPtr->l2cpGrpCtxt.l2cpGrpIndex == l2cpGrpIndex)
		{
			return (l2cpGrpEntryPtr);	//found it
		}
		l2cpGrpEntryPtr = TAILQ_NEXT(l2cpGrpEntryPtr,ll);
	}
	// if the l2cp group index is not found return NULL
	return (NULL);
}

static bool isL2cpGrpIndexValid(l2cpGrpIndex_t l2cpGrpIndex)
{
	l2cpGrpEntry_t		*l2cpGrpEntryPtr;

	l2cpGrpEntryPtr = getL2cpGrpWithIndex(l2cpGrpIndex);
	if (l2cpGrpEntryPtr == NULL)
	{
		return false;
	}
	return true;
}

/*
 * @brief Convert a l2cp index into an l2cpNum
 */
static l2cpNum_t	l2cpIndexToL2cpNumNoLocking(l2cpGrpEntry_t *l2cpGrpEntry, l2cpIndex_t l2cpIndex)
{
	l2cpEntry_t		*l2cpEntryPtr;
	l2cpNum_t		l2cpNum = -1;

	// Start searching valid MDs at the head of the list
	l2cpEntryPtr = TAILQ_FIRST(&l2cpGrpEntry->llL2cpHead);
	while (l2cpEntryPtr != NULL)
	{
		if (l2cpEntryPtr->l2cpCtxt.l2cpIndex == l2cpIndex)
		{
			l2cpNum = l2cpEntryPtr->l2cpCtxt.l2cpNum;
			return (l2cpNum);	//found it
		}
		l2cpEntryPtr = TAILQ_NEXT(l2cpEntryPtr,ll);
	}
	return -1;	// l2cpIndex not found
}

/*
 * @brief Convert a MD MIB index into an l2cpNum
 */
l2cpNum_t	_l2cpIndexToL2cpNum(l2cpGrpIndex_t l2cpGrpIndex, l2cpIndex_t l2cpIndex)
{
	l2cpGrpEntry_t 	*l2cpGrpEntryPtr;
	l2cpNum_t 		l2cpNum;

	LOCK_L2CP_LIST(0);
	l2cpGrpEntryPtr = getL2cpGrpWithIndex(l2cpGrpIndex);
	if (l2cpGrpEntryPtr == NULL)
	{
		return -1;	// l2cpIndex not found
	}

	l2cpNum = l2cpIndexToL2cpNumNoLocking(l2cpGrpEntryPtr, l2cpIndex);
	UNLOCK_L2CP_LIST();
	return l2cpNum;
}

/*
 * @brief Read the next entry in the L2CP List
 *
 * @param[in] l2cpIndex of previous entry or 0
 * @return	l2cpIndex of the next entry or 0 if no more entries
 *
 * Read the next entry in the L2CP List. If prevL2cpIndex is 0 it
 * returns the l2cpIndex of the first L2CP entry or 0 if there are
 * no L2CP entries. If a valid l2cpIndex is provided it returns
 * the l2cpIndex of the next L2CP in the list.
 */
l2cpGrpIndex_t svcL2cpGrpReadList(l2cpGrpIndex_t prevL2cpGrpIndex)
{
	l2cpGrpIndex_t	theIndex;
	l2cpGrpEntry_t	*l2cpGrpEntry;

	LOCK_L2CP_LIST(0);
	if (prevL2cpGrpIndex == 0)
	{
		if (!TAILQ_EMPTY(&llL2cpGrpHead))
		{
			theIndex = TAILQ_FIRST(&llL2cpGrpHead)->l2cpGrpCtxt.l2cpGrpIndex;
			UNLOCK_L2CP_LIST();
			return theIndex;
		}
	}
	else
	{
		l2cpGrpEntry = TAILQ_FIRST(&llL2cpGrpHead);
		while (l2cpGrpEntry != NULL)
		{
			if (l2cpGrpEntry->l2cpGrpCtxt.l2cpGrpIndex == prevL2cpGrpIndex)
			{
				if (TAILQ_NEXT(l2cpGrpEntry,ll) != NULL)
				{
					theIndex = TAILQ_NEXT(l2cpGrpEntry,ll)->l2cpGrpCtxt.l2cpGrpIndex;
					UNLOCK_L2CP_LIST();
					return theIndex;
				}
				else
				{
					UNLOCK_L2CP_LIST();
					return 0;
				}
			}
			l2cpGrpEntry = TAILQ_NEXT(l2cpGrpEntry,ll);
		}
	}
	UNLOCK_L2CP_LIST();
	return 0;
}

/*
 * @brief Read the next entry in the L2CP List
 *
 * @param[in] l2cpIndex of previous entry or 0
 * @return	l2cpIndex of the next entry or 0 if no more entries
 *
 * Read the next entry in the L2CP List. If prevL2cpIndex is 0 it
 * returns the l2cpIndex of the first L2CP entry or 0 if there are
 * no L2CP entries. If a valid l2cpIndex is provided it returns
 * the l2cpIndex of the next L2CP in the list.
 */
l2cpIndex_t svcL2cpReadList(l2cpGrpIndex_t l2cpGrpIndex, l2cpIndex_t prevL2cpIndex)
{
	l2cpGrpEntry_t	*l2cpGrpEntryPtr;
	l2cpNum_t 		nextL2cpNum;
	l2cpIndex_t		theIndex;

	LOCK_L2CP_LIST(0);

	l2cpGrpEntryPtr = getL2cpGrpWithIndex(l2cpGrpIndex);
	if (l2cpGrpEntryPtr == NULL)
	{
		return 0;	// l2cpIndex not found
	}

	if (prevL2cpIndex == 0)
	{
		if (!TAILQ_EMPTY(&l2cpGrpEntryPtr->llL2cpHead))
		{
			theIndex = TAILQ_FIRST(&l2cpGrpEntryPtr->llL2cpHead)->l2cpCtxt.l2cpIndex;
			UNLOCK_L2CP_LIST();
			return theIndex;
		}
	}
	else
	{
		nextL2cpNum = l2cpIndexToL2cpNumNoLocking(l2cpGrpEntryPtr, prevL2cpIndex);
		if (nextL2cpNum != -1)
		{
			if (TAILQ_NEXT(&l2cpArray[nextL2cpNum],ll) != NULL)
			{
				theIndex = TAILQ_NEXT(&l2cpArray[nextL2cpNum],ll)->l2cpCtxt.l2cpIndex;
				UNLOCK_L2CP_LIST();
				return theIndex;
			}
		}
	}
	UNLOCK_L2CP_LIST();
	return 0;
}

/*
 * @brief	Find the next free L2CP Entry
 */
static l2cpGrpEntry_t* getNextFreeL2cpGrpEntry(void)
{
	l2cpNum_t	k;

	LOCK_L2CP_LIST(0);
	for (k = 0; k < MAX_NUM_L2CP_GRPS; k++)
	{
		if (!l2cpGrpArray[k].l2cpGrpCtxt.inUse)
		{
			l2cpGrpArray[k].l2cpGrpCtxt.inUse = true;
			UNLOCK_L2CP_LIST();
			return (&l2cpGrpArray[k]);
		}
	}
	return NULL;
}

/*
 * @brief	Find the next free L2CP Entry
 */
static l2cpEntry_t* getNextFreeL2cpEntry(void)
{
	l2cpNum_t	k;

	LOCK_L2CP_LIST(0);
	for (k = 0; k < MAX_NUM_L2CP; k++)
	{
		if (!l2cpArray[k].l2cpCtxt.inUse)
		{
			l2cpArray[k].l2cpCtxt.l2cpNum = k;
			l2cpArray[k].l2cpCtxt.inUse = true;
			UNLOCK_L2CP_LIST();
			return (&l2cpArray[k]);
		}
	}
	return NULL;
}

/*
 * @brief	Add a new l2cpEntry into the L2CP List
 */
static void addL2cpGrpToList(l2cpGrpEntry_t *l2cpGrpEntry)
{
	ASSERT_NOT_NULL(l2cpGrpEntry);

	TAILQ_INSERT_TAIL(&llL2cpGrpHead,l2cpGrpEntry,ll);
	l2cpGrpCount++;

	// The L2CP Grp index can be specified by the user. So
	// it is necessary to keep the l2cpNextIndex aligned
	if (l2cpGrpNextIndex <= l2cpGrpEntry->l2cpGrpCtxt.l2cpGrpIndex)
	{
		l2cpGrpNextIndex = l2cpGrpEntry->l2cpGrpCtxt.l2cpGrpIndex + 1;
	}
}

/*
 * @brief	Add a new l2cpEntry into the L2CP List
 */
static void addL2cpToList(l2cpGrpEntry_t *l2cpGrpEntry, l2cpEntry_t *l2cpEntry)
{
	ASSERT_NOT_NULL(l2cpEntry);

	TAILQ_INSERT_TAIL(&l2cpGrpEntry->llL2cpHead,l2cpEntry,ll);
	l2cpCount++;

	// The L2CP index can be specified by the user. So
	// it is necessary to keep the l2cpNextIndex aligned
	if (l2cpGrpEntry->l2cpNextIndex <= l2cpEntry->l2cpCtxt.l2cpIndex)
	{
		l2cpGrpEntry->l2cpNextIndex = l2cpEntry->l2cpCtxt.l2cpIndex + 1;
	}
}

/*
 * @brief	Remove the L2CP group entry, pointed to by l2cpEntry, from the L2CP List
 */
static void removeL2cpGrpFromList(l2cpGrpEntry_t *l2cpGrpEntry)
{
	ASSERT_NOT_NULL(l2cpGrpEntry);

	TAILQ_REMOVE(&llL2cpGrpHead,l2cpGrpEntry,ll);
	setL2cpGrpDefaults(l2cpGrpEntry);
	l2cpGrpCount--;
}

/*
 * @brief	Remove the L2CP entry, pointed to by l2cpEntry, from the L2CP List
 */
static void removeL2cpFromList(l2cpGrpEntry_t *l2cpGrpEntry, l2cpEntry_t *l2cpEntry)
{
	ASSERT_NOT_NULL(l2cpEntry);

	TAILQ_REMOVE(&l2cpGrpEntry->llL2cpHead,l2cpEntry,ll);
	setL2cpDefaults(l2cpEntry);
	l2cpCount--;
}

/*
 * @brief Returns the next available L2CP Group index
 */
l2cpIndex_t	svcL2cpGrpGetNextIndex(void)
{
	return l2cpGrpNextIndex;
}

/*
 * @brief Returns the next available L2CP index
 */
l2cpIndex_t	svcL2cpGetNextL2cpIndex(l2cpGrpIndex_t l2cpGrpIndex)
{
	l2cpGrpEntry_t	*l2cpGrpEntry;
	l2cpIndex_t		theIndex;

	LOCK_L2CP_LIST(0);
	l2cpGrpEntry = TAILQ_FIRST(&llL2cpGrpHead);
	while (l2cpGrpEntry != NULL)
	{
		if (l2cpGrpEntry->l2cpGrpCtxt.l2cpGrpIndex == l2cpGrpIndex)
		{
			theIndex = l2cpGrpEntry->l2cpNextIndex;
			UNLOCK_L2CP_LIST();
			return theIndex;
		}
		l2cpGrpEntry = TAILQ_NEXT(l2cpGrpEntry,ll);
	}
	UNLOCK_L2CP_LIST();
	return 0;
}

/*
 * @brief Test if the supplied L2CP Index is valid
 */
bool svcL2cpGrpIsL2cpGrpIndexValid(l2cpGrpIndex_t l2cpGrpIndex)
{
	l2cpGrpEntry_t	*l2cpGrpEntryPtr;

	LOCK_L2CP_LIST(0);
	// Validate the l2cpIndex value provided

	l2cpGrpEntryPtr = getL2cpGrpWithIndex(l2cpGrpIndex);
	if (l2cpGrpEntryPtr == NULL)
	{
		UNLOCK_L2CP_LIST();
		DO_LOG_DEBUG(log_handle,"Operation rejected due to invalid L2CP group Index (%lu)",l2cpGrpIndex);
		return (false);
	}
	UNLOCK_L2CP_LIST();
	return(true);
}

/*
 * @brief Test if the supplied L2CP Index is valid
 */
static bool isL2cpIndexValid(l2cpGrpEntry_t	*l2cpGrpEntry, l2cpIndex_t l2cpIndex, l2cpNum_t *l2cpNum)
{
	l2cpNum_t	theL2cpNum;

	if ((l2cpIndex == 0) || (l2cpIndex >= l2cpGrpEntry->l2cpNextIndex))
	{
L2CP_INDEX_INVALID:
		DO_LOG_DEBUG(log_handle,"The L2CP Index (%lu) was invalid",l2cpIndex);
		return false;
	}

	// Test is the l2cpIndex known and get it's associated l2cpNum
	theL2cpNum = l2cpIndexToL2cpNumNoLocking(l2cpGrpEntry,l2cpIndex);
	if (theL2cpNum != -1)		// If the L2CP index is found
	{
		if (l2cpNum != NULL)
		{
			*l2cpNum = theL2cpNum;
		}
		return(true);
	}
	goto L2CP_INDEX_INVALID;
}

/*
 * @brief Test if the supplied L2CP Index is valid
 */
bool svcL2cpIsL2cpIndexValid(l2cpGrpIndex_t l2cpGrpIndex, l2cpIndex_t l2cpIndex)
{
	bool retVal;
	l2cpGrpEntry_t	*l2cpGrpEntryPtr;

	LOCK_L2CP_LIST(0);
	// Validate the l2cpIndex value provided

	l2cpGrpEntryPtr = getL2cpGrpWithIndex(l2cpGrpIndex);
	if (l2cpGrpEntryPtr != NULL)
	{
		UNLOCK_L2CP_LIST();
		DO_LOG_DEBUG(log_handle,"Operation rejected due to invalid L2CP group Index (%lu)",l2cpGrpIndex);
		return (false);
	}

	retVal = isL2cpIndexValid(l2cpGrpEntryPtr, l2cpIndex, NULL);
	UNLOCK_L2CP_LIST();
	return(retVal);
}

/*
 * @brief Create a new L2CP Group.
 */
int svcL2cpGrpCreate(l2cpGrpIndex_t l2cpGrpIndex)
{
	l2cpGrpEntry_t	*l2cpGrpEntryPtr;

	LOCK_L2CP_LIST(0);
	// Validate the l2cpIndex value provided
	if ((l2cpGrpIndex == 0) || isL2cpGrpIndexValid(l2cpGrpIndex))
	{
		UNLOCK_L2CP_LIST();
		DO_LOG_INFO(log_handle,"Operation rejected due to invalid L2CP group Index (%lu)",l2cpGrpIndex);
		return (1);
	}

	l2cpGrpEntryPtr = getNextFreeL2cpGrpEntry();
	if (l2cpGrpEntryPtr != NULL)
	{
		TAILQ_INIT(&l2cpGrpEntryPtr->llL2cpHead);

		l2cpGrpEntryPtr->l2cpGrpCtxt.l2cpGrpIndex = l2cpGrpIndex;
		// add the new L2CP to the list
		addL2cpGrpToList(l2cpGrpEntryPtr);
	}
	else
	{
		UNLOCK_L2CP_LIST();
		DO_LOG_INFO(log_handle,"Operation rejected due to insufficient resources");
		return (-1);
	}
	UNLOCK_L2CP_LIST();
	DO_LOG_NOTICE(log_handle,"Created L2CP Group with index = %lu",l2cpGrpIndex);
	return (2); // Operation accepted.
}

/**
 * @brief	Delete the L2CP Group with the supplied l2cpIndex.
 *
 */
int svcL2cpGrpDelete(l2cpGrpIndex_t l2cpGrpIndex)
{
	l2cpGrpEntry_t	*l2cpGrpEntry;
	svcIfaceCfg_t 	svcIFaceCfg;
	l2cpIndex_t		l2cpIndex;
	l2cpIndex_t		l2cpIndexToDelete;

	LOCK_L2CP_LIST(0);
	l2cpGrpEntry = TAILQ_FIRST(&llL2cpGrpHead);
	while (l2cpGrpEntry != NULL)
	{
		if (l2cpGrpEntry->l2cpGrpCtxt.l2cpGrpIndex == l2cpGrpIndex)
		{

		    EGL_ASSERT(svcIfaceRead(wanPort, &svcIFaceCfg,NULL,NULL) == 2, 1);
			if (svcIFaceCfg.l2cpGrpIndex == l2cpGrpIndex)
			{
L2CP_GRP_INUSE:
				UNLOCK_L2CP_LIST();
				DO_LOG_INFO(log_handle,"Operation rejected because L2CP group (index = %lu) is in use",l2cpGrpIndex);
				return 2;
			}
			EGL_ASSERT(svcIfaceRead(lanPort, &svcIFaceCfg,NULL,NULL) == 2, 1);
			if (svcIFaceCfg.l2cpGrpIndex == l2cpGrpIndex)
			{
				goto L2CP_GRP_INUSE;
			}
			if (_evcIsL2cpGrpInUse(l2cpGrpIndex))
			{
				goto L2CP_GRP_INUSE;
			}

			// Remove all associated L2CPs...
			l2cpIndex = svcL2cpReadList(l2cpGrpIndex,0);
			while(l2cpIndex !=0)
			{
				l2cpIndexToDelete = l2cpIndex;
				l2cpIndex = svcL2cpReadList(l2cpGrpIndex,l2cpIndex);
				svcL2cpDelete(l2cpGrpIndex, l2cpIndexToDelete);
			}
			removeL2cpGrpFromList(l2cpGrpEntry);
			UNLOCK_L2CP_LIST();
			DO_LOG_NOTICE(log_handle,"Deleted L2CP Group with index = %lu",l2cpGrpIndex);
			return 3;
		}
		l2cpGrpEntry = TAILQ_NEXT(l2cpGrpEntry,ll);
	}

	UNLOCK_L2CP_LIST();
	DO_LOG_INFO(log_handle,"Operation rejected due to invalid or nonexistent L2CP group (index = %lu)",l2cpGrpIndex);
	return (1);	//Operation rejected due to invalid or nonexistent Maintenance Domain
}

static void updateL2cpHardware(svcL2cpCfg_t	*svcL2cpCfg)
{
	uint32_t		selReg;
	uint32_t		data;
	uint32_t		actionCode;

	selReg = 0;
	if ((svcL2cpCfg->macAddress[5] & 0xF0) == 0x0)
	{
		selReg = BRIDGE_RX_ADDR_DEST_REG_ADDR;
	}

	if ((svcL2cpCfg->macAddress[5] & 0xF0) == 0x2)
	{
		selReg = MRP_RX_ADDR_DEST_REG_ADDR;
	}

	if (selReg != 0)
	{
		mrHalRegRead(selReg, &data);
		// clear the 2-bits for the selected address (bits 1:0 = DA -n0, bits 3:2 = DA -n1, ...,bits 31:30 = DA -nF; n=0/2)
		data &= ~(0x00000003 << ((svcL2cpCfg->macAddress[5] & 0xF) * 2));
		switch (svcL2cpCfg->l2cpType)
		{
			case l2cp_discard:
			{
				actionCode = 0x0;			// This MAC will be discarded
				break;
			}
			case l2cp_tunnel:
			case l2cp_passToEvc:
			{
				actionCode = 0x2;			// This MAC will pass straight through
				break;
			}
			case l2cp_peer:
			{
				actionCode = 0x1;			// This MAC will be diverted to the CPU for processing
				break;
			}
			default:
			{
				actionCode = 0x2;			// Default action for the MAC is to pass straight through
			}
		}
		data |= (actionCode << ((svcL2cpCfg->macAddress[5] & 0xF) * 2));
		mrHalRegWrite(selReg, data);
	}
}

static bool _l2cpGrpInUse(l2cpGrpIndex_t l2cpGrpIndex)
{
	svcIfaceCfg_t svcIfaceCfg;

	svcIfaceRead(wanPort, &svcIfaceCfg,NULL,NULL);
	if (svcIfaceCfg.l2cpGrpIndex == l2cpGrpIndex)
	{
		return (true);
	}

	svcIfaceRead(lanPort, &svcIfaceCfg,NULL,NULL);
	if (svcIfaceCfg.l2cpGrpIndex == l2cpGrpIndex)
	{
		return (true);
	}

	// TODO: if L2CP support for EVCs is ever added then need code to check if L2CP Group is in use on any EVC
	return(false);
}

/*
 * @brief Create a new service L2CP.
 */
int svcL2cpCreate(l2cpGrpIndex_t l2cpGrpIndex, l2cpIndex_t l2cpIndex, svcL2cpCfg_t *svcL2cpCfg)
{
	l2cpGrpEntry_t	*l2cpGrpEntryPtr;
	l2cpEntry_t		*l2cpEntryPtr;

	LOCK_L2CP_LIST(0);

	l2cpGrpEntryPtr = getL2cpGrpWithIndex(l2cpGrpIndex);
	if (l2cpGrpEntryPtr == NULL)
	{
		UNLOCK_L2CP_LIST();
		DO_LOG_INFO(log_handle,"Operation rejected due to invalid L2CP Group Index (%lu)",l2cpGrpIndex);
		return (1);
	}

	// Validate the l2cpIndex value provided
	if ((l2cpIndex == 0) || isL2cpIndexValid(l2cpGrpEntryPtr,l2cpIndex,NULL))
	{
		UNLOCK_L2CP_LIST();
		DO_LOG_INFO(log_handle,"Operation rejected due to invalid L2CP Index (%lu)",l2cpIndex);
		return (1);
	}

	if (svcL2cpCfg == NULL)
	{
		UNLOCK_L2CP_LIST();
		DO_LOG_INFO(log_handle,"Operation rejected due to NULL svcBwpCfg pointer");
		return (-1);
	}

	//Begin testing the fields of the entry...
	// Test the L2CP type
	if ((svcL2cpCfg->l2cpType > l2cp_passToEvc) || (svcL2cpCfg->l2cpType < l2cp_discard))
	{
		UNLOCK_L2CP_LIST();
		DO_LOG_INFO(log_handle,"Operation rejected due to invalid L2CP type (%d)",svcL2cpCfg->l2cpType);
		return(2); //Operation rejected due to invalid
	}

	// Test the L2CP matchScope
	if (svcL2cpCfg->matchScope != destinationAddressOnly)
	{
		UNLOCK_L2CP_LIST();
		DO_LOG_INFO(log_handle,"Operation rejected due to invalid L2CP matchScope (%d) only 1 supported",svcL2cpCfg->matchScope);
		return(3); //Operation rejected due to invalid
	}

	// Verify the MAC address provided. Valid values are 01-80-C2-00-00-00 through 01-80-C2-00-00-0F and 01-80-C2-00-00-20 through 01-80-C2-00-00-2F
	if (svcL2cpCfg->macAddress[0] != 0x01 ||
			svcL2cpCfg->macAddress[1] != 0x80 ||
			svcL2cpCfg->macAddress[2] != 0xC2 ||
			svcL2cpCfg->macAddress[3] != 0x00 ||
			svcL2cpCfg->macAddress[4] != 0x00 ||
			((svcL2cpCfg->macAddress[5] > 0x0F && svcL2cpCfg->macAddress[5] < 0x20) ||
			(svcL2cpCfg->macAddress[5] > 0x2F && svcL2cpCfg->macAddress[5] <= 0xFF)))
	{
		UNLOCK_L2CP_LIST();
		DO_LOG_INFO(log_handle,"Operation rejected due to invalid L2CP mac-address");
		return(3); //Operation rejected due to invalid
	}

	l2cpEntryPtr = getNextFreeL2cpEntry();
	if (l2cpEntryPtr != NULL)
	{
		l2cpEntryPtr->l2cpCtxt.l2cpIndex = l2cpIndex;
		// Set the mibIdx to the one provided and update the other values
		memcpy(&l2cpEntryPtr->cfg, svcL2cpCfg, sizeof(svcL2cpCfg_t));
		// add the new L2CP to the list
		addL2cpToList(l2cpGrpEntryPtr, l2cpEntryPtr);

		// Update hardware only if the L2CP Group index is 'in Use'( i.e. assigned to an interface (L2CP Not supported on EVCs in this version!))
		if (_l2cpGrpInUse(l2cpGrpIndex))
		{
			updateL2cpHardware(svcL2cpCfg);
		}
	}
	else
	{
		UNLOCK_L2CP_LIST();
		DO_LOG_INFO(log_handle,"Operation rejected due to insufficient resources");
		return (-2);
	}
	UNLOCK_L2CP_LIST();
	DO_LOG_NOTICE(log_handle,"Created L2CP with index = %lu",l2cpIndex);
	return (4); // Operation accepted.
}

/**
 * @brief	Delete the L2CP with the supplied l2cpIndex.
 *
 */
int svcL2cpDelete(l2cpGrpIndex_t l2cpGrpIndex, l2cpIndex_t l2cpIndex)
{
	l2cpGrpEntry_t	*l2cpGrpEntryPtr;
	l2cpNum_t		l2cpNum;

	LOCK_L2CP_LIST(0);

	l2cpGrpEntryPtr = getL2cpGrpWithIndex(l2cpGrpIndex);
	if (l2cpGrpEntryPtr == NULL)
	{
		UNLOCK_L2CP_LIST();
		DO_LOG_INFO(log_handle,"Operation rejected due to invalid L2CP Group Index (%lu)",l2cpGrpIndex);
		return (1);
	}

	// Test is the l2cpIndex known and get it's associated l2cpNum
	if (!isL2cpIndexValid(l2cpGrpEntryPtr,l2cpIndex, &l2cpNum))
	{
		UNLOCK_L2CP_LIST();
		DO_LOG_INFO(log_handle,"Operation rejected due to invalid or nonexistent L2CP (index = %lu)",l2cpIndex);
		return (1);	//Operation rejected due to invalid or nonexistent Maintenance Domain
	}

	if (_evcIsL2cpGrpInUse(l2cpGrpIndex))
	{
		UNLOCK_L2CP_LIST();
		DO_LOG_INFO(log_handle,"Operation rejected because L2CP (index = %lu.%lu) is in use",l2cpGrpIndex,l2cpIndex);
		return (2);	//Operation rejected due to invalid or nonexistent BWP index
	}

	// Return the hardware to tunnel default
	l2cpArray[l2cpNum].cfg.l2cpType = l2cp_tunnel;
	updateL2cpHardware(&l2cpArray[l2cpNum].cfg);

	removeL2cpFromList(l2cpGrpEntryPtr, &l2cpArray[l2cpNum]);

	UNLOCK_L2CP_LIST();
	DO_LOG_NOTICE(log_handle,"Deleted L2CP with index = %lu",l2cpIndex);
	return (3); // Operation accepted
}

/*
 * @brief Read the service L2CP configuration.
 */
int svcL2cpRead(l2cpGrpIndex_t l2cpGrpIndex, l2cpIndex_t l2cpIndex, svcL2cpCfg_t *svcL2cpCfg)
{
	l2cpGrpEntry_t	*l2cpGrpEntryPtr;
	l2cpNum_t		l2cpNum;

	LOCK_L2CP_LIST(0);

	l2cpGrpEntryPtr = getL2cpGrpWithIndex(l2cpGrpIndex);
	if (l2cpGrpEntryPtr == NULL)
	{
		UNLOCK_L2CP_LIST();
		DO_LOG_INFO(log_handle,"Operation rejected due to invalid L2CP Group Index (%lu)",l2cpGrpIndex);
		return (1);
	}

	// Test is the l2cpIndex known and get it's associated l2cpNum
	if (!isL2cpIndexValid(l2cpGrpEntryPtr, l2cpIndex, &l2cpNum))
	{
		UNLOCK_L2CP_LIST();
		DO_LOG_INFO(log_handle,"Operation rejected due to invalid or nonexistent L2CP index (%lu)",l2cpIndex);
		return (1);
	}

	if (svcL2cpCfg != NULL)
	{
		memcpy(svcL2cpCfg, &l2cpArray[l2cpNum].cfg, sizeof(svcL2cpCfg_t));
	}

	UNLOCK_L2CP_LIST();
	DO_LOG_DEBUG(log_handle,"Read info for L2CP (%lu)",l2cpIndex);
	return (2); // Operation accepted
}


/**
 * @brief Updated the Hardware L2CP settings. Must be called when a non zero L2CP group index is added to an interface
 * @param iFace
 * @param l2cpGrpIndex
 *
 * NOTE: There is only one set of L2CP control registers on the device, not one per port or per EVC as required by the standard.
 * So if this function is called with different group settings for different interfaces then only the most recent settings will
 * take effect.
 */
void addL2CPGroupToInterface(iFace_t iFace, l2cpGrpIndex_t l2cpGrpIndex)
{
	l2cpGrpEntry_t	*l2cpGrpEntryPtr;
	l2cpEntry_t		*l2cpEntryPtr;

	// Ignore any call with an index of 0
	if (l2cpGrpIndex == 0)
	{
		return;
	}

	l2cpGrpEntryPtr = getL2cpGrpWithIndex(l2cpGrpIndex);
	l2cpEntryPtr = TAILQ_FIRST(&(l2cpGrpEntryPtr->llL2cpHead));
	while (l2cpEntryPtr != NULL)
	{
		updateL2cpHardware(&l2cpEntryPtr->cfg);
		l2cpEntryPtr = TAILQ_NEXT(l2cpEntryPtr,ll);
	}
}
