Ankita Bajaj | 1bde794 | 2018-01-09 19:15:01 +0530 | [diff] [blame] | 1 | /* |
| 2 | * Sigma Control API DUT (station/AP) |
| 3 | * Copyright (c) 2018, The Linux Foundation |
| 4 | * All Rights Reserved. |
| 5 | * Licensed under the Clear BSD license. See README for more details. |
| 6 | */ |
| 7 | |
| 8 | #include "sigma_dut.h" |
| 9 | #include "wpa_helpers.h" |
| 10 | #include <netinet/if_ether.h> |
| 11 | #include <netinet/ip.h> |
| 12 | #include <netinet/udp.h> |
| 13 | #include <pcap.h> |
| 14 | #include <signal.h> |
| 15 | |
| 16 | #define DHCP_SERVER_PORT 67 |
| 17 | #define DHCP_CLIENT_PORT 68 |
| 18 | #define DHCP_ACK 5 |
| 19 | |
| 20 | #define UDP_PROTOCOL 17 |
| 21 | |
| 22 | static pcap_t *pcap = NULL; |
| 23 | |
| 24 | struct dhcp_pkt { |
| 25 | struct iphdr iph; |
| 26 | struct udphdr udph; |
| 27 | u8 op; |
| 28 | u8 htype; |
| 29 | u8 hlen; |
| 30 | u8 hops; |
| 31 | u32 xid; |
| 32 | u16 secs; |
| 33 | u16 flags; |
| 34 | u32 client_ip; |
| 35 | u32 your_ip; |
| 36 | u32 server_ip; |
| 37 | u32 relay_ip; |
| 38 | u8 hw_addr[16]; |
| 39 | u8 serv_name[64]; |
| 40 | u8 boot_file[128]; |
| 41 | u32 magic_cookie; |
| 42 | u8 options[314]; |
| 43 | } __attribute__ ((packed)); |
| 44 | |
| 45 | enum dhcp_options { |
| 46 | DHCP_OPT_SUBNET_MASK = 1, |
| 47 | DHCP_OPT_ROUTER = 3, |
| 48 | DHCP_OPT_MSG_TYPE = 53, |
| 49 | DHCP_OPT_END = 255 |
| 50 | }; |
| 51 | |
| 52 | |
| 53 | static u8 * get_dhcp_option(u8 *options, u8 type, int len) |
| 54 | { |
| 55 | u8 *pos = options; |
| 56 | u8 *end = pos + len; |
| 57 | |
| 58 | while (pos < end && pos + 1 < end && pos + 2 + pos[1] <= end) { |
| 59 | if (*pos == type) |
| 60 | return pos; |
| 61 | pos += 2 + pos[1]; |
| 62 | } |
| 63 | return NULL; |
| 64 | } |
| 65 | |
| 66 | |
| 67 | /** |
| 68 | * 1. Open UDP socket |
| 69 | * 2. read_msg |
| 70 | * 3. Process DHCP_ACK |
| 71 | */ |
| 72 | static void * process_dhcp_ack(void *ptr) |
| 73 | { |
| 74 | struct bpf_program pcap_fp; |
| 75 | char pcap_filter[200], pcap_err[PCAP_ERRBUF_SIZE]; |
| 76 | struct sigma_dut *dut = ptr; |
| 77 | int nbytes = 0; |
| 78 | u8 buf[1024]; |
| 79 | struct dhcp_pkt *dhcp; |
| 80 | int option_len; |
| 81 | u8 *msg_type, *val; |
| 82 | struct in_addr ip; |
| 83 | char your_ip[16], mask[16], router[16]; |
| 84 | unsigned short protocol, port_no; |
| 85 | bpf_u_int32 pcap_maskp, pcap_netp; |
| 86 | char ifname[20]; |
| 87 | |
| 88 | protocol = UDP_PROTOCOL; |
| 89 | port_no = DHCP_SERVER_PORT; |
| 90 | |
| 91 | strlcpy(ifname, get_main_ifname(), sizeof(ifname)); |
| 92 | |
| 93 | /* gives the network mask for ifname essential for applying filter */ |
| 94 | pcap_lookupnet(ifname, &pcap_netp, &pcap_maskp, pcap_err); |
| 95 | |
| 96 | sigma_dut_print(dut, DUT_MSG_INFO, "DHCP: ifname = %s", ifname); |
| 97 | |
| 98 | /* creates a session for sniffing */ |
| 99 | pcap = pcap_open_live(ifname, 2500, 0, 10, pcap_err); |
Ankita Bajaj | 1d79b3a | 2018-02-09 13:12:18 +0530 | [diff] [blame] | 100 | if (!pcap) { |
Ankita Bajaj | 1bde794 | 2018-01-09 19:15:01 +0530 | [diff] [blame] | 101 | sigma_dut_print(dut, DUT_MSG_INFO, "pcap_open_live: %s", |
| 102 | pcap_err); |
Ankita Bajaj | 1d79b3a | 2018-02-09 13:12:18 +0530 | [diff] [blame] | 103 | goto exit; |
| 104 | } |
Ankita Bajaj | 1bde794 | 2018-01-09 19:15:01 +0530 | [diff] [blame] | 105 | |
| 106 | snprintf(pcap_filter, sizeof(pcap_filter), |
| 107 | "ip proto 0x%x and udp src port 0x%x", |
| 108 | protocol, port_no); |
| 109 | sigma_dut_print(dut, DUT_MSG_INFO, "pcap_flter %s", pcap_filter); |
| 110 | |
| 111 | if (pcap_compile(pcap, &pcap_fp, pcap_filter, 1, pcap_netp) < 0) |
| 112 | sigma_dut_print(dut, DUT_MSG_INFO, "pcap_compile: %s", |
| 113 | pcap_geterr(pcap)); |
| 114 | |
| 115 | if (pcap_setfilter(pcap, &pcap_fp) < 0) |
| 116 | sigma_dut_print(dut, DUT_MSG_INFO, "pcap_setfilter: %s", |
| 117 | pcap_geterr(pcap)); |
| 118 | |
| 119 | pcap_freecode(&pcap_fp); |
| 120 | |
| 121 | while (1) { |
| 122 | memset(buf, 0, sizeof(buf)); |
| 123 | sigma_dut_print(dut, DUT_MSG_DEBUG, |
| 124 | "HLP: Waiting for message to receive"); |
| 125 | nbytes = recvfrom(pcap_get_selectable_fd(pcap), &buf, |
| 126 | sizeof(buf), 0, NULL, NULL); |
| 127 | if (nbytes == -1) { |
| 128 | sigma_dut_print(dut, DUT_MSG_ERROR, "HLP: failed: %s", |
| 129 | strerror(errno)); |
| 130 | goto exit; |
| 131 | } |
| 132 | |
| 133 | sigma_dut_print(dut, DUT_MSG_INFO, "HLP: Received %d bytes", |
| 134 | nbytes); |
| 135 | hex_dump(dut, buf, nbytes); |
| 136 | |
| 137 | if (nbytes < 314) { |
| 138 | sigma_dut_print(dut, DUT_MSG_DEBUG, |
| 139 | "HLP: Ignore MSG, Too short message received"); |
| 140 | continue; |
| 141 | } |
| 142 | nbytes -= 14; |
| 143 | |
| 144 | /* |
| 145 | * Process DHCP packet |
| 146 | * skip ethernet header from buf and then process the ack |
| 147 | */ |
| 148 | dhcp = (struct dhcp_pkt *) (buf + ETH_HLEN); |
| 149 | |
| 150 | option_len = nbytes - ((char *) dhcp->options - (char *) dhcp); |
| 151 | |
| 152 | sigma_dut_print(dut, DUT_MSG_DEBUG, |
| 153 | "option_len %d, First option : %02x", |
| 154 | option_len, *(dhcp->options)); |
| 155 | |
| 156 | /* Check for DHCP_ACK */ |
| 157 | msg_type = get_dhcp_option(dhcp->options, DHCP_OPT_MSG_TYPE, |
| 158 | option_len); |
| 159 | if (!msg_type) { |
| 160 | sigma_dut_print(dut, DUT_MSG_ERROR, |
| 161 | "Ignore MSG, DHCP OPT MSG_TYPE missing"); |
| 162 | continue; |
| 163 | } |
| 164 | |
| 165 | if (msg_type[2] != DHCP_ACK) { |
| 166 | sigma_dut_print(dut, DUT_MSG_ERROR, |
| 167 | "Ignore MSG, DHCP message type : %02x", |
| 168 | msg_type[2]); |
| 169 | continue; |
| 170 | } |
| 171 | |
| 172 | ip.s_addr = dhcp->your_ip; |
| 173 | strlcpy(your_ip, inet_ntoa(ip), sizeof(your_ip)); |
| 174 | |
| 175 | val = get_dhcp_option(dhcp->options, DHCP_OPT_SUBNET_MASK, |
| 176 | option_len); |
| 177 | if (!val) { |
| 178 | sigma_dut_print(dut, DUT_MSG_ERROR, |
| 179 | "DHCP option SUBNET_MASK missing"); |
| 180 | continue; |
| 181 | } |
| 182 | |
| 183 | memcpy(&(ip.s_addr), (val + 2), val[1]); |
| 184 | strlcpy(mask, inet_ntoa(ip), sizeof(mask)); |
| 185 | |
| 186 | val = get_dhcp_option(dhcp->options, DHCP_OPT_ROUTER, |
| 187 | option_len); |
| 188 | if (!val) { |
| 189 | sigma_dut_print(dut, DUT_MSG_ERROR, |
| 190 | "DHCP option DHCP_OPT_ROUTER missing"); |
| 191 | continue; |
| 192 | } |
| 193 | memcpy(&(ip.s_addr), val + 2, val[1]); |
| 194 | strlcpy(router, inet_ntoa(ip), sizeof(router)); |
| 195 | |
| 196 | sigma_dut_print(dut, DUT_MSG_DEBUG, |
| 197 | "OP: %d, your_ip: %s, netmask: %s, router: %s", |
| 198 | dhcp->op, your_ip, mask, router); |
| 199 | /* set ip configuration */ |
| 200 | if (!set_ipv4_addr(dut, ifname, your_ip, mask)) { |
| 201 | sigma_dut_print(dut, DUT_MSG_ERROR, |
| 202 | "Failed to set IP address"); |
| 203 | continue; |
| 204 | } |
| 205 | if (set_ipv4_gw(dut, router) < 1) { |
| 206 | sigma_dut_print(dut, DUT_MSG_ERROR, |
| 207 | "Failed to set Gateway address"); |
| 208 | continue; |
| 209 | } |
| 210 | |
| 211 | sigma_dut_print(dut, DUT_MSG_DEBUG, |
| 212 | "IP configuration completed"); |
| 213 | } |
| 214 | |
| 215 | exit: |
| 216 | sigma_dut_print(dut, DUT_MSG_INFO, "HLP: Received failed in exit"); |
| 217 | if (pcap) { |
| 218 | pcap_close(pcap); |
| 219 | pcap = NULL; |
| 220 | } |
| 221 | dut->hlp_thread = 0; |
| 222 | return NULL; |
| 223 | } |
| 224 | |
| 225 | |
| 226 | static void hlp_thread_exit(int signum) |
| 227 | { |
| 228 | pthread_exit(0); |
| 229 | } |
| 230 | |
| 231 | |
| 232 | void hlp_thread_cleanup(struct sigma_dut *dut) |
| 233 | { |
| 234 | if (pcap) { |
| 235 | pcap_close(pcap); |
| 236 | pcap = NULL; |
| 237 | } |
| 238 | if (dut->hlp_thread) { |
| 239 | sigma_dut_print(dut, DUT_MSG_DEBUG, "Kill thread: %ld", |
| 240 | dut->hlp_thread); |
| 241 | pthread_kill(dut->hlp_thread, SIGUSR1); |
| 242 | dut->hlp_thread = 0; |
| 243 | } |
| 244 | } |
| 245 | |
| 246 | |
| 247 | void process_fils_hlp(struct sigma_dut *dut) |
| 248 | { |
| 249 | static pthread_t hlp_thread; |
| 250 | |
| 251 | if (dut->hlp_thread) { |
| 252 | sigma_dut_print(dut, DUT_MSG_ERROR, |
| 253 | "FILS-HLP DHCP thread already running"); |
| 254 | return; |
| 255 | } |
| 256 | |
| 257 | signal(SIGUSR1, hlp_thread_exit); |
| 258 | |
| 259 | sigma_dut_print(dut, DUT_MSG_DEBUG, "Creating FILS_HLP thread-->"); |
| 260 | |
| 261 | /* create FILS_HLP thread */ |
| 262 | if (!pthread_create(&hlp_thread, NULL, &process_dhcp_ack, |
| 263 | (void *) dut)) { |
| 264 | dut->hlp_thread = hlp_thread; |
| 265 | } else { |
| 266 | sigma_dut_print(dut, DUT_MSG_DEBUG, |
| 267 | "FILS_HLP thread creation failed"); |
| 268 | } |
| 269 | |
| 270 | } |