![[BACK]](/cvsweb/icons/back.gif){kind=icon}
Return to ipv4.c CVS log ![[TXT]](/cvsweb/icons/text.gif){kind=icon}
![[DIR]](/cvsweb/icons/dir.gif){kind=icon}
Up to [Qemu by Fabrice Bellard] / qemu / roms / SLOF / clients / net-snk / app / netlib|
Return to ipv4.c CVS log | Up to [Qemu by Fabrice Bellard] / qemu / roms / SLOF / clients / net-snk / app / netlib |
1.1 ! root 1: /****************************************************************************** ! 2: * Copyright (c) 2004, 2008 IBM Corporation ! 3: * All rights reserved. ! 4: * This program and the accompanying materials ! 5: * are made available under the terms of the BSD License ! 6: * which accompanies this distribution, and is available at ! 7: * http://www.opensource.org/licenses/bsd-license.php ! 8: * ! 9: * Contributors: ! 10: * IBM Corporation - initial implementation ! 11: *****************************************************************************/ ! 12: ! 13: ! 14: /*>>>>>>>>>>>>>>>>>>>>> DEFINITIONS & DECLARATIONS <<<<<<<<<<<<<<<<<<<<<<*/ ! 15: ! 16: #include <ipv4.h> ! 17: #include <udp.h> ! 18: #include <tcp.h> ! 19: #include <ethernet.h> ! 20: #include <sys/socket.h> ! 21: #include <string.h> ! 22: ! 23: /* ARP Message types */ ! 24: #define ARP_REQUEST 1 ! 25: #define ARP_REPLY 2 ! 26: ! 27: /* ARP talbe size (+1) */ ! 28: #define ARP_ENTRIES 10 ! 29: ! 30: /* ICMP Message types */ ! 31: #define ICMP_ECHO_REPLY 0 ! 32: #define ICMP_DST_UNREACHABLE 3 ! 33: #define ICMP_SRC_QUENCH 4 ! 34: #define ICMP_REDIRECT 5 ! 35: #define ICMP_ECHO_REQUEST 8 ! 36: #define ICMP_TIME_EXCEEDED 11 ! 37: #define ICMP_PARAMETER_PROBLEM 12 ! 38: #define ICMP_TIMESTAMP_REQUEST 13 ! 39: #define ICMP_TIMESTAMP_REPLY 14 ! 40: #define ICMP_INFORMATION_REQUEST 15 ! 41: #define ICMP_INFORMATION_REPLY 16 ! 42: ! 43: /** \struct arp_entry ! 44: * A entry that describes a mapping between IPv4- and MAC-address. ! 45: */ ! 46: typedef struct arp_entry arp_entry_t; ! 47: struct arp_entry { ! 48: uint32_t ipv4_addr; ! 49: uint8_t mac_addr[6]; ! 50: uint8_t eth_frame[ETH_MTU_SIZE]; ! 51: int eth_len; ! 52: }; ! 53: ! 54: /** \struct icmphdr ! 55: * ICMP packet ! 56: */ ! 57: struct icmphdr { ! 58: unsigned char type; ! 59: unsigned char code; ! 60: unsigned short int checksum; ! 61: union { ! 62: /* for type 3 "Destination Unreachable" */ ! 63: unsigned int unused; ! 64: /* for type 0 and 8 */ ! 65: struct echo { ! 66: unsigned short int id; ! 67: unsigned short int seq; ! 68: } echo; ! 69: } options; ! 70: union { ! 71: /* payload for destination unreachable */ ! 72: struct dun { ! 73: unsigned char iphdr[20]; ! 74: unsigned char data[64]; ! 75: } dun; ! 76: /* payload for echo or echo reply */ ! 77: /* maximum size supported is 84 */ ! 78: unsigned char data[84]; ! 79: } payload; ! 80: }; ! 81: ! 82: /*>>>>>>>>>>>>>>>>>>>>>>>>>>>>> PROTOTYPES <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<*/ ! 83: ! 84: static unsigned short ! 85: checksum(unsigned short *packet, int words); ! 86: ! 87: static void ! 88: arp_send_request(uint32_t dest_ip); ! 89: ! 90: static void ! 91: arp_send_reply(uint32_t src_ip, uint8_t * src_mac); ! 92: ! 93: static void ! 94: fill_arphdr(uint8_t * packet, uint8_t opcode, ! 95: const uint8_t * src_mac, uint32_t src_ip, ! 96: const uint8_t * dest_mac, uint32_t dest_ip); ! 97: ! 98: static arp_entry_t* ! 99: lookup_mac_addr(uint32_t ipv4_addr); ! 100: ! 101: static void ! 102: fill_udp_checksum(struct iphdr *ipv4_hdr); ! 103: ! 104: static int8_t ! 105: handle_icmp(struct iphdr * iph, uint8_t * packet, int32_t packetsize); ! 106: ! 107: /*>>>>>>>>>>>>>>>>>>>>>>>>>>>>> LOCAL VARIABLES <<<<<<<<<<<<<<<<<<<<<<<<<*/ ! 108: ! 109: /* Routing parameters */ ! 110: static uint32_t own_ip = 0; ! 111: static uint32_t multicast_ip = 0; ! 112: static uint32_t router_ip = 0; ! 113: static uint32_t subnet_mask = 0; ! 114: ! 115: /* helper variables */ ! 116: static uint32_t ping_dst_ip; ! 117: static const uint8_t null_mac_addr[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; ! 118: static const uint8_t broadcast_mac[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; ! 119: static uint8_t multicast_mac[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; ! 120: ! 121: /* There are only (ARP_ENTRIES-1) effective entries because ! 122: * the entry that is pointed by arp_producer is never used. ! 123: */ ! 124: static unsigned int arp_consumer = 0; ! 125: static unsigned int arp_producer = 0; ! 126: static arp_entry_t arp_table[ARP_ENTRIES]; ! 127: ! 128: /* Function pointer send_ip. Points either to send_ipv4() or send_ipv6() */ ! 129: int (*send_ip) (void *, int); ! 130: ! 131: /*>>>>>>>>>>>>>>>>>>>>>>>>>>>> IMPLEMENTATION <<<<<<<<<<<<<<<<<<<<<<<<<<<*/ ! 132: ! 133: /** ! 134: * IPv4: Initialize the environment for the IPv4 layer. ! 135: */ ! 136: static void ! 137: ipv4_init(void) ! 138: { ! 139: int i; ! 140: ! 141: ping_dst_ip = 0; ! 142: ! 143: // clear ARP table ! 144: arp_consumer = 0; ! 145: arp_producer = 0; ! 146: for(i=0; i<ARP_ENTRIES; ++i) { ! 147: arp_table[i].ipv4_addr = 0; ! 148: memset(arp_table[i].mac_addr, 0, 6); ! 149: arp_table[i].eth_len = 0; ! 150: } ! 151: ! 152: /* Set IP send function to send_ipv4() */ ! 153: send_ip = &send_ipv4; ! 154: } ! 155: ! 156: /** ! 157: * IPv4: Set the own IPv4 address. ! 158: * ! 159: * @param _own_ip client IPv4 address (e.g. 127.0.0.1) ! 160: */ ! 161: void ! 162: set_ipv4_address(uint32_t _own_ip) ! 163: { ! 164: own_ip = _own_ip; ! 165: ipv4_init(); ! 166: } ! 167: ! 168: /** ! 169: * IPv4: Get the own IPv4 address. ! 170: * ! 171: * @return client IPv4 address (e.g. 127.0.0.1) ! 172: */ ! 173: uint32_t ! 174: get_ipv4_address(void) ! 175: { ! 176: return own_ip; ! 177: } ! 178: ! 179: /** ! 180: * IPv4: Set the IPv4 multicast address. ! 181: * ! 182: * @param _own_ip multicast IPv4 address (224.0.0.0 - 239.255.255.255) ! 183: */ ! 184: void ! 185: set_ipv4_multicast(uint32_t _multicast_ip) ! 186: { ! 187: // is this IP Multicast out of range (224.0.0.0 - 239.255.255.255) ! 188: if((htonl(_multicast_ip) < 0xE0000000) ! 189: || (htonl(_multicast_ip) > 0xEFFFFFFF)) { ! 190: multicast_ip = 0; ! 191: memset(multicast_mac, 0xFF, 6); ! 192: return; ! 193: } ! 194: ! 195: multicast_ip = _multicast_ip; ! 196: multicast_mac[0] = 0x01; ! 197: multicast_mac[1] = 0x00; ! 198: multicast_mac[2] = 0x5E; ! 199: multicast_mac[3] = (uint8_t) 0x7F & (multicast_ip >> 16); ! 200: multicast_mac[4] = (uint8_t) 0xFF & (multicast_ip >> 8); ! 201: multicast_mac[5] = (uint8_t) 0xFF & (multicast_ip >> 0); ! 202: } ! 203: ! 204: /** ! 205: * IPv4: Get the IPv4 multicast address. ! 206: * ! 207: * @return multicast IPv4 address (224.0.0.0 - 239.255.255.255 or 0 if not set) ! 208: */ ! 209: uint32_t ! 210: get_ipv4_multicast(void) ! 211: { ! 212: return multicast_ip; ! 213: } ! 214: ! 215: /** ! 216: * IPv4: Set the routers IPv4 address. ! 217: * ! 218: * @param _router_ip router IPv4 address ! 219: */ ! 220: void ! 221: set_ipv4_router(uint32_t _router_ip) ! 222: { ! 223: router_ip = _router_ip; ! 224: ipv4_init(); ! 225: } ! 226: ! 227: /** ! 228: * IPv4: Get the routers IPv4 address. ! 229: * ! 230: * @return router IPv4 address ! 231: */ ! 232: uint32_t ! 233: get_ipv4_router(void) ! 234: { ! 235: return router_ip; ! 236: } ! 237: ! 238: /** ! 239: * IPv4: Set the subnet mask. ! 240: * ! 241: * @param _subnet_mask netmask of the own IPv4 address ! 242: */ ! 243: void ! 244: set_ipv4_netmask(uint32_t _subnet_mask) ! 245: { ! 246: subnet_mask = _subnet_mask; ! 247: ipv4_init(); ! 248: } ! 249: ! 250: /** ! 251: * IPv4: Get the subnet mask. ! 252: * ! 253: * @return netmask of the own IPv4 address ! 254: */ ! 255: uint32_t ! 256: get_ipv4_netmask(void) ! 257: { ! 258: return subnet_mask; ! 259: } ! 260: ! 261: /** ! 262: * IPv4: Creates IP-packet. Places IP-header in a packet and fills it ! 263: * with corresponding information. ! 264: * <p> ! 265: * Use this function with similar functions for other network layers ! 266: * (fill_ethhdr, fill_udphdr, fill_dnshdr, fill_btphdr). ! 267: * ! 268: * @param packet Points to the place where IP-header must be placed. ! 269: * @param packetsize Size of the packet in bytes incl. this hdr and data. ! 270: * @param ip_proto Type of the next level protocol (e.g. UDP). ! 271: * @param ip_src Sender IP address ! 272: * @param ip_dst Receiver IP address ! 273: * @see iphdr ! 274: * @see fill_ethhdr ! 275: * @see fill_udphdr ! 276: * @see fill_dnshdr ! 277: * @see fill_btphdr ! 278: */ ! 279: void ! 280: fill_iphdr(uint8_t * packet, uint16_t packetsize, ! 281: uint8_t ip_proto, uint32_t ip_src, uint32_t ip_dst) { ! 282: struct iphdr * iph = (struct iphdr *) packet; ! 283: ! 284: iph -> ip_hlv = 0x45; ! 285: iph -> ip_tos = 0x10; ! 286: iph -> ip_len = htons(packetsize); ! 287: iph -> ip_id = htons(0); ! 288: iph -> ip_off = 0; ! 289: iph -> ip_ttl = 0xFF; ! 290: iph -> ip_p = ip_proto; ! 291: iph -> ip_src = htonl(ip_src); ! 292: iph -> ip_dst = htonl(ip_dst); ! 293: iph -> ip_sum = 0; ! 294: } ! 295: ! 296: /** ! 297: * IPv4: Handles IPv4-packets according to Receive-handle diagram. ! 298: * ! 299: * @param ip_packet IP-packet to be handled ! 300: * @param packetsize Length of the packet ! 301: * @return ZERO - packet handled successfully; ! 302: * NON ZERO - packet was not handled (e.g. bad format) ! 303: * @see receive_ether ! 304: * @see iphdr ! 305: */ ! 306: int8_t ! 307: handle_ipv4(uint8_t * ip_packet, int32_t packetsize) ! 308: { ! 309: struct iphdr * iph; ! 310: int32_t old_sum; ! 311: static uint8_t ip_heap[65536 + ETH_MTU_SIZE]; ! 312: ! 313: if (packetsize < sizeof(struct iphdr)) ! 314: return -1; // packet is too small ! 315: ! 316: iph = (struct iphdr * ) ip_packet; ! 317: ! 318: /* Drop it if destination IPv4 address is no IPv4 Broadcast, no ! 319: * registered IPv4 Multicast and not our Unicast address ! 320: */ ! 321: if((multicast_ip == 0 && iph->ip_dst >= 0xE0000000 && iph->ip_dst <= 0xEFFFFFFF) ! 322: || (multicast_ip != iph->ip_dst && iph->ip_dst != 0xFFFFFFFF && ! 323: own_ip != 0 && iph->ip_dst != own_ip)) { ! 324: return -1; ! 325: } ! 326: ! 327: old_sum = iph -> ip_sum; ! 328: iph -> ip_sum = 0; ! 329: if (old_sum != checksum((uint16_t *) iph, sizeof (struct iphdr) >> 1)) ! 330: return -1; // Wrong IP checksum ! 331: ! 332: // is it the first fragment in a packet? ! 333: if (((iph -> ip_off) & 0x1FFF) == 0) { ! 334: // is it part of more fragments? ! 335: if (((iph -> ip_off) & 0x2000) == 0x2000) { ! 336: memcpy(ip_heap, ip_packet, iph->ip_len); ! 337: return 0; ! 338: } ! 339: } ! 340: // it's not the first fragment ! 341: else { ! 342: // get the first fragment ! 343: struct iphdr * iph_first = (struct iphdr * ) ip_heap; ! 344: ! 345: // is this fragment not part of the first one, then exit ! 346: if ((iph_first->ip_id != iph->ip_id ) || ! 347: (iph_first->ip_p != iph->ip_p ) || ! 348: (iph_first->ip_src != iph->ip_src) || ! 349: (iph_first->ip_dst != iph->ip_dst)) { ! 350: return 0; ! 351: } ! 352: ! 353: // this fragment is part of the first one! ! 354: memcpy(ip_heap + sizeof(struct iphdr) + ! 355: ((iph -> ip_off) & 0x1FFF) * 8, ! 356: ip_packet + sizeof(struct iphdr), ! 357: iph -> ip_len - sizeof(struct iphdr)); ! 358: ! 359: // is it part of more fragments? Then return. ! 360: if (((iph -> ip_off) & 0x2000) == 0x2000) { ! 361: return 0; ! 362: } ! 363: ! 364: // packet is completly reassambled now! ! 365: ! 366: // recalculate ip_len and set iph and ip_packet to the ! 367: iph_first->ip_len = iph->ip_len + ((iph->ip_off) & 0x1FFF) * 8; ! 368: ! 369: // set iph and ip_packet to the resulting packet. ! 370: ip_packet = ip_heap; ! 371: iph = (struct iphdr * ) ip_packet; ! 372: } ! 373: ! 374: switch (iph -> ip_p) { ! 375: case IPTYPE_ICMP: ! 376: return handle_icmp(iph, ip_packet + sizeof(struct iphdr), ! 377: iph -> ip_len - sizeof(struct iphdr)); ! 378: case IPTYPE_UDP: ! 379: return handle_udp(ip_packet + sizeof(struct iphdr), ! 380: iph -> ip_len - sizeof(struct iphdr)); ! 381: case IPTYPE_TCP: ! 382: return handle_tcp(ip_packet + sizeof(struct iphdr), ! 383: iph -> ip_len - sizeof(struct iphdr)); ! 384: default: ! 385: break; ! 386: } ! 387: return -1; // Unknown protocol ! 388: } ! 389: ! 390: /** ! 391: * IPv4: Send IPv4-packets. ! 392: * ! 393: * Before the packet is sent there are some patcches performed: ! 394: * - IPv4 source address is replaced by our unicast IPV4 address ! 395: * if it is set to 0 or 1 ! 396: * - IPv4 destination address is replaced by our multicast IPV4 address ! 397: * if it is set to 1 ! 398: * - IPv4 checksum is calculaded. ! 399: * - If payload type is UDP, then the UDP checksum is calculated also. ! 400: * ! 401: * We sent an ARP request first, if this is the first packet sent to ! 402: * the declared IPv4 destination address. In this case we store the ! 403: * the packet and sent it later if we receive the ARP response. ! 404: * If the MAC address is known already, then we send the packet immediatly. ! 405: * If there is already an ARP request pending, then we drop this packet ! 406: * and send again an ARP request. ! 407: * ! 408: * @param ip_packet IP-packet to be handled ! 409: * @param packetsize Length of the packet ! 410: * @return -2 - packet dropped (MAC address not resolved - ARP request pending) ! 411: * -1 - packet dropped (bad format) ! 412: * 0 - packet stored (ARP request sent - packet will be sent if ! 413: * ARP response is received) ! 414: * >0 - packet send (number of transmitted bytes is returned) ! 415: * ! 416: * @see receive_ether ! 417: * @see iphdr ! 418: */ ! 419: int ! 420: send_ipv4(void* buffer, int len) ! 421: { ! 422: arp_entry_t *arp_entry; ! 423: struct iphdr *ip; ! 424: const uint8_t *mac_addr = 0; ! 425: ! 426: if(len + sizeof(struct ethhdr) > ETH_MTU_SIZE) ! 427: return -1; ! 428: ! 429: ip = (struct iphdr *) buffer; ! 430: ! 431: /* Replace source IPv4 address with our own unicast IPv4 address ! 432: * if it's 0 (= own unicast source address not specified). ! 433: */ ! 434: if(ip->ip_src == 0) { ! 435: ip->ip_src = htonl( own_ip ); ! 436: } ! 437: /* Replace source IPv4 address with our unicast IPv4 address and ! 438: * replace destination IPv4 address with our multicast IPv4 address ! 439: * if source address is set to 1. ! 440: */ ! 441: else if(ip->ip_src == 1) { ! 442: ip->ip_src = htonl( own_ip ); ! 443: ip->ip_dst = htonl( multicast_ip ); ! 444: } ! 445: ! 446: // Calculate the IPv4 checksum ! 447: ip->ip_sum = 0; ! 448: ip->ip_sum = checksum((uint16_t *) ip, sizeof (struct iphdr) >> 1); ! 449: ! 450: // if payload type is UDP, then we need to calculate the ! 451: // UDP checksum that depends on the IP header ! 452: if(ip->ip_p == IPTYPE_UDP) { ! 453: fill_udp_checksum(ip); ! 454: } ! 455: ! 456: // Check if the MAC address is already cached ! 457: if(~ip->ip_dst == 0 ! 458: || ( ((~subnet_mask) & ip->ip_dst) == ~subnet_mask && ! 459: ( subnet_mask & ip->ip_dst) == (subnet_mask & own_ip))) { ! 460: arp_entry = &arp_table[arp_producer]; ! 461: mac_addr = broadcast_mac; ! 462: } ! 463: else if(ip->ip_dst == multicast_ip) { ! 464: arp_entry = &arp_table[arp_producer]; ! 465: mac_addr = multicast_mac; ! 466: } ! 467: else { ! 468: // Check if IP address is in the same subnet as we are ! 469: if((subnet_mask & own_ip) == (subnet_mask & ip->ip_dst)) ! 470: arp_entry = lookup_mac_addr(ip->ip_dst); ! 471: // if not then we need to know the router's IP address ! 472: else ! 473: arp_entry = lookup_mac_addr(router_ip); ! 474: if(arp_entry && memcmp(arp_entry->mac_addr, null_mac_addr, 6) != 0) ! 475: mac_addr = arp_entry->mac_addr; ! 476: } ! 477: ! 478: // If we could not resolv the MAC address by our own... ! 479: if(!mac_addr) { ! 480: // send the ARP request ! 481: arp_send_request(ip->ip_dst); ! 482: ! 483: // drop the current packet if there is already a ARP request pending ! 484: if(arp_entry) ! 485: return -2; ! 486: ! 487: // take the next entry in the ARP table to prepare a the new ARP entry. ! 488: arp_entry = &arp_table[arp_producer]; ! 489: arp_producer = (arp_producer+1)%ARP_ENTRIES; ! 490: ! 491: // if ARP table is full then we must drop the oldes entry. ! 492: if(arp_consumer == arp_producer) ! 493: arp_consumer = (arp_consumer+1)%ARP_ENTRIES; ! 494: ! 495: // store the packet to be send if the ARP reply is received ! 496: arp_entry->ipv4_addr = ip->ip_dst; ! 497: memset(arp_entry->mac_addr, 0, 6); ! 498: fill_ethhdr (arp_entry->eth_frame, htons(ETHERTYPE_IP), ! 499: get_mac_address(), null_mac_addr); ! 500: memcpy(&arp_entry->eth_frame[sizeof(struct ethhdr)], ! 501: buffer, len); ! 502: arp_entry->eth_len = len + sizeof(struct ethhdr); ! 503: ! 504: return 0; ! 505: } ! 506: ! 507: // Send the packet with the known MAC address ! 508: fill_ethhdr(arp_entry->eth_frame, htons(ETHERTYPE_IP), ! 509: get_mac_address(), mac_addr); ! 510: memcpy(&arp_entry->eth_frame[sizeof(struct ethhdr)], buffer, len); ! 511: return send_ether(arp_entry->eth_frame, len + sizeof(struct ethhdr)); ! 512: } ! 513: ! 514: /** ! 515: * IPv4: Calculate UDP checksum. Places the result into the UDP-header. ! 516: * <p> ! 517: * Use this function after filling the UDP payload. ! 518: * ! 519: * @param ipv4_hdr Points to the place where IPv4-header starts. ! 520: */ ! 521: ! 522: static void ! 523: fill_udp_checksum(struct iphdr *ipv4_hdr) ! 524: { ! 525: int i; ! 526: unsigned long checksum = 0; ! 527: struct iphdr ip_hdr; ! 528: char *ptr; ! 529: udp_hdr_t *udp_hdr; ! 530: ! 531: udp_hdr = (udp_hdr_t *) (ipv4_hdr + 1); ! 532: udp_hdr->uh_sum = 0; ! 533: ! 534: memset(&ip_hdr, 0, sizeof(struct iphdr)); ! 535: ip_hdr.ip_src = ipv4_hdr->ip_src; ! 536: ip_hdr.ip_dst = ipv4_hdr->ip_dst; ! 537: ip_hdr.ip_len = udp_hdr->uh_ulen; ! 538: ip_hdr.ip_p = ipv4_hdr->ip_p; ! 539: ! 540: ptr = (char*) udp_hdr; ! 541: for (i = 0; i < udp_hdr->uh_ulen; i+=2) ! 542: checksum += *((uint16_t*) &ptr[i]); ! 543: ! 544: ptr = (char*) &ip_hdr; ! 545: for (i = 0; i < sizeof(struct iphdr); i+=2) ! 546: checksum += *((uint16_t*) &ptr[i]); ! 547: ! 548: checksum = (checksum >> 16) + (checksum & 0xffff); ! 549: checksum += (checksum >> 16); ! 550: udp_hdr->uh_sum = ~checksum; ! 551: } ! 552: ! 553: /** ! 554: * IPv4: Calculates checksum for IP header. ! 555: * ! 556: * @param packet Points to the IP-header ! 557: * @param words Size of the packet in words incl. IP-header and data. ! 558: * @return Checksum ! 559: * @see iphdr ! 560: */ ! 561: static unsigned short ! 562: checksum(unsigned short * packet, int words) ! 563: { ! 564: unsigned long checksum; ! 565: ! 566: for (checksum = 0; words > 0; words--) ! 567: checksum += *packet++; ! 568: checksum = (checksum >> 16) + (checksum & 0xffff); ! 569: checksum += (checksum >> 16); ! 570: ! 571: return ~checksum; ! 572: } ! 573: ! 574: static arp_entry_t* ! 575: lookup_mac_addr(uint32_t ipv4_addr) ! 576: { ! 577: unsigned int i; ! 578: ! 579: for(i=arp_consumer; i != arp_producer; i = ((i+1)%ARP_ENTRIES) ) { ! 580: if(arp_table[i].ipv4_addr == ipv4_addr) ! 581: return &arp_table[i]; ! 582: } ! 583: return 0; ! 584: } ! 585: ! 586: ! 587: /** ! 588: * ARP: Sends an ARP-request package. ! 589: * For given IPv4 retrieves MAC via ARP (makes several attempts) ! 590: * ! 591: * @param dest_ip IP of the host which MAC should be obtained ! 592: */ ! 593: static void ! 594: arp_send_request(uint32_t dest_ip) ! 595: { ! 596: arp_entry_t *arp_entry = &arp_table[arp_producer]; ! 597: ! 598: memset(arp_entry->eth_frame, 0, sizeof(struct ethhdr) + sizeof(struct arphdr)); ! 599: fill_arphdr(&arp_entry->eth_frame[sizeof(struct ethhdr)], ARP_REQUEST, ! 600: get_mac_address(), own_ip, broadcast_mac, dest_ip); ! 601: fill_ethhdr(arp_entry->eth_frame, ETHERTYPE_ARP, ! 602: get_mac_address(), broadcast_mac); ! 603: ! 604: send_ether(arp_entry->eth_frame, ! 605: sizeof(struct ethhdr) + sizeof(struct arphdr)); ! 606: } ! 607: ! 608: /** ! 609: * ARP: Sends an ARP-reply package. ! 610: * This package is used to serve foreign requests (in case IP in ! 611: * foreign request matches our host IP). ! 612: * ! 613: * @param src_ip requester IP address (foreign IP) ! 614: * @param src_mac requester MAC address (foreign MAC) ! 615: */ ! 616: static void ! 617: arp_send_reply(uint32_t src_ip, uint8_t * src_mac) ! 618: { ! 619: arp_entry_t *arp_entry = &arp_table[arp_producer]; ! 620: ! 621: memset(arp_entry->eth_frame, 0, sizeof(struct ethhdr) + sizeof(struct arphdr)); ! 622: fill_ethhdr(arp_entry->eth_frame, ETHERTYPE_ARP, ! 623: get_mac_address(), src_mac); ! 624: fill_arphdr(&arp_entry->eth_frame[sizeof(struct ethhdr)], ARP_REPLY, ! 625: get_mac_address(), own_ip, src_mac, src_ip); ! 626: ! 627: send_ether(arp_entry->eth_frame, ! 628: sizeof(struct ethhdr) + sizeof(struct arphdr)); ! 629: } ! 630: ! 631: /** ! 632: * ARP: Creates ARP package. Places ARP-header in a packet and fills it ! 633: * with corresponding information. ! 634: * <p> ! 635: * Use this function with similar functions for other network layers ! 636: * (fill_ethhdr). ! 637: * ! 638: * @param packet Points to the place where ARP-header must be placed. ! 639: * @param opcode Identifies is it request (ARP_REQUEST) ! 640: * or reply (ARP_REPLY) package. ! 641: * @param src_mac sender MAC address ! 642: * @param src_ip sender IP address ! 643: * @param dest_mac receiver MAC address ! 644: * @param dest_ip receiver IP address ! 645: * @see arphdr ! 646: * @see fill_ethhdr ! 647: */ ! 648: static void ! 649: fill_arphdr(uint8_t * packet, uint8_t opcode, ! 650: const uint8_t * src_mac, uint32_t src_ip, ! 651: const uint8_t * dest_mac, uint32_t dest_ip) ! 652: { ! 653: struct arphdr * arph = (struct arphdr *) packet; ! 654: ! 655: arph -> hw_type = htons(1); ! 656: arph -> proto_type = htons(ETHERTYPE_IP); ! 657: arph -> hw_len = 6; ! 658: arph -> proto_len = 4; ! 659: arph -> opcode = htons(opcode); ! 660: ! 661: memcpy(arph->src_mac, src_mac, 6); ! 662: arph->src_ip = htonl(src_ip); ! 663: memcpy(arph->dest_mac, dest_mac, 6); ! 664: arph->dest_ip = htonl(dest_ip); ! 665: } ! 666: ! 667: /** ! 668: * ARP: Handles ARP-messages according to Receive-handle diagram. ! 669: * Updates arp_table for outstanding ARP requests (see arp_getmac). ! 670: * ! 671: * @param packet ARP-packet to be handled ! 672: * @param packetsize length of the packet ! 673: * @return ZERO - packet handled successfully; ! 674: * NON ZERO - packet was not handled (e.g. bad format) ! 675: * @see arp_getmac ! 676: * @see receive_ether ! 677: * @see arphdr ! 678: */ ! 679: int8_t ! 680: handle_arp(uint8_t * packet, int32_t packetsize) ! 681: { ! 682: struct arphdr * arph = (struct arphdr *) packet; ! 683: ! 684: if (packetsize < sizeof(struct arphdr)) ! 685: return -1; // Packet is too small ! 686: ! 687: if (arph -> hw_type != htons(1) || arph -> proto_type != htons(ETHERTYPE_IP)) ! 688: return -1; // Unknown hardware or unsupported protocol ! 689: ! 690: if (arph -> dest_ip != htonl(own_ip)) ! 691: return -1; // receiver IP doesn't match our IP ! 692: ! 693: switch(htons(arph -> opcode)) { ! 694: case ARP_REQUEST: ! 695: // foreign request ! 696: if(own_ip != 0) ! 697: arp_send_reply(htonl(arph->src_ip), arph -> src_mac); ! 698: return 0; // no error ! 699: case ARP_REPLY: { ! 700: unsigned int i; ! 701: // if it is not for us -> return immediately ! 702: if(memcmp(get_mac_address(), arph->dest_mac, 6)) { ! 703: return 0; // no error ! 704: } ! 705: ! 706: if(arph->src_ip == 0) { ! 707: // we are not interested for a MAC address if ! 708: // the IPv4 address is 0.0.0.0 or ff.ff.ff.ff ! 709: return -1; ! 710: } ! 711: ! 712: // now let's find the corresponding entry in the ARP table ! 713: ! 714: for(i=arp_consumer; i != arp_producer; i = ((i+1)%ARP_ENTRIES) ) { ! 715: if(arp_table[i].ipv4_addr == arph->src_ip) ! 716: break; ! 717: } ! 718: if(i == arp_producer || memcmp(arp_table[i].mac_addr, null_mac_addr, 6) != 0) { ! 719: // we have not asked to resolve this IPv4 address ! ! 720: return -1; ! 721: } ! 722: ! 723: memcpy(arp_table[i].mac_addr, arph->src_mac, 6); ! 724: ! 725: // do we have something to send ! 726: if(arp_table[i].eth_len > 0) { ! 727: struct ethhdr * ethh = (struct ethhdr *) arp_table[i].eth_frame; ! 728: memcpy(ethh -> dest_mac, arp_table[i].mac_addr, 6); ! 729: ! 730: send_ether(arp_table[i].eth_frame, arp_table[i].eth_len); ! 731: arp_table[i].eth_len = 0; ! 732: } ! 733: return 0; // no error ! 734: } ! 735: default: ! 736: break; ! 737: } ! 738: return -1; // Invalid message type ! 739: } ! 740: ! 741: /** ! 742: * ICMP: Send an ICMP Echo request to destination IPv4 address. ! 743: * This function does also set a global variable to the ! 744: * destination IPv4 address. If there is an ICMP Echo Reply ! 745: * received later then the variable is set back to 0. ! 746: * In other words, reading a value of 0 form this variable ! 747: * means that an answer to the request has been arrived. ! 748: * ! 749: * @param _ping_dst_ip destination IPv4 address ! 750: */ ! 751: void ! 752: ping_ipv4(uint32_t _ping_dst_ip) ! 753: { ! 754: unsigned char packet[sizeof(struct iphdr) + sizeof(struct icmphdr)]; ! 755: struct icmphdr *icmp; ! 756: ! 757: ping_dst_ip = _ping_dst_ip; ! 758: ! 759: if(ping_dst_ip == 0) ! 760: return; ! 761: ! 762: fill_iphdr(packet, sizeof(struct iphdr) + sizeof(struct icmphdr), IPTYPE_ICMP, ! 763: 0, ping_dst_ip); ! 764: icmp = (struct icmphdr *) (packet + sizeof(struct iphdr)); ! 765: icmp->type = ICMP_ECHO_REQUEST; ! 766: icmp->code = 0; ! 767: icmp->checksum = 0; ! 768: icmp->options.echo.id = 0xd476; ! 769: icmp->options.echo.seq = 1; ! 770: ! 771: memset(icmp->payload.data, '*', sizeof(icmp->payload.data)); ! 772: ! 773: icmp->checksum = ! 774: checksum((unsigned short *) icmp, sizeof(struct icmphdr) >> 1); ! 775: send_ipv4(packet, sizeof(struct iphdr) + sizeof(struct icmphdr)); ! 776: } ! 777: ! 778: /** ! 779: * ICMP: Return host IPv4 address that we are waiting for a ! 780: * ICMP Echo reply message. If this value is 0 then we have ! 781: * received an reply. ! 782: * ! 783: * @return ping_dst_ip host IPv4 address ! 784: */ ! 785: uint32_t ! 786: pong_ipv4(void) ! 787: { ! 788: return ping_dst_ip; ! 789: } ! 790: ! 791: /** ! 792: * ICMP: Handles ICMP-packets according to Receive-handle diagram. ! 793: * ! 794: * @param icmp_packet ICMP-packet to be handled ! 795: * @param packetsize Length of the packet ! 796: * @return ZERO - packet handled successfully; ! 797: * NON ZERO - packet was not handled (e.g. bad format) ! 798: * @see handle_ipv4 ! 799: */ ! 800: static int8_t ! 801: handle_icmp(struct iphdr * iph, uint8_t * packet, int32_t packetsize) ! 802: { ! 803: struct icmphdr *icmp = (struct icmphdr *) packet; ! 804: ! 805: switch(icmp->type) { ! 806: case ICMP_ECHO_REPLY: ! 807: if (icmp->options.echo.id != 0xd476) ! 808: return -1; ! 809: if (icmp->options.echo.seq != 1) ! 810: return -1; ! 811: if(ping_dst_ip != iph->ip_src ! 812: || ping_dst_ip == 0) ! 813: return -1; ! 814: ping_dst_ip = 0; ! 815: break; ! 816: case ICMP_DST_UNREACHABLE: { ! 817: // We've got Destination Unreachable msg ! 818: // Inform corresponding upper network layers ! 819: struct iphdr * bad_iph = (struct iphdr * ) &icmp->payload; ! 820: ! 821: switch(bad_iph->ip_p) { ! 822: case IPTYPE_TCP: ! 823: handle_tcp_dun((uint8_t *) (bad_iph + 1), packetsize ! 824: - sizeof(struct icmphdr) ! 825: - sizeof(struct iphdr), icmp->code); ! 826: break; ! 827: case IPTYPE_UDP: ! 828: handle_udp_dun((uint8_t *) (bad_iph + 1), packetsize ! 829: - sizeof(struct icmphdr) ! 830: - sizeof(struct iphdr), icmp->code); ! 831: break; ! 832: } ! 833: break; ! 834: } ! 835: case ICMP_SRC_QUENCH: ! 836: break; ! 837: case ICMP_REDIRECT: ! 838: break; ! 839: case ICMP_ECHO_REQUEST: { ! 840: // We've got an Echo Request - answer with Echo Replay msg ! 841: unsigned char reply_packet[sizeof(struct iphdr) + packetsize]; ! 842: struct icmphdr *reply_icmph; ! 843: ! 844: fill_iphdr(reply_packet, sizeof(struct iphdr) + packetsize, ! 845: IPTYPE_ICMP, 0, iph->ip_src); ! 846: ! 847: reply_icmph = (struct icmphdr *) &reply_packet[sizeof(struct iphdr)]; ! 848: memcpy(reply_icmph, packet, packetsize); ! 849: reply_icmph -> type = ICMP_ECHO_REPLY; ! 850: reply_icmph -> checksum = 0; ! 851: reply_icmph->checksum = checksum((unsigned short *) reply_icmph, ! 852: sizeof(struct icmphdr) >> 1); ! 853: ! 854: send_ipv4(reply_packet, sizeof(struct iphdr) + packetsize); ! 855: break; ! 856: } ! 857: case ICMP_TIME_EXCEEDED: ! 858: break; ! 859: case ICMP_PARAMETER_PROBLEM: ! 860: break; ! 861: case ICMP_TIMESTAMP_REQUEST: ! 862: break; ! 863: case ICMP_TIMESTAMP_REPLY: ! 864: break; ! 865: case ICMP_INFORMATION_REQUEST: ! 866: break; ! 867: case ICMP_INFORMATION_REPLY: ! 868: break; ! 869: } ! 870: return 0; ! 871: }
This archive runs on limited infrastructure. Preserving old code on modern bandwidth. Automated agents are requested to crawl responsibly.