implement dhcp4 server

Makefile

@@ -22,7 +22,7 @@ OBJS = \
 	interface.o \
 	sock.o
 
-all: client test2
+all: client server test2
 
 %.o: %.cpp
 	$(CXX) $(CFLAGS) $(CXXFLAGS) $(FREETYPE_CFLAGS) -c $< -o $@

client.cpp

@@ -17,13 +17,22 @@
 #include "interface.h"
 #include "sock.h"
 
+void print_message(packet::packet const& p, dhcp4::options const& options)
+{
+  printf("dhcp4 op: %d\n", p.dhcp4->op);
+  printf("dhcp4 yiaddr: %s\n", ip4::tostr(p.dhcp4->yiaddr));
+
+  printf("message type: %s\n", dhcp4::message_type_tostr(options.message_type));
+  printf("subnet mask: %s\n", ip4::tostr(options.subnet_mask));
+  printf("router: %s\n", ip4::tostr(options.router));
+  printf("domain name server: %s\n", ip4::tostr(options.domain_name_server));
+}
+
 int main()
 {
   srand(time(NULL));
   uint32_t dhcp_transaction_id = rand();
 
-  static uint8_t buf[16384];
-
   int sock = sock::create(68);
 
   char const * interface_name = "wlp174s0";
@@ -37,6 +46,11 @@ int main()
   uint32_t ip4_source_address = 0x00000000;
   uint32_t ip4_destination_address = 0xffffffff;
 
+  packet::packet p{};
+  dhcp4::options options{};
+  static uint8_t recvbuf[65536];
+  static uint8_t sendbuf[16384];
+
   //////////////////////////////////////////////////////////////////////
   // dhcp send discover
   //////////////////////////////////////////////////////////////////////
@@ -44,26 +58,26 @@ int main()
     dhcp4::message_parameters params = {
     };
 
-    int length = packet::make(buf,
+    int length = packet::make(sendbuf,
+                              dhcp4::op::bootrequest,
                               eth_source_address,
                               eth_destination_address,
+                              eth_source_address, // eth_client_address
                               ip4_source_address,
                               ip4_destination_address,
+                              68, // udp4_source_port
+                              67, // udp4_destination_port
                               dhcp_transaction_id,
                               dhcp4::message_type::dhcpdiscover,
                               &params);
 
-    sock::send(sock, ifa_index, buf, length, eth_destination_address);
+    sock::send(sock, ifa_index, sendbuf, length, eth_destination_address);
   }
 
   //////////////////////////////////////////////////////////////////////
   // dhcp recv offer
   //////////////////////////////////////////////////////////////////////
 
-  packet::packet p{};
-  dhcp4::options options{};
-  uint8_t recvbuf[65536];
-
   {
     struct sockaddr_ll recv_address;
     int flags = 0;
@@ -76,16 +90,9 @@ int main()
     }
 
     packet::parse(recvbuf, rret, &p);
-
-    printf("dhcp4 op: %d\n", p.dhcp4->op);
-    printf("dhcp4 yiaddr: %s\n", ip4::tostr(p.dhcp4->yiaddr));
-
     int dhcp4_options_length = rret - (packet::dhcp4_offset + (offsetof (dhcp4::message, options)));
     dhcp4::parse_options(p.dhcp4, dhcp4_options_length, &options);
-    printf("message type: %s\n", dhcp4::message_type_tostr(options.message_type));
+    print_message(p, options);
-    printf("subnet mask: %s\n", ip4::tostr(options.subnet_mask));
-    printf("router: %s\n", ip4::tostr(options.router));
-    printf("domain name server: %s\n", ip4::tostr(options.domain_name_server));
 
     assert(p.dhcp4->op == dhcp4::op::bootreply);
     assert(options.message_type == dhcp4::message_type::dhcpoffer);
@@ -100,16 +107,20 @@ int main()
       .requested_ip_address = p.dhcp4->yiaddr,
     };
 
-    int length = packet::make(buf,
+    int length = packet::make(sendbuf,
+                              dhcp4::op::bootrequest,
                               eth_source_address,
                               eth_destination_address,
+                              eth_source_address, // eth_client_address
                               ip4_source_address,
                               ip4_destination_address,
+                              68, // udp4_source_port
+                              67, // udp4_destination_port
                               dhcp_transaction_id,
                               dhcp4::message_type::dhcprequest,
                               &params);
 
-    sock::send(sock, ifa_index, buf, length, eth_destination_address);
+    sock::send(sock, ifa_index, sendbuf, length, eth_destination_address);
   }
 
   //////////////////////////////////////////////////////////////////////
@@ -128,16 +139,9 @@ int main()
     }
 
     packet::parse(recvbuf, rret, &p);
-
-    printf("dhcp4 op: %d\n", p.dhcp4->op);
-    printf("dhcp4 yiaddr: %s\n", ip4::tostr(p.dhcp4->yiaddr));
-
     int dhcp4_options_length = rret - (packet::dhcp4_offset + (offsetof (dhcp4::message, options)));
     dhcp4::parse_options(p.dhcp4, dhcp4_options_length, &options);
-    printf("message type: %s\n", dhcp4::message_type_tostr(options.message_type));
+    print_message(p, options);
-    printf("subnet mask: %s\n", ip4::tostr(options.subnet_mask));
-    printf("router: %s\n", ip4::tostr(options.router));
-    printf("domain name server: %s\n", ip4::tostr(options.domain_name_server));
 
     assert(p.dhcp4->op == dhcp4::op::bootreply);
     assert(options.message_type == dhcp4::message_type::dhcpack);

dhcp4.cpp

@@ -76,6 +76,12 @@ namespace dhcp4 {
       index = add_uint32(msg, index, option::subnet_mask, params->subnet_mask);
     }
 
+    if (params->server_identifier != 0) {
+      index = add_uint32(msg, index, option::server_identifier, params->server_identifier);
+
+      index = add_uint32(msg, index, option::ip_address_lease_time, htonl(86400));
+    }
+
     if (params->router != 0) {
       index = add_uint32(msg, index, option::router, params->router);
     }
@@ -119,10 +125,11 @@ namespace dhcp4 {
     int index = 4;
     while (index + 2 <= options_length) {
       int option = options[index + 0];
+      if (option == option::end) {
+        break;
+      }
       int option_length = options[index + 1];
 
-      //printf("option %d length %d\n", option, option_length);
-
       switch (option) {
       case option::message_type:
         o->message_type = options[index + 2];
@@ -136,12 +143,19 @@ namespace dhcp4 {
       case option::domain_name_server:
         o->domain_name_server = *reinterpret_cast<uint32_t const *>(&options[index + 2]);
         break;
+      case option::requested_ip_address:
+        o->requested_ip_address = *reinterpret_cast<uint32_t const *>(&options[index + 2]);
+        break;
       default:
         break;
       }
 
       index += 2 + option_length;
     }
-    assert(options[index] == 255);
+
+    assert(options[index] == option::end);
+    for (index = index + 1; index < options_length; index++) {
+      assert(options[index] == 0);
+    }
   }
 }

dhcp4.h

@@ -28,6 +28,7 @@ namespace dhcp4 {
     uint32_t subnet_mask; // 1
     uint32_t router; // 3
     uint32_t domain_name_server; // 6
+    uint32_t requested_ip_address; // 50
   };
 
   struct op {
@@ -59,6 +60,7 @@ namespace dhcp4 {
       parameter_request_list = 55,
       maximum_message_size = 57,
       client_identifier = 61,
+      end = 255,
     };
   };
 
@@ -78,7 +80,7 @@ namespace dhcp4 {
   struct message_parameters {
     uint32_t your_ip_address;
     uint32_t next_server_ip_address;
-    uint32_t dhcp_server_identifier;
+    uint32_t server_identifier;
     uint32_t subnet_mask;
     uint32_t router;
     uint32_t domain_name_server;

interface.h

@@ -10,5 +10,3 @@ namespace interface {
                  uint32_t gateway);
 
 }
-{	>(/X k5@/X Xo#",nq
-/X 23{	>(R<  H`H` H`H`D(	>(T`&C.N#>
6

ip4.cpp

@@ -1,3 +1,4 @@
+#include <assert.h>
 #include <stdint.h>
 #include <stddef.h>
 #include <string.h>
@@ -14,6 +15,14 @@ namespace ip4 {
     return inet_ntop(AF_INET, reinterpret_cast<void const *>(&a), tmp, 16);
   }
 
+  uint32_t fromstr(char const * s)
+  {
+    uint32_t value;
+    int ret = inet_pton(AF_INET, s, reinterpret_cast<void *>(&value));
+    assert(ret == 1);
+    return value;
+  }
+
   uint32_t checksum_partial(uint32_t sum, void const * src, int length)
   {
     uint8_t const * addr = reinterpret_cast<uint8_t const *>(src);

ip4.h

@@ -20,6 +20,7 @@ namespace ip4 {
   static_assert((sizeof (header)) == 20);
 
   char const * tostr(uint32_t a);
+  uint32_t fromstr(char const * s);
 
   uint32_t checksum_partial(uint32_t sum, void const * src, int length);
   uint32_t checksum_finish(uint32_t sum);

packet.cpp

@@ -62,10 +62,14 @@ namespace packet {
   }
 
   int make(uint8_t * buf,
+           uint8_t const op,
            uint8_t const * eth_source_address,
            uint8_t const * eth_destination_address,
+           uint8_t const * eth_client_address,
            uint32_t const ip4_source_address,
            uint32_t const ip4_destination_address,
+           uint16_t const udp4_source_port,
+           uint16_t const udp4_destination_port,
            uint32_t const dhcp_transaction_id,
            uint32_t const dhcp_message_type,
            dhcp4::message_parameters const * params)
@@ -74,12 +78,10 @@ namespace packet {
     // dhcp4
     //////////////////////////////////////////////////////////////////////
 
-
-
     int dhcp4_length = dhcp4::make_message(reinterpret_cast<void *>(&buf[dhcp4_offset]),
-                                           dhcp4::op::bootrequest,
+                                           op,
                                            dhcp_transaction_id,
-                                           eth_source_address,
+                                           eth_client_address,
                                            dhcp_message_type,
                                            params);
 
@@ -109,8 +111,8 @@ namespace packet {
     //////////////////////////////////////////////////////////////////////
 
     udp4::make_header(reinterpret_cast<void *>(&buf[udp4_offset]),
-                      htons(68),
+                      htons(udp4_source_port),
-                      htons(67),
+                      htons(udp4_destination_port),
                       ip4_source_address,
                       ip4_destination_address,
                       &buf[dhcp4_offset],

packet.h

@@ -21,10 +21,14 @@ namespace packet {
   const int dhcp4_offset = (sizeof (eth::header)) + (sizeof (ip4::header)) + (sizeof (udp4::header));
 
   int make(uint8_t * buf,
+           uint8_t op,
            uint8_t const * eth_source_address,
            uint8_t const * eth_destination_address,
+           uint8_t const * eth_client_address,
            uint32_t const ip4_source_address,
            uint32_t const ip4_destination_address,
+           uint16_t const udp4_source_port,
+           uint16_t const udp4_destination_port,
            uint32_t const dhcp_transaction_id,
            uint32_t const dhcp_message_type,
            dhcp4::message_parameters const * params);

server.cpp

@@ -0,0 +1,131 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <string.h>
+#include <stddef.h>
+
+#include <fcntl.h>
+#include <arpa/inet.h>
+#include <linux/if_packet.h>
+
+#include "ip4.h"
+#include "udp4.h"
+#include "dhcp4.h"
+#include "eth.h"
+#include "packet.h"
+#include "interface.h"
+#include "sock.h"
+
+int main()
+{
+
+  int sock = sock::create(67);
+
+  char const * interface_name = "enp175s0";
+  int ifa_index = sock::interface_index(sock, interface_name);
+  sock::bind_interface(sock, ifa_index);
+
+  uint8_t eth_source_address[6];
+  sock::interface_address(sock, interface_name, eth_source_address);
+
+  packet::packet p{};
+  dhcp4::options options{};
+  static uint8_t recvbuf[65536];
+  static uint8_t sendbuf[16384];
+
+  uint32_t ip4_client_address = ip4::fromstr("10.0.0.50");
+  uint32_t ip4_server_address = ip4::fromstr("10.0.0.5");
+  uint32_t ip4_subnet_mask = ip4::fromstr("255.255.255.0");
+  uint32_t ip4_router = ip4::fromstr("10.0.0.5");
+  uint32_t ip4_domain_name_server = ip4::fromstr("8.8.8.8");
+
+  while (true) {
+    //////////////////////////////////////////////////////////////////////
+    // dhcp recv
+    //////////////////////////////////////////////////////////////////////
+    {
+      struct sockaddr_ll recv_address;
+      int flags = 0;
+      socklen_t socklen = (sizeof (struct sockaddr_ll));
+      ssize_t rret = recvfrom(sock, recvbuf, (sizeof (recvbuf)), flags,
+                              (struct sockaddr *)&recv_address, &socklen);
+      if (rret < 0) {
+        perror("recvfrom");
+        exit(EXIT_FAILURE);
+      }
+
+      packet::parse(recvbuf, rret, &p);
+      int dhcp4_options_length = rret - (packet::dhcp4_offset + (offsetof (dhcp4::message, options)));
+      dhcp4::parse_options(p.dhcp4, dhcp4_options_length, &options);
+
+      printf("dhcp4 op: %d\n", p.dhcp4->op);
+      printf("message type: %s\n", dhcp4::message_type_tostr(options.message_type));
+      printf("transaction id: %08x\n", ntohl(p.dhcp4->xid));
+    }
+
+    if (options.message_type == dhcp4::message_type::dhcpdiscover) {
+      dhcp4::message_parameters params = {
+        .your_ip_address = ip4_client_address,
+        .next_server_ip_address = ip4_server_address,
+        .server_identifier = ip4_server_address,
+        .subnet_mask = ip4_subnet_mask,
+        .router = ip4_router,
+        .domain_name_server = ip4_domain_name_server,
+      };
+
+      uint8_t const * eth_destination_address = p.eth->source_address;
+      uint32_t ip4_source_address = ip4_server_address;
+      uint32_t ip4_destination_address = ip4_client_address;
+      uint32_t dhcp_transaction_id = ntohl(p.dhcp4->xid);
+      int length = packet::make(sendbuf,
+                                dhcp4::op::bootreply,
+                                eth_source_address,
+                                eth_destination_address,
+                                eth_destination_address, // eth_client_address
+                                ip4_source_address,
+                                ip4_destination_address,
+                                67,
+                                68,
+                                dhcp_transaction_id,
+                                dhcp4::message_type::dhcpoffer,
+                                &params);
+
+      sock::send(sock, ifa_index, sendbuf, length, eth_destination_address);
+    }
+    if (options.message_type == dhcp4::message_type::dhcprequest) {
+      printf("requested ip address: %s\n", ip4::tostr(options.requested_ip_address));
+      if (options.requested_ip_address != ip4_client_address) {
+        printf("should NAK\n");
+        continue;
+      }
+
+      dhcp4::message_parameters params = {
+        .your_ip_address = ip4_client_address,
+        .next_server_ip_address = ip4_server_address,
+        .server_identifier = ip4_server_address,
+        .subnet_mask = ip4_subnet_mask,
+        .router = ip4_router,
+        .domain_name_server = ip4_domain_name_server,
+      };
+
+      uint8_t const * eth_destination_address = p.eth->source_address;
+      uint32_t ip4_source_address = ip4_server_address;
+      uint32_t ip4_destination_address = ip4_client_address;
+      uint32_t dhcp_transaction_id = ntohl(p.dhcp4->xid);
+      int length = packet::make(sendbuf,
+                                dhcp4::op::bootreply,
+                                eth_source_address,
+                                eth_destination_address,
+                                eth_destination_address, // eth_client_address
+                                ip4_source_address,
+                                ip4_destination_address,
+                                67,
+                                68,
+                                dhcp_transaction_id,
+                                dhcp4::message_type::dhcpack,
+                                &params);
+
+      sock::send(sock, ifa_index, sendbuf, length, eth_destination_address);
+    }
+  }
+}