DFU: Resolve DFU runtime descriptor dynamically from descriptors

We cannoy simply use the DFU runtime descriptor of the DFU mode, but we
have to use the descriptor of the specific currently-selected runtime
configuration.  Let's iterate over the descriptors of a configuration
and find the DFU runtime descriptor in it.

1 files changed, 25 insertions, 1 deletions

diff --git a/firmware/atmel_softpack_libraries/usb/device/dfu/dfu_runtime.c b/firmware/atmel_softpack_libraries/usb/device/dfu/dfu_runtime.c
index 537605c..4d8330f 100644
--- a/firmware/atmel_softpack_libraries/usb/device/dfu/dfu_runtime.c
+++ b/firmware/atmel_softpack_libraries/usb/device/dfu/dfu_runtime.c
@@ -22,6 +22,7 @@
  */
 
 #include <board.h>
+#include <assert.h>
 #include <core_cm3.h>
 
 #include <usb/include/USBD.h>
@@ -67,6 +68,26 @@ static void __dfufunc handle_getstate(void)
 	USBD_Write(0, (char *)&u8, sizeof(u8), NULL, 0);
 }
 
+static const uint8_t *get_dfu_func_desc(void)
+{
+	USBDDriver *usbdDriver = USBD_GetDriver();
+	const USBConfigurationDescriptor *cfg_desc;
+	const USBGenericDescriptor *gen_desc;
+
+	if (USBD_IsHighSpeed())
+		cfg_desc = &usbdDriver->pDescriptors->pHsConfiguration[usbdDriver->cfgnum];
+	else
+		cfg_desc = usbdDriver->pDescriptors->pFsConfiguration[usbdDriver->cfgnum];
+
+	for (gen_desc = (const USBGenericDescriptor *) cfg_desc;
+	     (const uint8_t *) gen_desc < (const uint8_t *) cfg_desc + cfg_desc->wTotalLength;
+	     gen_desc = (const USBGenericDescriptor *) ((const uint8_t *)gen_desc + gen_desc->bLength)) {
+		if (gen_desc->bDescriptorType == USB_DT_DFU)
+			return (const uint8_t *) gen_desc;
+	}
+	return NULL;
+}
+
 static void TerminateCtrlInWithNull(void *pArg,
                                     unsigned char status,
                                     unsigned long int transferred,
@@ -100,15 +121,18 @@ void USBDFU_Runtime_RequestHandler(const USBGenericRequest *request)
 	    USBGetDescriptorRequest_GetDescriptorType(request) == USB_DT_DFU) {
 		uint16_t length = sizeof(struct usb_dfu_func_descriptor);
 		const USBDeviceDescriptor *pDevice;
+		const uint8_t *dfu_func_desc = get_dfu_func_desc();
 		int terminateWithNull;
 
+		ASSERT(dfu_func_desc);
+
 		if (USBD_IsHighSpeed())
 			pDevice = usbdDriver->pDescriptors->pHsDevice;
 		else
 			pDevice = usbdDriver->pDescriptors->pFsDevice;
 
 		terminateWithNull = ((length % pDevice->bMaxPacketSize0) == 0);
-		USBD_Write(0, &dfu_cfg_descriptor.func_dfu, length,
+		USBD_Write(0, dfu_func_desc, length,
 			   terminateWithNull ? TerminateCtrlInWithNull : 0, 0);
 		return;
 	}