643624a278f4e95d282665dd330094ba88affe8a
[tpg/acess2.git] / Usermode / Applications / dhcpclient_src / main.c
1 /*
2  */
3 #include <unistd.h>
4 #include <stdint.h>
5 #include <stdio.h>
6 #include <string.h>
7 #include <net.h>
8
9 #define FILENAME_MAX    255
10 // --- Translation functions ---
11 static inline uint32_t htonl(uint32_t v)
12 {
13         return    (((v >> 24) & 0xFF) <<  0)
14                 | (((v >> 16) & 0xFF) <<  8)
15                 | (((v >>  8) & 0xFF) << 16)
16                 | (((v >>  0) & 0xFF) << 24);
17 }
18 static inline uint16_t htons(uint16_t v)
19 {
20         return    (((v >> 8) & 0xFF) <<  0)
21                 | (((v >> 0) & 0xFF) <<  8);
22 }
23 #define htonb(v)        v
24 #define ntohl(v)        htonl(v)
25 #define ntohs(v)        htons(v)
26
27 // === CONSTANTS ===
28 enum eStates
29 {
30         STATE_PREINIT,
31         STATE_DISCOVER_SENT,
32         STATE_REQUEST_SENT,
33         STATE_COMPLETE
34 };
35
36 // === STRUCTURES ===
37 #define DHCP_MAGIC      0x63825363
38 struct sDHCP_Message
39 {
40         uint8_t op;
41         uint8_t htype;  // 1 = Ethernet
42         uint8_t hlen;   // 6 bytes for MAC
43         uint8_t hops;   // Hop counting
44         uint32_t        xid;
45         uint16_t        secs;
46         uint16_t        flags;
47         uint32_t        ciaddr;
48         uint32_t        yiaddr;
49         uint32_t        siaddr;
50         uint32_t        giaddr;
51         uint8_t chaddr[16];
52         uint8_t sname[64];
53         uint8_t file[128];
54         uint32_t        dhcp_magic;     // 0x63 0x82 0x53 0x63
55         uint8_t options[];
56 };
57
58 typedef struct sInterface
59 {
60         struct sInterface       *Next;
61         char    *Adapter;
62          int    State;
63          int    Num;
64          int    SocketFD;
65          int    IfaceFD;
66 } tInterface;
67
68 // === PROTOTYPES ===
69 int     main(int argc, char *argv[]);
70 void    Scan_Dir(tInterface **IfaceList, const char *Directory);
71 int     Start_Interface(tInterface *Iface);
72 void    Send_DHCPDISCOVER(tInterface *Iface);
73 void    Send_DHCPREQUEST(tInterface *Iface, void *OfferBuffer, int TypeOffset);
74 int     Handle_Packet(tInterface *Iface);
75 void    SetAddress(tInterface *Iface, void *Addr, void *Mask, void *Router);
76
77 // === CODE ===
78 int main(int argc, char *argv[])
79 {
80         tInterface      *ifaces = NULL, *i;
81
82         // TODO: Scan /Devices and search for network adapters
83         if( argc > 2 ) {
84                 fprintf(stderr, "Usage: %s <interface>\n", argv[0]);
85                 return -1;
86         }
87         
88         if( argc == 2 ) {
89                 ifaces = malloc(sizeof(tInterface));
90                 ifaces->Next = NULL;
91                 ifaces->Adapter = argv[1];
92         }
93         else {
94                 Scan_Dir( &ifaces, "/Devices" );
95         }
96
97         for( i = ifaces; i; i = i->Next )
98         {
99                 i->State = STATE_PREINIT;
100                 if( Start_Interface(i) < 0 ) {
101                         return -1;
102                 }
103                 
104                 // Send request
105                 Send_DHCPDISCOVER(i);
106         }
107
108         while( ifaces ) 
109         {
110                  int    maxfd;
111                 fd_set  fds;
112                 tInterface      *p;
113         
114                 maxfd = 0;
115                 FD_ZERO(&fds);
116                 for( i = ifaces; i; i = i->Next )
117                 {
118                         FD_SET(i->SocketFD, &fds);
119                         if(maxfd < i->SocketFD) maxfd = i->SocketFD;
120                 }
121                 if( select(maxfd+1, &fds, NULL, NULL, NULL) < 0 )
122                 {
123                         // TODO: Check error result
124                         return -1;
125                 }
126
127                 // Check for changes (with magic to allow inline deletion)
128                 for( p = (void*)&ifaces, i = ifaces; i; p=i,i = i->Next )
129                 {
130                         if( FD_ISSET(i->SocketFD, &fds) )
131                         {
132                                 if( Handle_Packet( i ) )
133                                 {
134                                         close(i->SocketFD);
135                                         close(i->IfaceFD);
136                                         p->Next = i->Next;
137                                         free(i);
138                                         i = p;
139                                 }
140                         }
141                 }
142         }
143         return 0;
144 }
145
146 void Scan_Dir(tInterface **IfaceList, const char *Directory)
147 {
148         int dp = open(Directory, OPENFLAG_READ);
149         char filename[FILENAME_MAX];
150
151         while( readdir(dp, filename) )
152         {
153                  int    pathlen = strlen(Directory) + 1 + strlen(filename) + 1;
154                 char    path[pathlen];
155                  int    fd;
156                 t_sysFInfo      info;
157
158                 sprintf(path, "%s/%s", Directory, filename);
159                 fd = open(path, 0);
160
161                 // Check if the device type is 9 (Network)
162                 if( ioctl(fd, 0, NULL) != 9 )
163                         continue ;
164
165                 // Check if it's a directory
166                 finfo(fd, &info, 0);
167                 if( info.flags & FILEFLAG_DIRECTORY )
168                 {
169                         // If so, recurse
170                         Scan_Dir(IfaceList, path);
171                 }
172                 else
173                 {
174                         // Otherwise, add it to the list
175                         tInterface      *new = malloc(sizeof(tInterface) + pathlen);
176                         new->Adapter = (void*)(new + 1);
177                         strcpy(new->Adapter, path);
178                         new->Next = *IfaceList;
179                         *IfaceList = new;
180                 }
181         }
182         close(dp);
183 }
184
185 // RETURN: Client FD
186 int Start_Interface(tInterface *Iface)
187 {
188          int    fd, tmp;
189         char    path[] = "/Devices/ip/XXXXX/udpc";
190         char    addr[4] = {0,0,0,0};
191         
192         // TODO: Check that the adapter is not in use
193         
194         // Initialise an interface, with a dummy IP address (zero)
195         fd = open("/Devices/ip", 0);
196         if( fd == -1 ) {
197                 fprintf(stderr, "ERROR: Unable to open '/Devices/ip'\n"); 
198                 return -1;
199         }
200         Iface->Num = ioctl(fd, 4, (void*)Iface->Adapter);       // Create interface
201         if( Iface->Num == -1 ) {
202                 fprintf(stderr, "ERROR: Unable to create new interface\n");
203                 return -1;
204         }
205         close(fd);
206         
207         // Open new interface
208         snprintf(path, sizeof(path), "/Devices/ip/%i", Iface->Num);
209         Iface->IfaceFD = fd = open(path, 0);
210         if( fd == -1 ) {
211                 fprintf(stderr, "ERROR: Unable to open '%s'\n", path); 
212                 return -1;
213         }
214         tmp = 4; ioctl(fd, 4, &tmp);    // Set to IPv4
215         ioctl(fd, 6, addr);     // Set address to 0.0.0.0
216         tmp = 0; ioctl(fd, 7, &tmp);    // Set subnet mask to 0
217
218         // Open UDP Client
219         snprintf(path, sizeof(path), "/Devices/ip/%i/udp", Iface->Num);
220         Iface->SocketFD = fd = open(path, O_RDWR);
221         if( fd == -1 ) {
222                 fprintf(stderr, "ERROR: Unable to open '%s'\n", path); 
223                 return -1;
224         }
225         tmp = 68; ioctl(fd, 4, &tmp);   // Local port
226         tmp = 67; ioctl(fd, 5, &tmp);   // Remote port
227         tmp = 0;        ioctl(fd, 7, &tmp);     // Remote addr mask - we don't care where the reply comes from
228         addr[0] = addr[1] = addr[2] = addr[3] = 255;    // 255.255.255.255
229         ioctl(fd, 8, addr);     // Remote address
230         
231         return fd;
232 }
233
234 void Send_DHCPDISCOVER(tInterface *Iface)
235 {
236         uint32_t        transaction_id;
237         struct sDHCP_Message    *msg;
238         char    data[8 + sizeof(struct sDHCP_Message) + 3 + 1];
239         msg = (void*)data + 8;
240         
241         transaction_id = rand();
242
243         msg->op    = htonb(1);  // BOOTREQUEST
244         msg->htype = htonb(1);  // 10mb Ethernet
245         msg->hlen  = htonb(6);  // 6 byte MAC
246         msg->hops  = htonb(0);  // Hop count so far
247         msg->xid   = htonl(transaction_id);     // Transaction ID
248         msg->secs  = htons(0);  // secs - No time has elapsed
249         msg->flags = htons(0);  // flags - TODO: Check if broadcast bit need be set
250         msg->ciaddr = htonl(0); // ciaddr - Zero, as we don't have one yet
251         msg->yiaddr = htonl(0); // yiaddr - Zero?
252         msg->siaddr = htonl(0); // siaddr - Zero? maybe -1
253         msg->giaddr = htonl(0); // giaddr - Zero?
254         // Request MAC address from network adapter
255         {
256                 int fd = open(Iface->Adapter, 0);
257                 // TODO: Check if open() failed
258                 ioctl(fd, 4, msg->chaddr);
259                 // TODO: Check if ioctl() failed
260                 close(fd);
261         }
262         memset(msg->sname, 0, sizeof(msg->sname));      // Nuke the rest
263         memset(msg->file, 0, sizeof(msg->file));        // Nuke the rest
264         msg->dhcp_magic = htonl(DHCP_MAGIC);
265
266         int i = 0;
267         msg->options[i++] =  53;        // DHCP Message Type
268         msg->options[i++] =   1;
269         msg->options[i++] =   1;        // - DHCPDISCOVER
270         msg->options[i++] = 255;        // End of list
271         
272
273         data[0] = 67;   data[1] = 0;    // Port
274         data[2] = 4;    data[3] = 0;    // AddrType
275         data[4] = 255;  data[5] = 255;  data[6] = 255;  data[7] = 255;
276
277         write(Iface->SocketFD, data, sizeof(data));
278         Iface->State = STATE_DISCOVER_SENT;
279 }
280
281 void Send_DHCPREQUEST(tInterface *Iface, void *OfferPacket, int TypeOffset)
282 {
283         struct sDHCP_Message    *msg;
284         msg = (void*) ((char*)OfferPacket) + 8;
285
286         // Reuses old data :)
287         msg->op = 1;
288         msg->options[TypeOffset+2] = 3; // DHCPREQUEST
289         msg->options[TypeOffset+3] = 255;
290         
291         write(Iface->SocketFD, OfferPacket, 8 + sizeof(*msg) + TypeOffset + 4);
292         Iface->State = STATE_REQUEST_SENT;
293 }
294
295 int Handle_Packet(tInterface *Iface)
296 {
297         char    data[8+576];
298         struct sDHCP_Message    *msg = (void*)(data + 8);
299          int    len, i;
300         
301          int    dhcp_msg_type = 0, dhcp_msg_type_ofs;
302         void    *router = NULL;
303         void    *subnet_mask = NULL;
304         
305         _SysDebug("Doing read on %i", Iface->SocketFD);
306         len = read(Iface->SocketFD, data, sizeof(data));
307         _SysDebug("len = %i", len);
308
309         _SysDebug("*msg = {");
310         _SysDebug("  .op = %i", msg->op);
311         _SysDebug("  .htype = %i", msg->htype);
312         _SysDebug("  .ciaddr = 0x%x", ntohl(msg->ciaddr));
313         _SysDebug("  .yiaddr = 0x%x", ntohl(msg->yiaddr));
314         _SysDebug("}");
315         if( msg->op != 2 ) {
316                 // Not a response
317                 _SysDebug("Not a response message");
318                 return 0;
319         }
320
321         if( htonl(msg->dhcp_magic) != DHCP_MAGIC ) {
322                 _SysDebug("DHCP magic doesn't match (got 0x%x, expected 0x%x)",
323                         htonl(msg->dhcp_magic), DHCP_MAGIC);
324                 return 0;
325         }       
326
327         i = 0;
328         while( i < len - sizeof(*msg) - 8 && msg->options[i] != 255 )
329         {
330                 if( msg->options[i] == 0 ) {
331                         i ++;
332                         continue ;
333                 }
334                 _SysDebug("Option %i, %i bytes long", msg->options[i], msg->options[i+1]);
335                 switch(msg->options[i])
336                 {
337                 case 1:
338                         subnet_mask = &msg->options[i+2];
339                         break;
340                 case 3:
341                         router = &msg->options[i+2];
342                         break;
343                 case 53:
344                         dhcp_msg_type_ofs = i;
345                         dhcp_msg_type = msg->options[i+2];
346                         break;
347                 }
348                 i += msg->options[i+1]+2;
349         }
350         
351         _SysDebug("dhcp_msg_type = %i", dhcp_msg_type);
352
353         switch( dhcp_msg_type )
354         {
355         case 1: // DHCPDISCOVER - wut?
356                 break;
357         case 2: // DHCPOFFER
358                 // Send out request for this address
359                 if( Iface->State != STATE_DISCOVER_SENT )       return 0;
360                 Send_DHCPREQUEST(Iface, data, dhcp_msg_type_ofs);
361                 break;
362         case 3: // DHCPREQUEST - wut?
363                 break;
364         case 4: // DHCPDECLINE - ?
365                 break;
366         case 5: // DHCPACK
367                 // TODO: Apply address
368                 SetAddress(Iface, &msg->yiaddr, subnet_mask, router);
369                 return 1;
370         }
371         return 0;
372 }
373
374 void SetAddress(tInterface *Iface, void *Addr, void *Mask, void *Router)
375 {
376          int    mask_bits = 0;  
377
378         // Translate the mask
379         if( Mask )
380         {
381                 uint8_t *mask = Mask;
382                  int    i;
383                 _SysDebug("Mask %i.%i.%i.%i", mask[0], mask[1], mask[2], mask[3]);
384                 for( i = 0; i < 4 && mask[i] == 0xFF; i ++ ) ;
385                 mask_bits = i*8;
386                 if( i == 4 )
387                 {
388                         // Wut? /32?
389                 }
390                 else
391                 {
392                         switch(mask[i])
393                         {
394                         case 0x00:      mask_bits += 0; break;
395                         case 0x80:      mask_bits += 1; break;
396                         case 0xC0:      mask_bits += 2; break;
397                         case 0xE0:      mask_bits += 3; break;
398                         case 0xF0:      mask_bits += 4; break;
399                         case 0xF8:      mask_bits += 5; break;
400                         case 0xFC:      mask_bits += 6; break;
401                         case 0xFE:      mask_bits += 7; break;
402                         default:
403                                 // Bad mask!
404                                 break;
405                         }
406                 }
407         }
408         
409         {
410                 uint8_t *addr = Addr;
411                 _SysDebug("Addr %i.%i.%i.%i/%i", addr[0], addr[1], addr[2], addr[3], mask_bits);
412
413                 printf("Assigned %i.%i.%i.%i/%i to IF#%i (%s)\n",
414                         addr[0], addr[1], addr[2], addr[3], mask_bits,
415                         Iface->Num, Iface->Adapter
416                         );
417         }
418
419         ioctl(Iface->IfaceFD, 6, Addr);
420         ioctl(Iface->IfaceFD, 7, &mask_bits);
421
422         if( Router );
423         {
424                 uint8_t *addr = Router;
425                 _SysDebug("Router %i.%i.%i.%i", addr[0], addr[1], addr[2], addr[3]);
426                 
427                 // Set default route
428                  int    fd;
429                 fd = open("/Devices/ip/routes/4:00000000:0:0", OPENFLAG_CREATE);
430                 if(fd == -1) {
431                         fprintf(stderr, "ERROR: Unable to open default route\n");
432                 }
433                 else {
434                         char ifname[snprintf(NULL,0,"%i",Iface->Num)+1];
435                         sprintf(ifname, "%i", Iface->Num);
436                         ioctl(fd, ioctl(fd, 3, "set_nexthop"), Router);
437                         ioctl(fd, ioctl(fd, 3, "set_interface"), ifname);
438                         close(fd);
439                 }
440         }
441 }
442

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