Signed-off-by: Luca Ellero <luca.ell...@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..60fc1ee 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)              */
+               *((__be32 *)&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(*((__be32 *)&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
de...@linuxdriverproject.org
http://driverdev.linuxdriverproject.org/mailman/listinfo/driverdev-devel

Reply via email to