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
