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