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