Allow more than one usbd_register_set_config_callback

In a composite device if one want to separate code
for each interface, usbd_register_set_config_callback
can now register more than one callback.

Signed-off-by: Franck Jullien <franck.jullien@gmail.com>

lib/usb/usb_private.h

@@ -39,6 +39,7 @@ LGPL License Terms @ref lgpl_license
 #define __USB_PRIVATE_H
 
 #define MAX_USER_CONTROL_CALLBACK	4
+#define MAX_USER_SET_CONFIG_CALLBACK	4
 
 #define MIN(a, b) ((a) < (b) ? (a) : (b))
 
@@ -85,8 +86,8 @@ struct _usbd_device {
 	void (*user_callback_ctr[8][3])(usbd_device *usbd_dev, uint8_t ea);
 
 	/* User callback function for some standard USB function hooks */
-	void (*user_callback_set_config)(usbd_device *usbd_dev,
-					 uint16_t wValue);
+	void (*user_callback_set_config[MAX_USER_SET_CONFIG_CALLBACK])
+				(usbd_device *usbd_dev, uint16_t wValue);
 
 	const struct _usbd_driver *driver;
 

lib/usb/usb_standard.c

@@ -39,11 +39,21 @@ LGPL License Terms @ref lgpl_license
 #include <libopencm3/usb/usbd.h>
 #include "usb_private.h"
 
-void usbd_register_set_config_callback(usbd_device *usbd_dev,
+int usbd_register_set_config_callback(usbd_device *usbd_dev,
 				       void (*callback)(usbd_device *usbd_dev,
 				       uint16_t wValue))
 {
-	usbd_dev->user_callback_set_config = callback;
+	int i;
+
+	for (i = 0; i < MAX_USER_SET_CONFIG_CALLBACK; i++) {
+		if (usbd_dev->user_callback_set_config[i])
+			continue;
+
+		usbd_dev->user_callback_set_config[i] = callback;
+		return 0;
+	}
+
+	return -1;
 }
 
 static uint16_t build_config_descriptor(usbd_device *usbd_dev,
@@ -244,7 +254,12 @@ static int usb_standard_set_configuration(usbd_device *usbd_dev,
 			usbd_dev->user_control_callback[i].cb = NULL;
 		}
 
-		usbd_dev->user_callback_set_config(usbd_dev, req->wValue);
+		for (i = 0; i < MAX_USER_SET_CONFIG_CALLBACK; i++) {
+			if (usbd_dev->user_callback_set_config[i]) {
+				usbd_dev->user_callback_set_config[i](usbd_dev,
+								req->wValue);
+			}
+		}
 	}
 
 	return 1;