VILLASnode/lib/nodes/comedi.c

/** Node type: comedi
 *
 * @author Steffen Vogel <stvogel@eonerc.rwth-aachen.de>
 * @author Daniel Krebs <github@daniel-krebs.net>
 * @copyright 2018, Institute for Automation of Complex Power Systems, EONERC
 * @license GNU General Public License (version 3)
 *
 * VILLASnode
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *********************************************************************************/

#include <string.h>
#include <math.h>
#include <sys/mman.h>
#include <comedilib.h>
#include <comedi_errno.h>

#include <villas/plugin.h>
#include <villas/nodes/comedi.h>
#include <villas/utils.h>

// utility functions to dump a comedi_cmd graciously taken from comedilib demo
static char* comedi_cmd_trigger_src(unsigned int src, char *buf);
static void comedi_dump_cmd(comedi_cmd *cmd, int debug_level);

static int comedi_parse_direction(struct comedi *c, struct comedi_direction *d, json_t *cfg)
{
	int ret;

	json_t *json_chans;
	json_error_t err;

	// default values
	d->subdevice = -1;
	d->buffer_size = 16;

	ret = json_unpack_ex(cfg, &err, 0, "{ s?: i, s?: i, s: o, s: i }",
		"subdevice", &d->subdevice,
	    "bufsize", &d->buffer_size,
	    "signals", &json_chans,
	    "rate", &d->sample_rate_hz
	);
	if (ret)
		jerror(&err, "Failed to parse configuration");

	if (!json_is_array(json_chans))
		return -1;

	// convert kilobytes to bytes
	d->buffer_size = d->buffer_size << 10;

	size_t i;
	json_t *json_chan;

	d->chanlist_len = json_array_size(json_chans);
	if(d->chanlist_len == 0) {
		error("No channels configured");
		return 0;
	}

	d->chanlist = malloc(d->chanlist_len * sizeof(*d->chanlist));
	assert(d->chanlist != NULL);

	d->chanspecs = malloc(d->chanlist_len * sizeof(*d->chanspecs));
	assert(d->chanspecs != NULL);

	json_array_foreach(json_chans, i, json_chan) {
		int num, range, aref;
		ret = json_unpack_ex(json_chan, &err, 0, "{ s: i, s: i, s: i }",
		                     "channel", &num,
		                     "range", &range,
		                     "aref", &aref);
		if (ret)
			jerror(&err, "Failed to parse configuration");

		if(aref < AREF_GROUND || aref > AREF_OTHER)
			error("Invalid value for analog reference: aref=%d", aref);

		d->chanlist[i] = CR_PACK(num, range, aref);
	}

	return 0;
}


static int comedi_start_common(struct node *n)
{
	struct comedi *c = (struct comedi *) n->_vd;
	struct comedi_direction* directions[2] = { &c->in, &c->out };

	comedi_set_global_oor_behavior(COMEDI_OOR_NAN);


	for(int dirIdx = 0; dirIdx < 2; dirIdx++) {
		struct comedi_direction* d = directions[dirIdx];
		int ret;

		if(!d->present)
			continue;

		// sanity-check channel config and populate chanspec for later
		for(int i = 0; i < d->chanlist_len; i++) {
			const unsigned int channel = CR_CHAN(d->chanlist[i]);
			const int range = CR_RANGE(d->chanlist[i]);

			ret = comedi_get_n_ranges(c->dev, d->subdevice, channel);
			if(ret < 0)
				error("Failed to get ranges for channel %d on subdevice %d",
				      channel, d->subdevice);

			if(range >= ret)
				error("Invalid range for channel %d on subdevice %d: range=%d",
				      channel, d->subdevice, range);

			ret = comedi_get_maxdata(c->dev, d->subdevice, channel);
			if (ret <= 0)
				error("Failed to get max. data value for channel %d on subdevice %d",
				      channel, d->subdevice);

			d->chanspecs[i].maxdata = ret;
			d->chanspecs[i].range = comedi_get_range(c->dev, d->subdevice,
			                                         channel, range);

			info("%s channel: %d aref=%d range=%d maxdata=%d",
			     (d == &c->in ? "Input" : "Output"), channel,
			     CR_AREF(d->chanlist[i]), range, d->chanspecs[i].maxdata);
		}


		const int flags = comedi_get_subdevice_flags(c->dev, d->subdevice);
		d->sample_size = (flags & SDF_LSAMPL) ? sizeof(lsampl_t) : sizeof(sampl_t);

		/* Set buffer size */
		comedi_set_buffer_size(c->dev, d->subdevice, d->buffer_size);
		comedi_set_max_buffer_size(c->dev, d->subdevice, d->buffer_size);
		ret = comedi_get_buffer_size(c->dev, d->subdevice);
		if (ret != d->buffer_size)
			error("Failed to set buffer size for subdevice %d of node '%s'", d->subdevice, node_name(n));

		info("Set buffer size for subdevice %d to %d bytes", d->subdevice, d->buffer_size);

		ret = comedi_lock(c->dev, d->subdevice);
		if (ret)
			error("Failed to lock subdevice %d for node '%s'", d->subdevice, node_name(n));
	}

	return 0;
}


static int comedi_start_in(struct node *n)
{
	int ret;
	struct comedi *c = (struct comedi *) n->_vd;
	struct comedi_direction *d = &c->in;

	// try to find first analog input subdevice if not specified in config
	if (d->subdevice < 0) {
		d->subdevice = comedi_find_subdevice_by_type(c->dev, COMEDI_SUBD_AI, 0);
		if (d->subdevice < 0)
			error("Cannot find analog input device for node '%s'", node_name(n));
	} else {
		/* Check if subdevice is usable */
		ret = comedi_get_subdevice_type(c->dev, d->subdevice);
		if (ret != COMEDI_SUBD_AI)
			error("Input subdevice of node '%s' is not an analog input", node_name(n));
	}

	ret = comedi_get_subdevice_flags(c->dev, d->subdevice);
	if (ret < 0 || !(ret & SDF_CMD_READ))
		error("Input subdevice of node '%s' does not support 'read' commands", node_name(n));


	comedi_set_read_subdevice(c->dev, d->subdevice);
	ret = comedi_get_read_subdevice(c->dev);
	if (ret < 0 || ret != d->subdevice)
		error("Failed to change 'read' subdevice from %d to %d of node '%s'",
		      ret, d->subdevice, node_name(n));

	comedi_cmd cmd;
	memset(&cmd, 0, sizeof(cmd));

	cmd.subdev = d->subdevice;

	// make card send interrupts after every sample, not only when fifo is half
	// full (TODO: evaluate if this makes sense, leave as reminder)
	//cmd.flags = TRIG_WAKE_EOS;

	// start right now
	cmd.start_src = TRIG_NOW;

	// trigger scans periodically
	cmd.scan_begin_src = TRIG_TIMER;
	cmd.scan_begin_arg = 1e9 / d->sample_rate_hz;

	// do conversions in serial with 1ns inter-conversion delay
	cmd.convert_src = TRIG_TIMER;
	cmd.convert_arg = 1;	// inter-conversion delay in nanoseconds

	// terminate scan after each channel has been converted
	cmd.scan_end_src = TRIG_COUNT;
	cmd.scan_end_arg = d->chanlist_len;

	// contionous sampling
	cmd.stop_src = TRIG_NONE;

	cmd.chanlist = d->chanlist;
	cmd.chanlist_len = d->chanlist_len;


	// first run might change command, second should return successfully
	ret = comedi_command_test(c->dev, &cmd);
	ret = comedi_command_test(c->dev, &cmd);
	if (ret < 0)
		error("Invalid command for input subdevice of node '%s'", node_name(n));

	info("Input command:");
	comedi_dump_cmd(&cmd, 1);

	ret = comedi_command(c->dev, &cmd);
	if (ret < 0)
		error("Failed to issue command to input subdevice of node '%s'", node_name(n));

	d->started = time_now();
	d->counter = 0;
	d->running = true;


#if COMEDI_USE_READ
	// be prepared to consume one entire buffer
	c->buf = malloc(c->in.buffer_size);
	c->bufptr = c->buf;
	assert(c->bufptr != NULL);

	info("Compiled for kernel read() interface");
#else
	info("Compiled for kernel mmap() interface");
#endif

	return 0;
}

static int comedi_start_out(struct node *n)
{
	int ret;
	struct comedi *c = (struct comedi *) n->_vd;
	struct comedi_direction *d = &c->out;

	// try to find first analog output subdevice if not specified in config
	if (d->subdevice < 0) {
		d->subdevice = comedi_find_subdevice_by_type(c->dev, COMEDI_SUBD_AO, 0);
		if (d->subdevice < 0)
			error("Cannot find analog output device for node '%s'", node_name(n));
	} else {
		ret = comedi_get_subdevice_type(c->dev, d->subdevice);
		if (ret != COMEDI_SUBD_AO)
			error("Output subdevice of node '%s' is not an analog output", node_name(n));
	}

	ret = comedi_get_subdevice_flags(c->dev, d->subdevice);
	if (ret < 0 || !(ret & SDF_CMD_WRITE))
		error("Output subdevice of node '%s' does not support 'write' commands", node_name(n));


	comedi_set_write_subdevice(c->dev, d->subdevice);
	ret = comedi_get_write_subdevice(c->dev);
	if (ret < 0 || ret != d->subdevice)
		error("Failed to change 'write' subdevice from %d to %d of node '%s'",
		      ret, d->subdevice, node_name(n));


	comedi_cmd cmd;
	memset(&cmd, 0, sizeof(cmd));

	cmd.subdev = d->subdevice;

	cmd.flags = CMDF_WRITE;

	// wait for internal trigger, we will have to fill the buffer first
	cmd.start_src = TRIG_INT;
	cmd.start_arg = 0;

	cmd.scan_begin_src = TRIG_TIMER;
	cmd.scan_begin_arg = 1e9 / d->sample_rate_hz;

	cmd.convert_src = TRIG_NOW;
	cmd.convert_arg = 0;

	cmd.scan_end_src = TRIG_COUNT;
	cmd.scan_end_arg = d->chanlist_len;

	// continous sampling
	cmd.stop_src = TRIG_NONE;
	cmd.stop_arg = 0;

	cmd.chanlist = d->chanlist;
	cmd.chanlist_len = d->chanlist_len;


	// first run might change command, second should return successfully
	ret = comedi_command_test(c->dev, &cmd);
	if (ret < 0)
		error("Invalid command for input subdevice of node '%s'", node_name(n));

	ret = comedi_command_test(c->dev, &cmd);
	if (ret < 0)
		error("Invalid command for input subdevice of node '%s'", node_name(n));

	info("Output command:");
	comedi_dump_cmd(&cmd, 1);

	ret = comedi_command(c->dev, &cmd);
	if (ret < 0)
		error("Failed to issue command to input subdevice of node '%s'", node_name(n));

	// output will only start after the internal trigger
	d->running = false;
	d->last_debug = time_now();

	// allocate buffer for one complete villas sample
	// TODO: maybe increase buffer size according to c->vectorize
	const size_t local_buffer_size = d->sample_size * d->chanlist_len;
	d->buffer = malloc(local_buffer_size);
	d->bufptr = d->buffer;
	assert(d->buffer != NULL);

	// initialize local buffer used for write() syscalls
	for(int channel = 0; channel < d->chanlist_len; channel++) {
		const unsigned raw = comedi_from_phys(0.0f, d->chanspecs[channel].range, d->chanspecs[channel].maxdata);

		if(d->sample_size == sizeof(sampl_t)) {
			*((sampl_t *)d->bufptr) = raw;
		} else {
			*((lsampl_t *)d->bufptr) = raw;
		}

		d->bufptr += d->sample_size;
	}

	// preload comedi output buffer
	for(int i = 0; i < d->buffer_size / local_buffer_size; i++) {
		ret = write(comedi_fileno(c->dev), d->buffer, local_buffer_size);
		if(ret != local_buffer_size) {
			error("Cannot preload Comedi buffer");
		}
	}

	const size_t villas_samples_in_kernel_buf = d->buffer_size / (d->sample_size * d->chanlist_len);
	const double latencyMs = (double)villas_samples_in_kernel_buf / d->sample_rate_hz * 1e3;
	info("Added latency due to buffering: %4.1f ms\n", latencyMs);

	return 0;
}

static int comedi_stop_in(struct node *n)
{
	int ret;
	struct comedi *c = (struct comedi *) n->_vd;
	struct comedi_direction *d = &c->in;

	comedi_cancel(c->dev, d->subdevice);

	ret = comedi_unlock(c->dev, d->subdevice);
	if (ret)
		error("Failed to lock subdevice %d for node '%s'", d->subdevice, node_name(n));

	return 0;
}

static int comedi_stop_out(struct node *n)
{
	int ret;
	struct comedi *c = (struct comedi *) n->_vd;
	struct comedi_direction *d = &c->out;

	comedi_cancel(c->dev, d->subdevice);

	ret = comedi_unlock(c->dev, d->subdevice);
	if (ret)
		error("Failed to lock subdevice %d for node '%s'", d->subdevice, node_name(n));

	return 0;
}

int comedi_parse(struct node *n, json_t *cfg)
{
	int ret;
	struct comedi *c = (struct comedi *) n->_vd;

	const char *device;

	json_t *json_in = NULL;
	json_t *json_out = NULL;
	json_error_t err;

	ret = json_unpack_ex(cfg, &err, 0, "{ s: s, s?: o, s?: o }",
	                     "device", &device,
	                     "in", &json_in,
	                     "out", &json_out);

	if (ret)
		jerror(&err, "Failed to parse configuration of node %s", node_name(n));

	c->in.present = json_in != NULL;
	c->in.enabled = false;
	c->in.running = false;

	c->out.present = json_out != NULL;
	c->out.enabled = false;
	c->out.running = false;


	if (c->in.present) {
		ret = comedi_parse_direction(c, &c->in, json_in);
		if (ret)
			return ret;
	}

	if (c->out.present) {
		ret = comedi_parse_direction(c, &c->out, json_out);
		if (ret)
			return ret;
	}

	c->device = strdup(device);

	n->samplelen = c->in.chanlist_len;

	return 0;
}

char * comedi_print(struct node *n)
{
	struct comedi *c = (struct comedi *) n->_vd;

	char *buf = NULL;

	const char *board = comedi_get_board_name(c->dev);
	const char *driver = comedi_get_driver_name(c->dev);

	strcatf(&buf, "board=%s, driver=%s, device=%s", board, driver, c->device);

	return buf;
}

int comedi_start(struct node *n)
{
	struct comedi *c = (struct comedi *) n->_vd;

	c->dev = comedi_open(c->device);
	if (!c->dev) {
		const char *err = comedi_strerror(comedi_errno());
		error("Failed to open device: %s", err);
	}

	// enable non-blocking syscalls
	// TODO: verify if this works with both input and output, so comment out
	//if (fcntl(comedi_fileno(c->dev), F_SETFL, O_NONBLOCK))
	//	error("Failed to set non-blocking flag in Comedi FD of node '%s'", node_name(n));

	comedi_start_common(n);

	if(c->in.present) {
		int ret = comedi_start_in(n);
		if(ret)
			return ret;
		else
			c->in.enabled = true;
	}

	if(c->out.present) {
		int ret = comedi_start_out(n);
		if(ret)
			return ret;
		else
			c->out.enabled = true;
	}

#if !COMEDI_USE_READ
	info("Mapping Comedi buffer of %d bytes", c->in.buffer_size);
	c->map = mmap(NULL, c->in.buffer_size, PROT_READ, MAP_SHARED, comedi_fileno(c->dev), 0);
	if (c->map == MAP_FAILED)
		error("Failed to map comedi buffer of node '%s'", node_name(n));

	c->front = 0;
	c->back = 0;
	c->bufpos = 0;
#endif

	return 0;
}

int comedi_stop(struct node *n)
{
	int ret;
	struct comedi *c = (struct comedi *) n->_vd;

	if(c->in.enabled)
		comedi_stop_in(n);

	if(c->out.enabled)
		comedi_stop_out(n);

	ret = comedi_close(c->dev);
	if (ret)
		return ret;

	return 0;
}


#if COMEDI_USE_READ

int comedi_read(struct node *n, struct sample *smps[], unsigned cnt, unsigned *release)
{
	int ret;
	struct comedi *c = (struct comedi *) n->_vd;
	struct comedi_direction *d = &c->in;
	const size_t villas_sample_size = d->chanlist_len * d->sample_size;

	ret = comedi_get_buffer_contents(c->dev, d->subdevice);
	if(ret < 0) {
		if(comedi_errno() == EBUF_OVR)
			error("Comedi buffer overflow");
		else {
			error("Comedi error: %s", comedi_strerror(comedi_errno()));
		}
	}

	fd_set rdset;
	FD_ZERO(&rdset);
	FD_SET(comedi_fileno(c->dev), &rdset);

	struct timeval timeout;
	timeout.tv_sec = 0;
	timeout.tv_usec = 5000;

	ret = select(comedi_fileno(c->dev) + 1, &rdset, NULL, NULL, &timeout);
	if(ret < 0) {
		error("select");
	} else if(ret == 0) {
		/* hit timeout */
		return 0;
	} else if(FD_ISSET(comedi_fileno(c->dev), &rdset)) {
		/* comedi file descriptor became ready */

		const size_t buffer_bytes_free = d->buffer_size - (c->bufptr - c->buf);
		const size_t bytes_requested = cnt * villas_sample_size;

		ret = read(comedi_fileno(c->dev), c->bufptr, MIN(bytes_requested, buffer_bytes_free));
		if(ret < 0) {
			if(errno == EAGAIN) {
				error("read");
			} else {
				return 0;
			}

		} else if(ret == 0) {
			warn("select timeout, no samples available");
			return 0;
		} else {
			// sample handling here
			const size_t bytes_available = ret;
			const size_t raw_samples_available = bytes_available / d->sample_size;
			const size_t villas_samples_available = raw_samples_available / d->chanlist_len;

			info("there are %ld bytes available (%ld requested) => %ld villas samples",
			     bytes_available, bytes_requested, villas_samples_available);

			info("there are %ld kB available (%ld kB requested)",
			     bytes_available / 1024, bytes_requested / 1024);

			if(cnt > villas_samples_available)
				cnt = villas_samples_available;

			for(size_t i = 0; i < cnt; i++) {
				d->counter++;

				smps[i]->flags = SAMPLE_HAS_ORIGIN | SAMPLE_HAS_VALUES | SAMPLE_HAS_SEQUENCE;
				smps[i]->sequence = d->counter / d->chanlist_len;

				struct timespec offset = time_from_double(d->counter * 1.0 / d->sample_rate_hz);
				smps[i]->ts.origin = time_add(&d->started, &offset);

				smps[i]->length = d->chanlist_len;

				if(smps[i]->capacity < d->chanlist_len)
					error("Sample has insufficient capacity: %d < %ld",
					      smps[i]->capacity, d->chanlist_len);


				for(int si = 0; si < d->chanlist_len; si++) {
					unsigned int raw;

					if (d->sample_size == sizeof(sampl_t)) {
						raw = *((sampl_t *)(c->bufptr));
					} else {
						raw = *((lsampl_t *)(c->bufptr));
					}

					c->bufptr += d->sample_size;

					smps[i]->data[si].f = comedi_to_phys(raw, d->chanspecs[si].range, d->chanspecs[si].maxdata);
					sample_set_data_format(smps[i], si, SAMPLE_DATA_FORMAT_FLOAT);

					if(isnan(smps[i]->data[si].f)) {
						warn("Input: channel %d clipped", CR_CHAN(d->chanlist[si]));
					}
				}
			}

			const size_t bytes_consumed = cnt * villas_sample_size;
			const size_t bytes_left = bytes_available - bytes_consumed;
			if(bytes_left > 0) {
				// move leftover bytes to the beginning of buffer
				// TODO: optimize?
				memmove(c->buf, c->bufptr, bytes_left);
			}

			info("consumed %ld bytes", bytes_consumed);

			// start at the beginning again
			c->bufptr = c->buf;

			return cnt;
		}
	} else {
		/* unknown file descriptor became ready */
		printf("unknown file descriptor ready\n");
	}

	return -1;
}

#else

int comedi_read(struct node *n, struct sample *smps[], unsigned cnt, unsigned *release)
{
	int ret;
	struct comedi *c = (struct comedi *) n->_vd;
	struct comedi_direction *d = &c->in;

	const size_t villas_sample_size = d->chanlist_len * d->sample_size;

	comedi_set_read_subdevice(c->dev, d->subdevice);

	info("current bufpos=%ld", c->bufpos);

//	if(c->bufpos > (d->buffer_size - villas_sample_size)) {
//		ret = comedi_get_buffer_read_offset(c->dev, d->subdevice);
//		if(ret < 0)
//			error("Canot get offset");

//		c->bufpos = ret;
//		info("change bufpos=%ld", c->bufpos);
//	}

	ret = comedi_get_buffer_contents(c->dev, d->subdevice);
	if (ret == 0) {
		return 0;
	} else if(ret < 0) {
		if(comedi_errno() == EBUF_OVR)
			error("Comedi buffer overflow");
		else {
			error("Comedi error: %s", comedi_strerror(comedi_errno()));
		}
	}

	const size_t bytes_available = ret;
	const size_t raw_sample_count = bytes_available / d->sample_size;
	size_t villas_sample_count = raw_sample_count / d->chanlist_len;
	if(villas_sample_count == 0)
		return 0;

	info("there are %ld villas samples (%ld raw bytes, %ld channels)", villas_sample_count, bytes_available, d->chanlist_len);

//	if(villas_sample_count == 1) {
//		info("front=%ld back=%ld bufpos=%ld", c->front, c->back, c->bufpos);
//	}



//	if((c->bufpos + bytes_available) >= d->buffer_size) {
//		// let comedi do the wraparound, only consume until end of buffer
//		villas_sample_count = (d->buffer_size - c->bufpos) / villas_sample_size;
////		warn("Reducing consumption from %d to %ld bytes", ret, bytes_available);
//		warn("Only consume %ld villas samples b/c of buffer wraparound", villas_sample_count);
//	}

	if(cnt > villas_sample_count)
		cnt = villas_sample_count;

//	if(bytes_available != 0 && bytes_available < villas_sample_size) {
//		warn("Cannot consume samples, only %d bytes available, throw away", ret);

//		ret = comedi_mark_buffer_read(c->dev, d->subdevice, bytes_available);
//		if(ret != bytes_available)
//			error("Cannot throw away %ld bytes, returned %d, wtf comedi?!",
//			      bytes_available, ret);

//		return 0;
//	}



	const size_t samples_total_bytes = cnt * villas_sample_size;

	ret = comedi_mark_buffer_read(c->dev, d->subdevice, samples_total_bytes);
	if(ret == 0) {
		warn("Marking read buffer (%ld bytes) not working, try again later", samples_total_bytes);
		return 0;
	} else if(ret != samples_total_bytes) {
		warn("Can only mark %d bytes as read, reducing samples", ret);
		return 0;
	} else {
		info("Consume %d bytes", ret);
	}

	// align front to whole samples
	c->front = c->back + samples_total_bytes;

	for(size_t i = 0; i < cnt; i++) {
		d->counter++;

		smps[i]->flags = SAMPLE_HAS_ORIGIN | SAMPLE_HAS_VALUES | SAMPLE_HAS_SEQUENCE;
		smps[i]->sequence = d->counter / d->chanlist_len;

		struct timespec offset = time_from_double(d->counter * 1.0 / d->sample_rate_hz);
		smps[i]->ts.origin = time_add(&d->started, &offset);

		smps[i]->length = d->chanlist_len;

		if(smps[i]->capacity < d->chanlist_len)
			error("Sample has insufficient capacity: %d < %ld",
			      smps[i]->capacity, d->chanlist_len);


		for(int si = 0; si < d->chanlist_len; si++) {
			unsigned int raw;

			if (d->sample_size == sizeof(sampl_t)) {
				raw = *((sampl_t *)(c->map + c->bufpos));
			} else {
				raw = *((lsampl_t *)(c->map + c->bufpos));
			}

			smps[i]->data[si].f = comedi_to_phys(raw, d->chanspecs[si].range, d->chanspecs[si].maxdata);
			sample_set_data_format(smps[i], si, SAMPLE_DATA_FORMAT_FLOAT);

			if(isnan(smps[i]->data[si].f)) {
				error("got nan");
			}

//			smps[i]->data[si].i = raw;
//			sample_set_data_format(smps[i], si, SAMPLE_DATA_FORMAT_INT);

			c->bufpos += d->sample_size;
			if(c->bufpos >= d->buffer_size) {
				warn("read buffer wraparound");
//				c->bufpos = 0;
			}
		}
	}

//	const size_t bytes_consumed = c->front - c->back;

//	info("advance comedi buffer by %ld bytes", bytes_consumed);


	ret = comedi_get_buffer_read_offset(c->dev, d->subdevice);
	if(ret < 0) {
		if(comedi_errno() != EPIPE)
			error("Failed to get read buffer offset: %d, comedi errno %d", ret, comedi_errno());
		else
			ret = c->bufpos;
	}

	warn("change bufpos: %ld to %d", c->bufpos, ret);
	c->bufpos = ret;

#if 0
	ret = comedi_mark_buffer_read(c->dev, d->subdevice, bytes_consumed);
	if (ret < 0) { //!= bytes_consumed) {
		error("Failed to mark buffer position (ret=%d) for input stream of node '%s'", ret, node_name(n));
//	} else if(ret == 0) {
	} else {
		info("consumed %ld bytes", bytes_consumed);
		info("mark buffer returned %d", ret);

		if(ret == 0) {
			ret = comedi_mark_buffer_read(c->dev, d->subdevice, bytes_consumed);
			info("trying again, mark buffer returned now %d", ret);
		}


		if(ret > 0) {
			ret = comedi_get_buffer_read_offset(c->dev, d->subdevice);
			if(ret < 0)
				error("Failed to get read buffer offset");

			warn("change bufpos1: %ld to %d", c->bufpos, ret);
			c->bufpos = ret;
		} else {
//			warn("change bufpos2: %ld to %ld", c->bufpos, c->);
//			c->bufpos += bytes_consumed;
			warn("keep bufpos=%ld", c->bufpos);
		}

//		c->bufpos = 0;
	}
#endif

//	info("new bufpos: %ld", c->bufpos);

	c->back = c->front;

	return cnt;
}

#endif

int comedi_write(struct node *n, struct sample *smps[], unsigned cnt, unsigned *release)
{
	int ret;
	struct comedi *c = (struct comedi *) n->_vd;
	struct comedi_direction *d = &c->out;

	if(!d->enabled) {
		warn("Attempting to write, but output is not enabled");
		return 0;
	}

	if(!d->running) {
		// output was not yet running, so start now
		ret = comedi_internal_trigger(c->dev, d->subdevice, 0);
		if(ret < 0)
			error("Failed to trigger-start output");

		d->started = time_now();
		d->counter = 0;
		d->running = true;

		info("Starting output of node '%s'", node_name(n));
	}

	const size_t buffer_capacity_raw = d->buffer_size / d->sample_size;
	const size_t buffer_capacity_villas = buffer_capacity_raw / d->chanlist_len;
	const size_t villas_sample_size = d->sample_size * d->chanlist_len;


	ret = comedi_get_buffer_contents(c->dev, d->subdevice);
	if(ret < 0) {
		if(comedi_errno() == EBUF_OVR) {
			error("Comedi buffer overflow");
		}
		else {
			error("Comedi error: %s", comedi_strerror(comedi_errno()));
		}
	}

	const size_t bytes_in_buffer = ret;
	const size_t raw_samples_in_buffer = bytes_in_buffer / d->sample_size;
	const size_t villas_samples_in_buffer = raw_samples_in_buffer / d->chanlist_len;


	if(villas_samples_in_buffer == buffer_capacity_villas) {
		warn("Comedi buffer is full");
		return 0;
	} else {
		struct timespec now = time_now();
		if(time_delta(&d->last_debug, &now) >= 1) {
			debug(LOG_COMEDI | 2, "Comedi write buffer: %4ld villas samples (%2.0f%% of buffer)",
			      villas_samples_in_buffer,
			      (100.0f * villas_samples_in_buffer / buffer_capacity_villas));

			d->last_debug = time_now();
		}
	}

	size_t villas_samples_written = 0;

	while(villas_samples_written < cnt) {
		struct sample *sample = smps[villas_samples_written];
		if(sample->length != d->chanlist_len)
			error("Value count in sample (%d) != configured output channels (%ld)",
			      sample->length, d->chanlist_len);

		d->bufptr = d->buffer;

		// move samples from villas into local buffer for comedi
		for(int si = 0; si < sample->length; si++) {
			unsigned raw_value = 0;

			switch(sample_get_data_format(sample, si)) {
			case SAMPLE_DATA_FORMAT_FLOAT:
				raw_value = comedi_from_phys(sample->data[si].f, d->chanspecs[si].range, d->chanspecs[si].maxdata);
				break;
			case SAMPLE_DATA_FORMAT_INT:
				// treat sample as already raw DAC value
				raw_value = sample->data[si].i;
				break;
			}

			if(d->sample_size == sizeof(sampl_t)) {
				*((sampl_t *)d->bufptr) = raw_value;
			} else {
				*((lsampl_t *)d->bufptr) = raw_value;
			}

			d->bufptr += d->sample_size;
		}

		// try to write one complete villas sample to comedi
		ret = write(comedi_fileno(c->dev), d->buffer, villas_sample_size);
		if(ret < 0)
			error("write");
		else if(ret == 0)
			break;	// comedi doesn't accept any more samples at the moment
		else if(ret == villas_sample_size)
			villas_samples_written++;
		else
			error("Only partial sample written (%d bytes), oops", ret);
	}

	if(villas_samples_written == 0) {
		warn("Nothing done");
	}

	d->counter += villas_samples_written;

	return villas_samples_written;
}


char* comedi_cmd_trigger_src(unsigned int src, char *buf)
{
	buf[0] = 0;

	if(src & TRIG_NONE)		strcat(buf, "none|");
	if(src & TRIG_NOW)		strcat(buf, "now|");
	if(src & TRIG_FOLLOW)	strcat(buf, "follow|");
	if(src & TRIG_TIME)		strcat(buf, "time|");
	if(src & TRIG_TIMER)	strcat(buf, "timer|");
	if(src & TRIG_COUNT)	strcat(buf, "count|");
	if(src & TRIG_EXT)		strcat(buf, "ext|");
	if(src & TRIG_INT)		strcat(buf, "int|");
#ifdef TRIG_OTHER
	if(src & TRIG_OTHER)	strcat(buf, "other|");
#endif

	if(strlen(buf) == 0) {
		sprintf(buf, "unknown(0x%08x)", src);
	} else {
		buf[strlen(buf) - 1] = 0;
	}

	return buf;
}

void comedi_dump_cmd(comedi_cmd *cmd, int debug_level)
{
	char buf[256];
	char* src;

	debug(LOG_COMEDI | debug_level, "subdevice:           %u", cmd->subdev);

	src = comedi_cmd_trigger_src(cmd->start_src, buf);
	debug(LOG_COMEDI | debug_level, "start:      %-8s %u", src, cmd->start_arg);

	src = comedi_cmd_trigger_src(cmd->scan_begin_src, buf);
	debug(LOG_COMEDI | debug_level, "scan_begin: %-8s %u", src, cmd->scan_begin_arg);

	src = comedi_cmd_trigger_src(cmd->convert_src, buf);
	debug(LOG_COMEDI | debug_level, "convert:    %-8s %u", src, cmd->convert_arg);

	src = comedi_cmd_trigger_src(cmd->scan_end_src, buf);
	debug(LOG_COMEDI | debug_level, "scan_end:   %-8s %u", src, cmd->scan_end_arg);

	src = comedi_cmd_trigger_src(cmd->stop_src,buf);
	debug(LOG_COMEDI | debug_level, "stop:       %-8s %u", src, cmd->stop_arg);
}


int comedi_fd(struct node *n)
{
	struct comedi *c = (struct comedi *) n->_vd;
	return comedi_fileno(c->dev);
}

static struct plugin p = {
	.name			= "comedi",
	.description	= "Comedi-compatible DAQ/ADC cards",
	.type			= PLUGIN_TYPE_NODE,
	.node			= {
		.vectorize	= 0,
		.size		= sizeof(struct comedi),
		.parse		= comedi_parse,
		.print		= comedi_print,
		.start		= comedi_start,
		.stop		= comedi_stop,
		.read		= comedi_read,
		.write		= comedi_write,
	    .fd			= comedi_fd
	}
};

REGISTER_PLUGIN(&p)
LIST_INIT_STATIC(&p.node.instances)