diff --git a/platforms/nuttx/src/px4/common/console_buffer.cpp b/platforms/nuttx/src/px4/common/console_buffer.cpp index c949421d2670..6a35a2bdd9c5 100644 --- a/platforms/nuttx/src/px4/common/console_buffer.cpp +++ b/platforms/nuttx/src/px4/common/console_buffer.cpp @@ -206,13 +206,57 @@ ssize_t console_buffer_write(struct file *filep, const char *buffer, size_t len) return len; } +ssize_t console_buffer_read(struct file *filep, char *buffer, size_t buflen) +{ + return g_console_buffer.read(buffer, buflen, (int *)&filep->f_pos); +} + +off_t console_buffer_seek(struct file *filep, off_t offset, int whence) +{ + switch (whence) { + case SEEK_SET: + filep->f_pos = offset; + break; + + case SEEK_CUR: + filep->f_pos += offset; + break; + + case SEEK_END: + filep->f_pos = (g_console_buffer.size() + offset) % BOARD_CONSOLE_BUFFER_SIZE; + break; + + default: + return -1; + } + + return filep->f_pos; +} + +int console_buffer_ioctl(struct file *filep, int cmd, unsigned long arg) +{ + int ret = OK; + + switch (cmd) { + case FIONSPACE: + *(int *)arg = g_console_buffer.size(); + break; + + default: + ret = -ENOTTY; + break; + } + + return ret; +} + static const struct file_operations g_console_buffer_fops = { NULL, /* open */ NULL, /* close */ - NULL, /* read */ + console_buffer_read, /* read */ console_buffer_write, /* write */ - NULL, /* seek */ - NULL /* ioctl */ + console_buffer_seek, /* seek */ + console_buffer_ioctl /* ioctl */ #ifndef CONFIG_DISABLE_POLL , NULL /* poll */ #endif diff --git a/platforms/nuttx/src/px4/common/console_buffer_usr.cpp b/platforms/nuttx/src/px4/common/console_buffer_usr.cpp index 83023f934d0e..643fbdcf7d2e 100644 --- a/platforms/nuttx/src/px4/common/console_buffer_usr.cpp +++ b/platforms/nuttx/src/px4/common/console_buffer_usr.cpp @@ -38,28 +38,56 @@ #include #include #include +#include +#include #ifdef BOARD_ENABLE_CONSOLE_BUFFER -#ifndef BOARD_CONSOLE_BUFFER_SIZE -# define BOARD_CONSOLE_BUFFER_SIZE (1024*4) // default buffer size -#endif - -// TODO: User side implementation of px4_console_buffer +static int console_fd = -1; int px4_console_buffer_init() { - return 0; + console_fd = open(CONSOLE_BUFFER_DEVICE, O_RDWR); + + if (console_fd < 0) { + return ERROR; + } + + return OK; } int px4_console_buffer_size() { - return 0; + int size; + + if (ioctl(console_fd, FIONSPACE, &size) < 0) { + return 0; + } + + return size; } int px4_console_buffer_read(char *buffer, int buffer_length, int *offset) { - return 0; + FILE *fp; + ssize_t nread; + + /* Open a file stream to keep track of offset */ + fp = fdopen(dup(console_fd), "r"); + + if (fp == NULL) { + return -1; + } + + /* The driver does not utilize file position, we have to do it for it */ + fseek(fp, *offset, SEEK_SET); + nread = read(console_fd, buffer, buffer_length); + *offset = fseek(fp, 0, SEEK_CUR); + + /* Now we can close the file */ + fclose(fp); + + return (int)nread; } #endif /* BOARD_ENABLE_CONSOLE_BUFFER */ diff --git a/platforms/nuttx/src/px4/common/px4_userspace_init.cpp b/platforms/nuttx/src/px4/common/px4_userspace_init.cpp index 84dbc0d1a9d0..6d5cae675b99 100644 --- a/platforms/nuttx/src/px4/common/px4_userspace_init.cpp +++ b/platforms/nuttx/src/px4/common/px4_userspace_init.cpp @@ -38,6 +38,7 @@ */ #include +#include #include #include #include @@ -58,6 +59,8 @@ extern "C" void px4_userspace_init(void) px4_log_initialize(); + px4_console_buffer_init(); + #if defined(CONFIG_SYSTEM_CDCACM) && defined(CONFIG_BUILD_PROTECTED) cdcacm_init(); #endif