diff --git a/driver/stub/stub_usbd.c b/driver/stub/stub_usbd.c index 2eb22278..c7670259 100644 --- a/driver/stub/stub_usbd.c +++ b/driver/stub/stub_usbd.c @@ -359,6 +359,30 @@ reset_pipe(usbip_stub_dev_t *devstub, USBD_PIPE_HANDLE hPipe) return FALSE; } +int +set_feature(usbip_stub_dev_t *devstub, USHORT func, USHORT feature, USHORT index) +{ + URB urb; + NTSTATUS status; + + urb.UrbHeader.Function = func; + urb.UrbHeader.Length = sizeof(struct _URB_CONTROL_FEATURE_REQUEST); + urb.UrbControlFeatureRequest.FeatureSelector = feature; + urb.UrbControlFeatureRequest.Index = index; + /* should be NULL. If not, usbd returns STATUS_INVALID_PARAMETER */ + urb.UrbControlFeatureRequest.UrbLink = NULL; + status = call_usbd(devstub, &urb); + if (NT_SUCCESS(status)) + return 0; + /* + * TODO: Only applied to this routine beause it's unclear that the status is + * unsuccessful when a device is stalled. + */ + if (status == STATUS_UNSUCCESSFUL && urb.UrbHeader.Status == USBD_STATUS_STALL_PID) + return to_usbip_status(urb.UrbHeader.Status); + return -1; +} + BOOLEAN submit_class_vendor_req(usbip_stub_dev_t *devstub, BOOLEAN is_in, USHORT cmd, UCHAR reservedBits, UCHAR request, USHORT value, USHORT index, PVOID data, PULONG plen) { diff --git a/driver/stub/stub_usbd.h b/driver/stub/stub_usbd.h index d41c48ec..07e303ff 100644 --- a/driver/stub/stub_usbd.h +++ b/driver/stub/stub_usbd.h @@ -13,6 +13,7 @@ BOOLEAN select_usb_conf(usbip_stub_dev_t *devstub, USHORT idx); BOOLEAN select_usb_intf(usbip_stub_dev_t *devstub, UCHAR intf_num, USHORT alt_setting); BOOLEAN reset_pipe(usbip_stub_dev_t *devstub, USBD_PIPE_HANDLE hPipe); +int set_feature(usbip_stub_dev_t *devstub, USHORT func, USHORT feature, USHORT index); BOOLEAN submit_class_vendor_req(usbip_stub_dev_t *devstub, BOOLEAN is_in, USHORT cmd, UCHAR rv, UCHAR request, USHORT value, USHORT index, PVOID data, PULONG plen); diff --git a/driver/stub/stub_write.c b/driver/stub/stub_write.c index ad3cb0a8..cfb8eccc 100644 --- a/driver/stub/stub_write.c +++ b/driver/stub/stub_write.c @@ -126,6 +126,33 @@ process_clear_feature(usbip_stub_dev_t *devstub, unsigned int seqnum, usb_cspkt_ } } +static void +process_set_feature(usbip_stub_dev_t *devstub, unsigned int seqnum, usb_cspkt_t *csp) +{ + int res; + + DBGI(DBG_READWRITE, "set_feature: %s\n", dbg_cspkt_recipient(CSPKT_RECIPIENT(csp))); + + switch (CSPKT_RECIPIENT(csp)) { + case BMREQUEST_TO_DEVICE: + res = set_feature(devstub, URB_FUNCTION_SET_FEATURE_TO_DEVICE, csp->wValue.W, csp->wIndex.W); + break; + case BMREQUEST_TO_ENDPOINT: + res = set_feature(devstub, URB_FUNCTION_SET_FEATURE_TO_ENDPOINT, csp->wValue.W, csp->wIndex.W); + break; + default: + DBGE(DBG_READWRITE, "set_feature: not supported: %s\n", dbg_cspkt_recipient(CSPKT_RECIPIENT(csp))); + reply_stub_req_err(devstub, USBIP_RET_SUBMIT, seqnum, -1); + return; + } + if (res == 0) + reply_stub_req_hdr(devstub, USBIP_RET_SUBMIT, seqnum); + else { + DBGI(DBG_READWRITE, "failed to set feature\n"); + reply_stub_req_err(devstub, USBIP_RET_SUBMIT, seqnum, res); + } +} + static void process_select_conf(usbip_stub_dev_t *devstub, unsigned int seqnum, usb_cspkt_t *csp) { @@ -157,6 +184,9 @@ process_standard_request(usbip_stub_dev_t *devstub, unsigned int seqnum, usb_csp case USB_REQUEST_CLEAR_FEATURE: process_clear_feature(devstub, seqnum, csp); break; + case USB_REQUEST_SET_FEATURE: + process_set_feature(devstub, seqnum, csp); + break; case USB_REQUEST_SET_CONFIGURATION: process_select_conf(devstub, seqnum, csp); break;