diff --git a/include/libopencm3/usb/usbd.h b/include/libopencm3/usb/usbd.h
index 04b22c84..96d2c1d5 100644
--- a/include/libopencm3/usb/usbd.h
+++ b/include/libopencm3/usb/usbd.h
@@ -139,7 +139,8 @@ extern int usbd_register_control_callback(usbd_device *usbd_dev, uint8_t type,
 
 /* <usb_standard.c> */
 /** Registers a "Set Config" callback
- * @return 0 if successful
+ * @return 0 if successful or already existed.
+ * @return -1 if no more space was available for callbacks.
  */
 extern int usbd_register_set_config_callback(usbd_device *usbd_dev,
 					  usbd_set_config_callback callback);
diff --git a/lib/usb/usb_standard.c b/lib/usb/usb_standard.c
index fbe89d05..4ce7a8b1 100644
--- a/lib/usb/usb_standard.c
+++ b/lib/usb/usb_standard.c
@@ -46,6 +46,9 @@ int usbd_register_set_config_callback(usbd_device *usbd_dev,
 
 	for (i = 0; i < MAX_USER_SET_CONFIG_CALLBACK; i++) {
 		if (usbd_dev->user_callback_set_config[i]) {
+			if (usbd_dev->user_callback_set_config[i] == callback) {
+				return 0;
+			}
 			continue;
 		}