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 tSpinlock       glUDP_Servers;
  35 tUDPServer      *gpUDP_Servers;
  36
  37 tSpinlock       glUDP_Channels;
  38 tUDPChannel     *gpUDP_Channels;
  39
  40 tSpinlock       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 tUDPChannel's 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                         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                 LOCK(&chan->lQueue);
 100                 if(chan->Queue)
 101                         chan->QueueEnd->Next = pack;
 102                 else
 103                         chan->QueueEnd = chan->Queue = pack;
 104                 RELEASE(&chan->lQueue);
 105                 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         LOCK(&glUDP_Channels);
 128         ret = UDP_int_ScanList(gpUDP_Channels, Interface, Address, Length, Buffer);
 129         RELEASE(&glUDP_Channels);
 130         if(ret != 0)    return ;
 131
 132
 133         // TODO: Server/Listener
 134         LOCK(&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         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         LOCK(&glUDP_Servers);
 205         new->Next = gpUDP_Servers;
 206         gpUDP_Servers = new;
 207         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         LOCK( &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         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         LOCK(&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         RELEASE(&glUDP_Servers);
 341
 342
 343         LOCK(&srv->Lock);
 344         for(chan = srv->Channels;
 345                 chan;
 346                 chan = chan->Next)
 347         {
 348                 // Clear Queue
 349                 LOCK(&chan->lQueue);
 350                 while(chan->Queue)
 351                 {
 352                         tmp = chan->Queue;
 353                         chan->Queue = tmp->Next;
 354                         free(tmp);
 355                 }
 356                 RELEASE(&chan->lQueue);
 357
 358                 // Free channel structure
 359                 free(chan);
 360         }
 361         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         LOCK(&glUDP_Channels);
 381         new->Next = gpUDP_Channels;
 382         gpUDP_Channels = new;
 383         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                 LOCK(&chan->lQueue);
 404                 if(chan->Queue == NULL) {
 405                         RELEASE(&chan->lQueue);
 406                         continue;
 407                 }
 408                 pack = chan->Queue;
 409                 chan->Queue = pack->Next;
 410                 if(!chan->Queue)        chan->QueueEnd = NULL;
 411                 RELEASE(&chan->lQueue);
 412                 break;
 413         }
 414
 415         // Clip length to packet length
 416         if(Length > pack->Length)       Length = pack->Length;
 417         // Copy packet data from cache
 418         memcpy(Buffer, pack->Data, Length);
 419         // Free cached packet
 420         free(pack);
 421
 422         return Length;
 423 }
 424
 425 /**
 426  * \brief Write to the channel file (send a packet)
 427  */
 428 Uint64 UDP_Channel_Write(tVFS_Node *Node, Uint64 Offset, Uint64 Length, void *Buffer)
 429 {
 430         tUDPChannel     *chan = Node->ImplPtr;
 431         if(chan->RemotePort == 0)       return 0;
 432
 433         UDP_SendPacket(chan, Buffer, (size_t)Length);
 434
 435         return 0;
 436 }
 437
 438 /**
 439  * \brief Names for channel IOCtl Calls
 440  */
 441 static const char *casIOCtls_Channel[] = {
 442         DRV_IOCTLNAMES,
 443         "getset_localport",
 444         "getset_remoteport",
 445         "set_remoteaddr",
 446         NULL
 447         };
 448 /**
 449  * \brief Channel IOCtls
 450  */
 451 int UDP_Channel_IOCtl(tVFS_Node *Node, int ID, void *Data)
 452 {
 453         tUDPChannel     *chan = Node->ImplPtr;
 454         ENTER("pNode iID pData", Node, ID, Data);
 455         switch(ID)
 456         {
 457         BASE_IOCTLS(DRV_TYPE_MISC, "UDP Channel", 0x100, casIOCtls_Channel);
 458
 459         case 4: // getset_localport (returns bool success)
 460                 if(!Data)       LEAVE_RET('i', chan->LocalPort);
 461                 if(!CheckMem( Data, sizeof(Uint16) ) ) {
 462                         LOG("Invalid pointer %p", Data);
 463                         LEAVE_RET('i', -1);
 464                 }
 465                 // Set port
 466                 chan->LocalPort = *(Uint16*)Data;
 467                 // Permissions check (Ports lower than 1024 are root-only)
 468                 if(chan->LocalPort != 0 && chan->LocalPort < 1024) {
 469                         if( Threads_GetUID() != 0 ) {
 470                                 LOG("Attempt by non-superuser to listen on port %i", chan->LocalPort);
 471                                 chan->LocalPort = 0;
 472                                 LEAVE_RET('i', -1);
 473                         }
 474                 }
 475                 // Allocate a random port if requested
 476                 if( chan->LocalPort == 0 )
 477                         chan->LocalPort = UDP_int_AllocatePort();
 478                 else
 479                 {
 480                         // Else, mark the requested port as used
 481                         if( UDP_int_MarkPortAsUsed(chan->LocalPort) == 0 ) {
 482                                 LOG("Port %i us currently in use", chan->LocalPort);
 483                                 chan->LocalPort = 0;
 484                                 LEAVE_RET('i', 0);
 485                         }
 486                         LEAVE_RET('i', 1);
 487                 }
 488                 LEAVE_RET('i', 1);
 489
 490         case 5: // getset_remoteport (returns bool success)
 491                 if(!Data)       LEAVE_RET('i', chan->RemotePort);
 492                 if(!CheckMem( Data, sizeof(Uint16) ) ) {
 493                         LOG("Invalid pointer %p", Data);
 494                         LEAVE_RET('i', -1);
 495                 }
 496                 chan->RemotePort = *(Uint16*)Data;
 497                 return 1;
 498
 499         case 6: // set_remoteaddr (returns bool success)
 500                 switch(chan->Interface->Type)
 501                 {
 502                 case 4:
 503                         if(!CheckMem(Data, sizeof(tIPv4))) {
 504                                 LOG("Invalid pointer %p", Data);
 505                                 LEAVE_RET('i', -1);
 506                         }
 507                         chan->RemoteAddr.v4 = *(tIPv4*)Data;
 508                         break;
 509                 }
 510                 break;
 511         }
 512         LEAVE_RET('i', 0);
 513 }
 514
 515 /**
 516  * \brief Close and destroy an open channel
 517  */
 518 void UDP_Channel_Close(tVFS_Node *Node)
 519 {
 520         tUDPChannel     *chan = Node->ImplPtr;
 521         tUDPChannel     *prev;
 522
 523         // Remove from the main list first
 524         LOCK(&glUDP_Channels);
 525         if(gpUDP_Channels == chan)
 526                 gpUDP_Channels = gpUDP_Channels->Next;
 527         else
 528         {
 529                 for(prev = gpUDP_Channels;
 530                         prev->Next && prev->Next != chan;
 531                         prev = prev->Next);
 532                 if(!prev->Next)
 533                         Warning("[UDP  ] Bookeeping Fail, channel %p is not in main list", chan);
 534                 else
 535                         prev->Next = prev->Next->Next;
 536         }
 537         RELEASE(&glUDP_Channels);
 538
 539         // Clear Queue
 540         LOCK(&chan->lQueue);
 541         while(chan->Queue)
 542         {
 543                 tUDPPacket      *tmp;
 544                 tmp = chan->Queue;
 545                 chan->Queue = tmp->Next;
 546                 free(tmp);
 547         }
 548         RELEASE(&chan->lQueue);
 549
 550         // Free channel structure
 551         free(chan);
 552 }
 553
 554 /**
 555  * \return Port Number on success, or zero on failure
 556  */
 557 Uint16 UDP_int_AllocatePort()
 558 {
 559          int    i;
 560         LOCK(&glUDP_Ports);
 561         // Fast Search
 562         for( i = UDP_ALLOC_BASE; i < 0x10000; i += 32 )
 563                 if( gUDP_Ports[i/32] != 0xFFFFFFFF )
 564                         break;
 565         if(i == 0x10000)        return 0;
 566         for( ;; i++ )
 567         {
 568                 if( !(gUDP_Ports[i/32] & (1 << (i%32))) )
 569                         return i;
 570         }
 571         RELEASE(&glUDP_Ports);
 572 }
 573
 574 /**
 575  * \brief Allocate a specific port
 576  * \return Boolean Success
 577  */
 578 int UDP_int_MarkPortAsUsed(Uint16 Port)
 579 {
 580         LOCK(&glUDP_Ports);
 581         if( gUDP_Ports[Port/32] & (1 << (Port%32)) ) {
 582                 return 0;
 583                 RELEASE(&glUDP_Ports);
 584         }
 585         gUDP_Ports[Port/32] |= 1 << (Port%32);
 586         RELEASE(&glUDP_Ports);
 587         return 1;
 588 }
 589
 590 /**
 591  * \brief Free an allocated port
 592  */
 593 void UDP_int_FreePort(Uint16 Port)
 594 {
 595         LOCK(&glUDP_Ports);
 596         gUDP_Ports[Port/32] &= ~(1 << (Port%32));
 597         RELEASE(&glUDP_Ports);
 598 }