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_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);
  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_Unreachable);
  60         IPv4_RegisterCallback(IP4PROT_UDP, UDP_GetPacket);
  61 }
  62
  63 /**
  64  * \brief Scan a list of tUDPChannels and find process the first match
  65  * \return 0 if no match was found, -1 on error and 1 if a match was found
  66  */
  67 int UDP_int_ScanList(tUDPChannel *List, tInterface *Interface, void *Address, int Length, void *Buffer)
  68 {
  69         tUDPHeader      *hdr = Buffer;
  70
  71         for(tUDPChannel *chan = List; chan; chan = chan->Next)
  72         {
  73                 // Match local endpoint
  74                 LOG("(%p):%i - %s/%i:%i",
  75                         chan->Interface, chan->LocalPort,
  76                         IPStack_PrintAddress(chan->Remote.AddrType, &chan->Remote.Addr), chan->RemoteMask,
  77                         chan->Remote.Port
  78                         );
  79                 if(chan->Interface && chan->Interface != Interface)     continue;
  80                 if(chan->LocalPort != ntohs(hdr->DestPort))     continue;
  81
  82                 // Check for remote port restriction
  83                 if(chan->Remote.Port && chan->Remote.Port != ntohs(hdr->SourcePort))
  84                         continue;
  85                 // Check for remote address restriction
  86                 if(chan->RemoteMask)
  87                 {
  88                         if(chan->Remote.AddrType != Interface->Type)
  89                                 continue;
  90                         if(!IPStack_CompareAddress(Interface->Type, Address,
  91                                 &chan->Remote.Addr, chan->RemoteMask)
  92                                 )
  93                                 continue;
  94                 }
  95
  96                 Log_Log("UDP", "Recieved packet for %p", chan);
  97                 // Create the cached packet
  98                 int len = ntohs(hdr->Length);
  99                 tUDPPacket *pack = malloc(sizeof(tUDPPacket) + len);
 100                 pack->Next = NULL;
 101                 memcpy(&pack->Remote.Addr, Address, IPStack_GetAddressSize(Interface->Type));
 102                 pack->Remote.Port = ntohs(hdr->SourcePort);
 103                 pack->Remote.AddrType = Interface->Type;
 104                 pack->Length = len;
 105                 memcpy(pack->Data, hdr->Data, len);
 106
 107                 // Add the packet to the channel's queue
 108                 SHORTLOCK(&chan->lQueue);
 109                 if(chan->Queue)
 110                         chan->QueueEnd->Next = pack;
 111                 else
 112                         chan->QueueEnd = chan->Queue = pack;
 113                 SHORTREL(&chan->lQueue);
 114                 VFS_MarkAvaliable(&chan->Node, 1);
 115                 Mutex_Release(&glUDP_Channels);
 116                 return 1;
 117         }
 118         return 0;
 119 }
 120
 121 /**
 122  * \fn void UDP_GetPacket(tInterface *Interface, void *Address, int Length, void *Buffer)
 123  * \brief Handles a packet from the IP Layer
 124  */
 125 void UDP_GetPacket(tInterface *Interface, void *Address, int Length, void *Buffer)
 126 {
 127         tUDPHeader      *hdr = Buffer;
 128
 129         #if 1
 130         size_t len = strlen( IPStack_PrintAddress(Interface->Type, Address) );
 131         char    tmp[len+1];
 132         strcpy(tmp, IPStack_PrintAddress(Interface->Type, Address));
 133         Log_Debug("UDP", "%i bytes %s:%i -> %s:%i (Cksum 0x%04x)",
 134                 ntohs(hdr->Length),
 135                 tmp, ntohs(hdr->SourcePort),
 136                 IPStack_PrintAddress(Interface->Type, Interface->Address), ntohs(hdr->DestPort),
 137                 ntohs(hdr->Checksum));
 138         #endif
 139
 140         // Check registered connections
 141         Mutex_Acquire(&glUDP_Channels);
 142         UDP_int_ScanList(gpUDP_Channels, Interface, Address, Length, Buffer);
 143         Mutex_Release(&glUDP_Channels);
 144 }
 145
 146 /**
 147  * \brief Handle an ICMP Unrechable Error
 148  */
 149 void UDP_Unreachable(tInterface *Interface, int Code, void *Address, int Length, void *Buffer)
 150 {
 151
 152 }
 153
 154 /**
 155  * \brief Send a packet
 156  * \param Channel       Channel to send the packet from
 157  * \param Data  Packet data
 158  * \param Length        Length in bytes of packet data
 159  */
 160 void UDP_SendPacketTo(tUDPChannel *Channel, int AddrType, const void *Address, Uint16 Port, const void *Data, size_t Length)
 161 {
 162         tUDPHeader      hdr;
 163
 164         if(Channel->Interface && Channel->Interface->Type != AddrType) {
 165                 LOG("Bad interface type for channel packet, IF is %i, but packet is %i",
 166                         Channel->Interface->Type, AddrType);
 167                 return ;
 168         }
 169
 170         // Create the packet
 171         hdr.SourcePort = htons( Channel->LocalPort );
 172         hdr.DestPort = htons( Port );
 173         hdr.Length = htons( sizeof(tUDPHeader) + Length );
 174         hdr.Checksum = 0;
 175         hdr.Checksum = htons( UDP_int_MakeChecksum(Channel->Interface, Address, &hdr, Length, Data) );
 176
 177         tIPStackBuffer  *buffer;
 178         switch(AddrType)
 179         {
 180         case 4:
 181                 // Pass on the the IPv4 Layer
 182                 buffer = IPStack_Buffer_CreateBuffer(2 + IPV4_BUFFERS);
 183                 IPStack_Buffer_AppendSubBuffer(buffer, Length, 0, Data, NULL, NULL);
 184                 IPStack_Buffer_AppendSubBuffer(buffer, sizeof(hdr), 0, &hdr, NULL, NULL);
 185                 // TODO: What if Channel->Interface is NULL here?
 186                 ASSERT(Channel->Interface);
 187                 IPv4_SendPacket(Channel->Interface, *(tIPv4*)Address, IP4PROT_UDP, 0, buffer);
 188                 break;
 189         default:
 190                 Log_Warning("UDP", "TODO: Implement on proto %i", AddrType);
 191                 break;
 192         }
 193 }
 194
 195 // --- Client Channels
 196 tVFS_Node *UDP_Channel_Init(tInterface *Interface)
 197 {
 198         tUDPChannel     *new;
 199         new = calloc( sizeof(tUDPChannel), 1 );
 200         new->Interface = Interface;
 201         new->Node.Size = -1;
 202         new->Node.ImplPtr = new;
 203         new->Node.NumACLs = 1;
 204         new->Node.ACLs = &gVFS_ACL_EveryoneRW;
 205         new->Node.Type = &gUDP_NodeType;
 206
 207         Mutex_Acquire(&glUDP_Channels);
 208         new->Next = gpUDP_Channels;
 209         gpUDP_Channels = new;
 210         Mutex_Release(&glUDP_Channels);
 211
 212         return &new->Node;
 213 }
 214
 215 tUDPPacket *UDP_Channel_WaitForPacket(tUDPChannel *chan, Uint VFSFlags)
 216 {
 217         // EVIL - Yield until queue is created (avoids races)
 218         while(chan->Queue == NULL)
 219                 Threads_Yield();
 220
 221         for(;;)
 222         {
 223                 tTime   timeout_z = 0, *timeout = (VFSFlags & VFS_IOFLAG_NOBLOCK) ? &timeout_z : NULL;
 224                 int rv = VFS_SelectNode(&chan->Node, VFS_SELECT_READ, timeout, "UDP_Channel_Read");
 225                 if( rv == 0 ) {
 226                         errno = (VFSFlags & VFS_IOFLAG_NOBLOCK) ? EWOULDBLOCK : EINTR;
 227                         return NULL;
 228                 }
 229                 SHORTLOCK(&chan->lQueue);
 230                 if(chan->Queue == NULL) {
 231                         SHORTREL(&chan->lQueue);
 232                         continue;
 233                 }
 234                 tUDPPacket *pack = chan->Queue;
 235                 chan->Queue = pack->Next;
 236                 if(!chan->Queue) {
 237                         chan->QueueEnd = NULL;
 238                         VFS_MarkAvaliable(&chan->Node, 0);      // Nothing left
 239                 }
 240                 SHORTREL(&chan->lQueue);
 241                 return pack;
 242         }
 243         // Unreachable
 244 }
 245
 246 /**
 247  * \brief Read from the channel file (wait for a packet)
 248  */
 249 size_t UDP_Channel_Read(tVFS_Node *Node, off_t Offset, size_t Length, void *Buffer, Uint Flags)
 250 {
 251         tUDPChannel     *chan = Node->ImplPtr;
 252
 253         if(chan->LocalPort == 0) {
 254                 Log_Notice("UDP", "Channel %p sent with no local port", chan);
 255                 return 0;
 256         }
 257
 258         tUDPPacket      *pack = UDP_Channel_WaitForPacket(chan, Flags);
 259         if( !pack ) {
 260                 return 0;
 261         }
 262
 263         size_t addrlen = IPStack_GetAddressSize(pack->Remote.AddrType);
 264         tUDPEndpoint *ep = Buffer;
 265         size_t ofs = 4 + addrlen;
 266
 267         // Check that the header fits
 268         if(Length < ofs) {
 269                 free(pack);
 270                 Log_Notice("UDP", "Insuficient space for header in buffer (%i < %i)", (int)Length, ofs);
 271                 return 0;
 272         }
 273
 274         // Fill header
 275         ep->Port = pack->Remote.Port;
 276         ep->AddrType = pack->Remote.AddrType;
 277         memcpy(&ep->Addr, &pack->Remote.Addr, addrlen);
 278
 279         // Copy packet data
 280         if(Length > ofs + pack->Length) Length = ofs + pack->Length;
 281         memcpy((char*)Buffer + ofs, pack->Data, Length - ofs);
 282
 283         // Free cached packet
 284         free(pack);
 285
 286         return Length;
 287 }
 288
 289 /**
 290  * \brief Write to the channel file (send a packet)
 291  */
 292 size_t UDP_Channel_Write(tVFS_Node *Node, off_t Offset, size_t Length, const void *Buffer, Uint Flags)
 293 {
 294         tUDPChannel     *chan = Node->ImplPtr;
 295         const tUDPEndpoint      *ep;
 296         const void      *data;
 297          int    ofs;
 298
 299         if(chan->LocalPort == 0) {
 300                 Log_Notice("UDP", "Write to channel %p with zero local port", chan);
 301                 return 0;
 302         }
 303
 304         ep = Buffer;
 305         ofs = 2 + 2 + IPStack_GetAddressSize( ep->AddrType );
 306
 307         data = (const char *)Buffer + ofs;
 308
 309         UDP_SendPacketTo(chan, ep->AddrType, &ep->Addr, ep->Port, data, (size_t)Length - ofs);
 310
 311         return Length;
 312 }
 313
 314 /**
 315  * \brief Names for channel IOCtl Calls
 316  */
 317 static const char *casIOCtls_Channel[] = {
 318         DRV_IOCTLNAMES,
 319         "getset_localport",
 320         "getset_remoteport",
 321         "getset_remotemask",
 322         "set_remoteaddr",
 323         "sendto",
 324         "recvfrom",
 325         NULL
 326         };
 327 /**
 328  * \brief Channel IOCtls
 329  */
 330 int UDP_Channel_IOCtl(tVFS_Node *Node, int ID, void *Data)
 331 {
 332         tUDPChannel     *chan = Node->ImplPtr;
 333         ENTER("pNode iID pData", Node, ID, Data);
 334         switch(ID)
 335         {
 336         BASE_IOCTLS(DRV_TYPE_MISC, "UDP Channel", 0x100, casIOCtls_Channel);
 337
 338         case 4: { // getset_localport (returns bool success)
 339                 if(!Data)       LEAVE_RET('i', chan->LocalPort);
 340                 if(!CheckMem( Data, sizeof(Uint16) ) ) {
 341                         LOG("Invalid pointer %p", Data);
 342                         LEAVE_RET('i', -1);
 343                 }
 344                 // Set port
 345                 int req_port = *(Uint16*)Data;
 346                 // Permissions check (Ports lower than 1024 are root-only)
 347                 if(req_port != 0 && req_port < 1024) {
 348                         if( Threads_GetUID() != 0 ) {
 349                                 LOG("Attempt by non-superuser to listen on port %i", req_port);
 350                                 LEAVE_RET('i', -1);
 351                         }
 352                 }
 353                 // Allocate a random port if requested
 354                 if( req_port == 0 )
 355                         UDP_int_AllocatePort(chan);
 356                 // Else, mark the requested port as used
 357                 else if( UDP_int_ClaimPort(chan, req_port) ) {
 358                         LOG("Port %i is currently in use", req_port);
 359                         LEAVE_RET('i', 0);
 360                 }
 361                 LEAVE_RET('i', chan->LocalPort);
 362                 }
 363
 364         case 5: // getset_remoteport (returns bool success)
 365                 if(!Data)       LEAVE_RET('i', chan->Remote.Port);
 366                 if(!CheckMem( Data, sizeof(Uint16) ) ) {
 367                         LOG("Invalid pointer %p", Data);
 368                         LEAVE_RET('i', -1);
 369                 }
 370                 chan->Remote.Port = *(Uint16*)Data;
 371                 LEAVE('i', chan->Remote.Port);
 372                 return chan->Remote.Port;
 373
 374         case 6: // getset_remotemask (returns bool success)
 375                 if(!Data)       LEAVE_RET('i', chan->RemoteMask);
 376                 if(!CheckMem(Data, sizeof(int))) {
 377                         LOG("Data pointer invalid");
 378                         LEAVE_RET('i', -1);
 379                 }
 380                 if( !chan->Interface ) {
 381                         LOG("Can't set remote mask on NULL interface");
 382                         LEAVE_RET('i', -1);
 383                 }
 384                  int    mask = *(int*)Data;
 385                  int    addr_bits = IPStack_GetAddressSize(chan->Interface->Type) * 8;
 386                 if( mask > addr_bits ) {
 387                         LOG("Mask too large (%i > max %i)", mask, addr_bits);
 388                         LEAVE_RET('i', -1);
 389                 }
 390                 chan->RemoteMask = mask;
 391                 LEAVE('i', chan->RemoteMask);
 392                 return chan->RemoteMask;
 393
 394         case 7: // set_remoteaddr (returns bool success)
 395                 if( !chan->Interface ) {
 396                         LOG("Can't set remote address on NULL interface");
 397                         LEAVE_RET('i', -1);
 398                 }
 399                 if(!CheckMem(Data, IPStack_GetAddressSize(chan->Interface->Type))) {
 400                         LOG("Invalid pointer");
 401                         LEAVE_RET('i', -1);
 402                 }
 403                 LOG("Set remote addr %s", IPStack_PrintAddress(chan->Interface->Type, Data));
 404                 chan->Remote.AddrType = chan->Interface->Type;
 405                 memcpy(&chan->Remote.Addr, Data, IPStack_GetAddressSize(chan->Interface->Type));
 406                 LEAVE('i', 0);
 407                 return 0;
 408         case 8: {       // sendto
 409                 if(!CheckMem(Data, 2*sizeof(void*)+2)) {
 410                         LOG("Data pointer invalid");
 411                         LEAVE_RET('i', -1);
 412                 }
 413                 const struct sSendToArgs {
 414                         const tUDPEndpoint* ep;
 415                         const void* buf;
 416                         const Uint16 buflen;
 417                 } info = *(const struct sSendToArgs*)Data;
 418                 LOG("sendto(buf=%p + %u, ep=%p)", info.buf, info.buflen, info.ep);
 419                 if(!CheckMem(info.ep, 2+2) || !CheckMem(info.ep, 2+2+IPStack_GetAddressSize(info.ep->AddrType)) ) {
 420                         LEAVE_RET('i', -1);
 421                 }
 422                 if(!CheckMem(info.buf, info.buflen)) {
 423                         LEAVE_RET('i', -1);
 424                 }
 425
 426                 UDP_SendPacketTo(chan, info.ep->AddrType, &info.ep->Addr, info.ep->Port,
 427                         info.buf, (size_t)info.buflen);
 428
 429                 LEAVE_RET('i', info.buflen); }
 430         case 9: {       // recvfrom
 431                 if(!CheckMem(Data, 2*sizeof(void*)+2)) {
 432                         LOG("Data pointer invalid");
 433                         LEAVE_RET('i', -1);
 434                 }
 435                 const struct sRecvFromArgs {
 436                         tUDPEndpoint* ep;
 437                         void* buf;
 438                         Uint16 buflen;
 439                 } info = *(const struct sRecvFromArgs*)Data;
 440                 LOG("recvfrom(buf=%p + %u, ep=%p)", info.buf, info.buflen, info.ep);
 441                 if(!CheckMem(info.ep, 2+2)) {
 442                         LEAVE_RET('i', -1);
 443                 }
 444                 if(!CheckMem(info.buf, info.buflen)) {
 445                         LEAVE_RET('i', -1);
 446                 }
 447
 448                 tUDPPacket      *pack = UDP_Channel_WaitForPacket(chan, 0);
 449                 if( pack == NULL ) {
 450                         LOG("No packet");
 451                         LEAVE_RET('i', 0);
 452                 }
 453
 454                 size_t  addrsize = IPStack_GetAddressSize(pack->Remote.AddrType);
 455                 if( !CheckMem(info.ep, 2+2+addrsize) ) {
 456                         LOG("Insufficient space for source address");
 457                         free(pack);
 458                         LEAVE_RET('i', -1);
 459                 }
 460                 info.ep->Port = pack->Remote.Port;
 461                 info.ep->AddrType = pack->Remote.AddrType;
 462                 memcpy(&info.ep->Addr, &pack->Remote.Addr, addrsize);
 463
 464                 size_t  retlen = (info.buflen < pack->Length ? info.buflen : pack->Length);
 465                 memcpy(info.buf, pack->Data, retlen);
 466
 467                 free(pack);
 468
 469                 LEAVE_RET('i', retlen); }
 470         }
 471         LEAVE_RET('i', 0);
 472 }
 473
 474 /**
 475  * \brief Close and destroy an open channel
 476  */
 477 void UDP_Channel_Close(tVFS_Node *Node)
 478 {
 479         tUDPChannel     *chan = Node->ImplPtr;
 480         tUDPChannel     *prev;
 481
 482         // Remove from the main list first
 483         Mutex_Acquire(&glUDP_Channels);
 484         if(gpUDP_Channels == chan)
 485                 gpUDP_Channels = gpUDP_Channels->Next;
 486         else
 487         {
 488                 for(prev = gpUDP_Channels;
 489                         prev->Next && prev->Next != chan;
 490                         prev = prev->Next);
 491                 if(!prev->Next)
 492                         Log_Warning("UDP", "Bookeeping Fail, channel %p is not in main list", chan);
 493                 else
 494                         prev->Next = prev->Next->Next;
 495         }
 496         Mutex_Release(&glUDP_Channels);
 497
 498         // Clear Queue
 499         SHORTLOCK(&chan->lQueue);
 500         while(chan->Queue)
 501         {
 502                 tUDPPacket      *tmp;
 503                 tmp = chan->Queue;
 504                 chan->Queue = tmp->Next;
 505                 free(tmp);
 506         }
 507         SHORTREL(&chan->lQueue);
 508
 509         // Free channel structure
 510         free(chan);
 511 }
 512
 513 /**
 514  * \return Port Number on success, or zero on failure
 515  */
 516 Uint16 UDP_int_AllocatePort(tUDPChannel *Channel)
 517 {
 518         Mutex_Acquire(&glUDP_Ports);
 519         // Fast Search
 520         for( int base = UDP_ALLOC_BASE; base < 0x10000; base += 32 )
 521         {
 522                 if( gUDP_Ports[base/32] == 0xFFFFFFFF )
 523                         continue ;
 524                 for( int i = 0; i < 32; i++ )
 525                 {
 526                         if( gUDP_Ports[base/32] & (1 << i) )
 527                                 continue ;
 528                         gUDP_Ports[base/32] |= (1 << i);
 529                         Mutex_Release(&glUDP_Ports);
 530                         // If claim succeeds, good
 531                         if( UDP_int_ClaimPort(Channel, base + i) == 0 )
 532                                 return base + i;
 533                         // otherwise keep looking
 534                         Mutex_Acquire(&glUDP_Ports);
 535                         break;
 536                 }
 537         }
 538         Mutex_Release(&glUDP_Ports);
 539         return 0;
 540 }
 541
 542 /**
 543  * \brief Allocate a specific port
 544  * \return Boolean Success
 545  */
 546 int UDP_int_ClaimPort(tUDPChannel *Channel, Uint16 Port)
 547 {
 548         // Search channel list for a connection with same (or wildcard)
 549         // interface, and same port
 550         Mutex_Acquire(&glUDP_Channels);
 551         for( tUDPChannel *ch = gpUDP_Channels; ch; ch = ch->Next)
 552         {
 553                 if( ch == Channel )
 554                         continue ;
 555                 if( ch->Interface && ch->Interface != Channel->Interface )
 556                         continue ;
 557                 if( ch->LocalPort != Port )
 558                         continue ;
 559                 Mutex_Release(&glUDP_Channels);
 560                 return 1;
 561         }
 562         Channel->LocalPort = Port;
 563         Mutex_Release(&glUDP_Channels);
 564         return 0;
 565 }
 566
 567 /**
 568  * \brief Free an allocated port
 569  */
 570 void UDP_int_FreePort(Uint16 Port)
 571 {
 572         Mutex_Acquire(&glUDP_Ports);
 573         gUDP_Ports[Port/32] &= ~(1 << (Port%32));
 574         Mutex_Release(&glUDP_Ports);
 575 }
 576
 577 /**
 578  *
 579  */
 580 Uint16 UDP_int_MakeChecksum(tInterface *Interface, const void *Dest, tUDPHeader *Hdr, size_t Len, const void *Data)
 581 {
 582         size_t  addrsize = IPStack_GetAddressSize(Interface->Type);
 583         struct {
 584                 Uint8   Zeroes;
 585                 Uint8   Protocol;
 586                 Uint16  UDPLength;
 587         } pheader;
 588
 589         pheader.Zeroes = 0;
 590         switch(Interface->Type)
 591         {
 592         case 4: pheader.Protocol = IP4PROT_UDP; break;
 593         //case 6:       pheader.Protocol = IP6PROT_UDP; break;
 594         default:
 595                 Log_Warning("UDP", "Unimplemented _MakeChecksum proto %i", Interface->Type);
 596                 return 0;
 597         }
 598         pheader.UDPLength = Hdr->Length;
 599
 600         Uint16  csum = 0;
 601         csum = UDP_int_PartialChecksum(csum, addrsize, Interface->Address);
 602         csum = UDP_int_PartialChecksum(csum, addrsize, Dest);
 603         csum = UDP_int_PartialChecksum(csum, sizeof(pheader), &pheader);
 604         csum = UDP_int_PartialChecksum(csum, sizeof(tUDPHeader), Hdr);
 605         csum = UDP_int_PartialChecksum(csum, Len, Data);
 606
 607         return UDP_int_FinaliseChecksum(csum);
 608 }
 609
 610 static inline Uint16 _add_ones_complement16(Uint16 a, Uint16 b)
 611 {
 612         // One's complement arithmatic, overflows increment bottom bit
 613         return a + b + (b > 0xFFFF - a ? 1 : 0);
 614 }
 615
 616 Uint16 UDP_int_PartialChecksum(Uint16 Prev, size_t Len, const void *Data)
 617 {
 618         Uint16  ret = Prev;
 619         const Uint16    *data = Data;
 620         for( int i = 0; i < Len/2; i ++ )
 621                 ret = _add_ones_complement16(ret, htons(*data++));
 622         if( Len % 2 == 1 )
 623                 ret = _add_ones_complement16(ret, htons(*(const Uint8*)data));
 624         return ret;
 625 }
 626
 627 Uint16 UDP_int_FinaliseChecksum(Uint16 Value)
 628 {
 629         Value = ~Value; // One's complement it
 630         return (Value == 0 ? 0xFFFF : Value);
 631 }