X-Git-Url: https://git.ucc.asn.au/?a=blobdiff_plain;f=KernelLand%2FModules%2FIPStack%2Fudp.c;h=b9dd3231442851e934daf8e0086bf8b21f00a54c;hb=508aa7d2f1f441ab4c9a7cc4540eb418bc2f9da7;hp=6aba50e30efa7aab3c52e77a849b092b73a0a804;hpb=b35a37b73068a05d95c70af28e8731bece16e02e;p=tpg%2Facess2.git

diff --git a/KernelLand/Modules/IPStack/udp.c b/KernelLand/Modules/IPStack/udp.c
index 6aba50e3..b9dd3231 100644
--- a/KernelLand/Modules/IPStack/udp.c
+++ b/KernelLand/Modules/IPStack/udp.c
@@ -27,15 +27,20 @@ void	UDP_Channel_Close(tVFS_Node *Node);
 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 = {
+	.TypeName = "UDP",
+	.Flags = VFS_NODETYPEFLAG_STREAM,
 	.Read = UDP_Channel_Read,
 	.Write = UDP_Channel_Write,
 	.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,13 +67,15 @@ void UDP_Initialise()
 int UDP_int_ScanList(tUDPChannel *List, tInterface *Interface, void *Address, int Length, void *Buffer)
 {
 	tUDPHeader	*hdr = Buffer;
-	tUDPChannel	*chan;
-	tUDPPacket	*pack;
-	 int	len;
 	
-	for(chan = List; chan; chan = chan->Next)
+	for(tUDPChannel *chan = List; chan; chan = chan->Next)
 	{
 		// Match local endpoint
+		LOG("(%p):%i - %s/%i:%i",
+			chan->Interface, chan->LocalPort,
+			IPStack_PrintAddress(chan->Remote.AddrType, &chan->Remote.Addr), chan->RemoteMask,
+			chan->Remote.Port
+			);
 		if(chan->Interface && chan->Interface != Interface)	continue;
 		if(chan->LocalPort != ntohs(hdr->DestPort))	continue;
 		
@@ -88,8 +95,8 @@ int UDP_int_ScanList(tUDPChannel *List, tInterface *Interface, void *Address, in
 		
 		Log_Log("UDP", "Recieved packet for %p", chan);
 		// Create the cached packet
-		len = ntohs(hdr->Length);
-		pack = malloc(sizeof(tUDPPacket) + len);
+	 	int len = ntohs(hdr->Length);
+		tUDPPacket *pack = malloc(sizeof(tUDPPacket) + len);
 		pack->Next = NULL;
 		memcpy(&pack->Remote.Addr, Address, IPStack_GetAddressSize(Interface->Type));
 		pack->Remote.Port = ntohs(hdr->SourcePort);
@@ -119,8 +126,16 @@ void UDP_GetPacket(tInterface *Interface, void *Address, int Length, void *Buffe
 {
 	tUDPHeader	*hdr = Buffer;
 	
-	Log_Debug("UDP", "%i bytes :%i->:%i (Cksum 0x%04x)",
-		ntohs(hdr->Length), ntohs(hdr->SourcePort), ntohs(hdr->Length), 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);
@@ -146,23 +161,33 @@ void UDP_SendPacketTo(tUDPChannel *Channel, int AddrType, const void *Address, U
 {
 	tUDPHeader	hdr;
 
-	if(Channel->Interface && Channel->Interface->Type != AddrType)	return ;
+	if(Channel->Interface && Channel->Interface->Type != AddrType) {
+		LOG("Bad interface type for channel packet, IF is %i, but packet is %i",
+			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.SourcePort = htons( Channel->LocalPort );
-		hdr.DestPort = htons( Port );
-		hdr.Length = htons( sizeof(tUDPHeader) + Length );
-		hdr.Checksum = 0;	// Checksum can be zero on IPv4
 		// Pass on the the IPv4 Layer
-		tIPStackBuffer	*buffer = IPStack_Buffer_CreateBuffer(2 + IPV4_BUFFERS);
+		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?
+		ASSERT(Channel->Interface);
 		IPv4_SendPacket(Channel->Interface, *(tIPv4*)Address, IP4PROT_UDP, 0, buffer);
-		IPStack_Buffer_DestroyBuffer(buffer);
+		break;
+	default:
+		Log_Warning("UDP", "TODO: Implement on proto %i", AddrType);
 		break;
 	}
 }
@@ -173,6 +198,7 @@ tVFS_Node *UDP_Channel_Init(tInterface *Interface)
 	tUDPChannel	*new;
 	new = calloc( sizeof(tUDPChannel), 1 );
 	new->Interface = Interface;
+	new->Node.Size = -1;
 	new->Node.ImplPtr = new;
 	new->Node.NumACLs = 1;
 	new->Node.ACLs = &gVFS_ACL_EveryoneRW;
@@ -186,49 +212,59 @@ tVFS_Node *UDP_Channel_Init(tInterface *Interface)
 	return &new->Node;
 }
 
-/**
- * \brief Read from the channel file (wait for a packet)
- */
-size_t UDP_Channel_Read(tVFS_Node *Node, off_t Offset, size_t Length, void *Buffer, Uint Flags)
+tUDPPacket *UDP_Channel_WaitForPacket(tUDPChannel *chan, Uint VFSFlags)
 {
-	tUDPChannel	*chan = Node->ImplPtr;
-	tUDPPacket	*pack;
-	tUDPEndpoint	*ep;
-	 int	ofs, addrlen;
-	
-	if(chan->LocalPort == 0) {
-		Log_Notice("UDP", "Channel %p sent with no local port", chan);
-		return 0;
-	}
-	
-	while(chan->Queue == NULL)	Threads_Yield();
+	// EVIL - Yield until queue is created (avoids races)
+	while(chan->Queue == NULL)
+		Threads_Yield();
 	
 	for(;;)
 	{
-		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;
+		tTime	timeout_z = 0, *timeout = (VFSFlags & VFS_IOFLAG_NOBLOCK) ? &timeout_z : NULL;
+		int rv = VFS_SelectNode(&chan->Node, VFS_SELECT_READ, timeout, "UDP_Channel_Read");
+		if( rv == 0 ) {
+			errno = (VFSFlags & VFS_IOFLAG_NOBLOCK) ? EWOULDBLOCK : EINTR;
+			return NULL;
 		}
 		SHORTLOCK(&chan->lQueue);
 		if(chan->Queue == NULL) {
 			SHORTREL(&chan->lQueue);
 			continue;
 		}
-		pack = chan->Queue;
+		tUDPPacket *pack = chan->Queue;
 		chan->Queue = pack->Next;
 		if(!chan->Queue) {
 			chan->QueueEnd = NULL;
-			VFS_MarkAvaliable(Node, 0);	// Nothing left
+			VFS_MarkAvaliable(&chan->Node, 0);	// Nothing left
 		}
 		SHORTREL(&chan->lQueue);
-		break;
+		return pack;
+	}
+	// Unreachable
+}
+
+/**
+ * \brief Read from the channel file (wait for a packet)
+ */
+size_t UDP_Channel_Read(tVFS_Node *Node, off_t Offset, size_t Length, void *Buffer, Uint Flags)
+{
+	tUDPChannel	*chan = Node->ImplPtr;
+	
+	if(chan->LocalPort == 0) {
+		Log_Notice("UDP", "Channel %p sent with no local port", chan);
+		return 0;
+	}
+	
+	tUDPPacket	*pack = UDP_Channel_WaitForPacket(chan, Flags);
+	if( !pack ) {
+		return 0;
 	}
 
+	size_t addrlen = IPStack_GetAddressSize(pack->Remote.AddrType);
+	tUDPEndpoint *ep = Buffer;
+	size_t ofs = 4 + addrlen;
+	
 	// Check that the header fits
-	addrlen = IPStack_GetAddressSize(pack->Remote.AddrType);
-	ep = Buffer;
-	ofs = 4 + addrlen;
 	if(Length < ofs) {
 		free(pack);
 		Log_Notice("UDP", "Insuficient space for header in buffer (%i < %i)", (int)Length, ofs);
@@ -284,6 +320,8 @@ static const char *casIOCtls_Channel[] = {
 	"getset_remoteport",
 	"getset_remotemask",
 	"set_remoteaddr",
+	"sendto",
+	"recvfrom",
 	NULL
 	};
 /**
@@ -335,14 +373,21 @@ int UDP_Channel_IOCtl(tVFS_Node *Node, int ID, void *Data)
 	
 	case 6:	// getset_remotemask (returns bool success)
 		if(!Data)	LEAVE_RET('i', chan->RemoteMask);
-		if(!CheckMem(Data, sizeof(int)))	LEAVE_RET('i', -1);
+		if(!CheckMem(Data, sizeof(int))) {
+			LOG("Data pointer invalid");
+			LEAVE_RET('i', -1);
+		}
 		if( !chan->Interface ) {
 			LOG("Can't set remote mask on NULL interface");
 			LEAVE_RET('i', -1);
 		}
-		if( *(int*)Data > IPStack_GetAddressSize(chan->Interface->Type) )
+		 int	mask = *(int*)Data;
+		 int	addr_bits = IPStack_GetAddressSize(chan->Interface->Type) * 8;
+		if( mask > addr_bits ) {
+			LOG("Mask too large (%i > max %i)", mask, addr_bits);
 			LEAVE_RET('i', -1);
-		chan->RemoteMask = *(int*)Data;
+		}
+		chan->RemoteMask = mask;
 		LEAVE('i', chan->RemoteMask);
 		return chan->RemoteMask;	
 
@@ -355,9 +400,73 @@ int UDP_Channel_IOCtl(tVFS_Node *Node, int ID, void *Data)
 			LOG("Invalid pointer");
 			LEAVE_RET('i', -1);
 		}
+		LOG("Set remote addr %s", IPStack_PrintAddress(chan->Interface->Type, Data));
+		chan->Remote.AddrType = chan->Interface->Type;
 		memcpy(&chan->Remote.Addr, Data, IPStack_GetAddressSize(chan->Interface->Type));
 		LEAVE('i', 0);
 		return 0;
+	case 8: {	// sendto
+		if(!CheckMem(Data, 2*sizeof(void*)+2)) {
+			LOG("Data pointer invalid");
+			LEAVE_RET('i', -1);
+		}
+		const struct sSendToArgs {
+			const tUDPEndpoint* ep;
+			const void* buf;
+			const Uint16 buflen;
+		} info = *(const struct sSendToArgs*)Data;
+		LOG("sendto(buf=%p + %u, ep=%p)", info.buf, info.buflen, info.ep);
+		if(!CheckMem(info.ep, 2+2) || !CheckMem(info.ep, 2+2+IPStack_GetAddressSize(info.ep->AddrType)) ) {
+			LEAVE_RET('i', -1);
+		}
+		if(!CheckMem(info.buf, info.buflen)) {
+			LEAVE_RET('i', -1);
+		}
+		
+		UDP_SendPacketTo(chan, info.ep->AddrType, &info.ep->Addr, info.ep->Port,
+			info.buf, (size_t)info.buflen);
+		
+		LEAVE_RET('i', info.buflen); }
+	case 9: {	// recvfrom
+		if(!CheckMem(Data, 2*sizeof(void*)+2)) {
+			LOG("Data pointer invalid");
+			LEAVE_RET('i', -1);
+		}
+		const struct sRecvFromArgs {
+			tUDPEndpoint* ep;
+			void* buf;
+			Uint16 buflen;
+		} info = *(const struct sRecvFromArgs*)Data;
+		LOG("recvfrom(buf=%p + %u, ep=%p)", info.buf, info.buflen, info.ep);
+		if(!CheckMem(info.ep, 2+2)) {
+			LEAVE_RET('i', -1);
+		}
+		if(!CheckMem(info.buf, info.buflen)) {
+			LEAVE_RET('i', -1);
+		}
+		
+		tUDPPacket	*pack = UDP_Channel_WaitForPacket(chan, 0);
+		if( pack == NULL ) {
+			LOG("No packet");
+			LEAVE_RET('i', 0);
+		}
+		
+		size_t	addrsize = IPStack_GetAddressSize(pack->Remote.AddrType);
+		if( !CheckMem(info.ep, 2+2+addrsize) ) {
+			LOG("Insufficient space for source address");
+			free(pack);
+			LEAVE_RET('i', -1);
+		}
+		info.ep->Port = pack->Remote.Port;
+		info.ep->AddrType = pack->Remote.AddrType;
+		memcpy(&info.ep->Addr, &pack->Remote.Addr, addrsize);
+		
+		size_t	retlen = (info.buflen < pack->Length ? info.buflen : pack->Length);
+		memcpy(info.buf, pack->Data, retlen);
+
+		free(pack);
+	
+		LEAVE_RET('i', retlen); }
 	}
 	LEAVE_RET('i', 0);
 }
@@ -464,3 +573,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);
+}