[PATCH 5/9] staging: comedi: ni_usb6501: add ni6501_counter_command()

[Luca Ellero]

Signed-off-by: Luca Ellero <luca.ellero at brickedbrain.com>
---
 drivers/staging/comedi/drivers/ni_usb6501.c |  110 +++++++++++++++++++++++++++
 1 file changed, 110 insertions(+)

diff --git a/drivers/staging/comedi/drivers/ni_usb6501.c b/drivers/staging/comedi/drivers/ni_usb6501.c
index f55b9f8..cef93d2 100644
--- a/drivers/staging/comedi/drivers/ni_usb6501.c
+++ b/drivers/staging/comedi/drivers/ni_usb6501.c
@@ -272,6 +272,116 @@ end:
 	return ret;
 }
 
+static int ni6501_counter_command(struct comedi_device *dev, int command,
+				  u32 *counter)
+{
+	struct usb_device *usb = comedi_to_usb_dev(dev);
+	struct ni6501_private *devpriv = dev->private;
+	int request_size, response_size;
+	u8 *tx = devpriv->usb_tx_buf;
+	int ret;
+
+	if (!tx)
+		return -EINVAL;
+
+	if ((command == READ_COUNTER || command ==  WRITE_COUNTER) && !counter)
+		return -EINVAL;
+
+	down(&devpriv->sem);
+
+	switch (command) {
+	case START_COUNTER:
+
+		request_size = sizeof(START_COUNTER_REQUEST);
+		response_size = sizeof(GENERIC_RESPONSE);
+
+		memcpy(tx, START_COUNTER_REQUEST, request_size);
+
+		break;
+
+	case STOP_COUNTER:
+
+		request_size = sizeof(STOP_COUNTER_REQUEST);
+		response_size = sizeof(GENERIC_RESPONSE);
+
+		memcpy(tx, STOP_COUNTER_REQUEST, request_size);
+
+		break;
+
+	case READ_COUNTER:
+
+		request_size = sizeof(READ_COUNTER_REQUEST);
+		response_size = sizeof(READ_COUNTER_RESPONSE);
+
+		memcpy(tx, READ_COUNTER_REQUEST, request_size);
+
+		break;
+
+	case WRITE_COUNTER:
+
+		request_size = sizeof(WRITE_COUNTER_REQUEST);
+		response_size = sizeof(GENERIC_RESPONSE);
+
+		memcpy(tx, WRITE_COUNTER_REQUEST, request_size);
+
+		/* Setup tx packet: bytes 12,13,14,15 hold the */
+		/* u32 counter value (Big Endian)	       */
+		*((u32 *)&tx[12]) = cpu_to_be32(*counter);
+
+		break;
+
+	default:
+		ret = -EINVAL;
+		goto end;
+	}
+
+	ret = usb_bulk_msg(usb,
+			   usb_sndbulkpipe(usb,
+					   devpriv->ep_tx->bEndpointAddress),
+			   devpriv->usb_tx_buf,
+			   request_size,
+			   NULL,
+			   NI6501_TIMEOUT);
+	if (ret)
+		goto end;
+
+	ret = usb_bulk_msg(usb,
+			   usb_rcvbulkpipe(usb,
+					   devpriv->ep_rx->bEndpointAddress),
+			   devpriv->usb_rx_buf,
+			   response_size,
+			   NULL,
+			   NI6501_TIMEOUT);
+	if (ret)
+		goto end;
+
+	/* Check if results are valid */
+
+	if (command == READ_COUNTER) {
+		int i;
+
+		/* Read counter value: bytes 12,13,14,15 of rx packet */
+		/* hold the u32 counter value (Big Endian)	      */
+		*counter = be32_to_cpu(*((u32 *)&devpriv->usb_rx_buf[12]));
+
+		/* mask counter value for comparing */
+		for (i = 12; i < sizeof(READ_COUNTER_RESPONSE); ++i)
+			devpriv->usb_rx_buf[i] = 0x00;
+
+		if (memcmp(devpriv->usb_rx_buf, READ_COUNTER_RESPONSE,
+			   sizeof(READ_COUNTER_RESPONSE))) {
+			ret = -EINVAL;
+		}
+	} else if (memcmp(devpriv->usb_rx_buf, GENERIC_RESPONSE,
+			  sizeof(GENERIC_RESPONSE))) {
+		ret = -EINVAL;
+	}
+end:
+	up(&devpriv->sem);
+
+	return ret;
+}
+
 static int ni6501_dio_insn_config(struct comedi_device *dev,
 				  struct comedi_subdevice *s,
 				  struct comedi_insn *insn,
-- 
1.7.10.4