void icmpProcessMessage(NetInterface *interface, Ipv4Addr srcIpAddr, const ChunkedBuffer *buffer, size_t offset) { size_t length; IcmpHeader *header; //Retrieve the length of the ICMP message length = chunkedBufferGetLength(buffer) - offset; //Ensure the message length is correct if(length < sizeof(IcmpHeader)) { //Debug message TRACE_WARNING("ICMP message length is invalid!\r\n"); //Silently discard incoming message return; } //Point to the ICMP message header header = chunkedBufferAt(buffer, offset); //Sanity check if(!header) return; //Debug message TRACE_INFO("ICMP message received (%" PRIuSIZE " bytes)...\r\n", length); //Dump message contents for debugging purpose icmpDumpMessage(header); //Verify checksum value if(ipCalcChecksumEx(buffer, offset, length) != 0x0000) { //Debug message TRACE_WARNING("Wrong ICMP header checksum!\r\n"); //Drop incoming message return; } //Check the type of ICMP message switch(header->type) { //Echo request? case ICMP_TYPE_ECHO_REQUEST: //Process Echo Request message icmpProcessEchoRequest(interface, srcIpAddr, buffer, offset); break; //Unknown type? default: //Debug message TRACE_WARNING("Unknown ICMP message type!\r\n"); //Discard incoming ICMP message break; } }
void icmpProcessEchoRequest(NetInterface *interface, Ipv4Addr srcIpAddr, const ChunkedBuffer *request, size_t requestOffset) { error_t error; size_t requestLength; size_t replyOffset; size_t replyLength; ChunkedBuffer *reply; IcmpEchoMessage *requestHeader; IcmpEchoMessage *replyHeader; Ipv4PseudoHeader pseudoHeader; //Retrieve the length of the Echo Request message requestLength = chunkedBufferGetLength(request) - requestOffset; //Ensure the packet length is correct if(requestLength < sizeof(IcmpEchoMessage)) return; //Point to the Echo Request header requestHeader = chunkedBufferAt(request, requestOffset); //Sanity check if(!requestHeader) return; //Debug message TRACE_INFO("ICMP Echo Request message received (%" PRIuSIZE " bytes)...\r\n", requestLength); //Dump message contents for debugging purpose icmpDumpEchoMessage(requestHeader); //Allocate memory to hold the Echo Reply message reply = ipAllocBuffer(sizeof(IcmpEchoMessage), &replyOffset); //Failed to allocate memory? if(!reply) return; //Point to the Echo Reply header replyHeader = chunkedBufferAt(reply, replyOffset); //Format Echo Reply header replyHeader->type = ICMP_TYPE_ECHO_REPLY; replyHeader->code = 0; replyHeader->checksum = 0; replyHeader->identifier = requestHeader->identifier; replyHeader->sequenceNumber = requestHeader->sequenceNumber; //Point to the first data byte requestOffset += sizeof(IcmpEchoMessage); requestLength -= sizeof(IcmpEchoMessage); //Copy data error = chunkedBufferConcat(reply, request, requestOffset, requestLength); //Any error to report? if(error) { //Clean up side effects chunkedBufferFree(reply); //Exit immediately return; } //Get the length of the resulting message replyLength = chunkedBufferGetLength(reply) - replyOffset; //Calculate ICMP header checksum replyHeader->checksum = ipCalcChecksumEx(reply, replyOffset, replyLength); //Format IPv4 pseudo header pseudoHeader.srcAddr = interface->ipv4Config.addr; pseudoHeader.destAddr = srcIpAddr; pseudoHeader.reserved = 0; pseudoHeader.protocol = IPV4_PROTOCOL_ICMP; pseudoHeader.length = htons(replyLength); //Debug message TRACE_INFO("Sending ICMP Echo Reply message (%" PRIuSIZE " bytes)...\r\n", replyLength); //Dump message contents for debugging purpose icmpDumpEchoMessage(replyHeader); //Send Echo Reply message ipv4SendDatagram(interface, &pseudoHeader, reply, replyOffset, IPV4_DEFAULT_TTL); //Free previously allocated memory block chunkedBufferFree(reply); }
error_t icmpSendErrorMessage(NetInterface *interface, uint8_t type, uint8_t code, uint8_t parameter, const ChunkedBuffer *ipPacket) { error_t error; size_t offset; size_t length; Ipv4Header *ipHeader; ChunkedBuffer *icmpMessage; IcmpErrorMessage *icmpHeader; Ipv4PseudoHeader pseudoHeader; //Retrieve the length of the invoking IPv4 packet length = chunkedBufferGetLength(ipPacket); //Check the length of the IPv4 packet if(length < sizeof(Ipv4Header)) return ERROR_INVALID_LENGTH; //Point to the header of the invoking packet ipHeader = chunkedBufferAt(ipPacket, 0); //Sanity check if(!ipHeader) return ERROR_FAILURE; //Never respond to a packet destined to a broadcast or a multicast address if(ipv4IsBroadcastAddr(interface, ipHeader->destAddr) || ipv4IsMulticastAddr(ipHeader->destAddr)) { //Report an error return ERROR_INVALID_ADDRESS; } //Length of the data that will be returned along with the ICMP header length = MIN(length, ipHeader->headerLength * 4 + 8); //Allocate a memory buffer to hold the ICMP message icmpMessage = ipAllocBuffer(sizeof(IcmpErrorMessage), &offset); //Failed to allocate memory? if(!icmpMessage) return ERROR_OUT_OF_MEMORY; //Point to the ICMP header icmpHeader = chunkedBufferAt(icmpMessage, offset); //Format ICMP message icmpHeader->type = type; icmpHeader->code = code; icmpHeader->checksum = 0; icmpHeader->parameter = parameter; icmpHeader->unused = 0; //Copy the IP header and the first 8 bytes of the original datagram data error = chunkedBufferConcat(icmpMessage, ipPacket, 0, length); //Any error to report? if(error) { //Clean up side effects chunkedBufferFree(icmpMessage); //Exit immediately return error; } //Get the length of the resulting message length = chunkedBufferGetLength(icmpMessage) - offset; //Message checksum calculation icmpHeader->checksum = ipCalcChecksumEx(icmpMessage, offset, length); //Format IPv4 pseudo header pseudoHeader.srcAddr = ipHeader->destAddr; pseudoHeader.destAddr = ipHeader->srcAddr; pseudoHeader.reserved = 0; pseudoHeader.protocol = IPV4_PROTOCOL_ICMP; pseudoHeader.length = htons(length); //Debug message TRACE_INFO("Sending ICMP Error message (%" PRIuSIZE " bytes)...\r\n", length); //Dump message contents for debugging purpose icmpDumpErrorMessage(icmpHeader); //Send ICMP Error message error = ipv4SendDatagram(interface, &pseudoHeader, icmpMessage, offset, IPV4_DEFAULT_TTL); //Free previously allocated memory chunkedBufferFree(icmpMessage); //Return status code return error; }
error_t ipv4SendPacket(NetInterface *interface, Ipv4PseudoHeader *pseudoHeader, uint16_t fragId, uint16_t fragOffset, ChunkedBuffer *buffer, size_t offset, uint8_t timeToLive) { error_t error; size_t length; Ipv4Addr destIpAddr = 0; MacAddr destMacAddr; Ipv4Header *packet; //Is there enough space for the IPv4 header? if(offset < sizeof(Ipv4Header)) return ERROR_INVALID_PARAMETER; //Make room for the header offset -= sizeof(Ipv4Header); //Calculate the size of the entire packet, including header and data length = chunkedBufferGetLength(buffer) - offset; //Point to the IPv4 header packet = chunkedBufferAt(buffer, offset); //Format IPv4 header packet->version = IPV4_VERSION; packet->headerLength = 5; packet->typeOfService = 0; packet->totalLength = htons(length); packet->identification = htons(fragId); packet->fragmentOffset = htons(fragOffset); packet->timeToLive = timeToLive; packet->protocol = pseudoHeader->protocol; packet->headerChecksum = 0; packet->srcAddr = pseudoHeader->srcAddr; packet->destAddr = pseudoHeader->destAddr; //Calculate IP header checksum packet->headerChecksum = ipCalcChecksumEx(buffer, offset, packet->headerLength * 4); //Ensure the source address is valid error = ipv4CheckSourceAddr(interface, pseudoHeader->srcAddr); //Invalid source address? if(error) return error; //Destination address is the unspecified address? if(pseudoHeader->destAddr == IPV4_UNSPECIFIED_ADDR) { //Destination address is not acceptable error = ERROR_INVALID_ADDRESS; } //Destination address is the loopback address? else if(pseudoHeader->destAddr == IPV4_LOOPBACK_ADDR) { //Not yet implemented... error = ERROR_NOT_IMPLEMENTED; } //Destination address is a broadcast address? else if(ipv4IsBroadcastAddr(interface, pseudoHeader->destAddr)) { //Destination IPv4 address destIpAddr = pseudoHeader->destAddr; //Make use of the broadcast MAC address destMacAddr = MAC_BROADCAST_ADDR; //No error to report error = NO_ERROR; } //Destination address is a multicast address? else if(ipv4IsMulticastAddr(pseudoHeader->destAddr)) { //Destination IPv4 address destIpAddr = pseudoHeader->destAddr; //Map IPv4 multicast address to MAC-layer multicast address error = ipv4MapMulticastAddrToMac(pseudoHeader->destAddr, &destMacAddr); } //Destination host is in the local subnet? else if(ipv4IsInLocalSubnet(interface, pseudoHeader->destAddr)) { //Destination IPv4 address destIpAddr = pseudoHeader->destAddr; //Resolve host address before sending the packet error = arpResolve(interface, pseudoHeader->destAddr, &destMacAddr); } //Destination host is outside the local subnet? else { //Make sure the default gateway is properly set if(interface->ipv4Config.defaultGateway != IPV4_UNSPECIFIED_ADDR) { //Use the default gateway to forward the packet destIpAddr = interface->ipv4Config.defaultGateway; //Perform address resolution error = arpResolve(interface, interface->ipv4Config.defaultGateway, &destMacAddr); } else { //There is no route to the outside world... error = ERROR_NO_ROUTE; } } //Successful address resolution? if(!error) { //Debug message TRACE_INFO("Sending IPv4 packet (%u bytes)...\r\n", length); //Dump IP header contents for debugging purpose ipv4DumpHeader(packet); //Send Ethernet frame error = ethSendFrame(interface, &destMacAddr, buffer, offset, ETH_TYPE_IPV4); } //Address resolution is in progress? else if(error == ERROR_IN_PROGRESS) { //Debug message TRACE_INFO("Enqueuing IPv4 packet (%u bytes)...\r\n", length); //Dump IP header contents for debugging purpose ipv4DumpHeader(packet); //Enqueue packets waiting for address resolution error = arpEnqueuePacket(interface, destIpAddr, buffer, offset); } //Address resolution failed? else { //Debug message TRACE_WARNING("Cannot map IPv4 address to Ethernet address!\r\n"); } //Return status code return error; }