liubin | 281ac46 | 2023-07-19 14:22:54 +0800 | [diff] [blame] | 1 | #include <stdio.h> |
| 2 | #include <stdlib.h> |
| 3 | #include <errno.h> |
| 4 | #include <sys/time.h> |
| 5 | #include <poll.h> |
| 6 | #include <string.h> |
| 7 | #include <sys/socket.h> |
| 8 | #include <sys/uio.h> |
| 9 | #include <linux/if_ether.h> |
| 10 | #include <linux/if_packet.h> |
| 11 | #include <netinet/in.h> |
| 12 | #include <netinet/ip.h> |
| 13 | #include <netinet/udp.h> |
| 14 | #include <unistd.h> |
| 15 | #include <sys/select.h> |
| 16 | #include <sys/types.h> |
| 17 | #include <arpa/inet.h> |
| 18 | #include <time.h> |
| 19 | |
| 20 | #include "mbtk_log.h" |
| 21 | #include "mbtk_type.h" |
| 22 | #include "mbtk_ifc.h" |
| 23 | #include "mbtk_dhcp.h" |
| 24 | |
| 25 | #define VERBOSE 2 |
| 26 | #define STATE_SELECTING 1 |
| 27 | #define STATE_REQUESTING 2 |
| 28 | |
| 29 | #define TIMEOUT_INITIAL 4000 |
| 30 | #define TIMEOUT_MAX 32000 |
| 31 | |
| 32 | static int verbose = 1; |
| 33 | // static char errmsg[2048]; |
| 34 | |
| 35 | static void *init_dhcp_msg(dhcp_msg *msg, int type, void *hwaddr, uint32_t xid) |
| 36 | { |
| 37 | uint8_t *x; |
| 38 | |
| 39 | memset(msg, 0, sizeof(dhcp_msg)); |
| 40 | |
| 41 | msg->op = OP_BOOTREQUEST; |
| 42 | msg->htype = HTYPE_ETHER; |
| 43 | msg->hlen = 6; |
| 44 | msg->hops = 0; |
| 45 | |
| 46 | msg->flags = htons(FLAGS_BROADCAST); |
| 47 | |
| 48 | msg->xid = xid; |
| 49 | |
| 50 | memcpy(msg->chaddr, hwaddr, 6); |
| 51 | |
| 52 | x = msg->options; |
| 53 | |
| 54 | *x++ = OPT_COOKIE1; |
| 55 | *x++ = OPT_COOKIE2; |
| 56 | *x++ = OPT_COOKIE3; |
| 57 | *x++ = OPT_COOKIE4; |
| 58 | |
| 59 | *x++ = OPT_MESSAGE_TYPE; |
| 60 | *x++ = 1; |
| 61 | *x++ = type; |
| 62 | |
| 63 | return x; |
| 64 | } |
| 65 | |
| 66 | static int init_dhcp_discover_msg(dhcp_msg *msg, void *hwaddr, uint32_t xid) |
| 67 | { |
| 68 | uint8_t *x; |
| 69 | |
| 70 | x = init_dhcp_msg(msg, DHCPDISCOVER, hwaddr, xid); |
| 71 | |
| 72 | *x++ = OPT_PARAMETER_LIST; |
| 73 | *x++ = 4; |
| 74 | *x++ = OPT_SUBNET_MASK; |
| 75 | *x++ = OPT_GATEWAY; |
| 76 | *x++ = OPT_DNS; |
| 77 | *x++ = OPT_BROADCAST_ADDR; |
| 78 | |
| 79 | *x++ = OPT_END; |
| 80 | |
| 81 | return DHCP_MSG_FIXED_SIZE + (x - msg->options); |
| 82 | } |
| 83 | |
| 84 | static int init_dhcp_request_msg(dhcp_msg *msg, void *hwaddr, uint32_t xid, |
| 85 | uint32_t ipaddr, uint32_t serveraddr) |
| 86 | { |
| 87 | uint8_t *x; |
| 88 | |
| 89 | x = init_dhcp_msg(msg, DHCPREQUEST, hwaddr, xid); |
| 90 | |
| 91 | *x++ = OPT_PARAMETER_LIST; |
| 92 | *x++ = 4; |
| 93 | *x++ = OPT_SUBNET_MASK; |
| 94 | *x++ = OPT_GATEWAY; |
| 95 | *x++ = OPT_DNS; |
| 96 | *x++ = OPT_BROADCAST_ADDR; |
| 97 | |
| 98 | *x++ = OPT_REQUESTED_IP; |
| 99 | *x++ = 4; |
| 100 | memcpy(x, &ipaddr, 4); |
| 101 | x += 4; |
| 102 | |
| 103 | *x++ = OPT_SERVER_ID; |
| 104 | *x++ = 4; |
| 105 | memcpy(x, &serveraddr, 4); |
| 106 | x += 4; |
| 107 | |
| 108 | *x++ = OPT_END; |
| 109 | |
| 110 | return DHCP_MSG_FIXED_SIZE + (x - msg->options); |
| 111 | } |
| 112 | |
| 113 | |
| 114 | static msecs_t get_msecs(void) |
| 115 | { |
| 116 | struct timespec ts; |
| 117 | |
| 118 | if (clock_gettime(CLOCK_MONOTONIC, &ts)) |
| 119 | { |
| 120 | return 0; |
| 121 | } |
| 122 | else |
| 123 | { |
| 124 | return (((msecs_t) ts.tv_sec) * ((msecs_t) 1000)) + |
| 125 | (((msecs_t) ts.tv_nsec) / ((msecs_t) 1000000)); |
| 126 | } |
| 127 | } |
| 128 | |
| 129 | static int open_raw_socket(const char *ifname __attribute__((unused)), uint8_t *hwaddr, int if_index) |
| 130 | { |
| 131 | int s; |
| 132 | struct sockaddr_ll bindaddr; |
| 133 | |
| 134 | if((s = socket(PF_PACKET, SOCK_DGRAM, htons(ETH_P_IP))) < 0) |
| 135 | { |
| 136 | LOGE("socket(PF_PACKET)"); |
| 137 | return -1; |
| 138 | } |
| 139 | |
| 140 | memset(&bindaddr, 0, sizeof(bindaddr)); |
| 141 | bindaddr.sll_family = AF_PACKET; |
| 142 | bindaddr.sll_protocol = htons(ETH_P_IP); |
| 143 | bindaddr.sll_halen = ETH_ALEN; |
| 144 | memcpy(bindaddr.sll_addr, hwaddr, ETH_ALEN); |
| 145 | bindaddr.sll_ifindex = if_index; |
| 146 | |
| 147 | if (bind(s, (struct sockaddr *)&bindaddr, sizeof(bindaddr)) < 0) |
| 148 | { |
| 149 | LOGE("Cannot bind raw socket to interface"); |
| 150 | return -1; |
| 151 | } |
| 152 | |
| 153 | return s; |
| 154 | } |
| 155 | |
| 156 | static uint32_t checksum(void *buffer, unsigned int count, uint32_t startsum) |
| 157 | { |
| 158 | uint16_t *up = (uint16_t *)buffer; |
| 159 | uint32_t sum = startsum; |
| 160 | uint32_t upper16; |
| 161 | |
| 162 | while (count > 1) |
| 163 | { |
| 164 | sum += *up++; |
| 165 | count -= 2; |
| 166 | } |
| 167 | if (count > 0) |
| 168 | { |
| 169 | sum += (uint16_t) *(uint8_t *)up; |
| 170 | } |
| 171 | while ((upper16 = (sum >> 16)) != 0) |
| 172 | { |
| 173 | sum = (sum & 0xffff) + upper16; |
| 174 | } |
| 175 | return sum; |
| 176 | } |
| 177 | |
| 178 | static uint32_t finish_sum(uint32_t sum) |
| 179 | { |
| 180 | return ~sum & 0xffff; |
| 181 | } |
| 182 | |
| 183 | static int send_packet(int s, int if_index, dhcp_msg *msg, int size, |
| 184 | uint32_t saddr, uint32_t daddr, uint32_t sport, uint32_t dport) |
| 185 | { |
| 186 | struct iphdr ip; |
| 187 | struct udphdr udp; |
| 188 | struct iovec iov[3]; |
| 189 | uint32_t udpsum; |
| 190 | uint16_t temp; |
| 191 | struct msghdr msghdr; |
| 192 | struct sockaddr_ll destaddr; |
| 193 | |
| 194 | ip.version = IPVERSION; |
| 195 | ip.ihl = sizeof(ip) >> 2; |
| 196 | ip.tos = 0; |
| 197 | ip.tot_len = htons(sizeof(ip) + sizeof(udp) + size); |
| 198 | ip.id = 0; |
| 199 | ip.frag_off = 0; |
| 200 | ip.ttl = IPDEFTTL; |
| 201 | ip.protocol = IPPROTO_UDP; |
| 202 | ip.check = 0; |
| 203 | ip.saddr = saddr; |
| 204 | ip.daddr = daddr; |
| 205 | ip.check = finish_sum(checksum(&ip, sizeof(ip), 0)); |
| 206 | |
| 207 | udp.source = htons(sport); |
| 208 | udp.dest = htons(dport); |
| 209 | udp.len = htons(sizeof(udp) + size); |
| 210 | udp.check = 0; |
| 211 | |
| 212 | /* Calculate checksum for pseudo header */ |
| 213 | udpsum = checksum(&ip.saddr, sizeof(ip.saddr), 0); |
| 214 | udpsum = checksum(&ip.daddr, sizeof(ip.daddr), udpsum); |
| 215 | temp = htons(IPPROTO_UDP); |
| 216 | udpsum = checksum(&temp, sizeof(temp), udpsum); |
| 217 | temp = udp.len; |
| 218 | udpsum = checksum(&temp, sizeof(temp), udpsum); |
| 219 | |
| 220 | /* Add in the checksum for the udp header */ |
| 221 | udpsum = checksum(&udp, sizeof(udp), udpsum); |
| 222 | |
| 223 | /* Add in the checksum for the data */ |
| 224 | udpsum = checksum(msg, size, udpsum); |
| 225 | udp.check = finish_sum(udpsum); |
| 226 | |
| 227 | iov[0].iov_base = (char *)&ip; |
| 228 | iov[0].iov_len = sizeof(ip); |
| 229 | iov[1].iov_base = (char *)&udp; |
| 230 | iov[1].iov_len = sizeof(udp); |
| 231 | iov[2].iov_base = (char *)msg; |
| 232 | iov[2].iov_len = size; |
| 233 | memset(&destaddr, 0, sizeof(destaddr)); |
| 234 | destaddr.sll_family = AF_PACKET; |
| 235 | destaddr.sll_protocol = htons(ETH_P_IP); |
| 236 | destaddr.sll_ifindex = if_index; |
| 237 | destaddr.sll_halen = ETH_ALEN; |
| 238 | memcpy(destaddr.sll_addr, "\xff\xff\xff\xff\xff\xff", ETH_ALEN); |
| 239 | |
| 240 | msghdr.msg_name = &destaddr; |
| 241 | msghdr.msg_namelen = sizeof(destaddr); |
| 242 | msghdr.msg_iov = iov; |
| 243 | msghdr.msg_iovlen = sizeof(iov) / sizeof(struct iovec); |
| 244 | msghdr.msg_flags = 0; |
| 245 | msghdr.msg_control = 0; |
| 246 | msghdr.msg_controllen = 0; |
| 247 | return sendmsg(s, &msghdr, 0); |
| 248 | } |
| 249 | |
| 250 | static int receive_packet(int s, dhcp_msg *msg) |
| 251 | { |
| 252 | int nread; |
| 253 | int is_valid; |
| 254 | struct dhcp_packet |
| 255 | { |
| 256 | struct iphdr ip; |
| 257 | struct udphdr udp; |
| 258 | dhcp_msg dhcp; |
| 259 | } packet; |
| 260 | int dhcp_size; |
| 261 | uint32_t sum; |
| 262 | uint16_t temp; |
| 263 | uint32_t saddr, daddr; |
| 264 | |
| 265 | nread = read(s, &packet, sizeof(packet)); |
| 266 | if (nread < 0) |
| 267 | { |
| 268 | return -1; |
| 269 | } |
| 270 | /* |
| 271 | * The raw packet interface gives us all packets received by the |
| 272 | * network interface. We need to filter out all packets that are |
| 273 | * not meant for us. |
| 274 | */ |
| 275 | is_valid = 0; |
| 276 | if (nread < (int)(sizeof(struct iphdr) + sizeof(struct udphdr))) |
| 277 | { |
| 278 | #if VERBOSE |
| 279 | LOGD("Packet is too small (%d) to be a UDP datagram", nread); |
| 280 | #endif |
| 281 | } |
| 282 | else if (packet.ip.version != IPVERSION || packet.ip.ihl != (sizeof(packet.ip) >> 2)) |
| 283 | { |
| 284 | #if VERBOSE |
| 285 | LOGD("Not a valid IP packet"); |
| 286 | #endif |
| 287 | } |
| 288 | else if (nread < ntohs(packet.ip.tot_len)) |
| 289 | { |
| 290 | #if VERBOSE |
| 291 | LOGD("Packet was truncated (read %d, needed %d)", nread, ntohs(packet.ip.tot_len)); |
| 292 | #endif |
| 293 | } |
| 294 | else if (packet.ip.protocol != IPPROTO_UDP) |
| 295 | { |
| 296 | #if VERBOSE |
| 297 | LOGD("IP protocol (%d) is not UDP", packet.ip.protocol); |
| 298 | #endif |
| 299 | } |
| 300 | else if (packet.udp.dest != htons(PORT_BOOTP_CLIENT)) |
| 301 | { |
| 302 | #if VERBOSE |
| 303 | LOGD("UDP dest port (%d) is not DHCP client", ntohs(packet.udp.dest)); |
| 304 | #endif |
| 305 | } |
| 306 | else |
| 307 | { |
| 308 | is_valid = 1; |
| 309 | } |
| 310 | |
| 311 | if (!is_valid) |
| 312 | { |
| 313 | return -1; |
| 314 | } |
| 315 | |
| 316 | /* Seems like it's probably a valid DHCP packet */ |
| 317 | /* validate IP header checksum */ |
| 318 | sum = finish_sum(checksum(&packet.ip, sizeof(packet.ip), 0)); |
| 319 | if (sum != 0) |
| 320 | { |
| 321 | LOGW("IP header checksum failure (0x%x)", packet.ip.check); |
| 322 | return -1; |
| 323 | } |
| 324 | /* |
| 325 | * Validate the UDP checksum. |
| 326 | * Since we don't need the IP header anymore, we "borrow" it |
| 327 | * to construct the pseudo header used in the checksum calculation. |
| 328 | */ |
| 329 | dhcp_size = ntohs(packet.udp.len) - sizeof(packet.udp); |
| 330 | /* |
| 331 | * check validity of dhcp_size. |
| 332 | * 1) cannot be negative or zero. |
| 333 | * 2) src buffer contains enough bytes to copy |
| 334 | * 3) cannot exceed destination buffer |
| 335 | */ |
| 336 | if ((dhcp_size <= 0) || |
| 337 | ((int)(nread - sizeof(struct iphdr) - sizeof(struct udphdr)) < dhcp_size) || |
| 338 | ((int)sizeof(dhcp_msg) < dhcp_size)) |
| 339 | { |
| 340 | #if VERBOSE |
| 341 | LOGD("Malformed Packet"); |
| 342 | #endif |
| 343 | return -1; |
| 344 | } |
| 345 | saddr = packet.ip.saddr; |
| 346 | daddr = packet.ip.daddr; |
| 347 | nread = ntohs(packet.ip.tot_len); |
| 348 | memset(&packet.ip, 0, sizeof(packet.ip)); |
| 349 | packet.ip.saddr = saddr; |
| 350 | packet.ip.daddr = daddr; |
| 351 | packet.ip.protocol = IPPROTO_UDP; |
| 352 | packet.ip.tot_len = packet.udp.len; |
| 353 | temp = packet.udp.check; |
| 354 | packet.udp.check = 0; |
| 355 | sum = finish_sum(checksum(&packet, nread, 0)); |
| 356 | packet.udp.check = temp; |
| 357 | if (!sum) |
| 358 | sum = finish_sum(sum); |
| 359 | if (temp != sum) |
| 360 | { |
| 361 | LOGW("UDP header checksum failure (0x%x should be 0x%x)", sum, temp); |
| 362 | return -1; |
| 363 | } |
| 364 | memcpy(msg, &packet.dhcp, dhcp_size); |
| 365 | return dhcp_size; |
| 366 | } |
| 367 | |
| 368 | static void hex2str(char *buf, size_t buf_size, const unsigned char *array, int len) |
| 369 | { |
| 370 | int i; |
| 371 | char *cp = buf; |
| 372 | char *buf_end = buf + buf_size; |
| 373 | for (i = 0; i < len; i++) |
| 374 | { |
| 375 | cp += snprintf(cp, buf_end - cp, " %02x ", array[i]); |
| 376 | } |
| 377 | } |
| 378 | |
| 379 | static const char *ipaddr(in_addr_t addr) |
| 380 | { |
| 381 | struct in_addr in_addr; |
| 382 | |
| 383 | in_addr.s_addr = addr; |
| 384 | return inet_ntoa(in_addr); |
| 385 | } |
| 386 | |
| 387 | static const char *dhcp_type_to_name(uint32_t type) |
| 388 | { |
| 389 | switch(type) |
| 390 | { |
| 391 | case DHCPDISCOVER: |
| 392 | return "discover"; |
| 393 | case DHCPOFFER: |
| 394 | return "offer"; |
| 395 | case DHCPREQUEST: |
| 396 | return "request"; |
| 397 | case DHCPDECLINE: |
| 398 | return "decline"; |
| 399 | case DHCPACK: |
| 400 | return "ack"; |
| 401 | case DHCPNAK: |
| 402 | return "nak"; |
| 403 | case DHCPRELEASE: |
| 404 | return "release"; |
| 405 | case DHCPINFORM: |
| 406 | return "inform"; |
| 407 | default: |
| 408 | return "???"; |
| 409 | } |
| 410 | } |
| 411 | |
| 412 | static void dump_dhcp_msg(dhcp_msg *msg, int len) |
| 413 | { |
| 414 | unsigned char *x; |
| 415 | unsigned int n,c; |
| 416 | int optsz; |
| 417 | const char *name; |
| 418 | char buf[2048]; |
| 419 | |
| 420 | LOGD("===== DHCP message:"); |
| 421 | if (len < DHCP_MSG_FIXED_SIZE) |
| 422 | { |
| 423 | LOGD("Invalid length %d, should be %d", len, DHCP_MSG_FIXED_SIZE); |
| 424 | return; |
| 425 | } |
| 426 | |
| 427 | len -= DHCP_MSG_FIXED_SIZE; |
| 428 | |
| 429 | if (msg->op == OP_BOOTREQUEST) |
| 430 | name = "BOOTREQUEST"; |
| 431 | else if (msg->op == OP_BOOTREPLY) |
| 432 | name = "BOOTREPLY"; |
| 433 | else |
| 434 | name = "????"; |
| 435 | LOGD("op = %s (%d), htype = %d, hlen = %d, hops = %d", |
| 436 | name, msg->op, msg->htype, msg->hlen, msg->hops); |
| 437 | LOGD("xid = 0x%08x secs = %d, flags = 0x%04x optlen = %d", |
| 438 | ntohl(msg->xid), ntohs(msg->secs), ntohs(msg->flags), len); |
| 439 | LOGD("ciaddr = %s", ipaddr(msg->ciaddr)); |
| 440 | LOGD("yiaddr = %s", ipaddr(msg->yiaddr)); |
| 441 | LOGD("siaddr = %s", ipaddr(msg->siaddr)); |
| 442 | LOGD("giaddr = %s", ipaddr(msg->giaddr)); |
| 443 | |
| 444 | c = msg->hlen > 16 ? 16 : msg->hlen; |
| 445 | hex2str(buf, sizeof(buf), msg->chaddr, c); |
| 446 | LOGD("chaddr = {%s}", buf); |
| 447 | |
| 448 | for (n = 0; n < 64; n++) |
| 449 | { |
| 450 | unsigned char x = msg->sname[n]; |
| 451 | if ((x < ' ') || (x > 127)) |
| 452 | { |
| 453 | if (x == 0) break; |
| 454 | msg->sname[n] = '.'; |
| 455 | } |
| 456 | } |
| 457 | msg->sname[63] = 0; |
| 458 | |
| 459 | for (n = 0; n < 128; n++) |
| 460 | { |
| 461 | unsigned char x = msg->file[n]; |
| 462 | if ((x < ' ') || (x > 127)) |
| 463 | { |
| 464 | if (x == 0) break; |
| 465 | msg->file[n] = '.'; |
| 466 | } |
| 467 | } |
| 468 | msg->file[127] = 0; |
| 469 | |
| 470 | LOGD("sname = '%s'", msg->sname); |
| 471 | LOGD("file = '%s'", msg->file); |
| 472 | |
| 473 | if (len < 4) return; |
| 474 | len -= 4; |
| 475 | x = msg->options + 4; |
| 476 | |
| 477 | while (len > 2) |
| 478 | { |
| 479 | if (*x == 0) |
| 480 | { |
| 481 | x++; |
| 482 | len--; |
| 483 | continue; |
| 484 | } |
| 485 | if (*x == OPT_END) |
| 486 | { |
| 487 | break; |
| 488 | } |
| 489 | len -= 2; |
| 490 | optsz = x[1]; |
| 491 | if (optsz > len) break; |
| 492 | if (x[0] == OPT_DOMAIN_NAME || x[0] == OPT_MESSAGE) |
| 493 | { |
| 494 | if ((unsigned int)optsz < sizeof(buf) - 1) |
| 495 | { |
| 496 | n = optsz; |
| 497 | } |
| 498 | else |
| 499 | { |
| 500 | n = sizeof(buf) - 1; |
| 501 | } |
| 502 | memcpy(buf, &x[2], n); |
| 503 | buf[n] = '\0'; |
| 504 | } |
| 505 | else |
| 506 | { |
| 507 | hex2str(buf, sizeof(buf), &x[2], optsz); |
| 508 | } |
| 509 | if (x[0] == OPT_MESSAGE_TYPE) |
| 510 | name = dhcp_type_to_name(x[2]); |
| 511 | else |
| 512 | name = NULL; |
| 513 | LOGD("op %d len %d {%s} %s", x[0], optsz, buf, name == NULL ? "" : name); |
| 514 | len -= optsz; |
| 515 | x = x + optsz + 2; |
| 516 | } |
| 517 | } |
| 518 | |
| 519 | static int send_message(int sock, int if_index, dhcp_msg *msg, int size) |
| 520 | { |
| 521 | #if VERBOSE > 1 |
| 522 | dump_dhcp_msg(msg, size); |
| 523 | #endif |
| 524 | return send_packet(sock, if_index, msg, size, INADDR_ANY, INADDR_BROADCAST, |
| 525 | PORT_BOOTP_CLIENT, PORT_BOOTP_SERVER); |
| 526 | } |
| 527 | |
| 528 | // static dhcp_info last_good_info; |
| 529 | static int dhcp_configure(const char *ifname, dhcp_info *info) |
| 530 | { |
| 531 | //last_good_info = *info; |
| 532 | return mbtk_ifc_configure1(ifname, info->ipaddr, info->prefixLength, info->gateway, 0); |
| 533 | } |
| 534 | |
| 535 | static int is_valid_reply(dhcp_msg *msg, dhcp_msg *reply, int sz) |
| 536 | { |
| 537 | if (sz < DHCP_MSG_FIXED_SIZE) |
| 538 | { |
| 539 | if (verbose) LOGD("Wrong size %d != %d\n", sz, DHCP_MSG_FIXED_SIZE); |
| 540 | return 0; |
| 541 | } |
| 542 | if (reply->op != OP_BOOTREPLY) |
| 543 | { |
| 544 | if (verbose) LOGD("Wrong Op %d != %d\n", reply->op, OP_BOOTREPLY); |
| 545 | return 0; |
| 546 | } |
| 547 | if (reply->xid != msg->xid) |
| 548 | { |
| 549 | if (verbose) LOGD("Wrong Xid 0x%x != 0x%x\n", ntohl(reply->xid), |
| 550 | ntohl(msg->xid)); |
| 551 | return 0; |
| 552 | } |
| 553 | if (reply->htype != msg->htype) |
| 554 | { |
| 555 | if (verbose) LOGD("Wrong Htype %d != %d\n", reply->htype, msg->htype); |
| 556 | return 0; |
| 557 | } |
| 558 | if (reply->hlen != msg->hlen) |
| 559 | { |
| 560 | if (verbose) LOGD("Wrong Hlen %d != %d\n", reply->hlen, msg->hlen); |
| 561 | return 0; |
| 562 | } |
| 563 | if (memcmp(msg->chaddr, reply->chaddr, msg->hlen)) |
| 564 | { |
| 565 | if (verbose) LOGD("Wrong chaddr %x != %x\n", *(reply->chaddr),*(msg->chaddr)); |
| 566 | return 0; |
| 567 | } |
| 568 | return 1; |
| 569 | } |
| 570 | |
| 571 | int ipv4NetmaskToPrefixLength(in_addr_t mask) |
| 572 | { |
| 573 | int prefixLength = 0; |
| 574 | uint32_t m = (uint32_t)ntohl(mask); |
| 575 | while (m & 0x80000000) |
| 576 | { |
| 577 | prefixLength++; |
| 578 | m = m << 1; |
| 579 | } |
| 580 | return prefixLength; |
| 581 | } |
| 582 | |
| 583 | static int decode_dhcp_msg(dhcp_msg *msg, int len, dhcp_info *info) |
| 584 | { |
| 585 | uint8_t *x; |
| 586 | unsigned int opt; |
| 587 | int optlen; |
| 588 | |
| 589 | memset(info, 0, sizeof(dhcp_info)); |
| 590 | if (len < (DHCP_MSG_FIXED_SIZE + 4)) return -1; |
| 591 | |
| 592 | len -= (DHCP_MSG_FIXED_SIZE + 4); |
| 593 | |
| 594 | if (msg->options[0] != OPT_COOKIE1) return -1; |
| 595 | if (msg->options[1] != OPT_COOKIE2) return -1; |
| 596 | if (msg->options[2] != OPT_COOKIE3) return -1; |
| 597 | if (msg->options[3] != OPT_COOKIE4) return -1; |
| 598 | |
| 599 | x = msg->options + 4; |
| 600 | |
| 601 | while (len > 2) |
| 602 | { |
| 603 | opt = *x++; |
| 604 | if (opt == OPT_PAD) |
| 605 | { |
| 606 | len--; |
| 607 | continue; |
| 608 | } |
| 609 | if (opt == OPT_END) |
| 610 | { |
| 611 | break; |
| 612 | } |
| 613 | optlen = *x++; |
| 614 | len -= 2; |
| 615 | if (optlen > len) |
| 616 | { |
| 617 | break; |
| 618 | } |
| 619 | switch(opt) |
| 620 | { |
| 621 | case OPT_SUBNET_MASK: |
| 622 | if (optlen >= 4) |
| 623 | { |
| 624 | in_addr_t mask; |
| 625 | memcpy(&mask, x, 4); |
| 626 | info->prefixLength = ipv4NetmaskToPrefixLength(mask); |
| 627 | } |
| 628 | break; |
| 629 | case OPT_GATEWAY: |
| 630 | if (optlen >= 4) memcpy(&info->gateway, x, 4); |
| 631 | break; |
| 632 | case OPT_DNS: |
| 633 | if (optlen >= 4) memcpy(&info->dns1, x + 0, 4); |
| 634 | if (optlen >= 8) memcpy(&info->dns2, x + 4, 4); |
| 635 | break; |
| 636 | case OPT_LEASE_TIME: |
| 637 | if (optlen >= 4) |
| 638 | { |
| 639 | memcpy(&info->lease, x, 4); |
| 640 | info->lease = ntohl(info->lease); |
| 641 | } |
| 642 | break; |
| 643 | case OPT_SERVER_ID: |
| 644 | if (optlen >= 4) memcpy(&info->serveraddr, x, 4); |
| 645 | break; |
| 646 | case OPT_MESSAGE_TYPE: |
| 647 | info->type = *x; |
| 648 | break; |
| 649 | default: |
| 650 | break; |
| 651 | } |
| 652 | x += optlen; |
| 653 | len -= optlen; |
| 654 | } |
| 655 | |
| 656 | info->ipaddr = msg->yiaddr; |
| 657 | |
| 658 | return 0; |
| 659 | } |
| 660 | |
| 661 | void dump_dhcp_info(dhcp_info *info) |
| 662 | { |
| 663 | char addr[20], gway[20]; |
| 664 | LOGD("--- dhcp %s (%d) ---", |
| 665 | dhcp_type_to_name(info->type), info->type); |
| 666 | strcpy(addr, ipaddr(info->ipaddr)); |
| 667 | strcpy(gway, ipaddr(info->gateway)); |
| 668 | LOGD("ip %s gw %s prefixLength %d", addr, gway, info->prefixLength); |
| 669 | if (info->dns1) LOGD("dns1: %s", ipaddr(info->dns1)); |
| 670 | if (info->dns2) LOGD("dns2: %s", ipaddr(info->dns2)); |
| 671 | LOGD("server %s, lease %d seconds", |
| 672 | ipaddr(info->serveraddr), info->lease); |
| 673 | } |
| 674 | |
| 675 | static int dhcp_init_ifc(const char *ifname) |
| 676 | { |
| 677 | dhcp_msg discover_msg; |
| 678 | dhcp_msg request_msg; |
| 679 | dhcp_msg reply; |
| 680 | dhcp_msg *msg; |
| 681 | dhcp_info info; |
| 682 | int s, r, size; |
| 683 | int valid_reply; |
| 684 | uint32_t xid; |
| 685 | unsigned char hwaddr[6]; |
| 686 | struct pollfd pfd; |
| 687 | unsigned int state; |
| 688 | unsigned int timeout; |
| 689 | int if_index; |
| 690 | |
| 691 | xid = (uint32_t) get_msecs(); |
| 692 | if (mbtk_ifc_get_hwaddr(ifname, hwaddr)) |
| 693 | { |
| 694 | LOGE("cannot obtain interface address"); |
| 695 | return -1; |
| 696 | } |
| 697 | if (mbtk_ifc_get_ifindex(ifname, &if_index)) |
| 698 | { |
| 699 | LOGE("cannot obtain interface index"); |
| 700 | return -1; |
| 701 | } |
| 702 | |
| 703 | s = open_raw_socket(ifname, hwaddr, if_index); |
| 704 | |
| 705 | timeout = TIMEOUT_INITIAL; |
| 706 | state = STATE_SELECTING; |
| 707 | info.type = 0; |
| 708 | goto transmit; |
| 709 | |
| 710 | for (;;) |
| 711 | { |
| 712 | pfd.fd = s; |
| 713 | pfd.events = POLLIN; |
| 714 | pfd.revents = 0; |
| 715 | r = poll(&pfd, 1, timeout); |
| 716 | |
| 717 | if (r == 0) |
| 718 | { |
| 719 | #if VERBOSE |
| 720 | LOGE("TIMEOUT"); |
| 721 | #endif |
| 722 | if (timeout >= TIMEOUT_MAX) |
| 723 | { |
| 724 | LOGE("timed out"); |
| 725 | if ( info.type == DHCPOFFER ) |
| 726 | { |
| 727 | LOGE("no acknowledgement from DHCP server\nconfiguring %s with offered parameters", ifname); |
| 728 | return dhcp_configure(ifname, &info); |
| 729 | } |
| 730 | errno = ETIME; |
| 731 | close(s); |
| 732 | return -1; |
| 733 | } |
| 734 | timeout = timeout * 2; |
| 735 | |
| 736 | transmit: |
| 737 | size = 0; |
| 738 | msg = NULL; |
| 739 | switch(state) |
| 740 | { |
| 741 | case STATE_SELECTING: |
| 742 | msg = &discover_msg; |
| 743 | size = init_dhcp_discover_msg(msg, hwaddr, xid); |
| 744 | break; |
| 745 | case STATE_REQUESTING: |
| 746 | msg = &request_msg; |
| 747 | size = init_dhcp_request_msg(msg, hwaddr, xid, info.ipaddr, info.serveraddr); |
| 748 | break; |
| 749 | default: |
| 750 | r = 0; |
| 751 | } |
| 752 | if (size != 0) |
| 753 | { |
| 754 | r = send_message(s, if_index, msg, size); |
| 755 | if (r < 0) |
| 756 | { |
| 757 | LOGE("error sending dhcp msg: %s\n", strerror(errno)); |
| 758 | } |
| 759 | } |
| 760 | continue; |
| 761 | } |
| 762 | |
| 763 | if (r < 0) |
| 764 | { |
| 765 | if ((errno == EAGAIN) || (errno == EINTR)) |
| 766 | { |
| 767 | continue; |
| 768 | } |
| 769 | LOGE("poll failed"); |
| 770 | return -1; |
| 771 | } |
| 772 | |
| 773 | errno = 0; |
| 774 | r = receive_packet(s, &reply); |
| 775 | if (r < 0) |
| 776 | { |
| 777 | if (errno != 0) |
| 778 | { |
| 779 | LOGD("receive_packet failed (%d): %s", r, strerror(errno)); |
| 780 | if (errno == ENETDOWN || errno == ENXIO) |
| 781 | { |
| 782 | return -1; |
| 783 | } |
| 784 | } |
| 785 | continue; |
| 786 | } |
| 787 | |
| 788 | #if VERBOSE > 1 |
| 789 | dump_dhcp_msg(&reply, r); |
| 790 | #endif |
| 791 | decode_dhcp_msg(&reply, r, &info); |
| 792 | |
| 793 | if (state == STATE_SELECTING) |
| 794 | { |
| 795 | valid_reply = is_valid_reply(&discover_msg, &reply, r); |
| 796 | } |
| 797 | else |
| 798 | { |
| 799 | valid_reply = is_valid_reply(&request_msg, &reply, r); |
| 800 | } |
| 801 | if (!valid_reply) |
| 802 | { |
| 803 | LOGE("invalid reply"); |
| 804 | continue; |
| 805 | } |
| 806 | |
| 807 | if (verbose) |
| 808 | dump_dhcp_info(&info); |
| 809 | |
| 810 | switch(state) |
| 811 | { |
| 812 | case STATE_SELECTING: |
| 813 | if (info.type == DHCPOFFER) |
| 814 | { |
| 815 | state = STATE_REQUESTING; |
| 816 | timeout = TIMEOUT_INITIAL; |
| 817 | xid++; |
| 818 | goto transmit; |
| 819 | } |
| 820 | break; |
| 821 | case STATE_REQUESTING: |
| 822 | if (info.type == DHCPACK) |
| 823 | { |
| 824 | LOGE("configuring %s", ifname); |
| 825 | close(s); |
| 826 | return dhcp_configure(ifname, &info); |
| 827 | } |
| 828 | else if (info.type == DHCPNAK) |
| 829 | { |
| 830 | LOGE("configuration request denied"); |
| 831 | close(s); |
| 832 | return -1; |
| 833 | } |
| 834 | else |
| 835 | { |
| 836 | LOGE("ignoring %s message in state %d", |
| 837 | dhcp_type_to_name(info.type), state); |
| 838 | } |
| 839 | break; |
| 840 | } |
| 841 | } |
| 842 | close(s); |
| 843 | return 0; |
| 844 | } |
| 845 | |
| 846 | |
| 847 | int mbtk_do_dhcp(const char *name) |
| 848 | { |
| 849 | int ret = 0; |
| 850 | if(mbtk_ifc_open()) |
| 851 | { |
| 852 | LOGE("mbtk_ifc_open() fail."); |
| 853 | ret = -1; |
| 854 | goto return_result; |
| 855 | } |
| 856 | |
| 857 | if (mbtk_ifc_set_addr(name, 0, 0)) |
| 858 | { |
| 859 | LOGE("failed to set ip addr for %s to 0.0.0.0: %s", name, strerror(errno)); |
| 860 | ret = -1; |
| 861 | goto return_result; |
| 862 | } |
| 863 | |
| 864 | if (mbtk_ifc_up(name)) |
| 865 | { |
| 866 | LOGE("failed to bring up interface %s: %s", name, strerror(errno)); |
| 867 | ret = -1; |
| 868 | goto return_result; |
| 869 | } |
| 870 | |
| 871 | if(dhcp_init_ifc(name)) |
| 872 | { |
| 873 | LOGE("dhcp_init_ifc() fail."); |
| 874 | ret = -1; |
| 875 | goto return_result; |
| 876 | } |
| 877 | |
| 878 | return_result: |
| 879 | |
| 880 | mbtk_ifc_close(); |
| 881 | |
| 882 | return ret; |
| 883 | } |
| 884 | |
| 885 | |