From 258a343e215d9d8afbddb82b1f9467610df8c3d6 Mon Sep 17 00:00:00 2001
From: CTCaer <ctcaer@gmail.com>
Date: Tue, 14 Jun 2022 18:48:21 +0300
Subject: [PATCH] bdk: usb: support deconfiguration of endpoints

TODO: Signal that to userspace and manage it.
---
 bdk/usb/usbd.c  | 10 ++++++++++
 bdk/usb/xusbd.c | 15 +++++++++++----
 2 files changed, 21 insertions(+), 4 deletions(-)

diff --git a/bdk/usb/usbd.c b/bdk/usb/usbd.c
index ad5fafa..e6be7fb 100644
--- a/bdk/usb/usbd.c
+++ b/bdk/usb/usbd.c
@@ -1056,6 +1056,16 @@ static int _usbd_handle_set_request(bool *ep_stall)
 		if (!res)
 		{
 			usbd_otg->config_num = usbd_otg->control_setup.wValue;
+
+			// Remove configuration.
+			if (!usbd_otg->config_num)
+			{
+				//! TODO: Signal that to userspace.
+				_usb_reset_disable_ep1();
+				return res;
+			}
+
+			// Initialize configuration.
 			_usbd_initialize_ep_ctrl(USB_EP_BULK_OUT);
 			_usbd_initialize_ep_ctrl(USB_EP_BULK_IN);
 			usbd_otg->configuration_set = true;
diff --git a/bdk/usb/xusbd.c b/bdk/usb/xusbd.c
index ca6cb01..b3a9905 100644
--- a/bdk/usb/xusbd.c
+++ b/bdk/usb/xusbd.c
@@ -1608,9 +1608,18 @@ static void _xusb_handle_set_request_dev_address(usb_ctrl_setup_t *ctrl_setup)
 
 static void _xusb_handle_set_request_configuration(usb_ctrl_setup_t *ctrl_setup)
 {
-	u32 config_num = ctrl_setup->wValue;
-	if (!config_num) //TODO! we can change device_state here.
+	usbd_xotg->config_num = ctrl_setup->wValue;
+
+	// Remove configuration.
+	if (!usbd_xotg->config_num)
+	{
+		//! TODO: Signal that to userspace.
+		_xusb_disable_ep1();
+
+		_xusb_issue_status_trb(USB_DIR_IN);
+
 		return;
+	}
 
 	// Initialize BULK EPs.
 	_xusbd_ep_initialize(USB_EP_BULK_OUT);
@@ -1621,8 +1630,6 @@ static void _xusb_handle_set_request_configuration(usb_ctrl_setup_t *ctrl_setup)
 	XUSB_DEV_XHCI(XUSB_DEV_XHCI_ST)   |= XHCI_ST_RC;
 
 	_xusb_issue_status_trb(USB_DIR_IN);
-
-	usbd_xotg->config_num = config_num;
 	usbd_xotg->device_state = XUSB_CONFIGURED_STS_WAIT;
 }