diff options
-rw-r--r-- | hw/usb_cfs_client_common.c | 52 | ||||
-rw-r--r-- | hw/usb_client_common.c | 7 |
2 files changed, 47 insertions, 12 deletions
diff --git a/hw/usb_cfs_client_common.c b/hw/usb_cfs_client_common.c index f423d22..b3ddee2 100644 --- a/hw/usb_cfs_client_common.c +++ b/hw/usb_cfs_client_common.c @@ -421,8 +421,7 @@ static int cfs_get_current_gadget(struct usb_client *usb, if (!usb) return -EINVAL; - cfs_client = container_of(usb, struct cfs_client, - client); + cfs_client = container_of(usb, struct cfs_client, client); usb_gadget = zalloc(sizeof(*usb_gadget)); if (!usb_gadget) @@ -445,7 +444,6 @@ static int cfs_get_current_gadget(struct usb_client *usb, if (ret) goto free_strs; - n_funcs = cfs_count_functions(cfs_client->gadget); usb_funcs = calloc(n_funcs + 1, sizeof(*usb_funcs)); if (!usb_funcs) @@ -835,27 +833,65 @@ out: static int cfs_enable(struct usb_client *usb) { + int i; + int ret; + struct usb_gadget *gadget; struct cfs_client *cfs_client; + struct usb_function_with_service *fws; if (!usb) return -EINVAL; - cfs_client = container_of(usb, struct cfs_client, - client); + cfs_client = container_of(usb, struct cfs_client, client); + ret = usbg_enable_gadget(cfs_client->gadget, cfs_client->udc); + if (ret) + return ret; + + ret = cfs_get_current_gadget(usb, &gadget); + if (ret) { + usbg_disable_gadget(cfs_client->gadget); + return ret; + } + + for (i = 0; gadget->funcs[i]; ++i) { + if (gadget->funcs[i]->function_group != USB_FUNCTION_GROUP_WITH_POST_SERVICE) + continue; - return usbg_enable_gadget(cfs_client->gadget, cfs_client->udc); + fws = container_of(gadget->funcs[i], struct usb_function_with_service, func); + (void)systemd_start_unit_wait_started(fws->service, ".service", -1); + } + + cfs_free_gadget(gadget); + + return 0; } static int cfs_disable(struct usb_client *usb) { + int i; + int ret; + struct usb_gadget *gadget; struct cfs_client *cfs_client; + struct usb_function_with_service *fws; if (!usb) return -EINVAL; - cfs_client = container_of(usb, struct cfs_client, - client); + ret = cfs_get_current_gadget(usb, &gadget); + if (ret) + return ret; + + for (i = 0; gadget->funcs[i]; ++i) { + if (gadget->funcs[i]->function_group != USB_FUNCTION_GROUP_WITH_POST_SERVICE) + continue; + + fws = container_of(gadget->funcs[i], struct usb_function_with_service, func); + (void)systemd_stop_unit_wait_stopped(fws->service, ".service", -1); + } + + cfs_free_gadget(gadget); + cfs_client = container_of(usb, struct cfs_client, client); return usbg_disable_gadget(cfs_client->gadget); } diff --git a/hw/usb_client_common.c b/hw/usb_client_common.c index 95b815b..dc08bde 100644 --- a/hw/usb_client_common.c +++ b/hw/usb_client_common.c @@ -558,9 +558,8 @@ static int legacy_enable(struct usb_client *usb) if (gadget->funcs[i]->function_group == USB_FUNCTION_GROUP_SIMPLE) continue; - fws = container_of(gadget->funcs[i], - struct usb_function_with_service, func); - systemd_start_unit_wait_started(fws->service, ".service", -1); + fws = container_of(gadget->funcs[i], struct usb_function_with_service, func); + (void)systemd_start_unit_wait_started(fws->service, ".service", -1); } legacy_free_gadget(gadget); @@ -587,7 +586,7 @@ static int legacy_disable(struct usb_client *usb) continue; fws = container_of(gadget->funcs[i], struct usb_function_with_service, func); - systemd_stop_unit_wait_stopped(fws->service, ".service", -1); + (void)systemd_stop_unit_wait_stopped(fws->service, ".service", -1); } ret = sys_set_str(LEGACY_ENABLE_PATH, LEGACY_DISABLE); |