sd-dhcp6-client: Add basic DHCPv6 option handling

Add option appending and parsing. DHCPv6 options are not aligned, thus
the option handling code must be able to handle options starting at
any byte boundary.

Add a test case for the basic option handling.

3 files changed, 226 insertions, 0 deletions

diff --git a/src/libsystemd-network/dhcp6-internal.h b/src/libsystemd-network/dhcp6-internal.h
index 1cdb912548..30b624d54c 100644
--- a/src/libsystemd-network/dhcp6-internal.h
+++ b/src/libsystemd-network/dhcp6-internal.h
@@ -59,3 +59,9 @@ typedef struct DHCP6IA DHCP6IA;
 
 int dhcp_network_icmp6_bind_router_solicitation(int index);
 int dhcp_network_icmp6_send_router_solicitation(int s, const struct ether_addr *ether_addr);
+
+int dhcp6_option_append(uint8_t **buf, size_t *buflen, uint16_t code,
+                        size_t optlen, const void *optval);
+int dhcp6_option_append_ia(uint8_t **buf, size_t *buflen, DHCP6IA *ia);
+int dhcp6_option_parse(uint8_t **buf, size_t *buflen, uint16_t *optcode,
+                       size_t *optlen, uint8_t **optvalue);
diff --git a/src/libsystemd-network/dhcp6-option.c b/src/libsystemd-network/dhcp6-option.c
new file mode 100644
index 0000000000..cc4d261ae7
--- /dev/null
+++ b/src/libsystemd-network/dhcp6-option.c
@@ -0,0 +1,150 @@
+/*-*- Mode: C; c-basic-offset: 8; indent-tabs-mode: nil -*-*/
+
+/***
+  This file is part of systemd.
+
+  Copyright (C) 2014 Intel Corporation. All rights reserved.
+
+  systemd is free software; you can redistribute it and/or modify it
+  under the terms of the GNU Lesser General Public License as published by
+  the Free Software Foundation; either version 2.1 of the License, or
+  (at your option) any later version.
+
+  systemd is distributed in the hope that it will be useful, but
+  WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public License
+  along with systemd; If not, see <http://www.gnu.org/licenses/>.
+***/
+
+#include <netinet/in.h>
+#include <errno.h>
+#include <string.h>
+
+#include "sparse-endian.h"
+#include "util.h"
+
+#include "dhcp6-internal.h"
+#include "dhcp6-protocol.h"
+
+#define DHCP6_OPTION_HDR_LEN                    4
+#define DHCP6_OPTION_IA_NA_LEN                  12
+#define DHCP6_OPTION_IA_TA_LEN                  4
+#define DHCP6_OPTION_IAADDR_LEN                 24
+
+static int option_append_hdr(uint8_t **buf, size_t *buflen, uint16_t optcode,
+                             size_t optlen) {
+        assert_return(buf, -EINVAL);
+        assert_return(*buf, -EINVAL);
+        assert_return(buflen, -EINVAL);
+
+        if (optlen > 0xffff || *buflen < optlen + DHCP6_OPTION_HDR_LEN)
+                return -ENOBUFS;
+
+        (*buf)[0] = optcode >> 8;
+        (*buf)[1] = optcode & 0xff;
+        (*buf)[2] = optlen >> 8;
+        (*buf)[3] = optlen & 0xff;
+
+        *buf += DHCP6_OPTION_HDR_LEN;
+        *buflen -= DHCP6_OPTION_HDR_LEN;
+
+        return 0;
+}
+
+int dhcp6_option_append(uint8_t **buf, size_t *buflen, uint16_t code,
+                        size_t optlen, const void *optval) {
+        int r;
+
+        assert_return(optval, -EINVAL);
+
+        r = option_append_hdr(buf, buflen, code, optlen);
+        if (r < 0)
+                return r;
+
+        memcpy(*buf, optval, optlen);
+
+        *buf += optlen;
+        *buflen -= optlen;
+
+        return 0;
+}
+
+int dhcp6_option_append_ia(uint8_t **buf, size_t *buflen, DHCP6IA *ia) {
+        uint16_t len;
+        uint8_t *ia_hdr;
+        size_t ia_buflen, ia_addrlen = 0;
+        DHCP6Address *addr;
+        int r;
+
+        assert_return(buf && *buf && buflen && ia, -EINVAL);
+
+        switch (ia->type) {
+        case DHCP6_OPTION_IA_NA:
+                len = DHCP6_OPTION_IA_NA_LEN;
+                break;
+
+        case DHCP6_OPTION_IA_TA:
+                len = DHCP6_OPTION_IA_TA_LEN;
+                break;
+
+        default:
+                return -EINVAL;
+        }
+
+        if (*buflen < len)
+                return -ENOBUFS;
+
+        ia_hdr = *buf;
+        ia_buflen = *buflen;
+
+        *buf += DHCP6_OPTION_HDR_LEN;
+        *buflen -= DHCP6_OPTION_HDR_LEN;
+
+        memcpy(*buf, &ia->id, len);
+
+        *buf += len;
+        *buflen -= len;
+
+        LIST_FOREACH(addresses, addr, ia->addresses) {
+                r = option_append_hdr(buf, buflen, DHCP6_OPTION_IAADDR,
+                                      DHCP6_OPTION_IAADDR_LEN);
+                if (r < 0)
+                        return r;
+
+                memcpy(*buf, &addr->address, DHCP6_OPTION_IAADDR_LEN);
+
+                *buf += DHCP6_OPTION_IAADDR_LEN;
+                *buflen -= DHCP6_OPTION_IAADDR_LEN;
+
+                ia_addrlen += DHCP6_OPTION_HDR_LEN + DHCP6_OPTION_IAADDR_LEN;
+        }
+
+        r = option_append_hdr(&ia_hdr, &ia_buflen, ia->type, len + ia_addrlen);
+        if (r < 0)
+                return r;
+
+        return 0;
+}
+
+int dhcp6_option_parse(uint8_t **buf, size_t *buflen, uint16_t *optcode,
+                       size_t *optlen, uint8_t **optvalue) {
+        assert_return(buf && buflen && optcode && optlen && optvalue, -EINVAL);
+
+        if (*buflen == 0)
+                return -ENOMSG;
+
+        *optcode = (*buf)[0] << 8 | (*buf)[1];
+        *optlen = (*buf)[2] << 8 | (*buf)[3];
+
+        if (*optlen > *buflen - 4)
+                return -ENOBUFS;
+
+        *optvalue = &(*buf)[4];
+        *buflen -= (*optlen + 4);
+        (*buf) += (*optlen + 4);
+
+        return 0;
+}
diff --git a/src/libsystemd-network/test-dhcp6-client.c b/src/libsystemd-network/test-dhcp6-client.c
index b2dc8ba0c5..b52f40792b 100644
--- a/src/libsystemd-network/test-dhcp6-client.c
+++ b/src/libsystemd-network/test-dhcp6-client.c
@@ -28,6 +28,7 @@
 
 #include "sd-dhcp6-client.h"
 #include "dhcp6-protocol.h"
+#include "dhcp6-internal.h"
 
 static struct ether_addr mac_addr = {
         .ether_addr_octet = {'A', 'B', 'C', '1', '2', '3'}
@@ -61,6 +62,72 @@ static int test_client_basic(sd_event *e) {
         return 0;
 }
 
+static int test_option(sd_event *e) {
+        uint8_t packet[] = {
+                'F', 'O', 'O',
+                0x00, DHCP6_OPTION_ORO, 0x00, 0x07,
+                'A', 'B', 'C', 'D', 'E', 'F', 'G',
+                0x00, DHCP6_OPTION_VENDOR_CLASS, 0x00, 0x09,
+                '1', '2', '3', '4', '5', '6', '7', '8', '9',
+                'B', 'A', 'R',
+        };
+        uint8_t result[] = {
+                'F', 'O', 'O',
+                0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+                0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+                0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+                'B', 'A', 'R',
+        };
+        uint16_t optcode;
+        size_t optlen;
+        uint8_t *optval, *buf, *out;
+        size_t zero = 0, pos = 3;
+        size_t buflen = sizeof(packet), outlen = sizeof(result);
+
+        if (verbose)
+                printf("* %s\n", __FUNCTION__);
+
+        assert_se(buflen == outlen);
+
+        assert_se(dhcp6_option_parse(&buf, &zero, &optcode, &optlen,
+                                     &optval) == -ENOMSG);
+
+        buflen -= 3;
+        buf = &packet[3];
+        outlen -= 3;
+        out = &result[3];
+
+        assert_se(dhcp6_option_parse(&buf, &buflen, &optcode, &optlen,
+                                     &optval) >= 0);
+        pos += 4 + optlen;
+        assert_se(buf == &packet[pos]);
+        assert_se(optcode == DHCP6_OPTION_ORO);
+        assert_se(optlen == 7);
+        assert_se(buflen + pos == sizeof(packet));
+
+        assert_se(dhcp6_option_append(&out, &outlen, optcode, optlen,
+                                      optval) >= 0);
+        assert_se(out == &result[pos]);
+        assert_se(*out == 0x00);
+
+        assert_se(dhcp6_option_parse(&buf, &buflen, &optcode, &optlen,
+                                     &optval) >= 0);
+        pos += 4 + optlen;
+        assert_se(buf == &packet[pos]);
+        assert_se(optcode == DHCP6_OPTION_VENDOR_CLASS);
+        assert_se(optlen == 9);
+        assert_se(buflen + pos == sizeof(packet));
+
+        assert_se(dhcp6_option_append(&out, &outlen, optcode, optlen,
+                                      optval) >= 0);
+        assert_se(out == &result[pos]);
+        assert_se(*out == 'B');
+
+        assert_se(memcmp(packet, result, sizeof(packet)) == 0);
+
+        return 0;
+}
+
 int main(int argc, char *argv[]) {
         _cleanup_event_unref_ sd_event *e;
 
@@ -71,6 +138,9 @@ int main(int argc, char *argv[]) {
         log_open();
 
         test_client_basic(e);
+        test_option(e);
+
+        assert_se(!sd_event_unref(e));
 
         return 0;
 }