| /* main.c - Application main entry point */ |
| |
| /* |
| * Copyright (c) 2016 Intel Corporation |
| * |
| * SPDX-License-Identifier: Apache-2.0 |
| */ |
| |
| #include <zephyr.h> |
| #include <linker/sections.h> |
| |
| #include <zephyr/types.h> |
| #include <stddef.h> |
| #include <string.h> |
| #include <stdio.h> |
| #include <errno.h> |
| #include <device.h> |
| #include <init.h> |
| #include <net/net_core.h> |
| #include <net/net_pkt.h> |
| #include <net/net_ip.h> |
| #include <net/dhcpv4.h> |
| #include <net/ethernet.h> |
| #include <net/net_mgmt.h> |
| #include <net/udp.h> |
| |
| #include <tc_util.h> |
| #include <ztest.h> |
| |
| #define NET_LOG_ENABLED 1 |
| #include "net_private.h" |
| |
| /* Sample DHCP offer (382 bytes) */ |
| static const unsigned char offer[382] = { |
| 0x02, 0x01, 0x06, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x0a, 0xed, 0x48, 0x9e, 0x0a, 0xb8, |
| 0x09, 0x01, 0x0a, 0xed, 0x48, 0x02, 0x00, 0x00, |
| 0x5E, 0x00, 0x53, 0x01, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x63, 0x82, |
| 0x53, 0x63, 0x35, 0x01, 0x02, 0x01, 0x04, 0xff, |
| 0xff, 0xff, 0x00, 0x3a, 0x04, 0x00, 0x00, 0x54, |
| 0x60, 0x3b, 0x04, 0x00, 0x00, 0x93, 0xa8, 0x33, |
| 0x04, 0x00, 0x00, 0xa8, 0xc0, 0x36, 0x04, 0x0a, |
| 0xb8, 0x09, 0x01, 0x03, 0x04, 0x0a, 0xed, 0x48, |
| 0x01, 0x0f, 0x0d, 0x66, 0x69, 0x2e, 0x69, 0x6e, |
| 0x74, 0x65, 0x6c, 0x2e, 0x63, 0x6f, 0x6d, 0x00, |
| 0x06, 0x0c, 0x0a, 0xf8, 0x02, 0x01, 0xa3, 0x21, |
| 0xfd, 0x44, 0x0a, 0xb8, 0x09, 0x01, 0x77, 0x3d, |
| 0x02, 0x66, 0x69, 0x05, 0x69, 0x6e, 0x74, 0x65, |
| 0x6c, 0x03, 0x63, 0x6f, 0x6d, 0x00, 0x03, 0x67, |
| 0x65, 0x72, 0x04, 0x63, 0x6f, 0x72, 0x70, 0x05, |
| 0x69, 0x6e, 0x74, 0x65, 0x6c, 0x03, 0x63, 0x6f, |
| 0x6d, 0x00, 0x04, 0x63, 0x6f, 0x72, 0x70, 0x05, |
| 0x69, 0x6e, 0x74, 0x65, 0x6c, 0x03, 0x63, 0x6f, |
| 0x6d, 0x00, 0x05, 0x69, 0x6e, 0x74, 0x65, 0x6c, |
| 0x03, 0x63, 0x6f, 0x6d, 0x00, 0x2c, 0x08, 0xa3, |
| 0x21, 0x07, 0x56, 0x8f, 0xb6, 0xfa, 0x69, 0xff |
| }; |
| |
| /* Sample DHCPv4 ACK */ |
| static const unsigned char ack[382] = { |
| 0x02, 0x01, 0x06, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x0a, 0xed, 0x48, 0x9e, 0x00, 0x00, 0x00, 0x00, |
| 0x0a, 0xed, 0x48, 0x03, 0x00, 0x00, 0x5E, 0x00, |
| 0x53, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| 0x00, 0x00, 0x00, 0x00, 0x63, 0x82, 0x53, 0x63, |
| 0x35, 0x01, 0x05, 0x3a, 0x04, 0x00, 0x00, 0x54, |
| 0x60, 0x3b, 0x04, 0x00, 0x00, 0x93, 0xa8, 0x33, |
| 0x04, 0x00, 0x00, 0xa8, 0xc0, 0x36, 0x04, 0x0a, |
| 0xb8, 0x09, 0x01, 0x01, 0x04, 0xff, 0xff, 0xff, |
| 0x00, 0x03, 0x04, 0x0a, 0xed, 0x48, 0x01, 0x0f, |
| 0x0d, 0x66, 0x69, 0x2e, 0x69, 0x6e, 0x74, 0x65, |
| 0x6c, 0x2e, 0x63, 0x6f, 0x6d, 0x00, 0x06, 0x0c, |
| 0x0a, 0xf8, 0x02, 0x01, 0xa3, 0x21, 0xfd, 0x44, |
| 0x0a, 0xb8, 0x09, 0x01, 0x77, 0x3d, 0x02, 0x66, |
| 0x69, 0x05, 0x69, 0x6e, 0x74, 0x65, 0x6c, 0x03, |
| 0x63, 0x6f, 0x6d, 0x00, 0x03, 0x67, 0x65, 0x72, |
| 0x04, 0x63, 0x6f, 0x72, 0x70, 0x05, 0x69, 0x6e, |
| 0x74, 0x65, 0x6c, 0x03, 0x63, 0x6f, 0x6d, 0x00, |
| 0x04, 0x63, 0x6f, 0x72, 0x70, 0x05, 0x69, 0x6e, |
| 0x74, 0x65, 0x6c, 0x03, 0x63, 0x6f, 0x6d, 0x00, |
| 0x05, 0x69, 0x6e, 0x74, 0x65, 0x6c, 0x03, 0x63, |
| 0x6f, 0x6d, 0x00, 0x2c, 0x08, 0xa3, 0x21, 0x07, |
| 0x56, 0x8f, 0xb6, 0xfa, 0x69, 0xff |
| }; |
| |
| static const struct net_eth_addr src_addr = { |
| { 0x00, 0x00, 0x5E, 0x00, 0x53, 0x01 } }; |
| static const struct net_eth_addr dst_addr = { |
| { 0x00, 0x00, 0x5E, 0x00, 0x53, 0x02 } }; |
| static const struct in_addr server_addr = { { { 192, 0, 2, 1 } } }; |
| static const struct in_addr client_addr = { { { 255, 255, 255, 255 } } }; |
| |
| #define SERVER_PORT 67 |
| #define CLIENT_PORT 68 |
| #define MSG_TYPE 53 |
| #define DISCOVER 1 |
| #define REQUEST 3 |
| |
| struct dhcp_msg { |
| u32_t xid; |
| u8_t type; |
| }; |
| |
| static void test_result(bool pass) |
| { |
| if (pass) { |
| TC_END_REPORT(TC_PASS); |
| } else { |
| TC_END_REPORT(TC_PASS); |
| } |
| } |
| |
| struct net_dhcpv4_context { |
| u8_t mac_addr[sizeof(struct net_eth_addr)]; |
| struct net_linkaddr ll_addr; |
| }; |
| |
| static int net_dhcpv4_dev_init(struct device *dev) |
| { |
| struct net_dhcpv4_context *net_dhcpv4_context = dev->driver_data; |
| |
| net_dhcpv4_context = net_dhcpv4_context; |
| |
| return 0; |
| } |
| |
| static u8_t *net_dhcpv4_get_mac(struct device *dev) |
| { |
| struct net_dhcpv4_context *context = dev->driver_data; |
| |
| if (context->mac_addr[2] == 0x00) { |
| /* 00-00-5E-00-53-xx Documentation RFC 7042 */ |
| context->mac_addr[0] = 0x00; |
| context->mac_addr[1] = 0x00; |
| context->mac_addr[2] = 0x5E; |
| context->mac_addr[3] = 0x00; |
| context->mac_addr[4] = 0x53; |
| context->mac_addr[5] = 0x01; |
| } |
| |
| return context->mac_addr; |
| } |
| |
| static void net_dhcpv4_iface_init(struct net_if *iface) |
| { |
| u8_t *mac = net_dhcpv4_get_mac(net_if_get_device(iface)); |
| |
| net_if_set_link_addr(iface, mac, 6, NET_LINK_ETHERNET); |
| } |
| |
| static struct net_buf *pkt_get_data(struct net_pkt *pkt, struct net_if *iface) |
| { |
| struct net_buf *frag; |
| struct net_eth_hdr *hdr; |
| |
| net_pkt_set_ll_reserve(pkt, net_if_get_ll_reserve(iface, NULL)); |
| |
| frag = net_pkt_get_frag(pkt, K_FOREVER); |
| if (!frag) { |
| return NULL; |
| } |
| |
| hdr = (struct net_eth_hdr *)(frag->data - net_pkt_ll_reserve(pkt)); |
| hdr->type = htons(NET_ETH_PTYPE_IP); |
| |
| net_ipaddr_copy(&hdr->dst, &src_addr); |
| net_ipaddr_copy(&hdr->src, &dst_addr); |
| |
| return frag; |
| } |
| |
| static void set_ipv4_header(struct net_pkt *pkt) |
| { |
| struct net_ipv4_hdr *ipv4; |
| u16_t length; |
| |
| ipv4 = NET_IPV4_HDR(pkt); |
| |
| ipv4->vhl = 0x45; /* IP version and header length */ |
| ipv4->tos = 0x00; |
| |
| length = sizeof(offer) + sizeof(struct net_ipv4_hdr) + |
| sizeof(struct net_udp_hdr); |
| |
| ipv4->len = htons(length); |
| |
| memset(ipv4->id, 0, 4); /* id and offset */ |
| |
| ipv4->ttl = 0xFF; |
| ipv4->proto = IPPROTO_UDP; |
| |
| net_ipaddr_copy(&ipv4->src, &server_addr); |
| net_ipaddr_copy(&ipv4->dst, &client_addr); |
| } |
| |
| static void set_udp_header(struct net_pkt *pkt) |
| { |
| struct net_udp_hdr *udp; |
| u16_t length; |
| |
| udp = (struct net_udp_hdr *)((u8_t *)(NET_IPV4_HDR(pkt)) + |
| sizeof(struct net_ipv4_hdr)); |
| |
| udp->src_port = htons(SERVER_PORT); |
| udp->dst_port = htons(CLIENT_PORT); |
| |
| length = sizeof(offer) + sizeof(struct net_udp_hdr); |
| udp->len = htons(length); |
| udp->chksum = 0; |
| } |
| |
| struct net_pkt *prepare_dhcp_offer(struct net_if *iface, u32_t xid) |
| { |
| struct net_pkt *pkt; |
| struct net_buf *frag; |
| int bytes, remaining = sizeof(offer), pos = 0; |
| u16_t offset; |
| |
| pkt = net_pkt_get_reserve_rx(0, K_FOREVER); |
| if (!pkt) { |
| return NULL; |
| } |
| |
| frag = pkt_get_data(pkt, iface); |
| if (!frag) { |
| net_pkt_unref(pkt); |
| return NULL; |
| } |
| |
| net_pkt_set_iface(pkt, iface); |
| net_pkt_set_ll_reserve(pkt, net_buf_headroom(frag)); |
| net_pkt_set_family(pkt, AF_INET); |
| net_pkt_set_ip_hdr_len(pkt, sizeof(struct net_ipv4_hdr)); |
| |
| net_pkt_frag_add(pkt, frag); |
| |
| /* Place the IPv4 header */ |
| set_ipv4_header(pkt); |
| |
| /* Place the UDP header */ |
| set_udp_header(pkt); |
| |
| net_buf_add(frag, NET_IPV4UDPH_LEN); |
| offset = NET_IPV4UDPH_LEN; |
| |
| while (remaining > 0) { |
| int copy; |
| |
| bytes = net_buf_tailroom(frag); |
| copy = remaining > bytes ? bytes : remaining; |
| |
| memcpy(frag->data + offset, &offer[pos], copy); |
| |
| net_buf_add(frag, copy); |
| |
| pos += bytes; |
| remaining -= bytes; |
| |
| if (remaining > 0) { |
| frag = pkt_get_data(pkt, iface); |
| if (!frag) { |
| goto fail; |
| } |
| |
| offset = 0; |
| net_pkt_frag_add(pkt, frag); |
| } |
| } |
| |
| /* Now fixup the expect XID */ |
| frag = net_pkt_write_be32(pkt, pkt->frags, |
| (sizeof(struct net_ipv4_hdr) + |
| sizeof(struct net_udp_hdr)) + 4, |
| &offset, xid); |
| return pkt; |
| |
| fail: |
| net_pkt_unref(pkt); |
| return NULL; |
| } |
| |
| struct net_pkt *prepare_dhcp_ack(struct net_if *iface, u32_t xid) |
| { |
| struct net_pkt *pkt; |
| struct net_buf *frag; |
| int bytes, remaining = sizeof(ack), pos = 0; |
| u16_t offset; |
| |
| pkt = net_pkt_get_reserve_rx(0, K_FOREVER); |
| if (!pkt) { |
| return NULL; |
| } |
| |
| frag = pkt_get_data(pkt, iface); |
| if (!frag) { |
| net_pkt_unref(pkt); |
| return NULL; |
| } |
| |
| net_pkt_set_iface(pkt, iface); |
| net_pkt_set_ll_reserve(pkt, net_buf_headroom(frag)); |
| net_pkt_set_family(pkt, AF_INET); |
| net_pkt_set_ip_hdr_len(pkt, sizeof(struct net_ipv4_hdr)); |
| |
| net_pkt_frag_add(pkt, frag); |
| |
| /* Place the IPv4 header */ |
| set_ipv4_header(pkt); |
| |
| /* Place the UDP header */ |
| set_udp_header(pkt); |
| |
| net_buf_add(frag, NET_IPV4UDPH_LEN); |
| offset = NET_IPV4UDPH_LEN; |
| |
| while (remaining > 0) { |
| int copy; |
| |
| bytes = net_buf_tailroom(frag); |
| copy = remaining > bytes ? bytes : remaining; |
| |
| memcpy(frag->data + offset, &ack[pos], copy); |
| |
| net_buf_add(frag, copy); |
| |
| pos += bytes; |
| remaining -= bytes; |
| |
| if (remaining > 0) { |
| frag = pkt_get_data(pkt, iface); |
| if (!frag) { |
| goto fail; |
| } |
| |
| offset = 0; |
| net_pkt_frag_add(pkt, frag); |
| } |
| } |
| |
| /* Now fixup the expect XID */ |
| frag = net_pkt_write_be32(pkt, pkt->frags, |
| (sizeof(struct net_ipv4_hdr) + |
| sizeof(struct net_udp_hdr)) + 4, |
| &offset, xid); |
| return pkt; |
| |
| fail: |
| net_pkt_unref(pkt); |
| return NULL; |
| } |
| |
| static int parse_dhcp_message(struct net_pkt *pkt, struct dhcp_msg *msg) |
| { |
| struct net_buf *frag = pkt->frags; |
| u8_t type; |
| u16_t offset; |
| |
| frag = net_frag_skip(frag, 0, &offset, |
| /* size of op, htype, hlen, hops */ |
| (sizeof(struct net_ipv4_hdr) + |
| sizeof(struct net_udp_hdr)) + 4); |
| if (!frag) { |
| return 0; |
| } |
| |
| frag = net_frag_read_be32(frag, offset, &offset, &msg->xid); |
| if (!frag) { |
| return 0; |
| } |
| |
| frag = net_frag_skip(frag, offset, &offset, |
| /* size of op, htype ... cookie */ |
| (36 + 64 + 128 + 4)); |
| if (!frag) { |
| return 0; |
| } |
| |
| while (frag) { |
| u8_t length; |
| |
| frag = net_frag_read_u8(frag, offset, &offset, &type); |
| if (!frag) { |
| return 0; |
| } |
| |
| if (type == MSG_TYPE) { |
| frag = net_frag_skip(frag, offset, &offset, 1); |
| if (!frag) { |
| return 0; |
| } |
| |
| frag = net_frag_read_u8(frag, offset, &offset, |
| &msg->type); |
| if (!frag) { |
| return 0; |
| } |
| |
| return 1; |
| } |
| |
| frag = net_frag_read_u8(frag, offset, &offset, &length); |
| if (frag) { |
| frag = net_frag_skip(frag, offset, &offset, length); |
| if (!frag) { |
| return 0; |
| } |
| } |
| } |
| |
| return 0; |
| } |
| |
| static int tester_send(struct net_if *iface, struct net_pkt *pkt) |
| { |
| struct net_pkt *rpkt; |
| struct dhcp_msg msg; |
| |
| memset(&msg, 0, sizeof(msg)); |
| |
| if (!pkt->frags) { |
| TC_PRINT("No data to send!\n"); |
| |
| return -ENODATA; |
| } |
| |
| parse_dhcp_message(pkt, &msg); |
| |
| if (msg.type == DISCOVER) { |
| /* Reply with DHCPv4 offer message */ |
| rpkt = prepare_dhcp_offer(iface, msg.xid); |
| if (!rpkt) { |
| return -EINVAL; |
| } |
| } else if (msg.type == REQUEST) { |
| /* Reply with DHCPv4 ACK message */ |
| rpkt = prepare_dhcp_ack(iface, msg.xid); |
| if (!rpkt) { |
| return -EINVAL; |
| } |
| |
| } else { |
| /* Invalid message type received */ |
| return -EINVAL; |
| } |
| |
| if (net_recv_data(iface, rpkt)) { |
| net_pkt_unref(rpkt); |
| |
| return -EINVAL; |
| } |
| |
| net_pkt_unref(pkt); |
| return NET_OK; |
| } |
| |
| struct net_dhcpv4_context net_dhcpv4_context_data; |
| |
| static struct net_if_api net_dhcpv4_if_api = { |
| .init = net_dhcpv4_iface_init, |
| .send = tester_send, |
| }; |
| |
| NET_DEVICE_INIT(net_dhcpv4_test, "net_dhcpv4_test", |
| net_dhcpv4_dev_init, &net_dhcpv4_context_data, NULL, |
| CONFIG_KERNEL_INIT_PRIORITY_DEFAULT, |
| &net_dhcpv4_if_api, DUMMY_L2, |
| NET_L2_GET_CTX_TYPE(DUMMY_L2), 127); |
| |
| static struct net_mgmt_event_callback rx_cb; |
| |
| static void receiver_cb(struct net_mgmt_event_callback *cb, |
| u32_t nm_event, struct net_if *iface) |
| { |
| if (nm_event != NET_EVENT_IPV4_ADDR_ADD) { |
| /* Spurious callback. */ |
| return; |
| } |
| |
| test_result(true); |
| } |
| |
| void test_dhcp(void) |
| { |
| struct net_if *iface; |
| |
| k_thread_priority_set(k_current_get(), K_PRIO_COOP(7)); |
| net_mgmt_init_event_callback(&rx_cb, receiver_cb, |
| NET_EVENT_IPV4_ADDR_ADD); |
| |
| net_mgmt_add_event_callback(&rx_cb); |
| |
| iface = net_if_get_default(); |
| if (!iface) { |
| TC_PRINT("Interface not available n"); |
| return; |
| } |
| |
| net_dhcpv4_start(iface); |
| |
| k_yield(); |
| } |
| |
| /**test case main entry */ |
| void test_main(void) |
| { |
| ztest_test_suite(test_dhcpv4, |
| ztest_unit_test(test_dhcp)); |
| ztest_run_test_suite(test_dhcpv4); |
| } |