void ipv4ReassembleDatagram(NetInterface *interface,
const MacAddr *srcMacAddr, const Ipv4Header *packet, size_t length)
{
error_t error;
uint16_t offset;
uint16_t dataFirst;
uint16_t dataLast;
Ipv4FragDesc *frag;
Ipv4HoleDesc *hole;
Ipv4HoleDesc *prevHole;
//Get the length of the payload
length -= packet->headerLength * 4;
//Convert the fragment offset from network byte order
offset = ntohs(packet->fragmentOffset);
//Every fragment except the last must contain a multiple of 8 bytes of data
if((offset & IPV4_FLAG_MF) && (length % 8))
{
//Drop incoming packet
return;
}
//Calculate the index of the first byte
dataFirst = (offset & IPV4_OFFSET_MASK) * 8;
//Calculate the index immediately following the last byte
dataLast = dataFirst + length;
//Search for a matching IP datagram being reassembled
frag = ipv4SearchFragQueue(interface, packet);
//No matching entry in the reassembly queue?
if(!frag) return;
//The very first fragment requires special handling
if(!(offset & IPV4_OFFSET_MASK))
{
//Calculate the length of the IP header including options
frag->headerLength = packet->headerLength * 4;
//Enforce the size of the reconstructed datagram
if((frag->headerLength + frag->dataLength) > IPV4_MAX_FRAG_DATAGRAM_SIZE)
{
//Drop any allocated resources
chunkedBufferSetLength((ChunkedBuffer *) &frag->buffer, 0);
//Exit immediately
return;
}
//Make sure the IP header entirely fits in the first chunk
if(frag->headerLength > frag->buffer.chunk[0].size)
{
//Drop the reconstructed datagram
chunkedBufferSetLength((ChunkedBuffer *) &frag->buffer, 0);
//Exit immediately
return;
}
//Fix the length of the first chunk
frag->buffer.chunk[0].length = frag->headerLength;
//Always take the IP header from the first fragment
chunkedBufferWrite((ChunkedBuffer *) &frag->buffer, 0, packet, frag->headerLength);
}
//It may be necessary to increase the size of the buffer...
if(dataLast > frag->dataLength)
{
//Enforce the size of the reconstructed datagram
if((frag->headerLength + dataLast) > IPV4_MAX_FRAG_DATAGRAM_SIZE)
{
//Drop any allocated resources
chunkedBufferSetLength((ChunkedBuffer *) &frag->buffer, 0);
//Exit immediately
return;
}
//Adjust the size of the reconstructed datagram
error = chunkedBufferSetLength((ChunkedBuffer *) &frag->buffer,
frag->headerLength + dataLast + sizeof(Ipv4HoleDesc));
//Any error to report?
if(error)
{
//Drop the reconstructed datagram
chunkedBufferSetLength((ChunkedBuffer *) &frag->buffer, 0);
//Exit immediately
return;
}
//Actual length of the payload
frag->dataLength = dataLast;
}
//Select the first hole descriptor from the list
hole = ipv4FindHole(frag, frag->firstHole);
//Keep track of the previous hole in the list
prevHole = NULL;
//Iterate through the hole descriptors
while(hole != NULL)
{
//Save lower and upper boundaries for later use
uint16_t holeFirst = hole->first;
uint16_t holeLast = hole->last;
//Check whether the newly arrived fragment
//interacts with this hole in some way
if(dataFirst < holeLast && dataLast > holeFirst)
{
//The current descriptor is no longer valid. We will destroy
//it, and in the next two steps, we will determine whether
//or not it is necessary to create any new hole descriptors
if(prevHole != NULL)
prevHole->next = hole->next;
else
frag->firstHole = hole->next;
//Is there still a hole at the beginning of the segment?
if(dataFirst > holeFirst)
{
//Create a new entry that describes this hole
hole = ipv4FindHole(frag, holeFirst);
hole->first = holeFirst;
hole->last = dataFirst;
//Insert the newly created entry into the hole descriptor list
if(prevHole != NULL)
{
hole->next = prevHole->next;
prevHole->next = hole->first;
}
else
{
hole->next = frag->firstHole;
frag->firstHole = hole->first;
}
//Always keep track of the previous hole
prevHole = hole;
}
//Is there still a hole at the end of the segment?
if(dataLast < holeLast && (offset & IPV4_FLAG_MF))
{
//Create a new entry that describes this hole
hole = ipv4FindHole(frag, dataLast);
hole->first = dataLast;
hole->last = holeLast;
//Insert the newly created entry into the hole descriptor list
if(prevHole != NULL)
{
hole->next = prevHole->next;
prevHole->next = hole->first;
}
else
{
hole->next = frag->firstHole;
frag->firstHole = hole->first;
}
//Always keep track of the previous hole
prevHole = hole;
}
}
else
{
//The newly arrived fragment does not interact with the current hole
prevHole = hole;
}
//Select the next hole descriptor from the list
hole = ipv4FindHole(frag, prevHole ? prevHole->next : frag->firstHole);
}
//Copy data from the fragment to the reassembly buffer
chunkedBufferWrite((ChunkedBuffer *) &frag->buffer,
frag->headerLength + dataFirst, IPV4_DATA(packet), length);
//Dump hole descriptor list
ipv4DumpHoleList(frag);
//If the hole descriptor list is empty, the reassembly process is now complete
if(!ipv4FindHole(frag, frag->firstHole))
{
//Discard the extra hole descriptor that follows the reconstructed datagram
error = chunkedBufferSetLength((ChunkedBuffer *) &frag->buffer,
frag->headerLength + frag->dataLength);
//Check status code
if(!error)
{
//Point to the IP header
Ipv4Header *datagram = chunkedBufferAt((ChunkedBuffer *) &frag->buffer, 0);
//Fix IP header
datagram->totalLength = htons(frag->headerLength + frag->dataLength);
datagram->fragmentOffset = 0;
datagram->headerChecksum = 0;
//Recalculate IP header checksum
datagram->headerChecksum = ipCalcChecksum(datagram, frag->buffer.chunk[0].length);
//Pass the original IPv4 datagram to the higher protocol layer
ipv4ProcessDatagram(interface, srcMacAddr, (ChunkedBuffer *) &frag->buffer);
}
//Release previously allocated memory
chunkedBufferSetLength((ChunkedBuffer *) &frag->buffer, 0);
}
}
void ipv4ProcessPacket(NetInterface *interface,
const MacAddr *srcMacAddr, Ipv4Header *packet, size_t length)
{
//Ensure the packet length is greater than 20 bytes
if(length < sizeof(Ipv4Header))
return;
//Debug message
TRACE_INFO("IPv4 packet received (%u bytes)...\r\n", length);
//Dump IP header contents for debugging purpose
ipv4DumpHeader(packet);
//A packet whose version number is not 4 must be silently discarded
if(packet->version != IPV4_VERSION)
return;
//Valid IPv4 header shall contains more than five 32-bit words
if(packet->headerLength < 5)
return;
//Ensure the total length is correct before processing the packet
if(ntohs(packet->totalLength) < (packet->headerLength * 4))
return;
if(ntohs(packet->totalLength) > length)
return;
//Destination address filtering
if(ipv4CheckDestAddr(interface, packet->destAddr))
return;
//Source address filtering
if(ipv4CheckSourceAddr(interface, packet->srcAddr))
return;
//The host must verify the IP header checksum on every received
//datagram and silently discard every datagram that has a bad
//checksum (see RFC 1122 3.2.1.2)
if(ipCalcChecksum(packet, packet->headerLength * 4) != 0x0000)
{
//Debug message
TRACE_WARNING("Wrong IP header checksum!\r\n");
//Discard incoming packet
return;
}
//Convert the total length from network byte order
length = ntohs(packet->totalLength);
//A fragmented packet was received?
if(ntohs(packet->fragmentOffset) & (IPV4_FLAG_MF | IPV4_OFFSET_MASK))
{
#if (IPV4_FRAG_SUPPORT == ENABLED)
//Acquire exclusive access to the reassembly queue
osMutexAcquire(interface->ipv4FragQueueMutex);
//Reassemble the original datagram
ipv4ReassembleDatagram(interface, srcMacAddr, packet, length);
//Release exclusive access to the reassembly queue
osMutexRelease(interface->ipv4FragQueueMutex);
#endif
}
else
{
ChunkedBuffer1 buffer;
//Unfragmented datagrams fit in a single chunk
buffer.chunkCount = 1;
buffer.maxChunkCount = 1;
buffer.chunk[0].address = packet;
buffer.chunk[0].length = length;
//Pass the IPv4 datagram to the higher protocol layer
ipv4ProcessDatagram(interface, srcMacAddr, (ChunkedBuffer *) &buffer);
}
}
error_t ping(NetInterface *interface, const IpAddr *ipAddr, time_t timeout, time_t *rtt)
{
error_t error;
uint_t i;
size_t length;
uint16_t identifier;
uint16_t sequenceNumber;
time_t startTime;
time_t roundTripTime;
Socket *socket;
IcmpEchoMessage *message;
//Debug message
TRACE_INFO("Pinging %s with 64 bytes of data...\r\n", ipAddrToString(ipAddr, NULL));
//Length of the complete ICMP message including header and data
length = sizeof(IcmpEchoMessage) + PING_DATA_SIZE;
//Allocate memory buffer to hold an ICMP message
message = osMemAlloc(length);
//Failed to allocate memory?
if(!message) return ERROR_OUT_OF_MEMORY;
//Identifier field is used to help matching requests and replies
identifier = rand();
//Sequence Number field is increment each time an Echo Request is sent
sequenceNumber = osAtomicInc16(&pingSequenceNumber);
//Format ICMP Echo Request message
message->type = ICMP_TYPE_ECHO_REQUEST;
message->code = 0;
message->checksum = 0;
message->identifier = identifier;
message->sequenceNumber = sequenceNumber;
//Copy data
for(i = 0; i < PING_DATA_SIZE; i++)
message->data[i] = i;
#if (IPV4_SUPPORT == ENABLED)
//Target address is an IPv4 address?
if(ipAddr->length == sizeof(Ipv4Addr))
{
Ipv4Addr srcIpAddr;
//Select the source IPv4 address and the relevant network
//interface to use when pinging the specified host
error = ipv4SelectSourceAddr(&interface, ipAddr->ipv4Addr, &srcIpAddr);
//Any error to report?
if(error)
{
//Free previously allocated memory
osMemFree(message);
//Return the corresponding error code
return error;
}
//ICMP Echo Request message
message->type = ICMP_TYPE_ECHO_REQUEST;
//Message checksum calculation
message->checksum = ipCalcChecksum(message, length);
//Open a raw socket
socket = socketOpen(SOCKET_TYPE_RAW, SOCKET_PROTOCOL_ICMP);
}
else
#endif
#if (IPV6_SUPPORT == ENABLED)
//Target address is an IPv6 address?
if(ipAddr->length == sizeof(Ipv6Addr))
{
Ipv6PseudoHeader pseudoHeader;
//Select the source IPv6 address and the relevant network
//interface to use when pinging the specified host
error = ipv6SelectSourceAddr(&interface, &ipAddr->ipv6Addr, &pseudoHeader.srcAddr);
//Any error to report?
if(error)
{
//Free previously allocated memory
osMemFree(message);
//Return the corresponding error code
return error;
}
//ICMPv6 Echo Request message
message->type = ICMPV6_TYPE_ECHO_REQUEST;
//Format IPv6 pseudo header
pseudoHeader.destAddr = ipAddr->ipv6Addr;
pseudoHeader.length = htonl(length);
pseudoHeader.reserved = 0;
pseudoHeader.nextHeader = IPV6_ICMPV6_HEADER;
//Message checksum calculation
message->checksum = ipCalcUpperLayerChecksum(
&pseudoHeader, sizeof(Ipv6PseudoHeader), message, length);
//Open a raw socket
socket = socketOpen(SOCKET_TYPE_RAW, SOCKET_PROTOCOL_ICMPV6);
}
else
#endif
//Target address is not valid?
{
//Free previously allocated memory
osMemFree(message);
//Report an error
return ERROR_INVALID_ADDRESS;
}
//Failed to open socket?
if(!socket)
{
//Free previously allocated memory
osMemFree(message);
//Report an error
return ERROR_OPEN_FAILED;
}
//Associate the newly created socket with the relevant interface
error = socketBindToInterface(socket, interface);
//Unable to bind the socket to the desired interface?
if(error)
{
//Free previously allocated memory
osMemFree(message);
//Close socket
socketClose(socket);
//Return status code
return error;
}
//Connect the socket to the target host
error = socketConnect(socket, ipAddr, 0);
//Any error to report?
if(error)
{
//Free previously allocated memory
osMemFree(message);
//Close socket
socketClose(socket);
//Return status code
return error;
}
//Send Echo Request message
error = socketSend(socket, message, length, NULL, 0);
//Failed to send message ?
if(error)
{
//Free previously allocated memory
osMemFree(message);
//Close socket
socketClose(socket);
//Return status code
return error;
}
//Save the time at which the request was sent
startTime = osGetTickCount();
//Timeout value exceeded?
while((osGetTickCount() - startTime) < timeout)
{
//Adjust receive timeout
error = socketSetTimeout(socket, timeout);
//Any error to report?
if(error) break;
//Wait for an incoming ICMP message
error = socketReceive(socket, message,
sizeof(IcmpEchoMessage) + PING_DATA_SIZE, &length, 0);
//Any error to report?
if(error) break;
//Check message length
if(length != (sizeof(IcmpEchoMessage) + PING_DATA_SIZE))
continue;
//Verify message type
if(ipAddr->length == sizeof(Ipv4Addr) && message->type != ICMP_TYPE_ECHO_REPLY)
continue;
if(ipAddr->length == sizeof(Ipv6Addr) && message->type != ICMPV6_TYPE_ECHO_REPLY)
continue;
//Response identifier matches request identifier?
if(message->identifier != identifier)
continue;
//Make sure the sequence number is correct
if(message->sequenceNumber != sequenceNumber)
continue;
//Loop through data field
for(i = 0; i < PING_DATA_SIZE; i++)
{
//Compare received data against expected data
if(message->data[i] != i) break;
}
//Valid Echo Reply message received?
if(i == PING_DATA_SIZE)
{
//Calculate round-trip time
roundTripTime = osGetTickCount() - startTime;
//Debug message
TRACE_INFO("Echo received (round-trip time = %ums)...\r\n", roundTripTime);
//Free previously allocated memory
osMemFree(message);
//Close socket
socketClose(socket);
//Return round-trip time
if(rtt) *rtt = roundTripTime;
//No error to report
return NO_ERROR;
}
}
//Debug message
TRACE_INFO("No echo received!\r\n");
//Free previously allocated memory
osMemFree(message);
//Close socket
socketClose(socket);
//No Echo Reply received from host...
return ERROR_NO_RESPONSE;
}
