diff --git a/cmd/usb_mass_storage.c b/cmd/usb_mass_storage.c
index d565635c6c..cf2f55994e 100644
--- a/cmd/usb_mass_storage.c
+++ b/cmd/usb_mass_storage.c
@@ -170,7 +170,7 @@ static int do_usb_mass_storage(struct cmd_tbl *cmdtp, int flag,
 		goto cleanup_ums_init;
 	}
 
-	rc = fsg_init(ums, ums_count);
+	rc = fsg_init(ums, ums_count, controller_index);
 	if (rc) {
 		pr_err("fsg_init failed\n");
 		rc = CMD_RET_FAILURE;
diff --git a/drivers/usb/gadget/f_mass_storage.c b/drivers/usb/gadget/f_mass_storage.c
index 439a31c255..45f0504b6e 100644
--- a/drivers/usb/gadget/f_mass_storage.c
+++ b/drivers/usb/gadget/f_mass_storage.c
@@ -435,6 +435,7 @@ static void set_bulk_out_req_length(struct fsg_common *common,
 static struct ums *ums;
 static int ums_count;
 static struct fsg_common *the_fsg_common;
+static unsigned int controller_index;
 
 static int fsg_set_halt(struct fsg_dev *fsg, struct usb_ep *ep)
 {
@@ -679,7 +680,7 @@ static int sleep_thread(struct fsg_common *common)
 			k = 0;
 		}
 
-		usb_gadget_handle_interrupts(0);
+		usb_gadget_handle_interrupts(controller_index);
 	}
 	common->thread_wakeup_needed = 0;
 	return rc;
@@ -2764,10 +2765,11 @@ int fsg_add(struct usb_configuration *c)
 	return fsg_bind_config(c->cdev, c, fsg_common);
 }
 
-int fsg_init(struct ums *ums_devs, int count)
+int fsg_init(struct ums *ums_devs, int count, unsigned int controller_idx)
 {
 	ums = ums_devs;
 	ums_count = count;
+	controller_index = controller_idx;
 
 	return 0;
 }
diff --git a/include/usb_mass_storage.h b/include/usb_mass_storage.h
index c7b770fa3e..08ccc97cf2 100644
--- a/include/usb_mass_storage.h
+++ b/include/usb_mass_storage.h
@@ -25,7 +25,7 @@ struct ums {
 	struct blk_desc block_dev;
 };
 
-int fsg_init(struct ums *ums_devs, int count);
+int fsg_init(struct ums *ums_devs, int count, unsigned int controller_idx);
 void fsg_cleanup(void);
 int fsg_main_thread(void *);
 int fsg_add(struct usb_configuration *c);