Signed-off-by: Luca Ellero <luca.ellero@xxxxxxxxxxxxxxxx>
---
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
_______________________________________________
devel mailing list
devel@xxxxxxxxxxxxxxxxxxxxxx
http://driverdev.linuxdriverproject.org/mailman/listinfo/driverdev-devel
