diff --git a/payloads/libpayload/drivers/usb/usb.c b/payloads/libpayload/drivers/usb/usb.c index 1f21e6a994..1e8f24814c 100644 --- a/payloads/libpayload/drivers/usb/usb.c +++ b/payloads/libpayload/drivers/usb/usb.c @@ -205,22 +205,29 @@ set_configuration (usbdev_t *dev) dev->controller->control (dev, OUT, sizeof (dr), &dr, 0, 0); } +void +clear_feature (usbdev_t *dev, int endp, int feature, int rtype) +{ + dev_req_t dr; + + dr.bmRequestType = rtype; + dr.data_dir = host_to_device; + dr.bRequest = CLEAR_FEATURE; + dr.wValue = feature; + dr.wIndex = endp; + dr.wLength = 0; + dev->controller->control (dev, OUT, sizeof (dr), &dr, 0, 0); +} + int clear_stall (endpoint_t *ep) { usbdev_t *dev = ep->dev; int endp = ep->endpoint; - dev_req_t dr; + int rtype = gen_bmRequestType (host_to_device, standard_type, + endp ? endp_recp : dev_recp); - dr.bmRequestType = 0; - if (endp != 0) { - dr.req_recp = endp_recp; - } - dr.bRequest = CLEAR_FEATURE; - dr.wValue = ENDPOINT_HALT; - dr.wIndex = endp; - dr.wLength = 0; - dev->controller->control (dev, OUT, sizeof (dr), &dr, 0, 0); + clear_feature (dev, endp, ENDPOINT_HALT, rtype); ep->toggle = 0; return 0; } diff --git a/payloads/libpayload/include/usb/usb.h b/payloads/libpayload/include/usb/usb.h index e38aa78bd1..05ced4907a 100644 --- a/payloads/libpayload/include/usb/usb.h +++ b/payloads/libpayload/include/usb/usb.h @@ -215,6 +215,7 @@ void init_device_entry (hci_t *controller, int num); void set_feature (usbdev_t *dev, int endp, int feature, int rtype); void get_status (usbdev_t *dev, int endp, int rtype, int len, void *data); +void clear_feature (usbdev_t *dev, int endp, int feature, int rtype); int clear_stall (endpoint_t *ep); void usb_nop_init (usbdev_t *dev);