Modules/VIARhineII - Fiddling
[tpg/acess2.git] / 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         if(chan->LocalPort == 0)        return 0;
259         
260         ep = Buffer;    
261         ofs = 2 + 2 + IPStack_GetAddressSize( ep->AddrType );
262
263         data = (const char *)Buffer + ofs;
264
265         UDP_SendPacketTo(chan, ep->AddrType, &ep->Addr, ep->Port, data, (size_t)Length - ofs);
266         
267         return 0;
268 }
269
270 /**
271  * \brief Names for channel IOCtl Calls
272  */
273 static const char *casIOCtls_Channel[] = {
274         DRV_IOCTLNAMES,
275         "getset_localport",
276         "getset_remoteport",
277         "getset_remotemask",
278         "set_remoteaddr",
279         NULL
280         };
281 /**
282  * \brief Channel IOCtls
283  */
284 int UDP_Channel_IOCtl(tVFS_Node *Node, int ID, void *Data)
285 {
286         tUDPChannel     *chan = Node->ImplPtr;
287         ENTER("pNode iID pData", Node, ID, Data);
288         switch(ID)
289         {
290         BASE_IOCTLS(DRV_TYPE_MISC, "UDP Channel", 0x100, casIOCtls_Channel);
291         
292         case 4: // getset_localport (returns bool success)
293                 if(!Data)       LEAVE_RET('i', chan->LocalPort);
294                 if(!CheckMem( Data, sizeof(Uint16) ) ) {
295                         LOG("Invalid pointer %p", Data);
296                         LEAVE_RET('i', -1);
297                 }
298                 // Set port
299                 chan->LocalPort = *(Uint16*)Data;
300                 // Permissions check (Ports lower than 1024 are root-only)
301                 if(chan->LocalPort != 0 && chan->LocalPort < 1024) {
302                         if( Threads_GetUID() != 0 ) {
303                                 LOG("Attempt by non-superuser to listen on port %i", chan->LocalPort);
304                                 chan->LocalPort = 0;
305                                 LEAVE_RET('i', -1);
306                         }
307                 }
308                 // Allocate a random port if requested
309                 if( chan->LocalPort == 0 )
310                         chan->LocalPort = UDP_int_AllocatePort();
311                 else
312                 {
313                         // Else, mark the requested port as used
314                         if( UDP_int_MarkPortAsUsed(chan->LocalPort) == 0 ) {
315                                 LOG("Port %i us currently in use", chan->LocalPort);
316                                 chan->LocalPort = 0;
317                                 LEAVE_RET('i', 0);
318                         }
319                         LEAVE_RET('i', 1);
320                 }
321                 LEAVE_RET('i', 1);
322         
323         case 5: // getset_remoteport (returns bool success)
324                 if(!Data)       LEAVE_RET('i', chan->Remote.Port);
325                 if(!CheckMem( Data, sizeof(Uint16) ) ) {
326                         LOG("Invalid pointer %p", Data);
327                         LEAVE_RET('i', -1);
328                 }
329                 chan->Remote.Port = *(Uint16*)Data;
330                 return 1;
331         
332         case 6: // getset_remotemask (returns bool success)
333                 if(!Data)       LEAVE_RET('i', chan->RemoteMask);
334                 if(!CheckMem(Data, sizeof(int)))        LEAVE_RET('i', -1);
335                 if( !chan->Interface ) {
336                         LOG("Can't set remote mask on NULL interface");
337                         LEAVE_RET('i', -1);
338                 }
339                 if( *(int*)Data > IPStack_GetAddressSize(chan->Interface->Type) )
340                         LEAVE_RET('i', -1);
341                 chan->RemoteMask = *(int*)Data;
342                 return 1;       
343
344         case 7: // set_remoteaddr (returns bool success)
345                 if( !chan->Interface ) {
346                         LOG("Can't set remote address on NULL interface");
347                         LEAVE_RET('i', -1);
348                 }
349                 if(!CheckMem(Data, IPStack_GetAddressSize(chan->Interface->Type))) {
350                         LOG("Invalid pointer");
351                         LEAVE_RET('i', -1);
352                 }
353                 memcpy(&chan->Remote.Addr, Data, IPStack_GetAddressSize(chan->Interface->Type));
354                 return 0;
355         }
356         LEAVE_RET('i', 0);
357 }
358
359 /**
360  * \brief Close and destroy an open channel
361  */
362 void UDP_Channel_Close(tVFS_Node *Node)
363 {
364         tUDPChannel     *chan = Node->ImplPtr;
365         tUDPChannel     *prev;
366         
367         // Remove from the main list first
368         Mutex_Acquire(&glUDP_Channels);
369         if(gpUDP_Channels == chan)
370                 gpUDP_Channels = gpUDP_Channels->Next;
371         else
372         {
373                 for(prev = gpUDP_Channels;
374                         prev->Next && prev->Next != chan;
375                         prev = prev->Next);
376                 if(!prev->Next)
377                         Log_Warning("UDP", "Bookeeping Fail, channel %p is not in main list", chan);
378                 else
379                         prev->Next = prev->Next->Next;
380         }
381         Mutex_Release(&glUDP_Channels);
382         
383         // Clear Queue
384         SHORTLOCK(&chan->lQueue);
385         while(chan->Queue)
386         {
387                 tUDPPacket      *tmp;
388                 tmp = chan->Queue;
389                 chan->Queue = tmp->Next;
390                 free(tmp);
391         }
392         SHORTREL(&chan->lQueue);
393         
394         // Free channel structure
395         free(chan);
396 }
397
398 /**
399  * \return Port Number on success, or zero on failure
400  */
401 Uint16 UDP_int_AllocatePort()
402 {
403          int    i;
404         Mutex_Acquire(&glUDP_Ports);
405         // Fast Search
406         for( i = UDP_ALLOC_BASE; i < 0x10000; i += 32 )
407                 if( gUDP_Ports[i/32] != 0xFFFFFFFF )
408                         break;
409         if(i == 0x10000)        return 0;
410         for( ;; i++ )
411         {
412                 if( !(gUDP_Ports[i/32] & (1 << (i%32))) )
413                         return i;
414         }
415         Mutex_Release(&glUDP_Ports);
416 }
417
418 /**
419  * \brief Allocate a specific port
420  * \return Boolean Success
421  */
422 int UDP_int_MarkPortAsUsed(Uint16 Port)
423 {
424         Mutex_Acquire(&glUDP_Ports);
425         if( gUDP_Ports[Port/32] & (1 << (Port%32)) ) {
426                 return 0;
427                 Mutex_Release(&glUDP_Ports);
428         }
429         gUDP_Ports[Port/32] |= 1 << (Port%32);
430         Mutex_Release(&glUDP_Ports);
431         return 1;
432 }
433
434 /**
435  * \brief Free an allocated port
436  */
437 void UDP_int_FreePort(Uint16 Port)
438 {
439         Mutex_Acquire(&glUDP_Ports);
440         gUDP_Ports[Port/32] &= ~(1 << (Port%32));
441         Mutex_Release(&glUDP_Ports);
442 }

UCC git Repository :: git.ucc.asn.au