void ipv6FragTick(NetInterface *interface)
{
error_t error;
uint_t i;
systime_t time;
Ipv6HoleDesc *hole;
//Acquire exclusive access to the reassembly queue
osAcquireMutex(&interface->ipv6FragQueueMutex);
//Get current time
time = osGetSystemTime();
//Loop through the reassembly queue
for(i = 0; i < IPV6_MAX_FRAG_DATAGRAMS; i++)
{
//Point to the current entry in the reassembly queue
Ipv6FragDesc *frag = &interface->ipv6FragQueue[i];
//Make sure the entry is currently in use
if(frag->buffer.chunkCount > 0)
{
//If the timer runs out, the partially-reassembled datagram must be
//discarded and ICMPv6 Time Exceeded message sent to the source host
if((time - frag->timestamp) >= IPV6_FRAG_TIME_TO_LIVE)
{
//Debug message
TRACE_INFO("IPv6 fragment reassembly timeout...\r\n");
//Dump IP header contents for debugging purpose
ipv6DumpHeader(frag->buffer.chunk[0].address);
//Point to the first hole descriptor
hole = ipv6FindHole(frag, frag->firstHole);
//Make sure the fragment zero has been received
//before sending an ICMPv6 message
if(hole != NULL && hole->first > 0)
{
//Fix the size of the reconstructed datagram
error = chunkedBufferSetLength((ChunkedBuffer *) &frag->buffer,
frag->unfragPartLength + hole->first);
//Check status code
if(!error)
{
//Send an ICMPv6 Time Exceeded message
icmpv6SendErrorMessage(interface, ICMPV6_TYPE_TIME_EXCEEDED,
ICMPV6_CODE_REASSEMBLY_TIME_EXCEEDED, 0, (ChunkedBuffer *) &frag->buffer);
}
}
//Drop the partially reconstructed datagram
chunkedBufferSetLength((ChunkedBuffer *) &frag->buffer, 0);
}
}
}
//Release exclusive access to the reassembly queue
osReleaseMutex(&interface->ipv6FragQueueMutex);
}
void chunkedBufferFree(ChunkedBuffer *buffer)
{
//Properly dispose data chunks
chunkedBufferSetLength(buffer, 0);
//Release multi-part buffer
memPoolFree(buffer);
}
ChunkedBuffer *chunkedBufferAlloc(size_t length)
{
error_t error;
ChunkedBuffer *buffer;
//Allocate memory to hold the multi-part buffer
buffer = memPoolAlloc(MEM_POOL_BUFFER_SIZE);
//Failed to allocate memory?
if(!buffer) return NULL;
//The multi-part buffer consists of a single chunk
buffer->chunkCount = 1;
buffer->maxChunkCount = MAX_CHUNK_COUNT;
buffer->chunk[0].address = (uint8_t *) buffer + MAX_CHUNK_COUNT * sizeof(ChunkDesc);
buffer->chunk[0].length = MEM_POOL_BUFFER_SIZE - MAX_CHUNK_COUNT * sizeof(ChunkDesc);
buffer->chunk[0].size = 0;
//Adjust the length of the buffer
error = chunkedBufferSetLength(buffer, length);
//Any error to report?
if(error)
{
//Clean up side effects
chunkedBufferFree(buffer);
//Report an failure
return NULL;
}
//Successful memory allocation
return buffer;
}
error_t ipv4FragmentDatagram(NetInterface *interface, Ipv4PseudoHeader *pseudoHeader,
uint16_t id, const ChunkedBuffer *payload, size_t payloadOffset, uint8_t timeToLive)
{
error_t error;
size_t offset;
size_t length;
size_t payloadLength;
size_t fragmentOffset;
ChunkedBuffer *fragment;
//Retrieve the length of the payload
payloadLength = chunkedBufferGetLength(payload) - payloadOffset;
//Allocate a memory buffer to hold IP fragments
fragment = ipAllocBuffer(0, &fragmentOffset);
//Failed to allocate memory?
if(!fragment)
return ERROR_OUT_OF_MEMORY;
//Split the payload into multiple IP fragments
for(offset = 0; offset < payloadLength; offset += length)
{
//Flush the contents of the fragment
error = chunkedBufferSetLength(fragment, fragmentOffset);
//Sanity check
if(error) break;
//Process the last fragment?
if((payloadLength - offset) <= IPV4_MAX_FRAG_SIZE)
{
//Size of the current fragment
length = payloadLength - offset;
//Copy fragment data
chunkedBufferConcat(fragment, payload, payloadOffset + offset, length);
//Do not set the MF flag for the last fragment
error = ipv4SendPacket(interface, pseudoHeader, id,
offset / 8, fragment, fragmentOffset, timeToLive);
}
else
{
//Size of the current fragment (must be a multiple of 8-byte blocks)
length = IPV4_MAX_FRAG_SIZE;
//Copy fragment data
chunkedBufferConcat(fragment, payload, payloadOffset + offset, length);
//Fragmented packets must have the MF flag set
error = ipv4SendPacket(interface, pseudoHeader, id,
IPV4_FLAG_MF | (offset / 8), fragment, fragmentOffset, timeToLive);
}
//Failed to send current IP packet?
if(error) break;
}
//Free previously allocated memory
chunkedBufferFree(fragment);
//Return status code
return error;
}
void ipv6FlushFragQueue(NetInterface *interface)
{
uint_t i;
//Acquire exclusive access to the reassembly queue
osAcquireMutex(&interface->ipv6FragQueueMutex);
//Loop through the reassembly queue
for(i = 0; i < IPV6_MAX_FRAG_DATAGRAMS; i++)
{
//Drop any partially reconstructed datagram
chunkedBufferSetLength((ChunkedBuffer *) &interface->ipv6FragQueue[i].buffer, 0);
}
//Release exclusive access to the reassembly queue
osReleaseMutex(&interface->ipv6FragQueueMutex);
}
error_t ndpSendNeighborAdv(NetInterface *interface,
const Ipv6Addr *targetIpAddr, const Ipv6Addr *destIpAddr)
{
error_t error;
size_t offset;
size_t length;
ChunkedBuffer *buffer;
NdpNeighborAdvMessage *message;
Ipv6PseudoHeader pseudoHeader;
//Allocate a memory buffer to hold the Neighbor Advertisement
//message and the Target Link-Layer Address option
buffer = ipAllocBuffer(sizeof(NdpNeighborAdvMessage) +
sizeof(NdpLinkLayerAddrOption), &offset);
//Failed to allocate memory?
if(!buffer) return ERROR_OUT_OF_MEMORY;
//Point to the beginning of the message
message = chunkedBufferAt(buffer, offset);
//Format Neighbor Advertisement message
message->type = ICMPV6_TYPE_NEIGHBOR_ADV;
message->code = 0;
message->checksum = 0;
message->reserved1 = 0;
message->o = TRUE;
message->s = FALSE;
message->r = FALSE;
message->reserved2 = 0;
message->targetAddr = *targetIpAddr;
//Length of the message, excluding any option
length = sizeof(NdpNeighborAdvMessage);
//Add Target Link-Layer Address option
ndpAddOption(message, &length, NDP_OPT_TARGET_LINK_LAYER_ADDR,
&interface->macAddr, sizeof(MacAddr));
//Adjust the length of the multi-part buffer
chunkedBufferSetLength(buffer, offset + length);
//Format IPv6 pseudo header
pseudoHeader.srcAddr = *targetIpAddr;
pseudoHeader.destAddr = *destIpAddr;
pseudoHeader.length = htonl(length);
pseudoHeader.reserved = 0;
pseudoHeader.nextHeader = IPV6_ICMPV6_HEADER;
//Destination IP address is the unspecified address?
if(ipv6CompAddr(destIpAddr, &IPV6_UNSPECIFIED_ADDR))
{
//If the destination is the unspecified address, the node
//must set the Solicited flag to zero and multicast the
//advertisement to the all-nodes address
pseudoHeader.destAddr = IPV6_LINK_LOCAL_ALL_NODES_ADDR;
}
else
{
//Otherwise, the node must set the Solicited flag to one and
//unicast the advertisement to the destination IP address
message->s = TRUE;
}
//Calculate ICMPv6 header checksum
message->checksum = ipCalcUpperLayerChecksumEx(&pseudoHeader,
sizeof(Ipv6PseudoHeader), buffer, offset, length);
//Debug message
TRACE_INFO("Sending Neighbor Advertisement message (%" PRIuSIZE " bytes)...\r\n", length);
//Dump message contents for debugging purpose
ndpDumpNeighborAdvMessage(message);
//Send Neighbor Advertisement message
error = ipv6SendDatagram(interface, &pseudoHeader, buffer, offset, NDP_HOP_LIMIT);
//Free previously allocated memory
chunkedBufferFree(buffer);
//Return status code
return error;
}
error_t ndpSendNeighborSol(NetInterface *interface, const Ipv6Addr *targetIpAddr)
{
error_t error;
size_t offset;
size_t length;
ChunkedBuffer *buffer;
NdpNeighborSolMessage *message;
Ipv6PseudoHeader pseudoHeader;
//Allocate a memory buffer to hold the Neighbor Solicitation
//message and the Source Link-Layer Address option
buffer = ipAllocBuffer(sizeof(NdpNeighborSolMessage) +
sizeof(NdpLinkLayerAddrOption), &offset);
//Failed to allocate memory?
if(!buffer) return ERROR_OUT_OF_MEMORY;
//Point to the beginning of the message
message = chunkedBufferAt(buffer, offset);
//Format Neighbor Solicitation message
message->type = ICMPV6_TYPE_NEIGHBOR_SOL;
message->code = 0;
message->checksum = 0;
message->reserved = 0;
message->targetAddr = *targetIpAddr;
//Length of the message, excluding any option
length = sizeof(NdpNeighborSolMessage);
//Check whether the target address is a tentative address
if(ipv6IsTentativeAddr(interface, targetIpAddr))
{
//The IPv6 source is set to the unspecified address
pseudoHeader.srcAddr = IPV6_UNSPECIFIED_ADDR;
}
else
{
//The Source Link-Layer Address option must not be included
//when the host IPv6 address is unspecified
if(!ipv6CompAddr(&interface->ipv6Config.linkLocalAddr, &IPV6_UNSPECIFIED_ADDR))
{
//Add Source Link-Layer Address option
ndpAddOption(message, &length, NDP_OPT_SOURCE_LINK_LAYER_ADDR,
&interface->macAddr, sizeof(MacAddr));
}
//Set the IPv6 source address
pseudoHeader.srcAddr = interface->ipv6Config.linkLocalAddr;
}
//Adjust the length of the multi-part buffer
chunkedBufferSetLength(buffer, offset + length);
//Compute the solicited-node multicast address that
//corresponds to the target IPv6 address
ipv6ComputeSolicitedNodeAddr(targetIpAddr, &pseudoHeader.destAddr);
//Format IPv6 pseudo header
pseudoHeader.length = htonl(length);
pseudoHeader.reserved = 0;
pseudoHeader.nextHeader = IPV6_ICMPV6_HEADER;
//Calculate ICMPv6 header checksum
message->checksum = ipCalcUpperLayerChecksumEx(&pseudoHeader,
sizeof(Ipv6PseudoHeader), buffer, offset, length);
//Debug message
TRACE_INFO("Sending Neighbor Solicitation message (%" PRIuSIZE " bytes)...\r\n", length);
//Dump message contents for debugging purpose
ndpDumpNeighborSolMessage(message);
//Send Neighbor Solicitation message
error = ipv6SendDatagram(interface, &pseudoHeader, buffer, offset, NDP_HOP_LIMIT);
//Free previously allocated memory
chunkedBufferFree(buffer);
//Return status code
return error;
}
error_t ipv6FragmentDatagram(NetInterface *interface, Ipv6PseudoHeader *pseudoHeader,
const ChunkedBuffer *payload, size_t payloadOffset, uint8_t hopLimit)
{
error_t error;
uint32_t id;
size_t offset;
size_t length;
size_t payloadLength;
size_t fragmentOffset;
ChunkedBuffer *fragment;
//Identification field is used to identify fragments of an original IP datagram
id = osAtomicInc32(&interface->ipv6Identification);
//Retrieve the length of the payload
payloadLength = chunkedBufferGetLength(payload) - payloadOffset;
//Allocate a memory buffer to hold IP fragments
fragment = ipAllocBuffer(0, &fragmentOffset);
//Failed to allocate memory?
if(!fragment)
return ERROR_OUT_OF_MEMORY;
//Split the payload into multiple IP fragments
for(offset = 0; offset < payloadLength; offset += length)
{
//Flush the contents of the fragment
error = chunkedBufferSetLength(fragment, fragmentOffset);
//Sanity check
if(error) break;
//Process the last fragment?
if((payloadLength - offset) <= IPV6_MAX_FRAG_SIZE)
{
//Size of the current fragment
length = payloadLength - offset;
//Copy fragment data
chunkedBufferConcat(fragment, payload, payloadOffset + offset, length);
//Do not set the MF flag for the last fragment
error = ipv6SendPacket(interface, pseudoHeader, id,
offset, fragment, fragmentOffset, hopLimit);
}
else
{
//Size of the current fragment (must be a multiple of 8-byte blocks)
length = IPV6_MAX_FRAG_SIZE;
//Copy fragment data
chunkedBufferConcat(fragment, payload, payloadOffset + offset, length);
//Fragmented packets must have the M flag set
error = ipv6SendPacket(interface, pseudoHeader, id,
offset | IPV6_FLAG_M, fragment, fragmentOffset, hopLimit);
}
//Failed to send current IP fragment?
if(error) break;
}
//Free previously allocated memory
chunkedBufferFree(fragment);
//Return status code
return error;
}
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 ipv6ParseFragmentHeader(NetInterface *interface, 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, (ChunkedBuffer *) &frag->buffer);
}
//Release previously allocated memory
chunkedBufferSetLength((ChunkedBuffer *) &frag->buffer, 0);
}
}
error_t nbnsSendResponse(NetInterface *interface,
const IpAddr *destIpAddr, uint16_t destPort, uint16_t id)
{
error_t error;
size_t length;
size_t offset;
ChunkedBuffer *buffer;
NbnsHeader *message;
NbnsAddrEntry *addrEntry;
DnsResourceRecord *resourceRecord;
//Allocate a memory buffer to hold the NBNS response message
buffer = udpAllocBuffer(DNS_MESSAGE_MAX_SIZE, &offset);
//Failed to allocate buffer?
if(!buffer) return ERROR_OUT_OF_MEMORY;
//Point to the NBNS header
message = chunkedBufferAt(buffer, offset);
//Take the identifier from the query message
message->id = id;
//Format NBNS response header
message->qr = 1;
message->opcode = DNS_OPCODE_QUERY;
message->aa = 1;
message->tc = 0;
message->rd = 1;
message->ra = 1;
message->z = 0;
message->b = 0;
message->rcode = DNS_RCODE_NO_ERROR;
//The NBNS response contains 1 answer resource record
message->qdcount = 0;
message->ancount = HTONS(1);
message->nscount = 0;
message->arcount = 0;
//NBNS response message length
length = sizeof(DnsHeader);
//Encode the host name using the NBNS name notation
length += nbnsEncodeName(interface->hostname, (uint8_t *) message + length);
//Point to the corresponding resource record
resourceRecord = DNS_GET_RESOURCE_RECORD(message, length);
//Fill in resource record
resourceRecord->rtype = HTONS(DNS_RR_TYPE_NB);
resourceRecord->rclass = HTONS(DNS_RR_CLASS_IN);
resourceRecord->ttl = HTONL(NBNS_DEFAULT_RESOURCE_RECORD_TTL);
resourceRecord->rdlength = HTONS(sizeof(NbnsAddrEntry));
//Point to the address entry array
addrEntry = (NbnsAddrEntry *) resourceRecord->rdata;
//Fill in address entry
addrEntry->flags = HTONS(NBNS_G_UNIQUE | NBNS_ONT_BNODE);
addrEntry->addr = interface->ipv4Config.addr;
//Update the length of the NBNS response message
length += sizeof(DnsResourceRecord) + sizeof(NbnsAddrEntry);
//Adjust the length of the multi-part buffer
chunkedBufferSetLength(buffer, offset + length);
//Debug message
TRACE_INFO("Sending NBNS message (%" PRIuSIZE " bytes)...\r\n", length);
//Dump message
dnsDumpMessage((DnsHeader *) message, length);
//A response packet is always sent to the source UDP port and
//source IP address of the request packet
error = udpSendDatagramEx(interface, NBNS_PORT, destIpAddr,
destPort, buffer, offset, IPV4_DEFAULT_TTL);
//Free previously allocated memory
chunkedBufferFree(buffer);
//Return status code
return error;
}
error_t dnsSendQuery(DnsCacheEntry *entry)
{
error_t error;
size_t length;
size_t offset;
ChunkedBuffer *buffer;
DnsHeader *message;
DnsQuestion *dnsQuestion;
IpAddr destIpAddr;
#if (IPV4_SUPPORT == ENABLED)
//An IPv4 address is expected?
if(entry->type == HOST_TYPE_IPV4)
{
//Select the relevant DNS server
destIpAddr.length = sizeof(Ipv4Addr);
ipv4GetDnsServer(entry->interface, entry->dnsServerNum, &destIpAddr.ipv4Addr);
//Make sure the IP address is valid
if(destIpAddr.ipv4Addr == IPV4_UNSPECIFIED_ADDR)
return ERROR_NO_DNS_SERVER;
}
else
#endif
#if (IPV6_SUPPORT == ENABLED)
//An IPv6 address is expected?
if(entry->type == HOST_TYPE_IPV6)
{
//Select the relevant DNS server
destIpAddr.length = sizeof(Ipv6Addr);
ipv6GetDnsServer(entry->interface, entry->dnsServerNum, &destIpAddr.ipv6Addr);
//Make sure the IP address is valid
if(ipv6CompAddr(&destIpAddr.ipv6Addr, &IPV6_UNSPECIFIED_ADDR))
return ERROR_NO_DNS_SERVER;
}
else
#endif
//Invalid host type?
{
//Report an error
return ERROR_INVALID_PARAMETER;
}
//Allocate a memory buffer to hold the DNS query message
buffer = udpAllocBuffer(DNS_MESSAGE_MAX_SIZE, &offset);
//Failed to allocate buffer?
if(!buffer) return ERROR_OUT_OF_MEMORY;
//Point to the DNS header
message = chunkedBufferAt(buffer, offset);
//Format DNS query message
message->id = htons(entry->id);
message->qr = 0;
message->opcode = DNS_OPCODE_QUERY;
message->aa = 0;
message->tc = 0;
message->rd = 1;
message->ra = 0;
message->z = 0;
message->rcode = DNS_RCODE_NO_ERROR;
//The DNS query contains one question
message->qdcount = HTONS(1);
message->ancount = 0;
message->nscount = 0;
message->arcount = 0;
//Length of the DNS query message
length = sizeof(DnsHeader);
//Encode the host name using the DNS name notation
length += dnsEncodeName(entry->name, message->questions);
//Point to the corresponding question structure
dnsQuestion = DNS_GET_QUESTION(message, length);
#if (IPV4_SUPPORT == ENABLED)
//An IPv4 address is expected?
if(entry->type == HOST_TYPE_IPV4)
{
//Fill in question structure
dnsQuestion->qtype = HTONS(DNS_RR_TYPE_A);
dnsQuestion->qclass = HTONS(DNS_RR_CLASS_IN);
}
#endif
#if (IPV6_SUPPORT == ENABLED)
//An IPv6 address is expected?
if(entry->type == HOST_TYPE_IPV6)
{
//Fill in question structure
dnsQuestion->qtype = HTONS(DNS_RR_TYPE_AAAA);
dnsQuestion->qclass = HTONS(DNS_RR_CLASS_IN);
}
#endif
//Update the length of the DNS query message
length += sizeof(DnsQuestion);
//Adjust the length of the multi-part buffer
chunkedBufferSetLength(buffer, offset + length);
//Debug message
TRACE_INFO("Sending DNS message (%" PRIuSIZE " bytes)...\r\n", length);
//Dump message
dnsDumpMessage(message, length);
//Send DNS query message
error = udpSendDatagramEx(entry->interface, entry->port,
&destIpAddr, DNS_PORT, buffer, offset, 0);
//Free previously allocated memory
chunkedBufferFree(buffer);
//Return status code
return error;
}
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);
}
}
error_t tcpConnect(Socket *socket)
{
error_t error;
uint_t event;
//Socket already connected?
if(socket->state != TCP_STATE_CLOSED)
return ERROR_ALREADY_CONNECTED;
//The user owns the socket
socket->ownedFlag = TRUE;
//Number of chunks that comprise the TX and the RX buffers
socket->txBuffer.maxChunkCount = arraysize(socket->txBuffer.chunk);
socket->rxBuffer.maxChunkCount = arraysize(socket->rxBuffer.chunk);
//Allocate transmit buffer
error = chunkedBufferSetLength((ChunkedBuffer *) &socket->txBuffer, socket->txBufferSize);
//Allocate receive buffer
if(!error)
error = chunkedBufferSetLength((ChunkedBuffer *) &socket->rxBuffer, socket->rxBufferSize);
//Failed to allocate memory?
if(error)
{
//Free any previously allocated memory
tcpDeleteControlBlock(socket);
//Report an error to the caller
return error;
}
//Default MSS value
socket->mss = min(TCP_DEFAULT_MSS, TCP_MAX_MSS);
//An initial send sequence number is selected
socket->iss = rand();
//Initialize TCP control block
socket->sndUna = socket->iss;
socket->sndNxt = socket->iss + 1;
socket->rcvUser = 0;
socket->rcvWnd = socket->rxBufferSize;
//Default retransmission timeout
socket->rto = TCP_INITIAL_RTO;
//Send a SYN segment
error = tcpSendSegment(socket, TCP_FLAG_SYN, socket->iss, 0, 0, TRUE);
//Failed to send TCP segment?
if(error) return error;
//Switch to the SYN-SENT state
tcpChangeState(socket, TCP_STATE_SYN_SENT);
//Wait for the connection to be established
event = tcpWaitForEvents(socket, SOCKET_EVENT_CONNECTED |
SOCKET_EVENT_CLOSED, socket->timeout);
//Connection successfully established?
if(event == SOCKET_EVENT_CONNECTED)
return NO_ERROR;
//Failed to establish connection?
else if(event == SOCKET_EVENT_CLOSED)
return ERROR_CONNECTION_FAILED;
//Timeout exception?
else
return ERROR_TIMEOUT;
}
Socket *tcpAccept(Socket *socket, IpAddr *clientIpAddr, uint16_t *clientPort)
{
error_t error;
TcpSynQueueItem *queueItem;
Socket *newSocket;
//Ensure the socket was previously placed in the listening state
if(tcpGetState(socket) != TCP_STATE_LISTEN)
return NULL;
//Enter critical section
osMutexAcquire(socketMutex);
//Wait for an connection attempt
while(1)
{
//The SYN queue is empty?
if(!socket->synQueue)
{
//Set the events the application is interested in
socket->eventMask = SOCKET_EVENT_RX_READY;
//Reset the event object
osEventReset(socket->event);
//Leave critical section
osMutexRelease(socketMutex);
//Wait until a SYN message is received from a client
osEventWait(socket->event, socket->timeout);
//Enter critical section
osMutexAcquire(socketMutex);
}
//Check whether the queue is still empty
if(!socket->synQueue)
{
//Leave critical section
osMutexRelease(socketMutex);
//Timeout error
return NULL;
}
//Point to the first item in the receive queue
queueItem = socket->synQueue;
//Return the client IP address and port number
if(clientIpAddr)
*clientIpAddr = queueItem->srcAddr;
if(clientPort)
*clientPort = queueItem->srcPort;
//Leave critical section
osMutexRelease(socketMutex);
//Create a new socket to handle the incoming connection request
newSocket = socketOpen(SOCKET_TYPE_STREAM, SOCKET_PROTOCOL_TCP);
//Failed to open socket?
if(!newSocket)
{
//Debug message
TRACE_WARNING("Cannot accept TCP connection!\r\n");
//Remove the item from the SYN queue
socket->synQueue = queueItem->next;
//Deallocate memory buffer
memPoolFree(queueItem);
//Wait for the next connection attempt
continue;
}
//Enter critical section
osMutexAcquire(socketMutex);
//The user owns the socket
newSocket->ownedFlag = TRUE;
//Number of chunks that comprise the TX and the RX buffers
newSocket->txBuffer.maxChunkCount = arraysize(newSocket->txBuffer.chunk);
newSocket->rxBuffer.maxChunkCount = arraysize(newSocket->rxBuffer.chunk);
//Allocate transmit buffer
error = chunkedBufferSetLength((ChunkedBuffer *) &newSocket->txBuffer, newSocket->txBufferSize);
//Allocate receive buffer
if(!error)
error = chunkedBufferSetLength((ChunkedBuffer *) &newSocket->rxBuffer, newSocket->rxBufferSize);
//Failed to allocate memory?
if(error)
{
//Debug message
TRACE_WARNING("Cannot accept TCP connection!\r\n");
//Properly close the socket
tcpAbort(newSocket);
//Remove the item from the SYN queue
socket->synQueue = queueItem->next;
//Deallocate memory buffer
memPoolFree(queueItem);
//Wait for the next connection attempt
continue;
}
//Bind the newly created socket to the appropriate interface
newSocket->interface = queueItem->interface;
//Bind the socket to the specified address
newSocket->localIpAddr = queueItem->destAddr;
newSocket->localPort = socket->localPort;
//Save the port number and the IP address of the remote host
newSocket->remoteIpAddr = queueItem->srcAddr;
newSocket->remotePort = queueItem->srcPort;
//Save the maximum segment size
newSocket->mss = queueItem->mss;
//Initialize TCP control block
newSocket->iss = rand();
newSocket->irs = queueItem->isn;
newSocket->sndUna = newSocket->iss;
newSocket->sndNxt = newSocket->iss + 1;
newSocket->rcvNxt = newSocket->irs + 1;
newSocket->rcvUser = 0;
newSocket->rcvWnd = newSocket->rxBufferSize;
//Default retransmission timeout
newSocket->rto = TCP_INITIAL_RTO;
//Initial congestion window
newSocket->cwnd = min(TCP_INITIAL_WINDOW * newSocket->mss, newSocket->txBufferSize);
//Slow start threshold should be set arbitrarily high
newSocket->ssthresh = UINT16_MAX;
//Send a SYN ACK control segment
error = tcpSendSegment(newSocket, TCP_FLAG_SYN | TCP_FLAG_ACK,
newSocket->iss, newSocket->rcvNxt, 0, TRUE);
//Failed to send TCP segment?
if(error)
{
//Debug message
TRACE_WARNING("Cannot accept TCP connection!\r\n");
//Close previously created socket
tcpAbort(newSocket);
//Remove the item from the SYN queue
socket->synQueue = queueItem->next;
//Deallocate memory buffer
memPoolFree(queueItem);
//Wait for the next connection attempt
continue;
}
//Remove the item from the SYN queue
socket->synQueue = queueItem->next;
//Deallocate memory buffer
memPoolFree(queueItem);
//Update the state of events
tcpUpdateEvents(socket);
//The connection state should be changed to SYN-RECEIVED
tcpChangeState(newSocket, TCP_STATE_SYN_RECEIVED);
//Leave critical section
osMutexRelease(socketMutex);
//Return a handle to the newly created socket
return newSocket;
}
}
