+ // Locate the port\r
+ //\r
+ Index = *pIndex;\r
+ if ( -1 != Index ) {\r
+ pTftpPort = &pTftpServer->TftpPort[ *pIndex ];\r
+\r
+ //\r
+ // Handle input events\r
+ //\r
+ revents = pTftpPort->revents;\r
+ pTftpPort->revents = 0;\r
+ if ( 0 != ( revents & POLLRDNORM )) {\r
+ //\r
+ // Receive the message from the remote system\r
+ //\r
+ RemoteAddressLength = sizeof ( pTftpServer->RemoteAddress );\r
+ pTftpServer->RxBytes = recvfrom ( pTftpPort->fd,\r
+ &pTftpServer->RxBuffer[ 0 ],\r
+ sizeof ( pTftpServer->RxBuffer ),\r
+ 0,\r
+ (struct sockaddr *) &pTftpServer->RemoteAddress,\r
+ &RemoteAddressLength );\r
+ if ( -1 != pTftpServer->RxBytes ) {\r
+ if ( PcdGetBool ( Tftp_HighSpeed )) {\r
+ pTftpServer->RxTime = GetPerformanceCounter ( );\r
+ }\r
+ if ( AF_INET == pTftpServer->RemoteAddress.v4.sin_family ) {\r
+ DEBUG (( DEBUG_TFTP_PORT,\r
+ "Received %d bytes from %d.%d.%d.%d:%d\r\n",\r
+ pTftpServer->RxBytes,\r
+ pTftpServer->RemoteAddress.v4.sin_addr.s_addr & 0xff,\r
+ ( pTftpServer->RemoteAddress.v4.sin_addr.s_addr >> 8 ) & 0xff,\r
+ ( pTftpServer->RemoteAddress.v4.sin_addr.s_addr >> 16 ) & 0xff,\r
+ ( pTftpServer->RemoteAddress.v4.sin_addr.s_addr >> 24 ) & 0xff,\r
+ htons ( pTftpServer->RemoteAddress.v4.sin_port )));\r
+ }\r
+ else {\r
+ DEBUG (( DEBUG_TFTP_PORT,\r
+ "Received %d bytes from [%02x%02x:%02x%02x:%02x%02x:%02x%02x:%02x%02x:%02x%02x:%02x%02x:%02x%02x]:%d\r\n",\r
+ pTftpServer->RxBytes,\r
+ pTftpServer->RemoteAddress.v6.sin6_addr.__u6_addr.__u6_addr8[ 0 ],\r
+ pTftpServer->RemoteAddress.v6.sin6_addr.__u6_addr.__u6_addr8[ 1 ],\r
+ pTftpServer->RemoteAddress.v6.sin6_addr.__u6_addr.__u6_addr8[ 2 ],\r
+ pTftpServer->RemoteAddress.v6.sin6_addr.__u6_addr.__u6_addr8[ 3 ],\r
+ pTftpServer->RemoteAddress.v6.sin6_addr.__u6_addr.__u6_addr8[ 4 ],\r
+ pTftpServer->RemoteAddress.v6.sin6_addr.__u6_addr.__u6_addr8[ 5 ],\r
+ pTftpServer->RemoteAddress.v6.sin6_addr.__u6_addr.__u6_addr8[ 6 ],\r
+ pTftpServer->RemoteAddress.v6.sin6_addr.__u6_addr.__u6_addr8[ 7 ],\r
+ pTftpServer->RemoteAddress.v6.sin6_addr.__u6_addr.__u6_addr8[ 8 ],\r
+ pTftpServer->RemoteAddress.v6.sin6_addr.__u6_addr.__u6_addr8[ 9 ],\r
+ pTftpServer->RemoteAddress.v6.sin6_addr.__u6_addr.__u6_addr8[ 10 ],\r
+ pTftpServer->RemoteAddress.v6.sin6_addr.__u6_addr.__u6_addr8[ 11 ],\r
+ pTftpServer->RemoteAddress.v6.sin6_addr.__u6_addr.__u6_addr8[ 12 ],\r
+ pTftpServer->RemoteAddress.v6.sin6_addr.__u6_addr.__u6_addr8[ 13 ],\r
+ pTftpServer->RemoteAddress.v6.sin6_addr.__u6_addr.__u6_addr8[ 14 ],\r
+ pTftpServer->RemoteAddress.v6.sin6_addr.__u6_addr.__u6_addr8[ 15 ],\r
+ htons ( pTftpServer->RemoteAddress.v6.sin6_port )));\r
+ }\r
+\r
+ //\r
+ // Lookup connection context using the remote system address and port\r
+ // to determine if an existing connection to this remote\r
+ // system exists\r
+ //\r
+ pContext = ContextFind ( pTftpServer );\r
+\r
+ //\r
+ // Process the received message\r
+ //\r
+ TftpProcessRequest ( pTftpServer, pContext, pTftpPort->fd );\r
+ }\r
+ else {\r
+ //\r
+ // Receive error on the TFTP server port\r
+ // Close the server socket\r
+ //\r
+ DEBUG (( DEBUG_ERROR,\r
+ "ERROR - Failed receive on TFTP server port, errno: 0x%08x\r\n",\r
+ errno ));\r
+ revents |= POLLHUP;\r
+ }\r
+ }\r
+\r
+ //\r
+ // Handle the close event\r
+ //\r
+ if ( 0 != ( revents & POLLHUP )) {\r
+ //\r
+ // Close the port\r
+ //\r
+ close ( pTftpPort->fd );\r
+ pTftpPort->fd = -1;\r
+ *pIndex = -1;\r
+ pTftpServer->Entries -= 1;\r
+ ASSERT ( 0 <= pTftpServer->Entries );\r
+ }\r
+ }\r
+\r
+ DBG_EXIT ( );\r
+}\r
+\r
+\r
+/**\r
+ Build and send an error packet\r
+\r
+ @param [in] pContext Address of a ::TSDT_CONNECTION_CONTEXT structure\r
+ @param [in] Error Error number for the packet\r
+ @param [in] pError Zero terminated error string address\r
+\r
+ @retval EFI_SUCCESS Message processed successfully\r
+\r
+**/\r
+EFI_STATUS\r
+SendError (\r
+ IN TSDT_CONNECTION_CONTEXT * pContext,\r
+ IN UINT16 Error,\r
+ IN UINT8 * pError\r
+ )\r
+{\r
+ UINT8 Character;\r
+ UINT8 * pBuffer;\r
+ TFTP_PACKET * pPacket;\r
+ EFI_STATUS Status;\r
+\r
+ DBG_ENTER ( );\r
+\r
+ //\r
+ // Build the error packet\r
+ //\r
+ pPacket = &pContext->ErrorPacket;\r
+ pBuffer = &pPacket->TxBuffer[ 0 ];\r
+ pBuffer[ 0 ] = 0;\r
+ pBuffer[ 1 ] = TFTP_OP_ERROR;\r
+ pBuffer[ 2 ] = (UINT8)( Error >> 8 );\r
+ pBuffer[ 3 ] = (UINT8)Error;\r
+\r
+ //\r
+ // Copy the zero terminated string into the buffer\r
+ //\r
+ pBuffer += 4;\r
+ do {\r
+ Character = *pError++;\r
+ *pBuffer++ = Character;\r
+ } while ( 0 != Character );\r
+\r
+ //\r
+ // Send the error message\r
+ //\r
+ pPacket->TxBytes = pBuffer - &pPacket->TxBuffer[ 0 ];\r
+ Status = PacketTx ( pContext, pPacket );\r
+\r
+ //\r
+ // Return the operation status\r
+ //\r
+ DBG_EXIT_STATUS ( Status );\r
+ return Status;\r
+}\r
+\r
+\r
+/**\r
+ Scan the list of sockets and process any pending work\r
+\r
+ @param [in] pTftpServer Address of the ::TSDT_TFTP_SERVER structure\r
+\r
+**/\r
+VOID\r
+SocketPoll (\r
+ IN TSDT_TFTP_SERVER * pTftpServer\r
+ )\r
+{\r
+ int FDCount;\r
+\r
+ DEBUG (( DEBUG_SOCKET_POLL, "Entering SocketPoll\r\n" ));\r
+\r
+ //\r
+ // Determine if any ports are active\r
+ //\r
+ if ( 0 != pTftpServer->Entries ) {\r
+ FDCount = poll ( &pTftpServer->TftpPort[ 0 ],\r
+ pTftpServer->Entries,\r
+ CLIENT_POLL_DELAY );\r
+ if ( 0 < FDCount ) {\r
+ //\r
+ // Process this port\r
+ //\r
+ PortWork ( pTftpServer, &pTftpServer->Udpv4Index );\r
+ PortWork ( pTftpServer, &pTftpServer->Udpv6Index );\r
+ }\r
+ }\r
+\r
+ DEBUG (( DEBUG_SOCKET_POLL, "Exiting SocketPoll\r\n" ));\r
+}\r
+\r
+\r
+/**\r
+ Process the ACK\r
+\r
+ @param [in] pTftpServer Address of the ::TSDT_TFTP_SERVER structure\r
+ @param [in] pContext Connection context structure address\r
+\r
+ @retval TRUE if the context should be closed\r
+\r
+**/\r
+BOOLEAN\r
+TftpAck (\r
+ IN TSDT_TFTP_SERVER * pTftpServer,\r
+ IN TSDT_CONNECTION_CONTEXT * pContext\r
+ )\r
+{\r
+ INTN AckNumber;\r
+ BOOLEAN bCloseContext;\r
+ UINT16 BlockNumber;\r
+ UINT8 * pBuffer;\r
+ TFTP_PACKET * pPacket;\r
+ EFI_STATUS Status;\r
+\r
+ DBG_ENTER ( );\r
+\r
+ //\r
+ // Use break instead of goto\r
+ //\r
+ bCloseContext = FALSE;\r
+ for ( ; ; ) {\r
+ //\r
+ // Validate the parameters\r
+ //\r
+ if ( NULL == pContext ) {\r
+ if ( AF_INET == pTftpServer->RemoteAddress.v4.sin_family ) {\r
+ DEBUG (( DEBUG_ERROR,\r
+ "ERROR - File not open for %d.%d.%d.%d:%d\r\n",\r
+ (UINT8)pTftpServer->RemoteAddress.v4.sin_addr.s_addr,\r
+ (UINT8)( pTftpServer->RemoteAddress.v4.sin_addr.s_addr >> 8 ),\r
+ (UINT8)( pTftpServer->RemoteAddress.v4.sin_addr.s_addr >> 16 ),\r
+ (UINT8)( pTftpServer->RemoteAddress.v4.sin_addr.s_addr >> 24 ),\r
+ htons ( pTftpServer->RemoteAddress.v4.sin_port )));\r
+ }\r
+ else {\r
+ DEBUG (( DEBUG_ERROR,\r
+ "ERROR - File not open for [%02x%02x:%02x%02x:%02x%02x:%02x%02x:%02x%02x:%02x%02x:%02x%02x:%02x%02x]:%d\r\n",\r
+ pTftpServer->RemoteAddress.v6.sin6_addr.__u6_addr.__u6_addr8[ 0 ],\r
+ pTftpServer->RemoteAddress.v6.sin6_addr.__u6_addr.__u6_addr8[ 1 ],\r
+ pTftpServer->RemoteAddress.v6.sin6_addr.__u6_addr.__u6_addr8[ 2 ],\r
+ pTftpServer->RemoteAddress.v6.sin6_addr.__u6_addr.__u6_addr8[ 3 ],\r
+ pTftpServer->RemoteAddress.v6.sin6_addr.__u6_addr.__u6_addr8[ 4 ],\r
+ pTftpServer->RemoteAddress.v6.sin6_addr.__u6_addr.__u6_addr8[ 5 ],\r
+ pTftpServer->RemoteAddress.v6.sin6_addr.__u6_addr.__u6_addr8[ 6 ],\r
+ pTftpServer->RemoteAddress.v6.sin6_addr.__u6_addr.__u6_addr8[ 7 ],\r
+ pTftpServer->RemoteAddress.v6.sin6_addr.__u6_addr.__u6_addr8[ 8 ],\r
+ pTftpServer->RemoteAddress.v6.sin6_addr.__u6_addr.__u6_addr8[ 9 ],\r
+ pTftpServer->RemoteAddress.v6.sin6_addr.__u6_addr.__u6_addr8[ 10 ],\r
+ pTftpServer->RemoteAddress.v6.sin6_addr.__u6_addr.__u6_addr8[ 11 ],\r
+ pTftpServer->RemoteAddress.v6.sin6_addr.__u6_addr.__u6_addr8[ 12 ],\r
+ pTftpServer->RemoteAddress.v6.sin6_addr.__u6_addr.__u6_addr8[ 13 ],\r
+ pTftpServer->RemoteAddress.v6.sin6_addr.__u6_addr.__u6_addr8[ 14 ],\r
+ pTftpServer->RemoteAddress.v6.sin6_addr.__u6_addr.__u6_addr8[ 15 ],\r
+ htons ( pTftpServer->RemoteAddress.v6.sin6_port )));\r
+ }\r
+ break;\r
+ }\r
+\r
+ //\r
+ // Verify that the ACK was expected\r
+ //\r
+ pPacket = pContext->pTxHead;\r
+ if ( NULL == pPacket ) {\r
+ //\r
+ // ACK not expected!\r
+ //\r
+ DEBUG (( DEBUG_ERROR,\r
+ "ERROR - Expecting data not ACKs for pContext 0x%08x\r\n",\r
+ pContext ));\r
+ break;\r
+ }\r
+\r
+ //\r
+ // Get the ACKed block number\r
+ //\r
+ pBuffer = &pTftpServer->RxBuffer[ 0 ];\r
+ BlockNumber = HTONS ( *(UINT16 *)&pBuffer[ 2 ]);\r
+\r
+ //\r
+ // Determine if this is the correct ACK\r
+ //\r
+ DEBUG (( DEBUG_TFTP_ACK,\r
+ "ACK for block 0x%04x received\r\n",\r
+ BlockNumber ));\r
+ AckNumber = BlockNumber - pPacket->BlockNumber;\r
+ if (( 0 > AckNumber ) || ( AckNumber >= (INTN)pContext->PacketsInWindow )){\r
+ DEBUG (( DEBUG_WARN | DEBUG_TFTP_ACK,\r
+ "WARNING - Expecting ACK 0x%0x4 not received ACK 0x%08x\r\n",\r
+ pPacket->BlockNumber,\r
+ BlockNumber ));\r
+ break;\r
+ }\r
+\r
+ //\r
+ // Release the ACKed packets\r
+ //\r
+ do {\r
+ //\r
+ // Remove the packet from the transmit list and window\r
+ //\r
+ pPacket = PacketRemove ( pContext );\r
+\r
+ //\r
+ // Get the block number of this packet\r
+ //\r
+ AckNumber = pPacket->BlockNumber;\r
+\r
+ //\r
+ // Increase the size of the transmit window\r
+ //\r
+ if ( PcdGetBool ( Tftp_HighSpeed )\r
+ && ( AckNumber == BlockNumber )) {\r
+ WindowAck ( pTftpServer, pContext, pPacket );\r
+ }\r
+\r
+ //\r
+ // Free this packet\r
+ //\r
+ PacketFree ( pContext, pPacket );\r
+ } while (( NULL != pContext->pTxHead ) && ( AckNumber != BlockNumber ));\r
+\r
+ //\r
+ // Fill the window with packets\r
+ //\r
+ pPacket = pContext->pTxHead;\r
+ while (( NULL != pPacket )\r
+ && ( pContext->PacketsInWindow < pContext->WindowSize )\r
+ && ( !bCloseContext )) {\r
+ Status = PacketTx ( pContext, pPacket );\r
+ bCloseContext = (BOOLEAN)( EFI_ERROR ( Status ));\r
+ pPacket = pPacket->pNext;\r
+ }\r
+ \r
+ //\r
+ // Get more packets ready for transmission\r
+ //\r
+ PacketFill ( pContext );\r
+\r
+ //\r
+ // Close the context when the last packet is ACKed\r
+ //\r
+ if ( 0 == pContext->PacketsInWindow ) {\r
+ bCloseContext = TRUE;\r
+\r
+ //\r
+ // Display the bandwidth\r
+ //\r
+ if ( PcdGetBool ( Tftp_Bandwidth )) {\r
+ UINT64 Bandwidth;\r
+ UINT64 DeltaTime;\r
+ UINT64 NanoSeconds;\r
+ UINT32 Value;\r
+\r
+ //\r
+ // Compute the download time\r
+ //\r
+ DeltaTime = GetPerformanceCounter ( );\r
+ if ( pTftpServer->Time2 > pTftpServer->Time1 ) {\r
+ DeltaTime = DeltaTime - pContext->TimeStart;\r
+ }\r
+ else {\r
+ DeltaTime = pContext->TimeStart - DeltaTime;\r
+ }\r
+ NanoSeconds = GetTimeInNanoSecond ( DeltaTime );\r
+ Bandwidth = pContext->LengthInBytes;\r
+ DEBUG (( DEBUG_WINDOW,\r
+ "File Length %Ld, Transfer Time: %d.%03d Sec\r\n",\r
+ Bandwidth,\r
+ DivU64x32 ( NanoSeconds, 1000 * 1000 * 1000 ),\r
+ ((UINT32)DivU64x32 ( NanoSeconds, 1000 * 1000 )) % 1000 ));\r
+\r
+ //\r
+ // Display the round trip time\r
+ //\r
+ Bandwidth = MultU64x32 ( Bandwidth, 8 * 1000 * 1000 );\r
+ Bandwidth /= NanoSeconds;\r
+ if ( 1000 > Bandwidth ) {\r
+ Value = (UINT32)Bandwidth;\r
+ Print ( L"Bandwidth: %d Kbits/Sec\r\n",\r
+ Value );\r
+ }\r
+ else if (( 1000 * 1000 ) > Bandwidth ) {\r
+ Value = (UINT32)Bandwidth;\r
+ Print ( L"Bandwidth: %d.%03d Mbits/Sec\r\n",\r
+ Value / 1000,\r
+ Value % 1000 );\r
+ }\r
+ else {\r
+ Value = (UINT32)DivU64x32 ( Bandwidth, 1000 );\r
+ Print ( L"Bandwidth: %d.%03d Gbits/Sec\r\n",\r
+ Value / 1000,\r
+ Value % 1000 );\r
+ }\r
+ }\r
+ }\r
+ break;\r
+ }\r
+\r
+ //\r
+ // Return the operation status\r
+ //\r
+ DBG_EXIT ( );\r
+ return bCloseContext;\r
+}\r
+\r
+\r
+/**\r
+ Get the next TFTP option\r
+\r
+ @param [in] pOption Address of a zero terminated option string\r
+ @param [in] pEnd End of buffer address\r
+ @param [in] ppNextOption Address to receive the address of the next\r
+ zero terminated option string\r
+\r
+ @retval EFI_SUCCESS Message processed successfully\r
+\r
+**/\r
+EFI_STATUS\r
+TftpOptionGet (\r
+ IN UINT8 * pOption,\r
+ IN UINT8 * pEnd,\r
+ IN UINT8 ** ppNextOption\r
+ )\r
+{\r
+ UINT8 * pNextOption;\r
+ EFI_STATUS Status;\r
+\r
+ //\r
+ // Locate the end of the option\r
+ //\r
+ pNextOption = pOption;\r
+ while (( pEnd > pNextOption ) && ( 0 != *pNextOption )) {\r
+ pNextOption += 1;\r
+ }\r
+ if ( pEnd <= pNextOption ) {\r
+ //\r
+ // Error - end of buffer reached\r
+ //\r
+ DEBUG (( DEBUG_ERROR | DEBUG_TFTP_REQUEST,\r
+ "ERROR - Option without zero termination received!\r\n" ));\r
+ Status = EFI_INVALID_PARAMETER;\r
+ }\r
+ else {\r
+ //\r
+ // Zero terminated option found\r
+ //\r
+ pNextOption += 1;\r
+\r
+ //\r
+ // Display the zero terminated ASCII option string\r
+ //\r
+ DEBUG (( DEBUG_TFTP_REQUEST,\r
+ "Option: %a\r\n",\r
+ pOption ));\r
+ Status = EFI_SUCCESS;\r
+ }\r
+\r
+ //\r
+ // Return the next option address\r
+ //\r
+ *ppNextOption = pNextOption;\r
+\r
+ //\r
+ // Return the operation status\r
+ //\r
+ return Status;\r
+}\r
+\r
+\r
+/**\r
+ Place an option value into the option acknowledgement\r
+\r
+ @param [in] pOack Option acknowledgement address\r
+ @param [in] Value Value to translate into ASCII decimal\r
+\r
+ @return Option acknowledgement address\r
+\r
+**/\r
+UINT8 *\r
+TftpOptionSet (\r
+ IN UINT8 * pOack,\r
+ IN UINT64 Value\r
+ )\r
+{\r
+ UINT64 NextValue;\r
+\r
+ //\r
+ // Determine the next value\r
+ //\r
+ NextValue = Value / 10;\r
+\r
+ //\r
+ // Supress leading zeros\r