1#include <stdio.h> 2#include <sys/time.h> 3#include <sys/types.h> 4#include <sys/file.h> 5#include <unistd.h> 6#include <getopt.h> 7#include <stdlib.h> 8#include <sys/socket.h> 9#include <netinet/in.h> 10#include <arpa/inet.h> 11#include <signal.h> 12#include <time.h> 13#include <string.h> 14#include <sys/ioctl.h> 15#include <net/if.h> 16#include <errno.h> 17#include <netinet/udp.h> 18#include <netinet/ip.h> 19#include <netpacket/packet.h> 20#include <net/ethernet.h> 21#include <features.h> 22#include "dp.h" 23 24#define DHCP_DETECT 25#define DHCP_SOCKET 26 27#ifdef DEBUG 28#define csprintf(fmt, args...) do{\ 29 FILE *cp = fopen("/tmp/detect_wan_type.txt", "a+");\ 30 if(cp) {\ 31 fprintf(cp, fmt, ## args);\ 32 fclose(cp);\ 33 }\ 34}while(0) 35#else 36#define csprintf(fmt, args...) do{\ 37 FILE *cp = fopen("/dev/null", "a+");\ 38 if(cp) {\ 39 fprintf(cp, fmt, ## args);\ 40 fclose(cp);\ 41 }\ 42}while(0) 43#endif 44 45struct ifreq ifr; 46/***********************************************************************/ 47// ppp 48 49 50char * 51strDup(char const *str) 52{ 53 char *copy = malloc(strlen(str)+1); 54 if (!copy) { 55 //rp_fatal("strdup failed"); 56 csprintf("strdup failed\n"); 57 return (char*)0; 58 } 59 strcpy(copy, str); 60 return copy; 61} 62 63 64int 65openInterface(char const *ifname, UINT16_t type, unsigned char *hwaddr) 66{ 67 int optval=1; 68 int fd; 69 //struct ifreq ifr; 70 int domain, stype; 71 72 struct sockaddr sa; 73 74 memset(&sa, 0, sizeof(sa)); 75 76 domain = PF_INET; 77 stype = SOCK_PACKET; 78 79 if ((fd = socket(domain, stype, htons(type))) < 0) { 80 /* Give a more helpful message for the common error case */ 81 if (errno == EPERM) { 82 csprintf("Cannot create raw socket -- pppoe must be run as root.\n"); 83 return -1; 84 } 85 perror("socket"); 86 return -1; 87 } 88 89 if (setsockopt(fd, SOL_SOCKET, SO_BROADCAST, &optval, sizeof(optval)) < 0) { 90 perror("setsockopt"); 91 close(fd); 92 return -1; 93 } 94 95 /* Fill in hardware address */ 96 if (hwaddr) { 97 memcpy(hwaddr, ifr.ifr_hwaddr.sa_data, ETH_ALEN); 98 if (NOT_UNICAST(hwaddr)) { 99 char buffer[256]; 100 sprintf(buffer, "Interface %.16s has broadcast/multicast MAC address??", ifname); 101 //rp_fatal(buffer); 102 csprintf(buffer); 103 return -1; 104 } 105 } 106 107 /* Sanity check on MTU */ 108 strcpy(sa.sa_data, ifname); 109 110 /* We're only interested in packets on specified interface */ 111 if (bind(fd, (struct sockaddr *) &sa, sizeof(sa)) < 0) { 112 perror("bind "); 113 close(fd); 114 return -1; 115 } 116 117 return fd; 118} 119void 120dumpHex(FILE *fp, unsigned char const *buf, int len) 121{ 122 int i; 123 int base; 124 125 if (!fp) return; 126 127 /* do NOT dump PAP packets */ 128 if (len >= 2 && buf[0] == 0xC0 && buf[1] == 0x23) { 129 fprintf(fp, "(PAP Authentication Frame -- Contents not dumped)\n"); 130 return; 131 } 132 133 for (base=0; base<len; base += 16) { 134 for (i=base; i<base+16; i++) { 135 if (i < len) { 136 fprintf(fp, "%02x ", (unsigned) buf[i]); 137 } else { 138 fprintf(fp, " "); 139 } 140 } 141 fprintf(fp, " "); 142 for (i=base; i<base+16; i++) { 143 if (i < len) { 144 if (isprint(buf[i])) { 145 fprintf(fp, "%c", buf[i]); 146 } else { 147 fprintf(fp, "."); 148 } 149 } else { 150 break; 151 } 152 } 153 fprintf(fp, "\n"); 154 } 155} 156 157UINT16_t 158etherType(PPPoEPacket *packet) 159{ 160 UINT16_t type = (UINT16_t) ntohs(packet->ethHdr.h_proto); 161 if (type != Eth_PPPOE_Discovery && type != Eth_PPPOE_Session) { 162 //syslog(LOG_ERR, "Invalid ether type 0x%x", type); 163 csprintf("Invalid ether type 0x%x\n", type); 164 } 165 return type; 166} 167 168int 169parsePacket(PPPoEPacket *packet, ParseFunc *func, void *extra) 170{ 171 UINT16_t len = ntohs(packet->length); 172 unsigned char *curTag; 173 UINT16_t tagType, tagLen; 174 175 csprintf("parse packet\n"); 176 if (packet->ver != 1) { 177 //syslog(LOG_ERR, "Invalid PPPoE version (%d)", (int) packet->ver); 178 return -1; 179 } 180 if (packet->type != 1) { 181 //syslog(LOG_ERR, "Invalid PPPoE type (%d)", (int) packet->type); 182 return -1; 183 } 184 185 /* Do some sanity checks on packet */ 186 if (len > ETH_DATA_LEN - 6) { /* 6-byte overhead for PPPoE header */ 187 //syslog(LOG_ERR, "Invalid PPPoE packet length (%u)", len); 188 return -1; 189 } 190 191 /* Step through the tags */ 192 curTag = packet->payload; 193 while(curTag - packet->payload < len) { 194 /* Alignment is not guaranteed, so do this by hand... */ 195 tagType = (((UINT16_t) curTag[0]) << 8) + 196 (UINT16_t) curTag[1]; 197 tagLen = (((UINT16_t) curTag[2]) << 8) + 198 (UINT16_t) curTag[3]; 199 if (tagType == TAG_END_OF_LIST) { 200 return 0; 201 } 202 if ((curTag - packet->payload) + tagLen + TAG_HDR_SIZE > len) { 203 //syslog(LOG_ERR, "Invalid PPPoE tag length (%u)", tagLen); 204 return -1; 205 } 206 func(tagType, tagLen, curTag+TAG_HDR_SIZE, extra); 207 //curTag = curTag + TAG_HDR_SIZE + tagLen; 208 } 209 return 0; 210} 211 212void 213parseForHostUniq(UINT16_t type, UINT16_t len, unsigned char *data, 214 void *extra) 215{ 216 int *val = (int *) extra; 217 if (type == TAG_HOST_UNIQ && len == sizeof(pid_t)) { 218 pid_t tmp; 219 memcpy(&tmp, data, len); 220 if (tmp == getpid()) { 221 *val = 1; 222 } 223 } 224} 225 226int 227packetIsForMe(PPPoEConnection *conn, PPPoEPacket *packet) 228{ 229 int forMe = 0; 230 231 /* If packet is not directed to our MAC address, forget it */ 232 if (memcmp(packet->ethHdr.h_dest, conn->myEth, ETH_ALEN)) return 0; 233 234 235 /* If we're not using the Host-Unique tag, then accept the packet */ 236 if (!conn->useHostUniq) return 1; 237 238 parsePacket(packet, parseForHostUniq, &forMe); 239 return forMe; 240} 241 242int 243sendPacket(PPPoEConnection *conn, int sock, PPPoEPacket *pkt, int size) 244{ 245 struct sockaddr sa; 246 247 if (!conn) { 248 csprintf("relay and server not supported on Linux 2.0 kernels\n"); 249 return -1; 250 } 251 strcpy(sa.sa_data, conn->ifName); 252 if (sendto(sock, pkt, size, 0, &sa, sizeof(sa)) < 0) { 253 return -1; 254 } 255 return 0; 256} 257 258int 259receivePacket(int sock, PPPoEPacket *pkt, int *size) 260{ 261 if ((*size = recv(sock, pkt, sizeof(PPPoEPacket), 0)) < 0) { 262 return -1; 263 } 264 return 0; 265} 266 267void 268sendPADI(PPPoEConnection *conn) 269{ 270 PPPoEPacket packet; 271 unsigned char *cursor = packet.payload; 272 PPPoETag *svc = (PPPoETag *) (&packet.payload); 273 UINT16_t namelen = 0; 274 UINT16_t plen; 275 if (conn->serviceName) { 276 namelen = (UINT16_t) strlen(conn->serviceName); 277 } 278 plen = TAG_HDR_SIZE + namelen; 279 CHECK_ROOM(cursor, packet.payload, plen); 280 281 /* Set destination to Ethernet broadcast address */ 282 memset(packet.ethHdr.h_dest, 0xFF, ETH_ALEN); 283 memcpy(packet.ethHdr.h_source, conn->myEth, ETH_ALEN); 284 285 packet.ethHdr.h_proto = htons(Eth_PPPOE_Discovery); 286 packet.ver = 1; 287 packet.type = 1; 288 packet.code = CODE_PADI; 289 packet.session = 0; 290 291 svc->type = TAG_SERVICE_NAME; 292 svc->length = htons(namelen); 293 CHECK_ROOM(cursor, packet.payload, namelen+TAG_HDR_SIZE); 294 295 if (conn->serviceName) { 296 memcpy(svc->payload, conn->serviceName, strlen(conn->serviceName)); 297 } 298 cursor += namelen + TAG_HDR_SIZE; 299 300 /* If we're using Host-Uniq, copy it over */ 301 if (conn->useHostUniq) { 302 PPPoETag hostUniq; 303 pid_t pid = getpid(); 304 hostUniq.type = htons(TAG_HOST_UNIQ); 305 hostUniq.length = htons(sizeof(pid)); 306 memcpy(hostUniq.payload, &pid, sizeof(pid)); 307 CHECK_ROOM(cursor, packet.payload, sizeof(pid) + TAG_HDR_SIZE); 308 memcpy(cursor, &hostUniq, sizeof(pid) + TAG_HDR_SIZE); 309 cursor += sizeof(pid) + TAG_HDR_SIZE; 310 plen += sizeof(pid) + TAG_HDR_SIZE; 311 } 312 313 packet.length = htons(plen); 314 sendPacket(conn, conn->discoverySocket, &packet, (int) (plen + HDR_SIZE)); 315} 316 317/***********************************************************************/ 318 319u_int16_t checksum(void *addr, int count) 320{ 321 /* Compute Internet Checksum for "count" bytes 322 * beginning at location "addr". 323 */ 324 register int32_t sum = 0; 325 u_int16_t *source = (u_int16_t *) addr; 326 327 while (count > 1) { 328 /* This is the inner loop */ 329 sum += *source++; 330 count -= 2; 331 } 332 333 /* Add left-over byte, if any */ 334 if (count > 0) { 335 /* Make sure that the left-over byte is added correctly both 336 * with little and big endian hosts */ 337 u_int16_t tmp = 0; 338 *(unsigned char *) (&tmp) = * (unsigned char *) source; 339 sum += tmp; 340 } 341 /* Fold 32-bit sum to 16 bits */ 342 while (sum >> 16) 343 sum = (sum & 0xffff) + (sum >> 16); 344 345 return ~sum; 346} 347 348int listen_socket(unsigned int ip, int port, char *inf) 349{ 350 struct ifreq interface; 351 int fd; 352 struct sockaddr_in addr; 353 int n = 1; 354 355 //DEBUG(LOG_INFO, "Opening listen socket on 0x%08x:%d %s\n", ip, port, inf); 356 if ((fd = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP)) < 0) { 357 //DEBUG(LOG_ERR, "socket call failed: %s", strerror(errno)); 358 return -1; 359 } 360 361 memset(&addr, 0, sizeof(addr)); 362 addr.sin_family = AF_INET; 363 addr.sin_port = htons(port); 364 addr.sin_addr.s_addr = ip; 365 366 if (setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, (char *) &n, sizeof(n)) == -1) { 367 close(fd); 368 return -1; 369 } 370 if (setsockopt(fd, SOL_SOCKET, SO_BROADCAST, (char *) &n, sizeof(n)) == -1) { 371 close(fd); 372 return -1; 373 } 374 375 strncpy(interface.ifr_ifrn.ifrn_name, inf, IFNAMSIZ); 376 if (setsockopt(fd, SOL_SOCKET, SO_BINDTODEVICE,(char *)&interface, sizeof(interface)) < 0) { 377 close(fd); 378 return -1; 379 } 380 381 if (bind(fd, (struct sockaddr *)&addr, sizeof(struct sockaddr)) == -1) { 382 close(fd); 383 return -1; 384 } 385 386 return fd; 387} 388 389int raw_socket(int ifindex) 390{ 391 int fd; 392 struct sockaddr_ll sock; 393 394 if ((fd = socket(PF_PACKET, SOCK_DGRAM, htons(ETH_P_IP))) < 0) { 395 return -1; 396 } 397 398 sock.sll_family = AF_PACKET; 399 sock.sll_protocol = htons(ETH_P_IP); 400 sock.sll_ifindex = ifindex; 401 if (bind(fd, (struct sockaddr *) &sock, sizeof(sock)) < 0) { 402 close(fd); 403 return -1; 404 } 405 406 return fd; 407} 408 409/* Constuct a ip/udp header for a packet, and specify the source and dest hardware address */ 410int raw_packet(struct dhcpMessage *payload, u_int32_t source_ip, int source_port, 411 u_int32_t dest_ip, int dest_port, unsigned char *dest_arp, int ifindex) 412{ 413 int fd; 414 int result; 415 struct sockaddr_ll dest; 416 struct udp_dhcp_packet packet; 417 418 if ((fd = socket(PF_PACKET, SOCK_DGRAM, htons(ETH_P_IP))) < 0) { 419 //DEBUG(LOG_ERR, "socket call failed: %s", strerror(errno)); 420 csprintf("socket call failed: %s\n", strerror(errno)); 421 return -1; 422 } 423 424 memset(&dest, 0, sizeof(dest)); 425 memset(&packet, 0, sizeof(packet)); 426 427 dest.sll_family = AF_PACKET; 428 dest.sll_protocol = htons(ETH_P_IP); 429 dest.sll_ifindex = ifindex; 430 dest.sll_halen = 6; 431 memcpy(dest.sll_addr, dest_arp, 6); 432 if (bind(fd, (struct sockaddr *)&dest, sizeof(struct sockaddr_ll)) < 0) { 433 //DEBUG(LOG_ERR, "bind call failed: %s", strerror(errno)); 434 csprintf("bind call failed: %s\n", strerror(errno)); 435 close(fd); 436 return -1; 437 } 438 439 packet.ip.protocol = IPPROTO_UDP; 440 packet.ip.saddr = source_ip; 441 packet.ip.daddr = dest_ip; 442 packet.udp.source = htons(source_port); 443 packet.udp.dest = htons(dest_port); 444 packet.udp.len = htons(sizeof(packet.udp) + sizeof(struct dhcpMessage)); /* cheat on the psuedo-header */ 445 packet.ip.tot_len = packet.udp.len; 446 memcpy(&(packet.data), payload, sizeof(struct dhcpMessage)); 447 packet.udp.check = checksum(&packet, sizeof(struct udp_dhcp_packet)); 448 449 packet.ip.tot_len = htons(sizeof(struct udp_dhcp_packet)); 450 packet.ip.ihl = sizeof(packet.ip) >> 2; 451 packet.ip.version = IPVERSION; 452 packet.ip.ttl = IPDEFTTL; 453 packet.ip.check = checksum(&(packet.ip), sizeof(packet.ip)); 454 455 result = sendto(fd, &packet, sizeof(struct udp_dhcp_packet), 0, (struct sockaddr *) &dest, sizeof(dest)); 456 if (result <= 0) { 457 //DEBUG(LOG_ERR, "write on socket failed: %s", strerror(errno)); 458 csprintf("write on socket failed: %s", strerror(errno)); 459 } 460 close(fd); 461 return result; 462} 463 464int get_packet(struct dhcpMessage *packet, int fd) 465{ 466 int bytes; 467 int i; 468 const char broken_vendors[][8] = { 469 "MSFT 98", 470 "" 471 }; 472 char unsigned *vendor; 473 474 memset(packet, 0, sizeof(struct dhcpMessage)); 475 bytes = read(fd, packet, sizeof(struct dhcpMessage)); 476 if (bytes < 0) { 477 //DEBUG(LOG_INFO, "couldn't read on listening socket, ignoring"); 478 return -1; 479 } 480 481 if (ntohl(packet->cookie) != DHCP_MAGIC) { 482 //LOG(LOG_ERR, "received bogus message, ignoring"); 483 return -2; 484 } 485 //DEBUG(LOG_INFO, "Received a packet"); 486 487 if (packet->op == BOOTREQUEST && (vendor = get_option(packet, DHCP_VENDOR))) { 488 for (i = 0; broken_vendors[i][0]; i++) { 489 if (vendor[OPT_LEN - 2] == (unsigned char) strlen(broken_vendors[i]) && 490 !strncmp(vendor, broken_vendors[i], vendor[OPT_LEN - 2])) { 491 //DEBUG(LOG_INFO, "broken client (%s), forcing broadcast", 492 // broken_vendors[i]); 493 packet->flags |= htons(BROADCAST_FLAG); 494 } 495 } 496 } 497 498 return bytes; 499} 500 501/* return -1 on errors that are fatal for the socket, -2 for those that aren't */ 502int get_raw_packet(struct dhcpMessage *payload, int fd) 503{ 504 int bytes; 505 struct udp_dhcp_packet packet; 506 u_int32_t source, dest; 507 u_int16_t check; 508 509 memset(&packet, 0, sizeof(struct udp_dhcp_packet)); 510 bytes = read(fd, &packet, sizeof(struct udp_dhcp_packet)); 511 if (bytes < 0) { 512 //DEBUG(LOG_INFO, "couldn't read on raw listening socket -- ignoring"); 513 csprintf("couldn't read on raw listening socket -- ignoring"); 514 usleep(500000); /* possible down interface, looping condition */ 515csprintf("--- get_raw_packet: couldn't read on raw listening socket! ---\n"); 516 return -1; 517 } 518 519 if (bytes < (int) (sizeof(struct iphdr) + sizeof(struct udphdr))) { 520 //DEBUG(LOG_INFO, "message too short, ignoring"); 521csprintf("--- get_raw_packet: message too short! ---\n"); 522 return -2; 523 } 524 525 if (bytes < ntohs(packet.ip.tot_len)) { 526 //DEBUG(LOG_INFO, "Truncated packet"); 527csprintf("--- get_raw_packet: Truncated packet! ---\n"); 528 //return -2; 529 return -100; 530 } 531 532 /* ignore any extra garbage bytes */ 533 bytes = ntohs(packet.ip.tot_len); 534 535 /* Make sure its the right packet for us, and that it passes sanity checks */ 536 if (packet.ip.protocol != IPPROTO_UDP || packet.ip.version != IPVERSION || 537 packet.ip.ihl != sizeof(packet.ip) >> 2 || packet.udp.dest != htons(CLIENT_PORT) || 538 bytes > (int) sizeof(struct udp_dhcp_packet) || 539 ntohs(packet.udp.len) != (short) (bytes - sizeof(packet.ip))) { 540 //DEBUG(LOG_INFO, "unrelated/bogus packet"); 541csprintf("--- get_raw_packet: unrelated/bogus packet! ---\n"); 542 return -2; 543 } 544 545 /* check IP checksum */ 546 check = packet.ip.check; 547 packet.ip.check = 0; 548 if (check != checksum(&(packet.ip), sizeof(packet.ip))) { 549 //DEBUG(LOG_INFO, "bad IP header checksum, ignoring"); 550csprintf("--- get_raw_packet: bad IP header checksum! ---\n"); 551 return -1; 552 } 553 554 /* verify the UDP checksum by replacing the header with a psuedo header */ 555 source = packet.ip.saddr; 556 dest = packet.ip.daddr; 557 check = packet.udp.check; 558 packet.udp.check = 0; 559 memset(&packet.ip, 0, sizeof(packet.ip)); 560 561 packet.ip.protocol = IPPROTO_UDP; 562 packet.ip.saddr = source; 563 packet.ip.daddr = dest; 564 packet.ip.tot_len = packet.udp.len; /* cheat on the psuedo-header */ 565 if (check && check != checksum(&packet, bytes)) { 566 //DEBUG(LOG_ERR, "packet with bad UDP checksum received, ignoring"); 567csprintf("--- get_raw_packet: packet with bad UDP checksum received! ---\n"); 568 return -2; 569 } 570 571 memcpy(payload, &(packet.data), bytes - (sizeof(packet.ip) + sizeof(packet.udp))); 572 573 if (ntohl(payload->cookie) != DHCP_MAGIC) { 574 //LOG(LOG_ERR, "received bogus message (bad magic) -- ignoring"); 575csprintf("--- get_raw_packet: received bogus message! ---\n"); 576 return -2; 577 } 578 //DEBUG(LOG_INFO, "oooooh!!! got some!"); 579csprintf("--- get_raw_packet: Got some correct message! ---\n"); 580 return bytes - (sizeof(packet.ip) + sizeof(packet.udp)); 581 582} 583 584/* get an option with bounds checking (warning, not aligned). */ 585unsigned char *get_option(struct dhcpMessage *packet, int code) 586{ 587 int i, length; 588 unsigned char *optionptr; 589 int over = 0, done = 0, curr = OPTION_FIELD; 590 591 optionptr = packet->options; 592 i = 0; 593 length = 308; 594 while (!done) { 595 if (i >= length) { 596 //LOG(LOG_WARNING, "bogus packet, option fields too long."); 597 return NULL; 598 } 599 if (optionptr[i + OPT_CODE] == code) { 600 if (i + 1 + optionptr[i + OPT_LEN] >= length) { 601 //LOG(LOG_WARNING, "bogus packet, option fields too long."); 602 return NULL; 603 } 604 return optionptr + i + 2; 605 } 606 switch (optionptr[i + OPT_CODE]) { 607 case DHCP_PADDING: 608 i++; 609 break; 610 case DHCP_OPTION_OVER: 611 if (i + 1 + optionptr[i + OPT_LEN] >= length) { 612 //LOG(LOG_WARNING, "bogus packet, option fields too long."); 613 return NULL; 614 } 615 over = optionptr[i + 3]; 616 i += optionptr[OPT_LEN] + 2; 617 break; 618 case DHCP_END: 619 if (curr == OPTION_FIELD && over & FILE_FIELD) { 620 optionptr = packet->file; 621 i = 0; 622 length = 128; 623 curr = FILE_FIELD; 624 } else if (curr == FILE_FIELD && over & SNAME_FIELD) { 625 optionptr = packet->sname; 626 i = 0; 627 length = 64; 628 curr = SNAME_FIELD; 629 } else done = 1; 630 break; 631 default: 632 i += optionptr[OPT_LEN + i] + 2; 633 } 634 } 635 return NULL; 636} 637 638/* return the position of the 'end' option (no bounds checking) */ 639int end_option(unsigned char *optionptr) 640{ 641 int i = 0; 642 643 while (optionptr[i] != DHCP_END) { 644 if (optionptr[i] == DHCP_PADDING) i++; 645 else i += optionptr[i + OPT_LEN] + 2; 646 } 647 return i; 648} 649 650 651/* add an option string to the options (an option string contains an option code, 652 * length, then data) */ 653int add_option_string(unsigned char *optionptr, unsigned char *string) 654{ 655 int end = end_option(optionptr); 656 657 /* end position + string length + option code/length + end option */ 658 if (end + string[OPT_LEN] + 2 + 1 >= 308) { 659 csprintf("Option 0x%02x did not fit into the packet!\n", string[OPT_CODE]); 660 return 0; 661 } 662 663 memcpy(optionptr + end, string, string[OPT_LEN] + 2); 664 optionptr[end + string[OPT_LEN] + 2] = DHCP_END; 665 return string[OPT_LEN] + 2; 666} 667 668/* add a one to four byte option to a packet */ 669int add_simple_option(unsigned char *optionptr, unsigned char code, u_int32_t data) 670{ 671 char length = 0; 672 int i; 673 unsigned char option[2 + 4]; 674 unsigned char *u8; 675 u_int16_t *u16; 676 u_int32_t *u32; 677 u_int32_t aligned; 678 u8 = (unsigned char *) &aligned; 679 u16 = (u_int16_t *) &aligned; 680 u32 = &aligned; 681 682 for (i = 0; options[i].code; i++) 683 if (options[i].code == code) { 684 length = option_lengths[options[i].flags & TYPE_MASK]; 685 } 686 687 if (!length) { 688 //DEBUG(LOG_ERR, "Could not add option 0x%02x", code); 689 return 0; 690 } 691 692 option[OPT_CODE] = code; 693 option[OPT_LEN] = length; 694 695 switch (length) { 696 case 1: *u8 = data; break; 697 case 2: *u16 = data; break; 698 case 4: *u32 = data; break; 699 } 700 memcpy(option + 2, &aligned, length); 701 return add_option_string(optionptr, option); 702} 703 704void init_header(struct dhcpMessage *packet, char type) 705{ 706 memset(packet, 0, sizeof(struct dhcpMessage)); 707 switch (type) { 708 case DHCPDISCOVER: 709 case DHCPREQUEST: 710 case DHCPRELEASE: 711 case DHCPINFORM: 712 packet->op = BOOTREQUEST; 713 break; 714 case DHCPOFFER: 715 case DHCPACK: 716 case DHCPNAK: 717 packet->op = BOOTREPLY; 718 } 719 packet->htype = ETH_10MB; 720 packet->hlen = ETH_10MB_LEN; 721 packet->cookie = htonl(DHCP_MAGIC); 722 packet->options[0] = DHCP_END; 723 add_simple_option(packet->options, DHCP_MESSAGE_TYPE, type); 724} 725 726/* initialize a packet with the proper defaults */ 727static void init_packet(struct dhcpMessage *packet, char type) 728{ 729 struct vendor { 730 char vendor, length; 731 char str[sizeof("udhcp "VERSION)]; 732 } vendor_id = { DHCP_VENDOR, sizeof("udhcp "VERSION) - 1, "udhcp "VERSION}; 733 734 init_header(packet, type); 735 memcpy(packet->chaddr, client_config.arp, 6); 736 add_option_string(packet->options, client_config.clientid); 737 add_option_string(packet->options, (unsigned char *) &vendor_id); 738} 739 740/* Add a paramater request list for stubborn DHCP servers. Pull the data 741 * from the struct in options.c. Don't do bounds checking here because it 742 * goes towards the head of the packet. */ 743static void add_requests(struct dhcpMessage *packet) 744{ 745 int end = end_option(packet->options); 746 int i, len = 0; 747 748 packet->options[end + OPT_CODE] = DHCP_PARAM_REQ; 749 for (i = 0; options[i].code; i++) 750 if (options[i].flags & OPTION_REQ) 751 packet->options[end + OPT_DATA + len++] = options[i].code; 752 packet->options[end + OPT_LEN] = len; 753 packet->options[end + OPT_DATA + len] = DHCP_END; 754 755} 756 757/* Broadcast a DHCP discover packet to the network, with an optionally requested IP */ 758int send_dhcp_discover(unsigned long xid) 759{ 760 struct dhcpMessage packet; 761 762 init_packet(&packet, DHCPDISCOVER); 763 packet.xid = xid; 764 765 add_requests(&packet); 766 return raw_packet(&packet, INADDR_ANY, CLIENT_PORT, INADDR_BROADCAST, 767 SERVER_PORT, MAC_BCAST_ADDR, client_config.ifindex); 768} 769 770int read_interface(char *interface, int *ifindex, u_int32_t *addr, unsigned char *arp) 771{ 772 int fd; 773 //struct ifreq ifr; 774 struct sockaddr_in *our_ip; 775 776 memset(&ifr, 0, sizeof(struct ifreq)); 777 if((fd = socket(AF_INET, SOCK_RAW, IPPROTO_RAW)) >= 0) { 778 ifr.ifr_addr.sa_family = AF_INET; 779 strcpy(ifr.ifr_name, interface); 780 781 if (addr) { 782 if (ioctl(fd, SIOCGIFADDR, &ifr) == 0) { 783 our_ip = (struct sockaddr_in *) &ifr.ifr_addr; 784 *addr = our_ip->sin_addr.s_addr; 785 } else { 786 close(fd); 787 return -1; 788 } 789 } 790 791 if (ioctl(fd, SIOCGIFINDEX, &ifr) == 0) { 792 *ifindex = ifr.ifr_ifindex; 793 } else { 794 return -1; 795 } 796 if (ioctl(fd, SIOCGIFHWADDR, &ifr) == 0) { 797 memcpy(arp, ifr.ifr_hwaddr.sa_data, 6); 798 } else { 799 csprintf("get hardware address failed!: %s", strerror(errno)); 800 close(fd); 801 return -1; 802 } 803 } else { 804 return -1; 805 } 806 807 close(fd); 808 return 0; 809} 810 811/* Create a random xid */ 812unsigned long random_xid(void) 813{ 814 static int initialized; 815 if (!initialized) { 816 unsigned long seed; 817 818 seed = time(0); 819 820 srand(seed); 821 initialized++; 822 } 823 return rand(); 824} 825 826static void change_mode(int new_mode) 827{ 828 cfd = -1; 829 listen_mode = new_mode; 830} 831 832void closeall(int fd1, int fd2){ 833#ifdef DHCP_DETECT 834 close(fd1); 835#endif // DHCP_DETECT 836 close(fd2); 837} 838 839int discover_all(){ 840 unsigned char *message; 841 unsigned long xid = 0; 842 fd_set rfds; 843 int retval; 844 845 struct timeval tv; 846 int len; 847 struct dhcpMessage packet; 848 int max_fd; 849 850 PPPoEConnection conn; // ppp 851 852 PPPoEPacket ppp_packet; 853 int ppp_len; 854 855 /* Initialize connection info */ 856 memset(&conn, 0, sizeof(conn)); 857 conn.discoverySocket = -1; 858 conn.sessionSocket = -1; 859 conn.useHostUniq = 1; 860 861 /* Pick a default interface name */ 862 SET_STRING(conn.ifName, DEFAULT_IF); 863 //strncpy(conn.ifName, DEFAULT_IF, strlen(DEFAULT_IF)); 864 865 if(read_interface(client_config.interface, &client_config.ifindex, NULL, client_config.arp) < 0){ 866 csprintf("read interface error!\n"); 867 return -1; 868 } 869 870 if (!client_config.clientid) { 871 client_config.clientid = malloc(6 + 3); 872 client_config.clientid[OPT_CODE] = DHCP_CLIENT_ID; 873 client_config.clientid[OPT_LEN] = 7; 874 client_config.clientid[OPT_DATA] = 1; 875 memcpy(client_config.clientid + 3, client_config.arp, 6); 876 } 877 878#ifdef DHCP_DETECT 879 xid = random_xid(); 880 //xid = 10056; 881#endif // DHCP_DETECT 882 883 state = INIT_SELECTING; 884#ifdef DHCP_SOCKET 885 change_mode(LISTEN_KERNEL); 886#else 887 change_mode(LISTEN_RAW); 888#endif 889 890 if (cfd < 0) { 891 // ppp 892 if((conn.discoverySocket = openInterface(conn.ifName, Eth_PPPOE_Discovery, conn.myEth)) < 0){ 893 csprintf("open interface fail [%d]\n", conn.discoverySocket); 894 return -1; 895 } 896 897#ifdef DHCP_DETECT 898 // dhcp 899 if (listen_mode == LISTEN_KERNEL) 900 cfd = listen_socket(INADDR_ANY, CLIENT_PORT, client_config.interface); 901 else 902 cfd = raw_socket(client_config.ifindex); 903 904 if (cfd < 0) { 905 close(conn.discoverySocket); 906 csprintf("socket open error\n"); 907 return -1; 908 } 909#endif // DHCP_DETECT 910 } 911 912 int count = 0, got_DHCP, got_PPP; 913 for (;;) { 914#ifdef DHCP_DETECT 915 got_DHCP = 0; 916#endif // DHCP_DETECT 917 got_PPP = 0; 918 919 FD_ZERO(&rfds); 920 921#ifdef DHCP_DETECT 922 FD_SET(cfd, &rfds); // DHCP 923#endif // DHCP_DETECT 924 FD_SET(conn.discoverySocket, &rfds); // ppp 925 926#ifdef DHCP_DETECT 927 send_dhcp_discover(xid); /* broadcast */ 928#endif // DHCP_DETECT 929 sendPADI(&conn); 930 931 tv.tv_sec = 2; 932 tv.tv_usec = 0; 933#ifdef DHCP_DETECT 934 max_fd = cfd; 935#else // DHCP_DETECT 936 max_fd = conn.discoverySocket; 937#endif // DHCP_DETECT 938 939 retval = select(max_fd + 1, &rfds, NULL, NULL, &tv); 940 941#ifdef DHCP_DETECT 942 if(FD_ISSET(cfd, &rfds)) 943 got_DHCP = 1; 944#endif // DHCP_DETECT 945 if(FD_ISSET(conn.discoverySocket, &rfds)) 946 got_PPP = 1; 947 948 if (retval == 0) { 949csprintf("--- discover_all: retval == 0. ---\n"); 950 csprintf("timeout occur when discover dhcp or pppoe\n"); 951 FD_ZERO(&rfds); 952 closeall(cfd, conn.discoverySocket); 953 return 0; 954 } 955#ifdef DHCP_DETECT 956 if (retval > 0 && listen_mode != LISTEN_NONE && got_DHCP == 1) { 957csprintf("--- discover_all: discovery DHCP! ---\n"); 958 /* a packet is ready, read it */ 959 if (listen_mode == LISTEN_KERNEL) 960 len = get_packet(&packet, cfd); 961 else 962 len = get_raw_packet(&packet, cfd); 963 964 if (len == -1 && errno != EINTR) { 965 FD_ZERO(&rfds); 966 closeall(cfd, conn.discoverySocket); 967 return -1; 968 } 969 970 if ((len < 0) || (packet.xid != xid) || ((message = get_option(&packet, DHCP_MESSAGE_TYPE)) == NULL)){ 971 ++count; 972csprintf("--- discover_all: Got the wrong %d packet when detecting DHCP! ---\n", count); 973#ifdef DHCP_SOCKET 974 FD_ZERO(&rfds); 975 closeall(cfd, conn.discoverySocket); 976 return -1; 977#else 978 if(len != -100){ 979 FD_ZERO(&rfds); 980 closeall(cfd, conn.discoverySocket); 981 return -1; 982 } 983 else{ 984 // maybe receviced the packet from MOD. 985 ;//got_DHCP = -1; 986 } 987#endif 988 } 989 /* Must be a DHCPOFFER to one of our xid's */ 990 //if (*message == DHCPOFFER) { 991 else if (*message == DHCPOFFER) { 992csprintf("--- discover_all: Got the DHCP! ---\n"); 993 FD_ZERO(&rfds); 994 closeall(cfd, conn.discoverySocket); 995 return 1; 996 } 997 else 998 ;//got_DHCP = -1; 999csprintf("--- discover_all: end to analyse the DHCP's packet! ---\n"); 1000 } 1001#endif // DHCP_DETECT 1002 1003 if(FD_ISSET(conn.discoverySocket, &rfds)) 1004 got_PPP = 1; 1005 1006 //else if(retval > 0 && listen_mode != LISTEN_NONE && got_PPP){ 1007 if(retval > 0 && listen_mode != LISTEN_NONE && got_PPP == 1){ 1008csprintf("--- discover_all: discovery PPPoE! ---\n"); 1009 receivePacket(conn.discoverySocket, &ppp_packet, &ppp_len); 1010 1011 if (ppp_packet.code == CODE_PADO) { 1012csprintf("--- discover_all: Got the PPPoE! ---\n"); 1013 FD_ZERO(&rfds); 1014 closeall(cfd, conn.discoverySocket); 1015 return 2; 1016 } 1017 1018 //got_PPP = -1; 1019csprintf("--- discover_all: end to analyse the PPPoE's packet! ---\n"); 1020 } 1021 1022 if (retval == -1 && errno == EINTR) { 1023 /* a signal was caught */ 1024csprintf("--- discover_all: a signal was caught! ---\n"); 1025 } 1026 else { 1027csprintf("--- discover_all: error on sleect! ---\n"); 1028#ifdef DEBUG 1029csprintf("--- discover_all: error on sleect! retval=%d, errno=%d. ---\n", retval, errno); 1030csprintf("--- discover_all: got_DHCP=%d, got_PPP=%d. ---\n", got_DHCP, got_PPP); 1031#endif // DEBUG 1032 csprintf("error on sleect\n"); 1033 } 1034csprintf("--- Go to next detect loop. ---\n"); 1035 } 1036 1037 FD_ZERO(&rfds); 1038 closeall(cfd, conn.discoverySocket); 1039 return -1; 1040} 1041