diff --git a/src/device/usbd.c b/src/device/usbd.c
index b2fd54575..2735e35fd 100644
--- a/src/device/usbd.c
+++ b/src/device/usbd.c
@@ -495,10 +495,13 @@ bool tud_deinit(uint8_t rhport) {
 }
 
 static void configuration_reset(uint8_t rhport) {
+  // Save setup_count for restore
+  uint8_t setup_count =  _usbd_dev.setup_count;
   // Clear mounted status first to ensure tud_ready() return false before driver clean up.
   tu_varclr(&_usbd_dev);
   memset(_usbd_dev.itf2drv, DRVID_INVALID, sizeof(_usbd_dev.itf2drv)); // invalid mapping
   memset(_usbd_dev.ep2drv, DRVID_INVALID, sizeof(_usbd_dev.ep2drv)); // invalid mapping
+  _usbd_dev.setup_count = setup_count;
 
   for (uint8_t i = 0; i < TOTAL_DRIVER_COUNT; i++) {
     usbd_class_driver_t const* driver = get_driver(i);