dmesg functionality (console buffer)

boards/av/x-v1/default.cmake

@@ -86,6 +86,7 @@ px4_add_board(
 	SYSTEMCMDS
 		bl_update
 		config
+		dmesg
 		dumpfile
 		esc_calib
 		hardfault_log

boards/av/x-v1/src/board_config.h

@@ -329,6 +329,8 @@
 		GPIO_CAN1_TX,                     \
 	}
 
+#define BOARD_ENABLE_CONSOLE_BUFFER
+
 __BEGIN_DECLS
 
 /****************************************************************************************************

boards/px4/fmu-v5/default.cmake

@@ -91,6 +91,7 @@ px4_add_board(
 	SYSTEMCMDS
 		bl_update
 		config
+		dmesg
 		dumpfile
 		esc_calib
 		hardfault_log

boards/px4/fmu-v5/src/board_config.h

@@ -671,6 +671,8 @@
 		GPIO_nARMED_INIT                  \
 	}
 
+#define BOARD_ENABLE_CONSOLE_BUFFER
+
 __BEGIN_DECLS
 
 /****************************************************************************************************

platforms/nuttx/src/px4_layer/CMakeLists.txt

@@ -35,6 +35,7 @@
 if (NOT ${PX4_BOARD} MATCHES "px4_io")
 
 	add_library(px4_layer
+		console_buffer.cpp
 		px4_nuttx_tasks.c
 		px4_nuttx_impl.cpp
 		px4_init.cpp

platforms/nuttx/src/px4_layer/console_buffer.cpp

@@ -0,0 +1,242 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <px4_config.h>
+#include <px4_console_buffer.h>
+#include <px4_defines.h>
+#include <px4_sem.h>
+#include <pthread.h>
+#include <string.h>
+#include <fcntl.h>
+
+#ifdef BOARD_ENABLE_CONSOLE_BUFFER
+#ifndef BOARD_CONSOLE_BUFFER_SIZE
+# define BOARD_CONSOLE_BUFFER_SIZE (1024*4) // default buffer size
+#endif
+
+static ssize_t console_buffer_write(struct file *filep, const char *buffer, size_t buflen);
+
+
+class ConsoleBuffer
+{
+public:
+
+	void write(const char *buffer, size_t len);
+
+	void print(bool follow);
+
+	int size();
+
+	int read(char *buffer, int buffer_length, int *offset);
+
+private:
+	void		lock() { do {} while (px4_sem_wait(&_lock) != 0); }
+	void		unlock() { px4_sem_post(&_lock); }
+
+	char _buffer[BOARD_CONSOLE_BUFFER_SIZE];
+	int _head{0};
+	int _tail{0};
+	px4_sem_t _lock = SEM_INITIALIZER(1);
+	bool _is_printing{false};
+	pthread_t _printing_task;
+};
+
+void ConsoleBuffer::print(bool follow)
+{
+	lock();
+	_printing_task = pthread_self();
+	int i = _head;
+
+	while (true) {
+		_is_printing = true;
+
+		if (i < _tail) {
+			::write(1, _buffer + i, _tail - i);
+
+		} else if (_tail < i) {
+			::write(1, _buffer + i, BOARD_CONSOLE_BUFFER_SIZE - i);
+			::write(1, _buffer, _tail);
+		}
+
+		i = _tail;
+
+		_is_printing = false;
+
+		if (follow) {
+			unlock();
+			usleep(10000);
+			lock();
+
+		} else {
+			break;
+		}
+	}
+
+	unlock();
+}
+
+void ConsoleBuffer::write(const char *buffer, size_t len)
+{
+	if (_is_printing && pthread_self() == _printing_task) { // avoid adding to the buffer while we are printing it
+		return;
+	}
+
+	lock(); // same rule as for printf: this cannot be used from IRQ handlers
+
+	for (size_t i = 0; i < len; ++i) {
+		_buffer[_tail] = buffer[i];
+		_tail = (_tail + 1) % BOARD_CONSOLE_BUFFER_SIZE;
+
+		if (_tail == _head) {
+			_head = (_head + 1) % BOARD_CONSOLE_BUFFER_SIZE;
+		}
+	}
+
+	unlock();
+}
+
+int ConsoleBuffer::size()
+{
+	lock();
+	int size;
+
+	if (_head <= _tail) {
+		size = _tail - _head;
+
+	} else {
+		size = BOARD_CONSOLE_BUFFER_SIZE - (_head - _tail);
+	}
+
+	unlock();
+	return size;
+}
+
+int ConsoleBuffer::read(char *buffer, int buffer_length, int *offset)
+{
+	lock();
+
+	if (*offset == -1) {
+		*offset = _head;
+	}
+
+	int size = 0;
+
+	if (*offset < _tail) {
+		size = _tail - *offset;
+
+		if (size > buffer_length) {
+			size = buffer_length;
+		}
+
+		memcpy(buffer, _buffer + *offset, size);
+
+	} else if (_tail < *offset) {
+		size = BOARD_CONSOLE_BUFFER_SIZE - *offset;
+
+		if (size > buffer_length) {
+			size = buffer_length;
+		}
+
+		memcpy(buffer, _buffer + *offset, size);
+		buffer += size;
+		buffer_length -= size;
+
+		int size_secondary = _tail;
+
+		if (size_secondary > buffer_length) {
+			size_secondary = buffer_length;
+		}
+
+		if (size_secondary > 0) {
+			memcpy(buffer, _buffer, size_secondary);
+			size += size_secondary;
+		}
+	}
+
+	unlock();
+	*offset = (*offset + size) % BOARD_CONSOLE_BUFFER_SIZE;
+	return size;
+}
+
+static ConsoleBuffer g_console_buffer;
+
+
+void px4_console_buffer_print(bool follow)
+{
+	g_console_buffer.print(follow);
+}
+
+ssize_t console_buffer_write(struct file *filep, const char *buffer, size_t len)
+{
+	g_console_buffer.write(buffer, len);
+
+	// stderr still points to our original console and is available from everywhere, so we output to that.
+	// We could use up_putc() as well, but it is considerably less efficient.
+	// The drawback here is that a module writing to stderr bypasses the console buffer.
+	write(2, buffer, len);
+	fsync(2);
+
+	return len;
+}
+
+static const struct file_operations g_console_buffer_fops = {
+	NULL,         /* open */
+	NULL,         /* close */
+	NULL,         /* read */
+	console_buffer_write, /* write */
+	NULL,         /* seek */
+	NULL          /* ioctl */
+#ifndef CONFIG_DISABLE_POLL
+	, NULL        /* poll */
+#endif
+#ifndef CONFIG_DISABLE_PSEUDOFS_OPERATIONS
+	, NULL        /* unlink */
+#endif
+};
+
+int px4_console_buffer_init()
+{
+	return register_driver(CONSOLE_BUFFER_DEVICE, &g_console_buffer_fops, 0666, NULL);
+}
+
+int px4_console_buffer_size()
+{
+	return g_console_buffer.size();
+}
+
+int px4_console_buffer_read(char *buffer, int buffer_length, int *offset)
+{
+	return g_console_buffer.read(buffer, buffer_length, offset);
+}
+
+#endif /* BOARD_ENABLE_CONSOLE_BUFFER */

platforms/nuttx/src/px4_layer/px4_init.cpp

@@ -34,6 +34,7 @@
 #include "px4_init.h"
 
 #include <px4_config.h>
+#include <px4_console_buffer.h>
 #include <px4_defines.h>
 #include <drivers/drv_hrt.h>
 #include <lib/parameters/param.h>
@@ -96,6 +97,21 @@ int px4_platform_init(void)
 
 #endif
 
+	int ret = px4_console_buffer_init();
+
+	if (ret < 0) {
+		return ret;
+	}
+
+	// replace stdout with our buffered console
+	int fd_buf = open(CONSOLE_BUFFER_DEVICE, O_WRONLY);
+
+	if (fd_buf >= 0) {
+		dup2(fd_buf, 1);
+		// keep stderr(2) untouched: the buffered console will use it to output to the original console
+		close(fd_buf);
+	}
+
 	hrt_init();
 
 	param_init();

src/drivers/lights/blinkm/blinkm.cpp

@@ -1046,7 +1046,7 @@ blinkm_main(int argc, char *argv[])
 
 
 	if (g_blinkm == nullptr) {
-		fprintf(stderr, "not started\n");
+		PX4_ERR("not started");
 		blinkm_usage();
 		return 0;
 	}
@@ -1064,10 +1064,10 @@ blinkm_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "list")) {
 		for (unsigned i = 0; BlinkM::script_names[i] != nullptr; i++) {
-			fprintf(stderr, "    %s\n", BlinkM::script_names[i]);
+			PX4_ERR("    %s", BlinkM::script_names[i]);
 		}
 
-		fprintf(stderr, "    <html color number>\n");
+		PX4_ERR("    <html color number>");
 		return 0;
 	}
 

src/drivers/md25/md25_main.cpp

@@ -45,6 +45,7 @@
  */
 
 #include <px4_config.h>
+#include <px4_log.h>
 #include <unistd.h>
 #include <stdio.h>
 #include <stdlib.h>
@@ -79,10 +80,10 @@ static void
 usage(const char *reason)
 {
 	if (reason) {
-		fprintf(stderr, "%s\n", reason);
+		PX4_WARN("%s", reason);
 	}
 
-	fprintf(stderr, "usage: md25 {start|stop|read|status|search|test|change_address}\n\n");
+	PX4_INFO("usage: md25 {start|stop|read|status|search|test|change_address}");
 	exit(1);
 }