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/ip/adapters" );
  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         if( dp == -1 ) {
 152                 fprintf(stderr, "Unable to open directory '%s'\n", Directory);
 153         }
 154
 155         while( readdir(dp, filename) )
 156         {
 157                 if( filename[0] == '.' )        continue ;
 158                 if( strcmp(filename, "lo") == 0 )       continue ;
 159
 160                 tInterface *new = malloc(sizeof(tInterface) + strlen(filename)+1);
 161                 new->Adapter = (void*)(new + 1);
 162                 strcpy(new->Adapter, filename);
 163                 new->Next = *IfaceList;
 164                 *IfaceList = new;
 165         }
 166         close(dp);
 167 }
 168
 169 // RETURN: Client FD
 170 int Start_Interface(tInterface *Iface)
 171 {
 172          int    fd, tmp;
 173         char    path[] = "/Devices/ip/XXXXX/udpc";
 174         char    addr[4] = {0,0,0,0};
 175
 176         // TODO: Check that the adapter is not in use
 177
 178         // Initialise an interface, with a dummy IP address (zero)
 179         fd = open("/Devices/ip", 0);
 180         if( fd == -1 ) {
 181                 fprintf(stderr, "ERROR: Unable to open '/Devices/ip'\n");
 182                 return -1;
 183         }
 184         Iface->Num = ioctl(fd, 4, (void*)Iface->Adapter);       // Create interface
 185         if( Iface->Num == -1 ) {
 186                 fprintf(stderr, "ERROR: Unable to create new interface\n");
 187                 return -1;
 188         }
 189         close(fd);
 190
 191         // Open new interface
 192         snprintf(path, sizeof(path), "/Devices/ip/%i", Iface->Num);
 193         Iface->IfaceFD = fd = open(path, 0);
 194         if( fd == -1 ) {
 195                 fprintf(stderr, "ERROR: Unable to open '%s'\n", path);
 196                 return -1;
 197         }
 198         tmp = 4; ioctl(fd, 4, &tmp);    // Set to IPv4
 199         ioctl(fd, 6, addr);     // Set address to 0.0.0.0
 200         tmp = 0; ioctl(fd, 7, &tmp);    // Set subnet mask to 0
 201
 202         // Open UDP Client
 203         snprintf(path, sizeof(path), "/Devices/ip/%i/udp", Iface->Num);
 204         Iface->SocketFD = fd = open(path, O_RDWR);
 205         if( fd == -1 ) {
 206                 fprintf(stderr, "ERROR: Unable to open '%s'\n", path);
 207                 return -1;
 208         }
 209         tmp = 68; ioctl(fd, 4, &tmp);   // Local port
 210         tmp = 67; ioctl(fd, 5, &tmp);   // Remote port
 211         tmp = 0;        ioctl(fd, 7, &tmp);     // Remote addr mask bits - we don't care where the reply comes from
 212         addr[0] = addr[1] = addr[2] = addr[3] = 255;    // 255.255.255.255
 213         ioctl(fd, 8, addr);     // Remote address
 214
 215         return 0;
 216 }
 217
 218 void Send_DHCPDISCOVER(tInterface *Iface)
 219 {
 220         uint32_t        transaction_id;
 221         struct sDHCP_Message    *msg;
 222         char    data[8 + sizeof(struct sDHCP_Message) + 3 + 1];
 223         msg = (void*)data + 8;
 224
 225         transaction_id = rand();
 226
 227         msg->op    = htonb(1);  // BOOTREQUEST
 228         msg->htype = htonb(1);  // 10mb Ethernet
 229         msg->hlen  = htonb(6);  // 6 byte MAC
 230         msg->hops  = htonb(0);  // Hop count so far
 231         msg->xid   = htonl(transaction_id);     // Transaction ID
 232         msg->secs  = htons(0);  // secs - No time has elapsed
 233         msg->flags = htons(0);  // flags - TODO: Check if broadcast bit need be set
 234         msg->ciaddr = htonl(0); // ciaddr - Zero, as we don't have one yet
 235         msg->yiaddr = htonl(0); // yiaddr - Zero?
 236         msg->siaddr = htonl(0); // siaddr - Zero? maybe -1
 237         msg->giaddr = htonl(0); // giaddr - Zero?
 238         // Request MAC address from network adapter
 239         {
 240                 char    path[] = "/Devices/ip/adapters/ethXXXX";
 241                 sprintf(path, "/Devices/ip/adapters/%s", Iface->Adapter);
 242                 int fd = open(path, 0);
 243                 if(fd == -1) {
 244                         _SysDebug("Unable to open adapter %s", path);
 245                 }
 246                 else {
 247                         ioctl(fd, 4, msg->chaddr);
 248                         // TODO: Check if ioctl() failed
 249                         close(fd);
 250                 }
 251         }
 252         memset(msg->sname, 0, sizeof(msg->sname));      // Nuke the rest
 253         memset(msg->file, 0, sizeof(msg->file));        // Nuke the rest
 254         msg->dhcp_magic = htonl(DHCP_MAGIC);
 255
 256         int i = 0;
 257         msg->options[i++] =  53;        // DHCP Message Type
 258         msg->options[i++] =   1;
 259         msg->options[i++] =   1;        // - DHCPDISCOVER
 260         msg->options[i++] = 255;        // End of list
 261
 262
 263         data[0] = 67;   data[1] = 0;    // Port
 264         data[2] = 4;    data[3] = 0;    // AddrType
 265         data[4] = 255;  data[5] = 255;  data[6] = 255;  data[7] = 255;
 266
 267         write(Iface->SocketFD, data, sizeof(data));
 268         Iface->State = STATE_DISCOVER_SENT;
 269 }
 270
 271 void Send_DHCPREQUEST(tInterface *Iface, void *OfferPacket, int TypeOffset)
 272 {
 273         struct sDHCP_Message    *msg;
 274         msg = (void*) ((char*)OfferPacket) + 8;
 275
 276         // Reuses old data :)
 277         msg->op = 1;
 278         msg->options[TypeOffset+2] = 3; // DHCPREQUEST
 279         msg->options[TypeOffset+3] = 255;
 280
 281         write(Iface->SocketFD, OfferPacket, 8 + sizeof(*msg) + TypeOffset + 4);
 282         Iface->State = STATE_REQUEST_SENT;
 283 }
 284
 285 int Handle_Packet(tInterface *Iface)
 286 {
 287         char    data[8+576];
 288         struct sDHCP_Message    *msg = (void*)(data + 8);
 289          int    len, i;
 290
 291          int    dhcp_msg_type = 0, dhcp_msg_type_ofs;
 292         void    *router = NULL;
 293         void    *subnet_mask = NULL;
 294
 295         _SysDebug("Doing read on %i", Iface->SocketFD);
 296         len = read(Iface->SocketFD, data, sizeof(data));
 297         _SysDebug("len = %i", len);
 298
 299         _SysDebug("*msg = {");
 300         _SysDebug("  .op = %i", msg->op);
 301         _SysDebug("  .htype = %i", msg->htype);
 302         _SysDebug("  .ciaddr = 0x%x", ntohl(msg->ciaddr));
 303         _SysDebug("  .yiaddr = 0x%x", ntohl(msg->yiaddr));
 304         _SysDebug("}");
 305         if( msg->op != 2 ) {
 306                 // Not a response
 307                 _SysDebug("Not a response message");
 308                 return 0;
 309         }
 310
 311         if( htonl(msg->dhcp_magic) != DHCP_MAGIC ) {
 312                 _SysDebug("DHCP magic doesn't match (got 0x%x, expected 0x%x)",
 313                         htonl(msg->dhcp_magic), DHCP_MAGIC);
 314                 return 0;
 315         }
 316
 317         i = 0;
 318         while( i < len - sizeof(*msg) - 8 && msg->options[i] != 255 )
 319         {
 320                 if( msg->options[i] == 0 ) {
 321                         i ++;
 322                         continue ;
 323                 }
 324                 _SysDebug("Option %i, %i bytes long", msg->options[i], msg->options[i+1]);
 325                 switch(msg->options[i])
 326                 {
 327                 case 1:
 328                         subnet_mask = &msg->options[i+2];
 329                         break;
 330                 case 3:
 331                         router = &msg->options[i+2];
 332                         break;
 333                 case 53:
 334                         dhcp_msg_type_ofs = i;
 335                         dhcp_msg_type = msg->options[i+2];
 336                         break;
 337                 }
 338                 i += msg->options[i+1]+2;
 339         }
 340
 341         _SysDebug("dhcp_msg_type = %i", dhcp_msg_type);
 342
 343         switch( dhcp_msg_type )
 344         {
 345         case 1: // DHCPDISCOVER - wut?
 346                 break;
 347         case 2: // DHCPOFFER
 348                 // Send out request for this address
 349                 if( Iface->State != STATE_DISCOVER_SENT ) {
 350                         _SysDebug("Ignoring DHCPOFFER when not in STATE_DISCOVER_SENT");
 351                         return 0;
 352                 }
 353                 Send_DHCPREQUEST(Iface, data, dhcp_msg_type_ofs);
 354                 break;
 355         case 3: // DHCPREQUEST - wut?
 356                 break;
 357         case 4: // DHCPDECLINE - ?
 358                 break;
 359         case 5: // DHCPACK
 360                 // TODO: Apply address
 361                 SetAddress(Iface, &msg->yiaddr, subnet_mask, router);
 362                 return 1;
 363         }
 364         return 0;
 365 }
 366
 367 void SetAddress(tInterface *Iface, void *Addr, void *Mask, void *Router)
 368 {
 369          int    mask_bits = 0;
 370
 371         // Translate the mask
 372         if( Mask )
 373         {
 374                 uint8_t *mask = Mask;
 375                  int    i;
 376                 _SysDebug("Mask %i.%i.%i.%i", mask[0], mask[1], mask[2], mask[3]);
 377                 for( i = 0; i < 4 && mask[i] == 0xFF; i ++ ) ;
 378                 mask_bits = i*8;
 379                 if( i == 4 )
 380                 {
 381                         // Wut? /32?
 382                 }
 383                 else
 384                 {
 385                         switch(mask[i])
 386                         {
 387                         case 0x00:      mask_bits += 0; break;
 388                         case 0x80:      mask_bits += 1; break;
 389                         case 0xC0:      mask_bits += 2; break;
 390                         case 0xE0:      mask_bits += 3; break;
 391                         case 0xF0:      mask_bits += 4; break;
 392                         case 0xF8:      mask_bits += 5; break;
 393                         case 0xFC:      mask_bits += 6; break;
 394                         case 0xFE:      mask_bits += 7; break;
 395                         default:
 396                                 // Bad mask!
 397                                 break;
 398                         }
 399                 }
 400         }
 401
 402         {
 403                 uint8_t *addr = Addr;
 404                 _SysDebug("Addr %i.%i.%i.%i/%i", addr[0], addr[1], addr[2], addr[3], mask_bits);
 405
 406                 printf("Assigned %i.%i.%i.%i/%i to IF#%i (%s)\n",
 407                         addr[0], addr[1], addr[2], addr[3], mask_bits,
 408                         Iface->Num, Iface->Adapter
 409                         );
 410         }
 411
 412         ioctl(Iface->IfaceFD, 6, Addr);
 413         ioctl(Iface->IfaceFD, 7, &mask_bits);
 414
 415         if( Router );
 416         {
 417                 uint8_t *addr = Router;
 418                 _SysDebug("Router %i.%i.%i.%i", addr[0], addr[1], addr[2], addr[3]);
 419
 420                 // Set default route
 421                  int    fd;
 422                 fd = open("/Devices/ip/routes/4:00000000:0:0", OPENFLAG_CREATE);
 423                 if(fd == -1) {
 424                         fprintf(stderr, "ERROR: Unable to open default route\n");
 425                 }
 426                 else {
 427                         char ifname[snprintf(NULL,0,"%i",Iface->Num)+1];
 428                         sprintf(ifname, "%i", Iface->Num);
 429                         ioctl(fd, ioctl(fd, 3, "set_nexthop"), Router);
 430                         ioctl(fd, ioctl(fd, 3, "set_interface"), ifname);
 431                         close(fd);
 432                 }
 433         }
 434 }
 435