udp.c

opentcp_mcf5282原代码

/***************************************************************************************
File:			udp.c

Date:			15.7.2002

Version:		0.1

Author:			Jari Lahti ([e-mail])

Description:	This file contains functions for processing UDP

Version Info:	15.7.2002 - First version (JaL)
***************************************************************************************/


#include "opentcp.h"



struct ucb UDPSocket[NO_OF_UDPSOCKETS];
struct UDPFrame ReceivedUDPPacket;

/********************************************************************************
Function:		udp_init

Parameters:		void

Return val:		INT8 - (-1) Not OK
						>0 number of UDP sockets initialized

Date:			26.7.2002

Desc:			Initializes UDP stack and socket pool. Called once when processor
				starts!!
*********************************************************************************/

INT8 udp_init (void)
{
	UINT8 i;
	struct ucb* soc;

	if( NO_OF_UDPSOCKETS < 0 )
		return(-1);

	if( NO_OF_UDPSOCKETS == 0 )
		return(0);

	DEBUGOUT("Initializing UDP");

	for(i=0; i < NO_OF_UDPSOCKETS; i++)
	{
		soc = &UDPSocket[i];			/* Get Socket	*/

		soc->state = UDP_STATE_FREE;
		soc->tos = 0;
		soc->locport = 0;
		soc->opts = UDP_OPT_SEND_CS | UDP_OPT_CHECK_CS;
		soc->event_listener = 0;

		DEBUGOUT(".");

	}

	DEBUGOUT("\n\rUDP Initialized\n\r");

	/* Return number of sockets initialized	*/

	return(i+1);

}


/********************************************************************************
Function:		udp_getsocket

Parameters:		UINT8 tos	- type of service for socket
				INT32 (*listener)(INT8, UINT8, UINT32, UINT16, UINT16, UINT16) - listening function
				UINT8 opts - options for checksum generation & inspection

Return val:		INT8 - (-1) = Error
					 - >0 = handle to reserved socket

Date:			26.7.2002

Desc:			Try to get free socket from socket pool and return handle to it
*********************************************************************************/

INT8 udp_getsocket (UINT8 tos, INT32 (*listener)(INT8, UINT8, UINT32, UINT16, UINT16, UINT16), UINT8 opts )
{
	INT8 i;
	struct ucb* soc;

	if( NO_OF_UDPSOCKETS < 0 )
		return(-1);

	if( NO_OF_UDPSOCKETS == 0 )
		return(-1);


	if(listener == 0)
	{
		DEBUGOUT("ERROR:Event listener function for UDP not specified\r\n");
		return(-1);
	}

	DEBUGOUT("Searching for free UDP socket...\r\n");

	for(i=0; i < NO_OF_UDPSOCKETS; i++)
	{
		soc = &UDPSocket[i];			/* Get Socket	*/

		if(soc->state == UDP_STATE_FREE)
		{
			/* We found it	*/

			DEBUGOUT("Free socket found\r\n");

			soc->state = UDP_STATE_CLOSED;
			soc->tos = tos;
			soc->locport = 0;

			soc->opts = 0;

			if(opts & UDP_OPT_SEND_CS)
				soc->opts |= UDP_OPT_SEND_CS;
			if(opts & UDP_OPT_CHECK_CS)
				soc->opts |= UDP_OPT_CHECK_CS;

			soc->event_listener = listener;

			/* Return handle	*/

			return(i);
		}

	}

	/* We are there so no socket found	*/

	DEBUGOUT("No UDP socket found\r\n");
	return(-1);

}

/********************************************************************************
Function:		udp_releasesocket

Parameters:		INT8 sochandle - handle to socket to be released

Return val:		INT8 - (-1) = Error
					 - >0 = handle to released socket

Date:			26.7.2002

Desc:			Try to release given UDP socket
*********************************************************************************/

INT8 udp_releasesocket (INT8 sochandle)
{
	struct ucb* soc;

	if( NO_OF_UDPSOCKETS < 0 )
		return(-1);

	if( NO_OF_UDPSOCKETS == 0 )
		return(-1);

	if( sochandle > NO_OF_UDPSOCKETS )
	{
		DEBUGOUT("Socket handle non-valid\r\n");
		return(-1);
	}

	if( sochandle < 0 )
	{
		DEBUGOUT("Socket handle non-valid\r\n");
		return(-1);
	}

	soc = &UDPSocket[sochandle];		/* Get referense	*/

	soc->state = UDP_STATE_FREE;
	soc->tos = 0;
	soc->locport = 0;
	soc->opts = UDP_OPT_SEND_CS | UDP_OPT_CHECK_CS;
	soc->event_listener = 0;

	return(sochandle);

}

/********************************************************************************
Function:		udp_open

Parameters:		INT8 sochandle - handle to socket to be opened
				UINT16 locport - local port number

Return val:		INT8 - (-1) = Error
					 - >0 = handle to opened socket

Date:			26.7.2002

Desc:			Bind local port to given UDP socket and open the socket
				(virtually) in order to enable communication
*********************************************************************************/

INT8 udp_open (INT8 sochandle, UINT16 locport)
{
	struct ucb* soc;

	if( NO_OF_UDPSOCKETS < 0 )
		return(-1);

	if( NO_OF_UDPSOCKETS == 0 )
		return(-1);

	if( sochandle > NO_OF_UDPSOCKETS )
	{
		DEBUGOUT("Socket handle non-valid\r\n");
		return(-1);
	}

	if( sochandle < 0 )
	{
		DEBUGOUT("Socket handle non-valid\r\n");
		return(-1);
	}

	if(locport == 0)
	{
		locport = udp_getfreeport();
	}

	if(locport == 0)
	{
		DEBUGOUT("Local port 0 not allowed\r\n");
		return(-1);
	}

	soc = &UDPSocket[sochandle];		/* Get referense	*/

	soc->state = UDP_STATE_OPENED;
	soc->locport = locport;

	return(sochandle);

}


/********************************************************************************
Function:		udp_close

Parameters:		INT8 sochandle - handle to socket to be closed

Return val:		INT8 - (-1) = Error
					 - >0 = handle to closed socket

Date:			26.7.2002

Desc:			Close given socket to disable communication
*********************************************************************************/

INT8 udp_close (INT8 sochandle)
{
	struct ucb* soc;

	if( NO_OF_UDPSOCKETS < 0 )
		return(-1);

	if( NO_OF_UDPSOCKETS == 0 )
		return(-1);

	if( sochandle > NO_OF_UDPSOCKETS )
	{
		DEBUGOUT("Socket handle non-valid\r\n");
		return(-1);
	}

	if( sochandle < 0 )
	{
		DEBUGOUT("Socket handle non-valid\r\n");
		return(-1);
	}

	soc = &UDPSocket[sochandle];		/* Get referense	*/

	soc->state = UDP_STATE_CLOSED;

	return(sochandle);

}

/********************************************************************************
Function:		udp_send

Parameters:		INT8 sochandle - handle to socket used
				UINT32 remip - remote ip address
				UINT16 remport - remote port number
				UINT8* buf - data buffer (user data)
				UINT16 blen - buffer len in bytes (user buffer)
				UINT16 dlen - data lengt in bytes (user data)

Return val:		INT16 - (-1) = Error (General error, e.g. parameters)
						(-2) = ARP or lower layer not ready, try again
						(-3) = Socket closed or invalid local port
					 - >0 =  Number of bytes sent

Date:			26.7.2002

Desc:			Bind local port to given UDP socket and open the socket
				(virtually)
*********************************************************************************/

INT16 udp_send (INT8 sochandle, UINT32 remip, UINT16 remport, UINT8* buf, UINT16 blen, UINT16 dlen)
{
	struct ucb* soc;
	UINT8* user_buf_start;
	UINT16 cs;
	UINT8 cs_cnt;
	INT16 i;

	if( NO_OF_UDPSOCKETS < 0 )
		return(-1);

	if( NO_OF_UDPSOCKETS == 0 )
		return(-1);

	if( sochandle > NO_OF_UDPSOCKETS )
	{
		DEBUGOUT("Socket handle non-valid\r\n");
		return(-1);
	}

	if( sochandle < 0 )
	{
		DEBUGOUT("Socket handle non-valid\r\n");
		return(-1);
	}

	if(remip == 0)
	{
		DEBUGOUT("Remote IP 0 not allowed\r\n");
		return(-1);
	}

	if(remport == 0)
	{
		DEBUGOUT("Remote port 0 not allowed\r\n");
		return(-1);
	}

	if( dlen > blen )
		dlen = blen;

	if( (dlen + UDP_HLEN) > UDP_SEND_MTU)
		dlen = UDP_SEND_MTU - UDP_HLEN;

	soc = &UDPSocket[sochandle];		/* Get referense	*/

	if(soc->state != UDP_STATE_OPENED )
	{
		DEBUGOUT("UDP Socket Closed\r\n");
		return(-3);
	}

	if(soc->locport == 0)
	{
		DEBUGOUT("ERROR:Socket local port is zero\r\n");
		return(-1);
	}

	user_buf_start = buf;

	buf -= UDP_HLEN;

	/* Put header	*/

	*buf++ = (UINT8)(soc->locport >> 8);
	*buf++ = (UINT8)soc->locport;
	*buf++ = (UINT8)(remport >> 8);
	*buf++ = (UINT8)remport;
	*buf++ = (UINT8)((dlen + UDP_HLEN) >> 8);
	*buf++ = (UINT8)(dlen + UDP_HLEN);
	*buf++ = 0;
	*buf = 0;

	buf = user_buf_start;
	buf -= UDP_HLEN;

	/* Calculate checksum if needed	*/

	cs = 0;

	if( soc->opts & UDP_OPT_SEND_CS)
	{
		cs = 0;
		cs_cnt = 0;

		/* Do it firstly to IP pseudo header	*/

		cs = IpCheckSum(cs, (UINT8)(localmachine.localip >> 24), cs_cnt++);
		cs = IpCheckSum(cs, (UINT8)(localmachine.localip >> 16), cs_cnt++);
		cs = IpCheckSum(cs, (UINT8)(localmachine.localip >> 8), cs_cnt++);
		cs = IpCheckSum(cs, (UINT8)localmachine.localip, cs_cnt++);

		cs = IpCheckSum(cs, (UINT8)(remip >> 24), cs_cnt++);
		cs = IpCheckSum(cs, (UINT8)(remip >> 16), cs_cnt++);
		cs = IpCheckSum(cs, (UINT8)(remip >> 8), cs_cnt++);
		cs = IpCheckSum(cs, (UINT8)remip, cs_cnt++);

		cs = IpCheckSum(cs, 0, cs_cnt++);

		cs = IpCheckSum(cs, (UINT8)IP_UDP, cs_cnt++);

		cs = IpCheckSum(cs, (UINT8)((dlen + UDP_HLEN) >> 8), cs_cnt++);
		cs = IpCheckSum(cs, (UINT8)(dlen + UDP_HLEN), cs_cnt++);

		/* Go to UDP header + data	*/

		buf = user_buf_start;
		buf -= UDP_HLEN;

	//	for(i=0; i < (dlen + UDP_HLEN); i++)
	//		cs = IpCheckSum(cs, *buf++, cs_cnt++);

		cs = IpCheckSumBuf(cs, buf, dlen + UDP_HLEN);

		cs = ~ cs;

		if(cs == 0)
			cs = 0xFFFF;

		/* Save checksum in correct place	*/
		buf = user_buf_start;
		buf -= UDP_HLEN;
		buf += 6;
		*buf++ = (UINT8)(cs >> 8);
		*buf = (UINT8)cs;

		buf = user_buf_start;
		buf -= UDP_HLEN;

	}

	/* Send it to IP	*/

	DEBUGOUT("Sending UDP...\r\n");

	i = ProcessIPOut(remip, IP_UDP, soc->tos, 100, buf, dlen + UDP_HLEN);

	/* Errors?	*/

	if( i < 0 )
		return(i);

	DEBUGOUT("UDP packet sent\r\n");

	return(i - UDP_HLEN);


}



/********************************************************************************
Function:		ProcessUDPIn

Parameters:		struct IPFrame* frame - pointer to received IP frame structure
				UINT16 len - legth of data in bytes

Return val:		INT16 - (-1) Not OK
						>0 Packet OK


Date:			15.7.2002

Desc:			Check and process received UDP frame and call event_listener
				of UDP socket
*********************************************************************************/

INT16 ProcessUDPIn (struct IPFrame* frame, UINT16 len)
{
	struct ucb* soc;
	UINT16 checksum;
	UINT16 i;
	INT8 sochandle;

	/* Is this UDP?	*/

	DEBUGOUT("Processing UDP...\n\r");

	if( frame->protocol != IP_UDP )
	{
		DEBUGOUT("ERROR: The protocol is not UDP\n\r");
		return(-1);
	}

	/* Start processing the message	*/

	NETWORK_RECEIVE_INITIALIZE(frame->BufIndex);

	ReceivedUDPPacket.sport = RECEIVE_NETWORK_B();
	ReceivedUDPPacket.sport <<= 8;
	ReceivedUDPPacket.sport |= RECEIVE_NETWORK_B();

	ReceivedUDPPacket.dport = RECEIVE_NETWORK_B();
	ReceivedUDPPacket.dport <<= 8;
	ReceivedUDPPacket.dport |= RECEIVE_NETWORK_B();

	ReceivedUDPPacket.tlen = RECEIVE_NETWORK_B();
	ReceivedUDPPacket.tlen <<= 8;
	ReceivedUDPPacket.tlen |= RECEIVE_NETWORK_B();

	ReceivedUDPPacket.checksum = RECEIVE_NETWORK_B();
	ReceivedUDPPacket.checksum <<= 8;
	ReceivedUDPPacket.checksum |= RECEIVE_NETWORK_B();

	if(ReceivedUDPPacket.tlen < UDP_HLEN )
	{
		DEBUGOUT("UDP frame too short\n\r");
		return(-1);
	}


	/* Map UDP socket	*/

	sochandle = -1;

	for( i=0; i < NO_OF_UDPSOCKETS; i++)
	{
		soc = &UDPSocket[i];				/* Get referense	*/

		if(soc->state != UDP_STATE_OPENED )
			continue;

		if(soc->locport != ReceivedUDPPacket.dport)
			continue;

		/* Socket found	*/

		sochandle = i;

		break;
	}

	if( sochandle < 0 )
	{
		DEBUGOUT("No socket found for received UDP Packet\r\n");

		/* TODO: Send ICMP	*/

		return(-1);
	}

	/* Calculate checksum for received packet	*/

	if(soc->opts & UDP_OPT_CHECK_CS)
	{
		if(ReceivedUDPPacket.checksum != 0)
		{
			checksum = 0;
			i = 0;

			/* Do it firstly to IP pseudo header	*/

			checksum = IpCheckSum(checksum, (UINT8)(frame->dip >> 24), (UINT8)i++);
			checksum = IpCheckSum(checksum, (UINT8)(frame->dip >> 16), (UINT8)i++);
			checksum = IpCheckSum(checksum, (UINT8)(frame->dip >> 8), (UINT8)i++);
			checksum = IpCheckSum(checksum, (UINT8)frame->dip, (UINT8)i++);

			checksum = IpCheckSum(checksum, (UINT8)(frame->sip >> 24), (UINT8)i++);
			checksum = IpCheckSum(checksum, (UINT8)(frame->sip >> 16), (UINT8)i++);
			checksum = IpCheckSum(checksum, (UINT8)(frame->sip >> 8), (UINT8)i++);
			checksum = IpCheckSum(checksum, (UINT8)frame->sip, (UINT8)i++);

			checksum = IpCheckSum(checksum, 0, (UINT8)i++);

			checksum = IpCheckSum(checksum, (UINT8)IP_UDP, (UINT8)i++);

			checksum = IpCheckSum(checksum, (UINT8)(len >> 8), (UINT8)i++);
			checksum = IpCheckSum(checksum, (UINT8)len, (UINT8)i++);


			NETWORK_RECEIVE_INITIALIZE(frame->BufIndex);

			for(i=0; i < len; i++)
				checksum = IpCheckSum(checksum, RECEIVE_NETWORK_B(), (UINT8)i);

			checksum = ~ checksum;

			if(checksum != IP_GOOD_CS)
			{
				DEBUGOUT("ERROR: UDP Checksum failed!\n\r");
				return (-1);
			}

		}

		DEBUGOUT("UDP Checksum OK\n\r");

	}


	ReceivedUDPPacket.BufIndex = frame->BufIndex + UDP_HLEN;
	NETWORK_RECEIVE_INITIALIZE(ReceivedUDPPacket.BufIndex);

	/* Generate data event	*/

	soc->event_listener(sochandle, UDP_EVENT_DATA, frame->sip, ReceivedUDPPacket.sport, ReceivedUDPPacket.BufIndex, ReceivedUDPPacket.tlen - UDP_HLEN);

	return(1);

}


/********************************************************************************
Function:		udp_getfreeport

Parameters:		void


Return val:		UINT16 - 0 = no free ports
						 >0 = Free local UDP port

Date:			19.10.2002

Desc:			Gets next free (not used) local port
*********************************************************************************/

UINT16 udp_getfreeport (void)
{
	struct ucb* soc;
	static UINT16 lastport = 1;
	UINT16 start;
	UINT16 i;


	/* Try with every port to every socket untill free found	*/

	for( start = lastport++; start != lastport; lastport++)
	{
		if(lastport == UDP_PORTS_END)
			lastport = 1;

		for(i = 0; i < NO_OF_UDPSOCKETS; i++)
		{
			soc = &UDPSocket[i];					/* Get socket	*/

			if( (soc->state > UDP_STATE_CLOSED) && (soc->locport == lastport) )
			{
				/* Blaah, this socket has reserved the port, go to next one	*/
				break;
			}

		}

		/* Did we found it?	*/

		if( i == NO_OF_UDPSOCKETS)
			break;

	}

	if(lastport == start)
	{
		DEBUGOUT("Out of UDP ports!!\n\r");
		return(0);
	}

	return(lastport);

}



/* EOF	*/