/** Decodes an Ethernet frame header and prints its contents to through the USART in a human readable format.
*
* \param[in] InDataStart Pointer to the start of an Ethernet frame of data
*/
void DecodeEthernetFrameHeader(void* InDataStart)
{
#if !defined(NO_DECODE_ETHERNET)
Ethernet_Frame_Header_t* FrameHeader = (Ethernet_Frame_Header_t*)InDataStart;
printf_P(PSTR("\r\n"));
printf_P(PSTR(" ETHERNET\r\n"));
if (!(MAC_COMPARE(&FrameHeader->Destination, &ServerMACAddress)) &&
!(MAC_COMPARE(&FrameHeader->Destination, &BroadcastMACAddress)))
{
printf_P(PSTR(" + NOT ADDRESSED TO DEVICE\r\n"));
return;
}
printf_P(PSTR(" + MAC Source : %02X:%02X:%02X:%02X:%02X:%02X\r\n"), FrameHeader->Source.Octets[0],
FrameHeader->Source.Octets[1],
FrameHeader->Source.Octets[2],
FrameHeader->Source.Octets[3],
FrameHeader->Source.Octets[4],
FrameHeader->Source.Octets[5]);
printf_P(PSTR(" + MAC Dest: %02X:%02X:%02X:%02X:%02X:%02X\r\n"), FrameHeader->Destination.Octets[0],
FrameHeader->Destination.Octets[1],
FrameHeader->Destination.Octets[2],
FrameHeader->Destination.Octets[3],
FrameHeader->Destination.Octets[4],
FrameHeader->Destination.Octets[5]);
printf_P(PSTR(" + Protocol: 0x%04x\r\n"), SwapEndian_16(FrameHeader->EtherType));
#endif
}
/** 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)
*
* \return Boolean true if the command completed successfully, false otherwise.
*/
static bool 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 false;
}
#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);
return true;
}
/** Decodes an IP header and prints its contents to through the USART in a human readable format.
*
* \param[in] InDataStart Pointer to the start of an IP packet header
*/
void DecodeIPHeader(void* InDataStart)
{
#if !defined(NO_DECODE_IP)
IP_Header_t* IPHeader = (IP_Header_t*)InDataStart;
uint16_t HeaderLengthBytes = (IPHeader->HeaderLength * sizeof(uint32_t));
printf_P(PSTR(" \\\r\n IP\r\n"));
if (!(IP_COMPARE(&IPHeader->DestinationAddress, &ServerIPAddress)))
{
printf_P(PSTR(" + NOT ADDRESSED TO DEVICE\r\n"));
return;
}
printf_P(PSTR(" + Header Length: %u Bytes\r\n"), HeaderLengthBytes);
printf_P(PSTR(" + Packet Version: %u\r\n"), IPHeader->Version);
printf_P(PSTR(" + Total Length: %u\r\n"), SwapEndian_16(IPHeader->TotalLength));
printf_P(PSTR(" + Protocol: %u\r\n"), IPHeader->Protocol);
printf_P(PSTR(" + TTL: %u\r\n"), IPHeader->TTL);
printf_P(PSTR(" + IP Src: %u.%u.%u.%u\r\n"), IPHeader->SourceAddress.Octets[0],
IPHeader->SourceAddress.Octets[1],
IPHeader->SourceAddress.Octets[2],
IPHeader->SourceAddress.Octets[3]);
printf_P(PSTR(" + IP Dst: %u.%u.%u.%u\r\n"), IPHeader->DestinationAddress.Octets[0],
IPHeader->DestinationAddress.Octets[1],
IPHeader->DestinationAddress.Octets[2],
IPHeader->DestinationAddress.Octets[3]);
#endif
}
/** Command processing for an issued SCSI INQUIRY command. This command returns information about the device's features
* and capabilities to the host.
*
* \param[in] MSInterfaceInfo Pointer to the Mass Storage class interface structure that the command is associated with
*
* \return Boolean true if the command completed successfully, false otherwise.
*/
static bool SCSI_Command_Inquiry(USB_ClassInfo_MS_Device_t* const MSInterfaceInfo)
{
uint16_t AllocationLength = SwapEndian_16(*(uint16_t*)&MSInterfaceInfo->State.CommandBlock.SCSICommandData[3]);
uint16_t BytesTransferred = MIN(AllocationLength, sizeof(InquiryData));
/* Only the standard INQUIRY data is supported, check if any optional INQUIRY bits set */
if ((MSInterfaceInfo->State.CommandBlock.SCSICommandData[1] & ((1 << 0) | (1 << 1))) ||
MSInterfaceInfo->State.CommandBlock.SCSICommandData[2])
{
/* Optional but unsupported bits set - update the SENSE key and fail the request */
SCSI_SET_SENSE(SCSI_SENSE_KEY_ILLEGAL_REQUEST,
SCSI_ASENSE_INVALID_FIELD_IN_CDB,
SCSI_ASENSEQ_NO_QUALIFIER);
return false;
}
Endpoint_Write_Stream_LE(&InquiryData, BytesTransferred, NULL);
/* Pad out remaining bytes with 0x00 */
Endpoint_Null_Stream((AllocationLength - BytesTransferred), NULL);
/* Finalize the stream transfer to send the last packet */
Endpoint_ClearIN();
/* Succeed the command and update the bytes transferred counter */
MSInterfaceInfo->State.CommandBlock.DataTransferLength -= BytesTransferred;
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] 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 INQUIRY command. This command returns information about the device's features
* and capabilities to the host.
*
* \return Boolean true if the command completed successfully, false otherwise.
*/
static bool SCSI_Command_Inquiry(void)
{
uint16_t AllocationLength = SwapEndian_16(*(uint16_t*)&CommandBlock.SCSICommandData[3]);
uint16_t BytesTransferred = (AllocationLength < sizeof(InquiryData))? AllocationLength :
sizeof(InquiryData);
/* Only the standard INQUIRY data is supported, check if any optional INQUIRY bits set */
if ((CommandBlock.SCSICommandData[1] & ((1 << 0) | (1 << 1))) ||
CommandBlock.SCSICommandData[2])
{
/* Optional but unsupported bits set - update the SENSE key and fail the request */
SCSI_SET_SENSE(SCSI_SENSE_KEY_ILLEGAL_REQUEST,
SCSI_ASENSE_INVALID_FIELD_IN_CDB,
SCSI_ASENSEQ_NO_QUALIFIER);
return false;
}
/* Write the INQUIRY data to the endpoint */
Endpoint_Write_Stream_LE(&InquiryData, BytesTransferred, StreamCallback_AbortOnMassStoreReset);
uint8_t PadBytes[AllocationLength - BytesTransferred];
/* Pad out remaining bytes with 0x00 */
Endpoint_Write_Stream_LE(&PadBytes, sizeof(PadBytes), StreamCallback_AbortOnMassStoreReset);
/* Finalize the stream transfer to send the last packet */
Endpoint_ClearIN();
/* Succeed the command and update the bytes transferred counter */
CommandBlock.DataTransferLength -= BytesTransferred;
return true;
}
/** Decodes an ARP header and prints its contents to through the USART in a human readable format.
*
* \param[in] InDataStart Pointer to the start of an ARP packet header
*/
void DecodeARPHeader(void* InDataStart)
{
#if !defined(NO_DECODE_ARP)
ARP_Header_t* ARPHeader = (ARP_Header_t*)InDataStart;
printf_P(PSTR(" \\\r\n ARP\r\n"));
if (!(IP_COMPARE(&ARPHeader->TPA, &ServerIPAddress)) &&
!(MAC_COMPARE(&ARPHeader->THA, &ServerMACAddress)))
{
printf_P(PSTR(" + NOT ADDRESSED TO DEVICE\r\n"));
return;
}
printf_P(PSTR(" + Protocol: %x\r\n"), SwapEndian_16(ARPHeader->ProtocolType));
printf_P(PSTR(" + Operation: %u\r\n"), SwapEndian_16(ARPHeader->Operation));
if (SwapEndian_16(ARPHeader->ProtocolType) == ETHERTYPE_IPV4)
{
printf_P(PSTR(" + SHA MAC: %02X:%02X:%02X:%02X:%02X:%02X\r\n"), ARPHeader->SHA.Octets[0],
ARPHeader->SHA.Octets[1],
ARPHeader->SHA.Octets[2],
ARPHeader->SHA.Octets[3],
ARPHeader->SHA.Octets[4],
ARPHeader->SHA.Octets[5]);
printf_P(PSTR(" + SPA IP: %u.%u.%u.%u\r\n"), ARPHeader->SPA.Octets[0],
ARPHeader->SPA.Octets[1],
ARPHeader->SPA.Octets[2],
ARPHeader->SPA.Octets[3]);
printf_P(PSTR(" + THA MAC: %02X:%02X:%02X:%02X:%02X:%02X\r\n"), ARPHeader->THA.Octets[0],
ARPHeader->THA.Octets[1],
ARPHeader->THA.Octets[2],
ARPHeader->THA.Octets[3],
ARPHeader->THA.Octets[4],
ARPHeader->THA.Octets[5]);
printf_P(PSTR(" + TPA IP: %u.%u.%u.%u\r\n"), ARPHeader->TPA.Octets[0],
ARPHeader->TPA.Octets[1],
ARPHeader->TPA.Octets[2],
ARPHeader->TPA.Octets[3]);
}
#endif
}
/** Processes an ARP packet inside an Ethernet frame, and writes the appropriate response
* to the output Ethernet frame if the host is requesting the IP or MAC address of the
* virtual server device on the network.
*
* \param[in] InDataStart Pointer to the start of the incoming packet's ARP header
* \param[out] OutDataStart Pointer to the start of the outgoing packet's ARP header
*
* \return The number of bytes written to the out Ethernet frame if any, NO_RESPONSE otherwise
*/
int16_t ARP_ProcessARPPacket(void* InDataStart,
void* OutDataStart)
{
DecodeARPHeader(InDataStart);
ARP_Header_t* ARPHeaderIN = (ARP_Header_t*)InDataStart;
ARP_Header_t* ARPHeaderOUT = (ARP_Header_t*)OutDataStart;
/* Ensure that the ARP request is a IPv4 request packet */
if ((SwapEndian_16(ARPHeaderIN->ProtocolType) == ETHERTYPE_IPV4) &&
(SwapEndian_16(ARPHeaderIN->Operation) == ARP_OPERATION_REQUEST))
{
/* If the ARP packet is requesting the MAC or IP of the virtual webserver, return the response */
if (IP_COMPARE(&ARPHeaderIN->TPA, &ServerIPAddress) ||
MAC_COMPARE(&ARPHeaderIN->THA, &ServerMACAddress))
{
/* Fill out the ARP response header */
ARPHeaderOUT->HardwareType = ARPHeaderIN->HardwareType;
ARPHeaderOUT->ProtocolType = ARPHeaderIN->ProtocolType;
ARPHeaderOUT->HLEN = ARPHeaderIN->HLEN;
ARPHeaderOUT->PLEN = ARPHeaderIN->PLEN;
ARPHeaderOUT->Operation = SwapEndian_16(ARP_OPERATION_REPLY);
/* Copy over the sender MAC/IP to the target fields for the response */
ARPHeaderOUT->THA = ARPHeaderIN->SHA;
ARPHeaderOUT->TPA = ARPHeaderIN->SPA;
/* Copy over the new sender MAC/IP - MAC and IP addresses of the virtual webserver */
ARPHeaderOUT->SHA = ServerMACAddress;
ARPHeaderOUT->SPA = ServerIPAddress;
/* Return the size of the response so far */
return sizeof(ARP_Header_t);
}
}
return NO_RESPONSE;
}
/** 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
}
/** 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 an Ethernet frame header and prints its contents to through the USART in a human readable format.
*
* \param[in] FrameINData Pointer to the start of an Ethernet frame information structure
*/
void DecodeEthernetFrameHeader(Ethernet_Frame_Info_t* FrameINData)
{
#if !defined(NO_DECODE_ETHERNET)
Ethernet_Frame_Header_t* FrameHeader = (Ethernet_Frame_Header_t*)FrameINData->FrameData;
printf_P(PSTR("\r\n"));
printf_P(PSTR(" ETHERNET\r\n"));
printf_P(PSTR(" + Frame Size: %u\r\n"), FrameINData->FrameLength);
if (!(MAC_COMPARE(&FrameHeader->Destination, &ServerMACAddress)) &&
!(MAC_COMPARE(&FrameHeader->Destination, &BroadcastMACAddress)))
{
printf_P(PSTR(" + NOT ADDRESSED TO DEVICE\r\n"));
return;
}
printf_P(PSTR(" + MAC Source : %02X:%02X:%02X:%02X:%02X:%02X\r\n"), FrameHeader->Source.Octets[0],
FrameHeader->Source.Octets[1],
FrameHeader->Source.Octets[2],
FrameHeader->Source.Octets[3],
FrameHeader->Source.Octets[4],
FrameHeader->Source.Octets[5]);
printf_P(PSTR(" + MAC Dest: %02X:%02X:%02X:%02X:%02X:%02X\r\n"), FrameHeader->Destination.Octets[0],
FrameHeader->Destination.Octets[1],
FrameHeader->Destination.Octets[2],
FrameHeader->Destination.Octets[3],
FrameHeader->Destination.Octets[4],
FrameHeader->Destination.Octets[5]);
if (SwapEndian_16(FrameINData->FrameLength) > ETHERNET_VER2_MINSIZE)
printf_P(PSTR(" + Protocol: 0x%04x\r\n"), SwapEndian_16(FrameHeader->EtherType));
else
printf_P(PSTR(" + Protocol: UNKNOWN E1\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();
}
/** Adds all the Attributes in the given service table to the response that appear in the Attribute table.
*
* \param[in] AttributeTable Pointer to an Attribute table for the service to examine
* \param[in] AttributeList Pointer to a list of Attribute ranges
* \param[in] TotalAttributes Number of Attributes stored in the Attribute list
* \param[out] BufferPos Pointer to the output buffer position where the retrieved attributes are to be stored
*
* \return Number of bytes added to the output buffer
*/
static uint16_t SDP_AddListedAttributesToResponse(const ServiceAttributeTable_t* AttributeTable,
uint16_t AttributeList[][2],
const uint8_t TotalAttributes,
void** const BufferPos)
{
uint16_t TotalResponseSize;
/* Add an inner Data Element Sequence header for the current services's found Attributes */
uint16_t* AttributeListSize = SDP_AddSequence16(BufferPos);
/* Search through the list of Attributes one at a time looking for values in the current UUID's Attribute table */
for (uint8_t CurrAttribute = 0; CurrAttribute < TotalAttributes; CurrAttribute++)
{
uint16_t* AttributeIDRange = AttributeList[CurrAttribute];
void* AttributeValue;
/* Look through the current service's attribute list, examining all the attributes */
while ((AttributeValue = pgm_read_ptr(&AttributeTable->Data)) != NULL)
{
/* Get the current Attribute's ID from the current attribute table entry */
uint16_t CurrAttributeID = pgm_read_word(&AttributeTable->AttributeID);
/* Check if the current Attribute's ID is within the current Attribute range */
if ((CurrAttributeID >= AttributeIDRange[0]) && (CurrAttributeID <= AttributeIDRange[1]))
{
/* Increment the current UUID's returned Attribute container size by the number of added bytes */
*AttributeListSize += SDP_AddAttributeToResponse(CurrAttributeID, AttributeValue, BufferPos);
}
AttributeTable++;
}
}
/* Record the total number of added bytes to the buffer */
TotalResponseSize = 3 + *AttributeListSize;
/* Fix endianness of the added attribute data element sequence */
*AttributeListSize = SwapEndian_16(*AttributeListSize);
return TotalResponseSize;
}
/** Main Service Discovery Protocol packet processing routine. This function processes incoming SDP packets from
* a connected Bluetooth device, and sends back appropriate responses to allow other devices to determine the
* services the local device exposes.
*
* \param[in] Data Incoming packet data containing the SDP request
* \param[in] Channel ACL channel the request was issued to by the remote device
*/
void SDP_ProcessPacket(void* Data, Bluetooth_Channel_t* const Channel)
{
SDP_PDUHeader_t* SDPHeader = (SDP_PDUHeader_t*)Data;
SDPHeader->ParameterLength = SwapEndian_16(SDPHeader->ParameterLength);
BT_SDP_DEBUG(1, "SDP Packet Received");
BT_SDP_DEBUG(2, "-- PDU ID: 0x%02X", SDPHeader->PDU);
BT_SDP_DEBUG(2, "-- Param Length: 0x%04X", SDPHeader->ParameterLength);
/* Dispatch to the correct processing routine for the given SDP packet type */
switch (SDPHeader->PDU)
{
case SDP_PDU_SERVICESEARCHREQUEST:
SDP_ProcessServiceSearch(SDPHeader, Channel);
break;
case SDP_PDU_SERVICEATTRIBUTEREQUEST:
SDP_ProcessServiceAttribute(SDPHeader, Channel);
break;
case SDP_PDU_SERVICESEARCHATTRIBUTEREQUEST:
SDP_ProcessServiceSearchAttribute(SDPHeader, Channel);
break;
}
}
/** Issues a Printer class Get Device ID command to the attached device, to retrieve the device ID string (which indicates
* the accepted printer languages, the printer's model and other pertinent information).
*
* \param[out] DeviceIDString Pointer to the destination where the returned string should be stored
* \param[in] BufferSize Size in bytes of the allocated buffer for the returned Device ID string
*
* \return A value from the USB_Host_SendControlErrorCodes_t enum
*/
uint8_t Printer_GetDeviceID(char* DeviceIDString,
const uint16_t BufferSize)
{
uint8_t ErrorCode;
uint16_t DeviceIDStringLength = 0;
USB_ControlRequest = (USB_Request_Header_t)
{
.bmRequestType = (REQDIR_DEVICETOHOST | REQTYPE_CLASS | REQREC_INTERFACE),
.bRequest = PRNT_REQ_GetDeviceID,
.wValue = 0,
.wIndex = PrinterInterfaceNumber,
.wLength = sizeof(DeviceIDStringLength),
};
Pipe_SelectPipe(PIPE_CONTROLPIPE);
if ((ErrorCode = USB_Host_SendControlRequest(&DeviceIDStringLength)) != HOST_SENDCONTROL_Successful)
return ErrorCode;
if (!(DeviceIDStringLength))
{
DeviceIDString[0] = 0x00;
return HOST_SENDCONTROL_Successful;
}
DeviceIDStringLength = SwapEndian_16(DeviceIDStringLength);
if (DeviceIDStringLength > BufferSize)
DeviceIDStringLength = BufferSize;
USB_ControlRequest.wLength = DeviceIDStringLength;
if ((ErrorCode = USB_Host_SendControlRequest(DeviceIDString)) != HOST_SENDCONTROL_Successful)
return ErrorCode;
/* Move string back two characters to remove the string length value from the start of the array */
memmove(&DeviceIDString[0], &DeviceIDString[2], DeviceIDStringLength - 2);
DeviceIDString[DeviceIDStringLength - 2] = 0x00;
return HOST_SENDCONTROL_Successful;
}
/** Issues a Printer class Get Port Status command to the attached device, to retrieve the current status flags of the
* printer.
*
* \param[out] PortStatus Pointer to the destination where the printer's status flag values should be stored
*
* \return A value from the USB_Host_SendControlErrorCodes_t enum
*/
uint8_t Printer_GetPortStatus(uint8_t* const PortStatus)
{
USB_ControlRequest = (USB_Request_Header_t)
{
.bmRequestType = (REQDIR_DEVICETOHOST | REQTYPE_CLASS | REQREC_INTERFACE),
.bRequest = PRNT_REQ_GetPortStatus,
.wValue = 0,
.wIndex = PrinterInterfaceNumber,
.wLength = sizeof(uint8_t),
};
Pipe_SelectPipe(PIPE_CONTROLPIPE);
return USB_Host_SendControlRequest(PortStatus);
}
/** Issues a Printer class Soft Reset command to the attached device, to reset the printer ready for new input without
* physically cycling the printer's power.
*
* \return A value from the USB_Host_SendControlErrorCodes_t enum
*/
uint8_t Printer_SoftReset(void)
{
USB_ControlRequest = (USB_Request_Header_t)
{
.bmRequestType = (REQDIR_HOSTTODEVICE | REQTYPE_CLASS | REQREC_INTERFACE),
.bRequest = PRNT_REQ_SoftReset,
.wValue = 0,
.wIndex = PrinterInterfaceNumber,
.wLength = 0,
};
Pipe_SelectPipe(PIPE_CONTROLPIPE);
return USB_Host_SendControlRequest(NULL);
}
/** Handler for the CMD_PROGRAM_FLASH_ISP and CMD_PROGRAM_EEPROM_ISP commands, writing out bytes,
* words or pages of data to the attached device.
*
* \param[in] V2Command Issued V2 Protocol command byte from the host
*/
void ISPProtocol_ProgramMemory(uint8_t V2Command)
{
struct
{
uint16_t BytesToWrite;
uint8_t ProgrammingMode;
uint8_t DelayMS;
uint8_t ProgrammingCommands[3];
uint8_t PollValue1;
uint8_t PollValue2;
uint8_t ProgData[256]; // Note, the Jungo driver has a very short ACK timeout period, need to buffer the
} Write_Memory_Params; // whole page and ACK the packet as fast as possible to prevent it from aborting
Endpoint_Read_Stream_LE(&Write_Memory_Params, (sizeof(Write_Memory_Params) -
sizeof(Write_Memory_Params.ProgData)), NO_STREAM_CALLBACK);
Write_Memory_Params.BytesToWrite = SwapEndian_16(Write_Memory_Params.BytesToWrite);
if (Write_Memory_Params.BytesToWrite > sizeof(Write_Memory_Params.ProgData))
{
Endpoint_ClearOUT();
Endpoint_SelectEndpoint(AVRISP_DATA_IN_EPNUM);
Endpoint_SetEndpointDirection(ENDPOINT_DIR_IN);
Endpoint_Write_Byte(V2Command);
Endpoint_Write_Byte(STATUS_CMD_FAILED);
Endpoint_ClearIN();
return;
}
Endpoint_Read_Stream_LE(&Write_Memory_Params.ProgData, Write_Memory_Params.BytesToWrite, NO_STREAM_CALLBACK);
Endpoint_ClearOUT();
Endpoint_SelectEndpoint(AVRISP_DATA_IN_EPNUM);
Endpoint_SetEndpointDirection(ENDPOINT_DIR_IN);
uint8_t ProgrammingStatus = STATUS_CMD_OK;
uint16_t PollAddress = 0;
uint8_t PollValue = (V2Command == CMD_PROGRAM_FLASH_ISP) ? Write_Memory_Params.PollValue1 :
Write_Memory_Params.PollValue2;
uint8_t* NextWriteByte = Write_Memory_Params.ProgData;
/* Check the programming mode desired by the host, either Paged or Word memory writes */
if (Write_Memory_Params.ProgrammingMode & PROG_MODE_PAGED_WRITES_MASK)
{
uint16_t StartAddress = (CurrentAddress & 0xFFFF);
/* Check to see if we need to send a LOAD EXTENDED ADDRESS command to the target */
if (MustLoadExtendedAddress)
{
ISPTarget_LoadExtendedAddress();
MustLoadExtendedAddress = false;
}
/* Paged mode memory programming */
for (uint16_t CurrentByte = 0; CurrentByte < Write_Memory_Params.BytesToWrite; CurrentByte++)
{
bool IsOddByte = (CurrentByte & 0x01);
uint8_t ByteToWrite = *(NextWriteByte++);
ISPTarget_SendByte(Write_Memory_Params.ProgrammingCommands[0]);
ISPTarget_SendByte(CurrentAddress >> 8);
ISPTarget_SendByte(CurrentAddress & 0xFF);
ISPTarget_SendByte(ByteToWrite);
/* AVR FLASH addressing requires us to modify the write command based on if we are writing a high
* or low byte at the current word address */
if (V2Command == CMD_PROGRAM_FLASH_ISP)
Write_Memory_Params.ProgrammingCommands[0] ^= READ_WRITE_HIGH_BYTE_MASK;
/* Check to see the write completion method, to see if we have a valid polling address */
if (!(PollAddress) && (ByteToWrite != PollValue))
{
if (IsOddByte && (V2Command == CMD_PROGRAM_FLASH_ISP))
Write_Memory_Params.ProgrammingCommands[2] |= READ_WRITE_HIGH_BYTE_MASK;
PollAddress = (CurrentAddress & 0xFFFF);
}
/* EEPROM increments the address on each byte, flash needs to increment on each word */
if (IsOddByte || (V2Command == CMD_PROGRAM_EEPROM_ISP))
CurrentAddress++;
}
/* If the current page must be committed, send the PROGRAM PAGE command to the target */
if (Write_Memory_Params.ProgrammingMode & PROG_MODE_COMMIT_PAGE_MASK)
{
ISPTarget_SendByte(Write_Memory_Params.ProgrammingCommands[1]);
ISPTarget_SendByte(StartAddress >> 8);
ISPTarget_SendByte(StartAddress & 0xFF);
ISPTarget_SendByte(0x00);
/* Check if polling is possible and enabled, if not switch to timed delay mode */
if (!(PollAddress) && (Write_Memory_Params.ProgrammingMode & PROG_MODE_PAGED_VALUE_MASK))
{
Write_Memory_Params.ProgrammingMode &= ~PROG_MODE_PAGED_VALUE_MASK;
Write_Memory_Params.ProgrammingMode |= PROG_MODE_PAGED_TIMEDELAY_MASK;
}
ProgrammingStatus = ISPTarget_WaitForProgComplete(Write_Memory_Params.ProgrammingMode, PollAddress, PollValue,
Write_Memory_Params.DelayMS, Write_Memory_Params.ProgrammingCommands[2]);
/* Check to see if the FLASH address has crossed the extended address boundary */
if ((V2Command == CMD_PROGRAM_FLASH_ISP) && !(CurrentAddress & 0xFFFF))
MustLoadExtendedAddress = true;
}
/*
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);
}
/** 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();
}
/** Internal processing routine for SDP Service Search 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_ProcessServiceSearchAttribute(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 Search Attribute");
/* Retrieve the list of search UUIDs from the request */
uint8_t UUIDList[12][UUID_SIZE_BYTES];
uint8_t TotalUUIDs = SDP_GetUUIDList(UUIDList, &CurrentParameter);
BT_SDP_DEBUG(2, "-- Total UUIDs: %d", TotalUUIDs);
/* 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);
/* Add the outer Data Element Sequence header for all of the retrieved Attributes */
uint16_t* TotalResponseSize = SDP_AddSequence16(&CurrResponsePos);
/* Search through the global service 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]);
if (!(SDP_SearchServiceTable(UUIDList, TotalUUIDs, CurrAttributeTable)))
continue;
BT_SDP_DEBUG(2, " -- Found search match in table");
/* Add the listed attributes for the found UUID to the response */
*TotalResponseSize += SDP_AddListedAttributesToResponse(CurrAttributeTable, AttributeList, TotalAttributes,
&CurrResponsePos);
}
/* 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(3 + *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) +
(3 + *TotalResponseSize) +
sizeof(uint8_t));
/* Flip the endianness of the container's size */
*TotalResponseSize = SwapEndian_16(*TotalResponseSize);
/* Fill in the response packet's header */
ResponsePacket.SDPHeader.PDU = SDP_PDU_SERVICESEARCHATTRIBUTERESPONSE;
ResponsePacket.SDPHeader.TransactionID = SDPHeader->TransactionID;
ResponsePacket.SDPHeader.ParameterLength = SwapEndian_16(ParamLength);
BT_SDP_DEBUG(1, ">> Service Search 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);
}
/** Internal processing routine for SDP Service Search 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_ProcessServiceSearch(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 Search");
/* Retrieve the list of search UUIDs from the request */
uint8_t UUIDList[12][UUID_SIZE_BYTES];
uint8_t TotalUUIDs = SDP_GetUUIDList(UUIDList, &CurrentParameter);
BT_SDP_DEBUG(2, "-- Total UUIDs: %d", TotalUUIDs);
/* Retrieve the maximum service record response count from the request */
uint16_t MaxServiceRecordCount = SDP_ReadData16(&CurrentParameter);
BT_SDP_DEBUG(2, "-- Max Return Service Count: 0x%04X", MaxServiceRecordCount);
struct
{
SDP_PDUHeader_t SDPHeader;
uint16_t TotalServiceRecordCount;
uint16_t CurrentServiceRecordCount;
uint8_t ResponseData[100];
} ResponsePacket;
uint8_t AddedServiceHandles = 0;
/* Create a pointer to the buffer to indicate the current location for response data to be added */
void* CurrResponsePos = ResponsePacket.ResponseData;
/* Search through the global service 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]);
if (!(SDP_SearchServiceTable(UUIDList, TotalUUIDs, CurrAttributeTable)))
continue;
BT_SDP_DEBUG(2, " -- Found search match in table");
/* Retrieve a PROGMEM pointer to the value of the service's record handle */
const void* AttributeValue = SDP_GetAttributeValue(CurrAttributeTable, SDP_ATTRIBUTE_ID_SERVICERECORDHANDLE);
/* Copy over the service record handle to the response list */
uint8_t AttrHeaderSize;
uint8_t AttrSize = SDP_GetLocalAttributeContainerSize(AttributeValue, &AttrHeaderSize);
memcpy_P(CurrResponsePos, AttributeValue + AttrHeaderSize, AttrSize);
CurrResponsePos += AttrHeaderSize + AttrSize;
AddedServiceHandles++;
}
/* Continuation state - always zero */
SDP_WriteData8(&CurrResponsePos, 0);
/* Fill out the service record count values in the returned packet */
ResponsePacket.TotalServiceRecordCount = SwapEndian_16(AddedServiceHandles);
ResponsePacket.CurrentServiceRecordCount = ResponsePacket.TotalServiceRecordCount;
/* Calculate the total parameter length that is to be sent, including the fixed return parameters, the created service
handle list and the SDP continuation state */
uint16_t ParamLength = (ResponsePacket.CurrentServiceRecordCount << 2) +
sizeof(ResponsePacket.CurrentServiceRecordCount) +
sizeof(ResponsePacket.TotalServiceRecordCount) +
sizeof(uint8_t);
/* Fill in the response packet's header */
ResponsePacket.SDPHeader.PDU = SDP_PDU_SERVICESEARCHRESPONSE;
ResponsePacket.SDPHeader.TransactionID = SDPHeader->TransactionID;
ResponsePacket.SDPHeader.ParameterLength = SwapEndian_16(ParamLength);
BT_SDP_DEBUG(1, ">> Service Search Response");
/* 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 = 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();
}
/** Processes an IP packet inside an Ethernet frame, and writes the appropriate response
* to the output Ethernet frame if one is created by a sub-protocol handler.
*
* \param[in] InDataStart Pointer to the start of the incoming packet's IP header
* \param[out] OutDataStart Pointer to the start of the outgoing packet's IP header
*
* \return The number of bytes written to the out Ethernet frame if any, NO_RESPONSE if no
* response was generated, NO_PROCESS if the packet processing was deferred until the
* next Ethernet packet handler iteration
*/
int16_t IP_ProcessIPPacket(void* InDataStart,
void* OutDataStart)
{
DecodeIPHeader(InDataStart);
IP_Header_t* IPHeaderIN = (IP_Header_t*)InDataStart;
IP_Header_t* IPHeaderOUT = (IP_Header_t*)OutDataStart;
/* Header length is specified in number of longs in the packet header, convert to bytes */
uint16_t HeaderLengthBytes = (IPHeaderIN->HeaderLength * sizeof(uint32_t));
int16_t RetSize = NO_RESPONSE;
/* Check to ensure the IP packet is addressed to the virtual webserver's IP or the broadcast IP address */
if (!(IP_COMPARE(&IPHeaderIN->DestinationAddress, &ServerIPAddress)) &&
!(IP_COMPARE(&IPHeaderIN->DestinationAddress, &BroadcastIPAddress)))
{
return NO_RESPONSE;
}
/* Pass off the IP payload to the appropriate protocol processing routine */
switch (IPHeaderIN->Protocol)
{
case PROTOCOL_ICMP:
RetSize = ICMP_ProcessICMPPacket(&((uint8_t*)InDataStart)[HeaderLengthBytes],
&((uint8_t*)OutDataStart)[sizeof(IP_Header_t)]);
break;
case PROTOCOL_TCP:
RetSize = TCP_ProcessTCPPacket(InDataStart,
&((uint8_t*)InDataStart)[HeaderLengthBytes],
&((uint8_t*)OutDataStart)[sizeof(IP_Header_t)]);
break;
case PROTOCOL_UDP:
RetSize = UDP_ProcessUDPPacket(InDataStart,
&((uint8_t*)InDataStart)[HeaderLengthBytes],
&((uint8_t*)OutDataStart)[sizeof(IP_Header_t)]);
break;
}
/* Check to see if the protocol processing routine has filled out a response */
if (RetSize > 0)
{
/* Fill out the response IP packet header */
IPHeaderOUT->TotalLength = SwapEndian_16(sizeof(IP_Header_t) + RetSize);
IPHeaderOUT->TypeOfService = 0;
IPHeaderOUT->HeaderLength = (sizeof(IP_Header_t) / sizeof(uint32_t));
IPHeaderOUT->Version = 4;
IPHeaderOUT->Flags = 0;
IPHeaderOUT->FragmentOffset = 0;
IPHeaderOUT->Identification = 0;
IPHeaderOUT->HeaderChecksum = 0;
IPHeaderOUT->Protocol = IPHeaderIN->Protocol;
IPHeaderOUT->TTL = DEFAULT_TTL;
IPHeaderOUT->SourceAddress = IPHeaderIN->DestinationAddress;
IPHeaderOUT->DestinationAddress = IPHeaderIN->SourceAddress;
IPHeaderOUT->HeaderChecksum = Ethernet_Checksum16(IPHeaderOUT, sizeof(IP_Header_t));
/* Return the size of the response so far */
return (sizeof(IP_Header_t) + RetSize);
}
return RetSize;
}
/** Handler for the CMD_PROGRAM_FLASH_ISP and CMD_PROGRAM_EEPROM_ISP commands, writing out bytes,
* words or pages of data to the attached device.
*
* \param[in] V2Command Issued V2 Protocol command byte from the host
*/
void ISPProtocol_ProgramMemory(uint8_t V2Command)
{
struct
{
uint16_t BytesToWrite;
uint8_t ProgrammingMode;
uint8_t DelayMS;
uint8_t ProgrammingCommands[3];
uint8_t PollValue1;
uint8_t PollValue2;
uint8_t ProgData[256]; // Note, the Jungo driver has a very short ACK timeout period, need to buffer the
} Write_Memory_Params; // whole page and ACK the packet as fast as possible to prevent it from aborting
Endpoint_Read_Stream_LE(&Write_Memory_Params, (sizeof(Write_Memory_Params) -
sizeof(Write_Memory_Params.ProgData)), NULL);
Write_Memory_Params.BytesToWrite = SwapEndian_16(Write_Memory_Params.BytesToWrite);
if (Write_Memory_Params.BytesToWrite > sizeof(Write_Memory_Params.ProgData))
{
Endpoint_ClearOUT();
Endpoint_SelectEndpoint(AVRISP_DATA_IN_EPADDR);
Endpoint_SetEndpointDirection(ENDPOINT_DIR_IN);
Endpoint_Write_8(V2Command);
Endpoint_Write_8(STATUS_CMD_FAILED);
Endpoint_ClearIN();
return;
}
Endpoint_Read_Stream_LE(&Write_Memory_Params.ProgData, Write_Memory_Params.BytesToWrite, 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(Write_Memory_Params) - sizeof(Write_Memory_Params.ProgData)) +
Write_Memory_Params.BytesToWrite) % AVRISP_DATA_EPSIZE == 0)
{
Endpoint_ClearOUT();
Endpoint_WaitUntilReady();
}
Endpoint_ClearOUT();
Endpoint_SelectEndpoint(AVRISP_DATA_IN_EPADDR);
Endpoint_SetEndpointDirection(ENDPOINT_DIR_IN);
uint8_t ProgrammingStatus = STATUS_CMD_OK;
uint8_t PollValue = (V2Command == CMD_PROGRAM_FLASH_ISP) ? Write_Memory_Params.PollValue1 :
Write_Memory_Params.PollValue2;
uint16_t PollAddress = 0;
uint8_t* NextWriteByte = Write_Memory_Params.ProgData;
uint16_t PageStartAddress = (CurrentAddress & 0xFFFF);
for (uint16_t CurrentByte = 0; CurrentByte < Write_Memory_Params.BytesToWrite; CurrentByte++)
{
uint8_t ByteToWrite = *(NextWriteByte++);
uint8_t ProgrammingMode = Write_Memory_Params.ProgrammingMode;
/* Check to see if we need to send a LOAD EXTENDED ADDRESS command to the target */
if (MustLoadExtendedAddress)
{
ISPTarget_LoadExtendedAddress();
MustLoadExtendedAddress = false;
}
ISPTarget_SendByte(Write_Memory_Params.ProgrammingCommands[0]);
ISPTarget_SendByte(CurrentAddress >> 8);
ISPTarget_SendByte(CurrentAddress & 0xFF);
ISPTarget_SendByte(ByteToWrite);
/* AVR FLASH addressing requires us to modify the write command based on if we are writing a high
* or low byte at the current word address */
if (V2Command == CMD_PROGRAM_FLASH_ISP)
Write_Memory_Params.ProgrammingCommands[0] ^= READ_WRITE_HIGH_BYTE_MASK;
/* Check to see if we have a valid polling address */
if (!(PollAddress) && (ByteToWrite != PollValue))
{
if ((CurrentByte & 0x01) && (V2Command == CMD_PROGRAM_FLASH_ISP))
Write_Memory_Params.ProgrammingCommands[2] |= READ_WRITE_HIGH_BYTE_MASK;
else
Write_Memory_Params.ProgrammingCommands[2] &= ~READ_WRITE_HIGH_BYTE_MASK;
PollAddress = (CurrentAddress & 0xFFFF);
}
/* If in word programming mode, commit the byte to the target's memory */
if (!(ProgrammingMode & PROG_MODE_PAGED_WRITES_MASK))
{
/* If the current polling address is invalid, switch to timed delay write completion mode */
if (!(PollAddress) && !(ProgrammingMode & PROG_MODE_WORD_READYBUSY_MASK))
ProgrammingMode = (ProgrammingMode & ~PROG_MODE_WORD_VALUE_MASK) | PROG_MODE_WORD_TIMEDELAY_MASK;
ProgrammingStatus = ISPTarget_WaitForProgComplete(ProgrammingMode, PollAddress, PollValue,
Write_Memory_Params.DelayMS,
Write_Memory_Params.ProgrammingCommands[2]);
/* Abort the programming loop early if the byte/word programming failed */
if (ProgrammingStatus != STATUS_CMD_OK)
break;
/* Must reset the polling address afterwards, so it is not erroneously used for the next byte */
PollAddress = 0;
}
/* EEPROM just increments the address each byte, flash needs to increment on each word and
* also check to ensure that a LOAD EXTENDED ADDRESS command is issued each time the extended
* address boundary has been crossed during FLASH memory programming */
if ((CurrentByte & 0x01) || (V2Command == CMD_PROGRAM_EEPROM_ISP))
{
CurrentAddress++;
if ((V2Command == CMD_PROGRAM_FLASH_ISP) && !(CurrentAddress & 0xFFFF))
MustLoadExtendedAddress = true;
}
}
/* If the current page must be committed, send the PROGRAM PAGE command to the target */
if (Write_Memory_Params.ProgrammingMode & PROG_MODE_COMMIT_PAGE_MASK)
{
ISPTarget_SendByte(Write_Memory_Params.ProgrammingCommands[1]);
ISPTarget_SendByte(PageStartAddress >> 8);
ISPTarget_SendByte(PageStartAddress & 0xFF);
ISPTarget_SendByte(0x00);
/* Check if polling is enabled and possible, if not switch to timed delay mode */
if ((Write_Memory_Params.ProgrammingMode & PROG_MODE_PAGED_VALUE_MASK) && !(PollAddress))
{
Write_Memory_Params.ProgrammingMode = (Write_Memory_Params.ProgrammingMode & ~PROG_MODE_PAGED_VALUE_MASK) |
PROG_MODE_PAGED_TIMEDELAY_MASK;
}
ProgrammingStatus = ISPTarget_WaitForProgComplete(Write_Memory_Params.ProgrammingMode, PollAddress, PollValue,
Write_Memory_Params.DelayMS,
Write_Memory_Params.ProgrammingCommands[2]);
/* Check to see if the FLASH address has crossed the extended address boundary */
if ((V2Command == CMD_PROGRAM_FLASH_ISP) && !(CurrentAddress & 0xFFFF))
MustLoadExtendedAddress = true;
}
