error_t socketSend(Socket *socket, const void *data,
size_t length, size_t *written, uint_t flags)
{
//Use default remote IP address for connectionless or raw sockets
return socketSendTo(socket, &socket->remoteIpAddr,
socket->remotePort, data, length, written, flags);
}
void udpEchoTask(void *param)
{
error_t error;
uint_t length;
uint16_t port;
IpAddr ipAddr;
EchoServiceContext *context;
//Get a pointer to the context
context = (EchoServiceContext *) param;
//Main loop
while(1)
{
//Wait for an incoming datagram
error = socketReceiveFrom(context->socket, &ipAddr, &port,
context->buffer, ECHO_BUFFER_SIZE, &length, 0);
//Any datagram received?
if(!error)
{
//Debug message
TRACE_INFO("Echo service: %u bytes received from %s port %u\r\n",
length, ipAddrToString(&ipAddr, NULL), port);
//Send the data back to the source
error = socketSendTo(context->socket, &ipAddr, port,
context->buffer, length, NULL, 0);
}
}
}
void udpChargenTask(void *param)
{
error_t error;
size_t i;
size_t k;
size_t n;
size_t length;
uint16_t port;
IpAddr ipAddr;
ChargenServiceContext *context;
//Get a pointer to the context
context = (ChargenServiceContext *) param;
//Main loop
while(1)
{
//Wait for an incoming datagram
error = socketReceiveFrom(context->socket, &ipAddr, &port,
context->buffer, CHARGEN_BUFFER_SIZE, &n, 0);
//Any datagram received?
if(!error)
{
//When a datagram is received, an answering datagram is sent
//containing a random number (between 0 and 512) of characters
length = tcpIpStackGetRand() % 513;
//Reset line counter
n = 0;
//Format output data
for(i = 0; i < length; i += 74)
{
//Calculate the length of the current line
k = min(length - i, 74);
//Copy character pattern
memcpy(context->buffer + i, pattern + n, k);
//End each line with carriage return and line feed
if(k == 74)
{
context->buffer[i + 72] = '\r';
context->buffer[i + 73] = '\n';
}
//Increment line counter
if(++n >= 95) n = 0;
}
//Send data to the remote host
error = socketSendTo(context->socket, &ipAddr, port,
context->buffer, length, &n, 0);
//Debug message
TRACE_INFO("Chargen service: %" PRIuSIZE " bytes sent to %s port %" PRIu16 "\r\n",
n, ipAddrToString(&ipAddr, NULL), port);
}
}
}
int_t sendto(int_t s, const void *data, int_t length,
int_t flags, const sockaddr *addr, int_t addrlen)
{
error_t error;
size_t written;
uint16_t port;
IpAddr ipAddr;
Socket *socket;
//Make sure the socket descriptor is valid
if(s < 0 || s >= SOCKET_MAX_COUNT)
{
socketError(NULL, ERROR_INVALID_SOCKET);
return SOCKET_ERROR;
}
//Point to the socket structure
socket = &socketTable[s];
//Check the length of the address
if(addrlen < sizeof(sockaddr))
{
//Report an error
socketError(socket, ERROR_INVALID_PARAMETER);
return SOCKET_ERROR;
}
#if (IPV4_SUPPORT == ENABLED)
//IPv4 address?
if(addr->sa_family == AF_INET && addrlen == sizeof(sockaddr_in))
{
//Point to the IPv4 address information
sockaddr_in *sa = (sockaddr_in *) addr;
//Get port number
port = ntohs(sa->sin_port);
//Copy IPv4 address
ipAddr.length = sizeof(Ipv4Addr);
ipAddr.ipv4Addr = sa->sin_addr.s_addr;
}
else
#endif
#if (IPV6_SUPPORT == ENABLED)
//IPv6 address?
if(addr->sa_family == AF_INET6 && addrlen == sizeof(sockaddr_in6))
{
//Point to the IPv6 address information
sockaddr_in6 *sa = (sockaddr_in6 *) addr;
//Get port number
port = ntohs(sa->sin6_port);
//Copy IPv6 address
ipAddr.length = sizeof(Ipv6Addr);
ipv6CopyAddr(&ipAddr.ipv6Addr, sa->sin6_addr.s6_addr);
}
else
#endif
//Invalid address?
{
//Report an error
socketError(socket, ERROR_INVALID_PARAMETER);
return SOCKET_ERROR;
}
//Send data
error = socketSendTo(socket, &ipAddr, port, data, length, &written, flags << 8);
//Any error to report?
if(error)
{
socketError(socket, error);
return SOCKET_ERROR;
}
//Return the number of bytes sent
return written;
}
error_t snmpParseMessage(SnmpAgentContext *context, const uint8_t *p, size_t length)
{
error_t error;
int32_t version;
Asn1Tag tag;
//Debug message
TRACE_INFO("SNMP message received from %s port %" PRIu16 " (%" PRIuSIZE " bytes)...\r\n",
ipAddrToString(&context->remoteIpAddr, NULL), context->remotePort, length);
//Display the contents of the SNMP message
TRACE_DEBUG_ARRAY(" ", p, length);
//Dump ASN.1 structure
error = asn1DumpObject(p, length, 0);
//Any error to report?
if(error) return error;
//The SNMP message is encapsulated within a sequence
error = asn1ReadTag(p, length, &tag);
//Failed to decode ASN.1 tag?
if(error) return error;
//Enforce encoding, class and type
error = asn1CheckTag(&tag, TRUE, ASN1_CLASS_UNIVERSAL, ASN1_TYPE_SEQUENCE);
//The tag does not match the criteria?
if(error) return error;
//Point to the first field of the sequence
p = tag.value;
length = tag.length;
//Read version identifier
error = asn1ReadInt32(p, length, &tag, &version);
//Failed to decode ASN.1 tag?
if(error) return error;
//The SNMP agent verifies the version number. If there is a mismatch,
//it discards the datagram and performs no further actions
if(version != context->version)
return ERROR_INVALID_VERSION;
//Point to the next field
p += tag.totalLength;
length -= tag.totalLength;
//Read SNMP community name
error = asn1ReadTag(p, length, &tag);
//Failed to decode ASN.1 tag?
if(error) return error;
//Enforce encoding, class and type
error = asn1CheckTag(&tag, FALSE, ASN1_CLASS_UNIVERSAL, ASN1_TYPE_OCTET_STRING);
//The tag does not match the criteria?
if(error) return error;
//Save community name
context->request.community = tag.value;
context->request.communityLen = tag.length;
//Point to the next field
p += tag.totalLength;
length -= tag.totalLength;
//Read PDU
error = asn1ReadTag(p, length, &tag);
//Failed to decode ASN.1 tag?
if(error) return error;
//Check encoding
if(tag.constructed != TRUE)
return ERROR_WRONG_ENCODING;
//Enforce class
if(tag.class != ASN1_CLASS_CONTEXT_SPECIFIC)
return ERROR_INVALID_CLASS;
//Save PDU type
context->request.pduType = (SnmpPduType) tag.type;
//Process the protocol data unit
switch(context->request.pduType)
{
case SNMP_PDU_GET_REQUEST:
case SNMP_PDU_GET_NEXT_REQUEST:
//Parse GetRequest-PDU or GetNextRequest-PDU
error = snmpParseGetRequestPdu(context, tag.value, tag.length);
break;
case SNMP_PDU_GET_BULK_REQUEST:
//Parse GetBulkRequest-PDU
error = snmpParseGetBulkRequestPdu(context, tag.value, tag.length);
break;
case SNMP_PDU_SET_REQUEST:
//Parse SetRequest-PDU
error = snmpParseSetRequestPdu(context, tag.value, tag.length);
break;
default:
//Invalid PDU type
error = ERROR_INVALID_TYPE;
break;
}
//Failed to parse PDU?
if(error)
return error;
//Format response PDU header
error = snmpWritePduHeader(context);
//Any error to report?
if(error) return error;
//Debug message
TRACE_INFO("SNMP message sent (%" PRIuSIZE " bytes)...\r\n", context->response.messageLen);
//Display the contents of the SNMP message
TRACE_DEBUG_ARRAY(" ", context->response.message, context->response.messageLen);
//Display ASN.1 structure
error = asn1DumpObject(context->response.message, context->response.messageLen, 0);
//Any error to report?
if(error) return error;
//Send SNMP response message
error = socketSendTo(context->socket, &context->remoteIpAddr, context->remotePort,
context->response.message, context->response.messageLen, NULL, 0);
//Return status code
return error;
}
error_t dhcpv6ForwardRelayReplyMessage(Dhcpv6RelayCtx *context)
{
error_t error;
uint_t i;
uint32_t interfaceId;
size_t inputMessageLen;
size_t outputMessageLen;
Dhcpv6RelayMessage *inputMessage;
Dhcpv6Message *outputMessage;
Dhcpv6Option *option;
IpAddr ipAddr;
uint16_t port;
//Point to the buffer where to store the incoming DHCPv6 message
inputMessage = (Dhcpv6RelayMessage *) context->buffer;
//Read incoming message
error = socketReceiveFrom(context->serverSocket, &ipAddr, &port,
inputMessage, DHCPV6_MAX_MSG_SIZE, &inputMessageLen, 0);
//Any error to report?
if(error) return error;
//Debug message
TRACE_INFO("\r\nDHCPv6 message received on network-facing interface %s (%" PRIuSIZE " bytes)...\r\n",
context->serverInterface->name, inputMessageLen);
//Dump the contents of the message for debugging purpose
dhcpv6DumpMessage(inputMessage, inputMessageLen);
//Check the length of the DHCPv6 message
if(inputMessageLen < sizeof(Dhcpv6RelayMessage))
return ERROR_INVALID_MESSAGE;
//Inspect the message type and only forward Relay-Reply messages.
//Other DHCPv6 message types must be silently discarded
if(inputMessage->msgType != DHCPV6_MSG_TYPE_RELAY_REPL)
return ERROR_INVALID_MESSAGE;
//Get the length of the Options field
inputMessageLen -= sizeof(Dhcpv6Message);
//Check whether an Interface ID option is included in the Relay-Reply
option = dhcpv6GetOption(inputMessage->options, inputMessageLen, DHCPV6_OPTION_INTERFACE_ID);
//Failed to retrieve specified option?
if(!option || ntohs(option->length) != sizeof(interfaceId))
return ERROR_INVALID_MESSAGE;
//Read the Interface ID option contents
memcpy(&interfaceId, option->value, sizeof(interfaceId));
//Convert the 32-bit integer from network byte order
interfaceId = ntohl(interfaceId);
//The Relay-Reply message must include a Relay Message option
option = dhcpv6GetOption(inputMessage->options, inputMessageLen, DHCPV6_OPTION_RELAY_MSG);
//Failed to retrieve specified option?
if(!option || ntohs(option->length) < sizeof(Dhcpv6Message))
return ERROR_INVALID_MESSAGE;
//Extract the message from the Relay Message option
outputMessage = (Dhcpv6Message *) option->value;
//Save the length of the message
outputMessageLen = ntohs(option->length);
//Loop through client-facing interfaces
for(i = 0; i < context->clientInterfaceCount; i++)
{
//Check whether the current interface matches the Interface ID option
if(context->clientInterface[i]->id == interfaceId)
{
//Debug message
TRACE_INFO("Forwarding DHCPv6 message on client-facing interface %s (%" PRIuSIZE " bytes)...\r\n",
context->clientInterface[i]->name, outputMessageLen);
//Dump the contents of the message for debugging purpose
dhcpv6DumpMessage(outputMessage, outputMessageLen);
//Copy the peer-address into the destination IP address
ipAddr.length = sizeof(Ipv6Addr);
ipAddr.ipv6Addr = inputMessage->peerAddress;
//Select the relevant port number to use
if(outputMessage->msgType == DHCPV6_MSG_TYPE_RELAY_REPL)
port = DHCPV6_SERVER_PORT;
else
port = DHCPV6_CLIENT_PORT;
//Relay the DHCPv6 message to the client on the link
//identified by the Interface ID option
return socketSendTo(context->clientSocket[i], &ipAddr,
port, outputMessage, outputMessageLen, NULL, 0);
}
}
//Unknown interface identifier...
return ERROR_INVALID_OPTION;
}
error_t dhcpv6ForwardClientMessage(Dhcpv6RelayCtx *context, uint_t index)
{
error_t error;
uint32_t interfaceId;
size_t inputMessageLen;
size_t outputMessageLen;
Dhcpv6RelayMessage *inputMessage;
Dhcpv6RelayMessage *outputMessage;
Dhcpv6Option *option;
IpAddr ipAddr;
//Point to the buffer where to store the incoming DHCPv6 message
inputMessage = (Dhcpv6RelayMessage *) (context->buffer + DHCPV6_RELAY_FORW_OVERHEAD);
//Message that will be forwarded by the DHCPv6 relay agent
outputMessage = (Dhcpv6RelayMessage *) context->buffer;
//Read incoming message
error = socketReceiveFrom(context->clientSocket[index], &ipAddr, NULL, inputMessage,
DHCPV6_MAX_MSG_SIZE - DHCPV6_RELAY_FORW_OVERHEAD, &inputMessageLen, 0);
//Any error to report?
if(error) return error;
//Debug message
TRACE_INFO("\r\nDHCPv6 message received on client-facing interface %s (%" PRIuSIZE " bytes)...\r\n",
context->clientInterface[index]->name, inputMessageLen);
//Dump the contents of the message for debugging purpose
dhcpv6DumpMessage(inputMessage, inputMessageLen);
//The source address must be a valid IPv6 address
if(ipAddr.length != sizeof(Ipv6Addr))
return ERROR_INVALID_ADDRESS;
//Check the length of the DHCPv6 message
if(inputMessageLen < sizeof(Dhcpv6Message))
return ERROR_INVALID_MESSAGE;
//When the relay agent receives a valid message to be relayed,
//it constructs a new Relay-Forward message
outputMessage->msgType = DHCPV6_MSG_TYPE_RELAY_FORW;
//Inspect the message type
switch(inputMessage->msgType)
{
//Message received from a client?
case DHCPV6_MSG_TYPE_SOLICIT:
case DHCPV6_MSG_TYPE_REQUEST:
case DHCPV6_MSG_TYPE_CONFIRM:
case DHCPV6_MSG_TYPE_RENEW:
case DHCPV6_MSG_TYPE_REBIND:
case DHCPV6_MSG_TYPE_RELEASE:
case DHCPV6_MSG_TYPE_DECLINE:
case DHCPV6_MSG_TYPE_INFO_REQUEST:
//If the relay agent received the message to be relayed from a client
//the hop-count in the Relay-Forward message is set to 0
outputMessage->hopCount = 0;
//Continue processing
break;
//Message received from another relay agent?
case DHCPV6_MSG_TYPE_RELAY_FORW:
//If the message received by the relay agent is a Relay-Forward message
//and the hop-count in the message is greater than or equal to 32, the
//relay agent discards the received message
if(inputMessage->hopCount >= DHCPV6_HOP_COUNT_LIMIT)
return ERROR_INVALID_MESSAGE;
//Set the hop-count field to the value of the hop-count field in
//the received message incremented by 1
outputMessage->hopCount = inputMessage->hopCount + 1;
//Continue processing
break;
//Message received from a server?
default:
//Discard ADVERTISE, REPLY, RECONFIGURE and RELAY-REPL messages
return ERROR_INVALID_MESSAGE;
}
//Set the link-address field to the unspecified address
outputMessage->linkAddress = IPV6_UNSPECIFIED_ADDR;
//Copy the source address from the header of the IP datagram in
//which the message was received to the peer-address field
outputMessage->peerAddress = ipAddr.ipv6Addr;
//Size of the Relay-Forward message
outputMessageLen = sizeof(Dhcpv6RelayMessage);
//Get the interface identifier
interfaceId = context->clientInterface[index]->id;
//Convert the 32-bit integer to network byte order
interfaceId = htonl(interfaceId);
//If the relay agent cannot use the address in the link-address field
//to identify the interface through which the response to the client
//will be relayed, the relay agent must include an Interface ID option
dhcpv6AddOption(outputMessage, &outputMessageLen,
DHCPV6_OPTION_INTERFACE_ID, &interfaceId, sizeof(interfaceId));
//Copy the received DHCPv6 message into a Relay Message option
option = dhcpv6AddOption(outputMessage, &outputMessageLen,
DHCPV6_OPTION_RELAY_MSG, NULL, 0);
//Set the appropriate length of the option
option->length = htons(inputMessageLen);
//Adjust the length of the Relay-Forward message
outputMessageLen += inputMessageLen;
//Debug message
TRACE_INFO("Forwarding DHCPv6 message on network-facing interface %s (%" PRIuSIZE " bytes)...\r\n",
context->serverInterface->name, outputMessageLen);
//Dump the contents of the message for debugging purpose
dhcpv6DumpMessage(outputMessage, outputMessageLen);
//Destination address to be used when relaying the client message
ipAddr.length = sizeof(Ipv6Addr);
ipAddr.ipv6Addr = context->serverAddress;
//Relay the client message to the server
return socketSendTo(context->serverSocket, &ipAddr,
DHCPV6_SERVER_PORT, outputMessage, outputMessageLen, NULL, 0);
}