void rawSocketProcessEthPacket(NetInterface *interface,
EthHeader *ethFrame, size_t length)
{
uint_t i;
Socket *socket;
SocketQueueItem *queueItem;
ChunkedBuffer *p;
//Enter critical section
osAcquireMutex(&socketMutex);
//Loop through opened sockets
for(i = 0; i < SOCKET_MAX_COUNT; i++)
{
//Point to the current socket
socket = socketTable + i;
//Raw socket found?
if(socket->type != SOCKET_TYPE_RAW_ETH)
continue;
//Check whether the socket is bound to a particular interface
if(socket->interface && socket->interface != interface)
continue;
//Check protocol field
if(socket->protocol != SOCKET_ETH_PROTO_ALL && socket->protocol != ntohs(ethFrame->type))
continue;
//The current socket meets all the criteria
break;
}
//Drop incoming packet if no matching socket was found
if(i >= SOCKET_MAX_COUNT)
{
//Leave critical section
osReleaseMutex(&socketMutex);
//Return immediately
return;
}
//Empty receive queue?
if(!socket->receiveQueue)
{
//Allocate a memory buffer to hold the data and the associated descriptor
p = chunkedBufferAlloc(sizeof(SocketQueueItem) + length);
//Successful memory allocation?
if(p != NULL)
{
//Point to the newly created item
queueItem = chunkedBufferAt(p, 0);
queueItem->buffer = p;
//Add the newly created item to the queue
socket->receiveQueue = queueItem;
}
else
{
//Memory allocation failed
queueItem = NULL;
}
}
else
{
//Point to the very first item
queueItem = socket->receiveQueue;
//Reach the last item in the receive queue
for(i = 1; queueItem->next; i++)
queueItem = queueItem->next;
//Make sure the receive queue is not full
if(i >= RAW_SOCKET_RX_QUEUE_SIZE)
{
//Leave critical section
osReleaseMutex(&socketMutex);
//Return immediately
return;
}
//Allocate a memory buffer to hold the data and the associated descriptor
p = chunkedBufferAlloc(sizeof(SocketQueueItem) + length);
//Successful memory allocation?
if(p != NULL)
{
//Add the newly created item to the queue
queueItem->next = chunkedBufferAt(p, 0);
//Point to the newly created item
queueItem = queueItem->next;
queueItem->buffer = p;
}
else
{
//Memory allocation failed
queueItem = NULL;
}
}
//Failed to allocate memory?
if(!queueItem)
{
//Leave critical section
osReleaseMutex(&socketMutex);
//Return immediately
return;
}
//Initialize next field
queueItem->next = NULL;
//Other fields are meaningless
queueItem->srcPort = 0;
queueItem->srcIpAddr = IP_ADDR_ANY;
queueItem->destIpAddr = IP_ADDR_ANY;
//Offset to the raw datagram
queueItem->offset = sizeof(SocketQueueItem);
//Copy the raw data
chunkedBufferWrite(queueItem->buffer, queueItem->offset, ethFrame, length);
//Notify user that data is available
rawSocketUpdateEvents(socket);
//Leave critical section
osReleaseMutex(&socketMutex);
}
Ipv6FragDesc *ipv6SearchFragQueue(NetInterface *interface,
Ipv6Header *packet, Ipv6FragmentHeader *header)
{
error_t error;
uint_t i;
Ipv6Header *datagram;
Ipv6FragDesc *frag;
Ipv6HoleDesc *hole;
//Search for a matching IP datagram being reassembled
for(i = 0; i < IPV6_MAX_FRAG_DATAGRAMS; i++)
{
//Point to the current entry in the reassembly queue
frag = &interface->ipv6FragQueue[i];
//Check whether the current entry is used?
if(frag->buffer.chunkCount > 0)
{
//Point to the corresponding datagram
datagram = chunkedBufferAt((ChunkedBuffer *) &frag->buffer, 0);
//Check source and destination addresses
if(!ipv6CompAddr(&datagram->srcAddr, &packet->srcAddr))
continue;
if(!ipv6CompAddr(&datagram->destAddr, &packet->destAddr))
continue;
//Compare fragment identification fields
if(frag->identification != header->identification)
continue;
//A matching entry has been found in the reassembly queue
return frag;
}
}
//If the current packet does not match an existing entry
//in the reassembly queue, then create a new entry
for(i = 0; i < IPV6_MAX_FRAG_DATAGRAMS; i++)
{
//Point to the current entry in the reassembly queue
frag = &interface->ipv6FragQueue[i];
//The current entry is free?
if(!frag->buffer.chunkCount)
{
//Number of chunks that comprise the reassembly buffer
frag->buffer.maxChunkCount = arraysize(frag->buffer.chunk);
//Allocate sufficient memory to hold the IPv6 header and
//the first hole descriptor
error = chunkedBufferSetLength((ChunkedBuffer *) &frag->buffer,
MEM_POOL_BUFFER_SIZE + sizeof(Ipv6HoleDesc));
//Failed to allocate memory?
if(error)
{
//Clean up side effects
chunkedBufferSetLength((ChunkedBuffer *) &frag->buffer, 0);
//Exit immediately
return NULL;
}
//Initial length of the reconstructed datagram
frag->unfragPartLength = sizeof(Ipv6Header);
frag->fragPartLength = 0;
//Fix the length of the first chunk
frag->buffer.chunk[0].length = frag->unfragPartLength;
//Copy IPv6 header from the incoming fragment
chunkedBufferWrite((ChunkedBuffer *) &frag->buffer, 0, packet, frag->unfragPartLength);
//Save current time
frag->timestamp = osGetSystemTime();
//Record fragment identification field
frag->identification = header->identification;
//Create a new entry in the hole descriptor list
frag->firstHole = 0;
//Point to first hole descriptor
hole = ipv6FindHole(frag, frag->firstHole);
//The entry describes the datagram as being completely missing
hole->first = 0;
hole->last = IPV6_INFINITY;
hole->next = IPV6_INFINITY;
//Dump hole descriptor list
ipv6DumpHoleList(frag);
//Return the matching fragment descriptor
return frag;
}
}
//The reassembly queue is full
return NULL;
}
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);
}
}
