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