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         .Read = UDP_Channel_Read,
  37         .Write = UDP_Channel_Write,
  38         .IOCtl = UDP_Channel_IOCtl,
  39         .Close = UDP_Channel_Close
  40 };
  41 tMutex  glUDP_Channels;
  42 tUDPChannel     *gpUDP_Channels;
  43
  44 tMutex  glUDP_Ports;
  45 Uint32  gUDP_Ports[0x10000/32];
  46
  47 tSocketFile     gUDP_SocketFile = {NULL, "udp", UDP_Channel_Init};
  48
  49 // === CODE ===
  50 /**
  51  * \fn void TCP_Initialise()
  52  * \brief Initialise the TCP Layer
  53  */
  54 void UDP_Initialise()
  55 {
  56         IPStack_AddFile(&gUDP_SocketFile);
  57         //IPv4_RegisterCallback(IP4PROT_UDP, UDP_GetPacket, UDP_Unreachable);
  58         IPv4_RegisterCallback(IP4PROT_UDP, UDP_GetPacket);
  59 }
  60
  61 /**
  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
  64  */
  65 int UDP_int_ScanList(tUDPChannel *List, tInterface *Interface, void *Address, int Length, void *Buffer)
  66 {
  67         tUDPHeader      *hdr = Buffer;
  68         tUDPChannel     *chan;
  69         tUDPPacket      *pack;
  70          int    len;
  71
  72         for(chan = List; chan; chan = chan->Next)
  73         {
  74                 // Match local endpoint
  75                 if(chan->Interface && chan->Interface != Interface)     continue;
  76                 if(chan->LocalPort != ntohs(hdr->DestPort))     continue;
  77
  78                 // Check for remote port restriction
  79                 if(chan->Remote.Port && chan->Remote.Port != ntohs(hdr->SourcePort))
  80                         continue;
  81                 // Check for remote address restriction
  82                 if(chan->RemoteMask)
  83                 {
  84                         if(chan->Remote.AddrType != Interface->Type)
  85                                 continue;
  86                         if(!IPStack_CompareAddress(Interface->Type, Address,
  87                                 &chan->Remote.Addr, chan->RemoteMask)
  88                                 )
  89                                 continue;
  90                 }
  91
  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);
  96                 pack->Next = NULL;
  97                 memcpy(&pack->Remote.Addr, Address, IPStack_GetAddressSize(Interface->Type));
  98                 pack->Remote.Port = ntohs(hdr->SourcePort);
  99                 pack->Remote.AddrType = Interface->Type;
 100                 pack->Length = len;
 101                 memcpy(pack->Data, hdr->Data, len);
 102
 103                 // Add the packet to the channel's queue
 104                 SHORTLOCK(&chan->lQueue);
 105                 if(chan->Queue)
 106                         chan->QueueEnd->Next = pack;
 107                 else
 108                         chan->QueueEnd = chan->Queue = pack;
 109                 SHORTREL(&chan->lQueue);
 110                 VFS_MarkAvaliable(&chan->Node, 1);
 111                 Mutex_Release(&glUDP_Channels);
 112                 return 1;
 113         }
 114         return 0;
 115 }
 116
 117 /**
 118  * \fn void UDP_GetPacket(tInterface *Interface, void *Address, int Length, void *Buffer)
 119  * \brief Handles a packet from the IP Layer
 120  */
 121 void UDP_GetPacket(tInterface *Interface, void *Address, int Length, void *Buffer)
 122 {
 123         tUDPHeader      *hdr = Buffer;
 124
 125         #if 1
 126         size_t len = strlen( IPStack_PrintAddress(Interface->Type, Address) );
 127         char    tmp[len+1];
 128         strcpy(tmp, IPStack_PrintAddress(Interface->Type, Address));
 129         Log_Debug("UDP", "%i bytes %s:%i -> %s:%i (Cksum 0x%04x)",
 130                 ntohs(hdr->Length),
 131                 tmp, ntohs(hdr->SourcePort),
 132                 IPStack_PrintAddress(Interface->Type, Interface->Address), ntohs(hdr->DestPort),
 133                 ntohs(hdr->Checksum));
 134         #endif
 135
 136         // Check registered connections
 137         Mutex_Acquire(&glUDP_Channels);
 138         UDP_int_ScanList(gpUDP_Channels, Interface, Address, Length, Buffer);
 139         Mutex_Release(&glUDP_Channels);
 140 }
 141
 142 /**
 143  * \brief Handle an ICMP Unrechable Error
 144  */
 145 void UDP_Unreachable(tInterface *Interface, int Code, void *Address, int Length, void *Buffer)
 146 {
 147
 148 }
 149
 150 /**
 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
 155  */
 156 void UDP_SendPacketTo(tUDPChannel *Channel, int AddrType, const void *Address, Uint16 Port, const void *Data, size_t Length)
 157 {
 158         tUDPHeader      hdr;
 159
 160         if(Channel->Interface && Channel->Interface->Type != AddrType)  return ;
 161
 162         // Create the packet
 163         hdr.SourcePort = htons( Channel->LocalPort );
 164         hdr.DestPort = htons( Port );
 165         hdr.Length = htons( sizeof(tUDPHeader) + Length );
 166         hdr.Checksum = 0;
 167         hdr.Checksum = htons( UDP_int_MakeChecksum(Channel->Interface, Address, &hdr, Length, Data) );
 168
 169         tIPStackBuffer  *buffer;
 170         switch(AddrType)
 171         {
 172         case 4:
 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);
 179                 break;
 180         default:
 181                 Log_Warning("UDP", "TODO: Implement on proto %i", AddrType);
 182                 break;
 183         }
 184 }
 185
 186 // --- Client Channels
 187 tVFS_Node *UDP_Channel_Init(tInterface *Interface)
 188 {
 189         tUDPChannel     *new;
 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;
 196
 197         Mutex_Acquire(&glUDP_Channels);
 198         new->Next = gpUDP_Channels;
 199         gpUDP_Channels = new;
 200         Mutex_Release(&glUDP_Channels);
 201
 202         return &new->Node;
 203 }
 204
 205 /**
 206  * \brief Read from the channel file (wait for a packet)
 207  */
 208 size_t UDP_Channel_Read(tVFS_Node *Node, off_t Offset, size_t Length, void *Buffer, Uint Flags)
 209 {
 210         tUDPChannel     *chan = Node->ImplPtr;
 211         tUDPPacket      *pack;
 212         tUDPEndpoint    *ep;
 213          int    ofs, addrlen;
 214
 215         if(chan->LocalPort == 0) {
 216                 Log_Notice("UDP", "Channel %p sent with no local port", chan);
 217                 return 0;
 218         }
 219
 220         while(chan->Queue == NULL)      Threads_Yield();
 221
 222         for(;;)
 223         {
 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");
 226                 if( rv ) {
 227                         errno = (Flags & VFS_IOFLAG_NOBLOCK) ? EWOULDBLOCK : EINTR;
 228                 }
 229                 SHORTLOCK(&chan->lQueue);
 230                 if(chan->Queue == NULL) {
 231                         SHORTREL(&chan->lQueue);
 232                         continue;
 233                 }
 234                 pack = chan->Queue;
 235                 chan->Queue = pack->Next;
 236                 if(!chan->Queue) {
 237                         chan->QueueEnd = NULL;
 238                         VFS_MarkAvaliable(Node, 0);     // Nothing left
 239                 }
 240                 SHORTREL(&chan->lQueue);
 241                 break;
 242         }
 243
 244         // Check that the header fits
 245         addrlen = IPStack_GetAddressSize(pack->Remote.AddrType);
 246         ep = Buffer;
 247         ofs = 4 + addrlen;
 248         if(Length < ofs) {
 249                 free(pack);
 250                 Log_Notice("UDP", "Insuficient space for header in buffer (%i < %i)", (int)Length, ofs);
 251                 return 0;
 252         }
 253
 254         // Fill header
 255         ep->Port = pack->Remote.Port;
 256         ep->AddrType = pack->Remote.AddrType;
 257         memcpy(&ep->Addr, &pack->Remote.Addr, addrlen);
 258
 259         // Copy packet data
 260         if(Length > ofs + pack->Length) Length = ofs + pack->Length;
 261         memcpy((char*)Buffer + ofs, pack->Data, Length - ofs);
 262
 263         // Free cached packet
 264         free(pack);
 265
 266         return Length;
 267 }
 268
 269 /**
 270  * \brief Write to the channel file (send a packet)
 271  */
 272 size_t UDP_Channel_Write(tVFS_Node *Node, off_t Offset, size_t Length, const void *Buffer, Uint Flags)
 273 {
 274         tUDPChannel     *chan = Node->ImplPtr;
 275         const tUDPEndpoint      *ep;
 276         const void      *data;
 277          int    ofs;
 278
 279         if(chan->LocalPort == 0) {
 280                 Log_Notice("UDP", "Write to channel %p with zero local port", chan);
 281                 return 0;
 282         }
 283
 284         ep = Buffer;
 285         ofs = 2 + 2 + IPStack_GetAddressSize( ep->AddrType );
 286
 287         data = (const char *)Buffer + ofs;
 288
 289         UDP_SendPacketTo(chan, ep->AddrType, &ep->Addr, ep->Port, data, (size_t)Length - ofs);
 290
 291         return Length;
 292 }
 293
 294 /**
 295  * \brief Names for channel IOCtl Calls
 296  */
 297 static const char *casIOCtls_Channel[] = {
 298         DRV_IOCTLNAMES,
 299         "getset_localport",
 300         "getset_remoteport",
 301         "getset_remotemask",
 302         "set_remoteaddr",
 303         NULL
 304         };
 305 /**
 306  * \brief Channel IOCtls
 307  */
 308 int UDP_Channel_IOCtl(tVFS_Node *Node, int ID, void *Data)
 309 {
 310         tUDPChannel     *chan = Node->ImplPtr;
 311         ENTER("pNode iID pData", Node, ID, Data);
 312         switch(ID)
 313         {
 314         BASE_IOCTLS(DRV_TYPE_MISC, "UDP Channel", 0x100, casIOCtls_Channel);
 315
 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);
 320                         LEAVE_RET('i', -1);
 321                 }
 322                 // Set port
 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);
 328                                 LEAVE_RET('i', -1);
 329                         }
 330                 }
 331                 // Allocate a random port if requested
 332                 if( req_port == 0 )
 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);
 337                         LEAVE_RET('i', 0);
 338                 }
 339                 LEAVE_RET('i', chan->LocalPort);
 340                 }
 341
 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);
 346                         LEAVE_RET('i', -1);
 347                 }
 348                 chan->Remote.Port = *(Uint16*)Data;
 349                 LEAVE('i', chan->Remote.Port);
 350                 return chan->Remote.Port;
 351
 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");
 357                         LEAVE_RET('i', -1);
 358                 }
 359                 if( *(int*)Data > IPStack_GetAddressSize(chan->Interface->Type) )
 360                         LEAVE_RET('i', -1);
 361                 chan->RemoteMask = *(int*)Data;
 362                 LEAVE('i', chan->RemoteMask);
 363                 return chan->RemoteMask;
 364
 365         case 7: // set_remoteaddr (returns bool success)
 366                 if( !chan->Interface ) {
 367                         LOG("Can't set remote address on NULL interface");
 368                         LEAVE_RET('i', -1);
 369                 }
 370                 if(!CheckMem(Data, IPStack_GetAddressSize(chan->Interface->Type))) {
 371                         LOG("Invalid pointer");
 372                         LEAVE_RET('i', -1);
 373                 }
 374                 memcpy(&chan->Remote.Addr, Data, IPStack_GetAddressSize(chan->Interface->Type));
 375                 LEAVE('i', 0);
 376                 return 0;
 377         }
 378         LEAVE_RET('i', 0);
 379 }
 380
 381 /**
 382  * \brief Close and destroy an open channel
 383  */
 384 void UDP_Channel_Close(tVFS_Node *Node)
 385 {
 386         tUDPChannel     *chan = Node->ImplPtr;
 387         tUDPChannel     *prev;
 388
 389         // Remove from the main list first
 390         Mutex_Acquire(&glUDP_Channels);
 391         if(gpUDP_Channels == chan)
 392                 gpUDP_Channels = gpUDP_Channels->Next;
 393         else
 394         {
 395                 for(prev = gpUDP_Channels;
 396                         prev->Next && prev->Next != chan;
 397                         prev = prev->Next);
 398                 if(!prev->Next)
 399                         Log_Warning("UDP", "Bookeeping Fail, channel %p is not in main list", chan);
 400                 else
 401                         prev->Next = prev->Next->Next;
 402         }
 403         Mutex_Release(&glUDP_Channels);
 404
 405         // Clear Queue
 406         SHORTLOCK(&chan->lQueue);
 407         while(chan->Queue)
 408         {
 409                 tUDPPacket      *tmp;
 410                 tmp = chan->Queue;
 411                 chan->Queue = tmp->Next;
 412                 free(tmp);
 413         }
 414         SHORTREL(&chan->lQueue);
 415
 416         // Free channel structure
 417         free(chan);
 418 }
 419
 420 /**
 421  * \return Port Number on success, or zero on failure
 422  */
 423 Uint16 UDP_int_AllocatePort(tUDPChannel *Channel)
 424 {
 425         Mutex_Acquire(&glUDP_Ports);
 426         // Fast Search
 427         for( int base = UDP_ALLOC_BASE; base < 0x10000; base += 32 )
 428         {
 429                 if( gUDP_Ports[base/32] == 0xFFFFFFFF )
 430                         continue ;
 431                 for( int i = 0; i < 32; i++ )
 432                 {
 433                         if( gUDP_Ports[base/32] & (1 << i) )
 434                                 continue ;
 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 )
 439                                 return base + i;
 440                         // otherwise keep looking
 441                         Mutex_Acquire(&glUDP_Ports);
 442                         break;
 443                 }
 444         }
 445         Mutex_Release(&glUDP_Ports);
 446         return 0;
 447 }
 448
 449 /**
 450  * \brief Allocate a specific port
 451  * \return Boolean Success
 452  */
 453 int UDP_int_ClaimPort(tUDPChannel *Channel, Uint16 Port)
 454 {
 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)
 459         {
 460                 if( ch == Channel )
 461                         continue ;
 462                 if( ch->Interface && ch->Interface != Channel->Interface )
 463                         continue ;
 464                 if( ch->LocalPort != Port )
 465                         continue ;
 466                 Mutex_Release(&glUDP_Channels);
 467                 return 1;
 468         }
 469         Channel->LocalPort = Port;
 470         Mutex_Release(&glUDP_Channels);
 471         return 0;
 472 }
 473
 474 /**
 475  * \brief Free an allocated port
 476  */
 477 void UDP_int_FreePort(Uint16 Port)
 478 {
 479         Mutex_Acquire(&glUDP_Ports);
 480         gUDP_Ports[Port/32] &= ~(1 << (Port%32));
 481         Mutex_Release(&glUDP_Ports);
 482 }
 483
 484 /**
 485  *
 486  */
 487 Uint16 UDP_int_MakeChecksum(tInterface *Interface, const void *Dest, tUDPHeader *Hdr, size_t Len, const void *Data)
 488 {
 489         size_t  addrsize = IPStack_GetAddressSize(Interface->Type);
 490         struct {
 491                 Uint8   Zeroes;
 492                 Uint8   Protocol;
 493                 Uint16  UDPLength;
 494         } pheader;
 495
 496         pheader.Zeroes = 0;
 497         switch(Interface->Type)
 498         {
 499         case 4: pheader.Protocol = IP4PROT_UDP; break;
 500         //case 6:       pheader.Protocol = IP6PROT_UDP; break;
 501         default:
 502                 Log_Warning("UDP", "Unimplemented _MakeChecksum proto %i", Interface->Type);
 503                 return 0;
 504         }
 505         pheader.UDPLength = Hdr->Length;
 506
 507         Uint16  csum = 0;
 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);
 513
 514         return UDP_int_FinaliseChecksum(csum);
 515 }
 516
 517 static inline Uint16 _add_ones_complement16(Uint16 a, Uint16 b)
 518 {
 519         // One's complement arithmatic, overflows increment bottom bit
 520         return a + b + (b > 0xFFFF - a ? 1 : 0);
 521 }
 522
 523 Uint16 UDP_int_PartialChecksum(Uint16 Prev, size_t Len, const void *Data)
 524 {
 525         Uint16  ret = Prev;
 526         const Uint16    *data = Data;
 527         for( int i = 0; i < Len/2; i ++ )
 528                 ret = _add_ones_complement16(ret, htons(*data++));
 529         if( Len % 2 == 1 )
 530                 ret = _add_ones_complement16(ret, htons(*(const Uint8*)data));
 531         return ret;
 532 }
 533
 534 Uint16 UDP_int_FinaliseChecksum(Uint16 Value)
 535 {
 536         Value = ~Value; // One's complement it
 537         return (Value == 0 ? 0xFFFF : Value);
 538 }