2 files changed, 43 insertions, 12 deletions

diff --git a/lib/usb/usb_private.h b/lib/usb/usb_private.h
index 81466dfa..4d8f7ed4 100644
--- a/lib/usb/usb_private.h
+++ b/lib/usb/usb_private.h
@@ -89,6 +89,9 @@ struct _usbd_device {
 	void (*user_callback_set_config[MAX_USER_SET_CONFIG_CALLBACK])
 				(usbd_device *usbd_dev, uint16_t wValue);
 
+	void (*user_callback_set_altsetting)(usbd_device *usbd_dev,
+					     uint16_t wIndex, uint16_t wValue);
+
 	const struct _usbd_driver *driver;
 
 	/* private driver data */
diff --git a/lib/usb/usb_standard.c b/lib/usb/usb_standard.c
index 885f077b..8bdc5c9d 100644
--- a/lib/usb/usb_standard.c
+++ b/lib/usb/usb_standard.c
@@ -56,6 +56,13 @@ int usbd_register_set_config_callback(usbd_device *usbd_dev,
 	return -1;
 }
 
+void usbd_register_set_altsetting_callback(usbd_device *usbd_dev,
+					   void (*callback)(usbd_device *usbd_dev,
+							    uint16_t wIndex, uint16_t wValue))
+{
+	usbd_dev->user_callback_set_altsetting = callback;
+}
+
 static uint16_t build_config_descriptor(usbd_device *usbd_dev,
 				   uint8_t index, uint8_t *buf, uint16_t len)
 {
@@ -294,32 +301,53 @@ static int usb_standard_set_interface(usbd_device *usbd_dev,
 				      struct usb_setup_data *req,
 				      uint8_t **buf, uint16_t *len)
 {
-	(void)usbd_dev;
-	(void)req;
+	const struct usb_config_descriptor *cfx = &usbd_dev->config[usbd_dev->current_config - 1];
+	const struct usb_interface *iface;
+
 	(void)buf;
 
-	/* FIXME: Adapt if we have more than one interface. */
-	if (req->wValue != 0) {
-		return 0;
+	if (req->wIndex >= cfx->bNumInterfaces) {
+		return USBD_REQ_NOTSUPP;
+	}
+
+	iface = &cfx->interface[req->wIndex];
+
+	if (req->wValue >= iface->num_altsetting) {
+		return USBD_REQ_NOTSUPP;
+	}
+
+	if (iface->cur_altsetting) {
+		*iface->cur_altsetting = req->wValue;
+	} else if (req->wValue > 0) {
+		return USBD_REQ_NOTSUPP;
 	}
+
+	if (usbd_dev->user_callback_set_altsetting) {
+			usbd_dev->user_callback_set_altsetting(usbd_dev,
+							req->wIndex, req->wValue);
+	}
+
 	*len = 0;
 
-	return 1;
+	return USBD_REQ_HANDLED;
 }
 
 static int usb_standard_get_interface(usbd_device *usbd_dev,
 				      struct usb_setup_data *req,
 				      uint8_t **buf, uint16_t *len)
 {
-	(void)usbd_dev;
-	(void)req;
-	(void)buf;
+	uint8_t *cur_altsetting;
+	const struct usb_config_descriptor *cfx = &usbd_dev->config[usbd_dev->current_config - 1];
+
+	if (req->wIndex >= cfx->bNumInterfaces) {
+		return USBD_REQ_NOTSUPP;
+	}
 
-	/* FIXME: Adapt if we have more than one interface. */
 	*len = 1;
-	(*buf)[0] = 0;
+	cur_altsetting = cfx->interface[req->wIndex].cur_altsetting;
+	(*buf)[0] = (cur_altsetting) ? *cur_altsetting : 0;
 
-	return 1;
+	return USBD_REQ_HANDLED;
 }
 
 static int usb_standard_device_get_status(usbd_device *usbd_dev,