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