/**
*
* Write Data to RSA RAM at a given offset
*
* @param InstancePtr is a pointer to the XSecure_Aes instance.
* @param WrData is pointer to the data to be written to RSA RAM
* @param RamOffset is the offset for the data to be written in RSA RAM
*
* @return None
*
* @note None
*
******************************************************************************/
static void XSecure_RsaWriteMem(XSecure_Rsa *InstancePtr, u32* WrData,
u8 RamOffset)
{
/* Assert validates the input arguments */
Xil_AssertVoid(InstancePtr != NULL);
u32 Index = 0U;
u32 DataOffset = 0U;
u32 TmpIndex = 0U;
u32 Data = 0U;
/** Each of this loop will write 192 bits of data*/
for (DataOffset = 0U; DataOffset < 22U; DataOffset++)
{
for (Index = 0U; Index < 6U; Index++)
{
TmpIndex = (DataOffset*6) + Index;
/**
* Exponent size is only 4 bytes
* and rest of the data needs to be 0
*/
if(XSECURE_CSU_RSA_RAM_EXPO == RamOffset)
{
if(0U == TmpIndex )
{
Data = Xil_Htonl(*WrData);
}
else
{
Data = 0x0U;
}
}
else
{
if(TmpIndex >=128U)
{
Data = 0x0U;
}
else
{
/**
* The RSA data in Image is in Big Endian.
* So reverse it before putting in RSA memory,
* becasue RSA h/w expects it in Little endian.
*/
Data = Xil_Htonl(WrData[127U - TmpIndex]);
}
}
XSecure_WriteReg(InstancePtr->BaseAddress,
(XSECURE_CSU_RSA_WR_DATA_0_OFFSET + (Index * 4)),
Data);
}
XSecure_WriteReg(InstancePtr->BaseAddress,
XSECURE_CSU_RSA_WR_ADDR_OFFSET,
((RamOffset * 22) + DataOffset));
}
}
static void XSecure_RsaMod32Inverse(XSecure_Rsa *InstancePtr)
{
/* Assert validates the input arguments */
Xil_AssertVoid(InstancePtr != NULL);
/* Calculate the MINV */
u8 Count = 0U;
u32 *ModPtr = (u32 *)(InstancePtr->Mod);
u32 ModVal = Xil_Htonl(ModPtr[127]);
u32 Inv = 2U - ModVal;
for (Count = 0U; Count < 4U; ++Count)
{
Inv = (Inv * (2U - ( ModVal * Inv ) ) );
}
Inv = -Inv;
/* Put the value in MINV registers */
XSecure_WriteReg(InstancePtr->BaseAddress, XSECURE_CSU_RSA_MINV0_OFFSET,
(Inv & 0xFF ));
XSecure_WriteReg(InstancePtr->BaseAddress, XSECURE_CSU_RSA_MINV1_OFFSET,
((Inv >> 8) & 0xFF ));
XSecure_WriteReg(InstancePtr->BaseAddress, XSECURE_CSU_RSA_MINV2_OFFSET,
((Inv >> 16) & 0xFF ));
XSecure_WriteReg(InstancePtr->BaseAddress, XSECURE_CSU_RSA_MINV3_OFFSET,
((Inv >> 24) & 0xFF ));
}
/******************************************************************************
*
* This function calculates the next block size and updates the required
* parameters.
*
* @param PartitionParams is a pointer to XFsblPs_PlPartition
* @param ChunkAdrs is a pointer to the data location
* @param ChunkSize is the remaining chunk size
*
* @return
* Error code on failure
* XFSBL_SUCESS on success
*
* @note None
*
******************************************************************************/
static u32 XFsbl_DecrptSetUpNextBlk(XFsblPs_PlPartition *PartitionParams,
UINTPTR ChunkAdrs, u32 ChunkSize)
{
u32 Status = XFSBL_SUCCESS;
u32 SssAes;
u32 SssCfg;
/* Length of next block */
PartitionParams->PlEncrypt.NextBlkLen =
Xil_Htonl(XSecure_ReadReg(
PartitionParams->PlEncrypt.SecureAes->BaseAddress,
XSECURE_CSU_AES_IV_3_OFFSET)) * 4;
PartitionParams->PlEncrypt.SecureAes->Iv =
(u32 *)(PartitionParams->PlEncrypt.SecureAes->BaseAddress +
(UINTPTR)XSECURE_CSU_AES_IV_0_OFFSET);
/* Configure the SSS for AES. */
SssAes = XSecure_SssInputAes(XSECURE_CSU_SSS_SRC_SRC_DMA);
SssCfg = SssAes | XSecure_SssInputPcap(XSECURE_CSU_SSS_SRC_AES);
XSecure_SssSetup(SssCfg);
/* Start the message. */
XSecure_WriteReg(PartitionParams->PlEncrypt.SecureAes->BaseAddress,
XSECURE_CSU_AES_START_MSG_OFFSET,
XSECURE_CSU_AES_START_MSG);
/* Transfer IV of the next block */
XFsbl_DmaPlCopy(PartitionParams->PlEncrypt.SecureAes->CsuDmaPtr,
(UINTPTR)PartitionParams->PlEncrypt.SecureAes->Iv,
XSECURE_SECURE_GCM_TAG_SIZE/4, 0);
PartitionParams->PlEncrypt.SecureAes->SizeofData =
PartitionParams->PlEncrypt.NextBlkLen;
XSecure_WriteReg(PartitionParams->PlEncrypt.SecureAes->BaseAddress,
XSECURE_CSU_AES_KUP_WR_OFFSET, 0x0);
return Status;
}
/**
*
* Read back the resulting data from RSA RAM
*
* @param InstancePtr is a pointer to the XSecure_Rsa instance.
* @param RdData is the pointer to location where the decrypted data will
* be written
*
* @return None
*
* @note None
*
******************************************************************************/
static void XSecure_RsaGetData(XSecure_Rsa *InstancePtr, u32 *RdData)
{
/* Assert validates the input arguments */
Xil_AssertVoid(InstancePtr != NULL);
u32 Index = 0U;
u32 DataOffset = 0U;
s32 TmpIndex = 0;
/* Each of this loop will write 192 bits of data */
for (DataOffset = 0U; DataOffset < 22U; DataOffset++)
{
XSecure_WriteReg(InstancePtr->BaseAddress,
XSECURE_CSU_RSA_RD_ADDR_OFFSET,
(XSECURE_CSU_RSA_RAM_RES_Y * 22) + DataOffset);
Index = (DataOffset == 0U) ? 2: 0;
for (; Index < 6; Index++)
{
TmpIndex = 129 - ((DataOffset*6) + Index);
if(TmpIndex < 0)
{
break;
}
/*
* The Signature digest is compared in Big endian.
* So because RSA h/w results in Little endian,
* reverse it after reading it from RSA memory,
*/
RdData[TmpIndex] = Xil_Htonl(XSecure_ReadReg(
InstancePtr->BaseAddress,
(XSECURE_CSU_RSA_RD_DATA_0_OFFSET+ (Index * 4))));
}
}
}
/**
* Node Commands
*
* This function is part of the callback system for the Ethernet transport
* and will be executed when a valid node commands is recevied.
*
* @param Command Header - WARPNet Command Header
* Command Arguments - WARPNet Command Arguments
* Response Header - WARPNet Response Header
* Response Arguments - WARPNet Response Arguments
* Packet Source - Ethernet Packet Source
* Ethernet Device Number - Indicates which Ethernet device packet came from
*
* @return None.
*
* @note See on-line documentation for more information about the ethernet
* packet structure for WARPNet: www.warpproject.org
*
******************************************************************************/
int node_processCmd(const wn_cmdHdr* cmdHdr,const void* cmdArgs, wn_respHdr* respHdr,void* respArgs, void* pktSrc, unsigned int eth_dev_num){
//IMPORTANT ENDIAN NOTES:
// -cmdHdr is safe to access directly (pre-swapped if needed)
// -cmdArgs is *not* pre-swapped, since the framework doesn't know what it is
// -respHdr will be swapped by the framework; user code should fill it normally
// -respArgs will *not* be swapped by the framework, since only user code knows what it is
// Any data added to respArgs by the code below must be endian-safe (swapped on AXI hardware)
int status = 0;
const u32 * cmdArgs32 = cmdArgs;
u32 * respArgs32 = respArgs;
unsigned int respIndex = 0;
unsigned int respSent = NO_RESP_SENT;
unsigned int max_words = 320; // Max number of u32 words that can be sent in the packet (~1400 bytes)
// If we need more, then we will need to rework this to send multiple response packets
unsigned int temp, i;
// Variables for functions
u32 id;
u32 flags;
u32 start_address;
u32 curr_address;
u32 next_address;
u32 size;
u32 evt_log_size;
u32 transfer_size;
u32 bytes_per_pkt;
u32 num_bytes;
u32 num_pkts;
u32 interval;
unsigned int cmdID;
cmdID = WN_CMD_TO_CMDID(cmdHdr->cmd);
respHdr->cmd = cmdHdr->cmd;
respHdr->length = 0;
respHdr->numArgs = 0;
#ifdef _DEBUG_
xil_printf("In node_processCmd(): ID = %d \n", cmdID);
#endif
switch(cmdID){
//---------------------------------------------------------------------
case WARPNET_TYPE:
// Return the WARPNet Type
respArgs32[respIndex++] = Xil_Htonl( node_info.type );
respHdr->length += (respIndex * sizeof(respArgs32));
respHdr->numArgs = respIndex;
break;
//---------------------------------------------------------------------
case NODE_INFO:
// Return the info about the WLAN_EXP_NODE
// Send node parameters
temp = node_get_parameters( &respArgs32[respIndex], max_words, TRANSMIT_OVER_NETWORK);
respIndex += temp;
max_words -= temp;
if ( max_words <= 0 ) { xil_printf("No more space left in NODE_INFO packet \n"); };
// Send transport parameters
temp = transport_get_parameters( eth_dev_num, &respArgs32[respIndex], max_words, TRANSMIT_OVER_NETWORK);
respIndex += temp;
max_words -= temp;
if ( max_words <= 0 ) { xil_printf("No more space left in NODE_INFO packet \n"); };
#ifdef _DEBUG_
xil_printf("NODE INFO: \n");
for ( i = 0; i < respIndex; i++ ) {
xil_printf(" [%2d] = 0x%8x \n", i, respArgs32[i]);
}
xil_printf("END NODE INFO \n");
#endif
// --------------------------------
// Future parameters go here
// --------------------------------
// Finalize response
respHdr->length += (respIndex * sizeof(respArgs32));
respHdr->numArgs = respIndex;
break;
//---------------------------------------------------------------------
case NODE_IDENTIFY:
// Return Null Response
// The WLAN_EXP_NODE currently does not have access to LEDs
xil_printf(" Node: %d IP Address: %d.%d.%d.%d \n", node_info.node, node_info.ip_addr[0], node_info.ip_addr[1],node_info.ip_addr[2],node_info.ip_addr[3]);
// --------------------------------
// TODO: Add in visual identifiers for the node
// --------------------------------
break;
//---------------------------------------------------------------------
case NODE_CONFIG_SETUP:
// NODE_CONFIG_SETUP Packet Format:
// - Note: All u32 parameters in cmdArgs32 are byte swapped so use Xil_Ntohl()
//
// - cmdArgs32[0] - Serial Number
// - cmdArgs32[1] - Node ID
// - cmdArgs32[2] - IP Address
// - cmdArgs32[3] - Unicast Port
// - cmdArgs32[4] - Broadcast Port
//
if ( node_info.node == 0xFFFF ) {
// Only update the parameters if the serial numbers match
if ( node_info.serial_number == Xil_Ntohl(cmdArgs32[0]) ) {
xil_printf(" Reconfiguring ETH %c \n", wn_conv_eth_dev_num(eth_dev_num) );
node_info.node = Xil_Ntohl(cmdArgs32[1]) & 0xFFFF;
xil_printf(" New Node ID : %d \n", node_info.node);
// Grab New IP Address
node_info.ip_addr[0] = (Xil_Ntohl(cmdArgs32[2]) >> 24) & 0xFF;
node_info.ip_addr[1] = (Xil_Ntohl(cmdArgs32[2]) >> 16) & 0xFF;
node_info.ip_addr[2] = (Xil_Ntohl(cmdArgs32[2]) >> 8) & 0xFF;
node_info.ip_addr[3] = (Xil_Ntohl(cmdArgs32[2]) ) & 0xFF;
// Grab new ports
node_info.unicast_port = Xil_Ntohl(cmdArgs32[3]);
node_info.broadcast_port = Xil_Ntohl(cmdArgs32[4]);
xil_printf(" New IP Address : %d.%d.%d.%d \n", node_info.ip_addr[0], node_info.ip_addr[1],node_info.ip_addr[2],node_info.ip_addr[3]);
xil_printf(" New Unicast Port : %d \n", node_info.unicast_port);
xil_printf(" New Broadcast Port: %d \n", node_info.broadcast_port);
transport_set_hw_info( eth_dev_num, node_info.ip_addr, node_info.hw_addr);
status = transport_config_sockets(eth_dev_num, node_info.unicast_port, node_info.broadcast_port);
if(status != 0) {
xil_printf("Error binding transport...\n");
}
} else {
xil_printf("NODE_IP_SETUP Packet with Serial Number %d ignored. My serial number is %d \n", Xil_Ntohl(cmdArgs32[0]), node_info.serial_number);
}
}
/**
* This function validates the partition
*
* @param FsblInstancePtr is pointer to the XFsbl Instance
*
* @param PartitionNum is the partition number in the image to be loaded
*
* @return returns the error codes described in xfsbl_error.h on any error
* returns XFSBL_SUCCESS on success
*
*****************************************************************************/
static u32 XFsbl_PartitionValidation(XFsblPs * FsblInstancePtr,
u32 PartitionNum)
{
u32 Status=XFSBL_SUCCESS;
u32 ChecksumType=0U;
s32 IsEncryptionEnabled=FALSE;
s32 IsAuthenticationEnabled=FALSE;
u32 DestinationDevice=0U;
u32 DestinationCpu=0U;
u32 ExecState=0U;
u32 CpuNo=0U;
XFsblPs_PartitionHeader * PartitionHeader;
#if defined(XFSBL_RSA)
u32 HashLen=0U;
#endif
#if defined(XFSBL_AES)
u32 ImageOffset = 0U;
u32 FsblIv[XIH_BH_IV_LENGTH / 4U];
u32 UnencryptedLength = 0;
u32 IvLocation;
#endif
#if defined(XFSBL_RSA) || defined(XFSBL_AES)
u32 Length=0U;
#endif
#if defined(XFSBL_RSA) || defined(XFSBL_AES) || defined(XFSBL_BS)
PTRSIZE LoadAddress=0U;
#endif
#if defined(XFSBL_BS)
u32 BitstreamWordSize = 0;
#endif
/**
* Update the variables
*/
PartitionHeader =
&FsblInstancePtr->ImageHeader.PartitionHeader[PartitionNum];
ChecksumType = XFsbl_GetChecksumType(PartitionHeader);
/**
* Check the encryption status
*/
if (XFsbl_IsEncrypted(PartitionHeader) ==
XIH_PH_ATTRB_ENCRYPTION )
{
IsEncryptionEnabled = TRUE;
#ifdef XFSBL_AES
/* Copy the Iv from Flash into local memory */
IvLocation = ImageOffset + XIH_BH_IV_OFFSET;
Status = FsblInstancePtr->DeviceOps.DeviceCopy(IvLocation,
(PTRSIZE) FsblIv, XIH_BH_IV_LENGTH);
if (Status != XFSBL_SUCCESS) {
XFsbl_Printf(DEBUG_GENERAL,
"XFSBL_ERROR_DECRYPTION_IV_COPY_FAIL \r\n");
Status = XFSBL_ERROR_DECRYPTION_IV_COPY_FAIL;
goto END;
}
#else
XFsbl_Printf(DEBUG_GENERAL,"XFSBL_ERROR_AES_NOT_ENABLED \r\n");
Status = XFSBL_ERROR_AES_NOT_ENABLED;
goto END;
#endif
}
/**
* check the authentication status
*/
if (XFsbl_IsRsaSignaturePresent(PartitionHeader) ==
XIH_PH_ATTRB_RSA_SIGNATURE )
{
IsAuthenticationEnabled = TRUE;
}
DestinationDevice = XFsbl_GetDestinationDevice(PartitionHeader);
DestinationCpu = XFsbl_GetDestinationCpu(PartitionHeader);
/**
* Get the execution state
*/
ExecState = XFsbl_GetA53ExecState(PartitionHeader);
/**
* if destination cpu is not present, it means it is for same cpu
*/
if (DestinationCpu == XIH_PH_ATTRB_DEST_CPU_NONE)
{
DestinationCpu = FsblInstancePtr->ProcessorID;
}
/**
* Checksum Validation
*/
if (ChecksumType == XIH_PH_ATTRB_CHECKSUM_MD5)
{
/**
* Do the checksum validation
*/
}
#if defined(XFSBL_RSA) || defined(XFSBL_AES)
if ((IsAuthenticationEnabled == TRUE) || (IsEncryptionEnabled == TRUE))
{
LoadAddress = PartitionHeader->DestinationLoadAddress;
Length = PartitionHeader->TotalDataWordLength * 4U;
Status = XFsbl_GetLoadAddress(DestinationCpu,
&LoadAddress, Length);
if (XFSBL_SUCCESS != Status)
{
goto END;
}
}
#endif
#ifdef XFSBL_BS
if ((DestinationDevice == XIH_PH_ATTRB_DEST_DEVICE_PL) &&
(LoadAddress == 0U))
{
LoadAddress = XFSBL_DDR_TEMP_ADDRESS;
}
#endif
/**
* Authentication Check
*/
if (IsAuthenticationEnabled == TRUE)
{
XFsbl_Printf(DEBUG_INFO,"Authentication Enabled\r\n");
#ifdef XFSBL_RSA
/**
* Get the Sha type to be used from
* boot header attributes
*/
if ((FsblInstancePtr->BootHdrAttributes &
XIH_BH_IMAGE_ATTRB_SHA2_MASK) ==
XIH_BH_IMAGE_ATTRB_SHA2_MASK)
{
HashLen = XFSBL_HASH_TYPE_SHA2;
} else {
HashLen = XFSBL_HASH_TYPE_SHA3;
}
/**
* cache disbale can be removed
*/
Xil_DCacheDisable();
/**
* Do the authentication validation
*/
Status = XFsbl_Authentication(FsblInstancePtr, LoadAddress, Length,
(LoadAddress + Length) - XFSBL_AUTH_CERT_MIN_SIZE,
HashLen);
if (Status != XFSBL_SUCCESS)
{
goto END;
}
#else
XFsbl_Printf(DEBUG_GENERAL,"XFSBL_ERROR_RSA_NOT_ENABLED \r\n");
Status = XFSBL_ERROR_RSA_NOT_ENABLED;
goto END;
#endif
}
/**
* Decrypt image through CSU DMA
*/
if (IsEncryptionEnabled == TRUE) {
XFsbl_Printf(DEBUG_INFO, "Decryption Enabled\r\n");
#ifdef XFSBL_AES
/* AES expects IV in big endian form */
FsblIv[0] = Xil_Htonl(FsblIv[0]);
FsblIv[1] = Xil_Htonl(FsblIv[1]);
FsblIv[2] = Xil_Htonl(FsblIv[2]);
/* Initialize the Aes Instance so that it's ready to use */
XSecure_AesInitialize(&SecureAes, &CsuDma,
XSECURE_CSU_AES_KEY_SRC_DEV, FsblIv, NULL);
XFsbl_Printf(DEBUG_INFO, " Aes initialized \r\n");
UnencryptedLength = PartitionHeader->UnEncryptedDataWordLength * 4U;
if (DestinationDevice != XIH_PH_ATTRB_DEST_DEVICE_PL) {
Status = XSecure_AesDecrypt(&SecureAes, (u8 *) LoadAddress,
(u8 *) LoadAddress, UnencryptedLength);
if (Status != XFSBL_SUCCESS) {
Status = XFSBL_ERROR_DECRYPTION_FAIL;
XFsbl_Printf(DEBUG_GENERAL, "XFSBL_ERROR_DECRYPTION_FAIL\r\n");
goto END;
} else {
XFsbl_Printf(DEBUG_GENERAL, "Decryption Successful\r\n");
}
}
#else
XFsbl_Printf(DEBUG_GENERAL,"XFSBL_ERROR_AES_NOT_ENABLED \r\n");
Status = XFSBL_ERROR_AES_NOT_ENABLED;
goto END;
#endif
}
#ifdef XFSBL_BS
/**
* for PL image use CSU DMA to route to PL
*/
if (DestinationDevice == XIH_PH_ATTRB_DEST_DEVICE_PL)
{
/**
* Fsbl hook before bit stream download
*/
Status = XFsbl_HookBeforeBSDownload();
if (Status != XFSBL_SUCCESS)
{
Status = XFSBL_ERROR_HOOK_BEFORE_BITSTREAM_DOWNLOAD;
XFsbl_Printf(DEBUG_GENERAL,
"XFSBL_ERROR_HOOK_BEFORE_BITSTREAM_DOWNLOAD\r\n");
goto END;
}
XFsbl_Printf(DEBUG_GENERAL, "Bitstream download to start now\r\n");
Status = XFsbl_PcapInit();
if (Status != XFSBL_SUCCESS) {
goto END;
}
if (IsEncryptionEnabled == TRUE) {
#ifdef XFSBL_AES
/*
* The secure bitstream would be sent through CSU DMA to AES
* and the decrypted bitstream is sent directly to PCAP
* by configuring SSS appropriately
*/
Status = XSecure_AesDecrypt(&SecureAes,
(u8 *) XFSBL_DESTINATION_PCAP_ADDR,
(u8 *) LoadAddress, UnencryptedLength);
if (Status != XFSBL_SUCCESS) {
Status = XFSBL_ERROR_BITSTREAM_DECRYPTION_FAIL;
XFsbl_Printf(DEBUG_GENERAL,
"XFSBL_ERROR_BITSTREAM_DECRYPTION_FAIL\r\n");
/* Reset PL */
XFsbl_Out32(CSU_PCAP_PROG, 0x0);
goto END;
} else {
XFsbl_Printf(DEBUG_GENERAL,
"Bitstream decryption Successful\r\n");
}
#endif
}
else {
/* Use CSU DMA to load Bit stream to PL */
BitstreamWordSize = PartitionHeader->UnEncryptedDataWordLength;
Status = XFsbl_WriteToPcap(BitstreamWordSize, (u8 *) LoadAddress);
if (Status != XFSBL_SUCCESS) {
goto END;
}
}
Status = XFsbl_PLWaitForDone();
if (Status != XFSBL_SUCCESS) {
goto END;
}
/**
* Fsbl hook after bit stream download
*/
Status = XFsbl_HookAfterBSDownload();
if (Status != XFSBL_SUCCESS)
{
Status = XFSBL_ERROR_HOOK_AFTER_BITSTREAM_DOWNLOAD;
XFsbl_Printf(DEBUG_GENERAL,
"XFSBL_ERROR_HOOK_AFTER_BITSTREAM_DOWNLOAD\r\n");
goto END;
}
}
#endif
/**
* Update the handoff details
*/
if ((DestinationDevice != XIH_PH_ATTRB_DEST_DEVICE_PL) &&
(DestinationDevice != XIH_PH_ATTRB_DEST_DEVICE_PMU))
{
CpuNo = FsblInstancePtr->HandoffCpuNo;
if (XFsbl_CheckHandoffCpu(FsblInstancePtr,
DestinationCpu) == XFSBL_SUCCESS)
{
FsblInstancePtr->HandoffValues[CpuNo].CpuSettings =
DestinationCpu | ExecState;
FsblInstancePtr->HandoffValues[CpuNo].HandoffAddress =
PartitionHeader->DestinationExecutionAddress;
FsblInstancePtr->HandoffCpuNo += 1U;
}
}
END:
return Status;
}
/**
* Process Transport Commands
*
* This function is part of the Ethernet processing system and will process the
* various transport related commands.
*
* @param socket_index - Index of the socket on which to send message
* @param from - Pointer to socket address structure (struct sockaddr *) where command is from
* @param command - Pointer to Command
* @param response - Pointer to Response
* @param max_resp_len - Maximum number of u32 words allowed in response
*
* @return int - Status of the command:
* NO_RESP_SENT - No response has been sent
* RESP_SENT - A response has been sent
*
* @note See on-line documentation for more information about the Ethernet
* packet structure: www.warpproject.org
*
*****************************************************************************/
int process_transport_cmd(int socket_index, void * from, cmd_resp * command, cmd_resp * response, u32 max_resp_len) {
//
// IMPORTANT ENDIAN NOTES:
// - command
// - header - Already endian swapped by the framework (safe to access directly)
// - args - Must be endian swapped as necessary by code (framework does not know the contents of the command)
// - response
// - header - Will be endian swapped by the framework (safe to write directly)
// - args - Must be endian swapped as necessary by code (framework does not know the contents of the response)
//
// Standard variables
u32 resp_sent = NO_RESP_SENT;
cmd_resp_hdr * cmd_hdr = command->header;
u32 * cmd_args_32 = command->args;
u32 cmd_id = CMD_TO_CMDID(cmd_hdr->cmd);
cmd_resp_hdr * resp_hdr = response->header;
u32 * resp_args_32 = response->args;
u32 resp_index = 0;
u32 eth_dev_num = socket_get_eth_dev_num(socket_index);
// Set up the response header
resp_hdr->cmd = cmd_hdr->cmd;
resp_hdr->length = 0;
resp_hdr->num_args = 0;
// Check Ethernet device number
if (eth_dev_num == WARP_IP_UDP_INVALID_ETH_DEVICE) {
wlan_exp_printf(WLAN_EXP_PRINT_ERROR, print_type_transport, "Invalid socket index: %d\n", socket_index);
return resp_sent;
}
// Process the command
switch(cmd_id){
//---------------------------------------------------------------------
case CMDID_TRANSPORT_PING: {
//
// Nothing actually needs to be done when receiving the ping command. The framework is going
// to respond regardless, which is all the host wants.
//
}
break;
//---------------------------------------------------------------------
case CMDID_TRANSPORT_PAYLOAD_SIZE_TEST: {
//
// Due to packet fragmentation, it is not safe to just return the packet length. We have seen
// an issue where the host will send a 1514 byte fragment which results in a payload size of
// 1472 and causes the transport to not behave correctly. Therefore, we need to find the
// last valid command argument and check that against the packet length.
//
u32 temp;
u32 payload_index;
u32 payload_size;
u32 payload_num_words;
u32 header_size;
header_size = (sizeof(transport_header) + sizeof(cmd_resp_hdr)); // Transport / Command headers
payload_index = (((warp_ip_udp_buffer *)(command->buffer))->length - sizeof(cmd_resp_hdr)) / 4; // Final index into command args (/4 truncates)
// Check the value in the command args to make sure it matches the size_index
payload_num_words = Xil_Htonl(cmd_args_32[payload_index - 1]) + 1; // NOTE: Add 1 since the payload is zero indexed
payload_size = (payload_num_words * 4) + header_size;
temp = ((payload_index * 4) + header_size);
// Update the max_pkt_words field
if (payload_num_words > eth_devices[eth_dev_num].max_pkt_words) {
eth_devices[eth_dev_num].max_pkt_words = payload_num_words;
}
if (payload_size != temp) {
wlan_exp_printf(WLAN_EXP_PRINT_WARNING, print_type_transport, "Payload size mismatch. Value in command args does not match index: %d != %d\n", payload_size, temp);
}
resp_args_32[resp_index++] = Xil_Ntohl(payload_size);
resp_hdr->length += (resp_index * sizeof(resp_args_32));
resp_hdr->num_args = 1;
}
break;
//---------------------------------------------------------------------
case CMDID_TRANSPORT_NODE_GROUP_ID_ADD: {
eth_devices[eth_dev_num].info.group_id = (eth_devices[eth_dev_num].info.group_id | Xil_Htonl(cmd_args_32[0]));
}
break;
//---------------------------------------------------------------------
case CMDID_TRANSPORT_NODE_GROUP_ID_CLEAR: {
eth_devices[eth_dev_num].info.group_id = (eth_devices[eth_dev_num].info.group_id & ~Xil_Htonl(cmd_args_32[0]));
}
break;
//---------------------------------------------------------------------
default: {
wlan_exp_printf(WLAN_EXP_PRINT_ERROR, print_type_transport, "Unknown user command ID: %d\n", cmd_id);
}
break;
}
return resp_sent;
}
int user_processCmd(const wl_cmdHdr* cmdHdr,const void* cmdArgs, wl_respHdr* respHdr, void* respArgs, void* pktSrc) {
//IMPORTANT ENDIAN NOTES:
// -cmdHdr is safe to access directly (pre-swapped if needed)
// -cmdArgs is *not* pre-swapped, since the framework doesn't know what it is
// -respHdr will be swapped by the framework; user code should fill it normally
// -respArgs will *not* be swapped by the framework, since only user code knows what it is
// Any data added to respArgs by the code below must be endian-safe (swapped on AXI hardware)
u32 cmdID;
//Cast argument buffers into arrays for easy accessing
const u32 *cmdArgs32 = cmdArgs;
u32 *respArgs32 = respArgs;
/* Example variables for processing user commands
const u32 *cmdArgs32 = cmdArgs;
u32 *respArgs32 = respArgs;
int respIndex = 0;
u32 arg0, arg1, result;
*/
unsigned int respSent = NO_RESP_SENT;
cmdID = WL_CMD_TO_CMDID(cmdHdr->cmd);
respHdr->cmd = cmdHdr->cmd;
respHdr->length = 0;
respHdr->numArgs = 0;
#ifdef WARP_HW_VER_v3
u32 stringBuffer32[10];
u8 *stringBuffer8 = (u8*)stringBuffer32;
int k;
u32 readLength;
u32 eepromAddrOffset;
#endif
switch(cmdID) {
/* Example user command
case SOME_CMD_ID:
arg0 = Xil_Ntohl(cmdArgs[0]);
arg1 = Xil_Ntohl(cmdArgs[1]);
result = do_something_with_args(arg0, arg1);
respArgs32[respIndex++] = Xil_Htonl(result);
respHdr->length += (respIndex * sizeof(respArgs32));
respHdr->numArgs = respIndex;
break;
*/
#ifdef WARP_HW_VER_v3
case USER_EEPROM_WRITE_STRING:
eepromAddrOffset = Xil_Ntohl(cmdArgs32[0]);
for( k=0; k<((cmdHdr->length)/sizeof(u32)); k++ ) {
stringBuffer32[k] = Xil_Ntohl(cmdArgs32[k+1]);
}
for( k=0; k<(cmdHdr->length); k++ ) {
iic_eeprom_writeByte(EEPROM_BASEADDR,k+eepromAddrOffset,stringBuffer8[k]);
}
xil_printf("Wrote '%s' to EEPROM\n",stringBuffer8);
break;
case USER_EEPROM_READ_STRING:
eepromAddrOffset = Xil_Ntohl(cmdArgs32[0]);
readLength = Xil_Ntohl(cmdArgs32[1]);
for( k=0; k<readLength; k++ ) {
stringBuffer8[k] = iic_eeprom_readByte(EEPROM_BASEADDR,k+eepromAddrOffset);
}
xil_printf("Read '%s' from EEPROM\n",stringBuffer8);
for( k=0; k<((readLength)/sizeof(u32)); k++ ) {
respArgs32[k] = Xil_Htonl(stringBuffer32[k]);
}
respHdr->length += ((k) * sizeof(respArgs32));
respHdr->numArgs = (k);
break;
#endif
default:
xil_printf("Unknown user command ID: %d\n", cmdID);
break;
}
return respSent;
}
