From fe302f9649f4e8728390410f2f0e292300c12846 Mon Sep 17 00:00:00 2001 From: Daniel Krebs Date: Fri, 15 Jun 2018 15:42:46 +0200 Subject: [PATCH] comedi: implement ADC and DAC directions, only DAC tested The write / DAC direction has been tested with two output signals, see etc/comedi.conf for reference. For now, the buffer size may not be (considerably) smaller than 32kB, Comedi stops working for unknown reasons. To compensate for the latency (always approx. one buffer size) if only small sample rates are required, configure the path for upsampling (sample-and-hold via rate parameter) at the same rate as the out direction of the comedi node. --- etc/comedi.conf | 84 ++++ include/villas/log.h | 39 +- include/villas/nodes/comedi.h | 43 +- lib/nodes/comedi.c | 905 ++++++++++++++++++++++++++++++---- 4 files changed, 948 insertions(+), 123 deletions(-) create mode 100644 etc/comedi.conf diff --git a/etc/comedi.conf b/etc/comedi.conf new file mode 100644 index 000000000..30524df01 --- /dev/null +++ b/etc/comedi.conf @@ -0,0 +1,84 @@ +/** Example configuration file for VILLASnode/comedi. + * + * The syntax of this file is similar to JSON. + * A detailed description of the format can be found here: + * http://www.hyperrealm.com/libconfig/libconfig_manual.html#Configuration-Files + * + * @author Daniel Krebs + * @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 . + *********************************************************************************/ + +nodes = { + pcie6259 = { + type = "comedi", + device = "/dev/comedi0", + in = { + subdevice = 0, + rate = 1000, + signals = ( + # note: order in this array defines order in villas sample + { channel = 0, range = 0, aref = 0, name = "temperature_int" }, + { channel = 1, range = 0, aref = 0, name = "loopback_ao0" }, + { channel = 2, range = 0, aref = 0, name = "loopback_ao1" }, + { channel = 3, range = 0, aref = 0, name = "bnc_ext" } + ) + }, + out = { + subdevice = 1, + # Note: buffer size and rate shouldn't be changed at the moment + # output sample rate + rate = 10000, + # comedi write buffer in kilobytes + bufsize = 32, + signals = ( + # note: order in this array corresponds to order in villas sample + { name = "ao0", channel = 0, range = 0, aref = 0 }, + { name = "ao1", channel = 1, range = 0, aref = 0 } + ) + } + }, + + remote = { + type = "socket", + layer = "udp" + format = "protobuf", + local = "*:12000" + remote = "134.130.169.32:12000" + }, + + sine1 = { + type = "signal", + signal = "sine", + values = 1, + frequency = 50, + rate = 10000, + }, + + sine2 = { + type = "signal", + signal = "sine", + values = 1, + frequency = 100, + rate = 10000, + } +} + +paths = ( + { in = ("sine1.data[0]", "sine2.data[0]"), out = "pcie6259", rate = 10000, mask = () } +) diff --git a/include/villas/log.h b/include/villas/log.h index 22ca4b363..7188ef31e 100644 --- a/include/villas/log.h +++ b/include/villas/log.h @@ -55,30 +55,31 @@ extern "C" { * To be or-ed with the debug level */ enum log_facilities { - LOG_POOL = (1L << 8), - LOG_QUEUE = (1L << 9), + LOG_POOL = (1L << 8), + LOG_QUEUE = (1L << 9), LOG_CONFIG = (1L << 10), - LOG_HOOK = (1L << 11), - LOG_PATH = (1L << 12), - LOG_NODE = (1L << 13), - LOG_MEM = (1L << 14), - LOG_WEB = (1L << 15), - LOG_API = (1L << 16), - LOG_LOG = (1L << 17), - LOG_VFIO = (1L << 18), - LOG_PCI = (1L << 19), - LOG_XIL = (1L << 20), - LOG_TC = (1L << 21), - LOG_IF = (1L << 22), - LOG_ADVIO = (1L << 23), + LOG_HOOK = (1L << 11), + LOG_PATH = (1L << 12), + LOG_NODE = (1L << 13), + LOG_MEM = (1L << 14), + LOG_WEB = (1L << 15), + LOG_API = (1L << 16), + LOG_LOG = (1L << 17), + LOG_VFIO = (1L << 18), + LOG_PCI = (1L << 19), + LOG_XIL = (1L << 20), + LOG_TC = (1L << 21), + LOG_IF = (1L << 22), + LOG_ADVIO = (1L << 23), /* Node-types */ LOG_SOCKET = (1L << 24), - LOG_FILE = (1L << 25), - LOG_FPGA = (1L << 26), - LOG_NGSI = (1L << 27), + LOG_FILE = (1L << 25), + LOG_FPGA = (1L << 26), + LOG_NGSI = (1L << 27), LOG_WEBSOCKET = (1L << 28), - LOG_OPAL = (1L << 30), + LOG_OPAL = (1L << 30), + LOG_COMEDI = (1L << 31), /* Classes */ LOG_NODES = LOG_NODE | LOG_SOCKET | LOG_FILE | LOG_FPGA | LOG_NGSI | LOG_WEBSOCKET | LOG_OPAL, diff --git a/include/villas/nodes/comedi.h b/include/villas/nodes/comedi.h index fd61d52ff..6da5fd647 100644 --- a/include/villas/nodes/comedi.h +++ b/include/villas/nodes/comedi.h @@ -33,15 +33,33 @@ #include #include +#include + +// whether to use read() or mmap() kernel interface +#define COMEDI_USE_READ (1) +//#define COMEDI_USE_READ (0) + +struct comedi_chanspec { + unsigned int maxdata; + comedi_range *range; +}; struct comedi_direction { - double rate; + int subdevice; ///< Comedi subdevice + int buffer_size; ///< Comedi's kernel buffer size + int sample_size; ///< Size of a single measurement sample + int sample_rate_hz; ///< Sample rate in Hz + bool present; ///< Config present + bool enabled; ///< Card is started successfully + bool running; ///< Card is actively transfering samples + struct timespec started; ///< Timestamp when sampling started + int counter; ///< Number of villas samples transfered + struct comedi_chanspec *chanspecs; ///< Range and maxdata config of channels + unsigned *chanlist; ///< Channel list in comedi's packed format + size_t chanlist_len; ///< Number of channels for this direction - bool enabled; - int subdevice; - - unsigned *chanlist; - size_t chanlist_len; + char* buffer; + char* bufptr; }; struct comedi { @@ -49,7 +67,18 @@ struct comedi { struct comedi_direction in, out; - comedi_t *it; + comedi_t *dev; + +#if COMEDI_USE_READ + char* buf; + char* bufptr; +#else + char *map; + size_t bufpos; + size_t front; + size_t back; +#endif + }; /** @see node_type::print */ diff --git a/lib/nodes/comedi.c b/lib/nodes/comedi.c index 17d103602..72026318c 100644 --- a/lib/nodes/comedi.c +++ b/lib/nodes/comedi.c @@ -1,6 +1,7 @@ /** Node type: comedi * * @author Steffen Vogel + * @author Daniel Krebs * @copyright 2018, Institute for Automation of Complex Power Systems, EONERC * @license GNU General Public License (version 3) * @@ -21,25 +22,35 @@ *********************************************************************************/ #include +#include +#include +#include +#include #include #include #include -#include -static int comedi_parse_direction(struct comedi_direction *d, json_t *cfg) +// 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: o, s: F }", + ret = json_unpack_ex(cfg, &err, 0, "{ s?: i, s?: i, s: o, s: i }", "subdevice", &d->subdevice, - "channels", &json_chans, - "rate", &d->rate + "bufsize", &d->buffer_size, + "signals", &json_chans, + "rate", &d->sample_rate_hz ); if (ret) jerror(&err, "Failed to parse configuration"); @@ -47,63 +58,196 @@ static int comedi_parse_direction(struct comedi_direction *d, json_t *cfg) 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) { - if (!json_is_integer(json_chan)) - return -1; + 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; - if (d->subdevice >= 0) { - - } - else { - d->subdevice = comedi_find_subdevice_by_type(c->it, COMEDI_SUBD_AI, 0); + // 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)); } - /* Check if subdevice is usable */ - ret = comedi_get_subdevice_type(c->it, 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)); - ret = comedi_get_subdevice_flags(c->it, d->subdevice); - if (ret ) - ret = comedi_lock(c->it, d->subdevice); - if (ret) - error("Failed to lock subdevice %d for node '%s'", d->subdevice, 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)); -#if 0 - comedi_cmd cmd = { - .subdev = d->subdevice, - .flags = CMDF_READ, - .start_src = TRIG_INT, - .start_arg = 0, - .scan_begin_src = TRIG_TIMER, - .scan_begin_arg = 1e9 / c->in.rate, - .convert_src = TRIG_NOW, - .convert_arg = 0, - .scan_end_src = TRIG_COUNT, - .scan_end_arg = c->in.chanlist_len, - .stop_src = TRIG_NONE, - .stop_arg = 0, - .chanlist = c->in.chanlist, - .chanlist_len = c->in.chanlist_len, - }; + 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:"); { INDENT + 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; @@ -115,35 +259,104 @@ static int comedi_start_out(struct node *n) struct comedi *c = (struct comedi *) n->_vd; struct comedi_direction *d = &c->out; - ret = comedi_get_subdevice_type(c->it, d->subdevice); - if (ret != COMEDI_SUBD_AO) - error("Output subdevice of node '%s' is not an analog output", node_name(n)); + // 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->it, d->subdevice); - if (ret ) + 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)); - ret = comedi_lock(c->it, d->subdevice); - if (ret) - error("Failed to lock subdevice %d for node '%s'", d->subdevice, node_name(n)); -#if 0 - comedi_cmd cmd = { - .subdev = c->out.subdevice, - .flags = CMDF_WRITE, - .start_src = TRIG_INT, - .start_arg = 0, - .scan_begin_src = TRIG_TIMER, - .scan_begin_arg = 1e9 / c->out.rate, - .convert_src = TRIG_NOW, - .convert_arg = 0, - .scan_end_src = TRIG_COUNT, - .scan_end_arg = c->out.chanlist_len, - .stop_src = TRIG_NONE, - .stop_arg = 0, - .chanlist = c->out.chanlist, - .chanlist_len = c->out.chanlist_len, - }; -#endif + 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:"); { INDENT + 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; + + // 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"); + } + } return 0; } @@ -154,7 +367,9 @@ static int comedi_stop_in(struct node *n) struct comedi *c = (struct comedi *) n->_vd; struct comedi_direction *d = &c->in; - ret = comedi_unlock(c->it, d->subdevice); + 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)); @@ -167,7 +382,9 @@ static int comedi_stop_out(struct node *n) struct comedi *c = (struct comedi *) n->_vd; struct comedi_direction *d = &c->out; - ret = comedi_unlock(c->it, d->subdevice); + 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)); @@ -185,28 +402,39 @@ int comedi_parse(struct node *n, json_t *cfg) json_t *json_out = NULL; json_error_t err; - ret = json_unpack_ex(cfg, &err, 0, "{ s: s }", - "device", &device, - "in", &json_in, - "out", &json_out - ); + 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)); - if (json_in) { - ret = comedi_parse_direction(&c->in, json_in); + 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 -1; + return ret; } - if (json_out) { - ret = comedi_parse_direction(&c->out, json_out); + if (c->out.present) { + ret = comedi_parse_direction(c, &c->out, json_out); if (ret) - return -1; + return ret; } c->device = strdup(device); + n->samplelen = c->in.chanlist_len; + return 0; } @@ -216,8 +444,8 @@ char * comedi_print(struct node *n) char *buf = NULL; - const char *board = comedi_get_board_name(c->it); - const char *driver = comedi_get_driver_name(c->it); + 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); @@ -228,14 +456,45 @@ int comedi_start(struct node *n) { struct comedi *c = (struct comedi *) n->_vd; - c->it = comedi_open(c->device); - if (!c->it) { + c->dev = comedi_open(c->device); + if (!c->dev) { const char *err = comedi_strerror(comedi_errno()); error("Failed to open device: %s", err); } - comedi_start_in(n); - comedi_start_out(n); + // 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; } @@ -245,42 +504,496 @@ int comedi_stop(struct node *n) int ret; struct comedi *c = (struct comedi *) n->_vd; - comedi_stop_in(n); - comedi_stop_out(n); + if(c->in.enabled) + comedi_stop_in(n); - ret = comedi_close(c->it); + 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) { -// struct comedi *c = (struct comedi *) n->_vd; + 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) +{ + 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) { -// struct comedi *c = (struct comedi *) n->_vd; + int ret; + struct comedi *c = (struct comedi *) n->_vd; + struct comedi_direction *d = &c->out; - return -1; + 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 if(villas_samples_in_buffer % 1000 == 0) { + info("Comedi buffer: %4ld / %ld villas samples", + villas_samples_in_buffer, buffer_capacity_villas); + } + + 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->it); + return comedi_fileno(c->dev); } static struct plugin p = { - .name = "comedi", + .name = "comedi", .description = "Comedi-compatible DAQ/ADC cards", - .type = PLUGIN_TYPE_NODE, - .node = { + .type = PLUGIN_TYPE_NODE, + .node = { .vectorize = 0, .size = sizeof(struct comedi), .parse = comedi_parse, @@ -289,9 +1002,7 @@ static struct plugin p = { .stop = comedi_stop, .read = comedi_read, .write = comedi_write, -// .create = comedi_create, -// .destroy = comedi_destroy, - .fd = comedi_fd + .fd = comedi_fd } };