diff options
Diffstat (limited to 'cpukit/libmisc/redirector/stdio-redirect.c')
| -rw-r--r-- | cpukit/libmisc/redirector/stdio-redirect.c | 333 |
1 files changed, 333 insertions, 0 deletions
diff --git a/cpukit/libmisc/redirector/stdio-redirect.c b/cpukit/libmisc/redirector/stdio-redirect.c new file mode 100644 index 0000000000..e6610cbba1 --- /dev/null +++ b/cpukit/libmisc/redirector/stdio-redirect.c @@ -0,0 +1,333 @@ +/* + * Copyright (C) 2014 Chris Johns (chrisj@rtems.org) + * + * The license and distribution terms for this file may be + * found in the file LICENSE in this distribution. + * + * This software with is provided ``as is'' and with NO WARRANTY. + */ + +/* + * RTEMS std redirector. + */ + +#include <errno.h> +#include <stdio.h> +#include <stdlib.h> +#include <strings.h> +#include <sys/types.h> +#include <unistd.h> + +#include <rtems.h> +#include <rtems/error.h> + +#include "stdio-redirect.h" + +#define RTEMS_STDIO_REDIRECT_LOCK_ATTRIBS \ + (RTEMS_PRIORITY | RTEMS_BINARY_SEMAPHORE | \ + RTEMS_INHERIT_PRIORITY | RTEMS_NO_PRIORITY_CEILING | RTEMS_LOCAL) + +#define RTEMS_STDIO_REDIRECT_RUNNING (1 << 0) +#define RTEMS_STDIO_REDIRECT_FINISHED (1 << 1) + +static bool +rtems_stdio_redirect_lock(rtems_stdio_redirect_t* sr) +{ + rtems_status_code sc = rtems_semaphore_obtain (sr->lock, + RTEMS_WAIT, + RTEMS_NO_TIMEOUT); + if (sc != RTEMS_SUCCESSFUL) + { + fprintf(stderr, "error: stdio-redirect: lock failed: %s\n", rtems_status_text(sc)); + return false; + } + return true; +} + +static bool +rtems_stdio_redirect_unlock(rtems_stdio_redirect_t* sr) +{ + rtems_status_code sc = rtems_semaphore_release (sr->lock); + if (sc != RTEMS_SUCCESSFUL) + { + fprintf(stderr, "error: stdio-redirect: unlock failed: %s\n", rtems_status_text(sc)); + return false; + } + return true; +} + +static bool +rtems_stdio_redirect_write (rtems_stdio_redirect_t* sr, const char* buf, ssize_t len) +{ + if (!rtems_stdio_redirect_lock(sr)) + return false; + + if (sr->buffer) + { + if (len >= sr->buffer_size) + { + ssize_t offset = len - sr->buffer_size; + memcpy (sr->buffer, buf + offset, sr->buffer_size); + sr->in = 0; + sr->full = true; + } + else + { + if ((sr->in + len) > sr->buffer_size) + { + ssize_t bytes = sr->buffer_size - sr->in; + memcpy (sr->buffer + sr->in, buf, bytes); + buf += bytes; + len -= bytes; + sr->in = 0; + sr->full = true; + } + else + { + memcpy (sr->buffer + sr->in, buf, len); + sr->in += len; + } + } + } + + if (sr->handler) + sr->handler(buf, len); + + return rtems_stdio_redirect_lock(sr); +} + +static rtems_task +rtems_stdio_redirect_reader(rtems_task_argument arg) +{ + rtems_stdio_redirect_t* sr = (rtems_stdio_redirect_t*) arg; + + while (sr->state & RTEMS_STDIO_REDIRECT_RUNNING) + { + ssize_t r = read (sr->pipe[0], sr->input, sr->input_size); + + if (r <= 0) + break; + + if (sr->echo) + write (sr->fd_dup, sr->input, r); + + if (!rtems_stdio_redirect_write (sr, sr->input, r)) + break; + } + + sr->state |= RTEMS_STDIO_REDIRECT_FINISHED; + + rtems_task_delete(RTEMS_SELF); +} + +rtems_stdio_redirect_t* +rtems_stdio_redirect_open(int fd, + rtems_task_priority priority, + size_t stack_size, + ssize_t input_size, + ssize_t buffer_size, + bool echo, + rtems_stdio_redirect_handler handler) +{ + rtems_stdio_redirect_t* sr; + rtems_name name; + rtems_status_code sc; + + sr = malloc(sizeof(rtems_stdio_redirect_t)); + if (!sr) + { + fprintf(stderr, "error: stdio-redirect: no memory\n"); + return NULL; + } + + memset(sr, 0, sizeof(*sr)); + + sr->input_size = input_size; + sr->input = malloc(input_size); + if (!sr->input) + { + fprintf(stderr, "error: stdio-redirect: no memory for input\n"); + free(sr); + return NULL; + } + + if (buffer_size) + { + sr->buffer_size = buffer_size; + sr->buffer = malloc(buffer_size); + if (!sr->buffer) + { + fprintf(stderr, "error: stdio-redirect: no memory for buffer\n"); + free(sr->input); + free(sr); + return NULL; + } + } + + sr->fd = fd; + + sr->fd_dup = dup(fd); + if (sr->fd_dup < 0) + { + fprintf(stderr, "error: stdio-redirect: dup: %s\n", strerror(errno)); + free(sr->buffer); + free(sr->input); + free(sr); + return NULL; + } + + if (pipe(sr->pipe) < 0) + { + fprintf(stderr, "error: stdio-redirect: pipe create: %s\n", strerror(errno)); + free(sr->buffer); + free(sr->input); + free(sr); + return NULL; + + } + + sr->echo = echo; + sr->handler = handler; + + sc = rtems_semaphore_create (rtems_build_name ('R', 'S', 'R', 'l'), + 1, RTEMS_STDIO_REDIRECT_LOCK_ATTRIBS, 0, + &sr->lock); + if (sc != RTEMS_SUCCESSFUL) + { + fprintf(stderr, "error: stdio-redirect: lock create: %s\n", rtems_status_text(sc)); + free(sr->buffer); + free(sr->input); + free(sr); + return NULL; + } + + name = rtems_build_name ('S', 'R', '0' + (fd / 10), '0' + (fd % 10)); + sc = rtems_task_create (name, + priority, + stack_size, + RTEMS_PREEMPT | RTEMS_NO_TIMESLICE | RTEMS_NO_ASR, + RTEMS_LOCAL | RTEMS_NO_FLOATING_POINT, + &sr->reader); + if (sc != RTEMS_SUCCESSFUL) + { + fprintf(stderr, "error: stdio-redirect: reader create: %s\n", rtems_status_text(sc)); + rtems_semaphore_delete(sr->lock); + free(sr->buffer); + free(sr->input); + free(sr); + return NULL; + } + + sr->state |= RTEMS_STDIO_REDIRECT_RUNNING; + + if (dup2 (sr->pipe[1], fd) < 0) + { + fprintf(stderr, "error: stdio-redirect: dup2: %s\n", strerror(errno)); + free(sr->buffer); + free(sr->input); + free(sr); + return NULL; + } + + sc = rtems_task_start (sr->reader, + rtems_stdio_redirect_reader, + (rtems_task_argument) sr); + + if (sc != RTEMS_SUCCESSFUL) + { + fprintf(stderr, "error: stdio-redirect: reader start: %s\n", rtems_status_text(sc)); + rtems_task_delete(sr->reader); + rtems_semaphore_delete(sr->lock); + free(sr->buffer); + free(sr->input); + free(sr); + return NULL; + } + + return sr; +} + +void +rtems_stdio_redirect_close(rtems_stdio_redirect_t* sr) +{ + if (rtems_stdio_redirect_lock(sr)) + { + sr->state &= ~RTEMS_STDIO_REDIRECT_RUNNING; + close(sr->pipe[0]); + + rtems_stdio_redirect_unlock(sr); + + while (sr->state & RTEMS_STDIO_REDIRECT_FINISHED) + { + usleep(250 * 1000 * 1000); + } + + rtems_stdio_redirect_lock(sr); + + dup2(sr->fd, sr->fd_dup); + + free(sr->buffer); + free(sr->input); + + rtems_stdio_redirect_unlock(sr); + + rtems_semaphore_delete(sr->lock); + + free(sr); + } +} + +ssize_t +rtems_stdio_redirect_read(rtems_stdio_redirect_t* sr, + char* buffer, + ssize_t length) +{ + ssize_t written = 0; + + if (!rtems_stdio_redirect_lock(sr)) + return written; + + if (sr->buffer) + { + ssize_t rem = 0; + + if (sr->full) + { + if (length < sr->buffer_size) + { + if (length > sr->in) + { + rem = length - sr->in; + memcpy (buffer, sr->buffer + sr->buffer_size - rem, rem); + } + + memcpy (buffer + rem, sr->buffer, sr->in); + written = length; + } + else + { + rem = sr->buffer_size - sr->in; + memcpy (buffer, sr->buffer + sr->in, rem); + memcpy (buffer + rem, sr->buffer, sr->in); + written = sr->buffer_size; + } + + sr->full = false; + } + else if (length < sr->in) + { + rem = sr->in - length; + memcpy (buffer, sr->buffer + rem, length); + written = length; + } + else + { + memcpy (buffer, sr->buffer, sr->in); + written = sr->in; + } + } + + rtems_stdio_redirect_unlock(sr); + + return written; +} |