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 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; }