options.c

// Copyright 2004-2018 Nicholas J. Kain <njkain at gmail dot com>
// SPDX-License-Identifier: MIT
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <arpa/inet.h>
#include "nk/log.h"
#include <limits.h>

#include "options.h"

static int do_overload_value(const uint8_t *buf, size_t blen, int overload)
{
    size_t i = 0;
    while (i < blen) {
        if (buf[i] == DCODE_PADDING) {
            ++i;
            continue;
        }
        if (buf[i] == DCODE_END)
            break;
        if (i + 2 >= blen)
            break;
        if (buf[i] == DCODE_OVERLOAD) {
            if (buf[i+1] == 1) {
                overload |= buf[i+2];
                i += 3;
                continue;
            }
        }
        i += buf[i+1] + 2;
    }
    return overload;
}

static int overload_value(const struct dhcpmsg * const packet)
{
    int ol = do_overload_value(packet->options, sizeof packet->options, 0);
    if (ol & 1 && ol & 2)
        return ol;
    if (ol & 1) {
        ol |= do_overload_value(packet->file, sizeof packet->file, ol);
        return ol;
    }
    if (ol & 2) {
        ol |= do_overload_value(packet->sname, sizeof packet->sname, ol);
        return ol;
    }
    return ol; // ol == 0
}

static void do_get_dhcp_opt(const uint8_t *sbuf, size_t slen, uint8_t code,
                            uint8_t *dbuf, size_t dlen, size_t *didx)
{
    size_t i = 0;
    while (i < slen) {
        if (sbuf[i] == DCODE_PADDING) {
            ++i;
            continue;
        }
        if (sbuf[i] == DCODE_END)
            break;
        if (i >= slen - 2)
            break;
        size_t soptsiz = sbuf[i+1];
        if (sbuf[i] == code) {
            if (dlen < soptsiz + *didx)
                return;
            if (slen < soptsiz + i + 2)
                return;
            memcpy(dbuf + *didx, sbuf+i+2, soptsiz);
            *didx += soptsiz;
        }
        i += soptsiz + 2;
    }
}

size_t get_dhcp_opt(const struct dhcpmsg * const packet, uint8_t code,
                     uint8_t *dbuf, size_t dlen)
{
    int ol = overload_value(packet);
    size_t didx = 0;
    do_get_dhcp_opt(packet->options, sizeof packet->options, code,
                    dbuf, dlen, &didx);
    if (ol & 1)
        do_get_dhcp_opt(packet->file, sizeof packet->file, code,
                        dbuf, dlen, &didx);
    if (ol & 2)
        do_get_dhcp_opt(packet->sname, sizeof packet->sname, code,
                        dbuf, dlen, &didx);
    return didx;
}

// return the position of the 'end' option
ssize_t get_end_option_idx(const struct dhcpmsg * const packet)
{
    for (size_t i = 0; i < sizeof packet->options; ++i) {
        if (packet->options[i] == DCODE_END)
            return (ssize_t)i;
        if (packet->options[i] == DCODE_PADDING)
            continue;
        if (i + 1 >= sizeof packet->options)
            break;
        i += (size_t)packet->options[i+1] + 1;
    }
    log_line("get_end_option_idx: Did not find DCODE_END marker.\n");
    return -1;
}

static inline size_t sizeof_option_str(uint8_t code, size_t datalen)
{
    if (code == DCODE_PADDING || code == DCODE_END)
        return 1;
    return 2 + datalen;
}

// Add a raw data string to the options.  It will take a binary string suitable
// for use with eg memcpy() and will append it to the options[] field of
// a dhcp packet with the requested option code and proper length specifier.
size_t add_option_string(struct dhcpmsg *packet, uint8_t code,
                         const char * const str, size_t slen)
{
    size_t len = sizeof_option_str(code, slen);
    if (slen > 255 || len != slen + 2) {
        log_line("add_option_string: Length checks failed.\n");
        return 0;
    }

    ssize_t end = get_end_option_idx(packet);
    if (end < 0) {
        log_line("add_option_string: Buffer has no DCODE_END marker.\n");
        return 0;
    }
    if ((size_t)end + len >= sizeof packet->options) {
        log_line("add_option_string: No space for option 0x%02x.\n", code);
        return 0;
    }
    packet->options[end] = code;
    packet->options[end+1] = slen;
    memcpy(packet->options + end + 2, str, slen);
    packet->options[(size_t)end+len] = DCODE_END;
    return len;
}

static ssize_t add_option_check(struct dhcpmsg *packet, uint8_t code,
                                uint8_t rlen)
{
    ssize_t end = get_end_option_idx(packet);
    if (end < 0) {
        log_line("add_u%01u_option: Buffer has no DCODE_END marker.\n", rlen*8);
        return -1;
    }
    if ((size_t)end + 2 + rlen >= sizeof packet->options) {
        log_line("add_u%01u_option: No space for option 0x%02x.\n",
                 rlen*8, code);
        return -1;
    }
    return end;
}

static size_t add_u8_option(struct dhcpmsg *packet, uint8_t code, uint8_t data)
{
    ssize_t end = add_option_check(packet, code, 1);
    if (end < 0)
        return 0;
    packet->options[end] = code;
    packet->options[end+1] = 1;
    packet->options[end+2] = data;
    packet->options[end+3] = DCODE_END;
    return 3;
}

#ifndef NDHS_BUILD
// Data should be in network byte order.
static size_t add_u16_option(struct dhcpmsg *packet, uint8_t code,
                             uint16_t data)
{
    ssize_t end = add_option_check(packet, code, 2);
    if (end < 0)
        return 0;
    uint8_t *dp = (uint8_t *)&data;
    packet->options[end] = code;
    packet->options[end+1] = 2;
    packet->options[end+2] = dp[0];
    packet->options[end+3] = dp[1];
    packet->options[end+4] = DCODE_END;
    return 4;
}
#endif

// Data should be in network byte order.
size_t add_u32_option(struct dhcpmsg *packet, uint8_t code, uint32_t data)
{
    ssize_t end = add_option_check(packet, code, 4);
    if (end < 0)
        return 0;
    uint8_t *dp = (uint8_t *)&data;
    packet->options[end] = code;
    packet->options[end+1] = 4;
    packet->options[end+2] = dp[0];
    packet->options[end+3] = dp[1];
    packet->options[end+4] = dp[2];
    packet->options[end+5] = dp[3];
    packet->options[end+6] = DCODE_END;
    return 6;
}

// Add a parameter request list for stubborn DHCP servers
size_t add_option_request_list(struct dhcpmsg *packet)
{
    static const char reqdata[] = {
        DCODE_SUBNET, DCODE_ROUTER, DCODE_DNS, DCODE_HOSTNAME, DCODE_DOMAIN,
        DCODE_BROADCAST,
    };
    return add_option_string(packet, DCODE_PARAM_REQ,
                             reqdata, sizeof reqdata);
}

#ifdef NDHS_BUILD
size_t add_option_domain_name(struct dhcpmsg *packet, const char * const dom,
                              size_t domlen)
{
    return add_option_string(packet, DCODE_DOMAIN, dom, domlen);
}

void add_option_subnet_mask(struct dhcpmsg *packet, uint32_t subnet)
{
    add_u32_option(packet, DCODE_SUBNET, subnet);
}

void add_option_broadcast(struct dhcpmsg *packet, uint32_t bc)
{
    add_u32_option(packet, DCODE_BROADCAST, bc);
}
#endif

void add_option_msgtype(struct dhcpmsg *packet, uint8_t type)
{
    add_u8_option(packet, DCODE_MSGTYPE, type);
}

void add_option_reqip(struct dhcpmsg *packet, uint32_t ip)
{
    add_u32_option(packet, DCODE_REQIP, ip);
}

void add_option_serverid(struct dhcpmsg *packet, uint32_t sid)
{
    add_u32_option(packet, DCODE_SERVER_ID, sid);
}

void add_option_clientid(struct dhcpmsg *packet, const char * const clientid,
                         size_t clen)
{
    add_option_string(packet, DCODE_CLIENT_ID, clientid, clen);
}

#ifndef NDHS_BUILD
void add_option_maxsize(struct dhcpmsg *packet)
{
    add_u16_option(packet, DCODE_MAX_SIZE,
                   htons(sizeof(struct ip_udp_dhcp_packet)));
}

void add_option_vendor(struct dhcpmsg *packet, const char * const vendor,
                       size_t vsize)
{
    if (vsize)
        add_option_string(packet, DCODE_VENDOR, vendor, vsize);
}

void add_option_hostname(struct dhcpmsg *packet, const char * const hostname,
                         size_t hsize)
{
    if (hsize)
        add_option_string(packet, DCODE_HOSTNAME, hostname, hsize);
}
#endif

uint32_t get_option_router(const struct dhcpmsg * const packet)
{
    uint32_t ret = 0;
    uint8_t buf[MAX_DOPT_SIZE];
    const size_t ol = get_dhcp_opt(packet, DCODE_ROUTER, buf, sizeof buf);
    if (ol == sizeof ret)
        memcpy(&ret, buf, sizeof ret);
    return ret;
}

uint8_t get_option_msgtype(const struct dhcpmsg * const packet)
{
    uint8_t ret = 0;
    uint8_t buf[MAX_DOPT_SIZE];
    const size_t ol = get_dhcp_opt(packet, DCODE_MSGTYPE, buf, sizeof buf);
    if (ol == sizeof ret)
        ret = buf[0];
    return ret;
}

uint32_t get_option_serverid(const struct dhcpmsg *const packet, int *found)
{
    uint32_t ret = 0;
    uint8_t buf[MAX_DOPT_SIZE];
    *found = 0;
    const size_t ol = get_dhcp_opt(packet, DCODE_SERVER_ID, buf, sizeof buf);
    if (ol == sizeof ret) {
        *found = 1;
        memcpy(&ret, buf, sizeof ret);
    }
    return ret;
}

uint32_t get_option_subnet_mask(const struct dhcpmsg *const packet, int *found)
{
    uint32_t ret = 0;
    uint8_t buf[MAX_DOPT_SIZE];
    *found = 0;
    const size_t ol = get_dhcp_opt(packet, DCODE_SUBNET, buf, sizeof buf);
    if (ol == sizeof ret) {
        *found = 1;
        memcpy(&ret, buf, sizeof ret);
    }
    return ret;
}

uint32_t get_option_broadcast(const struct dhcpmsg *const packet, int *found)
{
    uint32_t ret = 0;
    uint8_t buf[MAX_DOPT_SIZE];
    *found = 0;
    const size_t ol = get_dhcp_opt(packet, DCODE_BROADCAST, buf, sizeof buf);
    if (ol == sizeof ret) {
        *found = 1;
        memcpy(&ret, buf, sizeof ret);
    }
    return ret;
}

uint32_t get_option_leasetime(const struct dhcpmsg * const packet)
{
    uint32_t ret = 0;
    uint8_t buf[MAX_DOPT_SIZE];
    const size_t ol = get_dhcp_opt(packet, DCODE_LEASET, buf, sizeof buf);
    if (ol == sizeof ret) {
        memcpy(&ret, buf, sizeof ret);
        ret = ntohl(ret);
    }
    return ret;
}

// Returned buffer is not nul-terminated.
size_t get_option_clientid(const struct dhcpmsg * const packet, char *cbuf,
                           size_t clen)
{
    if (clen < 1)
        return 0;
    return get_dhcp_opt(packet, DCODE_CLIENT_ID, (uint8_t *)cbuf, clen);
}