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
  31 // === GLOBALS ===
  32 tVFS_NodeType   gUDP_NodeType = {
  33         .Read = UDP_Channel_Read,
  34         .Write = UDP_Channel_Write,
  35         .IOCtl = UDP_Channel_IOCtl,
  36         .Close = UDP_Channel_Close
  37 };
  38 tMutex  glUDP_Channels;
  39 tUDPChannel     *gpUDP_Channels;
  40
  41 tMutex  glUDP_Ports;
  42 Uint32  gUDP_Ports[0x10000/32];
  43
  44 tSocketFile     gUDP_SocketFile = {NULL, "udp", 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_SocketFile);
  54         //IPv4_RegisterCallback(IP4PROT_UDP, UDP_GetPacket, UDP_Unreachable);
  55         IPv4_RegisterCallback(IP4PROT_UDP, UDP_GetPacket);
  56 }
  57
  58 /**
  59  * \brief Scan a list of tUDPChannels and find process the first match
  60  * \return 0 if no match was found, -1 on error and 1 if a match was found
  61  */
  62 int UDP_int_ScanList(tUDPChannel *List, tInterface *Interface, void *Address, int Length, void *Buffer)
  63 {
  64         tUDPHeader      *hdr = Buffer;
  65         tUDPChannel     *chan;
  66         tUDPPacket      *pack;
  67          int    len;
  68
  69         for(chan = List; chan; chan = chan->Next)
  70         {
  71                 // Match local endpoint
  72                 if(chan->Interface && chan->Interface != Interface)     continue;
  73                 if(chan->LocalPort != ntohs(hdr->DestPort))     continue;
  74
  75                 // Check for remote port restriction
  76                 if(chan->Remote.Port && chan->Remote.Port != ntohs(hdr->SourcePort))
  77                         continue;
  78                 // Check for remote address restriction
  79                 if(chan->RemoteMask)
  80                 {
  81                         if(chan->Remote.AddrType != Interface->Type)
  82                                 continue;
  83                         if(!IPStack_CompareAddress(Interface->Type, Address,
  84                                 &chan->Remote.Addr, chan->RemoteMask)
  85                                 )
  86                                 continue;
  87                 }
  88
  89                 Log_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                 memcpy(&pack->Remote.Addr, Address, IPStack_GetAddressSize(Interface->Type));
  95                 pack->Remote.Port = ntohs(hdr->SourcePort);
  96                 pack->Remote.AddrType = Interface->Type;
  97                 pack->Length = len;
  98                 memcpy(pack->Data, hdr->Data, len);
  99
 100                 // Add the packet to the channel's queue
 101                 SHORTLOCK(&chan->lQueue);
 102                 if(chan->Queue)
 103                         chan->QueueEnd->Next = pack;
 104                 else
 105                         chan->QueueEnd = chan->Queue = pack;
 106                 SHORTREL(&chan->lQueue);
 107                 VFS_MarkAvaliable(&chan->Node, 1);
 108                 Mutex_Release(&glUDP_Channels);
 109                 return 1;
 110         }
 111         return 0;
 112 }
 113
 114 /**
 115  * \fn void UDP_GetPacket(tInterface *Interface, void *Address, int Length, void *Buffer)
 116  * \brief Handles a packet from the IP Layer
 117  */
 118 void UDP_GetPacket(tInterface *Interface, void *Address, int Length, void *Buffer)
 119 {
 120         tUDPHeader      *hdr = Buffer;
 121
 122         Log_Debug("UDP", "%i bytes :%i->:%i (Cksum 0x%04x)",
 123                 ntohs(hdr->Length), ntohs(hdr->SourcePort), ntohs(hdr->Length), ntohs(hdr->Checksum));
 124
 125         // Check registered connections
 126         Mutex_Acquire(&glUDP_Channels);
 127         UDP_int_ScanList(gpUDP_Channels, Interface, Address, Length, Buffer);
 128         Mutex_Release(&glUDP_Channels);
 129 }
 130
 131 /**
 132  * \brief Handle an ICMP Unrechable Error
 133  */
 134 void UDP_Unreachable(tInterface *Interface, int Code, void *Address, int Length, void *Buffer)
 135 {
 136
 137 }
 138
 139 /**
 140  * \brief Send a packet
 141  * \param Channel       Channel to send the packet from
 142  * \param Data  Packet data
 143  * \param Length        Length in bytes of packet data
 144  */
 145 void UDP_SendPacketTo(tUDPChannel *Channel, int AddrType, const void *Address, Uint16 Port, const void *Data, size_t Length)
 146 {
 147         tUDPHeader      hdr;
 148
 149         if(Channel->Interface && Channel->Interface->Type != AddrType)  return ;
 150
 151         switch(AddrType)
 152         {
 153         case 4:
 154                 // Create the packet
 155                 hdr.SourcePort = htons( Channel->LocalPort );
 156                 hdr.DestPort = htons( Port );
 157                 hdr.Length = htons( sizeof(tUDPHeader) + Length );
 158                 hdr.Checksum = 0;       // Checksum can be zero on IPv4
 159                 // Pass on the the IPv4 Layer
 160                 tIPStackBuffer  *buffer = IPStack_Buffer_CreateBuffer(2 + IPV4_BUFFERS);
 161                 IPStack_Buffer_AppendSubBuffer(buffer, Length, 0, Data, NULL, NULL);
 162                 IPStack_Buffer_AppendSubBuffer(buffer, sizeof(hdr), 0, &hdr, NULL, NULL);
 163                 // TODO: What if Channel->Interface is NULL here?
 164                 IPv4_SendPacket(Channel->Interface, *(tIPv4*)Address, IP4PROT_UDP, 0, buffer);
 165                 break;
 166         }
 167 }
 168
 169 // --- Client Channels
 170 tVFS_Node *UDP_Channel_Init(tInterface *Interface)
 171 {
 172         tUDPChannel     *new;
 173         new = calloc( sizeof(tUDPChannel), 1 );
 174         new->Interface = Interface;
 175         new->Node.ImplPtr = new;
 176         new->Node.NumACLs = 1;
 177         new->Node.ACLs = &gVFS_ACL_EveryoneRW;
 178         new->Node.Type = &gUDP_NodeType;
 179
 180         Mutex_Acquire(&glUDP_Channels);
 181         new->Next = gpUDP_Channels;
 182         gpUDP_Channels = new;
 183         Mutex_Release(&glUDP_Channels);
 184
 185         return &new->Node;
 186 }
 187
 188 /**
 189  * \brief Read from the channel file (wait for a packet)
 190  */
 191 size_t UDP_Channel_Read(tVFS_Node *Node, off_t Offset, size_t Length, void *Buffer, Uint Flags)
 192 {
 193         tUDPChannel     *chan = Node->ImplPtr;
 194         tUDPPacket      *pack;
 195         tUDPEndpoint    *ep;
 196          int    ofs, addrlen;
 197
 198         if(chan->LocalPort == 0) {
 199                 Log_Notice("UDP", "Channel %p sent with no local port", chan);
 200                 return 0;
 201         }
 202
 203         while(chan->Queue == NULL)      Threads_Yield();
 204
 205         for(;;)
 206         {
 207                 tTime   timeout_z = 0, *timeout = (Flags & VFS_IOFLAG_NOBLOCK) ? &timeout_z : NULL;
 208                 int rv = VFS_SelectNode(Node, VFS_SELECT_READ, timeout, "UDP_Channel_Read");
 209                 if( rv ) {
 210                         errno = (Flags & VFS_IOFLAG_NOBLOCK) ? EWOULDBLOCK : EINTR;
 211                 }
 212                 SHORTLOCK(&chan->lQueue);
 213                 if(chan->Queue == NULL) {
 214                         SHORTREL(&chan->lQueue);
 215                         continue;
 216                 }
 217                 pack = chan->Queue;
 218                 chan->Queue = pack->Next;
 219                 if(!chan->Queue) {
 220                         chan->QueueEnd = NULL;
 221                         VFS_MarkAvaliable(Node, 0);     // Nothing left
 222                 }
 223                 SHORTREL(&chan->lQueue);
 224                 break;
 225         }
 226
 227         // Check that the header fits
 228         addrlen = IPStack_GetAddressSize(pack->Remote.AddrType);
 229         ep = Buffer;
 230         ofs = 4 + addrlen;
 231         if(Length < ofs) {
 232                 free(pack);
 233                 Log_Notice("UDP", "Insuficient space for header in buffer (%i < %i)", (int)Length, ofs);
 234                 return 0;
 235         }
 236
 237         // Fill header
 238         ep->Port = pack->Remote.Port;
 239         ep->AddrType = pack->Remote.AddrType;
 240         memcpy(&ep->Addr, &pack->Remote.Addr, addrlen);
 241
 242         // Copy packet data
 243         if(Length > ofs + pack->Length) Length = ofs + pack->Length;
 244         memcpy((char*)Buffer + ofs, pack->Data, Length - ofs);
 245
 246         // Free cached packet
 247         free(pack);
 248
 249         return Length;
 250 }
 251
 252 /**
 253  * \brief Write to the channel file (send a packet)
 254  */
 255 size_t UDP_Channel_Write(tVFS_Node *Node, off_t Offset, size_t Length, const void *Buffer, Uint Flags)
 256 {
 257         tUDPChannel     *chan = Node->ImplPtr;
 258         const tUDPEndpoint      *ep;
 259         const void      *data;
 260          int    ofs;
 261
 262         if(chan->LocalPort == 0) {
 263                 Log_Notice("UDP", "Write to channel %p with zero local port", chan);
 264                 return 0;
 265         }
 266
 267         ep = Buffer;
 268         ofs = 2 + 2 + IPStack_GetAddressSize( ep->AddrType );
 269
 270         data = (const char *)Buffer + ofs;
 271
 272         UDP_SendPacketTo(chan, ep->AddrType, &ep->Addr, ep->Port, data, (size_t)Length - ofs);
 273
 274         return Length;
 275 }
 276
 277 /**
 278  * \brief Names for channel IOCtl Calls
 279  */
 280 static const char *casIOCtls_Channel[] = {
 281         DRV_IOCTLNAMES,
 282         "getset_localport",
 283         "getset_remoteport",
 284         "getset_remotemask",
 285         "set_remoteaddr",
 286         NULL
 287         };
 288 /**
 289  * \brief Channel IOCtls
 290  */
 291 int UDP_Channel_IOCtl(tVFS_Node *Node, int ID, void *Data)
 292 {
 293         tUDPChannel     *chan = Node->ImplPtr;
 294         ENTER("pNode iID pData", Node, ID, Data);
 295         switch(ID)
 296         {
 297         BASE_IOCTLS(DRV_TYPE_MISC, "UDP Channel", 0x100, casIOCtls_Channel);
 298
 299         case 4: { // getset_localport (returns bool success)
 300                 if(!Data)       LEAVE_RET('i', chan->LocalPort);
 301                 if(!CheckMem( Data, sizeof(Uint16) ) ) {
 302                         LOG("Invalid pointer %p", Data);
 303                         LEAVE_RET('i', -1);
 304                 }
 305                 // Set port
 306                 int req_port = *(Uint16*)Data;
 307                 // Permissions check (Ports lower than 1024 are root-only)
 308                 if(req_port != 0 && req_port < 1024) {
 309                         if( Threads_GetUID() != 0 ) {
 310                                 LOG("Attempt by non-superuser to listen on port %i", req_port);
 311                                 LEAVE_RET('i', -1);
 312                         }
 313                 }
 314                 // Allocate a random port if requested
 315                 if( req_port == 0 )
 316                         UDP_int_AllocatePort(chan);
 317                 // Else, mark the requested port as used
 318                 else if( UDP_int_ClaimPort(chan, req_port) ) {
 319                         LOG("Port %i is currently in use", req_port);
 320                         LEAVE_RET('i', 0);
 321                 }
 322                 LEAVE_RET('i', chan->LocalPort);
 323                 }
 324
 325         case 5: // getset_remoteport (returns bool success)
 326                 if(!Data)       LEAVE_RET('i', chan->Remote.Port);
 327                 if(!CheckMem( Data, sizeof(Uint16) ) ) {
 328                         LOG("Invalid pointer %p", Data);
 329                         LEAVE_RET('i', -1);
 330                 }
 331                 chan->Remote.Port = *(Uint16*)Data;
 332                 LEAVE('i', chan->Remote.Port);
 333                 return chan->Remote.Port;
 334
 335         case 6: // getset_remotemask (returns bool success)
 336                 if(!Data)       LEAVE_RET('i', chan->RemoteMask);
 337                 if(!CheckMem(Data, sizeof(int)))        LEAVE_RET('i', -1);
 338                 if( !chan->Interface ) {
 339                         LOG("Can't set remote mask on NULL interface");
 340                         LEAVE_RET('i', -1);
 341                 }
 342                 if( *(int*)Data > IPStack_GetAddressSize(chan->Interface->Type) )
 343                         LEAVE_RET('i', -1);
 344                 chan->RemoteMask = *(int*)Data;
 345                 LEAVE('i', chan->RemoteMask);
 346                 return chan->RemoteMask;
 347
 348         case 7: // set_remoteaddr (returns bool success)
 349                 if( !chan->Interface ) {
 350                         LOG("Can't set remote address on NULL interface");
 351                         LEAVE_RET('i', -1);
 352                 }
 353                 if(!CheckMem(Data, IPStack_GetAddressSize(chan->Interface->Type))) {
 354                         LOG("Invalid pointer");
 355                         LEAVE_RET('i', -1);
 356                 }
 357                 memcpy(&chan->Remote.Addr, Data, IPStack_GetAddressSize(chan->Interface->Type));
 358                 LEAVE('i', 0);
 359                 return 0;
 360         }
 361         LEAVE_RET('i', 0);
 362 }
 363
 364 /**
 365  * \brief Close and destroy an open channel
 366  */
 367 void UDP_Channel_Close(tVFS_Node *Node)
 368 {
 369         tUDPChannel     *chan = Node->ImplPtr;
 370         tUDPChannel     *prev;
 371
 372         // Remove from the main list first
 373         Mutex_Acquire(&glUDP_Channels);
 374         if(gpUDP_Channels == chan)
 375                 gpUDP_Channels = gpUDP_Channels->Next;
 376         else
 377         {
 378                 for(prev = gpUDP_Channels;
 379                         prev->Next && prev->Next != chan;
 380                         prev = prev->Next);
 381                 if(!prev->Next)
 382                         Log_Warning("UDP", "Bookeeping Fail, channel %p is not in main list", chan);
 383                 else
 384                         prev->Next = prev->Next->Next;
 385         }
 386         Mutex_Release(&glUDP_Channels);
 387
 388         // Clear Queue
 389         SHORTLOCK(&chan->lQueue);
 390         while(chan->Queue)
 391         {
 392                 tUDPPacket      *tmp;
 393                 tmp = chan->Queue;
 394                 chan->Queue = tmp->Next;
 395                 free(tmp);
 396         }
 397         SHORTREL(&chan->lQueue);
 398
 399         // Free channel structure
 400         free(chan);
 401 }
 402
 403 /**
 404  * \return Port Number on success, or zero on failure
 405  */
 406 Uint16 UDP_int_AllocatePort(tUDPChannel *Channel)
 407 {
 408         Mutex_Acquire(&glUDP_Ports);
 409         // Fast Search
 410         for( int base = UDP_ALLOC_BASE; base < 0x10000; base += 32 )
 411         {
 412                 if( gUDP_Ports[base/32] == 0xFFFFFFFF )
 413                         continue ;
 414                 for( int i = 0; i < 32; i++ )
 415                 {
 416                         if( gUDP_Ports[base/32] & (1 << i) )
 417                                 continue ;
 418                         gUDP_Ports[base/32] |= (1 << i);
 419                         Mutex_Release(&glUDP_Ports);
 420                         // If claim succeeds, good
 421                         if( UDP_int_ClaimPort(Channel, base + i) == 0 )
 422                                 return base + i;
 423                         // otherwise keep looking
 424                         Mutex_Acquire(&glUDP_Ports);
 425                         break;
 426                 }
 427         }
 428         Mutex_Release(&glUDP_Ports);
 429         return 0;
 430 }
 431
 432 /**
 433  * \brief Allocate a specific port
 434  * \return Boolean Success
 435  */
 436 int UDP_int_ClaimPort(tUDPChannel *Channel, Uint16 Port)
 437 {
 438         // Search channel list for a connection with same (or wildcard)
 439         // interface, and same port
 440         Mutex_Acquire(&glUDP_Channels);
 441         for( tUDPChannel *ch = gpUDP_Channels; ch; ch = ch->Next)
 442         {
 443                 if( ch == Channel )
 444                         continue ;
 445                 if( ch->Interface && ch->Interface != Channel->Interface )
 446                         continue ;
 447                 if( ch->LocalPort != Port )
 448                         continue ;
 449                 Mutex_Release(&glUDP_Channels);
 450                 return 1;
 451         }
 452         Channel->LocalPort = Port;
 453         Mutex_Release(&glUDP_Channels);
 454         return 0;
 455 }
 456
 457 /**
 458  * \brief Free an allocated port
 459  */
 460 void UDP_int_FreePort(Uint16 Port)
 461 {
 462         Mutex_Acquire(&glUDP_Ports);
 463         gUDP_Ports[Port/32] &= ~(1 << (Port%32));
 464         Mutex_Release(&glUDP_Ports);
 465 }