GD32F450_BMC_BaseCode/app/bmc/msghndlr/MsgHndlrTask.c

/*
* Brief:	Receive message from Interface and send response to original interface.
			Process all IPMI standard command and oem command.
* Author:	Jimbo_Zhang@outlook.com
* Date:		2019-9-13, the Mid-autumn Festival;
*/

#include <stdio.h>
#include <sys/stat.h>
#include <sys/socket.h>
#include <sys/un.h>
#include <sys/prctl.h>
#include <errno.h>
#include <fcntl.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <pthread.h>
#include "MsgHndlr.h"
#include "PDKCmds.h"
#include "App.h"
#include "PICMG.h"
#include "Bridge.h"
#include "SensorEvent.h"
#include "Chassis.h"
#include "Storage.h"
#include "cmdselect.h"
#include "com_IPMI_App.h"
#include "com_IPMI_Storage.h"
#include "com_IPMI_SDR.h"
#include "DeviceConfig.h"

//#include "main.h"
#include <string.h>
#include "main.h"

static void ProcessIPMIReq (MsgPkt_T* pReq, MsgPkt_T* pRes);
static void PendingBridgeResTimerTask (void);
static void PendingSeqNoTimerTask (void);


MsgHndlrTbl_T m_MsgHndlrTbl [] =	//notice!
{
	{ NETFN_APP,                    g_App_CmdHndlr                  },
	{ NETFN_CHASSIS,                g_Chassis_CmdHndlr              },
	{ NETFN_BRIDGE,                 g_Bridge_CmdHndlr               },
	{ NETFN_SENSOR,                 g_SensorEvent_CmdHndlr          },
	{ NETFN_STORAGE,                g_Storage_CmdHndlr              },
	{ NETFN_TRANSPORT,              g_Config_CmdHndlr               },
//	{ NETFN_AMI,                    (CmdHndlrMap_T*)g_AMI_CmdHndlr  },
	{	0,							0	},
};


GroupExtnMsgHndlrTbl_T m_GroupExtnMsgHndlrTbl [] =		//jimbo add
{
	{0,		0},
};


TimerTaskTbl_T    m_TimerTaskTbl [20] =
{
//	{ 1,    PEFTimerTask	},
//	{ 1,    PETAckTimerTask },
//	{ 1,    PEFStartDlyTimerTask },
	{ 1,    SessionTimerTask },
//	{ 1,    PendingBridgeResTimerTask },
//	{ 1,    PendingSeqNoTimerTask },
//	{ 1,    FlashTimerTask },

//	#if FRB_SUPPORT == 1
//	{ 1,    FRB3TimerTask   },
//	#endif /* #if FRB_SUPPORT == 1 */
//
//	#if SERIAL_MODEM_CONNECTION_ACTIVITY  != UNIMPLEMENTED
//	{ 2,    SerialModemPingTask },
//	#endif /* SERIAL_MODEM_CONNECTION_ACTIVITY */
//
//	{ 1,    MonitorPassword },
	{ 1,    UDSSessionTimerTask },

};

//PendingBridgedResTbl_T	m_PendingBridgedResTbl[MAX_PENDING_BRIDGE_RES];

PendingBridgedResTbl_T	m_PendingBridgedResTbl[MAX_PENDING_BRIDGE_TBL][MAX_PENDING_BRIDGE_RES];
PendingSeqNoTbl_T	m_PendingSeqNoTbl[16][MAX_PENDING_SEQ_NO];
KCSBridgeResInfo_T       m_KCSBridgeResInfo;
int gFd_MsgHndlrIfc;
TLS_T g_tls;
/*!
    \brief      Message handler Task. Process all standard and oem ipmi message form interface, and send response back.
    \param[in]  none
    \param[out] none
    \retval     none
*/
void *MsgHndlrTask( void *pArg )
{
	MsgPkt_T reqMsgPkt;
	MsgPkt_T resMsgPkt;
	uint8_t channelinit = 0xF;

	printf("MsgHndlrTask start...\r\n");

	prctl(PR_SET_NAME,__FUNCTION__,0,0,0);

 	//create 
    if(-1 != access(MSG_HNDLR_Q, F_OK))
    {
        remove(MSG_HNDLR_Q);
    }
    if(0 != mkfifo (MSG_HNDLR_Q, 0777))
    {
        printf("%s: Create %s fifo failed! %s\n", __FUNCTION__, MSG_HNDLR_Q, strerror(errno));
        return (void*)-1;
    }
    gFd_MsgHndlrIfc = open (MSG_HNDLR_Q, O_RDWR);
    if(-1 == gFd_MsgHndlrIfc)
    {
        printf("%s: Open %s fifo failed! %s\n", __FUNCTION__, MSG_HNDLR_Q, strerror(errno));
        return (void*)-1;
    }
	
	pthread_key_create(&g_tls.CurSessionID,NULL);
	pthread_key_create(&g_tls.CurPrivLevel,NULL);
	pthread_key_create(&g_tls.CurChannel,NULL);
	pthread_key_create(&g_tls.CurKCSIfcNum,NULL);
	pthread_key_create(&g_tls.CurSessionType,NULL);
	pthread_key_create(&g_tls.OwnerLUN,NULL);

	pthread_setspecific(g_tls.CurChannel,&channelinit);
	while(1)
	{
		while(GetMsg(gFd_MsgHndlrIfc, &reqMsgPkt, WAIT_INFINITE) != 0);
		// printf("===> Mrx: ");
		// int i;
		// for(i=0;i<reqMsgPkt.Size;i++)
		// 	printf("%#x ", reqMsgPkt.Data[i]);
		// printf("\n");

		pthread_setspecific(g_tls.CurChannel,&reqMsgPkt.Channel);
		pthread_setspecific(g_tls.CurPrivLevel,&reqMsgPkt.Privilege);
		pthread_setspecific(g_tls.CurSessionID,&reqMsgPkt.SessionID);
		pthread_setspecific(g_tls.CurSessionType,&reqMsgPkt.SessionType);
		pthread_setspecific(g_tls.OwnerLUN,&reqMsgPkt.NetFnLUN);

		SwapIPMIMsgHdr ((IPMIMsgHdr_T*)reqMsgPkt.Data, (IPMIMsgHdr_T*)resMsgPkt.Data);
		ProcessIPMIReq(&reqMsgPkt, &resMsgPkt);

		/* Skip current request if no response from Command Handler */
		if ((reqMsgPkt.SrcQ == gFd_LanResQ) && (sizeof(IPMIMsgHdr_T) == resMsgPkt.Size) && (NORMAL_RESPONSE == resMsgPkt.Param) )
		{
			continue;
		}

		if ((reqMsgPkt.SrcQ == gFdUdsRes) && (resMsgPkt.Size == sizeof(IPMIUDSMsg_T)) && (NORMAL_RESPONSE == resMsgPkt.Param) )
		{
			continue;
		}

		//update checksum2
		resMsgPkt.Data[resMsgPkt.Size]	=	CalculateCheckSum2(resMsgPkt.Data, resMsgPkt.Size);
		resMsgPkt.Size++;

		if(g_BMCInfo.IpmiConfig.UDSIfcSupport == 0x01 && (reqMsgPkt.SrcQ == gFdUdsRes))
		{			
			//TODO:UDS的数据结构在最后多了一个0，找机会要给他去掉。
			resMsgPkt.Size = resMsgPkt.Size+1;
			SwapUDSIPMIMsg(&reqMsgPkt,&resMsgPkt);
			resMsgPkt.Data[resMsgPkt.Size-1] = 0;
		}

		if(SYS_IFC_CHANNEL == reqMsgPkt.Channel)
		{  
			/* Skip current request if no response from Command Handler */
			if (!resMsgPkt.Size && (NO_RESPONSE == resMsgPkt.Param) )
			{
				continue;
			}
			
			resMsgPkt.SessionID = reqMsgPkt.SessionID;
		}
		//int i;
		// printf("===> Mtx to %d: ", resMsgPkt.SrcQ);
		// for(i=0;i<resMsgPkt.Size;i++)
		// 	printf("%#x ", resMsgPkt.Data[i]);
		// printf("\n");

		//send response pkt to source queue.
		PostMsg(resMsgPkt.SrcQ, &resMsgPkt);
		
	}
}


/**
*@fn ValidateMsgHdr
*@brief Validates the Message header and keeps track of the messages that has been bridged
*@param pReq Request packet of the command to be executed
*@return Returns -1 in case of the response to the bridged message
*            Returns 0 otherwise
*/
int
ValidateMsgHdr (MsgPkt_T* pReq)
{
	int  j;
	uint8_t PBTbl = PRIMARY_PB_TBL;
	IPMIMsgHdr_T*	pIPMIMsgReq = (IPMIMsgHdr_T*)&pReq->Data;
	ChannelInfo_T*  pChannelInfo;
	uint8_t SeqNum = NET_FN(pIPMIMsgReq->RqSeqLUN);
	int Queuefd = 0;

	/* Check for the request message LUN */
	switch (pReq->NetFnLUN & 0x03)
	{
		case BMC_LUN_00:
			if (0 == (pReq->NetFnLUN & 0x04))
			{
				return 0;	//IPMB做从
			}

			PBTbl = ( (pReq->Channel == SECONDARY_IPMB_CHANNEL) ? SECONDARY_PB_TBL : PRIMARY_PB_TBL );
			
			
			if ( (TRUE 		== m_PendingBridgedResTbl[PBTbl][SeqNum].Used) &&
				 (NET_FN(pIPMIMsgReq->NetFnLUN)  == NET_FN((m_PendingBridgedResTbl[PBTbl][SeqNum].ReqMsgHdr.NetFnLUN + 0x04)))  &&
				 (pIPMIMsgReq->Cmd               == m_PendingBridgedResTbl[PBTbl][SeqNum].ReqMsgHdr.Cmd)  &&
				 (pIPMIMsgReq->ReqAddr           == m_PendingBridgedResTbl[PBTbl][SeqNum].ReqMsgHdr.ResAddr))
			{        

				 /* Change the requester's address, sequence number and LUN */
				 pIPMIMsgReq->ResAddr  = m_PendingBridgedResTbl[PBTbl][SeqNum].ReqMsgHdr.ReqAddr;
				 pIPMIMsgReq->RqSeqLUN = m_PendingBridgedResTbl[PBTbl][SeqNum].ReqMsgHdr.RqSeqLUN;

				 /* Calculate the Second CheckSum */
				 pReq->Data[ pReq->Size - 1 ] = CalculateCheckSum2( pReq->Data, pReq->Size - 1 );
				 pReq->Data[2] = (~(pReq->Data[0] + pReq->Data[1])) +1; //2's complement checksum

				 pReq->Param = BRIDGING_REQUEST;

				 
				 if (m_PendingBridgedResTbl[PBTbl][SeqNum].DestQ == gFd_LanIfcQ)
				 {			
					  for (j=pReq->Size - 1; j>=0; j--)
					  {
						   pReq->Data[j+1] = pReq->Data[j];
					  }
					  pReq->Data[0] = m_PendingBridgedResTbl[PBTbl][SeqNum].SrcSessionHandle;
					  pReq->Size++;
					  pReq->Cmd = PAYLOAD_IPMI_MSG;
					  Queuefd = gFd_LanIfcQ;
				 }
				 else if(m_PendingBridgedResTbl[PBTbl][SeqNum].DestQ == gFdUdsIfc)
				 {
				 	pReq->Cmd = PAYLOAD_IPMI_MSG;
				 	pReq->Socket = m_PendingBridgedResTbl[PBTbl][SeqNum].UDSSocket;
				 	memcpy(&pReq->Data[sizeof(IPMIUDSMsg_T)], &pReq->Data[0], pReq->Size);
				 	memcpy(&pReq->Data[0], &m_PendingBridgedResTbl[PBTbl][SeqNum].ResMsgHdr.UDSMsgHdr, sizeof(IPMIUDSMsg_T));
				 	pReq->Size += sizeof(IPMIUDSMsg_T);
				 	pReq->Size++;
				 	pReq->Data[pReq->Size-1] = 0;

				 	((IPMIUDSMsg_T*)&pReq->Data[0])->IPMIMsgLen = pReq->Size;

				 	Queuefd = gFdUdsIfc;
				 }
				 else
				 {
				 	Queuefd = m_PendingBridgedResTbl[PBTbl][SeqNum].DestQ;
				 }
						
				 /* Post the data to Destination Interface queue */
//				 printf("===> ValidateMsgHdr post message to %d\n", Queuefd);
				 PostMsg (Queuefd, pReq);

				 m_PendingBridgedResTbl[PBTbl][SeqNum].Used = FALSE;
				 m_PendingBridgedResTbl[PBTbl][SeqNum].ResDataOk = 0;
			}
			return -1;
		case BMC_LUN_01:
		case BMC_LUN_10:
		case BMC_LUN_11:
			//TODO:
		default:
			return -1;
	}
}

/**
*@fn ProcessIPMIReq
*@brief Processes the requested IPMI command
*@param pReq Request of the command
*@param pRes Response for the requested command
*@return none
*/
static void
ProcessIPMIReq (MsgPkt_T* pReq, MsgPkt_T* pRes)
{
	CmdHndlrMap_T *    	pCmdHndlrMap;
	uint32_t            HdrOffset = 0;
	uint8_t				CmdOverride = 1;
	int8_t 				MsgHndlrMapGot=0;
	uint8_t              PBTbl = PRIMARY_PB_TBL;
	
//	msgHndlr_dbg_printf ("Processing IPMI Packet.\r\n");

	/* Set the Cmd and Net function in response packet */
	pRes->Cmd		= 	pReq->Cmd;
	pRes->NetFnLUN	= 	pReq->NetFnLUN;
	pRes->Channel	=	pReq->Channel;
	pRes->SrcQ		=	pReq->SrcQ;
	pRes->Socket 	=	pReq->Socket;
	/* Normal IPMI Command response */
	pRes->Param 	= 	PARAM_NORMAL_RESPONSE;
	
	if ((SYS_IFC_CHANNEL != pReq->Channel) && (USB_CHANNEL != pReq->Channel) && (UDS_CHANNEL != pReq->Channel))
	{
		//printf("log 1, pReq->Channel: %#x\n", pReq->Channel);
		HdrOffset = sizeof (IPMIMsgHdr_T);
		pReq->Size  = pReq->Size - HdrOffset - 1;
	}
	if(UDS_CHANNEL == pReq->Channel)
	{
		//printf("log 2, pReq->Size: %d, HdrOffset: %d\n", pReq->Size, HdrOffset);
		HdrOffset = sizeof(IPMIUDSMsg_T);
		pReq->Size = pReq->Size -HdrOffset - 1;
	}
	
	SwapIPMIMsgHdr((IPMIMsgHdr_T*)pReq->Data, (IPMIMsgHdr_T*)pRes->Data);
	pRes->Size  = HdrOffset + sizeof(uint8_t);

	pthread_setspecific(g_tls.CurSessionID,&pReq->SessionID);

	if(IsCommandEnabled(NET_FN(pReq->NetFnLUN), &pReq->Data[HdrOffset], pReq->Cmd) != 0)
	{
		pRes->Data [HdrOffset] = CC_INV_CMD;
		printf ("Invalid Net Function 0x%x or Invalid Command 0x%x\n",NET_FN(pReq->NetFnLUN), pReq->Cmd);
		pRes->Size	+= 2;
		return;
	}
	
	//Get oem command map
	MsgHndlrMapGot = PDKGetOEMMsgHndlrMap(NET_FN(pReq->NetFnLUN),&pCmdHndlrMap);
	if(MsgHndlrMapGot == 0)
	{
		CmdOverride = GetCmdHndlr(pReq,pRes,pCmdHndlrMap,HdrOffset,CmdOverride,&pCmdHndlrMap);
	}	
	//get standard ipmi command map
	if((CmdOverride == 0 || MsgHndlrMapGot == -1) )	//OEM commands have no command map.
    {
		if (0 != GetMsgHndlrMap (NET_FN (pReq->NetFnLUN), &pCmdHndlrMap))
		{
			// if(pBMCInfo->IpmiConfig.GrpExtnSupport == 1)
			// {
				if (0 != GroupExtnGetMsgHndlrMap (NET_FN (pReq->NetFnLUN), pReq->Data [HdrOffset], &pCmdHndlrMap) )
				{
					pRes->Data [HdrOffset] = CC_INV_CMD;
					printf ("MsgHndlr.c : Invalid Net Function 0x%x or Invalid Command 0x%x\n",NET_FN(pReq->NetFnLUN), pReq->Cmd);
					pRes->Size	+= 2;
					return;
				}
			// }
			// else
			// {
			// 	pRes->Data [HdrOffset] = CC_INV_CMD;
			// 	printf ("MsgHndlr.c : Invalid Net Function 0x%x\n",NET_FN(pReq->NetFnLUN));
			// 	pRes->Size	+= 2;
			// 	return;
			// }
		}

		if(GetCmdHndlr(pReq,pRes,pCmdHndlrMap,HdrOffset,CmdOverride,&pCmdHndlrMap) == FALSE)
		{
			pRes->Data [HdrOffset] = CC_INV_CMD;
			pRes->Size	+= 2;
			//printf("log 3\n");
			return;
		}
   }
   
	/* Check for the request size */
	if (0xff != pCmdHndlrMap->ReqLen)
	{
		/* Check for invalid request size */
		if (pCmdHndlrMap->ReqLen != pReq->Size)
		{	
			printf("Warning: Request data len error, require %d\n", pCmdHndlrMap->ReqLen);
			pRes->Data [HdrOffset] = CC_REQ_INV_LEN;
			pRes->Size	+= 2;
			return;
		}
	}
	
	pRes->Size 	= 	pCmdHndlrMap->CmdHndlr (&pReq->Data [HdrOffset], pReq->Size, &pRes->Data [HdrOffset]) + HdrOffset;
		
	//prepare response data for send message command
	if( (CMD_SEND_MSG == pReq->Cmd) && (NETFN_APP == pReq->NetFnLUN >> 2))
	{
//		printf("log 4, size %d, Channel %d\n",pRes->Size, pRes->Channel);
		int Offset = 0;
		uint8_t SeqNum = g_BMCInfo.SendMsgSeqNum;
		if ((0 == pRes->Size) &&
			((g_BMCInfo.IpmiConfig.PrimaryIPMBSupport == 0x01  && PRIMARY_IPMB_CHANNEL == pRes->Channel) || 
			 (g_BMCInfo.IpmiConfig.SecondaryIPMBSupport == 0x01 && SECONDARY_IPMB_CHANNEL == pRes->Channel)) )
		{
			pRes->Param = PARAM_NO_RESPONSE;
			Offset = HdrOffset + 2;
		}
		else if (HdrOffset == pRes->Size)
		{
			Offset = HdrOffset + 1;
		}

	    PBTbl = ( ((pReq->Data[sizeof (IPMIMsgHdr_T)] & 0x0F) == SECONDARY_IPMB_CHANNEL) ? SECONDARY_PB_TBL : PRIMARY_PB_TBL );
		
		int i;
		while(TRUE)
		{ 
			if ( (TRUE == m_PendingBridgedResTbl[PBTbl][SeqNum].Used) 
				&& (0 == memcmp (&m_PendingBridgedResTbl[PBTbl][SeqNum].ReqMsgHdr, &pReq->Data[Offset], sizeof (IPMIMsgHdr_T))) )
			{
				if(m_PendingBridgedResTbl[PBTbl][SeqNum].DestQ == gFd_LanIfcQ)
				{				
					memcpy (&m_PendingBridgedResTbl[PBTbl][SeqNum].ResMsgHdr, pRes->Data, sizeof (IPMIMsgHdr_T));
				}
				else if(m_PendingBridgedResTbl[PBTbl][SeqNum].DestQ == gFdUdsIfc)
				{
					//pRes->Size ++;
					SwapUDSIPMIMsg ( pReq, pRes);
					//pRes->Data[pRes->Size-1] = 0;
					memcpy (&m_PendingBridgedResTbl[PBTbl][SeqNum].ResMsgHdr, pRes->Data, sizeof (IPMIUDSMsg_T));	
					m_PendingBridgedResTbl[PBTbl][SeqNum].UDSSocket = pRes->Socket;
					printf("log666 \n");
					for(i=0;i<sizeof (IPMIUDSMsg_T);i++)
						printf("%#x ", pRes->Data[i]);
					printf("\n");
				}
				 m_PendingBridgedResTbl[PBTbl][SeqNum].ResDataOk = 1;
				 break;    
			}
			else
			{
				SeqNum = (SeqNum - 1) & 0x3F;
				if(SeqNum == g_BMCInfo.SendMsgSeqNum)
				{
					break; 
				}
			}
		}
	}
	
	return;
}

/**
*@fn CalculateChecksum2
*@brief Calculates the checksum
*@param Pkt Pointer to the data for the checksum to be calculated
*@param Len Size of data for checksum calculation
*@return Returns the checksum value
*/
uint8_t CalculateCheckSum2 (uint8_t* Pkt, uint32_t Len)
{
	uint8_t	Sum;
	uint32_t	i;

	/* Get Checksum 2 */
	Sum = 0;
	for (i = 3; i < Len; i++)
	{
		Sum += Pkt [i];
	}
	return (uint8_t)(0xFF & (0x100 - Sum));
}


/**
*@fn GetMsgHndlrMap
*@brief Gets the exact command Handler by comparing NetFn
*@param Netfn -NetFunction of the Cmd to execute
*@param pCmdHndlrMap Pointer to the Command Handler
*@return Returns 0 on success
*            Returns -1 on failure
*/
int
GetMsgHndlrMap (uint8_t NetFn, CmdHndlrMap_T ** pCmdHndlrMap)
{
	int i;

	/* Get the command handler corresponding to the net function */
	for (i = 0; i <  sizeof(m_MsgHndlrTbl)/sizeof(m_MsgHndlrTbl[0]); i++)		//pBMCInfo->MsgHndlrTblSize 
	{
		if (m_MsgHndlrTbl [i].NetFn == NetFn) { break; }
	}

	/* Check if we have not found our net function */
	if (i ==  sizeof(m_MsgHndlrTbl)/sizeof(m_MsgHndlrTbl[0]) )
	{
		return -1;
	}

	/* Get the handler corresponding to the command */
	*pCmdHndlrMap = (CmdHndlrMap_T*)m_MsgHndlrTbl [i].CmdHndlrMap;
	return 0;
}

/**
*@fn GetCmdHndlr
*@brief Picks up the exact command to execute by comparing Cmd no.
*@param pReq Request buffer for the command
*@param pRes Response buffer for the command
*@param pCmdHndlrMap
*@param HdrOffset
*@param CmdOverride
*@param CmdHndlr
*@return Returns TRUE on success
*            Returns FALSE on failure
*/
int GetCmdHndlr(MsgPkt_T* pReq,MsgPkt_T* pRes,CmdHndlrMap_T* pCmdHndlrMap,
								uint32_t HdrOffset,uint8_t CmdOverride,CmdHndlrMap_T** CmdHndrl )
{
	int i=0;
	while (1)
	{
		/**
		* If we reached the end of the Command Handler map - invalid command
		**/
		if (0 == pCmdHndlrMap->CmdHndlr)
		{
			if(CmdOverride == FALSE)
			{
				pRes->Data [HdrOffset] = CC_INV_CMD;
				printf( "Warning: MsgHndlr.c : Invalid Command %#x\r\n", pReq->Cmd );
			}
			return FALSE;
		}

		if (pCmdHndlrMap->Cmd == pReq->Cmd)
		{
			break;
		}
		
		i++;
		pCmdHndlrMap++;

	}
	*CmdHndrl = pCmdHndlrMap;
	return TRUE;
}

/**
*@fn GroupExtnGetMsgHndlrMap
*@brief Gets the exact command Handler by comparing NetFn
*@param Netfn -NetFunction of the Cmd to execute
*@GroupExtnCode - Group Extension code
*@param pCmdHndlrMap Pointer to the Command Handler
*@return Returns 0 on success
*            Returns -1 on failure
*/
int
GroupExtnGetMsgHndlrMap (uint8_t NetFn, uint8_t GroupExtnCode, CmdHndlrMap_T ** pCmdHndlrMap)
{
	int i;

	/* Get the command handler corresponding to the net function */
	for (i = 0; i < sizeof (m_GroupExtnMsgHndlrTbl) / sizeof (m_GroupExtnMsgHndlrTbl [0]); i++)
	{
		if ((m_GroupExtnMsgHndlrTbl [i].NetFn == NetFn) && (m_GroupExtnMsgHndlrTbl [i].GroupExtnCode == GroupExtnCode)) 
		{
			break; 
		}
	}

	/* Check if we have not found our net function */
	if (i == sizeof (m_GroupExtnMsgHndlrTbl) / sizeof (m_GroupExtnMsgHndlrTbl[0]))
	{
		return -1;
	}

//	printf("---> GroupExtnGetMsgHndlrMap successful, i=%d\n",i);
	/* Get the handler corresponding to the command */
	*pCmdHndlrMap = (CmdHndlrMap_T*)m_GroupExtnMsgHndlrTbl [i].CmdHndlrMap;
	return 0;
}

/*------------------------------------------------------------------
 *@fn RespondSendMessage
 *@brief Frames the Response packet when a IPMB destination is
 *       unavialable
 *
 *@param pReq:    Request Message Packet address
 *@param Status   Status of SendIPMBPkt method
 *@param BMCInst: BMC Instance Number
 *
 *@return none
 *-----------------------------------------------------------------*/
void RespondSendMessage ( MsgPkt_T* pReq, uint8_t Status)
{
	uint8_t PBTbl = PRIMARY_PB_TBL;

	MsgPkt_T   ResPkt;
	IPMIMsgHdr_T*  pIPMIResHdr = ( IPMIMsgHdr_T*)ResPkt.Data;
	IPMIMsgHdr_T*  pIPMIReqHdr = ( IPMIMsgHdr_T*)pReq->Data;
	uint8_t SeqNum = NET_FN(pIPMIReqHdr->RqSeqLUN);		//IPMB的sequence
	int QueueFd;
	int HdrOffset = 0;
	
	//printf("---> RespondSendMessage Status = %d, log111\n", Status);
  /* Check for pending responses */
	PBTbl = ( (pReq->Channel == SECONDARY_IPMB_CHANNEL) ? SECONDARY_PB_TBL : PRIMARY_PB_TBL );
	if ( (TRUE 		== m_PendingBridgedResTbl[PBTbl][SeqNum].Used) &&
		 (NET_FN(pIPMIReqHdr->NetFnLUN)  == NET_FN(m_PendingBridgedResTbl[PBTbl][SeqNum].ReqMsgHdr.NetFnLUN )) &&
		 (pIPMIReqHdr->Cmd               == m_PendingBridgedResTbl[PBTbl][SeqNum].ReqMsgHdr.Cmd) &&
		 (pIPMIReqHdr->ResAddr           == m_PendingBridgedResTbl[PBTbl][SeqNum].ReqMsgHdr.ResAddr)  )
	{
		//printf("---> RespondSendMessage/ m_PendingBridgedResTbl: PBTbl: %d, SeqNum %d, log222\n", PBTbl, SeqNum);
		// int i;
		// for(i=0;i<6;i++)
		// 	printf("%#x ", ((uint8_t*)&m_PendingBridgedResTbl[PBTbl][SeqNum].ResMsgHdr.IPMIMsgHdr)[i]);
		// printf("\n");
		uint32_t tcnt = 0;
		while(m_PendingBridgedResTbl[PBTbl][SeqNum].ResDataOk == 0)
		{
			usleep(1);
			tcnt++;
			if(tcnt > 100000)	
			{
				printf("Warning: %s-%s Response data not ok!\n", __FILE__, __FUNCTION__);
				break;	//100ms
			}
		}

		if((m_PendingBridgedResTbl[PBTbl][SeqNum].OriginSrc == ORIGIN_INT_REQ) && (Status == STATUS_OK))
		{
			return;
		}

		if(m_PendingBridgedResTbl[PBTbl][SeqNum].DestQ == gFd_LanIfcQ)
		{
			memcpy (pIPMIResHdr, &m_PendingBridgedResTbl[PBTbl][SeqNum].ResMsgHdr.IPMIMsgHdr, sizeof (IPMIMsgHdr_T));
			HdrOffset = sizeof(IPMIMsgHdr_T);
		}
		else if(m_PendingBridgedResTbl[PBTbl][SeqNum].DestQ == gFdUdsIfc)
		{
			memcpy (pIPMIResHdr, &m_PendingBridgedResTbl[PBTbl][SeqNum].ResMsgHdr.UDSMsgHdr, sizeof (IPMIUDSMsg_T));
			HdrOffset = sizeof(IPMIUDSMsg_T);
		}
		else
		{
			//SwapIPMIMsgHdr ( pIPMIReqHdr, pIPMIResHdr);
		   	//QueueFd = m_PendingBridgedResTbl[PBTbl][SeqNum].DestQ;
			memcpy (pIPMIResHdr, &m_PendingBridgedResTbl[PBTbl][SeqNum].ResMsgHdr.IPMIMsgHdr, sizeof (IPMIMsgHdr_T));
			HdrOffset = sizeof(IPMIMsgHdr_T);
		}

		 if (STATUS_OK == Status)
		 {
			if ( ( m_PendingBridgedResTbl[PBTbl][SeqNum].DestQ == gFd_PrimaryIpmbIfcQ) ||
				 (m_PendingBridgedResTbl[PBTbl][SeqNum].DestQ == gFd_SecondaryIpmbIfcQ) ||
				 (ORIGIN_SENDMSG != m_PendingBridgedResTbl[PBTbl][SeqNum].OriginSrc) )
			{    
				 return;
			}
				
			ResPkt.Data [HdrOffset] = CC_NORMAL;
		 }
		 else if (STATUS_FAIL == Status)
		 {
			ResPkt.Data [HdrOffset] = CC_NO_ACK_FROM_SLAVE;
		 }
		 else
		 {
			ResPkt.Data [HdrOffset] = CC_UNSPECIFIED_ERR;
		 }

		 ResPkt.Size = HdrOffset + 1 + 1; // IPMI Header + Completion Code + Second Checksum
		 ResPkt.Cmd = pIPMIResHdr->Cmd;
		 ResPkt.NetFnLUN = pIPMIReqHdr->NetFnLUN;
		 ResPkt.Channel = pReq->Channel;
		 //ResPkt.Socket = pReq->Socket;

		 /* Calculate the Second CheckSum */
		 ResPkt.Data[ResPkt.Size - 1] = CalculateCheckSum2 (ResPkt.Data, ResPkt.Size-1);

		 ResPkt.Param = BRIDGING_REQUEST;

		 if (m_PendingBridgedResTbl[PBTbl][SeqNum].DestQ == gFd_LanIfcQ)
		 {
			 QueueFd = gFd_LanResQ;
		 }
		 else if(m_PendingBridgedResTbl[PBTbl][SeqNum].DestQ == gFdUdsIfc)
		 {
		 	ResPkt.Size++;
		 	ResPkt.Data[ResPkt.Size-1] = 0;
		 	((IPMIUDSMsg_T*)&ResPkt.Data[0])->IPMIMsgLen = ResPkt.Size;
		 	ResPkt.Socket = m_PendingBridgedResTbl[PBTbl][SeqNum].UDSSocket;
		 	QueueFd = gFdUdsRes;
		 }
		 else
		 {
			QueueFd = m_PendingBridgedResTbl[PBTbl][SeqNum].DestQ;
		 }

		 if (STATUS_OK != Status)
		 {
			 m_PendingBridgedResTbl[PBTbl][SeqNum].Used = FALSE;
			 m_PendingBridgedResTbl[PBTbl][SeqNum].ResDataOk = 0;
		 }
		 
		 // /* Post the data to Destination Interface queue */
		 // int i;
		 // printf("===> RespondSendMessage post to %d: ", QueueFd);
		 // for(i=0;i<ResPkt.Size;i++)
		 // 	printf("%#x ", ResPkt.Data[i]);
		 // printf("\n");
		 PostMsg (QueueFd, &ResPkt);
	}
}


/**
*@fn PendingBridgeResTimerTask
*@brief Sends the timeout message to response queue
*          if the message does not turn out within send message timeout
*@return none
*/
static void
PendingBridgeResTimerTask (void)
{
	uint8_t		i;
	uint8_t     PBTbl = PRIMARY_PB_TBL;
	int 		QueueFd = 0;
	printf("-> PendingBridgeResTimerTask\n");
	/* Check for any pending responses */
	for( PBTbl=0; PBTbl < MAX_PENDING_BRIDGE_TBL; PBTbl++)
	{
		for (i = 0; i < sizeof (m_PendingBridgedResTbl[0])/sizeof (m_PendingBridgedResTbl[0][0]); i++)
		{
			if (TRUE == m_PendingBridgedResTbl[PBTbl][i].Used)
			{
				m_PendingBridgedResTbl[PBTbl][i].TimeOut--;
				if (0 == m_PendingBridgedResTbl[PBTbl][i].TimeOut)
				{
				   MsgPkt_T		Timeout;
				   IPMIMsgHdr_T*   pIPMIMsgHdr  = (IPMIMsgHdr_T*) Timeout.Data;

				   /* Fill the response packet */
				   SwapIPMIMsgHdr (&m_PendingBridgedResTbl[PBTbl][i].ReqMsgHdr, pIPMIMsgHdr);

				   // slog(LANQueueName,"%s%d",LAN_IFC_Q,BMCInst);
				   // sprintf(PrimaryIPMBQueueName,"%s%d",IPMB_PRIMARY_IFC_Q,BMCInst);
				   // sprintf(SecondaryIPMBQueueName,"%s%d",IPMB_SECONDARY_IFC_Q,BMCInst);
				   // sprintf(SerialQueueName,"%s%d",SERIAL_IFC_Q,BMCInst);

				   if(PBTbl == PRIMARY_PB_TBL)
				   {
					   pIPMIMsgHdr->ReqAddr = g_BMCInfo.PrimaryIPMBAddr;
				   }
				   else if(PBTbl == SECONDARY_PB_TBL)
				   {
					   pIPMIMsgHdr->ReqAddr = g_BMCInfo.SecondaryIPMBAddr;
				   }
				   else
				   {
					   printf("Invalid PBTbl\n");;//pIPMIMsgHdr->ReqAddr = pBMCInfo->IpmiConfig.BMCSlaveAddr;
				   }

				   Timeout.Data [sizeof(IPMIMsgHdr_T)] = CC_TIMEOUT;

				   Timeout.Size = sizeof (IPMIMsgHdr_T) + 1 + 1; // IPMI Header + Completion Code + Second Checksum

				   /* Calculate the Second CheckSum */
				   Timeout.Data[Timeout.Size - 1] = CalculateCheckSum2 (Timeout.Data, Timeout.Size-1);

				   Timeout.Param = BRIDGING_REQUEST;

				   if (m_PendingBridgedResTbl[PBTbl][i].DestQ == gFd_LanIfcQ)
				   {
					   int j;
					   for (j = Timeout.Size - 1; j >= 0; --j)
					   {
						   Timeout.Data [j+1] = Timeout.Data [j];
					   }

					   Timeout.Data[0] = m_PendingBridgedResTbl[PBTbl][i].SrcSessionHandle;
					   Timeout.Size++;
					   Timeout.Cmd = PAYLOAD_IPMI_MSG;
					   QueueFd = gFd_LanIfcQ;
					}
				   // else if (0 == strcmp ((char *)m_PendingBridgedResTbl[PBTbl][i].DestQ, SerialQueueName))
				   // {
					  //  int j;
					  //  for (j = Timeout.Size - 1; j >= 0; --j)
					  //  {
						 //   Timeout.Data [j+1] = Timeout.Data [j];
					  //  }

					  //  Timeout.Data[0] = m_PendingBridgedResTbl[PBTbl][i].SrcSessionHandle;
					  //  Timeout.Size++;
					  //  Timeout.Cmd = PAYLOAD_IPMI_MSG;
					  //  strcpy(QueueName, SERIAL_IFC_Q);
				   // }
				 //   else if (g_BMCInfo.IpmiConfig.PrimaryIPMBSupport == 1 && m_PendingBridgedResTbl[PBTbl][i].DestQ == gFd_PrimaryIpmbIfcQ)
					// {
					//    int j;
					//    for (j = Timeout.Size - 1; j >= 0; --j)
					//    {
					// 	   Timeout.Data [j + sizeof (IPMIMsgHdr_T) + 1] = Timeout.Data [j];
					//    }
					//    memcpy (Timeout.Data, &m_PendingBridgedResTbl[PBTbl][i].ResMsgHdr, sizeof (IPMIMsgHdr_T));
					//    Timeout.Data[sizeof (IPMIMsgHdr_T)] = CC_NORMAL;
					//    Timeout.Size++;
					//    QueueFd = gFd_PrimaryIpmbIfcQ;

					//  }
					//  else if (g_BMCInfo.IpmiConfig.SecondaryIPMBSupport == 1 && m_PendingBridgedResTbl[PBTbl][i].DestQ == gFd_SecondaryIpmbIfcQ)
					//  {
					// 	int j;
					// 	for (j = Timeout.Size - 1; j >= 0; --j)
					// 	{
					// 		Timeout.Data [j + sizeof (IPMIMsgHdr_T) + 1] = Timeout.Data [j];
					// 	}
					// 	memcpy (Timeout.Data, &m_PendingBridgedResTbl[PBTbl][i].ResMsgHdr.IPMIMsgHdr, sizeof (IPMIMsgHdr_T));
					// 	Timeout.Data[sizeof (IPMIMsgHdr_T)] = CC_NORMAL;
					// 	Timeout.Size++;
					// 	QueueFd = gFd_SecondaryIpmbIfcQ;
					//  }
					 else if (m_PendingBridgedResTbl[PBTbl][i].DestQ = gFdUdsIfc)
					 {
					 	printf("---> UDS pending bridge TimeOut\n");
						 int j;
						 UDSSessionTbl_T *pUDSSessionInfo = NULL;
						 for (j = sizeof (IPMIMsgHdr_T); j < Timeout.Size ;j++)
						 {
							Timeout.Data [j + sizeof (IPMIUDSMsg_T)-sizeof (IPMIMsgHdr_T)] = Timeout.Data [j];
						 }
						 Timeout.Size = Timeout.Size + sizeof (IPMIUDSMsg_T)- sizeof (IPMIMsgHdr_T);
						 
						 m_PendingBridgedResTbl[PBTbl][i].ResMsgHdr.UDSMsgHdr.IPMIMsgLen = Timeout.Size;
						 memcpy (Timeout.Data, &m_PendingBridgedResTbl[PBTbl][i].ResMsgHdr.UDSMsgHdr, sizeof (IPMIUDSMsg_T));
						 Timeout.NetFnLUN = m_PendingBridgedResTbl[PBTbl][i].ResMsgHdr.UDSMsgHdr.NetFnLUN;
						 Timeout.SessionID = m_PendingBridgedResTbl[PBTbl][i].ResMsgHdr.UDSMsgHdr.SessionID;
						 Timeout.Cmd = m_PendingBridgedResTbl[PBTbl][i].ResMsgHdr.UDSMsgHdr.Cmd;
						 
						 pUDSSessionInfo = GetUDSSessionInfo (UDS_SESSION_ID_INFO,&Timeout.SessionID);
						 if(pUDSSessionInfo != NULL)
						 {
							 Timeout.Socket = pUDSSessionInfo->UDSSocket;
						 }
						 QueueFd = m_PendingBridgedResTbl[PBTbl][i].DestQ;
					 }
					 else 
					 {
						int j;
						for (j = Timeout.Size - 1; j >= 0; --j)
						{
						   Timeout.Data [j + sizeof (IPMIMsgHdr_T) + 1] = Timeout.Data [j];
						}
						memcpy (Timeout.Data, &m_PendingBridgedResTbl[PBTbl][i].ResMsgHdr.IPMIMsgHdr, sizeof (IPMIMsgHdr_T));
						Timeout.Data[sizeof (IPMIMsgHdr_T)] = CC_TIMEOUT;
						QueueFd = m_PendingBridgedResTbl[PBTbl][i].DestQ;
					 }

					  /* Post the data to Destination Interface queue */
					  PostMsg (QueueFd, &Timeout);

					  m_PendingBridgedResTbl[PBTbl][i].Used = FALSE;
					  printf( "MsgHndlr: clean pending index = %d.\n", i );
				}
		   }
		}
	}
}


/**
*@fn PendingSeqNoTimerTask
*@brief Timertask which helpful in IPMB sequence number validation
*@return none
*/
static void
PendingSeqNoTimerTask (void)
{
	uint8_t	i,j;
	printf("-> PendingSeqNoTimerTask\n");
	/* Check for any Sequence Number expiraied */
	for (i = 0; i < sizeof (m_PendingSeqNoTbl)/sizeof (m_PendingSeqNoTbl[0]); i++)
	{
		for (j = 0; j < sizeof (m_PendingSeqNoTbl[0])/sizeof (m_PendingSeqNoTbl[0][0]); j++)
		{
			if (TRUE == m_PendingSeqNoTbl[i][j].Used)
			{
				m_PendingSeqNoTbl[i][j].TimeOut--;
				if (0 == m_PendingSeqNoTbl[i][j].TimeOut)
				{
					m_PendingSeqNoTbl[i][j].Used = FALSE;
				}
			}
		}
	}
}


/**
*@fn SwapIPMIMsgHdr
*@brief Swaps the header and copies into response
*@param pIPMIMsgReq Header of the Request
*@param pIPMIMsgRes Header of the response
*@return none
*/
void
SwapIPMIMsgHdr (IPMIMsgHdr_T* pIPMIMsgReq, IPMIMsgHdr_T* pIPMIMsgRes)
{
	pIPMIMsgRes->ResAddr   = pIPMIMsgReq->ReqAddr;
	pIPMIMsgRes->NetFnLUN  = (pIPMIMsgReq->NetFnLUN & 0xFC) + 0x04;
	pIPMIMsgRes->NetFnLUN |= pIPMIMsgReq->RqSeqLUN & 0x03;

	/* Calculate the Checksum for above two bytes */
	pIPMIMsgRes->ChkSum   = (~(pIPMIMsgRes->ResAddr + pIPMIMsgRes->NetFnLUN) + 1);

	pIPMIMsgRes->ReqAddr  = pIPMIMsgReq->ResAddr;

	pIPMIMsgRes->RqSeqLUN = (pIPMIMsgReq->RqSeqLUN & 0xFC);
	pIPMIMsgRes->RqSeqLUN |= (pIPMIMsgReq->NetFnLUN & 0x03);

	pIPMIMsgRes->Cmd = pIPMIMsgReq->Cmd;

	return;
}

/*
	UDS data format, LSB first
	uint32_t  + uint16_t   + uint8_t  + uint8_t + uint8_t    + uint8_t* + uint8_t  + uint8_t
	SessionID + IPMIMsgLen + NetFnLUN + Cmd     + ChannelNum + Data[n]  + checksum +  0(多余的，想办法去掉)
*/

/**
 * *@fn Swap UDSIPMIMsg
 * *@brief Swaps the header and copies into response
 * *@param pIPMIMsgReq Header of the Request
 * *@param pIPMIMsgRes Header of the response
 * *@return none
 * */
void
SwapUDSIPMIMsg ( MsgPkt_T* pIPMIMsgReq,  MsgPkt_T* pIPMIMsgRes)
{
	IPMIUDSMsg_T *pIPMIUDSMsgRes = (IPMIUDSMsg_T *)&pIPMIMsgRes->Data[0];
	IPMIUDSMsg_T *pIPMIUDSMsgReq = (IPMIUDSMsg_T *)&pIPMIMsgReq->Data[0];

	pIPMIMsgRes->NetFnLUN = pIPMIMsgReq->NetFnLUN;
	pIPMIMsgRes->Cmd = pIPMIMsgReq->Cmd;
	pIPMIMsgRes->Channel = pIPMIMsgReq->Channel;
	pIPMIMsgRes->SessionID   = pIPMIMsgReq->SessionID;
	pIPMIMsgRes->Socket = pIPMIMsgReq->Socket;

	pIPMIUDSMsgRes->NetFnLUN = pIPMIMsgReq->NetFnLUN;
	pIPMIUDSMsgRes->Cmd = pIPMIMsgReq->Cmd;
	pIPMIUDSMsgRes->ChannelNum = pIPMIMsgReq->Channel;
	pIPMIUDSMsgRes->SessionID = pIPMIMsgReq->SessionID;
	pIPMIUDSMsgRes->IPMIMsgLen = pIPMIMsgRes->Size;
	return;
}