error_t udpProcessDatagram(NetInterface *interface,
IpPseudoHeader *pseudoHeader, const ChunkedBuffer *buffer, size_t offset)
{
error_t error;
uint_t i;
size_t length;
UdpHeader *header;
Socket *socket;
SocketQueueItem *queueItem;
ChunkedBuffer *p;
//Retrieve the length of the UDP datagram
length = chunkedBufferGetLength(buffer) - offset;
//Ensure the UDP header is valid
if(length < sizeof(UdpHeader))
{
//Debug message
TRACE_WARNING("UDP datagram length is invalid!\r\n");
//Report an error
return ERROR_INVALID_HEADER;
}
//Point to the UDP header
header = chunkedBufferAt(buffer, offset);
//Sanity check
if(!header) return ERROR_FAILURE;
//Debug message
TRACE_INFO("UDP datagram received (%" PRIuSIZE " bytes)...\r\n", length);
//Dump UDP header contents for debugging purpose
udpDumpHeader(header);
//When UDP runs over IPv6, the checksum is mandatory
if(header->checksum || pseudoHeader->length == sizeof(Ipv6PseudoHeader))
{
//Verify UDP checksum
if(ipCalcUpperLayerChecksumEx(pseudoHeader->data,
pseudoHeader->length, buffer, offset, length) != 0xFFFF)
{
//Debug message
TRACE_WARNING("Wrong UDP header checksum!\r\n");
//Report an error
return ERROR_WRONG_CHECKSUM;
}
}
//Enter critical section
osMutexAcquire(socketMutex);
//Loop through opened sockets
for(i = 0; i < SOCKET_MAX_COUNT; i++)
{
//Point to the current socket
socket = socketTable + i;
//UDP socket found?
if(socket->type != SOCKET_TYPE_DGRAM)
continue;
//Check whether the socket is bound to a particular interface
if(socket->interface && socket->interface != interface)
continue;
//Check destination port number
if(socket->localPort != ntohs(header->destPort))
continue;
//Source port number filtering
if(socket->remotePort && socket->remotePort != ntohs(header->srcPort))
continue;
#if (IPV4_SUPPORT == ENABLED)
//An IPv4 packet was received?
if(pseudoHeader->length == sizeof(Ipv4PseudoHeader))
{
//Destination IP address filtering
if(socket->localIpAddr.length)
{
//An IPv4 address is expected
if(socket->localIpAddr.length != sizeof(Ipv4Addr))
continue;
//Filter out non-matching addresses
if(socket->localIpAddr.ipv4Addr != pseudoHeader->ipv4Data.destAddr)
continue;
}
//Source IP address filtering
if(socket->remoteIpAddr.length)
{
//An IPv4 address is expected
if(socket->remoteIpAddr.length != sizeof(Ipv4Addr))
continue;
//Filter out non-matching addresses
if(socket->remoteIpAddr.ipv4Addr != pseudoHeader->ipv4Data.srcAddr)
continue;
}
}
else
#endif
#if (IPV6_SUPPORT == ENABLED)
//An IPv6 packet was received?
if(pseudoHeader->length == sizeof(Ipv6PseudoHeader))
{
//Destination IP address filtering
if(socket->localIpAddr.length)
{
//An IPv6 address is expected
if(socket->localIpAddr.length != sizeof(Ipv6Addr))
continue;
//Filter out non-matching addresses
if(!ipv6CompAddr(&socket->localIpAddr.ipv6Addr, &pseudoHeader->ipv6Data.destAddr))
continue;
}
//Source IP address filtering
if(socket->remoteIpAddr.length)
{
//An IPv6 address is expected
if(socket->remoteIpAddr.length != sizeof(Ipv6Addr))
continue;
//Filter out non-matching addresses
if(!ipv6CompAddr(&socket->remoteIpAddr.ipv6Addr, &pseudoHeader->ipv6Data.srcAddr))
continue;
}
}
else
#endif
//An invalid packet was received?
{
//This should never occur...
continue;
}
//The current socket meets all the criteria
break;
}
//Point to the payload
offset += sizeof(UdpHeader);
length -= sizeof(UdpHeader);
//No matching socket found?
if(i >= SOCKET_MAX_COUNT)
{
//Leave critical section
osMutexRelease(socketMutex);
//Invoke user callback, if any
error = udpInvokeRxCallback(interface, pseudoHeader, header, buffer, offset);
//Return status code
return error;
}
//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 >= UDP_RX_QUEUE_SIZE)
{
//Leave critical section
osMutexRelease(socketMutex);
//Notify the calling function that the queue is full
return ERROR_RECEIVE_QUEUE_FULL;
}
//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
osMutexRelease(socketMutex);
//Return error code
return ERROR_OUT_OF_MEMORY;
}
//Initialize next field
queueItem->next = NULL;
//Record the source port number
queueItem->srcPort = ntohs(header->srcPort);
#if (IPV4_SUPPORT == ENABLED)
//IPv4 remote address?
if(pseudoHeader->length == sizeof(Ipv4PseudoHeader))
{
//Save the source IPv4 address
queueItem->srcIpAddr.length = sizeof(Ipv4Addr);
queueItem->srcIpAddr.ipv4Addr = pseudoHeader->ipv4Data.srcAddr;
//Save the destination IPv4 address
queueItem->destIpAddr.length = sizeof(Ipv4Addr);
queueItem->destIpAddr.ipv4Addr = pseudoHeader->ipv4Data.destAddr;
}
#endif
#if (IPV6_SUPPORT == ENABLED)
//IPv6 remote address?
if(pseudoHeader->length == sizeof(Ipv6PseudoHeader))
{
//Save the source IPv6 address
queueItem->srcIpAddr.length = sizeof(Ipv6Addr);
queueItem->srcIpAddr.ipv6Addr = pseudoHeader->ipv6Data.srcAddr;
//Save the destination IPv6 address
queueItem->destIpAddr.length = sizeof(Ipv6Addr);
queueItem->destIpAddr.ipv6Addr = pseudoHeader->ipv6Data.destAddr;
}
#endif
//Offset to the payload
queueItem->offset = sizeof(SocketQueueItem);
//Copy the payload
chunkedBufferCopy(queueItem->buffer, queueItem->offset, buffer, offset, length);
//Notify user that data is available
udpUpdateEvents(socket);
//Leave critical section
osMutexRelease(socketMutex);
//Successful processing
return NO_ERROR;
}
error_t rawSocketProcessIpPacket(NetInterface *interface,
IpPseudoHeader *pseudoHeader, const ChunkedBuffer *buffer, size_t offset)
{
uint_t i;
size_t length;
Socket *socket;
SocketQueueItem *queueItem;
ChunkedBuffer *p;
//Retrieve the length of the raw IP packet
length = chunkedBufferGetLength(buffer) - offset;
//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_IP)
continue;
//Check whether the socket is bound to a particular interface
if(socket->interface && socket->interface != interface)
continue;
#if (IPV4_SUPPORT == ENABLED)
//An IPv4 packet was received?
if(pseudoHeader->length == sizeof(Ipv4PseudoHeader))
{
//Check protocol field
if(socket->protocol != pseudoHeader->ipv4Data.protocol)
continue;
//Destination IP address filtering
if(socket->localIpAddr.length)
{
//An IPv4 address is expected
if(socket->localIpAddr.length != sizeof(Ipv4Addr))
continue;
//Filter out non-matching addresses
if(socket->localIpAddr.ipv4Addr != pseudoHeader->ipv4Data.destAddr)
continue;
}
//Source IP address filtering
if(socket->remoteIpAddr.length)
{
//An IPv4 address is expected
if(socket->remoteIpAddr.length != sizeof(Ipv4Addr))
continue;
//Filter out non-matching addresses
if(socket->remoteIpAddr.ipv4Addr != pseudoHeader->ipv4Data.srcAddr)
continue;
}
}
else
#endif
#if (IPV6_SUPPORT == ENABLED)
//An IPv6 packet was received?
if(pseudoHeader->length == sizeof(Ipv6PseudoHeader))
{
//Check protocol field
if(socket->protocol != pseudoHeader->ipv6Data.nextHeader)
continue;
//Destination IP address filtering
if(socket->localIpAddr.length)
{
//An IPv6 address is expected
if(socket->localIpAddr.length != sizeof(Ipv6Addr))
continue;
//Filter out non-matching addresses
if(!ipv6CompAddr(&socket->localIpAddr.ipv6Addr, &pseudoHeader->ipv6Data.destAddr))
continue;
}
//Source IP address filtering
if(socket->remoteIpAddr.length)
{
//An IPv6 address is expected
if(socket->remoteIpAddr.length != sizeof(Ipv6Addr))
continue;
//Filter out non-matching addresses
if(!ipv6CompAddr(&socket->remoteIpAddr.ipv6Addr, &pseudoHeader->ipv6Data.srcAddr))
continue;
}
}
else
#endif
//An invalid packet was received?
{
//This should never occur...
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);
//Unreachable protocol...
return ERROR_PROTOCOL_UNREACHABLE;
}
//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);
//Notify the calling function that the queue is full
return ERROR_RECEIVE_QUEUE_FULL;
}
//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 error code
return ERROR_OUT_OF_MEMORY;
}
//Initialize next field
queueItem->next = NULL;
//Port number is unused
queueItem->srcPort = 0;
#if (IPV4_SUPPORT == ENABLED)
//IPv4 remote address?
if(pseudoHeader->length == sizeof(Ipv4PseudoHeader))
{
//Save the source IPv4 address
queueItem->srcIpAddr.length = sizeof(Ipv4Addr);
queueItem->srcIpAddr.ipv4Addr = pseudoHeader->ipv4Data.srcAddr;
//Save the destination IPv4 address
queueItem->destIpAddr.length = sizeof(Ipv4Addr);
queueItem->destIpAddr.ipv4Addr = pseudoHeader->ipv4Data.destAddr;
}
#endif
#if (IPV6_SUPPORT == ENABLED)
//IPv6 remote address?
if(pseudoHeader->length == sizeof(Ipv6PseudoHeader))
{
//Save the source IPv6 address
queueItem->srcIpAddr.length = sizeof(Ipv6Addr);
queueItem->srcIpAddr.ipv6Addr = pseudoHeader->ipv6Data.srcAddr;
//Save the destination IPv6 address
queueItem->destIpAddr.length = sizeof(Ipv6Addr);
queueItem->destIpAddr.ipv6Addr = pseudoHeader->ipv6Data.destAddr;
}
#endif
//Offset to the raw IP packet
queueItem->offset = sizeof(SocketQueueItem);
//Copy the raw data
chunkedBufferCopy(queueItem->buffer, queueItem->offset, buffer, offset, length);
//Notify user that data is available
rawSocketUpdateEvents(socket);
//Leave critical section
osReleaseMutex(&socketMutex);
//Successful processing
return NO_ERROR;
}
error_t arpEnqueuePacket(NetInterface *interface,
Ipv4Addr ipAddr, ChunkedBuffer *buffer, size_t offset)
{
error_t error;
uint_t i;
size_t length;
ArpCacheEntry *entry;
//Retrieve the length of the multi-part buffer
length = chunkedBufferGetLength(buffer);
//Acquire exclusive access to ARP cache
osMutexAcquire(interface->arpCacheMutex);
//Search the ARP cache for the specified Ipv4 address
entry = arpFindEntry(interface, ipAddr);
//No matching entry in ARP cache?
if(!entry)
{
//Release exclusive access to ARP cache
osMutexRelease(interface->arpCacheMutex);
//Report an error to the calling function
return ERROR_FAILURE;
}
//Check current state
if(entry->state == ARP_STATE_INCOMPLETE)
{
//Check whether the packet queue is full
if(entry->queueSize >= ARP_MAX_PENDING_PACKETS)
{
//When the queue overflows, the new arrival should replace the oldest entry
chunkedBufferFree(entry->queue[0].buffer);
//Make room for the new packet
for(i = 1; i < ARP_MAX_PENDING_PACKETS; i++)
entry->queue[i - 1] = entry->queue[i];
//Adjust the number of pending packets
entry->queueSize--;
}
//Index of the entry to be filled in
i = entry->queueSize;
//Allocate a memory buffer to store the packet
entry->queue[i].buffer = chunkedBufferAlloc(length);
//Failed to allocate memory?
if(!entry->queue[i].buffer)
{
//Release exclusive access to ARP cache
osMutexRelease(interface->arpCacheMutex);
//Report an error to the calling function
return ERROR_OUT_OF_MEMORY;
}
//Copy packet contents
chunkedBufferCopy(entry->queue[i].buffer, 0, buffer, 0, length);
//Offset to the first byte of the IPv4 header
entry->queue[i].offset = offset;
//Increment the number of queued packets
entry->queueSize++;
//The packet was successfully enqueued
error = NO_ERROR;
}
else
{
//Send immediately the packet since the address is already resolved
error = ethSendFrame(interface, &entry->macAddr, buffer, offset, ETH_TYPE_IPV4);
}
//Release exclusive access to ARP cache
osMutexRelease(interface->arpCacheMutex);
//Return status code
return error;
}
void ipv6ParseFragmentHeader(NetInterface *interface, const MacAddr *srcMacAddr,
const ChunkedBuffer *buffer, size_t fragHeaderOffset, size_t nextHeaderOffset)
{
error_t error;
size_t length;
uint16_t offset;
uint16_t dataFirst;
uint16_t dataLast;
Ipv6FragDesc *frag;
Ipv6HoleDesc *hole;
Ipv6HoleDesc *prevHole;
Ipv6Header *packet;
Ipv6FragmentHeader *header;
//Remaining bytes to process in the payload
length = chunkedBufferGetLength(buffer) - fragHeaderOffset;
//Ensure the fragment header is valid
if(length < sizeof(Ipv6FragmentHeader))
return;
//Point to the IPv6 header
packet = chunkedBufferAt(buffer, 0);
//Sanity check
if(!packet) return;
//Point to the IPv6 Fragment header
header = chunkedBufferAt(buffer, fragHeaderOffset);
//Sanity check
if(!header) return;
//Calculate the length of the fragment
length -= sizeof(Ipv6FragmentHeader);
//Convert the fragment offset from network byte order
offset = ntohs(header->fragmentOffset);
//Every fragment except the last must contain a multiple of 8 bytes of data
if((offset & IPV6_FLAG_M) && (length % 8))
{
//Compute the offset of the Payload Length field within the packet
size_t n = (uint8_t *) &packet->payloadLength - (uint8_t *) packet;
//The fragment must be discarded and an ICMP Parameter Problem
//message should be sent to the source of the fragment, pointing
//to the Payload Length field of the fragment packet
icmpv6SendErrorMessage(interface, ICMPV6_TYPE_PARAM_PROBLEM,
ICMPV6_CODE_INVALID_HEADER_FIELD, n, buffer);
//Exit immediately
return;
}
//Calculate the index of the first byte
dataFirst = offset & IPV6_OFFSET_MASK;
//Calculate the index immediately following the last byte
dataLast = dataFirst + length;
//Search for a matching IP datagram being reassembled
frag = ipv6SearchFragQueue(interface, packet, header);
//No matching entry in the reassembly queue?
if(!frag) return;
//The very first fragment requires special handling
if(!(offset & IPV6_OFFSET_MASK))
{
uint8_t *p;
//Calculate the length of the unfragmentable part
frag->unfragPartLength = fragHeaderOffset;
//The size of the reconstructed datagram exceeds the maximum value?
if((frag->unfragPartLength + frag->fragPartLength) > IPV6_MAX_FRAG_DATAGRAM_SIZE)
{
//Compute the offset of the Fragment Offset field within the packet
size_t n = fragHeaderOffset + (uint8_t *) &header->fragmentOffset -
(uint8_t *) header;
//The fragment must be discarded and an ICMP Parameter Problem
//message should be sent to the source of the fragment, pointing
//to the Fragment Offset field of the fragment packet
icmpv6SendErrorMessage(interface, ICMPV6_TYPE_PARAM_PROBLEM,
ICMPV6_CODE_INVALID_HEADER_FIELD, n, buffer);
//Drop any allocated resources
chunkedBufferSetLength((ChunkedBuffer *) &frag->buffer, 0);
//Exit immediately
return;
}
//Make sure the unfragmentable part entirely fits in the first chunk
if(frag->unfragPartLength > 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->unfragPartLength;
//The unfragmentable part of the reassembled packet consists
//of all headers up to, but not including, the Fragment header
//of the first fragment packet
chunkedBufferCopy((ChunkedBuffer *) &frag->buffer, 0, buffer, 0, frag->unfragPartLength);
//Point to the Next Header field of the last header
p = chunkedBufferAt((ChunkedBuffer *) &frag->buffer, nextHeaderOffset);
//The Next Header field of the last header of the unfragmentable
//part is obtained from the Next Header field of the first
//fragment's Fragment header
*p = header->nextHeader;
}
//It may be necessary to increase the size of the buffer...
if(dataLast > frag->fragPartLength)
{
//The size of the reconstructed datagram exceeds the maximum value?
if((frag->unfragPartLength + dataLast) > IPV6_MAX_FRAG_DATAGRAM_SIZE)
{
//Compute the offset of the Fragment Offset field within the packet
size_t n = fragHeaderOffset + (uint8_t *) &header->fragmentOffset - (uint8_t *) header;
//The fragment must be discarded and an ICMP Parameter Problem
//message should be sent to the source of the fragment, pointing
//to the Fragment Offset field of the fragment packet
icmpv6SendErrorMessage(interface, ICMPV6_TYPE_PARAM_PROBLEM,
ICMPV6_CODE_INVALID_HEADER_FIELD, n, buffer);
//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->unfragPartLength + dataLast + sizeof(Ipv6HoleDesc));
//Any error to report?
if(error)
{
//Drop the reconstructed datagram
chunkedBufferSetLength((ChunkedBuffer *) &frag->buffer, 0);
//Exit immediately
return;
}
//Actual length of the fragmentable part
frag->fragPartLength = dataLast;
}
//Select the first hole descriptor from the list
hole = ipv6FindHole(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 = ipv6FindHole(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 & IPV6_FLAG_M))
{
//Create a new entry that describes this hole
hole = ipv6FindHole(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 = ipv6FindHole(frag, prevHole ? prevHole->next : frag->firstHole);
}
//Copy data from the fragment to the reassembly buffer
chunkedBufferCopy((ChunkedBuffer *) &frag->buffer, frag->unfragPartLength + dataFirst,
buffer, fragHeaderOffset + sizeof(Ipv6FragmentHeader), length);
//Dump hole descriptor list
ipv6DumpHoleList(frag);
//If the hole descriptor list is empty, the reassembly process is now complete
if(!ipv6FindHole(frag, frag->firstHole))
{
//Discard the extra hole descriptor that follows the reconstructed datagram
error = chunkedBufferSetLength((ChunkedBuffer *) &frag->buffer,
frag->unfragPartLength + frag->fragPartLength);
//Check status code
if(!error)
{
//Point to the IPv6 header
Ipv6Header *datagram = chunkedBufferAt((ChunkedBuffer *) &frag->buffer, 0);
//Fix the Payload Length field
datagram->payloadLength = htons(frag->unfragPartLength +
frag->fragPartLength - sizeof(Ipv6Header));
//Pass the original IPv6 datagram to the higher protocol layer
ipv6ProcessPacket(interface, srcMacAddr, (ChunkedBuffer *) &frag->buffer);
}
//Release previously allocated memory
chunkedBufferSetLength((ChunkedBuffer *) &frag->buffer, 0);
}
}
