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.cpp
00001 /* 00002 * $Id: ICMPv4.c 29 2011-06-11 14:53:08Z benoit $ 00003 * $Author: benoit $ 00004 * $Date: 2011-06-11 16:53:08 +0200 (sam., 11 juin 2011) $ 00005 * $Rev: 29 $ 00006 * 00007 * 00008 * 00009 * 00010 * 00011 */ 00012 00013 #include "ICMPv4.h" 00014 #include "Ethernet.h" 00015 #include "Debug.h" 00016 00017 00018 #define DEBUG_CURRENT_MODULE_NAME "ICMPv4" 00019 #define DEBUG_CURRENT_MODULE_ID DEBUG_MODULE_ICMPV4 00020 00021 00022 static void Init(void); 00023 static void Handler(NetIF_t *netIF, NetPacket_t *packet); 00024 00025 00026 Protocol_Handler_t icmpv4 = 00027 { 00028 PROTOCOL_INDEX_NOT_INITIALIZED, /* Always PROTOCOL_INDEX_NOT_INITIALIZED at initialization */ 00029 Protocol_ID_ICMPv4, /* Protocol ID */ 00030 IPV4_PROTO_ICMPV4, /* Protocol number */ 00031 Init, /* Protocol initialisation function */ 00032 Handler, /* Protocol handler */ 00033 NULL, /* Protocol registration function */ 00034 NULL, /* API registration function */ 00035 }; 00036 00037 00038 static void Init(void) 00039 { 00040 DEBUG_MODULE(DEBUG_LEVEL_INFO, ("Initializing ICMPv4 layer")); 00041 } 00042 00043 00044 static void Handler(NetIF_t *netIF, NetPacket_t *packet) 00045 { 00046 ICMPv4_Header_t *icmpv4Packet; 00047 ICMPv4_Type_t type; 00048 ICMPv4_Code_t code; 00049 IPv4_Header_t *ipv4Header; 00050 Ethernet_Header_t *ethernetHeader; 00051 int32_t depth, lengthToSend; 00052 00053 //Debug_DumpBufferHex(packet, length); 00054 00055 icmpv4Packet = (ICMPv4_Header_t *)packet->data; 00056 type = icmpv4Packet->type; 00057 code = icmpv4Packet->code; 00058 00059 DEBUG_MODULE(DEBUG_LEVEL_VERBOSE1, ("icmpv4 frame of %d bytes icmpv4(%02x, %02x) frame of %d bytes", 00060 packet->length, 00061 type, 00062 code, 00063 packet->length 00064 )); 00065 depth = packet->depth; 00066 ipv4Header = (IPv4_Header_t *)packet->headerPtrTable[depth]; 00067 ethernetHeader = (Ethernet_Header_t *)packet->headerPtrTable[depth - 1]; 00068 00069 switch(type) 00070 { 00071 case ICMPV4_TYPE_ECHO_REQUEST: 00072 DEBUG_BLOCK(DEBUG_LEVEL_VERBOSE0) 00073 { 00074 ICMPv4_DumpHeader("Got ", ipv4Header); 00075 } 00076 00077 ipv4Header->dest.addr = ipv4Header->source.addr; 00078 ipv4Header->source.addr = netIF->ipv4Address.addr; 00079 ethernetHeader->destination = ethernetHeader->source; 00080 ethernetHeader->source = *((Ethernet_Addr_t *)netIF->driverParameter); 00081 icmpv4Packet->type = ICMPV4_TYPE_ECHO_REPLY; 00082 lengthToSend = packet->headerLenTable[depth - 1] + packet->headerLenTable[depth - 1] + ntohs(ipv4Header->totalLength); 00083 icmpv4Packet->crc = 0; 00084 icmpv4Packet->crc = ICMPv4_ComputeCRC(icmpv4Packet, ntohs(ipv4Header->totalLength) - packet->headerLenTable[depth]); 00085 00086 DEBUG_BLOCK(DEBUG_LEVEL_VERBOSE0) 00087 { 00088 ICMPv4_DumpHeader("Replying ", ipv4Header); 00089 } 00090 00091 netIF->driver->Write((uint8_t *)ethernetHeader, lengthToSend ); 00092 00093 break; 00094 00095 default: 00096 break; 00097 } 00098 } 00099 00100 00101 uint16_t ICMPv4_ComputeCRC(ICMPv4_Header_t *packet, int32_t length) 00102 { 00103 uint32_t crc = 0, size = length; 00104 uint16_t *data = (uint16_t *)packet, tmp; 00105 00106 while(size > 1) 00107 { 00108 tmp = ntohs(*data); 00109 crc += tmp; 00110 data++; 00111 size -= sizeof(uint16_t); 00112 } 00113 00114 if (size > 0) 00115 { 00116 tmp = (*((uint8_t *)data)) << 8; 00117 crc += tmp; 00118 } 00119 00120 crc = (crc >> 16) + (crc & 0xFFFF); 00121 if (crc & 0xFFFF0000) crc = (crc >> 16) + (crc & 0xFFFF); 00122 00123 return htons((~crc) & 0xFFFF); 00124 } 00125 00126 00127 Bool_t ICMPv4_CheckCRC(ICMPv4_Header_t *packet, int32_t length) 00128 { 00129 return True; 00130 } 00131 00132 00133 void ICMPv4_DumpHeader(const char *prefix, IPv4_Header_t *ipv4Header) 00134 { 00135 ICMPv4_Header_t *icmpv4Header = (ICMPv4_Header_t *)(ipv4Header + 1); 00136 00137 switch(icmpv4Header->type) 00138 { 00139 case ICMPV4_TYPE_ECHO_REQUEST: 00140 DEBUG_RAW(( 00141 "%sICMPv4 echo request from %d.%d.%d.%d", 00142 prefix != NULL ? prefix : "", 00143 ipv4Header->source.IP0, 00144 ipv4Header->source.IP1, 00145 ipv4Header->source.IP2, 00146 ipv4Header->source.IP3 00147 )); 00148 break; 00149 00150 case ICMPV4_TYPE_ECHO_REPLY: 00151 DEBUG_RAW(( 00152 "%sICMPv4 echo reply to %d.%d.%d.%d", 00153 prefix != NULL ? prefix : "", 00154 ipv4Header->dest.IP0, 00155 ipv4Header->dest.IP1, 00156 ipv4Header->dest.IP2, 00157 ipv4Header->dest.IP3 00158 )); 00159 break; 00160 00161 } 00162 }
Generated on Wed Jul 13 2022 06:09:33 by 1.7.2