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_IPError(tInterface *Interface, tIPErrorMode Code, const void *Address, int Length, const 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 = {
37 .Flags = VFS_NODETYPEFLAG_STREAM,
38 .Read = UDP_Channel_Read,
39 .Write = UDP_Channel_Write,
40 .IOCtl = UDP_Channel_IOCtl,
41 .Close = UDP_Channel_Close
43 tMutex glUDP_Channels; // TODO: Replace with a RWLock
44 tUDPChannel *gpUDP_Channels;
47 Uint32 gUDP_Ports[0x10000/32];
49 tSocketFile gUDP_SocketFile = {NULL, "udp", UDP_Channel_Init};
53 * \fn void TCP_Initialise()
54 * \brief Initialise the TCP Layer
58 IPStack_AddFile(&gUDP_SocketFile);
59 IPv4_RegisterCallback(IP4PROT_UDP, UDP_GetPacket, UDP_IPError);
63 * \brief Scan a list of tUDPChannels and find process the first match
64 * \return 0 if no match was found, -1 on error and 1 if a match was found
66 int UDP_int_ScanList(tUDPChannel *List, tInterface *Interface, void *Address, int Length, void *Buffer)
68 tUDPHeader *hdr = Buffer;
70 for(tUDPChannel *chan = List; chan; chan = chan->Next)
72 // Match local endpoint
73 LOG("(%p):%i - %s/%i:%i",
74 chan->Interface, chan->LocalPort,
75 IPStack_PrintAddress(chan->Remote.AddrType, &chan->Remote.Addr), chan->RemoteMask,
78 if(chan->Interface && chan->Interface != Interface) continue;
79 if(chan->LocalPort != ntohs(hdr->DestPort)) continue;
81 // Check for remote port restriction
82 if(chan->Remote.Port && chan->Remote.Port != ntohs(hdr->SourcePort))
84 // Check for remote address restriction
87 if(chan->Remote.AddrType != Interface->Type)
89 if(!IPStack_CompareAddress(Interface->Type, Address,
90 &chan->Remote.Addr, chan->RemoteMask)
95 Log_Log("UDP", "Recieved packet for %p", chan);
96 // Create the cached packet
97 int len = ntohs(hdr->Length);
98 tUDPPacket *pack = malloc(sizeof(tUDPPacket) + len);
100 memcpy(&pack->Remote.Addr, Address, IPStack_GetAddressSize(Interface->Type));
101 pack->Remote.Port = ntohs(hdr->SourcePort);
102 pack->Remote.AddrType = Interface->Type;
104 memcpy(pack->Data, hdr->Data, len);
106 // Add the packet to the channel's queue
107 SHORTLOCK(&chan->lQueue);
109 chan->QueueEnd->Next = pack;
111 chan->QueueEnd = chan->Queue = pack;
112 SHORTREL(&chan->lQueue);
113 VFS_MarkAvaliable(&chan->Node, 1);
114 Mutex_Release(&glUDP_Channels);
121 * \fn void UDP_GetPacket(tInterface *Interface, void *Address, int Length, void *Buffer)
122 * \brief Handles a packet from the IP Layer
124 void UDP_GetPacket(tInterface *Interface, void *Address, int Length, void *Buffer)
126 tUDPHeader *hdr = Buffer;
129 size_t len = strlen( IPStack_PrintAddress(Interface->Type, Address) );
131 strcpy(tmp, IPStack_PrintAddress(Interface->Type, Address));
132 Log_Debug("UDP", "%i bytes %s:%i -> %s:%i (Cksum 0x%04x)",
134 tmp, ntohs(hdr->SourcePort),
135 IPStack_PrintAddress(Interface->Type, Interface->Address), ntohs(hdr->DestPort),
136 ntohs(hdr->Checksum));
139 // Check registered connections
140 Mutex_Acquire(&glUDP_Channels);
141 UDP_int_ScanList(gpUDP_Channels, Interface, Address, Length, Buffer);
142 Mutex_Release(&glUDP_Channels);
146 * \brief Handle an ICMP Unrechable Error
148 void UDP_IPError(tInterface *Interface, tIPErrorMode Code, const void *Address, int Length, const void *Buffer)
154 * \brief Send a packet
155 * \param Channel Channel to send the packet from
156 * \param Data Packet data
157 * \param Length Length in bytes of packet data
159 void UDP_SendPacketTo(tUDPChannel *Channel, int AddrType, const void *Address, Uint16 Port, const void *Data, size_t Length)
163 if(Channel->Interface && Channel->Interface->Type != AddrType) {
164 LOG("Bad interface type for channel packet, IF is %i, but packet is %i",
165 Channel->Interface->Type, AddrType);
170 hdr.SourcePort = htons( Channel->LocalPort );
171 hdr.DestPort = htons( Port );
172 hdr.Length = htons( sizeof(tUDPHeader) + Length );
174 hdr.Checksum = htons( UDP_int_MakeChecksum(Channel->Interface, Address, &hdr, Length, Data) );
176 tIPStackBuffer *buffer;
180 // Pass on the the IPv4 Layer
181 buffer = IPStack_Buffer_CreateBuffer(2 + IPV4_BUFFERS);
182 IPStack_Buffer_AppendSubBuffer(buffer, Length, 0, Data, NULL, NULL);
183 IPStack_Buffer_AppendSubBuffer(buffer, sizeof(hdr), 0, &hdr, NULL, NULL);
184 // TODO: What if Channel->Interface is NULL here?
185 ASSERT(Channel->Interface);
186 IPv4_SendPacket(Channel->Interface, *(tIPv4*)Address, IP4PROT_UDP, 0, buffer);
189 Log_Warning("UDP", "TODO: Implement on proto %i", AddrType);
194 // --- Client Channels
195 tVFS_Node *UDP_Channel_Init(tInterface *Interface)
198 new = calloc( sizeof(tUDPChannel), 1 );
199 new->Interface = Interface;
201 new->Node.ImplPtr = new;
202 new->Node.NumACLs = 1;
203 new->Node.ACLs = &gVFS_ACL_EveryoneRW;
204 new->Node.Type = &gUDP_NodeType;
206 Mutex_Acquire(&glUDP_Channels);
207 new->Next = gpUDP_Channels;
208 gpUDP_Channels = new;
209 Mutex_Release(&glUDP_Channels);
214 tUDPPacket *UDP_Channel_WaitForPacket(tUDPChannel *chan, Uint VFSFlags)
216 // EVIL - Yield until queue is created (avoids races)
217 while(chan->Queue == NULL)
222 tTime timeout_z = 0, *timeout = (VFSFlags & VFS_IOFLAG_NOBLOCK) ? &timeout_z : NULL;
223 int rv = VFS_SelectNode(&chan->Node, VFS_SELECT_READ, timeout, "UDP_Channel_Read");
225 errno = (VFSFlags & VFS_IOFLAG_NOBLOCK) ? EWOULDBLOCK : EINTR;
228 SHORTLOCK(&chan->lQueue);
229 if(chan->Queue == NULL) {
230 SHORTREL(&chan->lQueue);
233 tUDPPacket *pack = chan->Queue;
234 chan->Queue = pack->Next;
236 chan->QueueEnd = NULL;
237 VFS_MarkAvaliable(&chan->Node, 0); // Nothing left
239 SHORTREL(&chan->lQueue);
246 * \brief Read from the channel file (wait for a packet)
248 size_t UDP_Channel_Read(tVFS_Node *Node, off_t Offset, size_t Length, void *Buffer, Uint Flags)
250 tUDPChannel *chan = Node->ImplPtr;
252 if(chan->LocalPort == 0) {
253 Log_Notice("UDP", "Channel %p sent with no local port", chan);
257 tUDPPacket *pack = UDP_Channel_WaitForPacket(chan, Flags);
262 size_t addrlen = IPStack_GetAddressSize(pack->Remote.AddrType);
263 tUDPEndpoint *ep = Buffer;
264 size_t ofs = 4 + addrlen;
266 // Check that the header fits
269 Log_Notice("UDP", "Insuficient space for header in buffer (%i < %i)", (int)Length, ofs);
274 ep->Port = pack->Remote.Port;
275 ep->AddrType = pack->Remote.AddrType;
276 memcpy(&ep->Addr, &pack->Remote.Addr, addrlen);
279 if(Length > ofs + pack->Length) Length = ofs + pack->Length;
280 memcpy((char*)Buffer + ofs, pack->Data, Length - ofs);
282 // Free cached packet
289 * \brief Write to the channel file (send a packet)
291 size_t UDP_Channel_Write(tVFS_Node *Node, off_t Offset, size_t Length, const void *Buffer, Uint Flags)
293 tUDPChannel *chan = Node->ImplPtr;
294 const tUDPEndpoint *ep;
298 if(chan->LocalPort == 0) {
299 Log_Notice("UDP", "Write to channel %p with zero local port", chan);
304 ofs = 2 + 2 + IPStack_GetAddressSize( ep->AddrType );
306 data = (const char *)Buffer + ofs;
308 UDP_SendPacketTo(chan, ep->AddrType, &ep->Addr, ep->Port, data, (size_t)Length - ofs);
314 * \brief Names for channel IOCtl Calls
316 static const char *casIOCtls_Channel[] = {
327 * \brief Channel IOCtls
329 int UDP_Channel_IOCtl(tVFS_Node *Node, int ID, void *Data)
331 tUDPChannel *chan = Node->ImplPtr;
332 ENTER("pNode iID pData", Node, ID, Data);
335 BASE_IOCTLS(DRV_TYPE_MISC, "UDP Channel", 0x100, casIOCtls_Channel);
337 case 4: { // getset_localport (returns bool success)
338 if(!Data) LEAVE_RET('i', chan->LocalPort);
339 if(!CheckMem( Data, sizeof(Uint16) ) ) {
340 LOG("Invalid pointer %p", Data);
344 int req_port = *(Uint16*)Data;
345 // Permissions check (Ports lower than 1024 are root-only)
346 if(req_port != 0 && req_port < 1024) {
347 if( Threads_GetUID() != 0 ) {
348 LOG("Attempt by non-superuser to listen on port %i", req_port);
352 // Allocate a random port if requested
354 UDP_int_AllocatePort(chan);
355 // Else, mark the requested port as used
356 else if( UDP_int_ClaimPort(chan, req_port) ) {
357 LOG("Port %i is currently in use", req_port);
360 LEAVE_RET('i', chan->LocalPort);
363 case 5: // getset_remoteport (returns bool success)
364 if(!Data) LEAVE_RET('i', chan->Remote.Port);
365 if(!CheckMem( Data, sizeof(Uint16) ) ) {
366 LOG("Invalid pointer %p", Data);
369 chan->Remote.Port = *(Uint16*)Data;
370 LEAVE('i', chan->Remote.Port);
371 return chan->Remote.Port;
373 case 6: // getset_remotemask (returns bool success)
374 if(!Data) LEAVE_RET('i', chan->RemoteMask);
375 if(!CheckMem(Data, sizeof(int))) {
376 LOG("Data pointer invalid");
379 if( !chan->Interface ) {
380 LOG("Can't set remote mask on NULL interface");
383 int mask = *(int*)Data;
384 int addr_bits = IPStack_GetAddressSize(chan->Interface->Type) * 8;
385 if( mask > addr_bits ) {
386 LOG("Mask too large (%i > max %i)", mask, addr_bits);
389 chan->RemoteMask = mask;
390 LEAVE('i', chan->RemoteMask);
391 return chan->RemoteMask;
393 case 7: // set_remoteaddr (returns bool success)
394 if( !chan->Interface ) {
395 LOG("Can't set remote address on NULL interface");
398 if(!CheckMem(Data, IPStack_GetAddressSize(chan->Interface->Type))) {
399 LOG("Invalid pointer");
402 LOG("Set remote addr %s", IPStack_PrintAddress(chan->Interface->Type, Data));
403 chan->Remote.AddrType = chan->Interface->Type;
404 memcpy(&chan->Remote.Addr, Data, IPStack_GetAddressSize(chan->Interface->Type));
408 if(!CheckMem(Data, 2*sizeof(void*)+2)) {
409 LOG("Data pointer invalid");
412 const struct sSendToArgs {
413 const tUDPEndpoint* ep;
416 } info = *(const struct sSendToArgs*)Data;
417 LOG("sendto(buf=%p + %u, ep=%p)", info.buf, info.buflen, info.ep);
418 if(!CheckMem(info.ep, 2+2) || !CheckMem(info.ep, 2+2+IPStack_GetAddressSize(info.ep->AddrType)) ) {
421 if(!CheckMem(info.buf, info.buflen)) {
425 UDP_SendPacketTo(chan, info.ep->AddrType, &info.ep->Addr, info.ep->Port,
426 info.buf, (size_t)info.buflen);
428 LEAVE_RET('i', info.buflen); }
429 case 9: { // recvfrom
430 if(!CheckMem(Data, 2*sizeof(void*)+2)) {
431 LOG("Data pointer invalid");
434 const struct sRecvFromArgs {
438 } info = *(const struct sRecvFromArgs*)Data;
439 LOG("recvfrom(buf=%p + %u, ep=%p)", info.buf, info.buflen, info.ep);
440 if(!CheckMem(info.ep, 2+2)) {
443 if(!CheckMem(info.buf, info.buflen)) {
447 tUDPPacket *pack = UDP_Channel_WaitForPacket(chan, 0);
453 size_t addrsize = IPStack_GetAddressSize(pack->Remote.AddrType);
454 if( !CheckMem(info.ep, 2+2+addrsize) ) {
455 LOG("Insufficient space for source address");
459 info.ep->Port = pack->Remote.Port;
460 info.ep->AddrType = pack->Remote.AddrType;
461 memcpy(&info.ep->Addr, &pack->Remote.Addr, addrsize);
463 size_t retlen = (info.buflen < pack->Length ? info.buflen : pack->Length);
464 memcpy(info.buf, pack->Data, retlen);
468 LEAVE_RET('i', retlen); }
474 * \brief Close and destroy an open channel
476 void UDP_Channel_Close(tVFS_Node *Node)
478 tUDPChannel *chan = Node->ImplPtr;
481 // Remove from the main list first
482 Mutex_Acquire(&glUDP_Channels);
483 if(gpUDP_Channels == chan)
484 gpUDP_Channels = gpUDP_Channels->Next;
487 for(prev = gpUDP_Channels;
488 prev->Next && prev->Next != chan;
491 Log_Warning("UDP", "Bookeeping Fail, channel %p is not in main list", chan);
493 prev->Next = prev->Next->Next;
495 Mutex_Release(&glUDP_Channels);
498 SHORTLOCK(&chan->lQueue);
503 chan->Queue = tmp->Next;
506 SHORTREL(&chan->lQueue);
508 // Free channel structure
513 * \return Port Number on success, or zero on failure
515 Uint16 UDP_int_AllocatePort(tUDPChannel *Channel)
517 Mutex_Acquire(&glUDP_Ports);
519 for( int base = UDP_ALLOC_BASE; base < 0x10000; base += 32 )
521 if( gUDP_Ports[base/32] == 0xFFFFFFFF )
523 for( int i = 0; i < 32; i++ )
525 if( gUDP_Ports[base/32] & (1 << i) )
527 gUDP_Ports[base/32] |= (1 << i);
528 Mutex_Release(&glUDP_Ports);
529 // If claim succeeds, good
530 if( UDP_int_ClaimPort(Channel, base + i) == 0 )
532 // otherwise keep looking
533 Mutex_Acquire(&glUDP_Ports);
537 Mutex_Release(&glUDP_Ports);
542 * \brief Allocate a specific port
543 * \return Boolean Success
545 int UDP_int_ClaimPort(tUDPChannel *Channel, Uint16 Port)
547 // Search channel list for a connection with same (or wildcard)
548 // interface, and same port
549 Mutex_Acquire(&glUDP_Channels);
550 for( tUDPChannel *ch = gpUDP_Channels; ch; ch = ch->Next)
554 if( ch->Interface && ch->Interface != Channel->Interface )
556 if( ch->LocalPort != Port )
558 Mutex_Release(&glUDP_Channels);
561 Channel->LocalPort = Port;
562 Mutex_Release(&glUDP_Channels);
567 * \brief Free an allocated port
569 void UDP_int_FreePort(Uint16 Port)
571 Mutex_Acquire(&glUDP_Ports);
572 gUDP_Ports[Port/32] &= ~(1 << (Port%32));
573 Mutex_Release(&glUDP_Ports);
579 Uint16 UDP_int_MakeChecksum(tInterface *Interface, const void *Dest, tUDPHeader *Hdr, size_t Len, const void *Data)
581 size_t addrsize = IPStack_GetAddressSize(Interface->Type);
589 switch(Interface->Type)
591 case 4: pheader.Protocol = IP4PROT_UDP; break;
592 //case 6: pheader.Protocol = IP6PROT_UDP; break;
594 Log_Warning("UDP", "Unimplemented _MakeChecksum proto %i", Interface->Type);
597 pheader.UDPLength = Hdr->Length;
600 csum = UDP_int_PartialChecksum(csum, addrsize, Interface->Address);
601 csum = UDP_int_PartialChecksum(csum, addrsize, Dest);
602 csum = UDP_int_PartialChecksum(csum, sizeof(pheader), &pheader);
603 csum = UDP_int_PartialChecksum(csum, sizeof(tUDPHeader), Hdr);
604 csum = UDP_int_PartialChecksum(csum, Len, Data);
606 return UDP_int_FinaliseChecksum(csum);
609 static inline Uint16 _add_ones_complement16(Uint16 a, Uint16 b)
611 // One's complement arithmatic, overflows increment bottom bit
612 return a + b + (b > 0xFFFF - a ? 1 : 0);
615 Uint16 UDP_int_PartialChecksum(Uint16 Prev, size_t Len, const void *Data)
618 const Uint16 *data = Data;
619 for( int i = 0; i < Len/2; i ++ )
620 ret = _add_ones_complement16(ret, htons(*data++));
622 ret = _add_ones_complement16(ret, htons(*(const Uint8*)data));
626 Uint16 UDP_int_FinaliseChecksum(Uint16 Value)
628 Value = ~Value; // One's complement it
629 return (Value == 0 ? 0xFFFF : Value);