diff --git a/platforms/common/include/px4_platform_common/log.h b/platforms/common/include/px4_platform_common/log.h index 895679c0bb..eb06ef5a09 100644 --- a/platforms/common/include/px4_platform_common/log.h +++ b/platforms/common/include/px4_platform_common/log.h @@ -123,14 +123,24 @@ __END_DECLS #include #include +#if defined(_MSC_VER) && !defined(__clang__) +# define __px4_log_format(_fmt, _args) +#elif defined(__PX4_WINDOWS) +# define __px4_log_format(_fmt, _args) __attribute__((format(gnu_printf, _fmt, _args))) +#else +# define __px4_log_format(_fmt, _args) __attribute__((format(printf, _fmt, _args))) +#endif + __BEGIN_DECLS __EXPORT extern const char *__px4_log_level_str[_PX4_LOG_LEVEL_PANIC + 1]; __EXPORT void px4_log_modulename(int level, const char *moduleName, const char *fmt, ...) -__attribute__((format(printf, 3, 4))); +__px4_log_format(3, 4); __EXPORT void px4_log_raw(int level, const char *fmt, ...) -__attribute__((format(printf, 2, 3))); +__px4_log_format(2, 3); __EXPORT void px4_log_history(FILE *out); +__EXPORT int px4_log_modulename_from_text(const char *line); +__EXPORT void px4_log_write_text(FILE *out, const char *data, size_t length); #if __GNUC__ // Allow empty format strings. @@ -168,8 +178,9 @@ __END_DECLS #define __px4__log_end_fmt "\n" #ifdef __PX4_POSIX -#define PX4_LOG_COLORIZED_OUTPUT //if defined and output is a tty, colorize the output according to the log level -#endif /* __PX4_POSIX */ +// If defined and output is a tty, colorize the output according to the log level. +#define PX4_LOG_COLORIZED_OUTPUT +#endif /**************************************************************************** diff --git a/platforms/common/px4_log.cpp b/platforms/common/px4_log.cpp index 73a3671c89..da3d1a77a8 100644 --- a/platforms/common/px4_log.cpp +++ b/platforms/common/px4_log.cpp @@ -32,9 +32,15 @@ ****************************************************************************/ #include +#include +#include #include #include #include +#if defined(__PX4_WINDOWS) +#include +#include +#endif #ifndef MODULE_NAME #define MODULE_NAME "log" @@ -57,6 +63,113 @@ static LogHistory g_log_history; static orb_advert_t orb_log_message_pub = nullptr; +#if defined(PX4_LOG_COLORIZED_OUTPUT) +static bool px4_force_color_enabled() +{ + const char *force_color = getenv("PX4_FORCE_COLOR"); + return force_color && strcmp(force_color, "1") == 0; +} + +static void px4_copy_without_ansi(char *dst, size_t dst_size, const char *src) +{ + if (dst_size == 0) { + return; + } + + size_t out = 0; + + for (size_t in = 0; src[in] != '\0' && out + 1 < dst_size;) { + if (src[in] == '\x1b' && src[in + 1] == '[') { + size_t end = in + 2; + + while (src[end] != '\0' && (src[end] < '@' || src[end] > '~')) { + end++; + } + + if (src[end] != '\0') { + in = end + 1; + continue; + } + } + + dst[out++] = src[in++]; + } + + dst[out] = '\0'; +} + +#if defined(__PX4_WINDOWS) +static bool px4_is_running_under_wine() +{ + if (HMODULE ntdll = GetModuleHandleA("ntdll.dll")) { + return GetProcAddress(ntdll, "wine_get_version") != nullptr; + } + + return false; +} + +static bool px4_terminal_likely_supports_ansi() +{ + const char *term = getenv("TERM"); + + if (term && term[0] != '\0' && strcmp(term, "dumb") != 0) { + return true; + } + + const char *colorterm = getenv("COLORTERM"); + + if (colorterm && colorterm[0] != '\0') { + return true; + } + +#if defined(__PX4_WINDOWS) + const char *ansicon = getenv("ANSICON"); + const char *conemu_ansi = getenv("ConEmuANSI"); + const char *wt_session = getenv("WT_SESSION"); + const char *term_program = getenv("TERM_PROGRAM"); + + if ((ansicon && ansicon[0] != '\0') + || (wt_session && wt_session[0] != '\0') + || (term_program && term_program[0] != '\0')) { + return true; + } + + if (conemu_ansi && strcmp(conemu_ansi, "ON") == 0) { + return true; + } +#endif + + return false; +} +#endif + +static bool px4_should_use_color_output(bool isatty_) +{ + bool use_color = isatty_; + + const char *no_color = getenv("NO_COLOR"); + + if (no_color && no_color[0] != '\0') { + return false; + } + + if (px4_force_color_enabled()) { + return true; + } + +#if defined(__PX4_WINDOWS) + // Under Wine, PX4's Windows build usually writes through Wine's console + // bridge to a host terminal. Keep POSIX's tty/TERM gating; the emit + // path below writes complete SGR buffers directly to the host fd. + if (px4_is_running_under_wine()) { + return use_color && px4_terminal_likely_supports_ansi(); + } +#endif + + return use_color; +} +#endif + __EXPORT const char *__px4_log_level_str[_PX4_LOG_LEVEL_PANIC + 1] = { "DEBUG", "INFO", "WARN", "ERROR", "PANIC" }; static constexpr const char *__px4_log_level_color[_PX4_LOG_LEVEL_PANIC + 1] { @@ -67,6 +180,338 @@ static constexpr const char *__px4_log_level_color[_PX4_LOG_LEVEL_PANIC + 1] { PX4_ANSI_COLOR_RED // PANIC }; +static int px4_log_level_from_text(const char *level_start, size_t level_len) +{ + for (int level = _PX4_LOG_LEVEL_DEBUG; level <= _PX4_LOG_LEVEL_PANIC; ++level) { + const char *level_text = __px4_log_level_str[level]; + + if (strlen(level_text) == level_len && strncmp(level_start, level_text, level_len) == 0) { + return level; + } + } + + return -1; +} + +__EXPORT int px4_log_modulename_from_text(const char *line) +{ + if (line == nullptr) { + return 0; + } + + const char *p = line; + + while (*p == ' ' || *p == '\t') { + ++p; + } + + const char *level_start = p; + + while (*p >= 'A' && *p <= 'Z') { + ++p; + } + + if (p == level_start || (*p != ' ' && *p != '\t')) { + return 0; + } + + const int level = px4_log_level_from_text(level_start, static_cast(p - level_start)); + + if (level < _PX4_LOG_LEVEL_INFO || level > _PX4_LOG_LEVEL_PANIC) { + return 0; + } + + while (*p == ' ' || *p == '\t') { + ++p; + } + + if (*p != '[') { + return 0; + } + + const char *module_start = ++p; + + while (*p != '\0' && *p != ']' && *p != '\r' && *p != '\n') { + ++p; + } + + if (*p != ']' || p == module_start) { + return 0; + } + + const size_t module_len = static_cast(p - module_start); + char module_name[64] {}; + + if (module_len >= sizeof(module_name)) { + return 0; + } + + memcpy(module_name, module_start, module_len); + module_name[module_len] = '\0'; + + ++p; + + while (*p == ' ' || *p == '\t') { + ++p; + } + + size_t message_len = strlen(p); + + while (message_len > 0 && (p[message_len - 1] == '\r' || p[message_len - 1] == '\n')) { + --message_len; + } + + if (message_len > static_cast(INT_MAX)) { + message_len = static_cast(INT_MAX); + } + + px4_log_modulename(level, module_name, "%.*s", static_cast(message_len), p); + return 1; +} + +__EXPORT void px4_log_write_text(FILE *out, const char *data, size_t length) +{ + if (out == nullptr) { + out = stdout; + } + + if (data == nullptr || length == 0) { + return; + } + + size_t start = 0; + + while (start < length) { + size_t end = start; + + while (end < length && data[end] != '\n') { + ++end; + } + + const bool has_newline = end < length; + const size_t chunk_len = end - start + (has_newline ? 1 : 0); + bool handled = false; + + if (has_newline) { + const size_t line_len = end - start; + char line_buf[sizeof(log_message_s::text) + 128] {}; + + if (line_len < sizeof(line_buf)) { + memcpy(line_buf, data + start, line_len); + line_buf[line_len] = '\0'; + handled = px4_log_modulename_from_text(line_buf) != 0; + } + } + + if (!handled) { + (void)fwrite(data + start, 1, chunk_len, out); + } + + start += chunk_len; + } + + fflush(out); +} + +#if defined(__PX4_WINDOWS) && defined(PX4_LOG_COLORIZED_OUTPUT) +static bool px4_windows_stdout_supports_vt(FILE *out) +{ + if (out != stdout || _isatty(_fileno(stdout)) == 0) { + return false; + } + + static int cached_result = -1; + + if (cached_result >= 0) { + return cached_result != 0; + } + + HANDLE stdout_handle = GetStdHandle(STD_OUTPUT_HANDLE); + + if (stdout_handle == INVALID_HANDLE_VALUE || stdout_handle == nullptr) { + cached_result = 0; + return false; + } + + DWORD mode = 0; + + if (!GetConsoleMode(stdout_handle, &mode)) { + cached_result = 0; + return false; + } + +#ifdef ENABLE_VIRTUAL_TERMINAL_PROCESSING + if ((mode & ENABLE_VIRTUAL_TERMINAL_PROCESSING) == 0) { + if (!SetConsoleMode(stdout_handle, mode | ENABLE_VIRTUAL_TERMINAL_PROCESSING)) { + cached_result = 0; + return false; + } + } +#endif + + cached_result = 1; + return true; +} + +#if defined(__x86_64__) && defined(__GNUC__) +static long long px4_linux_syscall3(long long num, long long a, long long b, long long c) +{ + long long ret; + __asm__ volatile ( + "syscall" + : "=a"(ret) + : "0"(num), "D"(a), "S"(b), "d"(c) + : "rcx", "r11", "memory" + ); + return ret; +} +#endif + +static bool px4_wine_write_stdout_direct(const char *buffer, size_t length) +{ +#if defined(__x86_64__) && defined(__GNUC__) + if (!px4_is_running_under_wine() || length == 0) { + return false; + } + + constexpr long long LX_SYS_write = 1; + size_t written = 0; + + while (written < length) { + const long long ret = px4_linux_syscall3(LX_SYS_write, 1, + reinterpret_cast(buffer + written), + static_cast(length - written)); + + if (ret <= 0) { + return false; + } + + written += static_cast(ret); + } + + return true; +#else + (void)buffer; + (void)length; + return false; +#endif +} + +static bool px4_windows_should_emit_ansi(FILE *out, bool use_color) +{ + if (!use_color) { + return false; + } + + if (px4_force_color_enabled()) { + if (px4_is_running_under_wine()) { + return out == stdout; + } + + return true; + } + + if (px4_is_running_under_wine()) { + return out == stdout; + } + + return px4_windows_stdout_supports_vt(out); +} + +static bool px4_get_windows_console(FILE *out, HANDLE *handle, WORD *default_attributes) +{ + if (out != stdout || _isatty(_fileno(stdout)) == 0) { + return false; + } + + HANDLE stdout_handle = GetStdHandle(STD_OUTPUT_HANDLE); + + if (stdout_handle == INVALID_HANDLE_VALUE || stdout_handle == nullptr) { + return false; + } + + CONSOLE_SCREEN_BUFFER_INFO info{}; + + if (!GetConsoleScreenBufferInfo(stdout_handle, &info)) { + return false; + } + + if (handle) { + *handle = stdout_handle; + } + + if (default_attributes) { + *default_attributes = info.wAttributes; + } + + return true; +} + +static WORD px4_windows_level_attributes(int level, WORD default_attributes) +{ + const WORD background = default_attributes & 0xF0; + + switch (level) { + case _PX4_LOG_LEVEL_DEBUG: + return background | FOREGROUND_GREEN | FOREGROUND_INTENSITY; + + case _PX4_LOG_LEVEL_WARN: + return background | FOREGROUND_RED | FOREGROUND_GREEN | FOREGROUND_INTENSITY; + + case _PX4_LOG_LEVEL_ERROR: + case _PX4_LOG_LEVEL_PANIC: + return background | FOREGROUND_RED | FOREGROUND_INTENSITY; + + case _PX4_LOG_LEVEL_INFO: + default: + return default_attributes; + } +} + +static WORD px4_windows_module_attributes(WORD default_attributes) +{ + const WORD background = default_attributes & 0xF0; + return background | FOREGROUND_RED | FOREGROUND_GREEN | FOREGROUND_BLUE; +} + +static bool px4_windows_write_modulename_log(FILE *out, int level, const char *module_name, const char *message) +{ + HANDLE console_handle = nullptr; + WORD default_attributes = 0; + + if (!px4_get_windows_console(out, &console_handle, &default_attributes)) { + return false; + } + + const WORD level_attributes = px4_windows_level_attributes(level, default_attributes); + const WORD module_attributes = px4_windows_module_attributes(default_attributes); + + SetConsoleTextAttribute(console_handle, level_attributes); + fprintf(out, __px4__log_level_fmt, __px4_log_level_str[level]); + SetConsoleTextAttribute(console_handle, module_attributes); + fprintf(out, __px4__log_modulename_pfmt, module_name); + SetConsoleTextAttribute(console_handle, level == _PX4_LOG_LEVEL_INFO ? default_attributes : level_attributes); + fprintf(out, "%s\n", message); + SetConsoleTextAttribute(console_handle, default_attributes); + return true; +} + +static bool px4_windows_write_raw_log(FILE *out, int level, const char *message) +{ + HANDLE console_handle = nullptr; + WORD default_attributes = 0; + + if (!px4_get_windows_console(out, &console_handle, &default_attributes)) { + return false; + } + + SetConsoleTextAttribute(console_handle, px4_windows_level_attributes(level, default_attributes)); + fputs(message, out); + SetConsoleTextAttribute(console_handle, default_attributes); + return true; +} +#endif + void px4_log_initialize(void) { // we need to advertise with a valid message @@ -92,13 +537,49 @@ __EXPORT void px4_log_modulename(int level, const char *module_name, const char out = get_stdout(&isatty_); #if defined(PX4_LOG_COLORIZED_OUTPUT) - use_color = isatty_; + use_color = px4_should_use_color_output(isatty_); #endif // PX4_LOG_COLORIZED_OUTPUT #endif // PX4_POSIX if (level >= _PX4_LOG_LEVEL_INFO) { + char message_buf[max_length + 1] {}; char buf[max_length + 1]; // same length as log_message_s::text, but add newline ssize_t pos = 0; + bool wrote_to_stdout = false; + + va_list argptr; + va_start(argptr, fmt); + vsnprintf(message_buf, max_length, fmt, argptr); + va_end(argptr); + +#if defined(PX4_LOG_COLORIZED_OUTPUT) + char plain_message_buf[max_length + 1] {}; + px4_copy_without_ansi(plain_message_buf, sizeof(plain_message_buf), message_buf); + const char *plain_message = plain_message_buf; + const char *output_message = use_color ? message_buf : plain_message; + const char *history_message = plain_message; +#else + const char *output_message = message_buf; + const char *history_message = message_buf; +#endif + (void)history_message; + +#if defined(__PX4_WINDOWS) && defined(PX4_LOG_COLORIZED_OUTPUT) + const bool use_ansi_color = px4_windows_should_emit_ansi(out, use_color); + + if (use_color && !use_ansi_color + && px4_windows_write_modulename_log(out, level, module_name, plain_message)) { + snprintf(buf, max_length, __px4__log_level_fmt, __px4_log_level_str[level]); + pos = strlen(buf); + pos += snprintf(buf + pos, math::max(max_length - pos, (ssize_t)0), __px4__log_modulename_pfmt, module_name); + pos += snprintf(buf + pos, math::max(max_length - pos, (ssize_t)0), "%s\n", plain_message); + buf[max_length] = 0; + wrote_to_stdout = true; + } + + use_color = use_ansi_color; + output_message = use_color ? message_buf : plain_message; +#endif #if defined(PX4_LOG_COLORIZED_OUTPUT) @@ -128,10 +609,7 @@ __EXPORT void px4_log_modulename(int level, const char *module_name, const char #endif // PX4_LOG_COLORIZED_OUTPUT - va_list argptr; - va_start(argptr, fmt); - pos += vsnprintf(buf + pos, math::max(max_length - pos, (ssize_t)0), fmt, argptr); - va_end(argptr); + pos += snprintf(buf + pos, math::max(max_length - pos, (ssize_t)0), "%s", output_message); #if defined(PX4_LOG_COLORIZED_OUTPUT) @@ -149,7 +627,16 @@ __EXPORT void px4_log_modulename(int level, const char *module_name, const char // ensure NULL termination (buffer is max_length + 1) buf[max_length] = 0; - fputs(buf, out); +#if defined(__PX4_WINDOWS) && defined(PX4_LOG_COLORIZED_OUTPUT) + if (!wrote_to_stdout && use_color && px4_is_running_under_wine()) { + fflush(out); + wrote_to_stdout = px4_wine_write_stdout_direct(buf, strlen(buf)); + } +#endif + + if (!wrote_to_stdout) { + fputs(buf, out); + } #if defined(BOARD_ENABLE_LOG_HISTORY) @@ -159,9 +646,7 @@ __EXPORT void px4_log_modulename(int level, const char *module_name, const char if (use_color) { pos = snprintf(buf, max_length, __px4__log_level_fmt, __px4_log_level_str[level]); pos += snprintf(buf + pos, math::max(max_length - pos, (ssize_t)0), __px4__log_modulename_pfmt, module_name); - va_start(argptr, fmt); - pos += vsnprintf(buf + pos, math::max(max_length - pos, (ssize_t)0), fmt, argptr); - va_end(argptr); + pos += snprintf(buf + pos, math::max(max_length - pos, (ssize_t)0), "%s", history_message); pos += snprintf(buf + math::min(pos, max_length - (ssize_t)1), 2, "\n"); buf[max_length] = 0; // ensure NULL termination } @@ -210,12 +695,48 @@ __EXPORT void px4_log_raw(int level, const char *fmt, ...) #ifdef __PX4_POSIX bool use_color = true; out = get_stdout(&use_color); +#if defined(PX4_LOG_COLORIZED_OUTPUT) + use_color = px4_should_use_color_output(use_color); +#endif #endif if (level >= _PX4_LOG_LEVEL_INFO) { static constexpr ssize_t max_length = sizeof(log_message_s::text); + char message_buf[max_length + 1] {}; char buf[max_length + 1]; // same length as log_message_s::text, but add newline ssize_t pos = 0; + bool wrote_to_stdout = false; + + va_list argptr; + va_start(argptr, fmt); + vsnprintf(message_buf, max_length, fmt, argptr); + va_end(argptr); + +#if defined(PX4_LOG_COLORIZED_OUTPUT) + char plain_message_buf[max_length + 1] {}; + px4_copy_without_ansi(plain_message_buf, sizeof(plain_message_buf), message_buf); + const char *plain_message = plain_message_buf; + const char *output_message = use_color ? message_buf : plain_message; + const char *history_message = plain_message; +#else + const char *output_message = message_buf; + const char *history_message = message_buf; +#endif + (void)history_message; + +#if defined(__PX4_WINDOWS) && defined(PX4_LOG_COLORIZED_OUTPUT) + const bool use_ansi_color = px4_windows_should_emit_ansi(out, use_color); + + if (use_color && !use_ansi_color + && px4_windows_write_raw_log(out, level, plain_message)) { + snprintf(buf, max_length, "%s", plain_message); + buf[max_length] = 0; + wrote_to_stdout = true; + } + + use_color = use_ansi_color; + output_message = use_color ? message_buf : plain_message; +#endif #if defined(PX4_LOG_COLORIZED_OUTPUT) @@ -225,10 +746,7 @@ __EXPORT void px4_log_raw(int level, const char *fmt, ...) #endif // PX4_LOG_COLORIZED_OUTPUT - va_list argptr; - va_start(argptr, fmt); - pos += vsnprintf(buf + pos, math::max(max_length - pos, (ssize_t)0), fmt, argptr); - va_end(argptr); + pos += snprintf(buf + pos, math::max(max_length - pos, (ssize_t)0), "%s", output_message); #if defined(PX4_LOG_COLORIZED_OUTPUT) @@ -250,7 +768,16 @@ __EXPORT void px4_log_raw(int level, const char *fmt, ...) // ensure NULL termination (buffer is max_length + 1) buf[max_length] = 0; - fputs(buf, out); +#if defined(__PX4_WINDOWS) && defined(PX4_LOG_COLORIZED_OUTPUT) + if (!wrote_to_stdout && use_color && px4_is_running_under_wine()) { + fflush(out); + wrote_to_stdout = px4_wine_write_stdout_direct(buf, strlen(buf)); + } +#endif + + if (!wrote_to_stdout) { + fputs(buf, out); + } #if defined(BOARD_ENABLE_LOG_HISTORY) @@ -258,9 +785,7 @@ __EXPORT void px4_log_raw(int level, const char *fmt, ...) // No color formatting for log history if (use_color) { - va_start(argptr, fmt); - pos = vsnprintf(buf, max_length, fmt, argptr); - va_end(argptr); + pos = snprintf(buf, max_length, "%s", history_message); if (pos > max_length) { // preserve newline if necessary diff --git a/platforms/posix/include/px4_daemon/server_io.h b/platforms/posix/include/px4_daemon/server_io.h index efeae545b8..cb42731583 100644 --- a/platforms/posix/include/px4_daemon/server_io.h +++ b/platforms/posix/include/px4_daemon/server_io.h @@ -48,5 +48,7 @@ __BEGIN_DECLS * @return The FILE* which represents the standard output of the current thread. */ __EXPORT FILE *get_stdout(bool *isatty_); +__EXPORT void set_stdout_isatty_override(bool isatty_); +__EXPORT void clear_stdout_isatty_override(); __END_DECLS diff --git a/platforms/posix/src/px4/common/px4_daemon/server_io.cpp b/platforms/posix/src/px4/common/px4_daemon/server_io.cpp index 7deb93b4b1..16281651c7 100644 --- a/platforms/posix/src/px4/common/px4_daemon/server_io.cpp +++ b/platforms/posix/src/px4/common/px4_daemon/server_io.cpp @@ -57,12 +57,22 @@ using namespace px4_daemon; +namespace +{ +thread_local int stdout_isatty_override = -1; +} FILE *get_stdout(bool *isatty_) { + if (stdout_isatty_override >= 0) { + if (isatty_) { + *isatty_ = stdout_isatty_override != 0; + } + } + // If the server is not running, we are not in a thread that has been started if (!Server::is_running()) { - if (isatty_) { *isatty_ = isatty(1); } + if (isatty_ && stdout_isatty_override < 0) { *isatty_ = isatty(1); } return stdout; } @@ -74,12 +84,22 @@ FILE *get_stdout(bool *isatty_) // have any thread specific data set and we won't have a pipe to write // stdout to. if (thread_data_ptr == nullptr || thread_data_ptr->thread_stdout == nullptr) { - if (isatty_) { *isatty_ = isatty(1); } + if (isatty_ && stdout_isatty_override < 0) { *isatty_ = isatty(1); } return stdout; } - if (isatty_) { *isatty_ = thread_data_ptr->is_atty; } + if (isatty_ && stdout_isatty_override < 0) { *isatty_ = thread_data_ptr->is_atty; } return thread_data_ptr->thread_stdout; } + +void set_stdout_isatty_override(bool isatty_) +{ + stdout_isatty_override = isatty_ ? 1 : 0; +} + +void clear_stdout_isatty_override() +{ + stdout_isatty_override = -1; +}