greybus: es1,es2: add USB vendor command to timestamp

As part of an effort to get deep inspection of latencies throughout the
greybus network including HSIC, UniPro and firmware incurred latencies a
new command to the APBridge to tag a known offset with timestamping data
has been introduced. This patch adds that code to the es1 and es2 drivers.

- latency_tag_enable
- latency_tag_disable

Respectively send the enable/disable command to APBridge on a per-CPort
basis. This allows only specified cports to have timestamping data added by
APBridge, leaving any CPort not specifically enabled untouched.

Signed-off-by: Bryan O'Donoghue <bryan.odonoghue@linaro.org>
Signed-off-by: Greg Kroah-Hartman <gregkh@google.com>

3 files changed, 111 insertions, 0 deletions

diff --git a/drivers/staging/greybus/es1.c b/drivers/staging/greybus/es1.c
index 4d70e89aedd9..ba025e782b0c 100644
--- a/drivers/staging/greybus/es1.c
+++ b/drivers/staging/greybus/es1.c
@@ -52,6 +52,10 @@ static DEFINE_KFIFO(apb1_log_fifo, char, APB1_LOG_SIZE);
 /* vendor request APB1 log */
 #define REQUEST_LOG		0x02
 
+/* vendor request to time the latency of messages on a given cport */
+#define REQUEST_LATENCY_TAG_EN	0x06
+#define REQUEST_LATENCY_TAG_DIS	0x07
+
 /**
  * es1_ap_dev - ES1 USB Bridge to AP structure
  * @usb_dev: pointer to the USB device we are.
@@ -273,10 +277,60 @@ static void message_cancel(struct gb_message *message)
 	usb_free_urb(urb);
 }
 
+static int latency_tag_enable(struct greybus_host_device *hd, u16 cport_id)
+{
+	int retval;
+	struct es1_ap_dev *es1 = hd_to_es1(hd);
+	struct usb_device *udev = es1->usb_dev;
+
+	if (!cport_id_valid(hd, cport_id)) {
+		dev_err(&udev->dev, "invalid destination cport 0x%02x\n",
+			cport_id);
+		return -EINVAL;
+	}
+
+	retval = usb_control_msg(udev, usb_sndctrlpipe(udev, 0),
+				 REQUEST_LATENCY_TAG_EN,
+				 USB_DIR_OUT | USB_TYPE_VENDOR |
+				 USB_RECIP_INTERFACE, cport_id, 0, NULL,
+				 0, ES1_TIMEOUT);
+
+	if (retval < 0)
+		dev_err(&udev->dev, "Cannot enable latency tag for cport %d\n",
+			cport_id);
+	return retval;
+}
+
+static int latency_tag_disable(struct greybus_host_device *hd, u16 cport_id)
+{
+	int retval;
+	struct es1_ap_dev *es1 = hd_to_es1(hd);
+	struct usb_device *udev = es1->usb_dev;
+
+	if (!cport_id_valid(hd, cport_id)) {
+		dev_err(&udev->dev, "invalid destination cport 0x%02x\n",
+			cport_id);
+		return -EINVAL;
+	}
+
+	retval = usb_control_msg(udev, usb_sndctrlpipe(udev, 0),
+				 REQUEST_LATENCY_TAG_DIS,
+				 USB_DIR_OUT | USB_TYPE_VENDOR |
+				 USB_RECIP_INTERFACE, cport_id, 0, NULL,
+				 0, ES1_TIMEOUT);
+
+	if (retval < 0)
+		dev_err(&udev->dev, "Cannot disable latency tag for cport %d\n",
+			cport_id);
+	return retval;
+}
+
 static struct greybus_host_driver es1_driver = {
 	.hd_priv_size		= sizeof(struct es1_ap_dev),
 	.message_send		= message_send,
 	.message_cancel		= message_cancel,
+	.latency_tag_enable	= latency_tag_enable,
+	.latency_tag_disable	= latency_tag_disable,
 };
 
 /* Common function to report consistent warnings based on URB status */
diff --git a/drivers/staging/greybus/es2.c b/drivers/staging/greybus/es2.c
index 5faf80a1d5dd..f947983cf56c 100644
--- a/drivers/staging/greybus/es2.c
+++ b/drivers/staging/greybus/es2.c
@@ -61,6 +61,10 @@ static DEFINE_KFIFO(apb1_log_fifo, char, APB1_LOG_SIZE);
 /* vendor request to reset a cport state */
 #define REQUEST_RESET_CPORT	0x05
 
+/* vendor request to time the latency of messages on a given cport */
+#define REQUEST_LATENCY_TAG_EN	0x06
+#define REQUEST_LATENCY_TAG_DIS	0x07
+
 /*
  * @endpoint: bulk in endpoint for CPort data
  * @urb: array of urbs for the CPort in messages
@@ -413,11 +417,61 @@ static int cport_enable(struct greybus_host_device *hd, u16 cport_id)
 	return 0;
 }
 
+static int latency_tag_enable(struct greybus_host_device *hd, u16 cport_id)
+{
+	int retval;
+	struct es1_ap_dev *es1 = hd_to_es1(hd);
+	struct usb_device *udev = es1->usb_dev;
+
+	if (!cport_id_valid(hd, cport_id)) {
+		dev_err(&udev->dev, "invalid destination cport 0x%02x\n",
+			cport_id);
+		return -EINVAL;
+	}
+
+	retval = usb_control_msg(udev, usb_sndctrlpipe(udev, 0),
+				 REQUEST_LATENCY_TAG_EN,
+				 USB_DIR_OUT | USB_TYPE_VENDOR |
+				 USB_RECIP_INTERFACE, cport_id, 0, NULL,
+				 0, ES1_TIMEOUT);
+
+	if (retval < 0)
+		dev_err(&udev->dev, "Cannot enable latency tag for cport %d\n",
+			cport_id);
+	return retval;
+}
+
+static int latency_tag_disable(struct greybus_host_device *hd, u16 cport_id)
+{
+	int retval;
+	struct es1_ap_dev *es1 = hd_to_es1(hd);
+	struct usb_device *udev = es1->usb_dev;
+
+	if (!cport_id_valid(hd, cport_id)) {
+		dev_err(&udev->dev, "invalid destination cport 0x%02x\n",
+			cport_id);
+		return -EINVAL;
+	}
+
+	retval = usb_control_msg(udev, usb_sndctrlpipe(udev, 0),
+				 REQUEST_LATENCY_TAG_DIS,
+				 USB_DIR_OUT | USB_TYPE_VENDOR |
+				 USB_RECIP_INTERFACE, cport_id, 0, NULL,
+				 0, ES1_TIMEOUT);
+
+	if (retval < 0)
+		dev_err(&udev->dev, "Cannot disable latency tag for cport %d\n",
+			cport_id);
+	return retval;
+}
+
 static struct greybus_host_driver es1_driver = {
 	.hd_priv_size		= sizeof(struct es1_ap_dev),
 	.message_send		= message_send,
 	.message_cancel		= message_cancel,
 	.cport_enable		= cport_enable,
+	.latency_tag_enable	= latency_tag_enable,
+	.latency_tag_disable	= latency_tag_disable,
 };
 
 /* Common function to report consistent warnings based on URB status */
diff --git a/drivers/staging/greybus/greybus.h b/drivers/staging/greybus/greybus.h
index e4e53c139ac4..ec4a73884b09 100644
--- a/drivers/staging/greybus/greybus.h
+++ b/drivers/staging/greybus/greybus.h
@@ -80,6 +80,9 @@ struct greybus_host_driver {
 	int (*message_send)(struct greybus_host_device *hd, u16 dest_cport_id,
 			struct gb_message *message, gfp_t gfp_mask);
 	void (*message_cancel)(struct gb_message *message);
+	int (*latency_tag_enable)(struct greybus_host_device *hd, u16 cport_id);
+	int (*latency_tag_disable)(struct greybus_host_device *hd,
+				   u16 cport_id);
 };
 
 struct greybus_host_device {