error_t rawSocketSendIpPacket(Socket *socket, const IpAddr *destIpAddr,
const void *data, size_t length, size_t *written)
{
error_t error;
size_t offset;
uint_t timeToLive;
NetBuffer *buffer;
NetInterface *interface;
IpPseudoHeader pseudoHeader;
//The socket may be bound to a particular network interface
interface = socket->interface;
//Allocate a buffer memory to hold the raw IP datagram
buffer = ipAllocBuffer(0, &offset);
//Failed to allocate memory?
if(!buffer) return ERROR_OUT_OF_MEMORY;
//Start of exception handling block
do
{
//Copy the raw data
error = netBufferAppend(buffer, data, length);
//Any error to report?
if(error) break;
#if (IPV4_SUPPORT == ENABLED)
//Destination address is an IPv4 address?
if(destIpAddr->length == sizeof(Ipv4Addr))
{
Ipv4Addr srcIpAddr;
//Select the source IPv4 address and the relevant network interface
//to use when sending data to the specified destination host
error = ipv4SelectSourceAddr(&interface, destIpAddr->ipv4Addr, &srcIpAddr);
//Any error to report?
if(error) break;
//Format IPv4 pseudo header
pseudoHeader.length = sizeof(Ipv4PseudoHeader);
pseudoHeader.ipv4Data.srcAddr = srcIpAddr;
pseudoHeader.ipv4Data.destAddr = destIpAddr->ipv4Addr;
pseudoHeader.ipv4Data.reserved = 0;
pseudoHeader.ipv4Data.protocol = socket->protocol;
pseudoHeader.ipv4Data.length = htons(length);
//Set TTL value
timeToLive = IPV4_DEFAULT_TTL;
}
else
#endif
#if (IPV6_SUPPORT == ENABLED)
//Destination address is an IPv6 address?
if(destIpAddr->length == sizeof(Ipv6Addr))
{
//Select the source IPv6 address and the relevant network interface
//to use when sending data to the specified destination host
error = ipv6SelectSourceAddr(&interface,
&destIpAddr->ipv6Addr, &pseudoHeader.ipv6Data.srcAddr);
//Any error to report?
if(error) break;
//Format IPv6 pseudo header
pseudoHeader.length = sizeof(Ipv6PseudoHeader);
pseudoHeader.ipv6Data.destAddr = destIpAddr->ipv6Addr;
pseudoHeader.ipv6Data.length = htonl(length);
pseudoHeader.ipv6Data.reserved = 0;
pseudoHeader.ipv6Data.nextHeader = socket->protocol;
//Set Hop Limit value
timeToLive = IPV6_DEFAULT_HOP_LIMIT;
}
else
#endif
//Invalid destination address?
{
//An internal error has occurred
error = ERROR_FAILURE;
//Exit immediately
break;
}
//Send raw IP datagram
error = ipSendDatagram(interface, &pseudoHeader, buffer, offset, timeToLive);
//Failed to send data?
if(error) break;
//Total number of bytes successfully transmitted
if(written != NULL)
*written = length;
//End of exception handling block
} while(0);
//Free previously allocated memory block
netBufferFree(buffer);
//Return status code
return error;
}
error_t udpSendDatagramEx(NetInterface *interface, uint16_t srcPort, const IpAddr *destIpAddr,
uint16_t destPort, NetBuffer *buffer, size_t offset, uint8_t ttl)
{
error_t error;
size_t length;
UdpHeader *header;
IpPseudoHeader pseudoHeader;
//Make room for the UDP header
offset -= sizeof(UdpHeader);
//Retrieve the length of the datagram
length = netBufferGetLength(buffer) - offset;
//Point to the UDP header
header = netBufferAt(buffer, offset);
//Sanity check
if(!header) return ERROR_FAILURE;
//Format UDP header
header->srcPort = htons(srcPort);
header->destPort = htons(destPort);
header->length = htons(length);
header->checksum = 0;
#if (IPV4_SUPPORT == ENABLED)
//Destination address is an IPv4 address?
if(destIpAddr->length == sizeof(Ipv4Addr))
{
Ipv4Addr srcIpAddr;
//Select the source IPv4 address and the relevant network interface
//to use when sending data to the specified destination host
error = ipv4SelectSourceAddr(&interface, destIpAddr->ipv4Addr, &srcIpAddr);
//Any error to report?
if(error) return error;
//Format IPv4 pseudo header
pseudoHeader.length = sizeof(Ipv4PseudoHeader);
pseudoHeader.ipv4Data.srcAddr = srcIpAddr;
pseudoHeader.ipv4Data.destAddr = destIpAddr->ipv4Addr;
pseudoHeader.ipv4Data.reserved = 0;
pseudoHeader.ipv4Data.protocol = IPV4_PROTOCOL_UDP;
pseudoHeader.ipv4Data.length = htons(length);
//Calculate UDP header checksum
header->checksum = ipCalcUpperLayerChecksumEx(&pseudoHeader.ipv4Data,
sizeof(Ipv4PseudoHeader), buffer, offset, length);
}
else
#endif
#if (IPV6_SUPPORT == ENABLED)
//Destination address is an IPv6 address?
if(destIpAddr->length == sizeof(Ipv6Addr))
{
//Select the source IPv6 address and the relevant network interface
//to use when sending data to the specified destination host
error = ipv6SelectSourceAddr(&interface,
&destIpAddr->ipv6Addr, &pseudoHeader.ipv6Data.srcAddr);
//Any error to report?
if(error) return error;
//Format IPv6 pseudo header
pseudoHeader.length = sizeof(Ipv6PseudoHeader);
pseudoHeader.ipv6Data.destAddr = destIpAddr->ipv6Addr;
pseudoHeader.ipv6Data.length = htonl(length);
pseudoHeader.ipv6Data.reserved = 0;
pseudoHeader.ipv6Data.nextHeader = IPV6_UDP_HEADER;
//Calculate UDP header checksum
header->checksum = ipCalcUpperLayerChecksumEx(&pseudoHeader.ipv6Data,
sizeof(Ipv6PseudoHeader), buffer, offset, length);
}
else
#endif
//Invalid destination address?
{
//An internal error has occurred
return ERROR_FAILURE;
}
//Debug message
TRACE_INFO("Sending UDP datagram (%" PRIuSIZE " bytes)\r\n", length);
//Dump UDP header contents for debugging purpose
udpDumpHeader(header);
//Send UDP datagram
return ipSendDatagram(interface, &pseudoHeader, buffer, offset, ttl);
}
error_t ping(NetInterface *interface, const IpAddr *ipAddr, time_t timeout, time_t *rtt)
{
error_t error;
uint_t i;
size_t length;
uint16_t identifier;
uint16_t sequenceNumber;
time_t startTime;
time_t roundTripTime;
Socket *socket;
IcmpEchoMessage *message;
//Debug message
TRACE_INFO("Pinging %s with 64 bytes of data...\r\n", ipAddrToString(ipAddr, NULL));
//Length of the complete ICMP message including header and data
length = sizeof(IcmpEchoMessage) + PING_DATA_SIZE;
//Allocate memory buffer to hold an ICMP message
message = osMemAlloc(length);
//Failed to allocate memory?
if(!message) return ERROR_OUT_OF_MEMORY;
//Identifier field is used to help matching requests and replies
identifier = rand();
//Sequence Number field is increment each time an Echo Request is sent
sequenceNumber = osAtomicInc16(&pingSequenceNumber);
//Format ICMP Echo Request message
message->type = ICMP_TYPE_ECHO_REQUEST;
message->code = 0;
message->checksum = 0;
message->identifier = identifier;
message->sequenceNumber = sequenceNumber;
//Copy data
for(i = 0; i < PING_DATA_SIZE; i++)
message->data[i] = i;
#if (IPV4_SUPPORT == ENABLED)
//Target address is an IPv4 address?
if(ipAddr->length == sizeof(Ipv4Addr))
{
Ipv4Addr srcIpAddr;
//Select the source IPv4 address and the relevant network
//interface to use when pinging the specified host
error = ipv4SelectSourceAddr(&interface, ipAddr->ipv4Addr, &srcIpAddr);
//Any error to report?
if(error)
{
//Free previously allocated memory
osMemFree(message);
//Return the corresponding error code
return error;
}
//ICMP Echo Request message
message->type = ICMP_TYPE_ECHO_REQUEST;
//Message checksum calculation
message->checksum = ipCalcChecksum(message, length);
//Open a raw socket
socket = socketOpen(SOCKET_TYPE_RAW, SOCKET_PROTOCOL_ICMP);
}
else
#endif
#if (IPV6_SUPPORT == ENABLED)
//Target address is an IPv6 address?
if(ipAddr->length == sizeof(Ipv6Addr))
{
Ipv6PseudoHeader pseudoHeader;
//Select the source IPv6 address and the relevant network
//interface to use when pinging the specified host
error = ipv6SelectSourceAddr(&interface, &ipAddr->ipv6Addr, &pseudoHeader.srcAddr);
//Any error to report?
if(error)
{
//Free previously allocated memory
osMemFree(message);
//Return the corresponding error code
return error;
}
//ICMPv6 Echo Request message
message->type = ICMPV6_TYPE_ECHO_REQUEST;
//Format IPv6 pseudo header
pseudoHeader.destAddr = ipAddr->ipv6Addr;
pseudoHeader.length = htonl(length);
pseudoHeader.reserved = 0;
pseudoHeader.nextHeader = IPV6_ICMPV6_HEADER;
//Message checksum calculation
message->checksum = ipCalcUpperLayerChecksum(
&pseudoHeader, sizeof(Ipv6PseudoHeader), message, length);
//Open a raw socket
socket = socketOpen(SOCKET_TYPE_RAW, SOCKET_PROTOCOL_ICMPV6);
}
else
#endif
//Target address is not valid?
{
//Free previously allocated memory
osMemFree(message);
//Report an error
return ERROR_INVALID_ADDRESS;
}
//Failed to open socket?
if(!socket)
{
//Free previously allocated memory
osMemFree(message);
//Report an error
return ERROR_OPEN_FAILED;
}
//Associate the newly created socket with the relevant interface
error = socketBindToInterface(socket, interface);
//Unable to bind the socket to the desired interface?
if(error)
{
//Free previously allocated memory
osMemFree(message);
//Close socket
socketClose(socket);
//Return status code
return error;
}
//Connect the socket to the target host
error = socketConnect(socket, ipAddr, 0);
//Any error to report?
if(error)
{
//Free previously allocated memory
osMemFree(message);
//Close socket
socketClose(socket);
//Return status code
return error;
}
//Send Echo Request message
error = socketSend(socket, message, length, NULL, 0);
//Failed to send message ?
if(error)
{
//Free previously allocated memory
osMemFree(message);
//Close socket
socketClose(socket);
//Return status code
return error;
}
//Save the time at which the request was sent
startTime = osGetTickCount();
//Timeout value exceeded?
while((osGetTickCount() - startTime) < timeout)
{
//Adjust receive timeout
error = socketSetTimeout(socket, timeout);
//Any error to report?
if(error) break;
//Wait for an incoming ICMP message
error = socketReceive(socket, message,
sizeof(IcmpEchoMessage) + PING_DATA_SIZE, &length, 0);
//Any error to report?
if(error) break;
//Check message length
if(length != (sizeof(IcmpEchoMessage) + PING_DATA_SIZE))
continue;
//Verify message type
if(ipAddr->length == sizeof(Ipv4Addr) && message->type != ICMP_TYPE_ECHO_REPLY)
continue;
if(ipAddr->length == sizeof(Ipv6Addr) && message->type != ICMPV6_TYPE_ECHO_REPLY)
continue;
//Response identifier matches request identifier?
if(message->identifier != identifier)
continue;
//Make sure the sequence number is correct
if(message->sequenceNumber != sequenceNumber)
continue;
//Loop through data field
for(i = 0; i < PING_DATA_SIZE; i++)
{
//Compare received data against expected data
if(message->data[i] != i) break;
}
//Valid Echo Reply message received?
if(i == PING_DATA_SIZE)
{
//Calculate round-trip time
roundTripTime = osGetTickCount() - startTime;
//Debug message
TRACE_INFO("Echo received (round-trip time = %ums)...\r\n", roundTripTime);
//Free previously allocated memory
osMemFree(message);
//Close socket
socketClose(socket);
//Return round-trip time
if(rtt) *rtt = roundTripTime;
//No error to report
return NO_ERROR;
}
}
//Debug message
TRACE_INFO("No echo received!\r\n");
//Free previously allocated memory
osMemFree(message);
//Close socket
socketClose(socket);
//No Echo Reply received from host...
return ERROR_NO_RESPONSE;
}
