3 * - By John Hodge (thePowersGang)
6 * - UDP Protocol handling
10 #include <api_drv_common.h>
13 #define UDP_ALLOC_BASE 0xC000
16 void UDP_Initialise();
17 void UDP_GetPacket(tInterface *Interface, void *Address, int Length, void *Buffer);
18 void UDP_Unreachable(tInterface *Interface, int Code, void *Address, int Length, void *Buffer);
19 void UDP_SendPacketTo(tUDPChannel *Channel, int AddrType, const void *Address, Uint16 Port, const void *Data, size_t Length);
20 // --- Client Channels
21 tVFS_Node *UDP_Channel_Init(tInterface *Interface);
22 size_t UDP_Channel_Read(tVFS_Node *Node, off_t Offset, size_t Length, void *Buffer, Uint Flags);
23 size_t UDP_Channel_Write(tVFS_Node *Node, off_t Offset, size_t Length, const void *Buffer, Uint Flags);
24 int UDP_Channel_IOCtl(tVFS_Node *Node, int ID, void *Data);
25 void UDP_Channel_Close(tVFS_Node *Node);
27 Uint16 UDP_int_AllocatePort(tUDPChannel *Channel);
28 int UDP_int_ClaimPort(tUDPChannel *Channel, Uint16 Port);
29 void UDP_int_FreePort(Uint16 Port);
30 Uint16 UDP_int_MakeChecksum(tInterface *Iface, const void *Dest, tUDPHeader *Hdr, size_t Len, const void *Data);
31 Uint16 UDP_int_PartialChecksum(Uint16 Prev, size_t Len, const void *Data);
32 Uint16 UDP_int_FinaliseChecksum(Uint16 Value);
35 tVFS_NodeType gUDP_NodeType = {
36 .Read = UDP_Channel_Read,
37 .Write = UDP_Channel_Write,
38 .IOCtl = UDP_Channel_IOCtl,
39 .Close = UDP_Channel_Close
41 tMutex glUDP_Channels; // TODO: Replace with a RWLock
42 tUDPChannel *gpUDP_Channels;
45 Uint32 gUDP_Ports[0x10000/32];
47 tSocketFile gUDP_SocketFile = {NULL, "udp", UDP_Channel_Init};
51 * \fn void TCP_Initialise()
52 * \brief Initialise the TCP Layer
56 IPStack_AddFile(&gUDP_SocketFile);
57 //IPv4_RegisterCallback(IP4PROT_UDP, UDP_GetPacket, UDP_Unreachable);
58 IPv4_RegisterCallback(IP4PROT_UDP, UDP_GetPacket);
62 * \brief Scan a list of tUDPChannels and find process the first match
63 * \return 0 if no match was found, -1 on error and 1 if a match was found
65 int UDP_int_ScanList(tUDPChannel *List, tInterface *Interface, void *Address, int Length, void *Buffer)
67 tUDPHeader *hdr = Buffer;
72 for(chan = List; chan; chan = chan->Next)
74 // Match local endpoint
75 if(chan->Interface && chan->Interface != Interface) continue;
76 if(chan->LocalPort != ntohs(hdr->DestPort)) continue;
78 // Check for remote port restriction
79 if(chan->Remote.Port && chan->Remote.Port != ntohs(hdr->SourcePort))
81 // Check for remote address restriction
84 if(chan->Remote.AddrType != Interface->Type)
86 if(!IPStack_CompareAddress(Interface->Type, Address,
87 &chan->Remote.Addr, chan->RemoteMask)
92 Log_Log("UDP", "Recieved packet for %p", chan);
93 // Create the cached packet
94 len = ntohs(hdr->Length);
95 pack = malloc(sizeof(tUDPPacket) + len);
97 memcpy(&pack->Remote.Addr, Address, IPStack_GetAddressSize(Interface->Type));
98 pack->Remote.Port = ntohs(hdr->SourcePort);
99 pack->Remote.AddrType = Interface->Type;
101 memcpy(pack->Data, hdr->Data, len);
103 // Add the packet to the channel's queue
104 SHORTLOCK(&chan->lQueue);
106 chan->QueueEnd->Next = pack;
108 chan->QueueEnd = chan->Queue = pack;
109 SHORTREL(&chan->lQueue);
110 VFS_MarkAvaliable(&chan->Node, 1);
111 Mutex_Release(&glUDP_Channels);
118 * \fn void UDP_GetPacket(tInterface *Interface, void *Address, int Length, void *Buffer)
119 * \brief Handles a packet from the IP Layer
121 void UDP_GetPacket(tInterface *Interface, void *Address, int Length, void *Buffer)
123 tUDPHeader *hdr = Buffer;
126 size_t len = strlen( IPStack_PrintAddress(Interface->Type, Address) );
128 strcpy(tmp, IPStack_PrintAddress(Interface->Type, Address));
129 Log_Debug("UDP", "%i bytes %s:%i -> %s:%i (Cksum 0x%04x)",
131 tmp, ntohs(hdr->SourcePort),
132 IPStack_PrintAddress(Interface->Type, Interface->Address), ntohs(hdr->DestPort),
133 ntohs(hdr->Checksum));
136 // Check registered connections
137 Mutex_Acquire(&glUDP_Channels);
138 UDP_int_ScanList(gpUDP_Channels, Interface, Address, Length, Buffer);
139 Mutex_Release(&glUDP_Channels);
143 * \brief Handle an ICMP Unrechable Error
145 void UDP_Unreachable(tInterface *Interface, int Code, void *Address, int Length, void *Buffer)
151 * \brief Send a packet
152 * \param Channel Channel to send the packet from
153 * \param Data Packet data
154 * \param Length Length in bytes of packet data
156 void UDP_SendPacketTo(tUDPChannel *Channel, int AddrType, const void *Address, Uint16 Port, const void *Data, size_t Length)
160 if(Channel->Interface && Channel->Interface->Type != AddrType) return ;
163 hdr.SourcePort = htons( Channel->LocalPort );
164 hdr.DestPort = htons( Port );
165 hdr.Length = htons( sizeof(tUDPHeader) + Length );
167 hdr.Checksum = htons( UDP_int_MakeChecksum(Channel->Interface, Address, &hdr, Length, Data) );
169 tIPStackBuffer *buffer;
173 // Pass on the the IPv4 Layer
174 buffer = IPStack_Buffer_CreateBuffer(2 + IPV4_BUFFERS);
175 IPStack_Buffer_AppendSubBuffer(buffer, Length, 0, Data, NULL, NULL);
176 IPStack_Buffer_AppendSubBuffer(buffer, sizeof(hdr), 0, &hdr, NULL, NULL);
177 // TODO: What if Channel->Interface is NULL here?
178 IPv4_SendPacket(Channel->Interface, *(tIPv4*)Address, IP4PROT_UDP, 0, buffer);
181 Log_Warning("UDP", "TODO: Implement on proto %i", AddrType);
186 // --- Client Channels
187 tVFS_Node *UDP_Channel_Init(tInterface *Interface)
190 new = calloc( sizeof(tUDPChannel), 1 );
191 new->Interface = Interface;
192 new->Node.ImplPtr = new;
193 new->Node.NumACLs = 1;
194 new->Node.ACLs = &gVFS_ACL_EveryoneRW;
195 new->Node.Type = &gUDP_NodeType;
197 Mutex_Acquire(&glUDP_Channels);
198 new->Next = gpUDP_Channels;
199 gpUDP_Channels = new;
200 Mutex_Release(&glUDP_Channels);
206 * \brief Read from the channel file (wait for a packet)
208 size_t UDP_Channel_Read(tVFS_Node *Node, off_t Offset, size_t Length, void *Buffer, Uint Flags)
210 tUDPChannel *chan = Node->ImplPtr;
215 if(chan->LocalPort == 0) {
216 Log_Notice("UDP", "Channel %p sent with no local port", chan);
220 while(chan->Queue == NULL) Threads_Yield();
224 tTime timeout_z = 0, *timeout = (Flags & VFS_IOFLAG_NOBLOCK) ? &timeout_z : NULL;
225 int rv = VFS_SelectNode(Node, VFS_SELECT_READ, timeout, "UDP_Channel_Read");
227 errno = (Flags & VFS_IOFLAG_NOBLOCK) ? EWOULDBLOCK : EINTR;
229 SHORTLOCK(&chan->lQueue);
230 if(chan->Queue == NULL) {
231 SHORTREL(&chan->lQueue);
235 chan->Queue = pack->Next;
237 chan->QueueEnd = NULL;
238 VFS_MarkAvaliable(Node, 0); // Nothing left
240 SHORTREL(&chan->lQueue);
244 // Check that the header fits
245 addrlen = IPStack_GetAddressSize(pack->Remote.AddrType);
250 Log_Notice("UDP", "Insuficient space for header in buffer (%i < %i)", (int)Length, ofs);
255 ep->Port = pack->Remote.Port;
256 ep->AddrType = pack->Remote.AddrType;
257 memcpy(&ep->Addr, &pack->Remote.Addr, addrlen);
260 if(Length > ofs + pack->Length) Length = ofs + pack->Length;
261 memcpy((char*)Buffer + ofs, pack->Data, Length - ofs);
263 // Free cached packet
270 * \brief Write to the channel file (send a packet)
272 size_t UDP_Channel_Write(tVFS_Node *Node, off_t Offset, size_t Length, const void *Buffer, Uint Flags)
274 tUDPChannel *chan = Node->ImplPtr;
275 const tUDPEndpoint *ep;
279 if(chan->LocalPort == 0) {
280 Log_Notice("UDP", "Write to channel %p with zero local port", chan);
285 ofs = 2 + 2 + IPStack_GetAddressSize( ep->AddrType );
287 data = (const char *)Buffer + ofs;
289 UDP_SendPacketTo(chan, ep->AddrType, &ep->Addr, ep->Port, data, (size_t)Length - ofs);
295 * \brief Names for channel IOCtl Calls
297 static const char *casIOCtls_Channel[] = {
306 * \brief Channel IOCtls
308 int UDP_Channel_IOCtl(tVFS_Node *Node, int ID, void *Data)
310 tUDPChannel *chan = Node->ImplPtr;
311 ENTER("pNode iID pData", Node, ID, Data);
314 BASE_IOCTLS(DRV_TYPE_MISC, "UDP Channel", 0x100, casIOCtls_Channel);
316 case 4: { // getset_localport (returns bool success)
317 if(!Data) LEAVE_RET('i', chan->LocalPort);
318 if(!CheckMem( Data, sizeof(Uint16) ) ) {
319 LOG("Invalid pointer %p", Data);
323 int req_port = *(Uint16*)Data;
324 // Permissions check (Ports lower than 1024 are root-only)
325 if(req_port != 0 && req_port < 1024) {
326 if( Threads_GetUID() != 0 ) {
327 LOG("Attempt by non-superuser to listen on port %i", req_port);
331 // Allocate a random port if requested
333 UDP_int_AllocatePort(chan);
334 // Else, mark the requested port as used
335 else if( UDP_int_ClaimPort(chan, req_port) ) {
336 LOG("Port %i is currently in use", req_port);
339 LEAVE_RET('i', chan->LocalPort);
342 case 5: // getset_remoteport (returns bool success)
343 if(!Data) LEAVE_RET('i', chan->Remote.Port);
344 if(!CheckMem( Data, sizeof(Uint16) ) ) {
345 LOG("Invalid pointer %p", Data);
348 chan->Remote.Port = *(Uint16*)Data;
349 LEAVE('i', chan->Remote.Port);
350 return chan->Remote.Port;
352 case 6: // getset_remotemask (returns bool success)
353 if(!Data) LEAVE_RET('i', chan->RemoteMask);
354 if(!CheckMem(Data, sizeof(int))) LEAVE_RET('i', -1);
355 if( !chan->Interface ) {
356 LOG("Can't set remote mask on NULL interface");
359 if( *(int*)Data > IPStack_GetAddressSize(chan->Interface->Type) )
361 chan->RemoteMask = *(int*)Data;
362 LEAVE('i', chan->RemoteMask);
363 return chan->RemoteMask;
365 case 7: // set_remoteaddr (returns bool success)
366 if( !chan->Interface ) {
367 LOG("Can't set remote address on NULL interface");
370 if(!CheckMem(Data, IPStack_GetAddressSize(chan->Interface->Type))) {
371 LOG("Invalid pointer");
374 memcpy(&chan->Remote.Addr, Data, IPStack_GetAddressSize(chan->Interface->Type));
382 * \brief Close and destroy an open channel
384 void UDP_Channel_Close(tVFS_Node *Node)
386 tUDPChannel *chan = Node->ImplPtr;
389 // Remove from the main list first
390 Mutex_Acquire(&glUDP_Channels);
391 if(gpUDP_Channels == chan)
392 gpUDP_Channels = gpUDP_Channels->Next;
395 for(prev = gpUDP_Channels;
396 prev->Next && prev->Next != chan;
399 Log_Warning("UDP", "Bookeeping Fail, channel %p is not in main list", chan);
401 prev->Next = prev->Next->Next;
403 Mutex_Release(&glUDP_Channels);
406 SHORTLOCK(&chan->lQueue);
411 chan->Queue = tmp->Next;
414 SHORTREL(&chan->lQueue);
416 // Free channel structure
421 * \return Port Number on success, or zero on failure
423 Uint16 UDP_int_AllocatePort(tUDPChannel *Channel)
425 Mutex_Acquire(&glUDP_Ports);
427 for( int base = UDP_ALLOC_BASE; base < 0x10000; base += 32 )
429 if( gUDP_Ports[base/32] == 0xFFFFFFFF )
431 for( int i = 0; i < 32; i++ )
433 if( gUDP_Ports[base/32] & (1 << i) )
435 gUDP_Ports[base/32] |= (1 << i);
436 Mutex_Release(&glUDP_Ports);
437 // If claim succeeds, good
438 if( UDP_int_ClaimPort(Channel, base + i) == 0 )
440 // otherwise keep looking
441 Mutex_Acquire(&glUDP_Ports);
445 Mutex_Release(&glUDP_Ports);
450 * \brief Allocate a specific port
451 * \return Boolean Success
453 int UDP_int_ClaimPort(tUDPChannel *Channel, Uint16 Port)
455 // Search channel list for a connection with same (or wildcard)
456 // interface, and same port
457 Mutex_Acquire(&glUDP_Channels);
458 for( tUDPChannel *ch = gpUDP_Channels; ch; ch = ch->Next)
462 if( ch->Interface && ch->Interface != Channel->Interface )
464 if( ch->LocalPort != Port )
466 Mutex_Release(&glUDP_Channels);
469 Channel->LocalPort = Port;
470 Mutex_Release(&glUDP_Channels);
475 * \brief Free an allocated port
477 void UDP_int_FreePort(Uint16 Port)
479 Mutex_Acquire(&glUDP_Ports);
480 gUDP_Ports[Port/32] &= ~(1 << (Port%32));
481 Mutex_Release(&glUDP_Ports);
487 Uint16 UDP_int_MakeChecksum(tInterface *Interface, const void *Dest, tUDPHeader *Hdr, size_t Len, const void *Data)
489 size_t addrsize = IPStack_GetAddressSize(Interface->Type);
497 switch(Interface->Type)
499 case 4: pheader.Protocol = IP4PROT_UDP; break;
500 //case 6: pheader.Protocol = IP6PROT_UDP; break;
502 Log_Warning("UDP", "Unimplemented _MakeChecksum proto %i", Interface->Type);
505 pheader.UDPLength = Hdr->Length;
508 csum = UDP_int_PartialChecksum(csum, addrsize, Interface->Address);
509 csum = UDP_int_PartialChecksum(csum, addrsize, Dest);
510 csum = UDP_int_PartialChecksum(csum, sizeof(pheader), &pheader);
511 csum = UDP_int_PartialChecksum(csum, sizeof(tUDPHeader), Hdr);
512 csum = UDP_int_PartialChecksum(csum, Len, Data);
514 return UDP_int_FinaliseChecksum(csum);
517 static inline Uint16 _add_ones_complement16(Uint16 a, Uint16 b)
519 // One's complement arithmatic, overflows increment bottom bit
520 return a + b + (b > 0xFFFF - a ? 1 : 0);
523 Uint16 UDP_int_PartialChecksum(Uint16 Prev, size_t Len, const void *Data)
526 const Uint16 *data = Data;
527 for( int i = 0; i < Len/2; i ++ )
528 ret = _add_ones_complement16(ret, htons(*data++));
530 ret = _add_ones_complement16(ret, htons(*(const Uint8*)data));
534 Uint16 UDP_int_FinaliseChecksum(Uint16 Value)
536 Value = ~Value; // One's complement it
537 return (Value == 0 ? 0xFFFF : Value);