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