/** Retrieves the total size of the given locally stored (in PROGMEM) attribute Data Element container.
*
* \param[in] AttributeData Pointer to the start of the Attribute container, located in PROGMEM
* \param[out] HeaderSize Pointer to a location where the header size of the data element is to be stored
*
* \return Size in bytes of the entire attribute container, including the header
*/
static uint32_t SDP_GetLocalAttributeContainerSize(const void* const AttributeData,
uint8_t* const HeaderSize)
{
/* Fetch the size of the Data Element structure from the header */
uint8_t SizeIndex = (pgm_read_byte(AttributeData) & 0x07);
uint32_t ElementValueSize;
/* Convert the Data Element size index into a size in bytes */
switch (SizeIndex)
{
case SDP_DATASIZE_Variable8Bit:
*HeaderSize = (1 + sizeof(uint8_t));
ElementValueSize = pgm_read_byte(AttributeData + 1);
break;
case SDP_DATASIZE_Variable16Bit:
*HeaderSize = (1 + sizeof(uint16_t));
ElementValueSize = SwapEndian_16(pgm_read_word(AttributeData + 1));
break;
case SDP_DATASIZE_Variable32Bit:
*HeaderSize = (1 + sizeof(uint32_t));
ElementValueSize = SwapEndian_32(pgm_read_dword(AttributeData + 1));
break;
default:
*HeaderSize = 1;
ElementValueSize = (1 << SizeIndex);
break;
}
return ElementValueSize;
}
/** Command processing for an issued SCSI READ (10) or WRITE (10) command. This command reads in the block start address
* and total number of blocks to process, then calls the appropriate low-level Dataflash routine to handle the actual
* reading and writing of the data.
*
* \param[in] MSInterfaceInfo Pointer to the Mass Storage class interface structure that the command is associated with
* \param[in] IsDataRead Indicates if the command is a READ (10) command or WRITE (10) command (DATA_READ or DATA_WRITE)
*
* \return Boolean true if the command completed successfully, false otherwise.
*/
static bool SCSI_Command_ReadWrite_10(USB_ClassInfo_MS_Device_t* const MSInterfaceInfo,
const bool IsDataRead)
{
uint32_t BlockAddress;
uint16_t TotalBlocks;
/* Load in the 32-bit block address (SCSI uses big-endian, so have to reverse the byte order) */
BlockAddress = SwapEndian_32(*(uint32_t*)&MSInterfaceInfo->State.CommandBlock.SCSICommandData[2]);
/* Load in the 16-bit total blocks (SCSI uses big-endian, so have to reverse the byte order) */
TotalBlocks = SwapEndian_16(*(uint16_t*)&MSInterfaceInfo->State.CommandBlock.SCSICommandData[7]);
/* Check if the block address is outside the maximum allowable value for the LUN */
if (BlockAddress >= VIRTUAL_MEMORY_BLOCKS)
{
/* Block address is invalid, update SENSE key and return command fail */
SCSI_SET_SENSE(SCSI_SENSE_KEY_ILLEGAL_REQUEST,
SCSI_ASENSE_LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE,
SCSI_ASENSEQ_NO_QUALIFIER);
return false;
}
/* Determine if the packet is a READ (10) or WRITE (10) command, call appropriate function */
if (IsDataRead == DATA_READ)
DataflashManager_ReadBlocks(MSInterfaceInfo, BlockAddress, TotalBlocks);
else
DataflashManager_WriteBlocks(MSInterfaceInfo, BlockAddress, TotalBlocks);
/* Update the bytes transferred counter and succeed the command */
MSInterfaceInfo->State.CommandBlock.DataTransferLength -= ((uint32_t)TotalBlocks * VIRTUAL_MEMORY_BLOCK_SIZE);
return true;
}
/** Command processing for an issued SCSI READ (10) or WRITE (10) command. This command reads in the block start address
* and total number of blocks to process, then calls the appropriate low-level Dataflash routine to handle the actual
* reading and writing of the data.
*
* \param[in] IsDataRead Indicates if the command is a READ (10) command or WRITE (10) command (DATA_READ or DATA_WRITE)
*/
static void SCSI_Command_ReadWrite_10(const bool IsDataRead)
{
uint32_t BlockAddress = SwapEndian_32(*(uint32_t*)&CommandBlock.SCSICommandData[2]);
uint16_t TotalBlocks = SwapEndian_16(*(uint16_t*)&CommandBlock.SCSICommandData[7]);
/* Check if the block address is outside the maximum allowable value for the LUN */
if (BlockAddress >= LUN_MEDIA_BLOCKS)
{
/* Block address is invalid, update SENSE key and return command fail */
SCSI_SET_SENSE(SCSI_SENSE_KEY_ILLEGAL_REQUEST,
SCSI_ASENSE_LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE,
SCSI_ASENSEQ_NO_QUALIFIER);
return;
}
#if (TOTAL_LUNS > 1)
/* Adjust the given block address to the real media address based on the selected LUN */
BlockAddress += ((uint32_t)CommandBlock.LUN * LUN_MEDIA_BLOCKS);
#endif
/* Determine if the packet is a READ (10) or WRITE (10) command, call appropriate function */
if (IsDataRead == DATA_READ)
DataflashManager_ReadBlocks(BlockAddress, TotalBlocks);
else
DataflashManager_WriteBlocks(BlockAddress, TotalBlocks);
/* Update the bytes transferred counter and succeed the command */
CommandBlock.DataTransferLength -= ((uint32_t)TotalBlocks * VIRTUAL_MEMORY_BLOCK_SIZE);
}
/** Command processing for an issued SCSI READ (10) or WRITE (10) command. This command reads in the block start address
* and total number of blocks to process, then calls the appropriate low-level Dataflash routine to handle the actual
* reading and writing of the data.
*
* \param[in] MSInterfaceInfo Pointer to the Mass Storage class interface structure that the command is associated with
* \param[in] IsDataRead Indicates if the command is a READ (10) command or WRITE (10) command (DATA_READ or DATA_WRITE)
*
* \return Boolean \c true if the command completed successfully, \c false otherwise.
*/
static bool SCSI_Command_ReadWrite_10(USB_ClassInfo_MS_Device_t* const MSInterfaceInfo,
const bool IsDataRead)
{
uint32_t BlockAddress;
uint16_t TotalBlocks;
/* Check if the disk is write protected or not */
if ((IsDataRead == DATA_WRITE) && DISK_READ_ONLY)
{
/* Block address is invalid, update SENSE key and return command fail */
SCSI_SET_SENSE(SCSI_SENSE_KEY_DATA_PROTECT,
SCSI_ASENSE_WRITE_PROTECTED,
SCSI_ASENSEQ_NO_QUALIFIER);
return false;
}
/* Load in the 32-bit block address (SCSI uses big-endian, so have to reverse the byte order) */
BlockAddress = SwapEndian_32(*(uint32_t*)&MSInterfaceInfo->State.CommandBlock.SCSICommandData[2]);
/* Load in the 16-bit total blocks (SCSI uses big-endian, so have to reverse the byte order) */
TotalBlocks = SwapEndian_16(*(uint16_t*)&MSInterfaceInfo->State.CommandBlock.SCSICommandData[7]);
/* Check if the block address is outside the maximum allowable value for the LUN */
if (BlockAddress >= MMC_MediaBlocks())
{
/* Block address is invalid, update SENSE key and return command fail */
SCSI_SET_SENSE(SCSI_SENSE_KEY_ILLEGAL_REQUEST,
SCSI_ASENSE_LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE,
SCSI_ASENSEQ_NO_QUALIFIER);
return false;
}
#if (TOTAL_LUNS > 1)
/* Adjust the given block address to the real media address based on the selected LUN */
BlockAddress += ((uint32_t)MSInterfaceInfo->State.CommandBlock.LUN * MMC_MediaBlocks());
#endif
/* Determine if the packet is a READ (10) or WRITE (10) command, call appropriate function */
if (IsDataRead == DATA_READ)
MMC_ReadBlocks(MSInterfaceInfo, BlockAddress, TotalBlocks);
else
MMC_WriteBlocks(MSInterfaceInfo, BlockAddress, TotalBlocks);
/* Update the bytes transferred counter and succeed the command */
MSInterfaceInfo->State.CommandBlock.DataTransferLength -= ((uint32_t)TotalBlocks * VIRTUAL_MEMORY_BLOCK_SIZE);
return true;
}
/** Decodes a TCP header and prints its contents to through the USART in a human readable format.
*
* \param[in] InDataStart Pointer to the start of a TCP packet header
*/
void DecodeTCPHeader(void* InDataStart)
{
#if !defined(NO_DECODE_TCP)
TCP_Header_t* TCPHeader = (TCP_Header_t*)InDataStart;
uint16_t HeaderLengthBytes = (TCPHeader->DataOffset * sizeof(uint32_t));
printf_P(PSTR(" \\\r\n TCP\r\n"));
printf_P(PSTR(" + Header Length: %u Bytes\r\n"), HeaderLengthBytes);
printf_P(PSTR(" + Source Port: %u\r\n"), SwapEndian_16(TCPHeader->SourcePort));
printf_P(PSTR(" + Destination Port: %u\r\n"), SwapEndian_16(TCPHeader->DestinationPort));
printf_P(PSTR(" + Sequence Number: %lu\r\n"), SwapEndian_32(TCPHeader->SequenceNumber));
printf_P(PSTR(" + Acknowledgment Number: %lu\r\n"), SwapEndian_32(TCPHeader->AcknowledgmentNumber));
printf_P(PSTR(" + Flags: 0x%02X\r\n"), TCPHeader->Flags);
if (TCP_GetPortState(TCPHeader->DestinationPort) == TCP_Port_Closed)
printf_P(PSTR(" + NOT LISTENING ON DESTINATION PORT\r\n"));
#endif
}
/** Handler for the XPROG READ_MEMORY command to read data from a specific address space within the
* attached device.
*/
static void XPROGProtocol_ReadMemory(void)
{
uint8_t ReturnStatus = XPROG_ERR_OK;
struct
{
uint8_t MemoryType;
uint32_t Address;
uint16_t Length;
} ReadMemory_XPROG_Params;
Endpoint_Read_Stream_LE(&ReadMemory_XPROG_Params, sizeof(ReadMemory_XPROG_Params), NULL);
ReadMemory_XPROG_Params.Address = SwapEndian_32(ReadMemory_XPROG_Params.Address);
ReadMemory_XPROG_Params.Length = SwapEndian_16(ReadMemory_XPROG_Params.Length);
Endpoint_ClearOUT();
Endpoint_SelectEndpoint(AVRISP_DATA_IN_EPADDR);
Endpoint_SetEndpointDirection(ENDPOINT_DIR_IN);
uint8_t ReadBuffer[256];
if (XPROG_SelectedProtocol == XPROG_PROTOCOL_PDI)
{
/* Read the PDI target's memory, indicate timeout if occurred */
if (!(XMEGANVM_ReadMemory(ReadMemory_XPROG_Params.Address, ReadBuffer, ReadMemory_XPROG_Params.Length)))
ReturnStatus = XPROG_ERR_TIMEOUT;
}
else
{
/* Read the TPI target's memory, indicate timeout if occurred */
if (!(TINYNVM_ReadMemory(ReadMemory_XPROG_Params.Address, ReadBuffer, ReadMemory_XPROG_Params.Length)))
ReturnStatus = XPROG_ERR_TIMEOUT;
}
Endpoint_Write_8(CMD_XPROG);
Endpoint_Write_8(XPROG_CMD_READ_MEM);
Endpoint_Write_8(ReturnStatus);
if (ReturnStatus == XPROG_ERR_OK)
Endpoint_Write_Stream_LE(ReadBuffer, ReadMemory_XPROG_Params.Length, NULL);
Endpoint_ClearIN();
}
/** Processes a DHCP packet inside an Ethernet frame, and writes the appropriate response
* to the output Ethernet frame if the host is requesting or accepting an IP address.
*
* \param[in] IPHeaderInStart Pointer to the start of the incoming packet's IP header
* \param[in] DHCPHeaderInStart Pointer to the start of the incoming packet's DHCP header
* \param[out] DHCPHeaderOutStart Pointer to the start of the outgoing packet's DHCP header
*
* \return The number of bytes written to the out Ethernet frame if any, NO_RESPONSE otherwise
*/
int16_t DHCP_ProcessDHCPPacket(void* IPHeaderInStart,
void* DHCPHeaderInStart,
void* DHCPHeaderOutStart)
{
IP_Header_t* IPHeaderIN = (IP_Header_t*)IPHeaderInStart;
DHCP_Header_t* DHCPHeaderIN = (DHCP_Header_t*)DHCPHeaderInStart;
DHCP_Header_t* DHCPHeaderOUT = (DHCP_Header_t*)DHCPHeaderOutStart;
uint8_t* DHCPOptionsINStart = (uint8_t*)(DHCPHeaderInStart + sizeof(DHCP_Header_t));
uint8_t* DHCPOptionsOUTStart = (uint8_t*)(DHCPHeaderOutStart + sizeof(DHCP_Header_t));
DecodeDHCPHeader(DHCPHeaderInStart);
/* Zero out the response DHCP packet, as much of it is legacy and left at 0 */
memset(DHCPHeaderOUT, 0, sizeof(DHCP_Header_t));
/* Fill out the response DHCP packet */
DHCPHeaderOUT->HardwareType = DHCPHeaderIN->HardwareType;
DHCPHeaderOUT->Operation = DHCP_OP_BOOTREPLY;
DHCPHeaderOUT->HardwareAddressLength = DHCPHeaderIN->HardwareAddressLength;
DHCPHeaderOUT->Hops = 0;
DHCPHeaderOUT->TransactionID = DHCPHeaderIN->TransactionID;
DHCPHeaderOUT->ElapsedSeconds = 0;
DHCPHeaderOUT->Flags = DHCPHeaderIN->Flags;
DHCPHeaderOUT->YourIP = ClientIPAddress;
memmove(&DHCPHeaderOUT->ClientHardwareAddress, &DHCPHeaderIN->ClientHardwareAddress, sizeof(MAC_Address_t));
DHCPHeaderOUT->Cookie = SwapEndian_32(DHCP_MAGIC_COOKIE);
/* Alter the incoming IP packet header so that the corrected IP source and destinations are used - this means that
when the response IP header is generated, it will use the corrected addresses and not the null/broatcast addresses */
IPHeaderIN->SourceAddress = ClientIPAddress;
IPHeaderIN->DestinationAddress = ServerIPAddress;
/* Process the incoming DHCP packet options */
while (DHCPOptionsINStart[0] != DHCP_OPTION_END)
{
/* Find the Message Type DHCP option, to determine the type of DHCP packet */
if (DHCPOptionsINStart[0] == DHCP_OPTION_MESSAGETYPE)
{
if ((DHCPOptionsINStart[2] == DHCP_MESSAGETYPE_DISCOVER) || (DHCPOptionsINStart[2] == DHCP_MESSAGETYPE_REQUEST))
{
/* Fill out the response DHCP packet options for a DHCP OFFER or ACK response */
*(DHCPOptionsOUTStart++) = DHCP_OPTION_MESSAGETYPE;
*(DHCPOptionsOUTStart++) = 1;
*(DHCPOptionsOUTStart++) = (DHCPOptionsINStart[2] == DHCP_MESSAGETYPE_DISCOVER) ? DHCP_MESSAGETYPE_OFFER
: DHCP_MESSAGETYPE_ACK;
*(DHCPOptionsOUTStart++) = DHCP_OPTION_SUBNETMASK;
*(DHCPOptionsOUTStart++) = 4;
*(DHCPOptionsOUTStart++) = 0xFF;
*(DHCPOptionsOUTStart++) = 0xFF;
*(DHCPOptionsOUTStart++) = 0xFF;
*(DHCPOptionsOUTStart++) = 0x00;
*(DHCPOptionsOUTStart++) = DHCP_OPTION_DHCPSERVER;
*(DHCPOptionsOUTStart++) = sizeof(IP_Address_t);
memcpy(DHCPOptionsOUTStart, &ServerIPAddress, sizeof(IP_Address_t));
DHCPOptionsOUTStart += sizeof(IP_Address_t);
*(DHCPOptionsOUTStart++) = DHCP_OPTION_END;
return (sizeof(DHCP_Header_t) + 12 + sizeof(IP_Address_t));
}
}
/* Go to the next DHCP option - skip one byte if option is a padding byte, else skip the complete option's size */
DHCPOptionsINStart += ((DHCPOptionsINStart[0] == DHCP_OPTION_PAD) ? 1 : (DHCPOptionsINStart[1] + 2));
}
return NO_RESPONSE;
}
/** Handler for the XPROG WRITE_MEMORY command to write to a specific memory space within the attached device. */
static void XPROGProtocol_WriteMemory(void)
{
uint8_t ReturnStatus = XPROG_ERR_OK;
struct
{
uint8_t MemoryType;
uint8_t PageMode;
uint32_t Address;
uint16_t Length;
uint8_t ProgData[256];
} WriteMemory_XPROG_Params;
Endpoint_Read_Stream_LE(&WriteMemory_XPROG_Params, (sizeof(WriteMemory_XPROG_Params) -
sizeof(WriteMemory_XPROG_Params).ProgData), NULL);
WriteMemory_XPROG_Params.Address = SwapEndian_32(WriteMemory_XPROG_Params.Address);
WriteMemory_XPROG_Params.Length = SwapEndian_16(WriteMemory_XPROG_Params.Length);
Endpoint_Read_Stream_LE(&WriteMemory_XPROG_Params.ProgData, WriteMemory_XPROG_Params.Length, NULL);
// The driver will terminate transfers that are a round multiple of the endpoint bank in size with a ZLP, need
// to catch this and discard it before continuing on with packet processing to prevent communication issues
if (((sizeof(uint8_t) + sizeof(WriteMemory_XPROG_Params) - sizeof(WriteMemory_XPROG_Params.ProgData)) +
WriteMemory_XPROG_Params.Length) % AVRISP_DATA_EPSIZE == 0)
{
Endpoint_ClearOUT();
Endpoint_WaitUntilReady();
}
Endpoint_ClearOUT();
Endpoint_SelectEndpoint(AVRISP_DATA_IN_EPADDR);
Endpoint_SetEndpointDirection(ENDPOINT_DIR_IN);
if (XPROG_SelectedProtocol == XPROG_PROTOCOL_PDI)
{
/* Assume FLASH page programming by default, as it is the common case */
uint8_t WriteCommand = XMEGA_NVM_CMD_WRITEFLASHPAGE;
uint8_t WriteBuffCommand = XMEGA_NVM_CMD_LOADFLASHPAGEBUFF;
uint8_t EraseBuffCommand = XMEGA_NVM_CMD_ERASEFLASHPAGEBUFF;
bool PagedMemory = true;
switch (WriteMemory_XPROG_Params.MemoryType)
{
case XPROG_MEM_TYPE_APPL:
WriteCommand = XMEGA_NVM_CMD_WRITEAPPSECPAGE;
break;
case XPROG_MEM_TYPE_BOOT:
WriteCommand = XMEGA_NVM_CMD_WRITEBOOTSECPAGE;
break;
case XPROG_MEM_TYPE_EEPROM:
WriteCommand = XMEGA_NVM_CMD_ERASEWRITEEEPROMPAGE;
WriteBuffCommand = XMEGA_NVM_CMD_LOADEEPROMPAGEBUFF;
EraseBuffCommand = XMEGA_NVM_CMD_ERASEEEPROMPAGEBUFF;
break;
case XPROG_MEM_TYPE_USERSIG:
WriteCommand = XMEGA_NVM_CMD_WRITEUSERSIG;
break;
case XPROG_MEM_TYPE_FUSE:
WriteCommand = XMEGA_NVM_CMD_WRITEFUSE;
PagedMemory = false;
break;
case XPROG_MEM_TYPE_LOCKBITS:
WriteCommand = XMEGA_NVM_CMD_WRITELOCK;
PagedMemory = false;
break;
}
/* Send the appropriate memory write commands to the device, indicate timeout if occurred */
if ((PagedMemory && !(XMEGANVM_WritePageMemory(WriteBuffCommand, EraseBuffCommand, WriteCommand,
WriteMemory_XPROG_Params.PageMode, WriteMemory_XPROG_Params.Address,
WriteMemory_XPROG_Params.ProgData, WriteMemory_XPROG_Params.Length))) ||
(!PagedMemory && !(XMEGANVM_WriteByteMemory(WriteCommand, WriteMemory_XPROG_Params.Address,
WriteMemory_XPROG_Params.ProgData[0]))))
{
ReturnStatus = XPROG_ERR_TIMEOUT;
}
}
else
{
/* Send write command to the TPI device, indicate timeout if occurred */
if (!(TINYNVM_WriteMemory(WriteMemory_XPROG_Params.Address, WriteMemory_XPROG_Params.ProgData,
WriteMemory_XPROG_Params.Length)))
{
ReturnStatus = XPROG_ERR_TIMEOUT;
}
}
Endpoint_Write_8(CMD_XPROG);
Endpoint_Write_8(XPROG_CMD_WRITE_MEM);
Endpoint_Write_8(ReturnStatus);
Endpoint_ClearIN();
}
/** Handler for the XPRG ERASE command to erase a specific memory address space in the attached device. */
static void XPROGProtocol_Erase(void)
{
uint8_t ReturnStatus = XPROG_ERR_OK;
struct
{
uint8_t MemoryType;
uint32_t Address;
} Erase_XPROG_Params;
Endpoint_Read_Stream_LE(&Erase_XPROG_Params, sizeof(Erase_XPROG_Params), NULL);
Erase_XPROG_Params.Address = SwapEndian_32(Erase_XPROG_Params.Address);
Endpoint_ClearOUT();
Endpoint_SelectEndpoint(AVRISP_DATA_IN_EPADDR);
Endpoint_SetEndpointDirection(ENDPOINT_DIR_IN);
uint8_t EraseCommand;
if (XPROG_SelectedProtocol == XPROG_PROTOCOL_PDI)
{
/* Determine which NVM command to send to the device depending on the memory to erase */
switch (Erase_XPROG_Params.MemoryType)
{
case XPROG_ERASE_CHIP:
EraseCommand = XMEGA_NVM_CMD_CHIPERASE;
break;
case XPROG_ERASE_APP:
EraseCommand = XMEGA_NVM_CMD_ERASEAPPSEC;
break;
case XPROG_ERASE_BOOT:
EraseCommand = XMEGA_NVM_CMD_ERASEBOOTSEC;
break;
case XPROG_ERASE_EEPROM:
EraseCommand = XMEGA_NVM_CMD_ERASEEEPROM;
break;
case XPROG_ERASE_APP_PAGE:
EraseCommand = XMEGA_NVM_CMD_ERASEAPPSECPAGE;
break;
case XPROG_ERASE_BOOT_PAGE:
EraseCommand = XMEGA_NVM_CMD_ERASEBOOTSECPAGE;
break;
case XPROG_ERASE_EEPROM_PAGE:
EraseCommand = XMEGA_NVM_CMD_ERASEEEPROMPAGE;
break;
case XPROG_ERASE_USERSIG:
EraseCommand = XMEGA_NVM_CMD_ERASEUSERSIG;
break;
default:
EraseCommand = XMEGA_NVM_CMD_NOOP;
break;
}
/* Erase the target memory, indicate timeout if occurred */
if (!(XMEGANVM_EraseMemory(EraseCommand, Erase_XPROG_Params.Address)))
ReturnStatus = XPROG_ERR_TIMEOUT;
}
else
{
if (Erase_XPROG_Params.MemoryType == XPROG_ERASE_CHIP)
EraseCommand = TINY_NVM_CMD_CHIPERASE;
else
EraseCommand = TINY_NVM_CMD_SECTIONERASE;
/* Erase the target memory, indicate timeout if occurred */
if (!(TINYNVM_EraseMemory(EraseCommand, Erase_XPROG_Params.Address)))
ReturnStatus = XPROG_ERR_TIMEOUT;
}
Endpoint_Write_8(CMD_XPROG);
Endpoint_Write_8(XPROG_CMD_ERASE);
Endpoint_Write_8(ReturnStatus);
Endpoint_ClearIN();
}
/** Internal processing routine for SDP Service Attribute Requests.
*
* \param[in] SDPHeader Pointer to the start of the issued SDP request
* \param[in] Channel Pointer to the Bluetooth channel structure the request was issued to
*/
static void SDP_ProcessServiceAttribute(const SDP_PDUHeader_t* const SDPHeader,
Bluetooth_Channel_t* const Channel)
{
const void* CurrentParameter = ((const void*)SDPHeader + sizeof(SDP_PDUHeader_t));
BT_SDP_DEBUG(1, "<< Service Attribute");
/* Retrieve the service handle whose attributes are to be examined */
uint32_t ServiceHandle = SDP_ReadData32(&CurrentParameter);
BT_SDP_DEBUG(2, "-- Service Handle: 0x%08lX", ServiceHandle);
/* Retrieve the maximum Attribute response size from the request */
uint16_t MaxAttributeSize = SDP_ReadData16(&CurrentParameter);
BT_SDP_DEBUG(2, "-- Max Return Attribute Bytes: 0x%04X", MaxAttributeSize);
/* Retrieve the list of Attributes from the request */
uint16_t AttributeList[8][2];
uint8_t TotalAttributes = SDP_GetAttributeList(AttributeList, &CurrentParameter);
BT_SDP_DEBUG(2, "-- Total Attributes: %d", TotalAttributes);
struct
{
SDP_PDUHeader_t SDPHeader;
uint16_t AttributeListByteCount;
uint8_t ResponseData[100];
} ResponsePacket;
/* Create a pointer to the buffer to indicate the current location for response data to be added */
void* CurrResponsePos = ResponsePacket.ResponseData;
/* Clamp the maximum attribute size to the size of the allocated buffer */
if (MaxAttributeSize > sizeof(ResponsePacket.ResponseData))
MaxAttributeSize = sizeof(ResponsePacket.ResponseData);
uint16_t TotalResponseSize = 0;
/* Search through the global UUID list an item at a time */
for (uint8_t CurrTableItem = 0; CurrTableItem < (sizeof(SDP_Services_Table) / sizeof(void*)); CurrTableItem++)
{
/* Read in a pointer to the current UUID table entry's Attribute table */
ServiceAttributeTable_t* CurrAttributeTable = pgm_read_ptr(&SDP_Services_Table[CurrTableItem]);
/* Retrieve a PROGMEM pointer to the value of the Service Record Handle */
const void* ServiceRecord = SDP_GetAttributeValue(CurrAttributeTable, SDP_ATTRIBUTE_ID_SERVICERECORDHANDLE);
/* Get the size of the header for the Service Record Handle */
uint8_t AttrHeaderSize;
SDP_GetLocalAttributeContainerSize(ServiceRecord, &AttrHeaderSize);
/* Retrieve the endian-swapped service handle of the current service being examined */
uint32_t CurrServiceHandle = SwapEndian_32(pgm_read_dword(ServiceRecord + AttrHeaderSize));
/* Check if the current service in the service table has the requested service handle */
if (ServiceHandle == CurrServiceHandle)
{
/* Add the listed attributes for the found UUID to the response */
TotalResponseSize = SDP_AddListedAttributesToResponse(CurrAttributeTable, AttributeList, TotalAttributes,
&CurrResponsePos);
/* Requested service found, abort the search through the service table */
break;
}
}
/* Continuation state - always zero */
SDP_WriteData8(&CurrResponsePos, 0);
/* Set the total response list size to the size of the outer container plus its header size and continuation state */
ResponsePacket.AttributeListByteCount = SwapEndian_16(TotalResponseSize);
/* Calculate the total parameter length that is to be sent, including the fixed return parameters, the created attribute
value list and the SDP continuation state */
uint16_t ParamLength = (sizeof(ResponsePacket.AttributeListByteCount) + TotalResponseSize + sizeof(uint8_t));
/* Fill in the response packet's header */
ResponsePacket.SDPHeader.PDU = SDP_PDU_SERVICEATTRIBUTERESPONSE;
ResponsePacket.SDPHeader.TransactionID = SDPHeader->TransactionID;
ResponsePacket.SDPHeader.ParameterLength = SwapEndian_16(ParamLength);
BT_SDP_DEBUG(1, ">> Service Attribute Response");
BT_SDP_DEBUG(2, "-- Param Len 0x%04X", ParamLength);
/* Send the completed response packet to the sender */
Bluetooth_SendPacket(&ResponsePacket, (sizeof(ResponsePacket.SDPHeader) + ParamLength), Channel);
}
/** Handler for the XPROG WRITE_MEMORY command to write to a specific memory space within the attached device. */
static void XPROGProtocol_WriteMemory(void)
{
uint8_t ReturnStatus = XPRG_ERR_OK;
struct
{
uint8_t MemoryType;
uint8_t PageMode;
uint32_t Address;
uint16_t Length;
uint8_t ProgData[256];
} WriteMemory_XPROG_Params;
Endpoint_Read_Stream_LE(&WriteMemory_XPROG_Params, (sizeof(WriteMemory_XPROG_Params) -
sizeof(WriteMemory_XPROG_Params).ProgData), NO_STREAM_CALLBACK);
WriteMemory_XPROG_Params.Address = SwapEndian_32(WriteMemory_XPROG_Params.Address);
WriteMemory_XPROG_Params.Length = SwapEndian_16(WriteMemory_XPROG_Params.Length);
Endpoint_Read_Stream_LE(&WriteMemory_XPROG_Params.ProgData, WriteMemory_XPROG_Params.Length, NO_STREAM_CALLBACK);
Endpoint_ClearOUT();
Endpoint_SelectEndpoint(AVRISP_DATA_IN_EPNUM);
Endpoint_SetEndpointDirection(ENDPOINT_DIR_IN);
if (XPROG_SelectedProtocol == XPRG_PROTOCOL_PDI)
{
/* Assume FLASH page programming by default, as it is the common case */
uint8_t WriteCommand = XMEGA_NVM_CMD_WRITEFLASHPAGE;
uint8_t WriteBuffCommand = XMEGA_NVM_CMD_LOADFLASHPAGEBUFF;
uint8_t EraseBuffCommand = XMEGA_NVM_CMD_ERASEFLASHPAGEBUFF;
bool PagedMemory = true;
switch (WriteMemory_XPROG_Params.MemoryType)
{
case XPRG_MEM_TYPE_APPL:
WriteCommand = XMEGA_NVM_CMD_WRITEAPPSECPAGE;
break;
case XPRG_MEM_TYPE_BOOT:
WriteCommand = XMEGA_NVM_CMD_WRITEBOOTSECPAGE;
break;
case XPRG_MEM_TYPE_EEPROM:
WriteCommand = XMEGA_NVM_CMD_ERASEWRITEEEPROMPAGE;
WriteBuffCommand = XMEGA_NVM_CMD_LOADEEPROMPAGEBUFF;
EraseBuffCommand = XMEGA_NVM_CMD_ERASEEEPROMPAGEBUFF;
break;
case XPRG_MEM_TYPE_USERSIG:
/* User signature is paged, but needs us to manually indicate the mode bits since the host doesn't set them */
WriteMemory_XPROG_Params.PageMode = (XPRG_PAGEMODE_ERASE | XPRG_PAGEMODE_WRITE);
WriteCommand = XMEGA_NVM_CMD_WRITEUSERSIG;
break;
case XPRG_MEM_TYPE_FUSE:
WriteCommand = XMEGA_NVM_CMD_WRITEFUSE;
PagedMemory = false;
break;
case XPRG_MEM_TYPE_LOCKBITS:
WriteCommand = XMEGA_NVM_CMD_WRITELOCK;
PagedMemory = false;
break;
}
/* Send the appropriate memory write commands to the device, indicate timeout if occurred */
if ((PagedMemory && !(XMEGANVM_WritePageMemory(WriteBuffCommand, EraseBuffCommand, WriteCommand,
WriteMemory_XPROG_Params.PageMode, WriteMemory_XPROG_Params.Address,
WriteMemory_XPROG_Params.ProgData, WriteMemory_XPROG_Params.Length))) ||
(!PagedMemory && !(XMEGANVM_WriteByteMemory(WriteCommand, WriteMemory_XPROG_Params.Address,
WriteMemory_XPROG_Params.ProgData[0]))))
{
ReturnStatus = XPRG_ERR_TIMEOUT;
}
}
else
{
/* Send write command to the TPI device, indicate timeout if occurred */
if (!(TINYNVM_WriteMemory(WriteMemory_XPROG_Params.Address, WriteMemory_XPROG_Params.ProgData,
WriteMemory_XPROG_Params.Length)))
{
ReturnStatus = XPRG_ERR_TIMEOUT;
}
}
Endpoint_Write_Byte(CMD_XPROG);
Endpoint_Write_Byte(XPRG_CMD_WRITE_MEM);
Endpoint_Write_Byte(ReturnStatus);
Endpoint_ClearIN();
}
/*
FAT_BOOT_RECORD fatBootData =
{
.bootstrap = {0xeb, 0x3c, 0x90},
.OEM = "OpenPCR ",
.iBytesPerSector = 512,
.iSectorsPerCluster = 1,
.iReservedSectors = 1,
.iFATs = 2,
.iRootEntries = 16,
.iTotalSectors = 128,
.iMediaDescr = 0xf0,
.iSectorsPerFAT = 1,
.iSectorsPerTrack = 0,
.iHeads = 0,
.iHiddenSectors = 0,
.iTotalSectorsEx = 0,
.iLogicDriveNumber = 0,
.extSignature = 0x29,
.serialNumber = USE_INTERNAL_SERIAL,
.volumeLabel = "No Name ",
.fatName = "FAT16 ",
.exeCode = "",
.exeEndMarker = {0x55, 0xaa}
};
FAT_BOOT_RECORD fatBootData =
{
.bootstrap = {0xeb, 0x3c, 0x90},
.OEM = "OpenPCR",
.iBytesPerSector = 512,
.iSectorsPerCluster = 1,
.iReservedSectors = 1,
.iFATs = 2,
.iRootEntries = 512,
.iTotalSectors = 128,
.iMediaDescr = 0xf0,
.iSectorsPerFAT = 64,
.iSectorsPerTrack = 0,
.iHeads = 0,
.iHiddenSectors = 0,
.iTotalSectorsEx = 0,
.iLogicDriveNumber = 0,
.extSignature = 0x29,
.serialNumber = USE_INTERNAL_SERIAL,
.volumeLabel = {'N', 'o', ' ', 'N', 'a', 'm', 'e'},
.fatName = {'F', 'A', 'T', '1', '6'},
.exeCode = {},
.exeEndMarker = {0x55, 0xaa}
};
*/
static void SCSI_Command_ReadWrite_10(USB_ClassInfo_MS_Device_t* const MSInterfaceInfo,
const bool IsDataRead)
{
uint32_t BlockAddress = SwapEndian_32(*(uint32_t*)&MSInterfaceInfo->State.CommandBlock.SCSICommandData[2]);
uint16_t TotalBlocks = SwapEndian_16(*(uint16_t*)&MSInterfaceInfo->State.CommandBlock.SCSICommandData[7]);
/* Check if the block address is outside the maximum allowable value for the LUN */
if (BlockAddress >= LUN_MEDIA_BLOCKS)
{
// Block address is invalid, update SENSE key and return command fail
SCSI_SET_SENSE(SCSI_SENSE_KEY_ILLEGAL_REQUEST,
SCSI_ASENSE_LOGICAL_BLOCK_ADDRESS_OUT_OF_RANGE,
SCSI_ASENSEQ_NO_QUALIFIER);
return;
}
#if (TOTAL_LUNS > 1)
/* Adjust the given block address to the real media address based on the selected LUN */
BlockAddress += ((uint32_t)MSInterfaceInfo->State.CommandBlock.LUN * LUN_MEDIA_BLOCKS);
#endif
#if 0
/* Determine if the packet is a READ (10) or WRITE (10) command, call appropriate function */
if (IsDataRead == DATA_READ){
int block_index = 0;
while (TotalBlocks){
if (BlockAddress == 0){ //BOOT Record
Endpoint_Write_Stream_LE(&fatBootData, sizeof(FAT_BOOT_RECORD), NO_STREAM_CALLBACK);
}
else if (BlockAddress == 1 || BlockAddress == 2){ //FAT TABLE 1 and 2
uint16_t firstBlock = 0xfff0;
uint16_t secondBlock = 0xffff;
Endpoint_Write_Stream_BE(&firstBlock, sizeof(firstBlock), NO_STREAM_CALLBACK);
Endpoint_Write_Stream_BE(&secondBlock, sizeof(secondBlock), NO_STREAM_CALLBACK);
for (block_index=4; block_index<VIRTUAL_MEMORY_BLOCK_SIZE; block_index++){
Endpoint_Write_Byte(0x00);
}
}
else{
for (block_index=0; block_index<VIRTUAL_MEMORY_BLOCK_SIZE; block_index++){
Endpoint_Write_Byte(0x00);
}
}
BlockAddress++;
TotalBlocks--;
}
}
Endpoint_ClearIN();
#endif
/*
if (IsDataRead == DATA_READ)
DataflashManager_ReadBlocks(MSInterfaceInfo, BlockAddress, TotalBlocks);
else
DataflashManager_WriteBlocks(MSInterfaceInfo, BlockAddress, TotalBlocks);
*/
/* Update the bytes transferred counter and succeed the command */
MSInterfaceInfo->State.CommandBlock.DataTransferLength -= ((uint32_t)TotalBlocks * VIRTUAL_MEMORY_BLOCK_SIZE);
}
