diff --git a/lib/usb/usb_private.h b/lib/usb/usb_private.h index 6d226681..81466dfa 100644 --- a/lib/usb/usb_private.h +++ b/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; diff --git a/lib/usb/usb_standard.c b/lib/usb/usb_standard.c index 0cf7f86e..3b0dac9a 100644 --- a/lib/usb/usb_standard.c +++ b/lib/usb/usb_standard.c @@ -39,11 +39,21 @@ LGPL License Terms @ref lgpl_license #include #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;