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 } };