Donate to e Foundation | Murena handsets with /e/OS | Own a part of Murena! Learn more

Commit 7baf9294 authored by Luca Ellero's avatar Luca Ellero Committed by Greg Kroah-Hartman
Browse files

staging: comedi: ni_usb6501: add counter subdevice 



Add counter support for NI USB-6501.

The following functions are introduced:
- ni6501_counter_command()
- ni6501_cnt_insn_config()
- ni6501_cnt_insn_read()
- ni6501_cnt_insn_write()

Signed-off-by: default avatarLuca Ellero <luca.ellero@brickedbrain.com>
Reviewed-by: default avatarH Hartley Sweeten <hsweeten@visionengravers.com>
Signed-off-by: default avatarGreg Kroah-Hartman <gregkh@linuxfoundation.org>
parent 5116a48c

drivers/staging/comedi/drivers/ni_usb6501.c

+166 −1
Original line number Diff line number Diff line
@@ -254,6 +254,96 @@ static int ni6501_port_command(struct comedi_device *dev, int command, 
	return ret;

}



static int ni6501_counter_command(struct comedi_device *dev, int command,

				  u32 *val)

{

	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 ((command == READ_COUNTER || command ==  WRITE_COUNTER) && !val)

		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(*val);

		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)	      */

		*val = 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,
@@ -311,6 +401,71 @@ static int ni6501_dio_insn_bits(struct comedi_device *dev, 
	return insn->n;

}



static int ni6501_cnt_insn_config(struct comedi_device *dev,

				  struct comedi_subdevice *s,

				  struct comedi_insn *insn,

				  unsigned int *data)

{

	int ret;

	u32 val = 0;



	switch (data[0]) {

	case INSN_CONFIG_ARM:

		ret = ni6501_counter_command(dev, START_COUNTER, NULL);

		break;

	case INSN_CONFIG_DISARM:

		ret = ni6501_counter_command(dev, STOP_COUNTER, NULL);

		break;

	case INSN_CONFIG_RESET:

		ret = ni6501_counter_command(dev, STOP_COUNTER, NULL);

		if (ret)

			break;

		ret = ni6501_counter_command(dev, WRITE_COUNTER, &val);

		break;

	default:

		return -EINVAL;

	}



	return ret ? ret : insn->n;

}



static int ni6501_cnt_insn_read(struct comedi_device *dev,

				struct comedi_subdevice *s,

				struct comedi_insn *insn,

				unsigned int *data)

{

	int ret;

	u32 val;

	unsigned int i;



	for (i = 0; i < insn->n; i++) {

		ret = ni6501_counter_command(dev, READ_COUNTER,	&val);

		if (ret)

			return ret;

		data[i] = val;

	}



	return insn->n;

}



static int ni6501_cnt_insn_write(struct comedi_device *dev,

				 struct comedi_subdevice *s,

				 struct comedi_insn *insn,

				 unsigned int *data)

{

	int ret;



	if (insn->n) {

		u32 val = data[insn->n - 1];



		ret = ni6501_counter_command(dev, WRITE_COUNTER, &val);

		if (ret)

			return ret;

	}



	return insn->n;

}



static int ni6501_alloc_usb_buffers(struct comedi_device *dev)

{

	struct ni6501_private *devpriv = dev->private;
@@ -389,7 +544,7 @@ static int ni6501_auto_attach(struct comedi_device *dev, 
	sema_init(&devpriv->sem, 1);

	usb_set_intfdata(intf, devpriv);



	ret = comedi_alloc_subdevices(dev, 1);

	ret = comedi_alloc_subdevices(dev, 2);

	if (ret)

		return ret;
@@ -403,6 +558,16 @@ static int ni6501_auto_attach(struct comedi_device *dev, 
	s->insn_bits	= ni6501_dio_insn_bits;

	s->insn_config	= ni6501_dio_insn_config;



	/* Counter subdevice */

	s = &dev->subdevices[1];

	s->type		= COMEDI_SUBD_COUNTER;

	s->subdev_flags	= SDF_READABLE | SDF_WRITEABLE | SDF_LSAMPL;

	s->n_chan	= 1;

	s->maxdata	= 0xffffffff;

	s->insn_read	= ni6501_cnt_insn_read;

	s->insn_write	= ni6501_cnt_insn_write;

	s->insn_config	= ni6501_cnt_insn_config;



	return 0;

}