KernelLand/Modules/IPStack/udp.c

   1 /*
   2  * Acess2 IP Stack
   3  * - UDP Handling
   4  */
   5 #include "ipstack.h"
   6 #include <api_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_SendPacketTo(tUDPChannel *Channel, int AddrType, const void *Address, Uint16 Port, const void *Data, size_t Length);
  16 // --- Client Channels
  17 tVFS_Node       *UDP_Channel_Init(tInterface *Interface);
  18 size_t  UDP_Channel_Read(tVFS_Node *Node, off_t Offset, size_t Length, void *Buffer);
  19 size_t  UDP_Channel_Write(tVFS_Node *Node, off_t Offset, size_t Length, const void *Buffer);
  20  int    UDP_Channel_IOCtl(tVFS_Node *Node, int ID, void *Data);
  21 void    UDP_Channel_Close(tVFS_Node *Node);
  22 // --- Helpers
  23 Uint16  UDP_int_AllocatePort();
  24  int    UDP_int_MarkPortAsUsed(Uint16 Port);
  25 void    UDP_int_FreePort(Uint16 Port);
  26
  27 // === GLOBALS ===
  28 tVFS_NodeType   gUDP_NodeType = {
  29         .Read = UDP_Channel_Read,
  30         .Write = UDP_Channel_Write,
  31         .IOCtl = UDP_Channel_IOCtl,
  32         .Close = UDP_Channel_Close
  33 };
  34 tMutex  glUDP_Channels;
  35 tUDPChannel     *gpUDP_Channels;
  36
  37 tMutex  glUDP_Ports;
  38 Uint32  gUDP_Ports[0x10000/32];
  39
  40 tSocketFile     gUDP_SocketFile = {NULL, "udp", UDP_Channel_Init};
  41
  42 // === CODE ===
  43 /**
  44  * \fn void TCP_Initialise()
  45  * \brief Initialise the TCP Layer
  46  */
  47 void UDP_Initialise()
  48 {
  49         IPStack_AddFile(&gUDP_SocketFile);
  50         //IPv4_RegisterCallback(IP4PROT_UDP, UDP_GetPacket, UDP_Unreachable);
  51         IPv4_RegisterCallback(IP4PROT_UDP, UDP_GetPacket);
  52 }
  53
  54 /**
  55  * \brief Scan a list of tUDPChannels and find process the first match
  56  * \return 0 if no match was found, -1 on error and 1 if a match was found
  57  */
  58 int UDP_int_ScanList(tUDPChannel *List, tInterface *Interface, void *Address, int Length, void *Buffer)
  59 {
  60         tUDPHeader      *hdr = Buffer;
  61         tUDPChannel     *chan;
  62         tUDPPacket      *pack;
  63          int    len;
  64
  65         for(chan = List;
  66                 chan;
  67                 chan = chan->Next)
  68         {
  69                 // Match local endpoint
  70                 if(chan->Interface && chan->Interface != Interface)     continue;
  71                 if(chan->LocalPort != ntohs(hdr->DestPort))     continue;
  72
  73                 // Check for remote port restriction
  74                 if(chan->Remote.Port && chan->Remote.Port != ntohs(hdr->SourcePort))
  75                         continue;
  76                 // Check for remote address restriction
  77                 if(chan->RemoteMask)
  78                 {
  79                         if(chan->Remote.AddrType != Interface->Type)
  80                                 continue;
  81                         if(!IPStack_CompareAddress(Interface->Type, Address,
  82                                 &chan->Remote.Addr, chan->RemoteMask)
  83                                 )
  84                                 continue;
  85                 }
  86
  87                 Log_Log("UDP", "Recieved packet for %p", chan);
  88                 // Create the cached packet
  89                 len = ntohs(hdr->Length);
  90                 pack = malloc(sizeof(tUDPPacket) + len);
  91                 pack->Next = NULL;
  92                 memcpy(&pack->Remote.Addr, Address, IPStack_GetAddressSize(Interface->Type));
  93                 pack->Remote.Port = ntohs(hdr->SourcePort);
  94                 pack->Remote.AddrType = Interface->Type;
  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                 VFS_MarkAvaliable(&chan->Node, 1);
 106                 Mutex_Release(&glUDP_Channels);
 107                 return 1;
 108         }
 109         return 0;
 110 }
 111
 112 /**
 113  * \fn void UDP_GetPacket(tInterface *Interface, void *Address, int Length, void *Buffer)
 114  * \brief Handles a packet from the IP Layer
 115  */
 116 void UDP_GetPacket(tInterface *Interface, void *Address, int Length, void *Buffer)
 117 {
 118         tUDPHeader      *hdr = Buffer;
 119
 120         Log_Debug("UDP", "hdr->SourcePort = %i", ntohs(hdr->SourcePort));
 121         Log_Debug("UDP", "hdr->DestPort = %i", ntohs(hdr->DestPort));
 122         Log_Debug("UDP", "hdr->Length = %i", ntohs(hdr->Length));
 123         Log_Debug("UDP", "hdr->Checksum = 0x%x", 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                 IPStack_Buffer_DestroyBuffer(buffer);
 166                 break;
 167         }
 168 }
 169
 170 // --- Client Channels
 171 tVFS_Node *UDP_Channel_Init(tInterface *Interface)
 172 {
 173         tUDPChannel     *new;
 174         new = calloc( sizeof(tUDPChannel), 1 );
 175         new->Interface = Interface;
 176         new->Node.ImplPtr = new;
 177         new->Node.NumACLs = 1;
 178         new->Node.ACLs = &gVFS_ACL_EveryoneRW;
 179         new->Node.Type = &gUDP_NodeType;
 180
 181         Mutex_Acquire(&glUDP_Channels);
 182         new->Next = gpUDP_Channels;
 183         gpUDP_Channels = new;
 184         Mutex_Release(&glUDP_Channels);
 185
 186         return &new->Node;
 187 }
 188
 189 /**
 190  * \brief Read from the channel file (wait for a packet)
 191  */
 192 size_t UDP_Channel_Read(tVFS_Node *Node, off_t Offset, size_t Length, void *Buffer)
 193 {
 194         tUDPChannel     *chan = Node->ImplPtr;
 195         tUDPPacket      *pack;
 196         tUDPEndpoint    *ep;
 197          int    ofs, addrlen;
 198
 199         if(chan->LocalPort == 0) {
 200                 Log_Notice("UDP", "Channel %p sent with no local port", chan);
 201                 return 0;
 202         }
 203
 204         while(chan->Queue == NULL)      Threads_Yield();
 205
 206         for(;;)
 207         {
 208                 VFS_SelectNode(Node, VFS_SELECT_READ, NULL, "UDP_Channel_Read");
 209                 SHORTLOCK(&chan->lQueue);
 210                 if(chan->Queue == NULL) {
 211                         SHORTREL(&chan->lQueue);
 212                         continue;
 213                 }
 214                 pack = chan->Queue;
 215                 chan->Queue = pack->Next;
 216                 if(!chan->Queue) {
 217                         chan->QueueEnd = NULL;
 218                         VFS_MarkAvaliable(Node, 0);     // Nothing left
 219                 }
 220                 SHORTREL(&chan->lQueue);
 221                 break;
 222         }
 223
 224         // Check that the header fits
 225         addrlen = IPStack_GetAddressSize(pack->Remote.AddrType);
 226         ep = Buffer;
 227         ofs = 4 + addrlen;
 228         if(Length < ofs) {
 229                 free(pack);
 230                 Log_Notice("UDP", "Insuficient space for header in buffer (%i < %i)", (int)Length, ofs);
 231                 return 0;
 232         }
 233
 234         // Fill header
 235         ep->Port = pack->Remote.Port;
 236         ep->AddrType = pack->Remote.AddrType;
 237         memcpy(&ep->Addr, &pack->Remote.Addr, addrlen);
 238
 239         // Copy packet data
 240         if(Length > ofs + pack->Length) Length = ofs + pack->Length;
 241         memcpy((char*)Buffer + ofs, pack->Data, Length - ofs);
 242
 243         // Free cached packet
 244         free(pack);
 245
 246         return Length;
 247 }
 248
 249 /**
 250  * \brief Write to the channel file (send a packet)
 251  */
 252 size_t UDP_Channel_Write(tVFS_Node *Node, off_t Offset, size_t Length, const void *Buffer)
 253 {
 254         tUDPChannel     *chan = Node->ImplPtr;
 255         const tUDPEndpoint      *ep;
 256         const void      *data;
 257          int    ofs;
 258
 259         if(chan->LocalPort == 0) {
 260                 Log_Notice("UDP", "Write to channel %p with zero local port", chan);
 261                 return 0;
 262         }
 263
 264         ep = Buffer;
 265         ofs = 2 + 2 + IPStack_GetAddressSize( ep->AddrType );
 266
 267         data = (const char *)Buffer + ofs;
 268
 269         UDP_SendPacketTo(chan, ep->AddrType, &ep->Addr, ep->Port, data, (size_t)Length - ofs);
 270
 271         return Length;
 272 }
 273
 274 /**
 275  * \brief Names for channel IOCtl Calls
 276  */
 277 static const char *casIOCtls_Channel[] = {
 278         DRV_IOCTLNAMES,
 279         "getset_localport",
 280         "getset_remoteport",
 281         "getset_remotemask",
 282         "set_remoteaddr",
 283         NULL
 284         };
 285 /**
 286  * \brief Channel IOCtls
 287  */
 288 int UDP_Channel_IOCtl(tVFS_Node *Node, int ID, void *Data)
 289 {
 290         tUDPChannel     *chan = Node->ImplPtr;
 291         ENTER("pNode iID pData", Node, ID, Data);
 292         switch(ID)
 293         {
 294         BASE_IOCTLS(DRV_TYPE_MISC, "UDP Channel", 0x100, casIOCtls_Channel);
 295
 296         case 4: // getset_localport (returns bool success)
 297                 if(!Data)       LEAVE_RET('i', chan->LocalPort);
 298                 if(!CheckMem( Data, sizeof(Uint16) ) ) {
 299                         LOG("Invalid pointer %p", Data);
 300                         LEAVE_RET('i', -1);
 301                 }
 302                 // Set port
 303                 chan->LocalPort = *(Uint16*)Data;
 304                 // Permissions check (Ports lower than 1024 are root-only)
 305                 if(chan->LocalPort != 0 && chan->LocalPort < 1024) {
 306                         if( Threads_GetUID() != 0 ) {
 307                                 LOG("Attempt by non-superuser to listen on port %i", chan->LocalPort);
 308                                 chan->LocalPort = 0;
 309                                 LEAVE_RET('i', -1);
 310                         }
 311                 }
 312                 // Allocate a random port if requested
 313                 if( chan->LocalPort == 0 )
 314                         chan->LocalPort = UDP_int_AllocatePort();
 315                 else
 316                 {
 317                         // Else, mark the requested port as used
 318                         if( UDP_int_MarkPortAsUsed(chan->LocalPort) == 0 ) {
 319                                 LOG("Port %i us currently in use", chan->LocalPort);
 320                                 chan->LocalPort = 0;
 321                                 LEAVE_RET('i', 0);
 322                         }
 323                         LEAVE_RET('i', 1);
 324                 }
 325                 LEAVE_RET('i', 1);
 326
 327         case 5: // getset_remoteport (returns bool success)
 328                 if(!Data)       LEAVE_RET('i', chan->Remote.Port);
 329                 if(!CheckMem( Data, sizeof(Uint16) ) ) {
 330                         LOG("Invalid pointer %p", Data);
 331                         LEAVE_RET('i', -1);
 332                 }
 333                 chan->Remote.Port = *(Uint16*)Data;
 334                 return 1;
 335
 336         case 6: // getset_remotemask (returns bool success)
 337                 if(!Data)       LEAVE_RET('i', chan->RemoteMask);
 338                 if(!CheckMem(Data, sizeof(int)))        LEAVE_RET('i', -1);
 339                 if( !chan->Interface ) {
 340                         LOG("Can't set remote mask on NULL interface");
 341                         LEAVE_RET('i', -1);
 342                 }
 343                 if( *(int*)Data > IPStack_GetAddressSize(chan->Interface->Type) )
 344                         LEAVE_RET('i', -1);
 345                 chan->RemoteMask = *(int*)Data;
 346                 return 1;
 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                 return 0;
 359         }
 360         LEAVE_RET('i', 0);
 361 }
 362
 363 /**
 364  * \brief Close and destroy an open channel
 365  */
 366 void UDP_Channel_Close(tVFS_Node *Node)
 367 {
 368         tUDPChannel     *chan = Node->ImplPtr;
 369         tUDPChannel     *prev;
 370
 371         // Remove from the main list first
 372         Mutex_Acquire(&glUDP_Channels);
 373         if(gpUDP_Channels == chan)
 374                 gpUDP_Channels = gpUDP_Channels->Next;
 375         else
 376         {
 377                 for(prev = gpUDP_Channels;
 378                         prev->Next && prev->Next != chan;
 379                         prev = prev->Next);
 380                 if(!prev->Next)
 381                         Log_Warning("UDP", "Bookeeping Fail, channel %p is not in main list", chan);
 382                 else
 383                         prev->Next = prev->Next->Next;
 384         }
 385         Mutex_Release(&glUDP_Channels);
 386
 387         // Clear Queue
 388         SHORTLOCK(&chan->lQueue);
 389         while(chan->Queue)
 390         {
 391                 tUDPPacket      *tmp;
 392                 tmp = chan->Queue;
 393                 chan->Queue = tmp->Next;
 394                 free(tmp);
 395         }
 396         SHORTREL(&chan->lQueue);
 397
 398         // Free channel structure
 399         free(chan);
 400 }
 401
 402 /**
 403  * \return Port Number on success, or zero on failure
 404  */
 405 Uint16 UDP_int_AllocatePort()
 406 {
 407          int    i;
 408         Mutex_Acquire(&glUDP_Ports);
 409         // Fast Search
 410         for( i = UDP_ALLOC_BASE; i < 0x10000; i += 32 )
 411                 if( gUDP_Ports[i/32] != 0xFFFFFFFF )
 412                         break;
 413         if(i == 0x10000)        return 0;
 414         for( ;; i++ )
 415         {
 416                 if( !(gUDP_Ports[i/32] & (1 << (i%32))) )
 417                         return i;
 418         }
 419         Mutex_Release(&glUDP_Ports);
 420 }
 421
 422 /**
 423  * \brief Allocate a specific port
 424  * \return Boolean Success
 425  */
 426 int UDP_int_MarkPortAsUsed(Uint16 Port)
 427 {
 428         Mutex_Acquire(&glUDP_Ports);
 429         if( gUDP_Ports[Port/32] & (1 << (Port%32)) ) {
 430                 return 0;
 431                 Mutex_Release(&glUDP_Ports);
 432         }
 433         gUDP_Ports[Port/32] |= 1 << (Port%32);
 434         Mutex_Release(&glUDP_Ports);
 435         return 1;
 436 }
 437
 438 /**
 439  * \brief Free an allocated port
 440  */
 441 void UDP_int_FreePort(Uint16 Port)
 442 {
 443         Mutex_Acquire(&glUDP_Ports);
 444         gUDP_Ports[Port/32] &= ~(1 << (Port%32));
 445         Mutex_Release(&glUDP_Ports);
 446 }