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 // --- Listening Server
  15 tVFS_Node       *UDP_Server_Init(tInterface *Interface);
  16 char    *UDP_Server_ReadDir(tVFS_Node *Node, int ID);
  17 tVFS_Node       UDP_Server_FindDir(tVFS_Node *Node, char *Name);
  18  int    UDP_Server_IOCtl(tVFS_Node *Node, int ID, void *Data);
  19 void    UDP_Server_Close(tVFS_Node *Node);
  20 // --- Client Channels
  21 tVFS_Node       *UDP_Channel_Init(tInterface *Interface);
  22 Uint64  UDP_Channel_Read(tVFS_Node *Node, Uint64 Offset, Uint64 Length, void *Buffer);
  23 Uint64  UDP_Channel_Write(tVFS_Node *Node, Uint64 Offset, Uint64 Length, void *Buffer);
  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();
  28  int    UDP_int_MarkPortAsUsed(Uint16 Port);
  29 void    UDP_int_FreePort(Uint16 Port);
  30
  31 // === GLOBALS ===
  32 tSpinlock       glUDP_Channels;
  33 tUDPChannel     *gpUDP_Channels;
  34 tSpinlock       glUDP_Ports;
  35 Uint32  gUDP_Ports[0x10000/32];
  36 //tSocketFile   gUDP_ServerFile = {NULL, "udps", UDP_Server_Init};
  37 tSocketFile     gUDP_ClientFile = {NULL, "udpc", UDP_Channel_Init};
  38
  39 // === CODE ===
  40 /**
  41  * \fn void TCP_Initialise()
  42  * \brief Initialise the TCP Layer
  43  */
  44 void UDP_Initialise()
  45 {
  46         IPStack_AddFile(&gUDP_ClientFile);
  47         IPv4_RegisterCallback(IP4PROT_UDP, UDP_GetPacket);
  48 }
  49
  50 /**
  51  * \fn void UDP_GetPacket(tInterface *Interface, void *Address, int Length, void *Buffer)
  52  * \brief Handles a packet from the IP Layer
  53  */
  54 void UDP_GetPacket(tInterface *Interface, void *Address, int Length, void *Buffer)
  55 {
  56         tUDPHeader      *hdr = Buffer;
  57         tUDPChannel     *chan;
  58         tUDPPacket      *pack;
  59          int    len;
  60
  61         Log("[UDP  ] hdr->SourcePort = %i", ntohs(hdr->SourcePort));
  62         Log("[UDP  ] hdr->DestPort = %i", ntohs(hdr->DestPort));
  63         Log("[UDP  ] hdr->Length = %i", ntohs(hdr->Length));
  64         Log("[UDP  ] hdr->Checksum = 0x%x", ntohs(hdr->Checksum));
  65
  66         // Check registered connections
  67         LOCK(&glUDP_Channels);
  68         for(chan = gpUDP_Channels;
  69                 chan;
  70                 chan = chan->Next)
  71         {
  72                 if(chan->Interface != Interface)        continue;
  73                 if(chan->LocalPort != ntohs(hdr->DestPort))     continue;
  74                 if(chan->RemotePort != ntohs(hdr->SourcePort))  continue;
  75
  76                 if(Interface->Type == 4) {
  77                         if(IP4_EQU(chan->RemoteAddr.v4, *(tIPv4*)Address))      continue;
  78                 }
  79                 else if(Interface->Type == 6) {
  80                         if(IP6_EQU(chan->RemoteAddr.v6, *(tIPv6*)Address))      continue;
  81                 }
  82                 else {
  83                         Warning("[UDP  ] Address type %i unknown", Interface->Type);
  84                         RELEASE(&glUDP_Channels);
  85                         return ;
  86                 }
  87
  88                 // Create the cached packet
  89                 len = ntohs(hdr->Length);
  90                 pack = malloc(sizeof(tUDPPacket) + len);
  91                 pack->Next = NULL;
  92                 pack->Length = len;
  93                 memcpy(pack->Data, hdr->Data, len);
  94
  95                 // Add the packet to the channel's queue
  96                 LOCK(&chan->lQueue);
  97                 if(chan->Queue)
  98                         chan->QueueEnd->Next = pack;
  99                 else
 100                         chan->QueueEnd = chan->Queue = pack;
 101                 RELEASE(&chan->lQueue);
 102                 RELEASE(&glUDP_Channels);
 103                 return ;
 104         }
 105
 106         // TODO: Server/Listener
 107
 108         RELEASE(&glUDP_Channels);
 109 }
 110
 111 /**
 112  * \brief Send a packet
 113  * \param Channel       Channel to send the packet from
 114  * \param Data  Packet data
 115  * \param Length        Length in bytes of packet data
 116  */
 117 void UDP_SendPacket(tUDPChannel *Channel, void *Data, size_t Length)
 118 {
 119         tUDPHeader      *hdr;
 120
 121         switch(Channel->Interface->Type)
 122         {
 123         case 4:
 124                 // Create the packet
 125                 hdr = malloc(sizeof(tUDPHeader)+Length);
 126                 hdr->SourcePort = htons( Channel->LocalPort );
 127                 hdr->DestPort = htons( Channel->RemotePort );
 128                 hdr->Length = htons( sizeof(tUDPHeader) + Length );
 129                 hdr->Checksum = 0;      // Checksum can be zero on IPv4
 130                 memcpy(hdr->Data, Data, Length);
 131                 // Pass on the the IPv4 Layer
 132                 IPv4_SendPacket(Channel->Interface, Channel->RemoteAddr.v4, IP4PROT_UDP, 0, sizeof(tUDPHeader)+Length, hdr);
 133                 // Free allocated packet
 134                 free(hdr);
 135                 break;
 136         }
 137 }
 138
 139 // --- Client Channels
 140 tVFS_Node *UDP_Channel_Init(tInterface *Interface)
 141 {
 142         tUDPChannel     *new;
 143         new = calloc( sizeof(tUDPChannel), 1 );
 144         new->Interface = Interface;
 145         new->Node.ImplPtr = new;
 146         new->Node.NumACLs = 1;
 147         new->Node.ACLs = &gVFS_ACL_EveryoneRW;
 148         new->Node.Read = UDP_Channel_Read;
 149         new->Node.Write = UDP_Channel_Write;
 150         new->Node.IOCtl = UDP_Channel_IOCtl;
 151         new->Node.Close = UDP_Channel_Close;
 152
 153         LOCK(&glUDP_Channels);
 154         new->Next = gpUDP_Channels;
 155         gpUDP_Channels = new;
 156         RELEASE(&glUDP_Channels);
 157
 158         return &new->Node;
 159 }
 160
 161 /**
 162  * \brief Read from the channel file (wait for a packet)
 163  */
 164 Uint64 UDP_Channel_Read(tVFS_Node *Node, Uint64 Offset, Uint64 Length, void *Buffer)
 165 {
 166         tUDPChannel     *chan = Node->ImplPtr;
 167         tUDPPacket      *pack;
 168
 169         if(chan->LocalPort == 0)        return 0;
 170         if(chan->RemotePort == 0)       return 0;
 171
 172         while(chan->Queue == NULL)      Threads_Yield();
 173
 174         for(;;)
 175         {
 176                 LOCK(&chan->lQueue);
 177                 if(chan->Queue == NULL) {
 178                         RELEASE(&chan->lQueue);
 179                         continue;
 180                 }
 181                 pack = chan->Queue;
 182                 chan->Queue = pack->Next;
 183                 if(!chan->Queue)        chan->QueueEnd = NULL;
 184                 RELEASE(&chan->lQueue);
 185                 break;
 186         }
 187
 188         // Clip length to packet length
 189         if(Length > pack->Length)       Length = pack->Length;
 190         // Copy packet data from cache
 191         memcpy(Buffer, pack->Data, Length);
 192         // Free cached packet
 193         free(pack);
 194
 195         return Length;
 196 }
 197
 198 /**
 199  * \brief Write to the channel file (send a packet)
 200  */
 201 Uint64 UDP_Channel_Write(tVFS_Node *Node, Uint64 Offset, Uint64 Length, void *Buffer)
 202 {
 203         tUDPChannel     *chan = Node->ImplPtr;
 204         if(chan->RemotePort == 0)       return 0;
 205
 206         UDP_SendPacket(chan, Buffer, (size_t)Length);
 207
 208         return 0;
 209 }
 210
 211 /**
 212  * \brief Names for channel IOCtl Calls
 213  */
 214 static const char *casIOCtls_Channel[] = {
 215         DRV_IOCTLNAMES,
 216         "getset_localport",
 217         "getset_remoteport",
 218         "set_remoteaddr",
 219         NULL
 220         };
 221 /**
 222  * \brief Channel IOCtls
 223  */
 224 int UDP_Channel_IOCtl(tVFS_Node *Node, int ID, void *Data)
 225 {
 226         tUDPChannel     *chan = Node->ImplPtr;
 227         ENTER("pNode iID pData", Node, ID, Data);
 228         switch(ID)
 229         {
 230         BASE_IOCTLS(DRV_TYPE_MISC, "UDP Channel", 0x100, casIOCtls_Channel);
 231
 232         case 4: // getset_localport (returns bool success)
 233                 if(!Data)       LEAVE_RET('i', chan->LocalPort);
 234                 if(!CheckMem( Data, sizeof(Uint16) ) ) {
 235                         LOG("Invalid pointer %p", Data);
 236                         LEAVE_RET('i', -1);
 237                 }
 238                 // Set port
 239                 chan->LocalPort = *(Uint16*)Data;
 240                 // Permissions check (Ports lower than 1024 are root-only)
 241                 if(chan->LocalPort != 0 && chan->LocalPort < 1024) {
 242                         if( Threads_GetUID() != 0 ) {
 243                                 LOG("Attempt by non-superuser to listen on port %i", chan->LocalPort);
 244                                 chan->LocalPort = 0;
 245                                 LEAVE_RET('i', -1);
 246                         }
 247                 }
 248                 // Allocate a random port if requested
 249                 if( chan->LocalPort == 0 )
 250                         chan->LocalPort = UDP_int_AllocatePort();
 251                 else
 252                 {
 253                         // Else, mark the requested port as used
 254                         if( UDP_int_MarkPortAsUsed(chan->LocalPort) == 0 ) {
 255                                 LOG("Port %i us currently in use", chan->LocalPort);
 256                                 chan->LocalPort = 0;
 257                                 LEAVE_RET('i', 0);
 258                         }
 259                         LEAVE_RET('i', 1);
 260                 }
 261                 LEAVE_RET('i', 1);
 262
 263         case 5: // getset_remoteport (returns bool success)
 264                 if(!Data)       LEAVE_RET('i', chan->RemotePort);
 265                 if(!CheckMem( Data, sizeof(Uint16) ) ) {
 266                         LOG("Invalid pointer %p", Data);
 267                         LEAVE_RET('i', -1);
 268                 }
 269                 chan->RemotePort = *(Uint16*)Data;
 270                 return 1;
 271
 272         case 6: // set_remoteaddr (returns bool success)
 273                 switch(chan->Interface->Type)
 274                 {
 275                 case 4:
 276                         if(!CheckMem(Data, sizeof(tIPv4))) {
 277                                 LOG("Invalid pointer %p", Data);
 278                                 LEAVE_RET('i', -1);
 279                         }
 280                         chan->RemoteAddr.v4 = *(tIPv4*)Data;
 281                         break;
 282                 }
 283                 break;
 284         }
 285         LEAVE_RET('i', 0);
 286 }
 287
 288 /**
 289  * \brief Close and destroy an open channel
 290  */
 291 void UDP_Channel_Close(tVFS_Node *Node)
 292 {
 293         tUDPChannel     *chan = Node->ImplPtr;
 294         tUDPChannel     *prev;
 295
 296         // Remove from the main list first
 297         LOCK(&glUDP_Channels);
 298         if(gpUDP_Channels == chan)
 299                 gpUDP_Channels = gpUDP_Channels->Next;
 300         else
 301         {
 302                 for(prev = gpUDP_Channels;
 303                         prev->Next && prev->Next != chan;
 304                         prev = prev->Next);
 305                 if(!prev->Next)
 306                         Warning("[UDP  ] Bookeeping Fail, channel %p is not in main list", chan);
 307                 else
 308                         prev->Next = prev->Next->Next;
 309         }
 310         RELEASE(&glUDP_Channels);
 311
 312         // Clear Queue
 313         LOCK(&chan->lQueue);
 314         while(chan->Queue)
 315         {
 316                 tUDPPacket      *tmp;
 317                 tmp = chan->Queue;
 318                 chan->Queue = tmp->Next;
 319                 free(tmp);
 320         }
 321         RELEASE(&chan->lQueue);
 322
 323         // Free channel structure
 324         free(chan);
 325 }
 326
 327 /**
 328  * \return Port Number on success, or zero on failure
 329  */
 330 Uint16 UDP_int_AllocatePort()
 331 {
 332          int    i;
 333         LOCK(&glUDP_Ports);
 334         // Fast Search
 335         for( i = UDP_ALLOC_BASE; i < 0x10000; i += 32 )
 336                 if( gUDP_Ports[i/32] != 0xFFFFFFFF )
 337                         break;
 338         if(i == 0x10000)        return 0;
 339         for( ;; i++ )
 340         {
 341                 if( !(gUDP_Ports[i/32] & (1 << (i%32))) )
 342                         return i;
 343         }
 344         RELEASE(&glUDP_Ports);
 345 }
 346
 347 /**
 348  * \brief Allocate a specific port
 349  * \return Boolean Success
 350  */
 351 int UDP_int_MarkPortAsUsed(Uint16 Port)
 352 {
 353         LOCK(&glUDP_Ports);
 354         if( gUDP_Ports[Port/32] & (1 << (Port%32)) ) {
 355                 return 0;
 356                 RELEASE(&glUDP_Ports);
 357         }
 358         gUDP_Ports[Port/32] |= 1 << (Port%32);
 359         RELEASE(&glUDP_Ports);
 360         return 1;
 361 }
 362
 363 /**
 364  * \brief Free an allocated port
 365  */
 366 void UDP_int_FreePort(Uint16 Port)
 367 {
 368         LOCK(&glUDP_Ports);
 369         gUDP_Ports[Port/32] &= ~(1 << (Port%32));
 370         RELEASE(&glUDP_Ports);
 371 }