| /** @file |
| * @brief DHCPv4 server implementation |
| */ |
| |
| /* |
| * Copyright (c) 2024 Nordic Semiconductor ASA |
| * |
| * SPDX-License-Identifier: Apache-2.0 |
| */ |
| |
| #include <zephyr/kernel.h> |
| #include <zephyr/logging/log.h> |
| #include <zephyr/net/net_ip.h> |
| #include <zephyr/net/dhcpv4.h> |
| #include <zephyr/net/dhcpv4_server.h> |
| #include <zephyr/net/ethernet.h> |
| #include <zephyr/net/icmp.h> |
| #include <zephyr/net/socket.h> |
| #include <zephyr/net/socket_service.h> |
| #include <zephyr/sys/byteorder.h> |
| |
| LOG_MODULE_REGISTER(net_dhcpv4_server, CONFIG_NET_DHCPV4_SERVER_LOG_LEVEL); |
| |
| #include "dhcpv4_internal.h" |
| #include "net_private.h" |
| #include "../../l2/ethernet/arp.h" |
| |
| #define DHCPV4_OPTIONS_MSG_TYPE_SIZE 3 |
| #define DHCPV4_OPTIONS_IP_LEASE_TIME_SIZE 6 |
| #define DHCPV4_OPTIONS_SERVER_ID_SIZE 6 |
| #define DHCPV4_OPTIONS_SUBNET_MASK_SIZE 6 |
| #define DHCPV4_OPTIONS_CLIENT_ID_MIN_SIZE 2 |
| |
| #define ADDRESS_RESERVED_TIMEOUT K_SECONDS(30) |
| #define ADDRESS_PROBE_TIMEOUT K_MSEC(CONFIG_NET_DHCPV4_SERVER_ICMP_PROBE_TIMEOUT) |
| #define ADDRESS_DECLINED_TIMEOUT K_SECONDS(CONFIG_NET_DHCPV4_SERVER_ADDR_DECLINE_TIME) |
| |
| #if (CONFIG_NET_DHCPV4_SERVER_ICMP_PROBE_TIMEOUT > 0) |
| #define DHCPV4_SERVER_ICMP_PROBE 1 |
| #endif |
| |
| /* RFC 1497 [17] */ |
| static const uint8_t magic_cookie[4] = { 0x63, 0x82, 0x53, 0x63 }; |
| |
| #define DHCPV4_MAX_PARAMETERS_REQUEST_LEN 16 |
| |
| struct dhcpv4_parameter_request_list { |
| uint8_t list[DHCPV4_MAX_PARAMETERS_REQUEST_LEN]; |
| uint8_t count; |
| }; |
| |
| struct dhcpv4_server_probe_ctx { |
| struct net_icmp_ctx icmp_ctx; |
| struct dhcp_msg discovery; |
| struct dhcpv4_parameter_request_list params; |
| struct dhcpv4_client_id client_id; |
| struct dhcpv4_addr_slot *slot; |
| }; |
| |
| struct dhcpv4_server_ctx { |
| struct net_if *iface; |
| int sock; |
| struct k_work_delayable timeout_work; |
| struct dhcpv4_addr_slot addr_pool[CONFIG_NET_DHCPV4_SERVER_ADDR_COUNT]; |
| struct in_addr server_addr; |
| struct in_addr netmask; |
| #if defined(DHCPV4_SERVER_ICMP_PROBE) |
| struct dhcpv4_server_probe_ctx probe_ctx; |
| #endif |
| }; |
| |
| static struct dhcpv4_server_ctx server_ctx[CONFIG_NET_DHCPV4_SERVER_INSTANCES]; |
| static struct zsock_pollfd fds[CONFIG_NET_DHCPV4_SERVER_INSTANCES]; |
| static K_MUTEX_DEFINE(server_lock); |
| |
| static void dhcpv4_server_timeout_recalc(struct dhcpv4_server_ctx *ctx) |
| { |
| k_timepoint_t next = sys_timepoint_calc(K_FOREVER); |
| k_timeout_t timeout; |
| |
| for (int i = 0; i < ARRAY_SIZE(ctx->addr_pool); i++) { |
| struct dhcpv4_addr_slot *slot = &ctx->addr_pool[i]; |
| |
| if (slot->state == DHCPV4_SERVER_ADDR_RESERVED || |
| slot->state == DHCPV4_SERVER_ADDR_ALLOCATED || |
| slot->state == DHCPV4_SERVER_ADDR_DECLINED) { |
| if (sys_timepoint_cmp(slot->expiry, next) < 0) { |
| next = slot->expiry; |
| } |
| } |
| } |
| |
| timeout = sys_timepoint_timeout(next); |
| |
| if (K_TIMEOUT_EQ(timeout, K_FOREVER)) { |
| LOG_DBG("No more addresses, canceling timer"); |
| k_work_cancel_delayable(&ctx->timeout_work); |
| } else { |
| k_work_reschedule(&ctx->timeout_work, timeout); |
| } |
| } |
| |
| /* Option parsing. */ |
| |
| static uint8_t *dhcpv4_find_option(uint8_t *data, size_t datalen, |
| uint8_t *optlen, uint8_t opt_code) |
| { |
| uint8_t *opt = NULL; |
| |
| while (datalen > 0) { |
| uint8_t code; |
| uint8_t len; |
| |
| code = *data; |
| |
| /* Two special cases (fixed sized options) */ |
| if (code == 0) { |
| data++; |
| datalen--; |
| continue; |
| } |
| |
| if (code == DHCPV4_OPTIONS_END) { |
| break; |
| } |
| |
| /* Length field should now follow. */ |
| if (datalen < 2) { |
| break; |
| } |
| |
| len = *(data + 1); |
| |
| if (datalen < len + 2) { |
| break; |
| } |
| |
| if (code == opt_code) { |
| /* Found the option. */ |
| opt = data + 2; |
| *optlen = len; |
| break; |
| } |
| |
| data += len + 2; |
| datalen -= len + 2; |
| } |
| |
| return opt; |
| } |
| |
| static int dhcpv4_find_message_type_option(uint8_t *data, size_t datalen, |
| uint8_t *msgtype) |
| { |
| uint8_t *opt; |
| uint8_t optlen; |
| |
| opt = dhcpv4_find_option(data, datalen, &optlen, |
| DHCPV4_OPTIONS_MSG_TYPE); |
| if (opt == NULL) { |
| return -ENOENT; |
| } |
| |
| if (optlen != 1) { |
| return -EINVAL; |
| } |
| |
| *msgtype = *opt; |
| |
| return 0; |
| } |
| |
| static int dhcpv4_find_server_id_option(uint8_t *data, size_t datalen, |
| struct in_addr *server_id) |
| { |
| uint8_t *opt; |
| uint8_t optlen; |
| |
| opt = dhcpv4_find_option(data, datalen, &optlen, |
| DHCPV4_OPTIONS_SERVER_ID); |
| if (opt == NULL) { |
| return -ENOENT; |
| } |
| |
| if (optlen != sizeof(struct in_addr)) { |
| return -EINVAL; |
| } |
| |
| memcpy(server_id, opt, sizeof(struct in_addr)); |
| |
| return 0; |
| } |
| |
| static int dhcpv4_find_client_id_option(uint8_t *data, size_t datalen, |
| uint8_t *client_id, uint8_t *len) |
| { |
| uint8_t *opt; |
| uint8_t optlen; |
| |
| opt = dhcpv4_find_option(data, datalen, &optlen, |
| DHCPV4_OPTIONS_CLIENT_ID); |
| if (opt == NULL) { |
| return -ENOENT; |
| } |
| |
| if (optlen < DHCPV4_OPTIONS_CLIENT_ID_MIN_SIZE) { |
| return -EINVAL; |
| } |
| |
| if (optlen > *len) { |
| LOG_ERR("Not enough memory for DHCPv4 client identifier."); |
| return -ENOMEM; |
| } |
| |
| memcpy(client_id, opt, optlen); |
| *len = optlen; |
| |
| return 0; |
| } |
| |
| static int dhcpv4_find_requested_ip_option(uint8_t *data, size_t datalen, |
| struct in_addr *requested_ip) |
| { |
| uint8_t *opt; |
| uint8_t optlen; |
| |
| opt = dhcpv4_find_option(data, datalen, &optlen, |
| DHCPV4_OPTIONS_REQ_IPADDR); |
| if (opt == NULL) { |
| return -ENOENT; |
| } |
| |
| if (optlen != sizeof(struct in_addr)) { |
| return -EINVAL; |
| } |
| |
| memcpy(requested_ip, opt, sizeof(struct in_addr)); |
| |
| return 0; |
| } |
| |
| static int dhcpv4_find_ip_lease_time_option(uint8_t *data, size_t datalen, |
| uint32_t *lease_time) |
| { |
| uint8_t *opt; |
| uint8_t optlen; |
| |
| opt = dhcpv4_find_option(data, datalen, &optlen, |
| DHCPV4_OPTIONS_LEASE_TIME); |
| if (opt == NULL) { |
| return -ENOENT; |
| } |
| |
| if (optlen != sizeof(uint32_t)) { |
| return -EINVAL; |
| } |
| |
| *lease_time = sys_get_be32(opt); |
| |
| return 0; |
| } |
| |
| static int dhcpv4_find_parameter_request_list_option( |
| uint8_t *data, size_t datalen, |
| struct dhcpv4_parameter_request_list *params) |
| { |
| uint8_t *opt; |
| uint8_t optlen; |
| |
| opt = dhcpv4_find_option(data, datalen, &optlen, |
| DHCPV4_OPTIONS_REQ_LIST); |
| if (opt == NULL) { |
| return -ENOENT; |
| } |
| |
| if (optlen > sizeof(params->list)) { |
| /* Best effort here, copy as much as we can. */ |
| optlen = sizeof(params->list); |
| } |
| |
| memcpy(params->list, opt, optlen); |
| params->count = optlen; |
| |
| return 0; |
| } |
| |
| /* Option encoding. */ |
| |
| static uint8_t *dhcpv4_encode_magic_cookie(uint8_t *buf, size_t *buflen) |
| { |
| if (buf == NULL || *buflen < SIZE_OF_MAGIC_COOKIE) { |
| return NULL; |
| } |
| |
| memcpy(buf, magic_cookie, SIZE_OF_MAGIC_COOKIE); |
| |
| *buflen -= SIZE_OF_MAGIC_COOKIE; |
| |
| return buf + SIZE_OF_MAGIC_COOKIE; |
| } |
| |
| static uint8_t *dhcpv4_encode_ip_lease_time_option(uint8_t *buf, size_t *buflen, |
| uint32_t lease_time) |
| { |
| if (buf == NULL || *buflen < DHCPV4_OPTIONS_IP_LEASE_TIME_SIZE) { |
| return NULL; |
| } |
| |
| buf[0] = DHCPV4_OPTIONS_LEASE_TIME; |
| buf[1] = sizeof(lease_time); |
| sys_put_be32(lease_time, &buf[2]); |
| |
| *buflen -= DHCPV4_OPTIONS_IP_LEASE_TIME_SIZE; |
| |
| return buf + DHCPV4_OPTIONS_IP_LEASE_TIME_SIZE; |
| } |
| |
| static uint8_t *dhcpv4_encode_message_type_option(uint8_t *buf, size_t *buflen, |
| uint8_t msgtype) |
| { |
| if (buf == NULL || *buflen < DHCPV4_OPTIONS_MSG_TYPE_SIZE) { |
| return NULL; |
| } |
| |
| buf[0] = DHCPV4_OPTIONS_MSG_TYPE; |
| buf[1] = 1; |
| buf[2] = msgtype; |
| |
| *buflen -= DHCPV4_OPTIONS_MSG_TYPE_SIZE; |
| |
| return buf + DHCPV4_OPTIONS_MSG_TYPE_SIZE; |
| } |
| |
| static uint8_t *dhcpv4_encode_server_id_option(uint8_t *buf, size_t *buflen, |
| struct in_addr *server_id) |
| { |
| if (buf == NULL || *buflen < DHCPV4_OPTIONS_SERVER_ID_SIZE) { |
| return NULL; |
| } |
| |
| buf[0] = DHCPV4_OPTIONS_SERVER_ID; |
| buf[1] = sizeof(struct in_addr); |
| memcpy(&buf[2], server_id->s4_addr, sizeof(struct in_addr)); |
| |
| *buflen -= DHCPV4_OPTIONS_SERVER_ID_SIZE; |
| |
| return buf + DHCPV4_OPTIONS_SERVER_ID_SIZE; |
| } |
| |
| static uint8_t *dhcpv4_encode_client_id_option(uint8_t *buf, size_t *buflen, |
| struct dhcpv4_client_id *client_id) |
| { |
| if (buf == NULL || *buflen < client_id->len + 2) { |
| return NULL; |
| } |
| |
| buf[0] = DHCPV4_OPTIONS_CLIENT_ID; |
| buf[1] = client_id->len; |
| memcpy(&buf[2], client_id->buf, client_id->len); |
| |
| *buflen -= client_id->len + 2; |
| |
| return buf + client_id->len + 2; |
| } |
| |
| static uint8_t *dhcpv4_encode_subnet_mask_option(uint8_t *buf, size_t *buflen, |
| struct in_addr *mask) |
| { |
| if (buf == NULL || *buflen < DHCPV4_OPTIONS_SUBNET_MASK_SIZE) { |
| return NULL; |
| } |
| |
| buf[0] = DHCPV4_OPTIONS_SUBNET_MASK; |
| buf[1] = sizeof(struct in_addr); |
| memcpy(&buf[2], mask->s4_addr, sizeof(struct in_addr)); |
| |
| *buflen -= DHCPV4_OPTIONS_SUBNET_MASK_SIZE; |
| |
| return buf + DHCPV4_OPTIONS_SUBNET_MASK_SIZE; |
| } |
| |
| static uint8_t *dhcpv4_encode_end_option(uint8_t *buf, size_t *buflen) |
| { |
| if (buf == NULL || *buflen < 1) { |
| return NULL; |
| } |
| |
| buf[0] = DHCPV4_OPTIONS_END; |
| |
| *buflen -= 1; |
| |
| return buf + 1; |
| } |
| |
| /* Response handlers. */ |
| |
| static uint8_t *dhcpv4_encode_header(uint8_t *buf, size_t *buflen, |
| struct dhcp_msg *msg, |
| struct in_addr *yiaddr) |
| { |
| struct dhcp_msg *reply_msg = (struct dhcp_msg *)buf; |
| |
| if (buf == NULL || *buflen < sizeof(struct dhcp_msg)) { |
| return NULL; |
| } |
| |
| reply_msg->op = DHCPV4_MSG_BOOT_REPLY; |
| reply_msg->htype = msg->htype; |
| reply_msg->hlen = msg->hlen; |
| reply_msg->hops = 0; |
| reply_msg->xid = msg->xid; |
| reply_msg->secs = 0; |
| reply_msg->flags = msg->flags; |
| memcpy(reply_msg->ciaddr, msg->ciaddr, sizeof(reply_msg->ciaddr)); |
| if (yiaddr != NULL) { |
| memcpy(reply_msg->yiaddr, yiaddr, sizeof(struct in_addr)); |
| } else { |
| memset(reply_msg->yiaddr, 0, sizeof(reply_msg->ciaddr)); |
| } |
| memset(reply_msg->siaddr, 0, sizeof(reply_msg->siaddr)); |
| memcpy(reply_msg->giaddr, msg->giaddr, sizeof(reply_msg->giaddr)); |
| memcpy(reply_msg->chaddr, msg->chaddr, sizeof(reply_msg->chaddr)); |
| |
| *buflen -= sizeof(struct dhcp_msg); |
| |
| return buf + sizeof(struct dhcp_msg); |
| } |
| |
| static uint8_t *dhcpv4_encode_string(uint8_t *buf, size_t *buflen, char *str, |
| size_t max_len) |
| { |
| if (buf == NULL || *buflen < max_len) { |
| return NULL; |
| } |
| |
| memset(buf, 0, max_len); |
| |
| if (str == NULL) { |
| goto out; |
| } |
| |
| strncpy(buf, str, max_len - 1); |
| |
| out: |
| *buflen -= max_len; |
| |
| return buf + max_len; |
| } |
| |
| static uint8_t *dhcpv4_encode_sname(uint8_t *buf, size_t *buflen, char *sname) |
| { |
| return dhcpv4_encode_string(buf, buflen, sname, SIZE_OF_SNAME); |
| } |
| |
| static uint8_t *dhcpv4_encode_file(uint8_t *buf, size_t *buflen, char *file) |
| { |
| return dhcpv4_encode_string(buf, buflen, file, SIZE_OF_FILE); |
| } |
| |
| static uint8_t *dhcpv4_encode_requested_params( |
| uint8_t *buf, size_t *buflen, |
| struct dhcpv4_server_ctx *ctx, |
| struct dhcpv4_parameter_request_list *params) |
| { |
| for (uint8_t i = 0; i < params->count; i++) { |
| switch (params->list[i]) { |
| case DHCPV4_OPTIONS_SUBNET_MASK: |
| buf = dhcpv4_encode_subnet_mask_option( |
| buf, buflen, &ctx->netmask); |
| if (buf == NULL) { |
| goto out; |
| } |
| break; |
| |
| /* Others - just ignore. */ |
| default: |
| break; |
| } |
| } |
| |
| out: |
| return buf; |
| } |
| |
| static int dhcpv4_send(struct dhcpv4_server_ctx *ctx, enum net_dhcpv4_msg_type type, |
| uint8_t *reply, size_t len, struct dhcp_msg *msg, |
| struct in_addr *yiaddr) |
| { |
| struct sockaddr_in dst_addr = { |
| .sin_family = AF_INET, |
| .sin_port = htons(DHCPV4_CLIENT_PORT), |
| }; |
| struct in_addr giaddr; /* Relay agent address */ |
| struct in_addr ciaddr; /* Client address */ |
| int ret; |
| |
| memcpy(&giaddr, msg->giaddr, sizeof(giaddr)); |
| memcpy(&ciaddr, msg->ciaddr, sizeof(ciaddr)); |
| |
| /* Select destination address as described in ch. 4.1. */ |
| if (!net_ipv4_is_addr_unspecified(&giaddr)) { |
| /* If the 'giaddr' field in a DHCP message from a client is |
| * non-zero, the server sends any return messages to the |
| * 'DHCP server' port on the BOOTP relay agent whose address |
| * appears in 'giaddr'. |
| */ |
| dst_addr.sin_addr = giaddr; |
| dst_addr.sin_port = htons(DHCPV4_SERVER_PORT); |
| } else if (type == NET_DHCPV4_MSG_TYPE_NAK) { |
| /* In all cases, when 'giaddr' is zero, the server broadcasts |
| * any DHCPNAK messages to 0xffffffff. |
| */ |
| dst_addr.sin_addr = *net_ipv4_broadcast_address(); |
| } else if (!net_ipv4_is_addr_unspecified(&ciaddr)) { |
| /* If the 'giaddr' field is zero and the 'ciaddr' field is |
| * nonzero, then the server unicasts DHCPOFFER and DHCPACK |
| * messages to the address in 'ciaddr'. |
| */ |
| dst_addr.sin_addr = ciaddr; |
| } else if (ntohs(msg->flags) & DHCPV4_MSG_BROADCAST) { |
| /* If 'giaddr' is zero and 'ciaddr' is zero, and the broadcast |
| * bit is set, then the server broadcasts DHCPOFFER and DHCPACK |
| * messages to 0xffffffff. |
| */ |
| dst_addr.sin_addr = *net_ipv4_broadcast_address(); |
| } else if (yiaddr != NULL) { |
| /* If the broadcast bit is not set and 'giaddr' is zero and |
| * 'ciaddr' is zero, then the server unicasts DHCPOFFER and |
| * DHCPACK messages to the client's hardware address and 'yiaddr' |
| * address. |
| */ |
| struct net_eth_addr hwaddr; |
| |
| memcpy(&hwaddr, msg->chaddr, sizeof(hwaddr)); |
| net_arp_update(ctx->iface, yiaddr, &hwaddr, false, true); |
| dst_addr.sin_addr = *yiaddr; |
| } else { |
| NET_ERR("Unspecified destination address."); |
| return -EDESTADDRREQ; |
| } |
| |
| ret = zsock_sendto(ctx->sock, reply, len, 0, (struct sockaddr *)&dst_addr, |
| sizeof(dst_addr)); |
| if (ret < 0) { |
| return -errno; |
| } |
| |
| return 0; |
| } |
| |
| static int dhcpv4_send_offer(struct dhcpv4_server_ctx *ctx, struct dhcp_msg *msg, |
| struct in_addr *addr, uint32_t lease_time, |
| struct dhcpv4_parameter_request_list *params, |
| struct dhcpv4_client_id *client_id) |
| { |
| uint8_t reply[NET_IPV4_MTU]; |
| uint8_t *buf = reply; |
| size_t buflen = sizeof(reply); |
| size_t reply_len = 0; |
| int ret; |
| |
| buf = dhcpv4_encode_header(buf, &buflen, msg, addr); |
| buf = dhcpv4_encode_sname(buf, &buflen, NULL); |
| buf = dhcpv4_encode_file(buf, &buflen, NULL); |
| buf = dhcpv4_encode_magic_cookie(buf, &buflen); |
| buf = dhcpv4_encode_ip_lease_time_option(buf, &buflen, lease_time); |
| buf = dhcpv4_encode_message_type_option(buf, &buflen, |
| NET_DHCPV4_MSG_TYPE_OFFER); |
| buf = dhcpv4_encode_server_id_option(buf, &buflen, &ctx->server_addr); |
| buf = dhcpv4_encode_client_id_option(buf, &buflen, client_id); |
| buf = dhcpv4_encode_requested_params(buf, &buflen, ctx, params); |
| buf = dhcpv4_encode_end_option(buf, &buflen); |
| |
| if (buf == NULL) { |
| LOG_ERR("Failed to encode %s message", "Offer"); |
| return -ENOMEM; |
| } |
| |
| reply_len = sizeof(reply) - buflen; |
| |
| ret = dhcpv4_send(ctx, NET_DHCPV4_MSG_TYPE_OFFER, reply, reply_len, |
| msg, addr); |
| if (ret < 0) { |
| LOG_ERR("Failed to send %s message, %d", "Offer", ret); |
| return ret; |
| } |
| |
| return 0; |
| } |
| |
| static int dhcpv4_send_ack(struct dhcpv4_server_ctx *ctx, struct dhcp_msg *msg, |
| struct in_addr *addr, uint32_t lease_time, |
| struct dhcpv4_parameter_request_list *params, |
| struct dhcpv4_client_id *client_id, |
| bool inform) |
| { |
| uint8_t reply[NET_IPV4_MTU]; |
| uint8_t *buf = reply; |
| size_t buflen = sizeof(reply); |
| size_t reply_len = 0; |
| int ret; |
| |
| buf = dhcpv4_encode_header(buf, &buflen, msg, inform ? NULL : addr); |
| buf = dhcpv4_encode_sname(buf, &buflen, NULL); |
| buf = dhcpv4_encode_file(buf, &buflen, NULL); |
| buf = dhcpv4_encode_magic_cookie(buf, &buflen); |
| if (!inform) { |
| buf = dhcpv4_encode_ip_lease_time_option(buf, &buflen, lease_time); |
| } |
| buf = dhcpv4_encode_message_type_option(buf, &buflen, |
| NET_DHCPV4_MSG_TYPE_ACK); |
| buf = dhcpv4_encode_server_id_option(buf, &buflen, &ctx->server_addr); |
| if (!inform) { |
| buf = dhcpv4_encode_client_id_option(buf, &buflen, client_id); |
| } |
| buf = dhcpv4_encode_requested_params(buf, &buflen, ctx, params); |
| buf = dhcpv4_encode_end_option(buf, &buflen); |
| |
| if (buf == NULL) { |
| LOG_ERR("Failed to encode %s message", "ACK"); |
| return -ENOMEM; |
| } |
| |
| reply_len = sizeof(reply) - buflen; |
| |
| ret = dhcpv4_send(ctx, NET_DHCPV4_MSG_TYPE_ACK, reply, reply_len, msg, |
| addr); |
| if (ret < 0) { |
| LOG_ERR("Failed to send %s message, %d", "ACK", ret); |
| return ret; |
| } |
| |
| return 0; |
| } |
| |
| static int dhcpv4_send_nak(struct dhcpv4_server_ctx *ctx, struct dhcp_msg *msg, |
| struct dhcpv4_client_id *client_id) |
| { |
| uint8_t reply[NET_IPV4_MTU]; |
| uint8_t *buf = reply; |
| size_t buflen = sizeof(reply); |
| size_t reply_len = 0; |
| int ret; |
| |
| buf = dhcpv4_encode_header(buf, &buflen, msg, NULL); |
| buf = dhcpv4_encode_sname(buf, &buflen, NULL); |
| buf = dhcpv4_encode_file(buf, &buflen, NULL); |
| buf = dhcpv4_encode_magic_cookie(buf, &buflen); |
| buf = dhcpv4_encode_message_type_option(buf, &buflen, |
| NET_DHCPV4_MSG_TYPE_NAK); |
| buf = dhcpv4_encode_server_id_option(buf, &buflen, &ctx->server_addr); |
| buf = dhcpv4_encode_client_id_option(buf, &buflen, client_id); |
| buf = dhcpv4_encode_end_option(buf, &buflen); |
| |
| if (buf == NULL) { |
| LOG_ERR("Failed to encode %s message", "NAK"); |
| return -ENOMEM; |
| } |
| |
| reply_len = sizeof(reply) - buflen; |
| |
| ret = dhcpv4_send(ctx, NET_DHCPV4_MSG_TYPE_NAK, reply, reply_len, msg, |
| NULL); |
| if (ret < 0) { |
| LOG_ERR("Failed to send %s message, %d", "NAK", ret); |
| return ret; |
| } |
| |
| return 0; |
| } |
| |
| /* Message handlers. */ |
| |
| static int dhcpv4_get_client_id(struct dhcp_msg *msg, uint8_t *options, |
| uint8_t optlen, struct dhcpv4_client_id *client_id) |
| { |
| int ret; |
| |
| client_id->len = sizeof(client_id->buf); |
| |
| ret = dhcpv4_find_client_id_option(options, optlen, client_id->buf, |
| &client_id->len); |
| if (ret == 0) { |
| return 0; |
| } |
| |
| /* No Client Id option or too long to use, fallback to hardware address. */ |
| if (msg->hlen > sizeof(msg->chaddr)) { |
| LOG_ERR("Malformed chaddr length."); |
| return -EINVAL; |
| } |
| |
| client_id->buf[0] = msg->htype; |
| client_id->buf[1] = msg->hlen; |
| memcpy(client_id->buf + 2, msg->chaddr, msg->hlen); |
| client_id->len = msg->hlen + 2; |
| |
| return 0; |
| } |
| |
| static uint32_t dhcpv4_get_lease_time(uint8_t *options, uint8_t optlen) |
| { |
| uint32_t lease_time; |
| |
| if (dhcpv4_find_ip_lease_time_option(options, optlen, |
| &lease_time) == 0) { |
| return lease_time; |
| } |
| |
| return CONFIG_NET_DHCPV4_SERVER_ADDR_LEASE_TIME; |
| } |
| |
| #if defined(DHCPV4_SERVER_ICMP_PROBE) |
| static int dhcpv4_probe_address(struct dhcpv4_server_ctx *ctx, |
| struct dhcpv4_addr_slot *slot) |
| { |
| struct sockaddr_in dest_addr = { |
| .sin_family = AF_INET, |
| .sin_addr = slot->addr, |
| }; |
| int ret; |
| |
| ret = net_icmp_send_echo_request(&ctx->probe_ctx.icmp_ctx, ctx->iface, |
| (struct sockaddr *)&dest_addr, |
| NULL, ctx); |
| if (ret < 0) { |
| LOG_ERR("Failed to send ICMP probe"); |
| } |
| |
| return ret; |
| } |
| |
| static int echo_reply_handler(struct net_icmp_ctx *icmp_ctx, |
| struct net_pkt *pkt, |
| struct net_icmp_ip_hdr *ip_hdr, |
| struct net_icmp_hdr *icmp_hdr, |
| void *user_data) |
| { |
| struct dhcpv4_server_ctx *ctx = user_data; |
| struct dhcpv4_server_probe_ctx *probe_ctx = &ctx->probe_ctx; |
| struct dhcpv4_addr_slot *new_slot = NULL; |
| struct in_addr peer_addr; |
| |
| ARG_UNUSED(icmp_ctx); |
| ARG_UNUSED(pkt); |
| ARG_UNUSED(ip_hdr); |
| ARG_UNUSED(icmp_hdr); |
| |
| k_mutex_lock(&server_lock, K_FOREVER); |
| |
| if (probe_ctx->slot == NULL) { |
| goto out; |
| } |
| |
| if (ip_hdr->family != AF_INET) { |
| goto out; |
| } |
| |
| net_ipv4_addr_copy_raw((uint8_t *)&peer_addr, ip_hdr->ipv4->src); |
| if (!net_ipv4_addr_cmp(&peer_addr, &probe_ctx->slot->addr)) { |
| goto out; |
| } |
| |
| LOG_DBG("Got ICMP probe response, blocking address %s", |
| net_sprint_ipv4_addr(&probe_ctx->slot->addr)); |
| |
| probe_ctx->slot->state = DHCPV4_SERVER_ADDR_DECLINED; |
| probe_ctx->slot->expiry = sys_timepoint_calc(ADDRESS_DECLINED_TIMEOUT); |
| |
| /* Try to find next free address */ |
| for (int i = 0; i < ARRAY_SIZE(ctx->addr_pool); i++) { |
| struct dhcpv4_addr_slot *slot = &ctx->addr_pool[i]; |
| |
| if (slot->state == DHCPV4_SERVER_ADDR_FREE) { |
| new_slot = slot; |
| break; |
| } |
| } |
| |
| if (new_slot == NULL) { |
| LOG_DBG("No more free addresses to assign, ICMP probing stopped"); |
| probe_ctx->slot = NULL; |
| dhcpv4_server_timeout_recalc(ctx); |
| goto out; |
| } |
| |
| if (dhcpv4_probe_address(ctx, new_slot) < 0) { |
| probe_ctx->slot = NULL; |
| dhcpv4_server_timeout_recalc(ctx); |
| goto out; |
| } |
| |
| new_slot->state = DHCPV4_SERVER_ADDR_RESERVED; |
| new_slot->expiry = sys_timepoint_calc(ADDRESS_PROBE_TIMEOUT); |
| new_slot->client_id.len = probe_ctx->slot->client_id.len; |
| memcpy(new_slot->client_id.buf, probe_ctx->slot->client_id.buf, |
| new_slot->client_id.len); |
| new_slot->lease_time = probe_ctx->slot->lease_time; |
| |
| probe_ctx->slot = new_slot; |
| |
| dhcpv4_server_timeout_recalc(ctx); |
| |
| out: |
| k_mutex_unlock(&server_lock); |
| |
| return 0; |
| } |
| |
| static int dhcpv4_server_probing_init(struct dhcpv4_server_ctx *ctx) |
| { |
| return net_icmp_init_ctx(&ctx->probe_ctx.icmp_ctx, |
| NET_ICMPV4_ECHO_REPLY, 0, |
| echo_reply_handler); |
| } |
| |
| static void dhcpv4_server_probing_deinit(struct dhcpv4_server_ctx *ctx) |
| { |
| (void)net_icmp_cleanup_ctx(&ctx->probe_ctx.icmp_ctx); |
| } |
| |
| static int dhcpv4_server_probe_setup(struct dhcpv4_server_ctx *ctx, |
| struct dhcpv4_addr_slot *slot, |
| struct dhcp_msg *msg, |
| struct dhcpv4_parameter_request_list *params, |
| struct dhcpv4_client_id *client_id) |
| { |
| int ret; |
| |
| if (ctx->probe_ctx.slot != NULL) { |
| return -EBUSY; |
| } |
| |
| ret = dhcpv4_probe_address(ctx, slot); |
| if (ret < 0) { |
| return ret; |
| } |
| |
| ctx->probe_ctx.slot = slot; |
| ctx->probe_ctx.discovery = *msg; |
| ctx->probe_ctx.params = *params; |
| ctx->probe_ctx.client_id = *client_id; |
| |
| return 0; |
| } |
| |
| static void dhcpv4_server_probe_timeout(struct dhcpv4_server_ctx *ctx, |
| struct dhcpv4_addr_slot *slot) |
| { |
| /* Probe timer expired, send offer. */ |
| ctx->probe_ctx.slot = NULL; |
| |
| (void)net_arp_clear_pending(ctx->iface, &slot->addr); |
| |
| if (dhcpv4_send_offer(ctx, &ctx->probe_ctx.discovery, &slot->addr, |
| slot->lease_time, &ctx->probe_ctx.params, |
| &ctx->probe_ctx.client_id) < 0) { |
| slot->state = DHCPV4_SERVER_ADDR_FREE; |
| return; |
| } |
| |
| slot->expiry = sys_timepoint_calc(ADDRESS_RESERVED_TIMEOUT); |
| } |
| |
| static bool dhcpv4_server_is_slot_probed(struct dhcpv4_server_ctx *ctx, |
| struct dhcpv4_addr_slot *slot) |
| { |
| return (ctx->probe_ctx.slot == slot); |
| } |
| #else /* defined(DHCPV4_SERVER_ICMP_PROBE) */ |
| #define dhcpv4_server_probing_init(...) (0) |
| #define dhcpv4_server_probing_deinit(...) |
| #define dhcpv4_server_probe_setup(...) (-ENOTSUP) |
| #define dhcpv4_server_probe_timeout(...) |
| #define dhcpv4_server_is_slot_probed(...) (false) |
| #endif /* defined(DHCPV4_SERVER_ICMP_PROBE) */ |
| |
| static void dhcpv4_handle_discover(struct dhcpv4_server_ctx *ctx, |
| struct dhcp_msg *msg, uint8_t *options, |
| uint8_t optlen) |
| { |
| struct dhcpv4_parameter_request_list params = { 0 }; |
| struct dhcpv4_addr_slot *selected = NULL; |
| struct dhcpv4_client_id client_id; |
| bool probe = false; |
| int ret; |
| |
| ret = dhcpv4_get_client_id(msg, options, optlen, &client_id); |
| if (ret < 0) { |
| return; |
| } |
| |
| (void)dhcpv4_find_parameter_request_list_option(options, optlen, ¶ms); |
| |
| /* Address pool and address selection algorithm as |
| * described in 4.3.1 |
| */ |
| |
| /* 1. Check for current bindings */ |
| for (int i = 0; i < ARRAY_SIZE(ctx->addr_pool); i++) { |
| struct dhcpv4_addr_slot *slot = &ctx->addr_pool[i]; |
| |
| if ((slot->state == DHCPV4_SERVER_ADDR_RESERVED || |
| slot->state == DHCPV4_SERVER_ADDR_ALLOCATED) && |
| slot->client_id.len == client_id.len && |
| memcmp(slot->client_id.buf, client_id.buf, |
| client_id.len) == 0) { |
| if (slot->state == DHCPV4_SERVER_ADDR_RESERVED && |
| dhcpv4_server_is_slot_probed(ctx, slot)) { |
| LOG_DBG("ICMP probing in progress, ignore Discovery"); |
| return; |
| } |
| |
| /* Got match in current bindings. */ |
| selected = slot; |
| break; |
| } |
| } |
| |
| /* 2. Skipped, for now expired/released entries are forgotten. */ |
| |
| /* 3. Check Requested IP Address option. */ |
| if (selected == NULL) { |
| struct in_addr requested_ip; |
| |
| ret = dhcpv4_find_requested_ip_option(options, optlen, |
| &requested_ip); |
| if (ret == 0) { |
| for (int i = 0; i < ARRAY_SIZE(ctx->addr_pool); i++) { |
| struct dhcpv4_addr_slot *slot = |
| &ctx->addr_pool[i]; |
| |
| if (net_ipv4_addr_cmp(&slot->addr, |
| &requested_ip) && |
| slot->state == DHCPV4_SERVER_ADDR_FREE) { |
| /* Requested address is free. */ |
| selected = slot; |
| probe = true; |
| break; |
| } |
| } |
| } |
| } |
| |
| /* 4. Allocate new address from pool, if available. */ |
| if (selected == NULL) { |
| struct in_addr giaddr; |
| |
| memcpy(&giaddr, msg->giaddr, sizeof(giaddr)); |
| if (!net_ipv4_is_addr_unspecified(&giaddr)) { |
| /* Only addresses in local subnet supproted for now. */ |
| return; |
| } |
| |
| for (int i = 0; i < ARRAY_SIZE(ctx->addr_pool); i++) { |
| struct dhcpv4_addr_slot *slot = &ctx->addr_pool[i]; |
| |
| if (slot->state == DHCPV4_SERVER_ADDR_FREE) { |
| /* Requested address is free. */ |
| selected = slot; |
| probe = true; |
| break; |
| } |
| } |
| } |
| |
| /* In case no free address slot was found, as a last resort, try to |
| * reuse the oldest declined entry, if present. |
| */ |
| if (selected == NULL) { |
| for (int i = 0; i < ARRAY_SIZE(ctx->addr_pool); i++) { |
| struct dhcpv4_addr_slot *slot = &ctx->addr_pool[i]; |
| |
| if (slot->state != DHCPV4_SERVER_ADDR_DECLINED) { |
| continue; |
| } |
| |
| /* Find first to expire (oldest) entry. */ |
| if ((selected == NULL) || |
| (sys_timepoint_cmp(slot->expiry, |
| selected->expiry) < 0)) { |
| selected = slot; |
| probe = true; |
| } |
| } |
| } |
| |
| if (selected == NULL) { |
| LOG_ERR("No free address found in address pool"); |
| } else { |
| uint32_t lease_time = dhcpv4_get_lease_time(options, optlen); |
| |
| if (IS_ENABLED(DHCPV4_SERVER_ICMP_PROBE) && probe) { |
| if (dhcpv4_server_probe_setup(ctx, selected, msg, |
| ¶ms, &client_id) < 0) { |
| /* Probing context already in use or failed to |
| * send probe, ignore Discovery for now and wait |
| * for retransmission. |
| */ |
| return; |
| } |
| |
| selected->expiry = |
| sys_timepoint_calc(ADDRESS_PROBE_TIMEOUT); |
| } else { |
| if (dhcpv4_send_offer(ctx, msg, &selected->addr, |
| lease_time, ¶ms, &client_id) < 0) { |
| return; |
| } |
| |
| selected->expiry = |
| sys_timepoint_calc(ADDRESS_RESERVED_TIMEOUT); |
| } |
| |
| LOG_DBG("DHCPv4 processing Discover - reserved %s", |
| net_sprint_ipv4_addr(&selected->addr)); |
| |
| selected->state = DHCPV4_SERVER_ADDR_RESERVED; |
| selected->client_id.len = client_id.len; |
| memcpy(selected->client_id.buf, client_id.buf, client_id.len); |
| selected->lease_time = lease_time; |
| dhcpv4_server_timeout_recalc(ctx); |
| } |
| } |
| |
| static void dhcpv4_handle_request(struct dhcpv4_server_ctx *ctx, |
| struct dhcp_msg *msg, uint8_t *options, |
| uint8_t optlen) |
| { |
| struct dhcpv4_parameter_request_list params = { 0 }; |
| struct dhcpv4_addr_slot *selected = NULL; |
| struct dhcpv4_client_id client_id; |
| struct in_addr requested_ip, server_id, ciaddr, giaddr; |
| int ret; |
| |
| memcpy(&ciaddr, msg->ciaddr, sizeof(ciaddr)); |
| memcpy(&giaddr, msg->giaddr, sizeof(giaddr)); |
| |
| if (!net_ipv4_is_addr_unspecified(&giaddr)) { |
| /* Only addresses in local subnet supported for now. */ |
| return; |
| } |
| |
| ret = dhcpv4_get_client_id(msg, options, optlen, &client_id); |
| if (ret < 0) { |
| /* Failed to obtain Client ID, ignore. */ |
| return; |
| } |
| |
| (void)dhcpv4_find_parameter_request_list_option(options, optlen, ¶ms); |
| |
| ret = dhcpv4_find_server_id_option(options, optlen, &server_id); |
| if (ret == 0) { |
| /* Server ID present, Request generated during SELECTING. */ |
| if (!net_ipv4_addr_cmp(&ctx->server_addr, &server_id)) { |
| /* Not for us, ignore. */ |
| return; |
| } |
| |
| ret = dhcpv4_find_requested_ip_option(options, optlen, |
| &requested_ip); |
| if (ret < 0) { |
| /* Requested IP missing, ignore. */ |
| return; |
| } |
| |
| if (!net_ipv4_is_addr_unspecified(&ciaddr)) { |
| /* ciaddr MUST be zero */ |
| return; |
| } |
| |
| for (int i = 0; i < ARRAY_SIZE(ctx->addr_pool); i++) { |
| struct dhcpv4_addr_slot *slot = &ctx->addr_pool[i]; |
| |
| if (net_ipv4_addr_cmp(&slot->addr, &requested_ip) && |
| slot->client_id.len == client_id.len && |
| memcmp(slot->client_id.buf, client_id.buf, |
| client_id.len) == 0 && |
| (slot->state == DHCPV4_SERVER_ADDR_RESERVED || |
| slot->state == DHCPV4_SERVER_ADDR_ALLOCATED)) { |
| selected = slot; |
| break; |
| } |
| } |
| |
| if (selected == NULL) { |
| LOG_ERR("No valid slot found for DHCPv4 Request"); |
| } else { |
| uint32_t lease_time = dhcpv4_get_lease_time(options, optlen); |
| |
| if (dhcpv4_send_ack(ctx, msg, &selected->addr, lease_time, |
| ¶ms, &client_id, false) < 0) { |
| return; |
| } |
| |
| LOG_DBG("DHCPv4 processing Request - allocated %s", |
| net_sprint_ipv4_addr(&selected->addr)); |
| |
| selected->lease_time = lease_time; |
| selected->expiry = sys_timepoint_calc( |
| K_SECONDS(lease_time)); |
| selected->state = DHCPV4_SERVER_ADDR_ALLOCATED; |
| dhcpv4_server_timeout_recalc(ctx); |
| } |
| |
| return; |
| } |
| |
| /* No server ID option - check requested address. */ |
| ret = dhcpv4_find_requested_ip_option(options, optlen, &requested_ip); |
| if (ret == 0) { |
| /* Requested IP present, Request generated during INIT-REBOOT. */ |
| if (!net_ipv4_is_addr_unspecified(&ciaddr)) { |
| /* ciaddr MUST be zero */ |
| return; |
| } |
| |
| if (!net_if_ipv4_addr_mask_cmp(ctx->iface, &requested_ip)) { |
| /* Wrong subnet. */ |
| dhcpv4_send_nak(ctx, msg, &client_id); |
| } |
| |
| for (int i = 0; i < ARRAY_SIZE(ctx->addr_pool); i++) { |
| struct dhcpv4_addr_slot *slot = &ctx->addr_pool[i]; |
| |
| if (slot->client_id.len == client_id.len && |
| memcmp(slot->client_id.buf, client_id.buf, |
| client_id.len) == 0 && |
| (slot->state == DHCPV4_SERVER_ADDR_RESERVED || |
| slot->state == DHCPV4_SERVER_ADDR_ALLOCATED)) { |
| selected = slot; |
| break; |
| } |
| } |
| |
| if (selected != NULL) { |
| if (net_ipv4_addr_cmp(&selected->addr, &requested_ip)) { |
| uint32_t lease_time = dhcpv4_get_lease_time( |
| options, optlen); |
| |
| if (dhcpv4_send_ack(ctx, msg, &selected->addr, |
| lease_time, ¶ms, |
| &client_id, false) < 0) { |
| return; |
| } |
| |
| selected->lease_time = lease_time; |
| selected->expiry = sys_timepoint_calc( |
| K_SECONDS(lease_time)); |
| dhcpv4_server_timeout_recalc(ctx); |
| } else { |
| dhcpv4_send_nak(ctx, msg, &client_id); |
| } |
| } else if (IS_ENABLED(CONFIG_NET_DHCPV4_SERVER_NAK_UNRECOGNIZED_REQUESTS)) { |
| dhcpv4_send_nak(ctx, msg, &client_id); |
| } |
| |
| /* No notion of the client, remain silent. */ |
| return; |
| } |
| |
| /* Neither server ID or requested IP set, Request generated during |
| * RENEWING or REBINDING. |
| */ |
| |
| if (!net_if_ipv4_addr_mask_cmp(ctx->iface, &ciaddr)) { |
| /* Wrong subnet. */ |
| dhcpv4_send_nak(ctx, msg, &client_id); |
| } |
| |
| for (int i = 0; i < ARRAY_SIZE(ctx->addr_pool); i++) { |
| struct dhcpv4_addr_slot *slot = &ctx->addr_pool[i]; |
| |
| if (net_ipv4_addr_cmp(&slot->addr, &ciaddr)) { |
| selected = slot; |
| break; |
| } |
| } |
| |
| if (selected != NULL) { |
| if (selected->state == DHCPV4_SERVER_ADDR_ALLOCATED && |
| selected->client_id.len == client_id.len && |
| memcmp(selected->client_id.buf, client_id.buf, |
| client_id.len) == 0) { |
| uint32_t lease_time = dhcpv4_get_lease_time( |
| options, optlen); |
| |
| if (dhcpv4_send_ack(ctx, msg, &ciaddr, lease_time, |
| ¶ms, &client_id, false) < 0) { |
| return; |
| } |
| |
| selected->lease_time = lease_time; |
| selected->expiry = sys_timepoint_calc( |
| K_SECONDS(lease_time)); |
| dhcpv4_server_timeout_recalc(ctx); |
| } else { |
| dhcpv4_send_nak(ctx, msg, &client_id); |
| } |
| } |
| } |
| |
| static void dhcpv4_handle_decline(struct dhcpv4_server_ctx *ctx, |
| struct dhcp_msg *msg, uint8_t *options, |
| uint8_t optlen) |
| { |
| struct dhcpv4_client_id client_id; |
| struct in_addr requested_ip, server_id; |
| int ret; |
| |
| ret = dhcpv4_find_server_id_option(options, optlen, &server_id); |
| if (ret < 0) { |
| /* No server ID, ignore. */ |
| return; |
| } |
| |
| if (!net_ipv4_addr_cmp(&ctx->server_addr, &server_id)) { |
| /* Not for us, ignore. */ |
| return; |
| } |
| |
| ret = dhcpv4_get_client_id(msg, options, optlen, &client_id); |
| if (ret < 0) { |
| /* Failed to obtain Client ID, ignore. */ |
| return; |
| } |
| |
| ret = dhcpv4_find_requested_ip_option(options, optlen, |
| &requested_ip); |
| if (ret < 0) { |
| /* Requested IP missing, ignore. */ |
| return; |
| } |
| |
| LOG_ERR("Received DHCPv4 Decline for %s (address already in use)", |
| net_sprint_ipv4_addr(&requested_ip)); |
| |
| for (int i = 0; i < ARRAY_SIZE(ctx->addr_pool); i++) { |
| struct dhcpv4_addr_slot *slot = &ctx->addr_pool[i]; |
| |
| if (net_ipv4_addr_cmp(&slot->addr, &requested_ip) && |
| slot->client_id.len == client_id.len && |
| memcmp(slot->client_id.buf, client_id.buf, |
| client_id.len) == 0 && |
| (slot->state == DHCPV4_SERVER_ADDR_RESERVED || |
| slot->state == DHCPV4_SERVER_ADDR_ALLOCATED)) { |
| slot->state = DHCPV4_SERVER_ADDR_DECLINED; |
| slot->expiry = |
| sys_timepoint_calc(ADDRESS_DECLINED_TIMEOUT); |
| dhcpv4_server_timeout_recalc(ctx); |
| break; |
| } |
| } |
| } |
| |
| static void dhcpv4_handle_release(struct dhcpv4_server_ctx *ctx, |
| struct dhcp_msg *msg, uint8_t *options, |
| uint8_t optlen) |
| { |
| struct dhcpv4_client_id client_id; |
| struct in_addr ciaddr, server_id; |
| int ret; |
| |
| ret = dhcpv4_find_server_id_option(options, optlen, &server_id); |
| if (ret < 0) { |
| /* No server ID, ignore. */ |
| return; |
| } |
| |
| if (!net_ipv4_addr_cmp(&ctx->server_addr, &server_id)) { |
| /* Not for us, ignore. */ |
| return; |
| } |
| |
| ret = dhcpv4_get_client_id(msg, options, optlen, &client_id); |
| if (ret < 0) { |
| /* Failed to obtain Client ID, ignore. */ |
| return; |
| } |
| |
| memcpy(&ciaddr, msg->ciaddr, sizeof(ciaddr)); |
| |
| for (int i = 0; i < ARRAY_SIZE(ctx->addr_pool); i++) { |
| struct dhcpv4_addr_slot *slot = &ctx->addr_pool[i]; |
| |
| if (net_ipv4_addr_cmp(&slot->addr, &ciaddr) && |
| slot->client_id.len == client_id.len && |
| memcmp(slot->client_id.buf, client_id.buf, |
| client_id.len) == 0 && |
| (slot->state == DHCPV4_SERVER_ADDR_RESERVED || |
| slot->state == DHCPV4_SERVER_ADDR_ALLOCATED)) { |
| LOG_DBG("DHCPv4 processing Release - %s", |
| net_sprint_ipv4_addr(&slot->addr)); |
| |
| slot->state = DHCPV4_SERVER_ADDR_FREE; |
| slot->expiry = sys_timepoint_calc(K_FOREVER); |
| dhcpv4_server_timeout_recalc(ctx); |
| break; |
| } |
| } |
| } |
| |
| static void dhcpv4_handle_inform(struct dhcpv4_server_ctx *ctx, |
| struct dhcp_msg *msg, uint8_t *options, |
| uint8_t optlen) |
| { |
| struct dhcpv4_parameter_request_list params = { 0 }; |
| |
| (void)dhcpv4_find_parameter_request_list_option(options, optlen, ¶ms); |
| (void)dhcpv4_send_ack(ctx, msg, (struct in_addr *)msg->ciaddr, 0, |
| ¶ms, NULL, true); |
| } |
| |
| /* Server core. */ |
| |
| static void dhcpv4_server_timeout(struct k_work *work) |
| { |
| struct k_work_delayable *dwork = k_work_delayable_from_work(work); |
| struct dhcpv4_server_ctx *ctx = |
| CONTAINER_OF(dwork, struct dhcpv4_server_ctx, timeout_work); |
| |
| k_mutex_lock(&server_lock, K_FOREVER); |
| |
| for (int i = 0; i < ARRAY_SIZE(ctx->addr_pool); i++) { |
| struct dhcpv4_addr_slot *slot = &ctx->addr_pool[i]; |
| |
| if ((slot->state == DHCPV4_SERVER_ADDR_RESERVED || |
| slot->state == DHCPV4_SERVER_ADDR_ALLOCATED) && |
| sys_timepoint_expired(slot->expiry)) { |
| if (slot->state == DHCPV4_SERVER_ADDR_RESERVED && |
| dhcpv4_server_is_slot_probed(ctx, slot)) { |
| dhcpv4_server_probe_timeout(ctx, slot); |
| } else { |
| LOG_DBG("Address %s expired", |
| net_sprint_ipv4_addr(&slot->addr)); |
| slot->state = DHCPV4_SERVER_ADDR_FREE; |
| } |
| } |
| |
| if (slot->state == DHCPV4_SERVER_ADDR_DECLINED && |
| sys_timepoint_expired(slot->expiry)) { |
| slot->state = DHCPV4_SERVER_ADDR_FREE; |
| } |
| } |
| |
| dhcpv4_server_timeout_recalc(ctx); |
| |
| k_mutex_unlock(&server_lock); |
| } |
| |
| static void dhcpv4_process_data(struct dhcpv4_server_ctx *ctx, uint8_t *data, |
| size_t datalen) |
| { |
| struct dhcp_msg *msg; |
| uint8_t msgtype; |
| int ret; |
| |
| if (datalen < sizeof(struct dhcp_msg)) { |
| LOG_DBG("DHCPv4 server malformed message"); |
| return; |
| } |
| |
| msg = (struct dhcp_msg *)data; |
| |
| if (msg->op != DHCPV4_MSG_BOOT_REQUEST) { |
| /* Silently drop messages other than BOOTREQUEST */ |
| return; |
| } |
| |
| data += sizeof(struct dhcp_msg); |
| datalen -= sizeof(struct dhcp_msg); |
| |
| /* Skip server hostname/filename/option cookie */ |
| if (datalen < (SIZE_OF_SNAME + SIZE_OF_FILE + SIZE_OF_MAGIC_COOKIE)) { |
| return; |
| } |
| |
| data += SIZE_OF_SNAME + SIZE_OF_FILE + SIZE_OF_MAGIC_COOKIE; |
| datalen -= SIZE_OF_SNAME + SIZE_OF_FILE + SIZE_OF_MAGIC_COOKIE; |
| |
| /* Search options for DHCP message type. */ |
| ret = dhcpv4_find_message_type_option(data, datalen, &msgtype); |
| if (ret < 0) { |
| LOG_ERR("No message type option"); |
| return; |
| } |
| |
| k_mutex_lock(&server_lock, K_FOREVER); |
| |
| switch (msgtype) { |
| case NET_DHCPV4_MSG_TYPE_DISCOVER: |
| dhcpv4_handle_discover(ctx, msg, data, datalen); |
| break; |
| case NET_DHCPV4_MSG_TYPE_REQUEST: |
| dhcpv4_handle_request(ctx, msg, data, datalen); |
| break; |
| case NET_DHCPV4_MSG_TYPE_DECLINE: |
| dhcpv4_handle_decline(ctx, msg, data, datalen); |
| break; |
| case NET_DHCPV4_MSG_TYPE_RELEASE: |
| dhcpv4_handle_release(ctx, msg, data, datalen); |
| break; |
| case NET_DHCPV4_MSG_TYPE_INFORM: |
| dhcpv4_handle_inform(ctx, msg, data, datalen); |
| break; |
| |
| case NET_DHCPV4_MSG_TYPE_OFFER: |
| case NET_DHCPV4_MSG_TYPE_ACK: |
| case NET_DHCPV4_MSG_TYPE_NAK: |
| default: |
| /* Ignore server initiated and unknown message types. */ |
| break; |
| } |
| |
| k_mutex_unlock(&server_lock); |
| } |
| |
| static void dhcpv4_server_cb(struct k_work *work) |
| { |
| struct net_socket_service_event *evt = |
| CONTAINER_OF(work, struct net_socket_service_event, work); |
| struct dhcpv4_server_ctx *ctx = NULL; |
| uint8_t recv_buf[NET_IPV4_MTU]; |
| int ret; |
| |
| for (int i = 0; i < ARRAY_SIZE(server_ctx); i++) { |
| if (server_ctx[i].sock == evt->event.fd) { |
| ctx = &server_ctx[i]; |
| break; |
| } |
| } |
| |
| if (ctx == NULL) { |
| LOG_ERR("No DHCPv4 server context found for given FD."); |
| return; |
| } |
| |
| if (evt->event.revents & ZSOCK_POLLERR) { |
| LOG_ERR("DHCPv4 server poll revents error"); |
| net_dhcpv4_server_stop(ctx->iface); |
| return; |
| } |
| |
| if (!(evt->event.revents & ZSOCK_POLLIN)) { |
| return; |
| } |
| |
| ret = zsock_recvfrom(evt->event.fd, recv_buf, sizeof(recv_buf), |
| ZSOCK_MSG_DONTWAIT, NULL, 0); |
| if (ret < 0) { |
| if (errno == EAGAIN) { |
| return; |
| } |
| |
| LOG_ERR("DHCPv4 server recv error, %d", errno); |
| net_dhcpv4_server_stop(ctx->iface); |
| return; |
| } |
| |
| dhcpv4_process_data(ctx, recv_buf, ret); |
| } |
| |
| NET_SOCKET_SERVICE_SYNC_DEFINE_STATIC(dhcpv4_server, NULL, dhcpv4_server_cb, |
| CONFIG_NET_DHCPV4_SERVER_INSTANCES); |
| |
| int net_dhcpv4_server_start(struct net_if *iface, struct in_addr *base_addr) |
| { |
| struct sockaddr_in addr = { |
| .sin_family = AF_INET, |
| .sin_addr = INADDR_ANY_INIT, |
| .sin_port = htons(DHCPV4_SERVER_PORT), |
| }; |
| struct ifreq ifreq = { 0 }; |
| int ret, sock = -1, slot = -1; |
| const struct in_addr *server_addr; |
| struct in_addr netmask; |
| |
| if (iface == NULL || base_addr == NULL) { |
| return -EINVAL; |
| } |
| |
| if (!net_if_ipv4_addr_mask_cmp(iface, base_addr)) { |
| LOG_ERR("Address pool does not belong to the interface subnet."); |
| return -EINVAL; |
| } |
| |
| server_addr = net_if_ipv4_select_src_addr(iface, base_addr); |
| if (server_addr == NULL) { |
| LOG_ERR("Failed to obtain a valid server address."); |
| return -EINVAL; |
| } |
| |
| if ((htonl(server_addr->s_addr) >= htonl(base_addr->s_addr)) && |
| (htonl(server_addr->s_addr) < |
| htonl(base_addr->s_addr) + CONFIG_NET_DHCPV4_SERVER_ADDR_COUNT)) { |
| LOG_ERR("Address pool overlaps with server address."); |
| return -EINVAL; |
| } |
| |
| netmask = net_if_ipv4_get_netmask_by_addr(iface, server_addr); |
| if (net_ipv4_is_addr_unspecified(&netmask)) { |
| LOG_ERR("Failed to obtain subnet mask."); |
| return -EINVAL; |
| } |
| |
| k_mutex_lock(&server_lock, K_FOREVER); |
| |
| for (int i = 0; i < ARRAY_SIZE(server_ctx); i++) { |
| if (server_ctx[i].iface != NULL) { |
| if (server_ctx[i].iface == iface) { |
| LOG_ERR("DHCPv4 server instance already running."); |
| ret = -EALREADY; |
| goto error; |
| } |
| } else { |
| if (slot < 0) { |
| slot = i; |
| } |
| } |
| } |
| |
| if (slot < 0) { |
| LOG_ERR("No free DHCPv4 server intance."); |
| ret = -ENOMEM; |
| goto error; |
| } |
| |
| ret = net_if_get_name(iface, ifreq.ifr_name, sizeof(ifreq.ifr_name)); |
| if (ret < 0) { |
| LOG_ERR("Failed to obtain interface name."); |
| goto error; |
| } |
| |
| sock = zsock_socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); |
| if (sock < 0) { |
| ret = errno; |
| LOG_ERR("Failed to create DHCPv4 server socket, %d", ret); |
| goto error; |
| } |
| |
| ret = zsock_setsockopt(sock, SOL_SOCKET, SO_BINDTODEVICE, &ifreq, |
| sizeof(ifreq)); |
| if (ret < 0) { |
| ret = errno; |
| LOG_ERR("Failed to bind DHCPv4 server socket with interface, %d", |
| ret); |
| goto error; |
| } |
| |
| ret = zsock_bind(sock, (struct sockaddr *)&addr, sizeof(addr)); |
| if (ret < 0) { |
| ret = errno; |
| LOG_ERR("Failed to bind DHCPv4 server socket, %d", ret); |
| goto error; |
| } |
| |
| fds[slot].fd = sock; |
| fds[slot].events = ZSOCK_POLLIN; |
| |
| server_ctx[slot].iface = iface; |
| server_ctx[slot].sock = sock; |
| server_ctx[slot].server_addr = *server_addr; |
| server_ctx[slot].netmask = netmask; |
| |
| k_work_init_delayable(&server_ctx[slot].timeout_work, |
| dhcpv4_server_timeout); |
| |
| LOG_DBG("Started DHCPv4 server, address pool:"); |
| for (int i = 0; i < ARRAY_SIZE(server_ctx[slot].addr_pool); i++) { |
| server_ctx[slot].addr_pool[i].state = DHCPV4_SERVER_ADDR_FREE; |
| server_ctx[slot].addr_pool[i].addr.s_addr = |
| htonl(ntohl(base_addr->s_addr) + i); |
| |
| LOG_DBG("\t%2d: %s", i, |
| net_sprint_ipv4_addr( |
| &server_ctx[slot].addr_pool[i].addr)); |
| } |
| |
| ret = dhcpv4_server_probing_init(&server_ctx[slot]); |
| if (ret < 0) { |
| LOG_ERR("Failed to register probe handler, %d", ret); |
| goto cleanup; |
| } |
| |
| ret = net_socket_service_register(&dhcpv4_server, fds, ARRAY_SIZE(fds), |
| NULL); |
| if (ret < 0) { |
| LOG_ERR("Failed to register socket service, %d", ret); |
| dhcpv4_server_probing_deinit(&server_ctx[slot]); |
| goto cleanup; |
| } |
| |
| k_mutex_unlock(&server_lock); |
| |
| return 0; |
| |
| cleanup: |
| memset(&server_ctx[slot], 0, sizeof(server_ctx[slot])); |
| fds[slot].fd = -1; |
| |
| error: |
| if (sock >= 0) { |
| (void)zsock_close(sock); |
| } |
| |
| k_mutex_unlock(&server_lock); |
| |
| return ret; |
| } |
| |
| int net_dhcpv4_server_stop(struct net_if *iface) |
| { |
| struct k_work_sync sync; |
| int slot = -1; |
| int ret = 0; |
| bool service_stop = true; |
| |
| if (iface == NULL) { |
| return -EINVAL; |
| } |
| |
| k_mutex_lock(&server_lock, K_FOREVER); |
| |
| for (int i = 0; i < ARRAY_SIZE(server_ctx); i++) { |
| if (server_ctx[i].iface == iface) { |
| slot = i; |
| break; |
| } |
| } |
| |
| if (slot < 0) { |
| ret = -ENOENT; |
| goto out; |
| } |
| |
| fds[slot].fd = -1; |
| (void)zsock_close(server_ctx[slot].sock); |
| |
| dhcpv4_server_probing_deinit(&server_ctx[slot]); |
| k_work_cancel_delayable_sync(&server_ctx[slot].timeout_work, &sync); |
| |
| memset(&server_ctx[slot], 0, sizeof(server_ctx[slot])); |
| |
| for (int i = 0; i < ARRAY_SIZE(fds); i++) { |
| if (fds[i].fd >= 0) { |
| service_stop = false; |
| break; |
| } |
| } |
| |
| if (service_stop) { |
| ret = net_socket_service_unregister(&dhcpv4_server); |
| } else { |
| ret = net_socket_service_register(&dhcpv4_server, fds, |
| ARRAY_SIZE(fds), NULL); |
| } |
| |
| out: |
| k_mutex_unlock(&server_lock); |
| |
| return ret; |
| } |
| |
| static void dhcpv4_server_foreach_lease_on_ctx(struct dhcpv4_server_ctx *ctx, |
| net_dhcpv4_lease_cb_t cb, |
| void *user_data) |
| { |
| for (int i = 0; i < ARRAY_SIZE(ctx->addr_pool); i++) { |
| struct dhcpv4_addr_slot *addr = &ctx->addr_pool[i]; |
| |
| if (addr->state != DHCPV4_SERVER_ADDR_FREE) { |
| cb(ctx->iface, addr, user_data); |
| } |
| } |
| } |
| |
| int net_dhcpv4_server_foreach_lease(struct net_if *iface, |
| net_dhcpv4_lease_cb_t cb, |
| void *user_data) |
| { |
| int slot = -1; |
| int ret = 0; |
| |
| k_mutex_lock(&server_lock, K_FOREVER); |
| |
| if (iface == NULL) { |
| for (int i = 0; i < ARRAY_SIZE(server_ctx); i++) { |
| if (server_ctx[i].iface != NULL) { |
| dhcpv4_server_foreach_lease_on_ctx( |
| &server_ctx[i], cb, user_data); |
| } |
| } |
| |
| return 0; |
| } |
| |
| for (int i = 0; i < ARRAY_SIZE(server_ctx); i++) { |
| if (server_ctx[i].iface == iface) { |
| slot = i; |
| break; |
| } |
| } |
| |
| if (slot < 0) { |
| ret = -ENOENT; |
| goto out; |
| } |
| |
| dhcpv4_server_foreach_lease_on_ctx(&server_ctx[slot], cb, user_data); |
| |
| out: |
| k_mutex_unlock(&server_lock); |
| |
| return ret; |
| } |
| |
| void net_dhcpv4_server_init(void) |
| { |
| for (int i = 0; i < ARRAY_SIZE(fds); i++) { |
| fds[i].fd = -1; |
| } |
| } |