usb/microsoft: Implemented support for sending more of the descriptor set tree

lib/usb/usb_microsoft.c

@@ -17,16 +17,79 @@
  * along with this library.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include <string.h>
 #include <libopencm3/usb/usbd.h>
 #include <libopencm3/usb/microsoft.h>
 #include "usb_private.h"
 
+static uint16_t build_function_subset(const microsoft_os_descriptor_function_subset_header *const subset,
+	uint8_t *const buf, uint16_t len)
+{
+	uint16_t count = MIN(len, subset->wLength);
+	memcpy(buf, subset, count);
+	uint16_t total_length = subset->wLength;
+
+	((microsoft_os_descriptor_function_subset_header *)buf)->wTotalLength = total_length;
+	return total_length;
+}
+
+static uint16_t build_config_subset(const microsoft_os_descriptor_config_subset_header *const subset,
+	uint8_t *const buf, uint16_t len)
+{
+	uint16_t total = 0;
+	uint16_t count = MIN(len, subset->wLength);
+	memcpy(buf, subset, count);
+	len -= count;
+	total += count;
+	uint16_t total_length = subset->wLength;
+
+	for (size_t i = 0; i < subset->num_function_subset_headers; ++i) {
+		const uint16_t subset_header_len = build_function_subset(&subset->function_subset_headers[i],
+			buf + total, len);
+		count = MIN(len, subset_header_len);
+		len -= count;
+		total += count;
+		total_length += subset_header_len;
+	}
+
+	((microsoft_os_descriptor_config_subset_header *)buf)->wTotalLength = total_length;
+	return total_length;
+}
+
+static uint16_t build_descriptor_set(const microsoft_os_descriptor_set_header *const set,
+	uint8_t *const buf, uint16_t len)
+{
+	uint16_t count = MIN(len, set->wLength);
+	memcpy(buf, set, count);
+	len -= count;
+	uint16_t total = count;
+	uint16_t total_length = set->wLength;
+
+	for (size_t i = 0; i < set->num_config_subset_headers; ++i) {
+		const uint16_t subset_header_len = build_config_subset(&set->config_subset_headers[i], buf + total, len);
+		count = MIN(len, subset_header_len);
+		len -= count;
+		total += count;
+		total_length += subset_header_len;
+	}
+
+	((microsoft_os_descriptor_set_header *)buf)->wTotalLength = total_length;
+	return total;
+}
+
 static enum usbd_request_return_codes microsoft_os_get_descriptor_set(usbd_device *const usbd_dev,
 	struct usb_setup_data *const req, uint8_t **const buf, uint16_t *const len)
 {
-	const microsoft_os_descriptor_set_header *sets = usbd_dev->microsoft_os_descriptor_sets;
+	if (req->wValue != 0)
+		return USBD_REQ_NOTSUPP;
+
+	const microsoft_os_descriptor_set_header *const sets = usbd_dev->microsoft_os_descriptor_sets;
 	for (size_t i = 0; i < usbd_dev->num_microsoft_os_descriptor_sets; ++i) {
-		//
+		if (sets[i].vendor_code == req->bRequest) {
+			*buf = usbd_dev->ctrl_buf;
+			*len = build_descriptor_set(&sets[i], *buf, *len);
+			return USBD_REQ_HANDLED;
+		}
 	}
 	return USBD_REQ_NOTSUPP;
 }