cs.c

包括ＥＰＡ协议栈

#include "list.h"
#include "tcpip.h"
#define CS_GLOBALS
#include "cs.h"
#undef CS_GLOBALS
#include "epa.h"
#include "ctimer.h"
#include <string.h>

#define CS_NO_NPKT          (0)

#define PTP_EVENT_PORT      (319)
#define PTP_GERNERAL_PORT   (320)
#define NODE_AMOUNT_ALL     (0x20)

static OS_STK    CSendStack[STACK_SIZE_CSEND];
static void*     CSendMsgQTbl[MSG_AMOUNT_CSEND];
static OS_EVENT* gpNodeSem;
static OS_MEM*   gpNodeMCB;

static Foo       gFooPool[NODE_AMOUNT_ALL];
static Foo       gPrdList;
static Foo       gNonPrdList;
static Foo       gGlbNonPrdList;
static uint8     gAnnedNPkt;

static void CSendTask(void* pdata);

/*-----------------------------------------------------------------------*
 *- Template of EndAnn pkt
 *----------------------------
 *- Destination MAC address(0) :               FF FF FF FF FF FF
 *- Source MAC address(6) :                    CD CD CD CD CD CD
 *- Frame type(12) :                           08 00
 *- Destination IP address(26) :               CD CD CD CD
 *- Source IP address(30) :                    CD CD CD CD
 *- Destination port(34) :                     88 BC
 *- Source port(36) :                          88 BC
 *- Annanciation Tag(42) :                     CD
 *- Priority(43) :                             CD
 *-----------------------------------------------------------------------*/
static uint8 gCSAnn[CSANN_LEN] = {
	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 
	0xCD, 0xCD, 0xCD, 0xCD, 0xCD, 0xCD,
	0x08, 0x00,
	0x45, 0x00, 0x00, 0x4A, 0x04, 0x57, 0x00, 0x00, 0x20, 0x11,	0xED, 0x2F, 
	0xCD, 0xCD, 0xCD, 0xCD, 
	0xCD, 0xCD, 0xCD, 0xCD,
	0x88, 0xBC, 0x88, 0xBC, 0x00, 0x1A, 0x00, 0x00,
	0xCD,
	0xCD,
	0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
	0x20, 0x20, 0x20, 0x20
};

__inline PFoo GetNode(void) {
	CpuSr sr;
	uint8 err;
	PFoo  pnode;

	SpinLock(sr);
	OSSemPend(gpNodeSem, 0, &err);
	pnode = (PFoo)OSMemGet(gpNodeMCB, &err);
	SpinUnLock(sr);
	return (pnode);
}

__inline void PutNode(PFoo pnode) {
	CpuSr sr;
	
	SpinLock(sr);
	OSSemPost(gpNodeSem);
	OSMemPut(gpNodeMCB, (void*)pnode);
	SpinUnLock(sr);
}

void CSInit(void) {
	uint8 err;

	gAtcPrdOff = 0;
	gNonPrdOff = 0;
	gMacroCyc = 0;
	LIST_INIT(gPrdList);
	LIST_INIT(gNonPrdList);
	LIST_INIT(gGlbNonPrdList);
	gpCSendMsgQ = OSQCreate(CSendMsgQTbl, MSG_AMOUNT_CSEND);
	OSTaskCreate(CSendTask, (void*)0, (void*)(&CSendStack[STACK_SIZE_CSEND]), TASK_PRIORITY_CSEND);
	gpNodeSem = OSSemCreate(NODE_AMOUNT_ALL);
	gpNodeMCB = OSMemCreate(gFooPool, NODE_AMOUNT_ALL, sizeof(Foo), &err);
}

void CSStart(PNetIF pni) {
	
	CSTimerStart();
	pni->output = CSend;
}

void CSStop(void) {
	CSTimerStop();
}

uint8 CSend(PSock psock) {
	CpuSr  sr;
	PNetIF pni;
	uint8  srvid, protocol;
	uint16 frmtype, dstport;
	uint32 tag;

	pni = psock->pni;
	n2h16(psock->payload + 12, &frmtype);
	if(frmtype == IP_TYPE){
		protocol = psock->payload[23];
		n2h16(psock->payload + 34, &dstport);
		if(protocol == UDP_PROTOCOL && dstport == EPA_PORT) {
			srvid = psock->payload[42];
			if(srvid == EVS_DISTRIBUTE) {
				tag = (0 | (pni->ip & ~pni->mask));
				Push(&gPrdList, &(psock->node), tag);
				return EPA_NO_ERR;
			}
			else if(srvid & EPA_SERVICEID_MASK <=  EES_ALTER_MONITOR &&
					srvid & EPA_SERVICEID_MASK >= EES_ACK_NOTI) {
					tag = ((1 << 24) | (pni->ip & ~pni->mask));
			}
			else {
				tag = ((2 << 24) | (pni->ip & ~pni->mask));
			}
		}
		else if(protocol == UDP_PROTOCOL 
			&& (dstport == PTP_EVENT_PORT || dstport == PTP_GERNERAL_PORT)) {
			tag = ((3 << 24) | (pni->ip & ~pni->mask));;
		}
		else {
			tag = ((4 << 24) | (pni->ip & ~pni->mask));;
		}
	}
	else {
		tag = ((1 << 24) | (pni->ip & ~pni->mask));;
	}		
	SpinLock(sr);
	InsertByTag(&gNonPrdList, &(psock->node), tag);
	SpinUnLock(sr);
	return SYS_NO_ERR;
}

void CSAnnInput(PSock psock, uint8 com_type, uint16 msg_id) {
	PFoo pnode;

	if(psock->payload[1] != 0xFF) {
		pnode = GetNode();
		InsertByTag(&gGlbNonPrdList, pnode, ((uint32)(psock->payload[1]) << 24) | (psock->src_ip & ~psock->pni->mask));
	}
	PutSock(psock);
}

void CSEndAnnInput(PSock psock, uint8 com_type, uint16 msg_id) {
	uint32 idx;
	PFoo   pnode;

	if(gCStatus == CS_NONPRDTIME) {
		if(gNonPrdList.tag > 0) {
			pnode = LIST_HEAD(gGlbNonPrdList);
			if(((pnode->tag) & ~psock->pni->mask) == (psock->src_ip & ~psock->pni->mask)) {
				PutNode(Pop(&gGlbNonPrdList));
			}
			else {
				idx = 0;
				do {
					if((pnode->tag & ~psock->pni->mask) == (psock->src_ip & ~psock->pni->mask)) {
						PutNode(DeleteAt(&gGlbNonPrdList, idx));
						break;
					}
					pnode = pnode->next;
					idx += 1;
				} while(idx < gGlbNonPrdList.tag);
			}
			if(psock->payload[1] != 0xFF) {
				pnode = GetNode();
				InsertByTag(&gGlbNonPrdList, pnode, ((uint32)(psock->payload[1]) << 24) | (psock->src_ip & ~psock->pni->mask));
			}
			if(gAnnedNPkt != CS_NO_NPKT) {
				while(gNonPrdList.tag > 0 && (gGlbNonPrdList.tag == 0 || LIST_HEAD(gNonPrdList)->tag < LIST_HEAD(gGlbNonPrdList)->tag)) {
					pnode = Pop(&gNonPrdList);
					LLOutput(LIST_ENTRY(pnode, Sock, node));
					PutNode(pnode);
				}
				if(gNonPrdList.tag > 0) {
					CSEndAnnOutput((uint8)(LIST_HEAD(gNonPrdList)->tag >> 24), psock->pni);
				}
				else {
					CSEndAnnOutput(0xFF, psock->pni);
				}
			}
		}
	}
	PutSock(psock);
}

void CSAnnOutput(uint8 prio, PNetIF pni) {
	PSock psock;

	psock = GetSock(PROTOCOL_ETHER, CSANN_LEN, 0, BCInetAddr(pni), pni);
	memcpy(psock->payload, gCSAnn, 60);
	memcpy(psock->payload + 6, pni->mac, 6);
	h2n32(pni->ip, psock->payload + 26);
	h2n32(BCInetAddr(pni), psock->payload + 30);
	psock->payload[42] = NPMA_TAG;
	psock->payload[43] = prio;
	psock->blank = 0;
	LLOutput(psock);
}

void CSEndAnnOutput(uint8 prio, PNetIF pni) {
	PSock psock;

	psock = GetSock(PROTOCOL_ETHER, CSANN_LEN, 0, BCInetAddr(pni), pni);
	memcpy(psock->payload, gCSAnn, 60);
	memcpy(psock->payload + 6, pni->mac, 6);
	h2n32(pni->ip, psock->payload + 26);
	h2n32(BCInetAddr(pni), psock->payload + 30);
	psock->payload[42] = ENPMA_TAG;
	psock->payload[43] = prio;
	psock->blank = 0;
	LLOutput(psock);
}

static void CSendTask(void* pdata) {
	uint8  err;
	PNetIF pni;
	PFoo   pnode;
	
	while(1) {
		pni = (PNetIF)OSQPend(gpCSendMsgQ, TASK_TIMEOUT_CSEND, &err);
		if(gCStatus & CS_CYCSTART) {
			while(gGlbNonPrdList.tag > 0) {
				PutNode(Pop(&gGlbNonPrdList));
			}
		}
		if(gCStatus & CS_ACTIVETIME) {
			while(gPrdList.tag) {
				pnode = Pop(&gPrdList);
				pni->output(LIST_ENTRY(pnode, Sock, node));
			}
			if(gNonPrdList.tag > 0) {
				CSAnnOutput((uint8)(LIST_HEAD(gNonPrdList)->tag >> 24), pni);
				gAnnedNPkt = gNonPrdList.tag;
			}
			else {
				CSAnnOutput(0xFF, pni);
				gAnnedNPkt = CS_NO_NPKT;
			}
		}
		else if((gCStatus == CS_NONPRDTIME) && (gNonPrdList.tag > 0) && (gAnnedNPkt != CS_NO_NPKT)) {
			while(gNonPrdList.tag > 0 && (gGlbNonPrdList.tag == 0 || LIST_HEAD(gNonPrdList)->tag < LIST_HEAD(gGlbNonPrdList)->tag)) {
				pnode = Pop(&gNonPrdList);
				LLOutput(LIST_ENTRY(pnode, Sock, node));
			}
			if(gNonPrdList.tag > 0) {
				CSEndAnnOutput((uint8)(LIST_HEAD(gNonPrdList)->tag >> 24), pni);
			}
			else {
				CSEndAnnOutput(0xFF, pni);
			}
		}
	}	// while(1)
}