KernelLand/Modules/IPStack/udp.c

   1 /*
   2  * Acess2 IP Stack
   3  * - By John Hodge (thePowersGang)
   4  *
   5  * udp.c
   6  * - UDP Protocol handling
   7  */
   8 #define DEBUG   1
   9 #include "ipstack.h"
  10 #include <api_drv_common.h>
  11 #include "udp.h"
  12
  13 #define UDP_ALLOC_BASE  0xC000
  14
  15 // === PROTOTYPES ===
  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);
  26 // --- Helpers
  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);
  33
  34 // === GLOBALS ===
  35 tVFS_NodeType   gUDP_NodeType = {
  36         .TypeName = "UDP",
  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
  42 };
  43 tMutex  glUDP_Channels; // TODO: Replace with a RWLock
  44 tUDPChannel     *gpUDP_Channels;
  45
  46 tMutex  glUDP_Ports;
  47 Uint32  gUDP_Ports[0x10000/32];
  48
  49 tSocketFile     gUDP_SocketFile = {NULL, "udp", UDP_Channel_Init};
  50
  51 // === CODE ===
  52 /**
  53  * \fn void TCP_Initialise()
  54  * \brief Initialise the TCP Layer
  55  */
  56 void UDP_Initialise()
  57 {
  58         IPStack_AddFile(&gUDP_SocketFile);
  59         IPv4_RegisterCallback(IP4PROT_UDP, UDP_GetPacket, UDP_IPError);
  60 }
  61
  62 /**
  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
  65  */
  66 int UDP_int_ScanList(tUDPChannel *List, tInterface *Interface, void *Address, int Length, void *Buffer)
  67 {
  68         tUDPHeader      *hdr = Buffer;
  69
  70         for(tUDPChannel *chan = List; chan; chan = chan->Next)
  71         {
  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,
  76                         chan->Remote.Port
  77                         );
  78                 if(chan->Interface && chan->Interface != Interface)     continue;
  79                 if(chan->LocalPort != ntohs(hdr->DestPort))     continue;
  80
  81                 // Check for remote port restriction
  82                 if(chan->Remote.Port && chan->Remote.Port != ntohs(hdr->SourcePort))
  83                         continue;
  84                 // Check for remote address restriction
  85                 if(chan->RemoteMask)
  86                 {
  87                         if(chan->Remote.AddrType != Interface->Type)
  88                                 continue;
  89                         if(!IPStack_CompareAddress(Interface->Type, Address,
  90                                 &chan->Remote.Addr, chan->RemoteMask)
  91                                 )
  92                                 continue;
  93                 }
  94
  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);
  99                 pack->Next = NULL;
 100                 memcpy(&pack->Remote.Addr, Address, IPStack_GetAddressSize(Interface->Type));
 101                 pack->Remote.Port = ntohs(hdr->SourcePort);
 102                 pack->Remote.AddrType = Interface->Type;
 103                 pack->Length = len;
 104                 memcpy(pack->Data, hdr->Data, len);
 105
 106                 // Add the packet to the channel's queue
 107                 SHORTLOCK(&chan->lQueue);
 108                 if(chan->Queue)
 109                         chan->QueueEnd->Next = pack;
 110                 else
 111                         chan->QueueEnd = chan->Queue = pack;
 112                 SHORTREL(&chan->lQueue);
 113                 VFS_MarkAvaliable(&chan->Node, 1);
 114                 Mutex_Release(&glUDP_Channels);
 115                 return 1;
 116         }
 117         return 0;
 118 }
 119
 120 /**
 121  * \fn void UDP_GetPacket(tInterface *Interface, void *Address, int Length, void *Buffer)
 122  * \brief Handles a packet from the IP Layer
 123  */
 124 void UDP_GetPacket(tInterface *Interface, void *Address, int Length, void *Buffer)
 125 {
 126         tUDPHeader      *hdr = Buffer;
 127
 128         #if 1
 129         size_t len = strlen( IPStack_PrintAddress(Interface->Type, Address) );
 130         char    tmp[len+1];
 131         strcpy(tmp, IPStack_PrintAddress(Interface->Type, Address));
 132         Log_Debug("UDP", "%i bytes %s:%i -> %s:%i (Cksum 0x%04x)",
 133                 ntohs(hdr->Length),
 134                 tmp, ntohs(hdr->SourcePort),
 135                 IPStack_PrintAddress(Interface->Type, Interface->Address), ntohs(hdr->DestPort),
 136                 ntohs(hdr->Checksum));
 137         #endif
 138
 139         // Check registered connections
 140         Mutex_Acquire(&glUDP_Channels);
 141         UDP_int_ScanList(gpUDP_Channels, Interface, Address, Length, Buffer);
 142         Mutex_Release(&glUDP_Channels);
 143 }
 144
 145 /**
 146  * \brief Handle an ICMP Unrechable Error
 147  */
 148 void UDP_IPError(tInterface *Interface, tIPErrorMode Code, const void *Address, int Length, const void *Buffer)
 149 {
 150
 151 }
 152
 153 /**
 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
 158  */
 159 void UDP_SendPacketTo(tUDPChannel *Channel, int AddrType, const void *Address, Uint16 Port, const void *Data, size_t Length)
 160 {
 161         tUDPHeader      hdr;
 162
 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);
 166                 return ;
 167         }
 168
 169         // Create the packet
 170         hdr.SourcePort = htons( Channel->LocalPort );
 171         hdr.DestPort = htons( Port );
 172         hdr.Length = htons( sizeof(tUDPHeader) + Length );
 173         hdr.Checksum = 0;
 174         hdr.Checksum = htons( UDP_int_MakeChecksum(Channel->Interface, Address, &hdr, Length, Data) );
 175
 176         tIPStackBuffer  *buffer;
 177         switch(AddrType)
 178         {
 179         case 4:
 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);
 187                 break;
 188         default:
 189                 Log_Warning("UDP", "TODO: Implement on proto %i", AddrType);
 190                 break;
 191         }
 192 }
 193
 194 // --- Client Channels
 195 tVFS_Node *UDP_Channel_Init(tInterface *Interface)
 196 {
 197         tUDPChannel     *new;
 198         new = calloc( sizeof(tUDPChannel), 1 );
 199         new->Interface = Interface;
 200         new->Node.Size = -1;
 201         new->Node.ImplPtr = new;
 202         new->Node.NumACLs = 1;
 203         new->Node.ACLs = &gVFS_ACL_EveryoneRW;
 204         new->Node.Type = &gUDP_NodeType;
 205
 206         Mutex_Acquire(&glUDP_Channels);
 207         new->Next = gpUDP_Channels;
 208         gpUDP_Channels = new;
 209         Mutex_Release(&glUDP_Channels);
 210
 211         return &new->Node;
 212 }
 213
 214 tUDPPacket *UDP_Channel_WaitForPacket(tUDPChannel *chan, Uint VFSFlags)
 215 {
 216         // EVIL - Yield until queue is created (avoids races)
 217         while(chan->Queue == NULL)
 218                 Threads_Yield();
 219
 220         for(;;)
 221         {
 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");
 224                 if( rv == 0 ) {
 225                         errno = (VFSFlags & VFS_IOFLAG_NOBLOCK) ? EWOULDBLOCK : EINTR;
 226                         return NULL;
 227                 }
 228                 SHORTLOCK(&chan->lQueue);
 229                 if(chan->Queue == NULL) {
 230                         SHORTREL(&chan->lQueue);
 231                         continue;
 232                 }
 233                 tUDPPacket *pack = chan->Queue;
 234                 chan->Queue = pack->Next;
 235                 if(!chan->Queue) {
 236                         chan->QueueEnd = NULL;
 237                         VFS_MarkAvaliable(&chan->Node, 0);      // Nothing left
 238                 }
 239                 SHORTREL(&chan->lQueue);
 240                 return pack;
 241         }
 242         // Unreachable
 243 }
 244
 245 /**
 246  * \brief Read from the channel file (wait for a packet)
 247  */
 248 size_t UDP_Channel_Read(tVFS_Node *Node, off_t Offset, size_t Length, void *Buffer, Uint Flags)
 249 {
 250         tUDPChannel     *chan = Node->ImplPtr;
 251
 252         if(chan->LocalPort == 0) {
 253                 Log_Notice("UDP", "Channel %p sent with no local port", chan);
 254                 return 0;
 255         }
 256
 257         tUDPPacket      *pack = UDP_Channel_WaitForPacket(chan, Flags);
 258         if( !pack ) {
 259                 return 0;
 260         }
 261
 262         size_t addrlen = IPStack_GetAddressSize(pack->Remote.AddrType);
 263         tUDPEndpoint *ep = Buffer;
 264         size_t ofs = 4 + addrlen;
 265
 266         // Check that the header fits
 267         if(Length < ofs) {
 268                 free(pack);
 269                 Log_Notice("UDP", "Insuficient space for header in buffer (%i < %i)", (int)Length, ofs);
 270                 return 0;
 271         }
 272
 273         // Fill header
 274         ep->Port = pack->Remote.Port;
 275         ep->AddrType = pack->Remote.AddrType;
 276         memcpy(&ep->Addr, &pack->Remote.Addr, addrlen);
 277
 278         // Copy packet data
 279         if(Length > ofs + pack->Length) Length = ofs + pack->Length;
 280         memcpy((char*)Buffer + ofs, pack->Data, Length - ofs);
 281
 282         // Free cached packet
 283         free(pack);
 284
 285         return Length;
 286 }
 287
 288 /**
 289  * \brief Write to the channel file (send a packet)
 290  */
 291 size_t UDP_Channel_Write(tVFS_Node *Node, off_t Offset, size_t Length, const void *Buffer, Uint Flags)
 292 {
 293         tUDPChannel     *chan = Node->ImplPtr;
 294         const tUDPEndpoint      *ep;
 295         const void      *data;
 296          int    ofs;
 297
 298         if(chan->LocalPort == 0) {
 299                 Log_Notice("UDP", "Write to channel %p with zero local port", chan);
 300                 return 0;
 301         }
 302
 303         ep = Buffer;
 304         ofs = 2 + 2 + IPStack_GetAddressSize( ep->AddrType );
 305
 306         data = (const char *)Buffer + ofs;
 307
 308         UDP_SendPacketTo(chan, ep->AddrType, &ep->Addr, ep->Port, data, (size_t)Length - ofs);
 309
 310         return Length;
 311 }
 312
 313 /**
 314  * \brief Names for channel IOCtl Calls
 315  */
 316 static const char *casIOCtls_Channel[] = {
 317         DRV_IOCTLNAMES,
 318         "getset_localport",
 319         "getset_remoteport",
 320         "getset_remotemask",
 321         "set_remoteaddr",
 322         "sendto",
 323         "recvfrom",
 324         NULL
 325         };
 326 /**
 327  * \brief Channel IOCtls
 328  */
 329 int UDP_Channel_IOCtl(tVFS_Node *Node, int ID, void *Data)
 330 {
 331         tUDPChannel     *chan = Node->ImplPtr;
 332         ENTER("pNode iID pData", Node, ID, Data);
 333         switch(ID)
 334         {
 335         BASE_IOCTLS(DRV_TYPE_MISC, "UDP Channel", 0x100, casIOCtls_Channel);
 336
 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);
 341                         LEAVE_RET('i', -1);
 342                 }
 343                 // Set port
 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);
 349                                 LEAVE_RET('i', -1);
 350                         }
 351                 }
 352                 // Allocate a random port if requested
 353                 if( req_port == 0 )
 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);
 358                         LEAVE_RET('i', 0);
 359                 }
 360                 LEAVE_RET('i', chan->LocalPort);
 361                 }
 362
 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);
 367                         LEAVE_RET('i', -1);
 368                 }
 369                 chan->Remote.Port = *(Uint16*)Data;
 370                 LEAVE('i', chan->Remote.Port);
 371                 return chan->Remote.Port;
 372
 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");
 377                         LEAVE_RET('i', -1);
 378                 }
 379                 if( !chan->Interface ) {
 380                         LOG("Can't set remote mask on NULL interface");
 381                         LEAVE_RET('i', -1);
 382                 }
 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);
 387                         LEAVE_RET('i', -1);
 388                 }
 389                 chan->RemoteMask = mask;
 390                 LEAVE('i', chan->RemoteMask);
 391                 return chan->RemoteMask;
 392
 393         case 7: // set_remoteaddr (returns bool success)
 394                 if( !chan->Interface ) {
 395                         LOG("Can't set remote address on NULL interface");
 396                         LEAVE_RET('i', -1);
 397                 }
 398                 if(!CheckMem(Data, IPStack_GetAddressSize(chan->Interface->Type))) {
 399                         LOG("Invalid pointer");
 400                         LEAVE_RET('i', -1);
 401                 }
 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));
 405                 LEAVE('i', 0);
 406                 return 0;
 407         case 8: {       // sendto
 408                 if(!CheckMem(Data, 2*sizeof(void*)+2)) {
 409                         LOG("Data pointer invalid");
 410                         LEAVE_RET('i', -1);
 411                 }
 412                 const struct sSendToArgs {
 413                         const tUDPEndpoint* ep;
 414                         const void* buf;
 415                         const Uint16 buflen;
 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)) ) {
 419                         LEAVE_RET('i', -1);
 420                 }
 421                 if(!CheckMem(info.buf, info.buflen)) {
 422                         LEAVE_RET('i', -1);
 423                 }
 424
 425                 UDP_SendPacketTo(chan, info.ep->AddrType, &info.ep->Addr, info.ep->Port,
 426                         info.buf, (size_t)info.buflen);
 427
 428                 LEAVE_RET('i', info.buflen); }
 429         case 9: {       // recvfrom
 430                 if(!CheckMem(Data, 2*sizeof(void*)+2)) {
 431                         LOG("Data pointer invalid");
 432                         LEAVE_RET('i', -1);
 433                 }
 434                 const struct sRecvFromArgs {
 435                         tUDPEndpoint* ep;
 436                         void* buf;
 437                         Uint16 buflen;
 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)) {
 441                         LEAVE_RET('i', -1);
 442                 }
 443                 if(!CheckMem(info.buf, info.buflen)) {
 444                         LEAVE_RET('i', -1);
 445                 }
 446
 447                 tUDPPacket      *pack = UDP_Channel_WaitForPacket(chan, 0);
 448                 if( pack == NULL ) {
 449                         LOG("No packet");
 450                         LEAVE_RET('i', 0);
 451                 }
 452
 453                 size_t  addrsize = IPStack_GetAddressSize(pack->Remote.AddrType);
 454                 if( !CheckMem(info.ep, 2+2+addrsize) ) {
 455                         LOG("Insufficient space for source address");
 456                         free(pack);
 457                         LEAVE_RET('i', -1);
 458                 }
 459                 info.ep->Port = pack->Remote.Port;
 460                 info.ep->AddrType = pack->Remote.AddrType;
 461                 memcpy(&info.ep->Addr, &pack->Remote.Addr, addrsize);
 462
 463                 size_t  retlen = (info.buflen < pack->Length ? info.buflen : pack->Length);
 464                 memcpy(info.buf, pack->Data, retlen);
 465
 466                 free(pack);
 467
 468                 LEAVE_RET('i', retlen); }
 469         }
 470         LEAVE_RET('i', 0);
 471 }
 472
 473 /**
 474  * \brief Close and destroy an open channel
 475  */
 476 void UDP_Channel_Close(tVFS_Node *Node)
 477 {
 478         tUDPChannel     *chan = Node->ImplPtr;
 479         tUDPChannel     *prev;
 480
 481         // Remove from the main list first
 482         Mutex_Acquire(&glUDP_Channels);
 483         if(gpUDP_Channels == chan)
 484                 gpUDP_Channels = gpUDP_Channels->Next;
 485         else
 486         {
 487                 for(prev = gpUDP_Channels;
 488                         prev->Next && prev->Next != chan;
 489                         prev = prev->Next);
 490                 if(!prev->Next)
 491                         Log_Warning("UDP", "Bookeeping Fail, channel %p is not in main list", chan);
 492                 else
 493                         prev->Next = prev->Next->Next;
 494         }
 495         Mutex_Release(&glUDP_Channels);
 496
 497         // Clear Queue
 498         SHORTLOCK(&chan->lQueue);
 499         while(chan->Queue)
 500         {
 501                 tUDPPacket      *tmp;
 502                 tmp = chan->Queue;
 503                 chan->Queue = tmp->Next;
 504                 free(tmp);
 505         }
 506         SHORTREL(&chan->lQueue);
 507
 508         // Free channel structure
 509         free(chan);
 510 }
 511
 512 /**
 513  * \return Port Number on success, or zero on failure
 514  */
 515 Uint16 UDP_int_AllocatePort(tUDPChannel *Channel)
 516 {
 517         Mutex_Acquire(&glUDP_Ports);
 518         // Fast Search
 519         for( int base = UDP_ALLOC_BASE; base < 0x10000; base += 32 )
 520         {
 521                 if( gUDP_Ports[base/32] == 0xFFFFFFFF )
 522                         continue ;
 523                 for( int i = 0; i < 32; i++ )
 524                 {
 525                         if( gUDP_Ports[base/32] & (1 << i) )
 526                                 continue ;
 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 )
 531                                 return base + i;
 532                         // otherwise keep looking
 533                         Mutex_Acquire(&glUDP_Ports);
 534                         break;
 535                 }
 536         }
 537         Mutex_Release(&glUDP_Ports);
 538         return 0;
 539 }
 540
 541 /**
 542  * \brief Allocate a specific port
 543  * \return Boolean Success
 544  */
 545 int UDP_int_ClaimPort(tUDPChannel *Channel, Uint16 Port)
 546 {
 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)
 551         {
 552                 if( ch == Channel )
 553                         continue ;
 554                 if( ch->Interface && ch->Interface != Channel->Interface )
 555                         continue ;
 556                 if( ch->LocalPort != Port )
 557                         continue ;
 558                 Mutex_Release(&glUDP_Channels);
 559                 return 1;
 560         }
 561         Channel->LocalPort = Port;
 562         Mutex_Release(&glUDP_Channels);
 563         return 0;
 564 }
 565
 566 /**
 567  * \brief Free an allocated port
 568  */
 569 void UDP_int_FreePort(Uint16 Port)
 570 {
 571         Mutex_Acquire(&glUDP_Ports);
 572         gUDP_Ports[Port/32] &= ~(1 << (Port%32));
 573         Mutex_Release(&glUDP_Ports);
 574 }
 575
 576 /**
 577  *
 578  */
 579 Uint16 UDP_int_MakeChecksum(tInterface *Interface, const void *Dest, tUDPHeader *Hdr, size_t Len, const void *Data)
 580 {
 581         size_t  addrsize = IPStack_GetAddressSize(Interface->Type);
 582         struct {
 583                 Uint8   Zeroes;
 584                 Uint8   Protocol;
 585                 Uint16  UDPLength;
 586         } pheader;
 587
 588         pheader.Zeroes = 0;
 589         switch(Interface->Type)
 590         {
 591         case 4: pheader.Protocol = IP4PROT_UDP; break;
 592         //case 6:       pheader.Protocol = IP6PROT_UDP; break;
 593         default:
 594                 Log_Warning("UDP", "Unimplemented _MakeChecksum proto %i", Interface->Type);
 595                 return 0;
 596         }
 597         pheader.UDPLength = Hdr->Length;
 598
 599         Uint16  csum = 0;
 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);
 605
 606         return UDP_int_FinaliseChecksum(csum);
 607 }
 608
 609 static inline Uint16 _add_ones_complement16(Uint16 a, Uint16 b)
 610 {
 611         // One's complement arithmatic, overflows increment bottom bit
 612         return a + b + (b > 0xFFFF - a ? 1 : 0);
 613 }
 614
 615 Uint16 UDP_int_PartialChecksum(Uint16 Prev, size_t Len, const void *Data)
 616 {
 617         Uint16  ret = Prev;
 618         const Uint16    *data = Data;
 619         for( int i = 0; i < Len/2; i ++ )
 620                 ret = _add_ones_complement16(ret, htons(*data++));
 621         if( Len % 2 == 1 )
 622                 ret = _add_ones_complement16(ret, htons(*(const Uint8*)data));
 623         return ret;
 624 }
 625
 626 Uint16 UDP_int_FinaliseChecksum(Uint16 Value)
 627 {
 628         Value = ~Value; // One's complement it
 629         return (Value == 0 ? 0xFFFF : Value);
 630 }