Modules/IPStack/udp.c

   1 /*
   2  * Acess2 IP Stack
   3  * - UDP Handling
   4  */
   5 #include "ipstack.h"
   6 #include <tpl_drv_common.h>
   7 #include "udp.h"
   8
   9 #define UDP_ALLOC_BASE  0xC000
  10
  11 // === PROTOTYPES ===
  12 void    UDP_Initialise();
  13 void    UDP_GetPacket(tInterface *Interface, void *Address, int Length, void *Buffer);
  14 // --- Listening Server
  15 tVFS_Node       *UDP_Server_Init(tInterface *Interface);
  16 char    *UDP_Server_ReadDir(tVFS_Node *Node, int ID);
  17 tVFS_Node       *UDP_Server_FindDir(tVFS_Node *Node, char *Name);
  18  int    UDP_Server_IOCtl(tVFS_Node *Node, int ID, void *Data);
  19 void    UDP_Server_Close(tVFS_Node *Node);
  20 // --- Client Channels
  21 tVFS_Node       *UDP_Channel_Init(tInterface *Interface);
  22 Uint64  UDP_Channel_Read(tVFS_Node *Node, Uint64 Offset, Uint64 Length, void *Buffer);
  23 Uint64  UDP_Channel_Write(tVFS_Node *Node, Uint64 Offset, Uint64 Length, void *Buffer);
  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();
  28  int    UDP_int_MarkPortAsUsed(Uint16 Port);
  29 void    UDP_int_FreePort(Uint16 Port);
  30
  31 // === GLOBALS ===
  32 tSpinlock       glUDP_Servers;
  33 tUDPServer      *gpUDP_Servers;
  34
  35 tSpinlock       glUDP_Channels;
  36 tUDPChannel     *gpUDP_Channels;
  37
  38 tSpinlock       glUDP_Ports;
  39 Uint32  gUDP_Ports[0x10000/32];
  40
  41 tSocketFile     gUDP_ServerFile = {NULL, "udps", UDP_Server_Init};
  42 tSocketFile     gUDP_ClientFile = {NULL, "udpc", UDP_Channel_Init};
  43
  44 // === CODE ===
  45 /**
  46  * \fn void TCP_Initialise()
  47  * \brief Initialise the TCP Layer
  48  */
  49 void UDP_Initialise()
  50 {
  51         IPStack_AddFile(&gUDP_ServerFile);
  52         IPStack_AddFile(&gUDP_ClientFile);
  53         IPv4_RegisterCallback(IP4PROT_UDP, UDP_GetPacket);
  54 }
  55
  56 /**
  57  * \brief Scan a list of tUDPChannel's and find process the first match
  58  * \return 0 if no match was found, -1 on error and 1 if a match was found
  59  */
  60 int UDP_int_ScanList(tUDPChannel *List, tInterface *Interface, void *Address, int Length, void *Buffer)
  61 {
  62         tUDPHeader      *hdr = Buffer;
  63         tUDPChannel     *chan;
  64         tUDPPacket      *pack;
  65          int    len;
  66
  67         for(chan = List;
  68                 chan;
  69                 chan = chan->Next)
  70         {
  71                 if(chan->Interface != Interface)        continue;
  72                 //Log("[UDP  ] Local (0x%04x) == Dest (0x%04x)", chan->LocalPort, ntohs(hdr->DestPort));
  73                 if(chan->LocalPort != ntohs(hdr->DestPort))     continue;
  74                 //Log("[UDP  ] Remote (0x%04x) == Source (0x%04x)", chan->RemotePort, ntohs(hdr->SourcePort));
  75                 if(chan->RemotePort != ntohs(hdr->SourcePort))  continue;
  76
  77                 if(Interface->Type == 4) {
  78                         if(!IP4_EQU(chan->RemoteAddr.v4, *(tIPv4*)Address))     continue;
  79                 }
  80                 else if(Interface->Type == 6) {
  81                         if(!IP6_EQU(chan->RemoteAddr.v6, *(tIPv6*)Address))     continue;
  82                 }
  83                 else {
  84                         Warning("[UDP  ] Address type %i unknown", Interface->Type);
  85                         RELEASE(&glUDP_Channels);
  86                         return -1;
  87                 }
  88
  89                 Log("[UDP  ] Recieved packet for %p", chan);
  90                 // Create the cached packet
  91                 len = ntohs(hdr->Length);
  92                 pack = malloc(sizeof(tUDPPacket) + len);
  93                 pack->Next = NULL;
  94                 pack->Length = len;
  95                 memcpy(pack->Data, hdr->Data, len);
  96
  97                 // Add the packet to the channel's queue
  98                 LOCK(&chan->lQueue);
  99                 if(chan->Queue)
 100                         chan->QueueEnd->Next = pack;
 101                 else
 102                         chan->QueueEnd = chan->Queue = pack;
 103                 RELEASE(&chan->lQueue);
 104                 RELEASE(&glUDP_Channels);
 105                 return 1;
 106         }
 107         return 0;
 108 }
 109
 110 /**
 111  * \fn void UDP_GetPacket(tInterface *Interface, void *Address, int Length, void *Buffer)
 112  * \brief Handles a packet from the IP Layer
 113  */
 114 void UDP_GetPacket(tInterface *Interface, void *Address, int Length, void *Buffer)
 115 {
 116         tUDPHeader      *hdr = Buffer;
 117         tUDPServer      *srv;
 118          int    ret;
 119
 120         Log("[UDP  ] hdr->SourcePort = %i", ntohs(hdr->SourcePort));
 121         Log("[UDP  ] hdr->DestPort = %i", ntohs(hdr->DestPort));
 122         Log("[UDP  ] hdr->Length = %i", ntohs(hdr->Length));
 123         Log("[UDP  ] hdr->Checksum = 0x%x", ntohs(hdr->Checksum));
 124
 125         // Check registered connections
 126         LOCK(&glUDP_Channels);
 127         ret = UDP_int_ScanList(gpUDP_Channels, Interface, Address, Length, Buffer);
 128         RELEASE(&glUDP_Channels);
 129         if(ret != 0)    return ;
 130
 131
 132         // TODO: Server/Listener
 133         LOCK(&glUDP_Servers);
 134         for(srv = gpUDP_Servers;
 135                 srv;
 136                 srv = srv->Next)
 137         {
 138                 if(srv->Interface != Interface) continue;
 139                 if(srv->ListenPort != ntohs(hdr->DestPort))     continue;
 140                 ret = UDP_int_ScanList(srv->Channels, Interface, Address, Length, Buffer);
 141                 if(ret != 0)    break;
 142
 143                 // Add connection
 144                 Warning("[UDP  ] TODO - Add channel on connection");
 145                 //TODO
 146         }
 147         RELEASE(&glUDP_Servers);
 148
 149 }
 150
 151 /**
 152  * \brief Send a packet
 153  * \param Channel       Channel to send the packet from
 154  * \param Data  Packet data
 155  * \param Length        Length in bytes of packet data
 156  */
 157 void UDP_SendPacket(tUDPChannel *Channel, void *Data, size_t Length)
 158 {
 159         tUDPHeader      *hdr;
 160
 161         switch(Channel->Interface->Type)
 162         {
 163         case 4:
 164                 // Create the packet
 165                 hdr = malloc(sizeof(tUDPHeader)+Length);
 166                 hdr->SourcePort = htons( Channel->LocalPort );
 167                 hdr->DestPort = htons( Channel->RemotePort );
 168                 hdr->Length = htons( sizeof(tUDPHeader) + Length );
 169                 hdr->Checksum = 0;      // Checksum can be zero on IPv4
 170                 memcpy(hdr->Data, Data, Length);
 171                 // Pass on the the IPv4 Layer
 172                 IPv4_SendPacket(Channel->Interface, Channel->RemoteAddr.v4, IP4PROT_UDP, 0, sizeof(tUDPHeader)+Length, hdr);
 173                 // Free allocated packet
 174                 free(hdr);
 175                 break;
 176         }
 177 }
 178
 179 // --- Listening Server
 180 tVFS_Node *UDP_Server_Init(tInterface *Interface)
 181 {
 182         tUDPServer      *new;
 183         new = calloc( sizeof(tUDPServer), 1 );
 184         if(!new)        return NULL;
 185
 186         new->Node.ImplPtr = new;
 187         new->Node.Flags = VFS_FFLAG_DIRECTORY;
 188         new->Node.NumACLs = 1;
 189         new->Node.ACLs = &gVFS_ACL_EveryoneRX;
 190         new->Node.ReadDir = UDP_Server_ReadDir;
 191         new->Node.FindDir = UDP_Server_FindDir;
 192         new->Node.IOCtl = UDP_Server_IOCtl;
 193         new->Node.Close = UDP_Server_Close;
 194
 195         LOCK(&glUDP_Servers);
 196         new->Next = gpUDP_Servers;
 197         gpUDP_Servers = new;
 198         RELEASE(&glUDP_Servers);
 199
 200         return &new->Node;
 201 }
 202
 203 /**
 204  * \brief Wait for a connection and return its ID in a string
 205  */
 206 char *UDP_Server_ReadDir(tVFS_Node *Node, int ID)
 207 {
 208         tUDPServer      *srv = Node->ImplPtr;
 209         tUDPChannel     *chan;
 210         char    *ret;
 211
 212         if( srv->ListenPort == 0 )      return NULL;
 213
 214         // Lock (so another thread can't collide with us here) and wait for a connection
 215         LOCK( &srv->Lock );
 216         while( srv->NewChannels == NULL )       Threads_Yield();
 217         // Pop the connection off the new list
 218         chan = srv->NewChannels;
 219         srv->NewChannels = chan->Next;
 220         // Release the lock
 221         RELEASE( &srv->Lock );
 222
 223         // Create the ID string and return it
 224         ret = malloc(11+1);
 225         sprintf(ret, "%i", chan->Node.ImplInt);
 226
 227         return ret;
 228 }
 229
 230 /**
 231  * \brief Take a string and find the channel
 232  */
 233 tVFS_Node *UDP_Server_FindDir(tVFS_Node *Node, char *Name)
 234 {
 235         tUDPServer      *srv = Node->ImplPtr;
 236         tUDPChannel     *chan;
 237          int    id = atoi(Name);
 238
 239         for(chan = srv->Channels;
 240                 chan;
 241                 chan = chan->Next)
 242         {
 243                 if( chan->Node.ImplInt < id )   continue;
 244                 if( chan->Node.ImplInt > id )   break;  // Go sorted lists!
 245
 246                 return &chan->Node;
 247         }
 248
 249         return NULL;
 250 }
 251
 252 /**
 253  * \brief Names for server IOCtl Calls
 254  */
 255 static const char *casIOCtls_Server[] = {
 256         DRV_IOCTLNAMES,
 257         "getset_listenport",
 258         NULL
 259         };
 260 /**
 261  * \brief Channel IOCtls
 262  */
 263 int UDP_Server_IOCtl(tVFS_Node *Node, int ID, void *Data)
 264 {
 265         tUDPServer      *srv = Node->ImplPtr;
 266
 267         ENTER("pNode iID pData", Node, ID, Data);
 268         switch(ID)
 269         {
 270         BASE_IOCTLS(DRV_TYPE_MISC, "UDP Server", 0x100, casIOCtls_Server);
 271
 272         case 4: // getset_localport (returns bool success)
 273                 if(!Data)       LEAVE_RET('i', srv->ListenPort);
 274                 if(!CheckMem( Data, sizeof(Uint16) ) ) {
 275                         LOG("Invalid pointer %p", Data);
 276                         LEAVE_RET('i', -1);
 277                 }
 278                 // Set port
 279                 srv->ListenPort = *(Uint16*)Data;
 280                 // Permissions check (Ports lower than 1024 are root-only)
 281                 if(srv->ListenPort != 0 && srv->ListenPort < 1024) {
 282                         if( Threads_GetUID() != 0 ) {
 283                                 LOG("Attempt by non-superuser to listen on port %i", srv->ListenPort);
 284                                 srv->ListenPort = 0;
 285                                 LEAVE_RET('i', -1);
 286                         }
 287                 }
 288                 // Allocate a random port if requested
 289                 if( srv->ListenPort == 0 )
 290                         srv->ListenPort = UDP_int_AllocatePort();
 291                 else
 292                 {
 293                         // Else, mark the requested port as used
 294                         if( UDP_int_MarkPortAsUsed(srv->ListenPort) == 0 ) {
 295                                 LOG("Port %i us currently in use", srv->ListenPort);
 296                                 srv->ListenPort = 0;
 297                                 LEAVE_RET('i', -1);
 298                         }
 299                         LEAVE_RET('i', 1);
 300                 }
 301                 LEAVE_RET('i', 1);
 302
 303         default:
 304                 LEAVE_RET('i', -1);
 305         }
 306         LEAVE_RET('i', 0);
 307 }
 308
 309 void UDP_Server_Close(tVFS_Node *Node)
 310 {
 311         tUDPServer      *srv = Node->ImplPtr;
 312         tUDPServer      *prev;
 313         tUDPChannel     *chan;
 314         tUDPPacket      *tmp;
 315
 316
 317         // Remove from the main list first
 318         LOCK(&glUDP_Servers);
 319         if(gpUDP_Servers == srv)
 320                 gpUDP_Servers = gpUDP_Servers->Next;
 321         else
 322         {
 323                 for(prev = gpUDP_Servers;
 324                         prev->Next && prev->Next != srv;
 325                         prev = prev->Next);
 326                 if(!prev->Next)
 327                         Warning("[UDP  ] Bookeeping Fail, server %p is not in main list", srv);
 328                 else
 329                         prev->Next = prev->Next->Next;
 330         }
 331         RELEASE(&glUDP_Servers);
 332
 333
 334         LOCK(&srv->Lock);
 335         for(chan = srv->Channels;
 336                 chan;
 337                 chan = chan->Next)
 338         {
 339                 // Clear Queue
 340                 LOCK(&chan->lQueue);
 341                 while(chan->Queue)
 342                 {
 343                         tmp = chan->Queue;
 344                         chan->Queue = tmp->Next;
 345                         free(tmp);
 346                 }
 347                 RELEASE(&chan->lQueue);
 348
 349                 // Free channel structure
 350                 free(chan);
 351         }
 352         RELEASE(&srv->Lock);
 353
 354         free(srv);
 355 }
 356
 357 // --- Client Channels
 358 tVFS_Node *UDP_Channel_Init(tInterface *Interface)
 359 {
 360         tUDPChannel     *new;
 361         new = calloc( sizeof(tUDPChannel), 1 );
 362         new->Interface = Interface;
 363         new->Node.ImplPtr = new;
 364         new->Node.NumACLs = 1;
 365         new->Node.ACLs = &gVFS_ACL_EveryoneRW;
 366         new->Node.Read = UDP_Channel_Read;
 367         new->Node.Write = UDP_Channel_Write;
 368         new->Node.IOCtl = UDP_Channel_IOCtl;
 369         new->Node.Close = UDP_Channel_Close;
 370
 371         LOCK(&glUDP_Channels);
 372         new->Next = gpUDP_Channels;
 373         gpUDP_Channels = new;
 374         RELEASE(&glUDP_Channels);
 375
 376         return &new->Node;
 377 }
 378
 379 /**
 380  * \brief Read from the channel file (wait for a packet)
 381  */
 382 Uint64 UDP_Channel_Read(tVFS_Node *Node, Uint64 Offset, Uint64 Length, void *Buffer)
 383 {
 384         tUDPChannel     *chan = Node->ImplPtr;
 385         tUDPPacket      *pack;
 386
 387         if(chan->LocalPort == 0)        return 0;
 388         if(chan->RemotePort == 0)       return 0;
 389
 390         while(chan->Queue == NULL)      Threads_Yield();
 391
 392         for(;;)
 393         {
 394                 LOCK(&chan->lQueue);
 395                 if(chan->Queue == NULL) {
 396                         RELEASE(&chan->lQueue);
 397                         continue;
 398                 }
 399                 pack = chan->Queue;
 400                 chan->Queue = pack->Next;
 401                 if(!chan->Queue)        chan->QueueEnd = NULL;
 402                 RELEASE(&chan->lQueue);
 403                 break;
 404         }
 405
 406         // Clip length to packet length
 407         if(Length > pack->Length)       Length = pack->Length;
 408         // Copy packet data from cache
 409         memcpy(Buffer, pack->Data, Length);
 410         // Free cached packet
 411         free(pack);
 412
 413         return Length;
 414 }
 415
 416 /**
 417  * \brief Write to the channel file (send a packet)
 418  */
 419 Uint64 UDP_Channel_Write(tVFS_Node *Node, Uint64 Offset, Uint64 Length, void *Buffer)
 420 {
 421         tUDPChannel     *chan = Node->ImplPtr;
 422         if(chan->RemotePort == 0)       return 0;
 423
 424         UDP_SendPacket(chan, Buffer, (size_t)Length);
 425
 426         return 0;
 427 }
 428
 429 /**
 430  * \brief Names for channel IOCtl Calls
 431  */
 432 static const char *casIOCtls_Channel[] = {
 433         DRV_IOCTLNAMES,
 434         "getset_localport",
 435         "getset_remoteport",
 436         "set_remoteaddr",
 437         NULL
 438         };
 439 /**
 440  * \brief Channel IOCtls
 441  */
 442 int UDP_Channel_IOCtl(tVFS_Node *Node, int ID, void *Data)
 443 {
 444         tUDPChannel     *chan = Node->ImplPtr;
 445         ENTER("pNode iID pData", Node, ID, Data);
 446         switch(ID)
 447         {
 448         BASE_IOCTLS(DRV_TYPE_MISC, "UDP Channel", 0x100, casIOCtls_Channel);
 449
 450         case 4: // getset_localport (returns bool success)
 451                 if(!Data)       LEAVE_RET('i', chan->LocalPort);
 452                 if(!CheckMem( Data, sizeof(Uint16) ) ) {
 453                         LOG("Invalid pointer %p", Data);
 454                         LEAVE_RET('i', -1);
 455                 }
 456                 // Set port
 457                 chan->LocalPort = *(Uint16*)Data;
 458                 // Permissions check (Ports lower than 1024 are root-only)
 459                 if(chan->LocalPort != 0 && chan->LocalPort < 1024) {
 460                         if( Threads_GetUID() != 0 ) {
 461                                 LOG("Attempt by non-superuser to listen on port %i", chan->LocalPort);
 462                                 chan->LocalPort = 0;
 463                                 LEAVE_RET('i', -1);
 464                         }
 465                 }
 466                 // Allocate a random port if requested
 467                 if( chan->LocalPort == 0 )
 468                         chan->LocalPort = UDP_int_AllocatePort();
 469                 else
 470                 {
 471                         // Else, mark the requested port as used
 472                         if( UDP_int_MarkPortAsUsed(chan->LocalPort) == 0 ) {
 473                                 LOG("Port %i us currently in use", chan->LocalPort);
 474                                 chan->LocalPort = 0;
 475                                 LEAVE_RET('i', 0);
 476                         }
 477                         LEAVE_RET('i', 1);
 478                 }
 479                 LEAVE_RET('i', 1);
 480
 481         case 5: // getset_remoteport (returns bool success)
 482                 if(!Data)       LEAVE_RET('i', chan->RemotePort);
 483                 if(!CheckMem( Data, sizeof(Uint16) ) ) {
 484                         LOG("Invalid pointer %p", Data);
 485                         LEAVE_RET('i', -1);
 486                 }
 487                 chan->RemotePort = *(Uint16*)Data;
 488                 return 1;
 489
 490         case 6: // set_remoteaddr (returns bool success)
 491                 switch(chan->Interface->Type)
 492                 {
 493                 case 4:
 494                         if(!CheckMem(Data, sizeof(tIPv4))) {
 495                                 LOG("Invalid pointer %p", Data);
 496                                 LEAVE_RET('i', -1);
 497                         }
 498                         chan->RemoteAddr.v4 = *(tIPv4*)Data;
 499                         break;
 500                 }
 501                 break;
 502         }
 503         LEAVE_RET('i', 0);
 504 }
 505
 506 /**
 507  * \brief Close and destroy an open channel
 508  */
 509 void UDP_Channel_Close(tVFS_Node *Node)
 510 {
 511         tUDPChannel     *chan = Node->ImplPtr;
 512         tUDPChannel     *prev;
 513
 514         // Remove from the main list first
 515         LOCK(&glUDP_Channels);
 516         if(gpUDP_Channels == chan)
 517                 gpUDP_Channels = gpUDP_Channels->Next;
 518         else
 519         {
 520                 for(prev = gpUDP_Channels;
 521                         prev->Next && prev->Next != chan;
 522                         prev = prev->Next);
 523                 if(!prev->Next)
 524                         Warning("[UDP  ] Bookeeping Fail, channel %p is not in main list", chan);
 525                 else
 526                         prev->Next = prev->Next->Next;
 527         }
 528         RELEASE(&glUDP_Channels);
 529
 530         // Clear Queue
 531         LOCK(&chan->lQueue);
 532         while(chan->Queue)
 533         {
 534                 tUDPPacket      *tmp;
 535                 tmp = chan->Queue;
 536                 chan->Queue = tmp->Next;
 537                 free(tmp);
 538         }
 539         RELEASE(&chan->lQueue);
 540
 541         // Free channel structure
 542         free(chan);
 543 }
 544
 545 /**
 546  * \return Port Number on success, or zero on failure
 547  */
 548 Uint16 UDP_int_AllocatePort()
 549 {
 550          int    i;
 551         LOCK(&glUDP_Ports);
 552         // Fast Search
 553         for( i = UDP_ALLOC_BASE; i < 0x10000; i += 32 )
 554                 if( gUDP_Ports[i/32] != 0xFFFFFFFF )
 555                         break;
 556         if(i == 0x10000)        return 0;
 557         for( ;; i++ )
 558         {
 559                 if( !(gUDP_Ports[i/32] & (1 << (i%32))) )
 560                         return i;
 561         }
 562         RELEASE(&glUDP_Ports);
 563 }
 564
 565 /**
 566  * \brief Allocate a specific port
 567  * \return Boolean Success
 568  */
 569 int UDP_int_MarkPortAsUsed(Uint16 Port)
 570 {
 571         LOCK(&glUDP_Ports);
 572         if( gUDP_Ports[Port/32] & (1 << (Port%32)) ) {
 573                 return 0;
 574                 RELEASE(&glUDP_Ports);
 575         }
 576         gUDP_Ports[Port/32] |= 1 << (Port%32);
 577         RELEASE(&glUDP_Ports);
 578         return 1;
 579 }
 580
 581 /**
 582  * \brief Free an allocated port
 583  */
 584 void UDP_int_FreePort(Uint16 Port)
 585 {
 586         LOCK(&glUDP_Ports);
 587         gUDP_Ports[Port/32] &= ~(1 << (Port%32));
 588         RELEASE(&glUDP_Ports);
 589 }