Module: xenomai-forge Branch: next Commit: 48a82ea133d152958e16b086c4ef94dba04350ae URL: http://git.xenomai.org/?p=xenomai-forge.git;a=commit;h=48a82ea133d152958e16b086c4ef94dba04350ae
Author: Jorge Ramirez-Ortiz <j...@xenomai.org> Date: Sun Jul 6 11:51:20 2014 -0400 utils/analogy: cmd_read over posix interface --- utils/analogy/cmd_read.c | 472 +++++++++++++++++++++++++--------------------- 1 file changed, 252 insertions(+), 220 deletions(-) diff --git a/utils/analogy/cmd_read.c b/utils/analogy/cmd_read.c index 816bb30..c114331 100644 --- a/utils/analogy/cmd_read.c +++ b/utils/analogy/cmd_read.c @@ -27,40 +27,42 @@ #include <errno.h> #include <getopt.h> #include <string.h> -#include <alchemy/task.h> +#include <signal.h> +#include <pthread.h> #include <rtdm/analogy.h> -/* Default command's parameters */ +typedef int (*dump_function_t) (a4l_desc_t *, a4l_cmd_t*, unsigned char *, int); +static pthread_t thread; +struct arguments { + int argc; + char **argv; +}; -/* For read operation, we consider - the default subdevice index is 0 */ -#define ID_SUBD 0 -/* For simplicity sake, a maximum channel - count is defined */ #define MAX_NB_CHAN 32 -/* One hundred triggered scans by default */ #define NB_SCAN 100 +#define ID_SUBD 0 #define FILENAME "analogy0" - #define BUF_SIZE 10000 +static unsigned int chans[MAX_NB_CHAN]; static unsigned char buf[BUF_SIZE]; -static char *filename = FILENAME; static char *str_chans = "0,1,2,3"; -static unsigned int chans[MAX_NB_CHAN]; -static int verbose = 0; +static char *filename = FILENAME; + +static unsigned long wake_count = 0; static int real_time = 0; static int use_mmap = 0; -static unsigned long wake_count = 0; - -static RT_TASK rt_task_desc; +static int verbose = 0; +#define ERR(fmt, args ...) fprintf(stderr, fmt, ##args) +#define OUT(fmt, args ...) fprintf(stdout, fmt, ##args) #define DBG(fmt, args...) \ do { \ if (verbose) \ printf(fmt, ##args); \ - } while (0); + } while (0) + /* The command to send by default */ a4l_cmd_t cmd = { @@ -96,21 +98,17 @@ struct option cmd_read_opts[] = { static void do_print_usage(void) { - fprintf(stdout, "usage:\tcmd_read [OPTS]\n"); - fprintf(stdout, "\tOPTS:\t -v, --verbose: verbose output\n"); - fprintf(stdout, - "\t\t -r, --real-time: enable real-time acquisition mode\n"); - fprintf(stdout, - "\t\t -d, --device: device filename (analogy0, analogy1, ...)\n"); - fprintf(stdout, "\t\t -s, --subdevice: subdevice index\n"); - fprintf(stdout, "\t\t -S, --scan-count: count of scan to perform\n"); - fprintf(stdout, "\t\t -c, --channels: channels to use (ex.: -c 0,1)\n"); - fprintf(stdout, "\t\t -m, --mmap: mmap the buffer\n"); - fprintf(stdout, "\t\t -w, --raw: dump data in raw format\n"); - fprintf(stdout, - "\t\t -k, --wake-count: " - "space available before waking up the process\n"); - fprintf(stdout, "\t\t -h, --help: print this help\n"); + OUT("usage:\tcmd_read [OPTS]\n"); + OUT("\tOPTS:\t -v, --verbose: verbose output\n"); + OUT("\t\t -r, --real-time: enable real-time acquisition mode\n"); + OUT("\t\t -d, --device: device filename (analogy0, analogy1, ...)\n"); + OUT("\t\t -s, --subdevice: subdevice index\n"); + OUT("\t\t -S, --scan-count: count of scan to perform\n"); + OUT("\t\t -c, --channels: channels to use (ex.: -c 0,1)\n"); + OUT("\t\t -m, --mmap: mmap the buffer\n"); + OUT("\t\t -w, --raw: dump data in raw format\n"); + OUT("\t\t -k, --wake-count: space available before waking up the process\n"); + OUT("\t\t -h, --help: print this help\n"); } static int dump_raw(a4l_desc_t *dsc, a4l_cmd_t *cmd, unsigned char *buf, int size) @@ -120,35 +118,28 @@ static int dump_raw(a4l_desc_t *dsc, a4l_cmd_t *cmd, unsigned char *buf, int siz static int dump_text(a4l_desc_t *dsc, a4l_cmd_t *cmd, unsigned char *buf, int size) { - static int cur_chan; - + a4l_chinfo_t *chans[MAX_NB_CHAN]; int i, err = 0, tmp_size = 0; char *fmts[MAX_NB_CHAN]; - a4l_chinfo_t *chans[MAX_NB_CHAN]; + static int cur_chan; for (i = 0; i < cmd->nb_chan; i++) { int width; - err = a4l_get_chinfo(dsc, - cmd->idx_subd, - cmd->chan_descs[i], &chans[i]); + err = a4l_get_chinfo(dsc, cmd->idx_subd, cmd->chan_descs[i], &chans[i]); if (err < 0) { - fprintf(stderr, - "cmd_read: a4l_get_chinfo failed (ret=%d)\n", - err); + ERR("cmd_read: a4l_get_chinfo failed (ret=%d)\n", err); goto out; } width = a4l_sizeof_chan(chans[i]); if (width < 0) { - fprintf(stderr, - "cmd_read: incoherent info for channel %d\n", - cmd->chan_descs[i]); + ERR("cmd_read: incoherent info for channel %d\n", cmd->chan_descs[i]); err = width; goto out; } - switch(width) { + switch (width) { case 1: fmts[i] = "0x%02x "; break; @@ -164,46 +155,149 @@ static int dump_text(a4l_desc_t *dsc, a4l_cmd_t *cmd, unsigned char *buf, int si while (tmp_size < size) { unsigned long value; - err = a4l_rawtoul(chans[cur_chan], &value, buf + tmp_size, 1); if (err < 0) goto out; - fprintf(stdout, fmts[cur_chan], value); + OUT(fmts[cur_chan], value); - /* We assume a4l_sizeof_chan() cannot return because - we already called it on the very same channel - descriptor */ + /* We assume a4l_sizeof_chan() cannot return because we already + * called it on the very same channel descriptor */ tmp_size += a4l_sizeof_chan(chans[cur_chan]); - if(++cur_chan == cmd->nb_chan) { - fprintf(stdout, "\n"); + if (++cur_chan == cmd->nb_chan) { + OUT("\n"); cur_chan = 0; } } fflush(stdout); - out: return err; } -int main(int argc, char *argv[]) +static int fetch_data(a4l_desc_t *dsc, void *buf, unsigned int *cnt, dump_function_t dump) { - int ret = 0, len, ofs; - unsigned int i, scan_size = 0, cnt = 0; + int ret; + + for (;;) { + ret = a4l_async_read(dsc, buf, BUF_SIZE, A4L_INFINITE); + + if (ret == 0) { + DBG("cmd_read: no more data in the buffer \n"); + break; + } + + if (ret < 0) { + ERR("cmd_read: a4l_read failed (ret=%d)\n", ret); + return ret; + } + + *cnt += ret; + + ret = dump(dsc, &cmd, buf, ret); + if (ret < 0) + return -EIO; + } + + return ret; +} + +static int fetch_data_mmap(a4l_desc_t *dsc, unsigned int *cnt, dump_function_t dump, + void *map, unsigned long buf_size) +{ + unsigned long cnt_current = 0, cnt_updated = 0; + int ret; + + for (;;) { + + /* Retrieve and update the buffer's state + * In input case, recover how many bytes are available to read + */ + ret = a4l_mark_bufrw(dsc, cmd.idx_subd, cnt_current, &cnt_updated); + + if (ret == -ENOENT) + break; + + if (ret < 0) { + ERR("cmd_read: a4l_mark_bufrw() failed (ret=%d)\n", ret); + return ret; + } + + /* If there is nothing to read, wait for an event + (Note that a4l_poll() also retrieves the data amount + to read; in our case it is useless as we have to update + the data read counter) */ + if (!cnt_updated) { + ret = a4l_poll(dsc, cmd.idx_subd, A4L_INFINITE); + + if (ret == 0) + break; + + if (ret < 0) { + ERR("cmd_read: a4l_poll() failed (ret=%d)\n", ret); + return ret; + } + + cnt_current = cnt_updated; + continue; + } + + ret = dump(dsc, &cmd, map + (*cnt % buf_size), cnt_updated); + if (ret < 0) + return -EIO; + + *cnt += cnt_updated; + cnt_current = cnt_updated; + } + + return 0; +} + +static int map_subdevice_buffer(a4l_desc_t *dsc, unsigned long *buf_size, void **map) +{ + void *buf; + int ret; + + /* Get the buffer size to map */ + ret = a4l_get_bufsize(dsc, cmd.idx_subd, buf_size); + if (ret < 0) { + ERR("cmd_read: a4l_get_bufsize() failed (ret=%d)\n", ret); + return ret; + } + DBG("cmd_read: buffer size = %lu bytes\n", *buf_size); + + /* Map the analog input subdevice buffer */ + ret = a4l_mmap(dsc, cmd.idx_subd, *buf_size, &buf); + if (ret < 0) { + ERR("cmd_read: a4l_mmap() failed (ret=%d)\n", ret); + return ret; + } + DBG("cmd_read: mmap performed successfully (map=0x%p)\n", buf); + *map = buf; + + return 0; +} + +static void *cmd_read(void *arg) +{ + unsigned int i, scan_size = 0, cnt = 0, ret = 0, len, ofs; + dump_function_t dump_function = dump_text; + a4l_desc_t dsc = { .sbdata = NULL }; unsigned long buf_size; void *map = NULL; - a4l_desc_t dsc = { .sbdata = NULL }; - int (*dump_function) (a4l_desc_t *, a4l_cmd_t*, unsigned char *, int) = - dump_text; + struct arguments *p = arg; + char **argv = p->argv; + int argc = p->argc; + + for (;;) { + ret = getopt_long(argc, argv, "vrd:s:S:c:mwk:h", + cmd_read_opts, NULL); + + if (ret == -1) + break; - /* Compute arguments */ - while ((ret = getopt_long(argc, - argv, - "vrd:s:S:c:mwk:h", - cmd_read_opts, NULL)) >= 0) { switch (ret) { case 'v': verbose = 1; @@ -235,14 +329,14 @@ int main(int argc, char *argv[]) case 'h': default: do_print_usage(); - return 0; + pthread_exit(0); } } if (isatty(STDOUT_FILENO) && dump_function == dump_raw) { - fprintf(stderr, - "cmd_read: cannot dump raw data on a terminal\n\n"); - return -EINVAL; + ERR("cmd_read: cannot dump raw data on a terminal\n\n"); + ret = -EINVAL; + pthread_exit(&ret); } /* Recover the channels to compute */ @@ -251,8 +345,9 @@ int main(int argc, char *argv[]) len = strlen(str_chans); ofs = strcspn(str_chans, ","); if (sscanf(str_chans, "%u", &chans[cmd.nb_chan - 1]) == 0) { - fprintf(stderr, "cmd_read: bad channel argument\n"); - return -EINVAL; + ERR("cmd_read: bad channel argument\n"); + ret = -EINVAL; + pthread_exit(&ret); } str_chans += ofs + 1; } while (len != ofs); @@ -261,35 +356,10 @@ int main(int argc, char *argv[]) cmd.scan_end_arg = cmd.nb_chan; cmd.stop_src = cmd.stop_arg != 0 ? TRIG_COUNT : TRIG_NONE; - if (real_time != 0) { - - DBG("cmd_read: switching to real-time mode\n"); - - /* Prevent any memory-swapping for this program */ - ret = mlockall(MCL_CURRENT | MCL_FUTURE); - if (ret < 0) { - ret = errno; - fprintf(stderr, "cmd_read: mlockall failed (ret=%d)\n", - ret); - goto out_main; - } - - /* Turn the current process into an RT task */ - ret = rt_task_shadow(&rt_task_desc, NULL, 1, 0); - if (ret < 0) { - fprintf(stderr, - "cmd_read: rt_task_shadow failed (ret=%d)\n", - ret); - goto out_main; - } - } - - /* Open the device */ ret = a4l_open(&dsc, filename); if (ret < 0) { - fprintf(stderr, "cmd_read: a4l_open %s failed (ret=%d)\n", - filename, ret); - return ret; + ERR("cmd_read: a4l_open %s failed (ret=%d)\n", filename, ret); + pthread_exit(&ret); } DBG("cmd_read: device %s opened (fd=%d)\n", filename, dsc.fd); @@ -301,32 +371,26 @@ int main(int argc, char *argv[]) /* Allocate a buffer so as to get more info (subd, chan, rng) */ dsc.sbdata = malloc(dsc.sbsize); if (dsc.sbdata == NULL) { - fprintf(stderr, "cmd_read: malloc failed \n"); - return -ENOMEM; + ERR("cmd_read: malloc failed \n"); + ret = -ENOMEM; + pthread_exit(&ret); } /* Get this data */ ret = a4l_fill_desc(&dsc); if (ret < 0) { - fprintf(stderr, - "cmd_read: a4l_fill_desc failed (ret=%d)\n", ret); - goto out_main; + ERR("cmd_read: a4l_fill_desc failed (ret=%d)\n", ret); + goto out; } - - DBG("cmd_read: complex descriptor retrieved\n"); /* Get the size of a single acquisition */ for (i = 0; i < cmd.nb_chan; i++) { a4l_chinfo_t *info; - - ret = a4l_get_chinfo(&dsc, - cmd.idx_subd, cmd.chan_descs[i], &info); + ret = a4l_get_chinfo(&dsc,cmd.idx_subd, cmd.chan_descs[i], &info); if (ret < 0) { - fprintf(stderr, - "cmd_read: a4l_get_chinfo failed (ret=%d)\n", - ret); - goto out_main; + ERR("cmd_read: a4l_get_chinfo failed (ret=%d)\n", ret); + goto out; } DBG("cmd_read: channel %x\n", cmd.chan_descs[i]); @@ -335,152 +399,120 @@ int main(int argc, char *argv[]) scan_size += a4l_sizeof_chan(info); } - DBG("cmd_read: scan size = %u\n", scan_size); DBG("cmd_read: size to read = %u\n", scan_size * cmd.stop_arg); /* Cancel any former command which might be in progress */ a4l_snd_cancel(&dsc, cmd.idx_subd); - if (use_mmap != 0) { - - /* Get the buffer size to map */ - ret = a4l_get_bufsize(&dsc, cmd.idx_subd, &buf_size); - if (ret < 0) { - fprintf(stderr, - "cmd_read: a4l_get_bufsize() failed (ret=%d)\n", - ret); - goto out_main; - } - - DBG("cmd_read: buffer size = %lu bytes\n", buf_size); - - /* Map the analog input subdevice buffer */ - ret = a4l_mmap(&dsc, cmd.idx_subd, buf_size, &map); - if (ret < 0) { - fprintf(stderr, - "cmd_read: a4l_mmap() failed (ret=%d)\n", - ret); - goto out_main; - } - - DBG("cmd_read: mmap performed successfully (map=0x%p)\n",map); + if (use_mmap) { + ret = map_subdevice_buffer(&dsc, &buf_size, &map); + if (ret) + goto out; } ret = a4l_set_wakesize(&dsc, wake_count); if (ret < 0) { - fprintf(stderr, - "cmd_read: a4l_set_wakesize failed (ret=%d)\n", ret); - goto out_main; + ERR("cmd_read: a4l_set_wakesize failed (ret=%d)\n", ret); + goto out; } - DBG("cmd_read: wake size successfully set (%lu)\n", wake_count); /* Send the command to the input device */ ret = a4l_snd_command(&dsc, &cmd); if (ret < 0) { - fprintf(stderr, "cmd_read: a4l_snd_command failed (ret=%d)\n", ret); - goto out_main; + ERR("cmd_read: a4l_snd_command failed (ret=%d)\n", ret); + goto out; } - DBG("cmd_read: command successfully sent\n"); - if (use_mmap == 0) { - - /* Fetch data */ - do { - /* Perform the read operation */ - ret = a4l_async_read(&dsc, buf, BUF_SIZE, A4L_INFINITE); - if (ret < 0) { - fprintf(stderr, - "cmd_read: a4l_read failed (ret=%d)\n", - ret); - goto out_main; - } + if (!use_mmap) { + ret = fetch_data(&dsc, buf, &cnt, dump_function); + if (ret) { + ERR("cmd_read: failed to fetch_data (ret=%d)\n", ret); + goto out; + } + } + else { + ret = fetch_data_mmap(&dsc, &cnt, dump_function, map, buf_size); + if (ret) { + ERR("cmd_read: failed to fetch_data_mmap (ret=%d)\n", ret); + goto out; + } + } + DBG("cmd_read: %d bytes successfully received\n", cnt); - DBG("cmd_read: read %d bytes \n", ret); + pthread_exit(NULL); - /* Display the results */ - if (dump_function(&dsc, &cmd, buf, ret) < 0) { - ret = -EIO; - goto out_main; - } + return NULL; +out: + if (use_mmap != 0) + munmap(map, buf_size); - /* Update the counter */ - cnt += ret; + /* Free the buffer used as device descriptor */ + if (dsc.sbdata != NULL) + free(dsc.sbdata); - if (ret == 0) { - DBG("cmd_read: no more data in the buffer \n"); - } + /* Release the file descriptor */ + a4l_close(&dsc); - } while (ret > 0); + pthread_exit(&ret); +} - } else { - unsigned long front = 0; +static void cleanup_upon_sig(int sig) +{ + pthread_cancel(thread); + signal(sig, SIG_DFL); +} - /* Fetch data without any memcpy */ - do { +int main(int argc, char *argv[]) +{ + struct sched_param param = {.sched_priority = 99}; + struct arguments args = {.argc = argc, .argv = argv}; + pthread_attr_t attr; + sigset_t mask; + int ret; - /* Retrieve and update the buffer's state - (In input case, we recover how many bytes are available - to read) */ - ret = a4l_mark_bufrw(&dsc, cmd.idx_subd, front, &front); - if (ret == -ENOENT) - break; - else if (ret < 0) { - fprintf(stderr, - "cmd_read: a4l_mark_bufrw() failed (ret=%d)\n", - ret); - goto out_main; - } + sigemptyset(&mask); - /* If there is nothing to read, wait for an event - (Note that a4l_poll() also retrieves the data amount - to read; in our case it is useless as we have to update - the data read counter) */ - if (front == 0) { - ret = a4l_poll(&dsc, cmd.idx_subd, A4L_INFINITE); - if (ret == 0) - break; - else if (ret < 0) { - fprintf(stderr, - "cmd_read: a4l_poll() failed (ret=%d)\n", - ret); - goto out_main; - } - } + sigaddset(&mask, SIGTERM); + sigaddset(&mask, SIGINT); + sigaddset(&mask, SIGHUP); - /* Display the results */ - if (dump_function(&dsc, - &cmd, - &((unsigned char *)map)[cnt % buf_size], - front) < 0) { - ret = -EIO; - goto out_main; - } + signal(SIGTERM, cleanup_upon_sig); + signal(SIGINT, cleanup_upon_sig); + signal(SIGHUP, cleanup_upon_sig); - /* Update the counter */ - cnt += front; + pthread_sigmask(SIG_BLOCK, &mask, NULL); - } while (1); + ret = mlockall(MCL_CURRENT | MCL_FUTURE); + if (ret < 0) { + ret = errno; + ERR("cmd_read: mlockall failed (ret=%d)\n", ret); + return -1; } - DBG("cmd_read: %d bytes successfully received\n", cnt); - - ret = 0; - -out_main: + /* delegate all the processing to a realtime thread */ + pthread_attr_init(&attr); + pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE); + pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED); + pthread_attr_setschedpolicy(&attr, SCHED_FIFO); - if (use_mmap != 0) - /* Clean the pages table */ - munmap(map, buf_size); + ret = pthread_attr_setschedparam(&attr, ¶m); + if (ret) { + ERR("pthread_attr_setschedparam failed (ret=%d)\n", ret); + return -1; + } - /* Free the buffer used as device descriptor */ - if (dsc.sbdata != NULL) - free(dsc.sbdata); + ret = pthread_create(&thread, &attr, &cmd_read, &args); + if (ret) { + ERR("pthread_create failed (ret=%d)\n", ret); + return -1; + } - /* Release the file descriptor */ - a4l_close(&dsc); + pthread_join(thread, (void **) &ret); + if (ret) + ERR("cmd_read failed (ret=%d) \n", ret); return ret; } _______________________________________________ Xenomai-git mailing list Xenomai-git@xenomai.org http://www.xenomai.org/mailman/listinfo/xenomai-git