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;
}
