X-Git-Url: https://git.ucc.asn.au/?a=blobdiff_plain;f=KernelLand%2FModules%2FIPStack%2Fudp.c;h=325a09e86c6d5a3ad82a241583d3b90a82ddd438;hb=6a0792d4e8f886c590e008f8c3fea32ed7382410;hp=cbdcd56be61a319b2ba270bd38f20d0437a056d5;hpb=51ab5f489bc356940c95cc936fd0508e8f07ea97;p=tpg%2Facess2.git

diff --git a/KernelLand/Modules/IPStack/udp.c b/KernelLand/Modules/IPStack/udp.c
index cbdcd56b..325a09e8 100644
--- a/KernelLand/Modules/IPStack/udp.c
+++ b/KernelLand/Modules/IPStack/udp.c
@@ -1,7 +1,11 @@
 /*
  * Acess2 IP Stack
- * - UDP Handling
+ * - By John Hodge (thePowersGang)
+ *
+ * udp.c
+ * - UDP Protocol handling
  */
+#define DEBUG	1
 #include "ipstack.h"
 #include <api_drv_common.h>
 #include "udp.h"
@@ -15,14 +19,17 @@ void	UDP_Unreachable(tInterface *Interface, int Code, void *Address, int Length,
 void	UDP_SendPacketTo(tUDPChannel *Channel, int AddrType, const void *Address, Uint16 Port, const void *Data, size_t Length);
 // --- Client Channels
 tVFS_Node	*UDP_Channel_Init(tInterface *Interface);
-Uint64	UDP_Channel_Read(tVFS_Node *Node, Uint64 Offset, Uint64 Length, void *Buffer);
-Uint64	UDP_Channel_Write(tVFS_Node *Node, Uint64 Offset, Uint64 Length, const void *Buffer);
+size_t	UDP_Channel_Read(tVFS_Node *Node, off_t Offset, size_t Length, void *Buffer, Uint Flags);
+size_t	UDP_Channel_Write(tVFS_Node *Node, off_t Offset, size_t Length, const void *Buffer, Uint Flags);
  int	UDP_Channel_IOCtl(tVFS_Node *Node, int ID, void *Data);
 void	UDP_Channel_Close(tVFS_Node *Node);
 // --- Helpers
-Uint16	UDP_int_AllocatePort();
- int	UDP_int_MarkPortAsUsed(Uint16 Port);
+Uint16	UDP_int_AllocatePort(tUDPChannel *Channel);
+ int	UDP_int_ClaimPort(tUDPChannel *Channel, Uint16 Port);
 void	UDP_int_FreePort(Uint16 Port);
+Uint16	UDP_int_MakeChecksum(tInterface *Iface, const void *Dest, tUDPHeader *Hdr, size_t Len, const void *Data); 
+Uint16	UDP_int_PartialChecksum(Uint16 Prev, size_t Len, const void *Data);
+Uint16	UDP_int_FinaliseChecksum(Uint16 Value);
 
 // === GLOBALS ===
 tVFS_NodeType	gUDP_NodeType = {
@@ -31,7 +38,7 @@ tVFS_NodeType	gUDP_NodeType = {
 	.IOCtl = UDP_Channel_IOCtl,
 	.Close = UDP_Channel_Close
 };
-tMutex	glUDP_Channels;
+tMutex	glUDP_Channels;	// TODO: Replace with a RWLock
 tUDPChannel	*gpUDP_Channels;
 
 tMutex	glUDP_Ports;
@@ -62,9 +69,7 @@ int UDP_int_ScanList(tUDPChannel *List, tInterface *Interface, void *Address, in
 	tUDPPacket	*pack;
 	 int	len;
 	
-	for(chan = List;
-		chan;
-		chan = chan->Next)
+	for(chan = List; chan; chan = chan->Next)
 	{
 		// Match local endpoint
 		if(chan->Interface && chan->Interface != Interface)	continue;
@@ -117,10 +122,16 @@ void UDP_GetPacket(tInterface *Interface, void *Address, int Length, void *Buffe
 {
 	tUDPHeader	*hdr = Buffer;
 	
-	Log_Debug("UDP", "hdr->SourcePort = %i", ntohs(hdr->SourcePort));
-	Log_Debug("UDP", "hdr->DestPort = %i", ntohs(hdr->DestPort));
-	Log_Debug("UDP", "hdr->Length = %i", ntohs(hdr->Length));
-	Log_Debug("UDP", "hdr->Checksum = 0x%x", ntohs(hdr->Checksum));
+	#if 1
+	size_t len = strlen( IPStack_PrintAddress(Interface->Type, Address) );
+	char	tmp[len+1];
+	strcpy(tmp, IPStack_PrintAddress(Interface->Type, Address));
+	Log_Debug("UDP", "%i bytes %s:%i -> %s:%i (Cksum 0x%04x)",
+		ntohs(hdr->Length),
+		tmp, ntohs(hdr->SourcePort),
+		IPStack_PrintAddress(Interface->Type, Interface->Address), ntohs(hdr->DestPort),
+		ntohs(hdr->Checksum));
+	#endif
 	
 	// Check registered connections
 	Mutex_Acquire(&glUDP_Channels);
@@ -144,25 +155,30 @@ void UDP_Unreachable(tInterface *Interface, int Code, void *Address, int Length,
  */
 void UDP_SendPacketTo(tUDPChannel *Channel, int AddrType, const void *Address, Uint16 Port, const void *Data, size_t Length)
 {
-	tUDPHeader	*hdr;
+	tUDPHeader	hdr;
 
 	if(Channel->Interface && Channel->Interface->Type != AddrType)	return ;
 	
+	// Create the packet
+	hdr.SourcePort = htons( Channel->LocalPort );
+	hdr.DestPort = htons( Port );
+	hdr.Length = htons( sizeof(tUDPHeader) + Length );
+	hdr.Checksum = 0;
+	hdr.Checksum = htons( UDP_int_MakeChecksum(Channel->Interface, Address, &hdr, Length, Data) );
+	
+	tIPStackBuffer	*buffer;
 	switch(AddrType)
 	{
 	case 4:
-		// Create the packet
-		hdr = malloc(sizeof(tUDPHeader)+Length);
-		hdr->SourcePort = htons( Channel->LocalPort );
-		hdr->DestPort = htons( Port );
-		hdr->Length = htons( sizeof(tUDPHeader) + Length );
-		hdr->Checksum = 0;	// Checksum can be zero on IPv4
-		memcpy(hdr->Data, Data, Length);
 		// Pass on the the IPv4 Layer
+		buffer = IPStack_Buffer_CreateBuffer(2 + IPV4_BUFFERS);
+		IPStack_Buffer_AppendSubBuffer(buffer, Length, 0, Data, NULL, NULL);
+		IPStack_Buffer_AppendSubBuffer(buffer, sizeof(hdr), 0, &hdr, NULL, NULL);
 		// TODO: What if Channel->Interface is NULL here?
-		IPv4_SendPacket(Channel->Interface, *(tIPv4*)Address, IP4PROT_UDP, 0, sizeof(tUDPHeader)+Length, hdr);
-		// Free allocated packet
-		free(hdr);
+		IPv4_SendPacket(Channel->Interface, *(tIPv4*)Address, IP4PROT_UDP, 0, buffer);
+		break;
+	default:
+		Log_Warning("UDP", "TODO: Implement on proto %i", AddrType);
 		break;
 	}
 }
@@ -189,7 +205,7 @@ tVFS_Node *UDP_Channel_Init(tInterface *Interface)
 /**
  * \brief Read from the channel file (wait for a packet)
  */
-Uint64 UDP_Channel_Read(tVFS_Node *Node, Uint64 Offset, Uint64 Length, void *Buffer)
+size_t UDP_Channel_Read(tVFS_Node *Node, off_t Offset, size_t Length, void *Buffer, Uint Flags)
 {
 	tUDPChannel	*chan = Node->ImplPtr;
 	tUDPPacket	*pack;
@@ -205,7 +221,11 @@ Uint64 UDP_Channel_Read(tVFS_Node *Node, Uint64 Offset, Uint64 Length, void *Buf
 	
 	for(;;)
 	{
-		VFS_SelectNode(Node, VFS_SELECT_READ, NULL, "UDP_Channel_Read");
+		tTime	timeout_z = 0, *timeout = (Flags & VFS_IOFLAG_NOBLOCK) ? &timeout_z : NULL;
+		int rv = VFS_SelectNode(Node, VFS_SELECT_READ, timeout, "UDP_Channel_Read");
+		if( rv ) {
+			errno = (Flags & VFS_IOFLAG_NOBLOCK) ? EWOULDBLOCK : EINTR;
+		}
 		SHORTLOCK(&chan->lQueue);
 		if(chan->Queue == NULL) {
 			SHORTREL(&chan->lQueue);
@@ -249,13 +269,17 @@ Uint64 UDP_Channel_Read(tVFS_Node *Node, Uint64 Offset, Uint64 Length, void *Buf
 /**
  * \brief Write to the channel file (send a packet)
  */
-Uint64 UDP_Channel_Write(tVFS_Node *Node, Uint64 Offset, Uint64 Length, const void *Buffer)
+size_t UDP_Channel_Write(tVFS_Node *Node, off_t Offset, size_t Length, const void *Buffer, Uint Flags)
 {
 	tUDPChannel	*chan = Node->ImplPtr;
 	const tUDPEndpoint	*ep;
 	const void	*data;
 	 int	ofs;
-	if(chan->LocalPort == 0)	return 0;
+	
+	if(chan->LocalPort == 0) {
+		Log_Notice("UDP", "Write to channel %p with zero local port", chan);
+		return 0;
+	}
 	
 	ep = Buffer;	
 	ofs = 2 + 2 + IPStack_GetAddressSize( ep->AddrType );
@@ -264,7 +288,7 @@ Uint64 UDP_Channel_Write(tVFS_Node *Node, Uint64 Offset, Uint64 Length, const vo
 
 	UDP_SendPacketTo(chan, ep->AddrType, &ep->Addr, ep->Port, data, (size_t)Length - ofs);
 	
-	return 0;
+	return Length;
 }
 
 /**
@@ -289,36 +313,31 @@ int UDP_Channel_IOCtl(tVFS_Node *Node, int ID, void *Data)
 	{
 	BASE_IOCTLS(DRV_TYPE_MISC, "UDP Channel", 0x100, casIOCtls_Channel);
 	
-	case 4:	// getset_localport (returns bool success)
+	case 4:	{ // getset_localport (returns bool success)
 		if(!Data)	LEAVE_RET('i', chan->LocalPort);
 		if(!CheckMem( Data, sizeof(Uint16) ) ) {
 			LOG("Invalid pointer %p", Data);
 			LEAVE_RET('i', -1);
 		}
 		// Set port
-		chan->LocalPort = *(Uint16*)Data;
+		int req_port = *(Uint16*)Data;
 		// Permissions check (Ports lower than 1024 are root-only)
-		if(chan->LocalPort != 0 && chan->LocalPort < 1024) {
+		if(req_port != 0 && req_port < 1024) {
 			if( Threads_GetUID() != 0 ) {
-				LOG("Attempt by non-superuser to listen on port %i", chan->LocalPort);
-				chan->LocalPort = 0;
+				LOG("Attempt by non-superuser to listen on port %i", req_port);
 				LEAVE_RET('i', -1);
 			}
 		}
 		// Allocate a random port if requested
-		if( chan->LocalPort == 0 )
-			chan->LocalPort = UDP_int_AllocatePort();
-		else
-		{
-			// Else, mark the requested port as used
-			if( UDP_int_MarkPortAsUsed(chan->LocalPort) == 0 ) {
-				LOG("Port %i us currently in use", chan->LocalPort);
-				chan->LocalPort = 0;
-				LEAVE_RET('i', 0);
-			}
-			LEAVE_RET('i', 1);
+		if( req_port == 0 )
+			UDP_int_AllocatePort(chan);
+		// Else, mark the requested port as used
+		else if( UDP_int_ClaimPort(chan, req_port) ) {
+			LOG("Port %i is currently in use", req_port);
+			LEAVE_RET('i', 0);
+		}
+		LEAVE_RET('i', chan->LocalPort);
 		}
-		LEAVE_RET('i', 1);
 	
 	case 5:	// getset_remoteport (returns bool success)
 		if(!Data)	LEAVE_RET('i', chan->Remote.Port);
@@ -327,7 +346,8 @@ int UDP_Channel_IOCtl(tVFS_Node *Node, int ID, void *Data)
 			LEAVE_RET('i', -1);
 		}
 		chan->Remote.Port = *(Uint16*)Data;
-		return 1;
+		LEAVE('i', chan->Remote.Port);
+		return chan->Remote.Port;
 	
 	case 6:	// getset_remotemask (returns bool success)
 		if(!Data)	LEAVE_RET('i', chan->RemoteMask);
@@ -339,7 +359,8 @@ int UDP_Channel_IOCtl(tVFS_Node *Node, int ID, void *Data)
 		if( *(int*)Data > IPStack_GetAddressSize(chan->Interface->Type) )
 			LEAVE_RET('i', -1);
 		chan->RemoteMask = *(int*)Data;
-		return 1;	
+		LEAVE('i', chan->RemoteMask);
+		return chan->RemoteMask;	
 
 	case 7:	// set_remoteaddr (returns bool success)
 		if( !chan->Interface ) {
@@ -351,6 +372,7 @@ int UDP_Channel_IOCtl(tVFS_Node *Node, int ID, void *Data)
 			LEAVE_RET('i', -1);
 		}
 		memcpy(&chan->Remote.Addr, Data, IPStack_GetAddressSize(chan->Interface->Type));
+		LEAVE('i', 0);
 		return 0;
 	}
 	LEAVE_RET('i', 0);
@@ -398,37 +420,55 @@ void UDP_Channel_Close(tVFS_Node *Node)
 /**
  * \return Port Number on success, or zero on failure
  */
-Uint16 UDP_int_AllocatePort()
+Uint16 UDP_int_AllocatePort(tUDPChannel *Channel)
 {
-	 int	i;
 	Mutex_Acquire(&glUDP_Ports);
 	// Fast Search
-	for( i = UDP_ALLOC_BASE; i < 0x10000; i += 32 )
-		if( gUDP_Ports[i/32] != 0xFFFFFFFF )
-			break;
-	if(i == 0x10000)	return 0;
-	for( ;; i++ )
+	for( int base = UDP_ALLOC_BASE; base < 0x10000; base += 32 )
 	{
-		if( !(gUDP_Ports[i/32] & (1 << (i%32))) )
-			return i;
+		if( gUDP_Ports[base/32] == 0xFFFFFFFF )
+			continue ;
+		for( int i = 0; i < 32; i++ )
+		{
+			if( gUDP_Ports[base/32] & (1 << i) )
+				continue ;
+			gUDP_Ports[base/32] |= (1 << i);
+			Mutex_Release(&glUDP_Ports);
+			// If claim succeeds, good
+			if( UDP_int_ClaimPort(Channel, base + i) == 0 )
+				return base + i;
+			// otherwise keep looking
+			Mutex_Acquire(&glUDP_Ports);
+			break;
+		}
 	}
 	Mutex_Release(&glUDP_Ports);
+	return 0;
 }
 
 /**
  * \brief Allocate a specific port
  * \return Boolean Success
  */
-int UDP_int_MarkPortAsUsed(Uint16 Port)
+int UDP_int_ClaimPort(tUDPChannel *Channel, Uint16 Port)
 {
-	Mutex_Acquire(&glUDP_Ports);
-	if( gUDP_Ports[Port/32] & (1 << (Port%32)) ) {
-		return 0;
-		Mutex_Release(&glUDP_Ports);
+	// Search channel list for a connection with same (or wildcard)
+	// interface, and same port
+	Mutex_Acquire(&glUDP_Channels);
+	for( tUDPChannel *ch = gpUDP_Channels; ch; ch = ch->Next)
+	{
+		if( ch == Channel )
+			continue ;
+		if( ch->Interface && ch->Interface != Channel->Interface )
+			continue ;
+		if( ch->LocalPort != Port )
+			continue ;
+		Mutex_Release(&glUDP_Channels);
+		return 1;
 	}
-	gUDP_Ports[Port/32] |= 1 << (Port%32);
-	Mutex_Release(&glUDP_Ports);
-	return 1;
+	Channel->LocalPort = Port;
+	Mutex_Release(&glUDP_Channels);
+	return 0;
 }
 
 /**
@@ -440,3 +480,59 @@ void UDP_int_FreePort(Uint16 Port)
 	gUDP_Ports[Port/32] &= ~(1 << (Port%32));
 	Mutex_Release(&glUDP_Ports);
 }
+
+/**
+ *
+ */
+Uint16 UDP_int_MakeChecksum(tInterface *Interface, const void *Dest, tUDPHeader *Hdr, size_t Len, const void *Data)
+{
+	size_t	addrsize = IPStack_GetAddressSize(Interface->Type);
+	struct {
+		Uint8	Zeroes;
+		Uint8	Protocol;
+		Uint16	UDPLength;
+	} pheader;
+	
+	pheader.Zeroes = 0;
+	switch(Interface->Type)
+	{
+	case 4:	pheader.Protocol = IP4PROT_UDP;	break;
+	//case 6:	pheader.Protocol = IP6PROT_UDP;	break;
+	default:
+		Log_Warning("UDP", "Unimplemented _MakeChecksum proto %i", Interface->Type);
+		return 0;
+	}
+	pheader.UDPLength = Hdr->Length;
+	
+	Uint16	csum = 0;
+	csum = UDP_int_PartialChecksum(csum, addrsize, Interface->Address);
+	csum = UDP_int_PartialChecksum(csum, addrsize, Dest);
+	csum = UDP_int_PartialChecksum(csum, sizeof(pheader), &pheader);
+	csum = UDP_int_PartialChecksum(csum, sizeof(tUDPHeader), Hdr);
+	csum = UDP_int_PartialChecksum(csum, Len, Data);
+	
+	return UDP_int_FinaliseChecksum(csum);
+}
+
+static inline Uint16 _add_ones_complement16(Uint16 a, Uint16 b)
+{
+	// One's complement arithmatic, overflows increment bottom bit
+	return a + b + (b > 0xFFFF - a ? 1 : 0);
+}
+
+Uint16 UDP_int_PartialChecksum(Uint16 Prev, size_t Len, const void *Data)
+{
+	Uint16	ret = Prev;
+	const Uint16	*data = Data;
+	for( int i = 0; i < Len/2; i ++ )
+		ret = _add_ones_complement16(ret, htons(*data++));
+	if( Len % 2 == 1 )
+		ret = _add_ones_complement16(ret, htons(*(const Uint8*)data));
+	return ret;
+}
+
+Uint16 UDP_int_FinaliseChecksum(Uint16 Value)
+{
+	Value = ~Value;	// One's complement it
+	return (Value == 0 ? 0xFFFF : Value);
+}