error_t udpProcessDatagram(NetInterface *interface, IpPseudoHeader *pseudoHeader, const NetBuffer *buffer, size_t offset) { error_t error; uint_t i; size_t length; UdpHeader *header; Socket *socket; SocketQueueItem *queueItem; NetBuffer *p; //Retrieve the length of the UDP datagram length = netBufferGetLength(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 = netBufferAt(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 osAcquireMutex(&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 osReleaseMutex(&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 = netBufferAlloc(sizeof(SocketQueueItem) + length); //Successful memory allocation? if(p != NULL) { //Point to the newly created item queueItem = netBufferAt(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 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 = netBufferAlloc(sizeof(SocketQueueItem) + length); //Successful memory allocation? if(p != NULL) { //Add the newly created item to the queue queueItem->next = netBufferAt(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; //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 netBufferCopy(queueItem->buffer, queueItem->offset, buffer, offset, length); //Notify user that data is available udpUpdateEvents(socket); //Leave critical section osReleaseMutex(&socketMutex); //Successful processing return NO_ERROR; }
error_t rawSocketSendEthPacket(Socket *socket, const void *data, size_t length, size_t *written) { error_t error; uint32_t crc; NetBuffer *buffer; NetInterface *interface; //Select the relevant network interface if(!socket->interface) interface = netGetDefaultInterface(); else interface = socket->interface; //Allocate a buffer memory to hold the raw Ethernet packet buffer = netBufferAlloc(0); //Failed to allocate buffer? if(!buffer) return ERROR_OUT_OF_MEMORY; //Copy the raw data error = netBufferAppend(buffer, data, length); //Successful processing? if(!error) { //Automatic padding not supported by hardware? if(!interface->nicDriver->autoPadding) { //The host controller should manually add padding //to the packet before transmitting it if(length < (ETH_MIN_FRAME_SIZE - ETH_CRC_SIZE)) { //Add padding as necessary size_t n = (ETH_MIN_FRAME_SIZE - ETH_CRC_SIZE) - length; //Append padding bytes error = netBufferAppend(buffer, ethPadding, n); //Any error to report? if(error) return error; //Adjust frame length length += n; } } //CRC generation not supported by hardware? if(!interface->nicDriver->autoCrcGen) { //Compute CRC over the header and payload crc = ethCalcCrcEx(buffer, 0, length); //Convert from host byte order to little-endian byte order crc = htole32(crc); //Append the calculated CRC value error = netBufferAppend(buffer, &crc, sizeof(crc)); //Any error to report? if(error) return error; //Adjust frame length length += sizeof(crc); } //Debug message TRACE_DEBUG("Sending raw Ethernet frame (%" PRIuSIZE " bytes)...\r\n", length); //Send the resulting packet over the specified link error = nicSendPacket(interface, buffer, 0); } //Successful processing? if(!error) { //Total number of bytes successfully transmitted if(written != NULL) *written = length; } //Free previously allocated memory block netBufferFree(buffer); //Return status code return error; }
error_t rawSocketProcessIpPacket(NetInterface *interface, IpPseudoHeader *pseudoHeader, const NetBuffer *buffer, size_t offset) { uint_t i; size_t length; Socket *socket; SocketQueueItem *queueItem; NetBuffer *p; //Retrieve the length of the raw IP packet length = netBufferGetLength(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 = netBufferAlloc(sizeof(SocketQueueItem) + length); //Successful memory allocation? if(p != NULL) { //Point to the newly created item queueItem = netBufferAt(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 = netBufferAlloc(sizeof(SocketQueueItem) + length); //Successful memory allocation? if(p != NULL) { //Add the newly created item to the queue queueItem->next = netBufferAt(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 netBufferCopy(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; }
void rawSocketProcessEthPacket(NetInterface *interface, EthHeader *ethFrame, size_t length) { uint_t i; Socket *socket; SocketQueueItem *queueItem; NetBuffer *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 = netBufferAlloc(sizeof(SocketQueueItem) + length); //Successful memory allocation? if(p != NULL) { //Point to the newly created item queueItem = netBufferAt(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 = netBufferAlloc(sizeof(SocketQueueItem) + length); //Successful memory allocation? if(p != NULL) { //Add the newly created item to the queue queueItem->next = netBufferAt(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 netBufferWrite(queueItem->buffer, queueItem->offset, ethFrame, length); //Notify user that data is available rawSocketUpdateEvents(socket); //Leave critical section osReleaseMutex(&socketMutex); }