| /* |
| * Copyright (c) 2019 Intel corporation |
| * |
| * SPDX-License-Identifier: Apache-2.0 |
| */ |
| |
| #include <zephyr/init.h> |
| #include <stdio.h> |
| #include <ctype.h> |
| #include <zephyr/kernel.h> |
| #include <mipi_syst.h> |
| #include <zephyr/spinlock.h> |
| #include <zephyr/toolchain.h> |
| #include <zephyr/sys/__assert.h> |
| #include <zephyr/linker/utils.h> |
| #include <zephyr/logging/log.h> |
| #include <zephyr/logging/log_ctrl.h> |
| #include <zephyr/logging/log_output.h> |
| |
| static struct mipi_syst_header log_syst_header; |
| static struct mipi_syst_handle log_syst_handle; |
| |
| #define HEXDUMP_BYTES_IN_LINE 16 |
| |
| #ifdef CONFIG_LOG_STRDUP_MAX_STRING |
| #define STRING_BUF_MAX_LEN CONFIG_LOG_STRDUP_MAX_STRING |
| #else |
| #define STRING_BUF_MAX_LEN 128 |
| #endif |
| |
| #if defined(MIPI_SYST_PCFG_ENABLE_PLATFORM_STATE_DATA) |
| #if defined(CONFIG_MIPI_SYST_STP) |
| static mipi_syst_u16 master = 128; |
| static mipi_syst_u16 channel = 1; |
| |
| static struct stp_writer_data writer_state; |
| #elif !defined(CONFIG_MIPI_SYST_RAW_DATA) |
| static const char pattern[] = "SYS-T RAW DATA: "; |
| static const char valToHex[] = "0123456789ABCDEF"; |
| #endif |
| |
| static int out_func(int c, void *ctx) |
| { |
| const struct log_output *out_ctx = (const struct log_output *)ctx; |
| |
| out_ctx->buf[out_ctx->control_block->offset] = (uint8_t)c; |
| out_ctx->control_block->offset++; |
| |
| __ASSERT_NO_MSG(out_ctx->control_block->offset <= out_ctx->size); |
| |
| if (out_ctx->control_block->offset == out_ctx->size) { |
| log_output_flush(out_ctx); |
| } |
| |
| return 0; |
| } |
| |
| #if defined(CONFIG_MIPI_SYST_STP) |
| static void stp_write_putNibble(struct mipi_syst_handle *systh, |
| struct stp_writer_data *p, mipi_syst_u8 n) |
| { |
| p->current |= (n << 4); |
| p->byteDone = !p->byteDone; |
| |
| if (p->byteDone) { |
| out_func(p->current, systh->systh_platform.log_output); |
| p->current = 0; |
| } else { |
| p->current >>= 4; |
| } |
| } |
| |
| static void stp_write_flush(struct mipi_syst_handle *systh, |
| struct stp_writer_data *p) |
| { |
| if (!p->byteDone) { |
| stp_write_putNibble(systh, p, 0); |
| } |
| } |
| |
| static void stp_write_d4(struct mipi_syst_handle *systh, |
| struct stp_writer_data *p, mipi_syst_u8 v) |
| { |
| stp_write_putNibble(systh, p, v); |
| } |
| |
| static void stp_write_payload8(struct mipi_syst_handle *systh, |
| struct stp_writer_data *p, mipi_syst_u8 v) |
| { |
| stp_write_d4(systh, p, v); |
| stp_write_d4(systh, p, v>>4); |
| } |
| |
| static void stp_write_payload16(struct mipi_syst_handle *systh, |
| struct stp_writer_data *p, mipi_syst_u16 v) |
| { |
| stp_write_payload8(systh, p, (mipi_syst_u8)v); |
| stp_write_payload8(systh, p, (mipi_syst_u8)(v>>8)); |
| } |
| |
| static void stp_write_payload32(struct mipi_syst_handle *systh, |
| struct stp_writer_data *p, mipi_syst_u32 v) |
| { |
| stp_write_payload16(systh, p, (mipi_syst_u16)v); |
| stp_write_payload16(systh, p, (mipi_syst_u16)(v>>16)); |
| } |
| |
| static void stp_write_payload64(struct mipi_syst_handle *systh, |
| struct stp_writer_data *p, mipi_syst_u64 v) |
| { |
| stp_write_payload32(systh, p, (mipi_syst_u32)v); |
| stp_write_payload32(systh, p, (mipi_syst_u32)(v>>32)); |
| } |
| |
| static mipi_syst_u64 deltaTime(struct stp_writer_data *p) |
| { |
| mipi_syst_u64 delta; |
| |
| delta = mipi_syst_get_epoch() - p->timestamp; |
| return delta * 60; |
| } |
| |
| static void stp_write_d32mts(struct mipi_syst_handle *systh, |
| struct stp_writer_data *p, mipi_syst_u32 v) |
| { |
| stp_write_d4(systh, p, 0xA); |
| stp_write_payload32(systh, p, v); |
| |
| stp_write_d4(systh, p, 0xE); |
| stp_write_payload64(systh, p, deltaTime(p)); |
| } |
| |
| static void stp_write_d64mts(struct mipi_syst_handle *systh, |
| struct stp_writer_data *p, mipi_syst_u64 v) |
| { |
| stp_write_d4(systh, p, 0xB); |
| stp_write_payload64(systh, p, v); |
| |
| stp_write_d4(systh, p, 0xE); |
| stp_write_payload64(systh, p, deltaTime(p)); |
| } |
| |
| static void stp_write_d32ts(struct mipi_syst_handle *systh, |
| struct stp_writer_data *p, mipi_syst_u32 v) |
| { |
| stp_write_d4(systh, p, 0xF); |
| stp_write_d4(systh, p, 0x6); |
| |
| stp_write_payload32(systh, p, v); |
| |
| stp_write_d4(systh, p, 0xE); |
| stp_write_payload64(systh, p, deltaTime(p)); |
| } |
| |
| static void stp_write_d8(struct mipi_syst_handle *systh, |
| struct stp_writer_data *p, mipi_syst_u8 v) |
| { |
| stp_write_d4(systh, p, 0x4); |
| stp_write_payload8(systh, p, v); |
| } |
| |
| static void stp_write_d16(struct mipi_syst_handle *systh, |
| struct stp_writer_data *p, mipi_syst_u16 v) |
| { |
| stp_write_d4(systh, p, 0x5); |
| stp_write_payload16(systh, p, v); |
| } |
| |
| static void stp_write_d32(struct mipi_syst_handle *systh, |
| struct stp_writer_data *p, mipi_syst_u32 v) |
| { |
| stp_write_d4(systh, p, 0x6); |
| stp_write_payload32(systh, p, v); |
| } |
| |
| #if defined(MIPI_SYST_PCFG_ENABLE_64BIT_IO) |
| static void stp_write_d64(struct mipi_syst_handle *systh, |
| struct stp_writer_data *p, mipi_syst_u64 v) |
| { |
| stp_write_d4(systh, p, 0x7); |
| stp_write_payload64(systh, p, v); |
| } |
| #endif |
| |
| static void stp_write_flag(struct mipi_syst_handle *systh, |
| struct stp_writer_data *p) |
| { |
| stp_write_d4(systh, p, 0xF); |
| stp_write_d4(systh, p, 0xE); |
| } |
| |
| static void stp_write_async(struct mipi_syst_handle *systh, |
| struct stp_writer_data *p) |
| { |
| for (int i = 0; i < 21; ++i) { |
| stp_write_d4(systh, p, 0xF); |
| } |
| |
| stp_write_d4(systh, p, 0x0); |
| } |
| |
| static void stp_write_version(struct mipi_syst_handle *systh, |
| struct stp_writer_data *p) |
| { |
| stp_write_d4(systh, p, 0xF); |
| stp_write_d4(systh, p, 0x0); |
| stp_write_d4(systh, p, 0x0); |
| |
| stp_write_d4(systh, p, 0x3); |
| |
| p->master = 0; |
| p->channel = 0; |
| } |
| |
| static void stp_write_freq(struct mipi_syst_handle *systh, |
| struct stp_writer_data *p) |
| { |
| stp_write_d4(systh, p, 0xF); |
| stp_write_d4(systh, p, 0x0); |
| stp_write_d4(systh, p, 0x8); |
| stp_write_payload32(systh, p, 60 * 1000 * 1000); |
| } |
| |
| static void stp_write_setMC(struct mipi_syst_handle *systh, |
| struct stp_writer_data *p, |
| mipi_syst_u16 master, |
| mipi_syst_u16 channel) |
| { |
| if (!(p->recordCount++ % 20)) { |
| stp_write_async(systh, p); |
| stp_write_version(systh, p); |
| stp_write_freq(systh, p); |
| } |
| |
| if (p->master != master) { |
| stp_write_d4(systh, p, 0xF); |
| stp_write_d4(systh, p, 0x1); |
| stp_write_payload16(systh, p, master); |
| |
| p->master = master; |
| p->channel = 0; |
| } |
| |
| if (p->channel != channel) { |
| stp_write_d4(systh, p, 0xF); |
| stp_write_d4(systh, p, 0x3); |
| stp_write_payload16(systh, p, channel); |
| |
| p->channel = channel; |
| } |
| } |
| #else |
| static void write_raw(struct mipi_syst_handle *systh, const void *p, int n) |
| { |
| int i; |
| uint8_t c; |
| |
| #if defined(MIPI_SYST_BIG_ENDIAN) |
| for (i = n - 1; i >= 0; --i) { |
| #else |
| for (i = 0; i < n; ++i) { |
| #endif |
| c = ((const uint8_t *)p)[i]; |
| #if defined(CONFIG_MIPI_SYST_RAW_DATA) |
| out_func(c, systh->systh_platform.log_output); |
| #else |
| out_func(valToHex[c >> 0x4], systh->systh_platform.log_output); |
| out_func(valToHex[c & 0xF], systh->systh_platform.log_output); |
| #endif |
| } |
| } |
| #endif |
| |
| static void write_d8(struct mipi_syst_handle *systh, uint8_t v) |
| { |
| #if defined(CONFIG_MIPI_SYST_STP) |
| struct stp_writer_data *writer = |
| systh->systh_header->systh_platform.stpWriter; |
| |
| stp_write_d8(systh, writer, v); |
| #else |
| write_raw(systh, &v, sizeof(v)); |
| #endif |
| } |
| |
| static void write_d16(struct mipi_syst_handle *systh, uint16_t v) |
| { |
| #if defined(CONFIG_MIPI_SYST_STP) |
| struct stp_writer_data *writer = |
| systh->systh_header->systh_platform.stpWriter; |
| |
| stp_write_d16(systh, writer, v); |
| #else |
| write_raw(systh, &v, sizeof(v)); |
| #endif |
| } |
| |
| static void write_d32(struct mipi_syst_handle *systh, uint32_t v) |
| { |
| #if defined(CONFIG_MIPI_SYST_STP) |
| struct stp_writer_data *writer = |
| systh->systh_header->systh_platform.stpWriter; |
| |
| stp_write_d32(systh, writer, v); |
| #else |
| write_raw(systh, &v, sizeof(v)); |
| #endif |
| } |
| |
| #if defined(MIPI_SYST_PCFG_ENABLE_64BIT_IO) |
| static void write_d64(struct mipi_syst_handle *systh, uint64_t v) |
| { |
| #if defined(CONFIG_MIPI_SYST_STP) |
| struct stp_writer_data *writer = |
| systh->systh_header->systh_platform.stpWriter; |
| |
| stp_write_d64(systh, writer, v); |
| #else |
| write_raw(systh, &v, sizeof(v)); |
| #endif |
| } |
| #endif |
| |
| static void write_d32ts(struct mipi_syst_handle *systh, uint32_t v) |
| { |
| #if defined(CONFIG_MIPI_SYST_STP) |
| struct stp_writer_data *writer = |
| systh->systh_header->systh_platform.stpWriter; |
| |
| stp_write_setMC(systh, writer, |
| systh->systh_platform.master, |
| systh->systh_platform.channel); |
| stp_write_d32ts(systh, writer, v); |
| #elif defined(CONFIG_MIPI_SYST_RAW_DATA) |
| ARG_UNUSED(systh); |
| |
| write_raw(systh, &v, sizeof(v)); |
| #else |
| for (int i = 0; i < strlen(pattern); i++) { |
| out_func(pattern[i], systh->systh_platform.log_output); |
| } |
| |
| write_raw(systh, &v, sizeof(v)); |
| #endif |
| } |
| |
| static void write_d32mts(struct mipi_syst_handle *systh, mipi_syst_u32 v) |
| { |
| #if defined(CONFIG_MIPI_SYST_STP) |
| struct stp_writer_data *writer = |
| systh->systh_header->systh_platform.stpWriter; |
| |
| stp_write_setMC(systh, writer, |
| systh->systh_platform.master, |
| systh->systh_platform.channel); |
| stp_write_d32mts(systh, writer, v); |
| #else |
| ARG_UNUSED(systh); |
| ARG_UNUSED(v); |
| #endif |
| } |
| |
| static void write_d64mts(struct mipi_syst_handle *systh, mipi_syst_u64 v) |
| { |
| #if defined(CONFIG_MIPI_SYST_STP) |
| struct stp_writer_data *writer = |
| systh->systh_header->systh_platform.stpWriter; |
| |
| stp_write_setMC(systh, writer, |
| systh->systh_platform.master, |
| systh->systh_platform.channel); |
| stp_write_d64mts(systh, writer, v); |
| #else |
| ARG_UNUSED(systh); |
| ARG_UNUSED(v); |
| #endif |
| } |
| |
| static void write_flag(struct mipi_syst_handle *systh) |
| { |
| #if defined(CONFIG_MIPI_SYST_STP) |
| struct stp_writer_data *writer = |
| systh->systh_header->systh_platform.stpWriter; |
| |
| stp_write_flag(systh, writer); |
| stp_write_flush(systh, writer); |
| #elif defined(CONFIG_MIPI_SYST_RAW_DATA) |
| ARG_UNUSED(systh); |
| #else |
| uint32_t flag = systh->systh_platform.flag; |
| |
| if ((flag & LOG_OUTPUT_FLAG_CRLF_NONE) != 0U) { |
| return; |
| } |
| |
| if ((flag & LOG_OUTPUT_FLAG_CRLF_LFONLY) != 0U) { |
| out_func('\n', systh->systh_platform.log_output); |
| } else { |
| out_func('\r', systh->systh_platform.log_output); |
| out_func('\n', systh->systh_platform.log_output); |
| } |
| #endif |
| } |
| #endif |
| |
| #if defined(MIPI_SYST_PCFG_ENABLE_TIMESTAMP) |
| mipi_syst_u64 mipi_syst_get_epoch(void) |
| { |
| return k_uptime_ticks(); |
| } |
| #endif |
| |
| static void update_systh_platform_data(struct mipi_syst_handle *handle, |
| const struct log_output *log_output, |
| uint32_t flag) |
| { |
| #if defined(MIPI_SYST_PCFG_ENABLE_PLATFORM_STATE_DATA) |
| handle->systh_platform.flag = (mipi_syst_u32)flag; |
| handle->systh_platform.log_output = (struct log_output *)log_output; |
| #endif |
| } |
| |
| #if defined(CONFIG_LOG1) || defined(CONFIG_LOG_MIPI_SYST_OUTPUT_LOG_MSG_SRC_ID) |
| /** |
| * @brief Set module ID in the origin unit of Sys-T message |
| * |
| * Note that this only sets the module ID if |
| * CONFIG_LOG_MIPI_SYST_OUTPUT_LOG_MSG_SRC_ID is enabled. |
| * Otherwise, this is a no-op as the module ID is set to |
| * default at boot time, and no need to be set again. |
| * |
| * @param handle Pointer to mipi_syst_handle struct |
| * @param module_id Module ID to be set (range 0x00 - 0x7F) |
| */ |
| static void update_handle_origin_unit(struct mipi_syst_handle *handle, |
| int16_t module_id) |
| { |
| handle->systh_tag.et_modunit = |
| _MIPI_SYST_MK_MODUNIT_ORIGIN( |
| module_id, |
| CONFIG_LOG_MIPI_SYST_MSG_DEFAULT_UNIT_ID |
| ); |
| } |
| #endif |
| |
| #if defined(MIPI_SYST_PCFG_ENABLE_PLATFORM_HANDLE_DATA) |
| /* |
| * Platform specific SyS-T handle initialization hook function |
| * |
| * @param systh pointer to the SyS-T handle structure |
| */ |
| static void platform_handle_init(struct mipi_syst_handle *systh) |
| { |
| #if defined(CONFIG_MIPI_SYST_STP) |
| if (channel > 127) { |
| ++master; |
| channel = 1; |
| } |
| |
| systh->systh_platform.channel = channel++; |
| systh->systh_platform.master = master; |
| #endif |
| |
| #if defined(MIPI_SYST_PCFG_LENGTH_FIELD) |
| MIPI_SYST_ENABLE_HANDLE_LENGTH(systh, 1); |
| #endif |
| |
| #if defined(MIPI_SYST_PCFG_ENABLE_TIMESTAMP) |
| MIPI_SYST_ENABLE_HANDLE_TIMESTAMP(systh, 1); |
| #endif |
| } |
| |
| static void platform_handle_release(struct mipi_syst_handle *systh) |
| { |
| ARG_UNUSED(systh); |
| } |
| #endif |
| |
| /* |
| * Platform specific global state initialization hook function |
| * |
| * @param systh pointer to the new SyS-T handle structure |
| * @param platform_data user defined data for the init function. |
| * |
| */ |
| static void mipi_syst_platform_init(struct mipi_syst_header *systh, |
| const void *platform_data) |
| { |
| #if defined(CONFIG_MIPI_SYST_STP) |
| writer_state.byteDone = 0; |
| writer_state.current = 0; |
| writer_state.master = 0; |
| writer_state.channel = 0; |
| writer_state.recordCount = 0; |
| writer_state.timestamp = mipi_syst_get_epoch(); |
| |
| systh->systh_platform.stpWriter = &writer_state; |
| #endif |
| |
| #if defined(MIPI_SYST_PCFG_ENABLE_PLATFORM_HANDLE_DATA) |
| systh->systh_inith = platform_handle_init; |
| systh->systh_releaseh = platform_handle_release; |
| #endif |
| #if defined(MIPI_SYST_PCFG_ENABLE_PLATFORM_STATE_DATA) |
| systh->systh_platform.write_d8 = write_d8; |
| systh->systh_platform.write_d16 = write_d16; |
| systh->systh_platform.write_d32 = write_d32; |
| #if defined(MIPI_SYST_PCFG_ENABLE_64BIT_IO) |
| systh->systh_platform.write_d64 = write_d64; |
| #endif |
| systh->systh_platform.write_d32ts = write_d32ts; |
| systh->systh_platform.write_d32mts = write_d32mts; |
| systh->systh_platform.write_d64mts = write_d64mts; |
| systh->systh_platform.write_flag = write_flag; |
| #endif |
| } |
| |
| /* |
| * 0 MIPI_SYST_SEVERITY_MAX no assigned severity |
| * 1 MIPI_SYST_SEVERITY_FATAL critical error level |
| * 2 MIPI_SYST_SEVERITY_ERROR error message level |
| * 3 MIPI_SYST_SEVERITY_WARNING warning message level |
| * 4 MIPI_SYST_SEVERITY_INFO information message level |
| * 5 MIPI_SYST_SEVERITY_USER1 user defined level 5 |
| * 6 MIPI_SYST_SEVERITY_USER2 user defined level 6 |
| * 7 MIPI_SYST_SEVERITY_DEBUG debug information level |
| */ |
| static uint32_t level_to_syst_severity(uint32_t level) |
| { |
| uint32_t ret; |
| |
| switch (level) { |
| case LOG_LEVEL_NONE: |
| ret = 0U; |
| break; |
| case LOG_LEVEL_ERR: |
| ret = 2U; |
| break; |
| case LOG_LEVEL_WRN: |
| ret = 3U; |
| break; |
| case LOG_LEVEL_INF: |
| ret = 4U; |
| break; |
| case LOG_LEVEL_DBG: |
| ret = 7U; |
| break; |
| default: |
| ret = 7U; |
| break; |
| } |
| |
| return ret; |
| } |
| |
| static void hexdump_line_print(const uint8_t *data, uint32_t length, |
| uint32_t severity) |
| { |
| char hexdump_buf[HEXDUMP_BYTES_IN_LINE * 4 + 4]; |
| |
| hexdump_buf[sizeof(hexdump_buf) - 1] = '\0'; |
| char *buf = &hexdump_buf[0]; |
| |
| for (int i = 0; i < HEXDUMP_BYTES_IN_LINE; i++) { |
| if (i > 0 && !(i % 8)) { |
| *buf = ' '; |
| buf++; |
| } |
| |
| if (i < length) { |
| sprintf(buf, "%02x ", data[i]); |
| } else { |
| sprintf(buf, " "); |
| } |
| |
| buf += 3; |
| } |
| |
| *buf = '|'; |
| buf++; |
| |
| for (int i = 0; i < HEXDUMP_BYTES_IN_LINE; i++) { |
| if (i > 0 && !(i % 8)) { |
| *buf = ' '; |
| buf++; |
| } |
| |
| if (i < length) { |
| char c = (char)data[i]; |
| |
| *buf = isprint((int)c) ? c : '.'; |
| } else { |
| *buf = ' '; |
| } |
| |
| buf++; |
| } |
| |
| MIPI_SYST_PRINTF(&log_syst_handle, severity, "%s", hexdump_buf); |
| } |
| |
| #ifndef CONFIG_LOG2 |
| static void std_print(struct log_msg *msg, |
| const struct log_output *log_output) |
| { |
| const char *str = log_msg_str_get(msg); |
| uint32_t nargs = log_msg_nargs_get(msg); |
| uint32_t *args = alloca(sizeof(uint32_t)*nargs); |
| uint32_t severity = level_to_syst_severity(log_msg_level_get(msg)); |
| |
| for (int i = 0; i < nargs; i++) { |
| args[i] = log_msg_arg_get(msg, i); |
| } |
| |
| switch (log_msg_nargs_get(msg)) { |
| case 0: |
| MIPI_SYST_PRINTF(&log_syst_handle, severity, str); |
| break; |
| case 1: |
| MIPI_SYST_PRINTF(&log_syst_handle, severity, str, args[0]); |
| break; |
| case 2: |
| MIPI_SYST_PRINTF(&log_syst_handle, severity, str, args[0], |
| args[1]); |
| break; |
| case 3: |
| MIPI_SYST_PRINTF(&log_syst_handle, severity, str, args[0], |
| args[1], args[2]); |
| break; |
| case 4: |
| MIPI_SYST_PRINTF(&log_syst_handle, severity, str, args[0], |
| args[1], args[2], args[3]); |
| break; |
| case 5: |
| MIPI_SYST_PRINTF(&log_syst_handle, severity, str, args[0], |
| args[1], args[2], args[3], args[4]); |
| break; |
| case 6: |
| MIPI_SYST_PRINTF(&log_syst_handle, severity, str, args[0], |
| args[1], args[2], args[3], args[4], args[5]); |
| break; |
| case 7: |
| MIPI_SYST_PRINTF(&log_syst_handle, severity, str, args[0], |
| args[1], args[2], args[3], args[4], args[5], |
| args[6]); |
| break; |
| case 8: |
| MIPI_SYST_PRINTF(&log_syst_handle, severity, str, args[0], |
| args[1], args[2], args[3], args[4], args[5], |
| args[6], args[7]); |
| break; |
| case 9: |
| MIPI_SYST_PRINTF(&log_syst_handle, severity, str, args[0], |
| args[1], args[2], args[3], args[4], args[5], |
| args[6], args[7], args[8]); |
| break; |
| case 10: |
| MIPI_SYST_PRINTF(&log_syst_handle, severity, str, args[0], |
| args[1], args[2], args[3], args[4], args[5], |
| args[6], args[7], args[8], args[9]); |
| break; |
| case 11: |
| MIPI_SYST_PRINTF(&log_syst_handle, severity, str, args[0], |
| args[1], args[2], args[3], args[4], args[5], |
| args[6], args[7], args[8], args[9], args[10]); |
| break; |
| case 12: |
| MIPI_SYST_PRINTF(&log_syst_handle, severity, str, args[0], |
| args[1], args[2], args[3], args[4], args[5], |
| args[6], args[7], args[8], args[9], args[10], |
| args[11]); |
| break; |
| case 13: |
| MIPI_SYST_PRINTF(&log_syst_handle, severity, str, args[0], |
| args[1], args[2], args[3], args[4], args[5], |
| args[6], args[7], args[8], args[9], args[10], |
| args[11], args[12]); |
| break; |
| case 14: |
| MIPI_SYST_PRINTF(&log_syst_handle, severity, str, args[0], |
| args[1], args[2], args[3], args[4], args[5], |
| args[6], args[7], args[8], args[9], args[10], |
| args[11], args[12], args[13]); |
| break; |
| case 15: |
| MIPI_SYST_PRINTF(&log_syst_handle, severity, str, args[0], |
| args[1], args[2], args[3], args[4], args[5], |
| args[6], args[7], args[8], args[9], args[10], |
| args[11], args[12], args[13], args[14]); |
| break; |
| default: |
| /* Unsupported number of arguments. */ |
| __ASSERT_NO_MSG(true); |
| break; |
| } |
| } |
| |
| static void raw_string_print(struct log_msg *msg, |
| const struct log_output *log_output) |
| { |
| char buf[STRING_BUF_MAX_LEN + 1]; |
| size_t length = STRING_BUF_MAX_LEN; |
| uint32_t severity = level_to_syst_severity(log_msg_level_get(msg)); |
| |
| log_msg_hexdump_data_get(msg, buf, &length, 0); |
| |
| buf[length] = '\0'; |
| |
| MIPI_SYST_PRINTF(&log_syst_handle, severity, buf); |
| } |
| |
| static void hexdump_print(struct log_msg *msg, |
| const struct log_output *log_output) |
| { |
| uint32_t offset = 0U; |
| uint8_t buf[HEXDUMP_BYTES_IN_LINE]; |
| size_t length; |
| uint32_t severity = level_to_syst_severity(log_msg_level_get(msg)); |
| |
| MIPI_SYST_PRINTF(&log_syst_handle, severity, "%s", log_msg_str_get(msg)); |
| |
| do { |
| length = sizeof(buf); |
| log_msg_hexdump_data_get(msg, buf, &length, offset); |
| |
| if (length) { |
| hexdump_line_print(buf, length, severity); |
| offset += length; |
| } else { |
| break; |
| } |
| } while (true); |
| } |
| |
| void log_output_msg_syst_process(const struct log_output *log_output, |
| struct log_msg *msg, uint32_t flag) |
| { |
| uint8_t level = (uint8_t)log_msg_level_get(msg); |
| bool raw_string = (level == LOG_LEVEL_INTERNAL_RAW_STRING); |
| |
| update_systh_platform_data(&log_syst_handle, log_output, flag); |
| |
| #ifdef CONFIG_LOG_MIPI_SYST_OUTPUT_LOG_MSG_SRC_ID |
| int16_t source_id = (int16_t)log_msg_source_id_get(msg); |
| |
| update_handle_origin_unit(&log_syst_handle, source_id); |
| #endif |
| |
| if (log_msg_is_std(msg)) { |
| std_print(msg, log_output); |
| } else if (raw_string) { |
| raw_string_print(msg, log_output); |
| } else { |
| hexdump_print(msg, log_output); |
| } |
| } |
| |
| void log_output_string_syst_process(const struct log_output *log_output, |
| struct log_msg_ids src_level, |
| const char *fmt, va_list ap, uint32_t flag, |
| int16_t source_id) |
| { |
| uint32_t severity = level_to_syst_severity((uint32_t)src_level.level); |
| |
| update_systh_platform_data(&log_syst_handle, log_output, flag); |
| update_handle_origin_unit(&log_syst_handle, source_id); |
| |
| MIPI_SYST_VPRINTF(&log_syst_handle, severity, fmt, ap); |
| } |
| |
| void log_output_hexdump_syst_process(const struct log_output *log_output, |
| struct log_msg_ids src_level, |
| const char *metadata, |
| const uint8_t *data, uint32_t length, |
| uint32_t flag, int16_t source_id) |
| { |
| uint32_t severity = level_to_syst_severity((uint32_t)src_level.level); |
| |
| update_systh_platform_data(&log_syst_handle, log_output, flag); |
| update_handle_origin_unit(&log_syst_handle, source_id); |
| |
| MIPI_SYST_PRINTF(&log_syst_handle, severity, "%s", metadata); |
| |
| while (length != 0U) { |
| uint32_t part_len = MIN(length, HEXDUMP_BYTES_IN_LINE); |
| |
| hexdump_line_print(data, part_len, severity); |
| |
| data += part_len; |
| length -= part_len; |
| } |
| } |
| |
| #else /* !CONFIG_LOG2 */ |
| static void hexdump2_print(const uint8_t *data, uint32_t length, |
| uint32_t severity) |
| { |
| while (length != 0U) { |
| uint32_t part_len = MIN(length, HEXDUMP_BYTES_IN_LINE); |
| |
| hexdump_line_print(data, part_len, severity); |
| |
| data += part_len; |
| length -= part_len; |
| } |
| } |
| |
| #ifndef CONFIG_LOG_MIPI_SYST_USE_CATALOG |
| static int mipi_vprintf_formatter(cbprintf_cb out, void *ctx, |
| const char *fmt, va_list ap) |
| { |
| struct log_msg2 *msg = ctx; |
| uint32_t severity = level_to_syst_severity(log_msg2_get_level(msg)); |
| |
| MIPI_SYST_VPRINTF(&log_syst_handle, severity, fmt, ap); |
| |
| return 0; |
| } |
| #else |
| |
| /* |
| * TODO: Big endian support. |
| * |
| * MIPI Sys-T catalog messages require arguments to be in little endian. |
| * Currently, if the format strings are removed (which is very highly |
| * probable with usage of catalog messages), there is no way to |
| * distinguish arguments in va_list anymore, and thus no longer able |
| * to convert endianness. So assert that we only support little endian |
| * machines for now. |
| */ |
| BUILD_ASSERT(__BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__, |
| "Does not support big endian machines at the moment!"); |
| |
| /* |
| * TODO: Support x86-32 and long double. |
| * |
| * The argument of long double on x86 32-bit is 3 bytes. However, |
| * MIPI Sys-T requires 4 bytes for this. Currently, since we are |
| * copying the argument list as-is, and no way to know which |
| * argument is long double, a long double argument cannot be |
| * expanded to 4 bytes. So error out here to prevent it being |
| * used at all. |
| */ |
| #if defined(CONFIG_X86) && !defined(CONFIG_X86_64) && defined(CONFIG_CBPRINTF_PACKAGE_LONGDOUBLE) |
| #error "x86-32 and CONFIG_CBPRINTF_PACKAGE_LONGDOUBLE is not supported!" |
| #endif |
| |
| /* |
| * TODO: Support integer arguments under 64-bit systems. |
| * |
| * On 64-bit systems, an integer argument is of 8 bytes even though |
| * sizeof(int) == 4. MIPI Sys-T expects an integer argument to be 4 bytes. |
| * Currently, since we are copying argument list as-is, and no way to |
| * know which argument is integer, a 32-bit integer cannot be |
| * shrunk to 4 bytes for output. So error out here to prevent it |
| * being used at all. |
| */ |
| #ifdef CONFIG_64BIT |
| #error "64-bit is not support at the moment!" |
| #endif |
| |
| #ifdef CONFIG_64BIT |
| #define MIPI_SYST_CATMSG_ARGS_COPY MIPI_SYST_CATALOG64_ARGS_COPY |
| #else |
| #define MIPI_SYST_CATMSG_ARGS_COPY MIPI_SYST_CATALOG32_ARGS_COPY |
| #endif |
| |
| static inline bool is_in_log_strings_section(const void *addr) |
| { |
| extern const char __log_strings_start[]; |
| extern const char __log_strings_end[]; |
| |
| if (((const char *)addr >= (const char *)__log_strings_start) && |
| ((const char *)addr < (const char *)__log_strings_end)) { |
| return true; |
| } |
| |
| return false; |
| } |
| |
| static struct k_spinlock payload_lock; |
| static uint8_t payload_buf[CONFIG_LOG_MIPI_SYST_CATALOG_ARGS_BUFFER_SIZE]; |
| static const uint8_t * const payload_buf_end = |
| &payload_buf[CONFIG_LOG_MIPI_SYST_CATALOG_ARGS_BUFFER_SIZE]; |
| |
| enum string_list { |
| NO_STRING_LIST, |
| RO_STR_IDX_LIST, |
| RW_STR_IDX_LIST, |
| APPENDED_STR_LIST, |
| }; |
| |
| /* |
| * @brief Figure out where is the next string argument. |
| * |
| * @param[out] pos Offset in byte of string argument from beginning of package. |
| * |
| * @param[in] ros_remaining How many read-only strings left |
| * @param[in] ros_str_pos Pointer to the read-only string indexes |
| * |
| * @param[in] rws_remaining How many read-write strings left |
| * @param[in] rws_str_pos Pointer to the read-write string indexes |
| * |
| * @param[in] s_remaining How many appended strings left |
| * @param[in] str_pos Pointer to the appended string list |
| * |
| * @retval NO_STRING_LIST No string picked. Usually means there is |
| * no more strings remaining in lists. |
| * @retval RO_STR_IDX_LIST Position coming from read-only string list. |
| * @retval RW_STR_IDX_LIST Position coming from read-write string list. |
| * @retval APPENDED_STR_LIST Position coming from appended string list. |
| */ |
| static inline |
| enum string_list get_next_string_arg(uint16_t *pos, |
| uint8_t ros_remaining, uint8_t *ro_str_pos, |
| uint8_t rws_remaining, uint8_t *rw_str_pos, |
| uint8_t s_remaining, uint8_t *str_pos) |
| { |
| enum string_list which_list = NO_STRING_LIST; |
| |
| if (ros_remaining > 0) { |
| *pos = ro_str_pos[0]; |
| which_list = RO_STR_IDX_LIST; |
| } |
| |
| if (rws_remaining > 0) { |
| if ((which_list == NO_STRING_LIST) || (rw_str_pos[0] < *pos)) { |
| *pos = rw_str_pos[0]; |
| which_list = RW_STR_IDX_LIST; |
| } |
| } |
| |
| if (s_remaining > 0) { |
| /* |
| * The first uint8_t in the appended string list for |
| * each string is its supposed position in |
| * the argument list. |
| */ |
| if ((which_list == NO_STRING_LIST) || (str_pos[0] < *pos)) { |
| *pos = str_pos[0]; |
| which_list = APPENDED_STR_LIST; |
| } |
| } |
| |
| if (which_list != NO_STRING_LIST) { |
| /* |
| * Note that the stored position value is in |
| * multiple of uint32_t. So need to convert it |
| * back to number of bytes. |
| */ |
| *pos *= sizeof(uint32_t); |
| } |
| |
| return which_list; |
| } |
| |
| /** |
| * @brief Build the catalog message payload and send it out. |
| * |
| * This builds the catalog message payload to be sent through MIPI Sys-T |
| * interface. |
| * |
| * For format strings without any string arguments, the argument list |
| * is provided to the MIPI library as-is without any processing. |
| * Otherwise, the strings replace the arguments in the argument list |
| * by replacing the string arguments with the full strings. |
| * |
| * @param[in] severity Severity of log message. |
| * @param[in] fmt Format string. |
| * @param[in] pkg Pointer to log message package. |
| * @param[in] pkg_sz Log message package size in bytes. |
| * @param[in] arg Pointer to argument list inside log message package. |
| * @param[in] arg_sz Argument list size in bytes inside log message package. |
| * @param[in] s_nbr Number of appended strings in log message package. |
| * @param[in] ros_nbr Number of read-only string indexes in log message package. |
| * @param[in] rws_nbr Number of read-write string indexes in log message package. |
| * |
| * @retval 0 Success |
| * @retval -ENOSPC Payload buffer size is too small. |
| */ |
| static int build_catalog_payload(uint32_t severity, const char *fmt, |
| uint8_t *pkg, size_t pkg_sz, |
| uint8_t *arg, size_t arg_sz, |
| uint8_t s_nbr, uint8_t ros_nbr, uint8_t rws_nbr) |
| { |
| uint8_t *cur_str_arg_pos = NULL; |
| uint8_t *payload = payload_buf; |
| enum string_list which_str_list = NO_STRING_LIST; |
| int ret = 0; |
| |
| /* End of argument list. */ |
| uint8_t * const arg_end = arg + arg_sz; |
| |
| /* |
| * Start of read-only strings indexes, which is |
| * after the argument list. Skip the first ro-string |
| * index as it points to the format string. |
| */ |
| uint8_t *ro_str_pos = arg_end + 1; |
| |
| /* |
| * Start of read-write strings indexes, which is |
| * after the argument list, and read-only string |
| * indexes. |
| */ |
| uint8_t *rw_str_pos = arg_end + ros_nbr; |
| |
| /* Start of appended strings in package */ |
| uint8_t *str_pos = arg_end + ros_nbr + rws_nbr; |
| |
| /* Number of strings remaining to be processed */ |
| uint8_t ros_remaining = ros_nbr - 1; |
| uint8_t rws_remaining = rws_nbr; |
| uint8_t s_remaining = s_nbr; |
| |
| k_spinlock_key_t key = k_spin_lock(&payload_lock); |
| |
| do { |
| if (payload == payload_buf_end) { |
| /* |
| * No space left in payload buffer but there are |
| * still arguments left. So bail out. |
| */ |
| ret = -ENOSPC; |
| goto out; |
| } |
| |
| if (cur_str_arg_pos == NULL) { |
| uint16_t str_arg_pos; |
| |
| which_str_list = get_next_string_arg(&str_arg_pos, |
| ros_remaining, ro_str_pos, |
| rws_remaining, rw_str_pos, |
| s_remaining, str_pos); |
| |
| if (which_str_list != NO_STRING_LIST) { |
| cur_str_arg_pos = pkg + str_arg_pos; |
| } else { |
| /* |
| * No more strings remaining from the lists. |
| * Set the pointer to the end of argument list, |
| * which it will never match the incrementally |
| * moving arg, and also string selection will |
| * no longer be carried out. |
| */ |
| cur_str_arg_pos = arg + arg_sz; |
| } |
| } |
| |
| if (arg == cur_str_arg_pos) { |
| /* |
| * The current argument is a string pointer. |
| * So need to copy the string into the payload. |
| */ |
| |
| size_t str_sz = 0; |
| |
| /* Extract the string pointer from package */ |
| uint8_t *s = *((uint8_t **)arg); |
| |
| /* Skip the string pointer for next argument */ |
| arg += sizeof(void *); |
| |
| /* Copy the string over. */ |
| while (s[0] != '\0') { |
| payload[0] = s[0]; |
| payload++; |
| str_sz++; |
| s++; |
| |
| if (payload == payload_buf_end) { |
| /* |
| * No space left in payload buffer but there are |
| * still characters to be copied. So bail out. |
| */ |
| ret = -ENOSPC; |
| goto out; |
| } |
| } |
| |
| /* Need to terminate the string */ |
| payload[0] = '\0'; |
| payload++; |
| |
| if (which_str_list == RO_STR_IDX_LIST) { |
| /* Move to next read-only string index */ |
| ro_str_pos++; |
| ros_remaining--; |
| } else if (which_str_list == RW_STR_IDX_LIST) { |
| /* Move to next read-write string index */ |
| rw_str_pos++; |
| rws_remaining--; |
| } else if (which_str_list == APPENDED_STR_LIST) { |
| /* |
| * str_pos needs to skip the string index, |
| * the string itself, and the NULL character. |
| * So next time it points to another position |
| * index. |
| */ |
| str_pos += str_sz + 2; |
| s_remaining--; |
| } |
| |
| cur_str_arg_pos = NULL; |
| } else { |
| /* |
| * Copy until the next string argument |
| * (or until the end of argument list). |
| */ |
| while (arg < cur_str_arg_pos) { |
| payload[0] = arg[0]; |
| payload++; |
| arg++; |
| |
| if (payload == payload_buf_end) { |
| /* |
| * No space left in payload buffer but there are |
| * still characters to be copied. So bail out. |
| */ |
| ret = -ENOSPC; |
| goto out; |
| } |
| } |
| } |
| } while (arg < arg_end); |
| |
| MIPI_SYST_CATMSG_ARGS_COPY(&log_syst_handle, severity, |
| (uintptr_t)fmt, |
| payload_buf, |
| (size_t)(payload - payload_buf)); |
| out: |
| k_spin_unlock(&payload_lock, key); |
| return ret; |
| } |
| |
| static int mipi_catalog_formatter(cbprintf_cb out, void *ctx, |
| const char *fmt, va_list ap) |
| { |
| int ret = 0; |
| struct log_msg2 *msg = ctx; |
| uint32_t severity = level_to_syst_severity(log_msg2_get_level(msg)); |
| |
| if (is_in_log_strings_section(fmt)) { |
| /* |
| * Note that only format strings that are in |
| * the log_strings_section are processed as |
| * catalog messages because only these strings |
| * are in the collateral XML file. |
| */ |
| |
| size_t pkg_sz, arg_sz; |
| uint8_t s_nbr, ros_nbr, rws_nbr, total_str; |
| uint8_t *arg; |
| |
| uint8_t *pkg = log_msg2_get_package(msg, &pkg_sz); |
| |
| /* |
| * Need to skip the package header (of pointer size), |
| * and the pointer to format string to get to |
| * the argument list. |
| */ |
| arg = pkg + 2 * sizeof(void *); |
| arg_sz = pkg[0] * sizeof(uint32_t) - 2 * sizeof(void *); |
| |
| /* Number of appended strings already in the package. */ |
| s_nbr = pkg[1]; |
| |
| /* Number of string indexes, for both read-only and read-write strings. */ |
| ros_nbr = pkg[2]; |
| rws_nbr = pkg[3]; |
| |
| total_str = s_nbr + rws_nbr; |
| if (ros_nbr > 0) { |
| /* |
| * If there are read-only string indexes, the first |
| * is the format string. So we can ignore that as |
| * we are not copying it to the catalog message |
| * payload. |
| */ |
| total_str += ros_nbr - 1; |
| } |
| |
| if (total_str == 0) { |
| /* |
| * There are no string arguments so the argument list |
| * inside the package can be used as-is. The first |
| * read-only string pointer is the the format string so |
| * we can ignore that. |
| */ |
| MIPI_SYST_CATMSG_ARGS_COPY(&log_syst_handle, severity, |
| (uintptr_t)fmt, |
| arg, |
| arg_sz); |
| } else { |
| ret = build_catalog_payload(severity, fmt, pkg, pkg_sz, |
| arg, arg_sz, |
| s_nbr, ros_nbr, rws_nbr); |
| } |
| |
| } else { |
| MIPI_SYST_VPRINTF(&log_syst_handle, severity, fmt, ap); |
| } |
| |
| return ret; |
| } |
| #endif /* !CONFIG_LOG_MIPI_SYST_USE_CATALOG */ |
| |
| void log_output_msg2_syst_process(const struct log_output *output, |
| struct log_msg2 *msg, uint32_t flag) |
| { |
| size_t len, hexdump_len; |
| |
| update_systh_platform_data(&log_syst_handle, output, flag); |
| |
| #ifdef CONFIG_LOG_MIPI_SYST_OUTPUT_LOG_MSG_SRC_ID |
| uint8_t level = log_msg2_get_level(msg); |
| bool raw_string = (level == LOG_LEVEL_INTERNAL_RAW_STRING); |
| int16_t source_id = CONFIG_LOG_MIPI_SYST_MSG_DEFAULT_MODULE_ID; |
| |
| /* Set the log source ID as Sys-T message module ID */ |
| if (!raw_string) { |
| void *source = (void *)log_msg2_get_source(msg); |
| |
| if (source != NULL) { |
| source_id = IS_ENABLED(CONFIG_LOG_RUNTIME_FILTERING) ? |
| log_dynamic_source_id(source) : |
| log_const_source_id(source); |
| } |
| } |
| |
| update_handle_origin_unit(&log_syst_handle, source_id); |
| #endif |
| |
| uint8_t *data = log_msg2_get_package(msg, &len); |
| |
| if (len) { |
| (void)cbpprintf_external(NULL, |
| #ifdef CONFIG_LOG_MIPI_SYST_USE_CATALOG |
| mipi_catalog_formatter, |
| #else |
| mipi_vprintf_formatter, |
| #endif |
| msg, data); |
| } |
| |
| data = log_msg2_get_data(msg, &hexdump_len); |
| if (hexdump_len) { |
| uint32_t severity = level_to_syst_severity(log_msg2_get_level(msg)); |
| |
| hexdump2_print(data, hexdump_len, severity); |
| } |
| } |
| #endif /* !CONFIG_LOG2 */ |
| |
| static int syst_init(const struct device *arg) |
| { |
| ARG_UNUSED(arg); |
| |
| MIPI_SYST_INIT_STATE(&log_syst_header, |
| mipi_syst_platform_init, (void *)0); |
| |
| MIPI_SYST_INIT_HANDLE_STATE(&log_syst_header, |
| &log_syst_handle, NULL); |
| |
| log_syst_handle.systh_tag.et_guid = 0; |
| |
| #ifndef CONFIG_LOG_MIPI_SYST_OUTPUT_LOG_MSG_SRC_ID |
| /* Set the default here once as it won't be modified anymore. */ |
| log_syst_handle.systh_tag.et_modunit = |
| _MIPI_SYST_MK_MODUNIT_ORIGIN( |
| CONFIG_LOG_MIPI_SYST_MSG_DEFAULT_MODULE_ID, |
| CONFIG_LOG_MIPI_SYST_MSG_DEFAULT_UNIT_ID |
| ); |
| #endif |
| |
| return 0; |
| } |
| |
| SYS_INIT(syst_init, POST_KERNEL, 0); |