Benoît Locher / mbedNet

Rewrite from scratch a TCP/IP stack for mbed. So far the following parts are usable: Drivers: - EMAC driver (from CMSIS 2.0) Protocols: - Ethernet protocol - ARP over ethernet for IPv4 - IPv4 over Ethernet - ICMPv4 over IPv4 - UDPv4 over IPv4 APIs: - Sockets for UDPv4 The structure of this stack is designed to be very modular. Each protocol can register one or more protocol to handle its payload, and in each protocol, an API can be hooked (like Sockets for example). This is an early release.

ICMPv4.c@0:19f5f51584de, 2011-06-12 (annotated)

Committer:
Benoit
Date:
Sun Jun 12 11:23:03 2011 +0000
Revision:
0:19f5f51584de
Initial release (alpha quality)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Benoit 0:19f5f51584de 1 /*
Benoit 0:19f5f51584de 2 * $Id: ICMPv4.c 29 2011-06-11 14:53:08Z benoit $
Benoit 0:19f5f51584de 3 * $Author: benoit $
Benoit 0:19f5f51584de 4 * $Date: 2011-06-11 16:53:08 +0200 (sam., 11 juin 2011) $
Benoit 0:19f5f51584de 5 * $Rev: 29 $
Benoit 0:19f5f51584de 6 *
Benoit 0:19f5f51584de 7 *
Benoit 0:19f5f51584de 8 *
Benoit 0:19f5f51584de 9 *
Benoit 0:19f5f51584de 10 *
Benoit 0:19f5f51584de 11 */
Benoit 0:19f5f51584de 12
Benoit 0:19f5f51584de 13 #include "ICMPv4.h"
Benoit 0:19f5f51584de 14 #include "Ethernet.h"
Benoit 0:19f5f51584de 15 #include "Debug.h"
Benoit 0:19f5f51584de 16
Benoit 0:19f5f51584de 17
Benoit 0:19f5f51584de 18 #define DEBUG_CURRENT_MODULE_NAME "ICMPv4"
Benoit 0:19f5f51584de 19 #define DEBUG_CURRENT_MODULE_ID DEBUG_MODULE_ICMPV4
Benoit 0:19f5f51584de 20
Benoit 0:19f5f51584de 21
Benoit 0:19f5f51584de 22 static void Init(void);
Benoit 0:19f5f51584de 23 static void Handler(NetIF_t *netIF, Packet_t *packet);
Benoit 0:19f5f51584de 24
Benoit 0:19f5f51584de 25
Benoit 0:19f5f51584de 26 Protocol_Handler_t icmpv4 =
Benoit 0:19f5f51584de 27 {
Benoit 0:19f5f51584de 28 PROTOCOL_INDEX_NOT_INITIALIZED, /* Always PROTOCOL_INDEX_NOT_INITIALIZED at initialization */
Benoit 0:19f5f51584de 29 Protocol_ID_ICMPv4, /* Protocol ID */
Benoit 0:19f5f51584de 30 IPV4_PROTO_ICMPV4, /* Protocol number */
Benoit 0:19f5f51584de 31 Init, /* Protocol initialisation function */
Benoit 0:19f5f51584de 32 Handler, /* Protocol handler */
Benoit 0:19f5f51584de 33 NULL, /* Protocol registration function */
Benoit 0:19f5f51584de 34 NULL, /* API registration function */
Benoit 0:19f5f51584de 35 };
Benoit 0:19f5f51584de 36
Benoit 0:19f5f51584de 37
Benoit 0:19f5f51584de 38 static void Init(void)
Benoit 0:19f5f51584de 39 {
Benoit 0:19f5f51584de 40 DEBUG_MODULE(DEBUG_LEVEL_INFO, ("Initializing ICMPv4 layer"));
Benoit 0:19f5f51584de 41 }
Benoit 0:19f5f51584de 42
Benoit 0:19f5f51584de 43
Benoit 0:19f5f51584de 44 static void Handler(NetIF_t *netIF, Packet_t *packet)
Benoit 0:19f5f51584de 45 {
Benoit 0:19f5f51584de 46 ICMPv4_Header_t *icmpv4Packet;
Benoit 0:19f5f51584de 47 ICMPv4_Type_t type;
Benoit 0:19f5f51584de 48 ICMPv4_Code_t code;
Benoit 0:19f5f51584de 49 IPv4_Header_t *ipv4Header;
Benoit 0:19f5f51584de 50 Ethernet_Header_t *ethernetHeader;
Benoit 0:19f5f51584de 51 int32_t depth, lengthToSend;
Benoit 0:19f5f51584de 52
Benoit 0:19f5f51584de 53 //Debug_DumpBufferHex(packet, length);
Benoit 0:19f5f51584de 54
Benoit 0:19f5f51584de 55
Benoit 0:19f5f51584de 56 icmpv4Packet = (ICMPv4_Header_t *)packet->data;
Benoit 0:19f5f51584de 57 type = icmpv4Packet->type;
Benoit 0:19f5f51584de 58 code = icmpv4Packet->code;
Benoit 0:19f5f51584de 59 code = code;
Benoit 0:19f5f51584de 60
Benoit 0:19f5f51584de 61 DEBUG_MODULE(DEBUG_LEVEL_VERBOSE1, ("icmpv4 frame of %d bytes icmpv4(%02x, %02x) frame of %d bytes",
Benoit 0:19f5f51584de 62 packet->length,
Benoit 0:19f5f51584de 63 icmpv4Packet->type,
Benoit 0:19f5f51584de 64 icmpv4Packet->code,
Benoit 0:19f5f51584de 65 packet->length
Benoit 0:19f5f51584de 66 ));
Benoit 0:19f5f51584de 67 depth = packet->depth;
Benoit 0:19f5f51584de 68 ipv4Header = (IPv4_Header_t *)packet->headerPtrTable[depth];
Benoit 0:19f5f51584de 69 ethernetHeader = (Ethernet_Header_t *)packet->headerPtrTable[depth - 1];
Benoit 0:19f5f51584de 70
Benoit 0:19f5f51584de 71 switch(type)
Benoit 0:19f5f51584de 72 {
Benoit 0:19f5f51584de 73 case ICMPV4_TYPE_ECHO_REQUEST:
Benoit 0:19f5f51584de 74 DEBUG_BLOCK(DEBUG_LEVEL_VERBOSE0)
Benoit 0:19f5f51584de 75 {
Benoit 0:19f5f51584de 76 ICMPv4_DumpHeader("Got ", ipv4Header);
Benoit 0:19f5f51584de 77 }
Benoit 0:19f5f51584de 78
Benoit 0:19f5f51584de 79 ipv4Header->dest.addr = ipv4Header->source.addr;
Benoit 0:19f5f51584de 80 ipv4Header->source.addr = netIF->ipv4Address.addr;
Benoit 0:19f5f51584de 81 ethernetHeader->destination = ethernetHeader->source;
Benoit 0:19f5f51584de 82 ethernetHeader->source = *((Ethernet_Addr_t *)netIF->driverParameter);
Benoit 0:19f5f51584de 83 icmpv4Packet->type = ICMPV4_TYPE_ECHO_REPLY;
Benoit 0:19f5f51584de 84 lengthToSend = packet->headerLenTable[depth - 1] + packet->headerLenTable[depth - 1] + ntohs(ipv4Header->totalLength);
Benoit 0:19f5f51584de 85 icmpv4Packet->crc = 0;
Benoit 0:19f5f51584de 86 icmpv4Packet->crc = ICMPv4_ComputeCRC(icmpv4Packet, ntohs(ipv4Header->totalLength) - packet->headerLenTable[depth]);
Benoit 0:19f5f51584de 87
Benoit 0:19f5f51584de 88 DEBUG_BLOCK(DEBUG_LEVEL_VERBOSE0)
Benoit 0:19f5f51584de 89 {
Benoit 0:19f5f51584de 90 ICMPv4_DumpHeader("Replying ", ipv4Header);
Benoit 0:19f5f51584de 91 }
Benoit 0:19f5f51584de 92
Benoit 0:19f5f51584de 93 netIF->driver->Write((uint8_t *)ethernetHeader, lengthToSend );
Benoit 0:19f5f51584de 94
Benoit 0:19f5f51584de 95 break;
Benoit 0:19f5f51584de 96
Benoit 0:19f5f51584de 97 default:
Benoit 0:19f5f51584de 98 break;
Benoit 0:19f5f51584de 99 }
Benoit 0:19f5f51584de 100 }
Benoit 0:19f5f51584de 101
Benoit 0:19f5f51584de 102
Benoit 0:19f5f51584de 103 uint16_t ICMPv4_ComputeCRC(ICMPv4_Header_t *packet, int32_t length)
Benoit 0:19f5f51584de 104 {
Benoit 0:19f5f51584de 105 uint32_t crc = 0, size = length;
Benoit 0:19f5f51584de 106 uint16_t *data = (uint16_t *)packet, tmp;
Benoit 0:19f5f51584de 107
Benoit 0:19f5f51584de 108 while(size > 1)
Benoit 0:19f5f51584de 109 {
Benoit 0:19f5f51584de 110 tmp = ntohs(*data);
Benoit 0:19f5f51584de 111 crc += tmp;
Benoit 0:19f5f51584de 112 data++;
Benoit 0:19f5f51584de 113 size -= sizeof(uint16_t);
Benoit 0:19f5f51584de 114 }
Benoit 0:19f5f51584de 115
Benoit 0:19f5f51584de 116 if (size > 0)
Benoit 0:19f5f51584de 117 {
Benoit 0:19f5f51584de 118 tmp = (*((uint8_t *)data)) << 8;
Benoit 0:19f5f51584de 119 crc += tmp;
Benoit 0:19f5f51584de 120 }
Benoit 0:19f5f51584de 121
Benoit 0:19f5f51584de 122 crc = (crc >> 16) + (crc & 0xFFFF);
Benoit 0:19f5f51584de 123 if (crc & 0xFFFF0000) crc = (crc >> 16) + (crc & 0xFFFF);
Benoit 0:19f5f51584de 124
Benoit 0:19f5f51584de 125 return htons((~crc) & 0xFFFF);
Benoit 0:19f5f51584de 126 }
Benoit 0:19f5f51584de 127
Benoit 0:19f5f51584de 128
Benoit 0:19f5f51584de 129 Bool_t ICMPv4_CheckCRC(ICMPv4_Header_t *packet, int32_t length)
Benoit 0:19f5f51584de 130 {
Benoit 0:19f5f51584de 131 return True;
Benoit 0:19f5f51584de 132 }
Benoit 0:19f5f51584de 133
Benoit 0:19f5f51584de 134
Benoit 0:19f5f51584de 135 void ICMPv4_DumpHeader(const char *prefix, IPv4_Header_t *ipv4Header)
Benoit 0:19f5f51584de 136 {
Benoit 0:19f5f51584de 137 ICMPv4_Header_t *icmpv4Header = (ICMPv4_Header_t *)(ipv4Header + 1);
Benoit 0:19f5f51584de 138
Benoit 0:19f5f51584de 139 switch(icmpv4Header->type)
Benoit 0:19f5f51584de 140 {
Benoit 0:19f5f51584de 141 case ICMPV4_TYPE_ECHO_REQUEST:
Benoit 0:19f5f51584de 142 DEBUG_RAW((
Benoit 0:19f5f51584de 143 "%sICMPv4 echo request from %d.%d.%d.%d",
Benoit 0:19f5f51584de 144 prefix != NULL ? prefix : "",
Benoit 0:19f5f51584de 145 ipv4Header->source.IP0,
Benoit 0:19f5f51584de 146 ipv4Header->source.IP1,
Benoit 0:19f5f51584de 147 ipv4Header->source.IP2,
Benoit 0:19f5f51584de 148 ipv4Header->source.IP3
Benoit 0:19f5f51584de 149 ));
Benoit 0:19f5f51584de 150 break;
Benoit 0:19f5f51584de 151
Benoit 0:19f5f51584de 152 case ICMPV4_TYPE_ECHO_REPLY:
Benoit 0:19f5f51584de 153 DEBUG_RAW((
Benoit 0:19f5f51584de 154 "%sICMPv4 echo reply to %d.%d.%d.%d",
Benoit 0:19f5f51584de 155 prefix != NULL ? prefix : "",
Benoit 0:19f5f51584de 156 ipv4Header->dest.IP0,
Benoit 0:19f5f51584de 157 ipv4Header->dest.IP1,
Benoit 0:19f5f51584de 158 ipv4Header->dest.IP2,
Benoit 0:19f5f51584de 159 ipv4Header->dest.IP3
Benoit 0:19f5f51584de 160 ));
Benoit 0:19f5f51584de 161 break;
Benoit 0:19f5f51584de 162
Benoit 0:19f5f51584de 163 }
Benoit 0:19f5f51584de 164 }