From 901f920f6da6b51cf361f416c6c319732fecb9a6 Mon Sep 17 00:00:00 2001 From: dragonmux Date: Thu, 11 Aug 2022 23:45:32 +0100 Subject: [PATCH] usb/microsoft: Implemented support for sending more of the descriptor set tree --- lib/usb/usb_microsoft.c | 67 +++++++++++++++++++++++++++++++++++++++-- 1 file changed, 65 insertions(+), 2 deletions(-) diff --git a/lib/usb/usb_microsoft.c b/lib/usb/usb_microsoft.c index 9ba90e9f..ae1b63da 100644 --- a/lib/usb/usb_microsoft.c +++ b/lib/usb/usb_microsoft.c @@ -17,16 +17,79 @@ * along with this library. If not, see . */ +#include #include #include #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; }