static struct pico_dhcp_server_negotiation *pico_dhcp_server_add_negotiation(struct pico_device *dev, struct pico_dhcp_hdr *hdr)
{
struct pico_dhcp_server_negotiation *dhcpn = NULL;
struct pico_dhcp_server_setting test = {
0
};
if (pico_dhcp_server_find_negotiation(hdr->xid))
return NULL;
dhcpn = PICO_ZALLOC(sizeof(struct pico_dhcp_server_negotiation));
if (!dhcpn) {
pico_err = PICO_ERR_ENOMEM;
return NULL;
}
dhcpn->xid = hdr->xid;
dhcpn->state = PICO_DHCP_STATE_DISCOVER;
dhcpn->bcast = ((short_be(hdr->flags) & PICO_DHCP_FLAG_BROADCAST) != 0) ? (1) : (0);
memcpy(dhcpn->hwaddr.addr, hdr->hwaddr, PICO_SIZE_ETH);
test.dev = dev;
dhcpn->dhcps = pico_tree_findKey(&DHCPSettings, &test);
if (!dhcpn->dhcps) {
dhcps_dbg("DHCP server WARNING: received DHCP message on unconfigured link %s\n", dev->name);
PICO_FREE(dhcpn);
return NULL;
}
dhcp_negotiation_set_ciaddr(dhcpn);
pico_tree_insert(&DHCPNegotiations, dhcpn);
return dhcpn;
}
static struct pico_dhcp_server_negotiation *pico_dhcp_server_add_negotiation(struct pico_device *dev, struct pico_dhcp_hdr *hdr)
{
struct pico_dhcp_server_negotiation *dhcpn = NULL;
struct pico_dhcp_server_setting test = {0};
struct pico_ip4 *ciaddr = NULL;
if (pico_dhcp_server_find_negotiation(hdr->xid))
return NULL;
dhcpn = pico_zalloc(sizeof(struct pico_dhcp_server_negotiation));
if (!dhcpn) {
pico_err = PICO_ERR_ENOMEM;
return NULL;
}
dhcpn->xid = hdr->xid;
dhcpn->state = PICO_DHCP_STATE_DISCOVER;
memcpy(dhcpn->hwaddr.addr, hdr->hwaddr, PICO_SIZE_ETH);
test.dev = dev;
dhcpn->dhcps = pico_tree_findKey(&DHCPSettings, &test);
if (!dhcpn->dhcps) {
dhcps_dbg("DHCP server WARNING: received DHCP message on unconfigured link %s\n", dev->name);
pico_free(dhcpn);
return NULL;
}
ciaddr = pico_arp_reverse_lookup(&dhcpn->hwaddr);
if (!ciaddr) {
dhcpn->ciaddr.addr = dhcpn->dhcps->pool_next;
dhcpn->dhcps->pool_next = long_be(long_be(dhcpn->dhcps->pool_next) + 1);
pico_arp_create_entry(dhcpn->hwaddr.addr, dhcpn->ciaddr, dhcpn->dhcps->dev);
} else {
dhcpn->ciaddr = *ciaddr;
}
pico_tree_insert(&DHCPNegotiations, dhcpn);
return dhcpn;
}
static void dhcpd_make_reply(struct pico_dhcp_server_negotiation *dhcpn, uint8_t msg_type)
{
int r = 0, optlen = 0, offset = 0;
struct pico_ip4 broadcast = {
0
}, dns = {
0
}, destination = {
.addr = 0xFFFFFFFF
};
struct pico_dhcp_hdr *hdr = NULL;
dns.addr = DHCP_SERVER_OPENDNS;
broadcast.addr = dhcpn->dhcps->server_ip.addr | ~(dhcpn->dhcps->netmask.addr);
optlen = PICO_DHCP_OPTLEN_MSGTYPE + PICO_DHCP_OPTLEN_SERVERID + PICO_DHCP_OPTLEN_LEASETIME + PICO_DHCP_OPTLEN_NETMASK + PICO_DHCP_OPTLEN_ROUTER
+ PICO_DHCP_OPTLEN_BROADCAST + PICO_DHCP_OPTLEN_DNS + PICO_DHCP_OPTLEN_END;
hdr = PICO_ZALLOC(sizeof(struct pico_dhcp_hdr) + (uint32_t)optlen);
if (!hdr) {
return;
}
hdr->op = PICO_DHCP_OP_REPLY;
hdr->htype = PICO_DHCP_HTYPE_ETH;
hdr->hlen = PICO_SIZE_ETH;
hdr->xid = dhcpn->xid;
hdr->yiaddr = dhcpn->ciaddr.addr;
hdr->siaddr = dhcpn->dhcps->server_ip.addr;
hdr->dhcp_magic = PICO_DHCPD_MAGIC_COOKIE;
memcpy(hdr->hwaddr, dhcpn->hwaddr.addr, PICO_SIZE_ETH);
/* options */
offset += pico_dhcp_opt_msgtype(DHCP_OPT(hdr,offset), msg_type);
offset += pico_dhcp_opt_serverid(DHCP_OPT(hdr,offset), &dhcpn->dhcps->server_ip);
offset += pico_dhcp_opt_leasetime(DHCP_OPT(hdr,offset), dhcpn->dhcps->lease_time);
offset += pico_dhcp_opt_netmask(DHCP_OPT(hdr,offset), &dhcpn->dhcps->netmask);
offset += pico_dhcp_opt_router(DHCP_OPT(hdr,offset), &dhcpn->dhcps->server_ip);
offset += pico_dhcp_opt_broadcast(DHCP_OPT(hdr,offset), &broadcast);
offset += pico_dhcp_opt_dns(DHCP_OPT(hdr,offset), &dns);
offset += pico_dhcp_opt_end(DHCP_OPT(hdr,offset));
if (dhcpn->bcast == 0)
destination.addr = hdr->yiaddr;
else {
hdr->flags |= short_be(PICO_DHCP_FLAG_BROADCAST);
destination.addr = broadcast.addr;
}
r = pico_socket_sendto(dhcpn->dhcps->s, hdr, (int)(sizeof(struct pico_dhcp_hdr) + (uint32_t)optlen), &destination, PICO_DHCP_CLIENT_PORT);
if (r < 0)
dhcps_dbg("DHCP server WARNING: failure sending: %s!\n", strerror(pico_err));
PICO_FREE(hdr);
return;
}
static inline void parse_opt_msgtype(struct pico_dhcp_opt *opt, uint8_t *msgtype)
{
if (opt->code == PICO_DHCP_OPT_MSGTYPE) {
*msgtype = opt->ext.msg_type.type;
dhcps_dbg("DHCP server: message type %u\n", msgtype);
}
}
static inline void parse_opt_reqip(struct pico_dhcp_opt *opt, struct pico_ip4 *reqip)
{
if (opt->code == PICO_DHCP_OPT_REQIP)
reqip->addr = opt->ext.req_ip.ip.addr;
}
static inline void parse_opt_serverid(struct pico_dhcp_opt *opt, struct pico_ip4 *serverid)
{
if (opt->code == PICO_DHCP_OPT_SERVERID)
*serverid = opt->ext.server_id.ip;
}
static inline void dhcps_make_reply_to_request_msg(struct pico_dhcp_server_negotiation *dhcpn, int bound_valid_flag)
{
if ((dhcpn->state == PICO_DHCP_STATE_BOUND) && bound_valid_flag)
dhcpd_make_reply(dhcpn, PICO_DHCP_MSG_ACK);
if (dhcpn->state == PICO_DHCP_STATE_OFFER) {
dhcpn->state = PICO_DHCP_STATE_BOUND;
dhcpd_make_reply(dhcpn, PICO_DHCP_MSG_ACK);
}
}
static inline void dhcps_make_reply_to_discover_or_request(struct pico_dhcp_server_negotiation *dhcpn, uint8_t msgtype, int bound_valid_flag)
{
if (PICO_DHCP_MSG_DISCOVER == msgtype) {
dhcpd_make_reply(dhcpn, PICO_DHCP_MSG_OFFER);
dhcpn->state = PICO_DHCP_STATE_OFFER;
} else if (PICO_DHCP_MSG_REQUEST == msgtype) {
dhcps_make_reply_to_request_msg(dhcpn, bound_valid_flag);
}
}
static inline void dhcps_parse_options_loop(struct pico_dhcp_server_negotiation *dhcpn, struct pico_dhcp_hdr *hdr)
{
struct pico_dhcp_opt *opt = DHCP_OPT(hdr,0);
uint8_t msgtype = 0;
struct pico_ip4 reqip = {
0
}, server_id = {
0
};
do {
parse_opt_msgtype(opt, &msgtype);
parse_opt_reqip(opt, &reqip);
parse_opt_serverid(opt, &server_id);
} while (pico_dhcp_next_option(&opt));
dhcps_make_reply_to_discover_or_request(dhcpn, msgtype, (!reqip.addr) && (!server_id.addr) && (hdr->ciaddr == dhcpn->ciaddr.addr));
}
static void pico_dhcp_server_recv(struct pico_socket *s, uint8_t *buf, uint32_t len)
{
int32_t optlen = (int32_t)(len - sizeof(struct pico_dhcp_hdr));
struct pico_dhcp_hdr *hdr = (struct pico_dhcp_hdr *)buf;
struct pico_dhcp_server_negotiation *dhcpn = NULL;
struct pico_device *dev = NULL;
if (!pico_dhcp_are_options_valid(DHCP_OPT(hdr,0), optlen))
return;
dev = pico_ipv4_link_find(&s->local_addr.ip4);
dhcpn = pico_dhcp_server_find_negotiation(hdr->xid);
if (!dhcpn)
dhcpn = pico_dhcp_server_add_negotiation(dev, hdr);
if (!ip_address_is_in_dhcp_range(dhcpn, dhcpn->ciaddr.addr))
return;
dhcps_parse_options_loop(dhcpn, hdr);
}
static void dhcpd_make_reply(struct pico_dhcp_server_negotiation *dhcpn, uint8_t msg_type)
{
int r = 0, optlen = 0, offset = 0;
struct pico_ip4 broadcast = {0}, dns = {0}, destination = { .addr = 0xFFFFFFFF};
struct pico_dhcp_hdr *hdr = NULL;
dns.addr = DHCP_SERVER_OPENDNS;
broadcast.addr = dhcpn->dhcps->server_ip.addr | ~(dhcpn->dhcps->netmask.addr);
optlen = PICO_DHCP_OPTLEN_MSGTYPE + PICO_DHCP_OPTLEN_SERVERID + PICO_DHCP_OPTLEN_LEASETIME + PICO_DHCP_OPTLEN_NETMASK + PICO_DHCP_OPTLEN_ROUTER
+ PICO_DHCP_OPTLEN_BROADCAST + PICO_DHCP_OPTLEN_DNS + PICO_DHCP_OPTLEN_END;
hdr = pico_zalloc(sizeof(struct pico_dhcp_hdr) + (uint32_t)optlen);
hdr->op = PICO_DHCP_OP_REPLY;
hdr->htype = PICO_DHCP_HTYPE_ETH;
hdr->hlen = PICO_SIZE_ETH;
hdr->xid = dhcpn->xid;
hdr->yiaddr = dhcpn->ciaddr.addr;
hdr->siaddr = dhcpn->dhcps->server_ip.addr;
hdr->dhcp_magic = PICO_DHCPD_MAGIC_COOKIE;
memcpy(hdr->hwaddr, dhcpn->hwaddr.addr, PICO_SIZE_ETH);
/* options */
offset += pico_dhcp_opt_msgtype(&hdr->options[offset], msg_type);
offset += pico_dhcp_opt_serverid(&hdr->options[offset], &dhcpn->dhcps->server_ip);
offset += pico_dhcp_opt_leasetime(&hdr->options[offset], dhcpn->dhcps->lease_time);
offset += pico_dhcp_opt_netmask(&hdr->options[offset], &dhcpn->dhcps->netmask);
offset += pico_dhcp_opt_router(&hdr->options[offset], &dhcpn->dhcps->server_ip);
offset += pico_dhcp_opt_broadcast(&hdr->options[offset], &broadcast);
offset += pico_dhcp_opt_dns(&hdr->options[offset], &dns);
offset += pico_dhcp_opt_end(&hdr->options[offset]);
destination.addr = hdr->yiaddr;
r = pico_socket_sendto(dhcpn->dhcps->s, hdr, (int)(sizeof(struct pico_dhcp_hdr) + (uint32_t)optlen), &destination, PICO_DHCP_CLIENT_PORT);
if (r < 0)
dhcps_dbg("DHCP server WARNING: failure sending: %s!\n", strerror(pico_err));
return;
}
static void pico_dhcp_server_recv(struct pico_socket *s, uint8_t *buf, uint32_t len)
{
uint8_t msgtype = 0;
int32_t optlen = (int32_t)(len - sizeof(struct pico_dhcp_hdr));
struct pico_dhcp_hdr *hdr = (struct pico_dhcp_hdr *)buf;
struct pico_dhcp_opt *opt = (struct pico_dhcp_opt *)hdr->options;
struct pico_dhcp_server_negotiation *dhcpn = NULL;
struct pico_ip4 reqip = {0}, server_id = {0};
struct pico_device *dev = NULL;
if (!pico_dhcp_are_options_valid(hdr->options, optlen))
return;
dev = pico_ipv4_link_find(&s->local_addr.ip4);
dhcpn = pico_dhcp_server_find_negotiation(hdr->xid);
if (!dhcpn)
dhcpn = pico_dhcp_server_add_negotiation(dev, hdr);
if (!ip_inrange(dhcpn->ciaddr.addr))
return;
do {
switch (opt->code)
{
case PICO_DHCP_OPT_PAD:
break;
case PICO_DHCP_OPT_END:
break;
case PICO_DHCP_OPT_MSGTYPE:
msgtype = opt->ext.msg_type.type;
dhcps_dbg("DHCP server: message type %u\n", msgtype);
break;
case PICO_DHCP_OPT_REQIP:
reqip = opt->ext.req_ip.ip;
dhcps_dbg("DHCP server: requested IP %08X\n", reqip.addr);
break;
case PICO_DHCP_OPT_SERVERID:
server_id = opt->ext.server_id.ip;
dhcps_dbg("DHCP server: server ID %08X\n", server_id.addr);
break;
default:
dhcps_dbg("DHCP server WARNING: unsupported option %u\n", opt->code);
break;
}
} while (pico_dhcp_next_option(&opt));
switch (msgtype)
{
case PICO_DHCP_MSG_DISCOVER:
dhcpd_make_reply(dhcpn, PICO_DHCP_MSG_OFFER);
dhcpn->state = PICO_DHCP_STATE_OFFER;
break;
case PICO_DHCP_MSG_REQUEST:
if ((dhcpn->state == PICO_DHCP_STATE_BOUND) && (!reqip.addr) && (!server_id.addr) && (hdr->ciaddr == dhcpn->ciaddr.addr))
dhcpd_make_reply(dhcpn, PICO_DHCP_MSG_ACK);
if (dhcpn->state == PICO_DHCP_STATE_OFFER) {
dhcpn->state = PICO_DHCP_STATE_BOUND;
dhcpd_make_reply(dhcpn, PICO_DHCP_MSG_ACK);
}
break;
default:
dhcps_dbg("DHCP server WARNING: unsupported message type %u\n", msgtype);
break;
}
return;
}
