Add SET_FEATURE to stub driver

cezanne · May 5, 2021 · 243d75c · 243d75c
1 parent aec5df4
commit 243d75c
Show file tree

Hide file tree

Showing 3 changed files with 55 additions and 0 deletions.
diff --git 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
@@ -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
@@ -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;