blob: 156cf06f5b5f71305473408013b5ac010939f401 [file] [log] [blame]
Wentong Wudd948092019-10-29 05:45:55 +08001/*
2 * Copyright (c) 2019 Intel corporation
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
Gerard Marull-Paretas5113c142022-05-06 11:12:04 +02007#include <zephyr/init.h>
Wentong Wudd948092019-10-29 05:45:55 +08008#include <stdio.h>
9#include <ctype.h>
Gerard Marull-Paretas5113c142022-05-06 11:12:04 +020010#include <zephyr/kernel.h>
Wentong Wudd948092019-10-29 05:45:55 +080011#include <mipi_syst.h>
Gerard Marull-Paretas5113c142022-05-06 11:12:04 +020012#include <zephyr/spinlock.h>
Gerard Marull-Paretas5113c142022-05-06 11:12:04 +020013#include <zephyr/sys/__assert.h>
Daniel Leung134d26b2022-02-04 13:18:13 -080014#include <zephyr/sys/check.h>
Gerard Marull-Paretas5113c142022-05-06 11:12:04 +020015#include <zephyr/linker/utils.h>
16#include <zephyr/logging/log.h>
17#include <zephyr/logging/log_ctrl.h>
18#include <zephyr/logging/log_output.h>
Wentong Wudd948092019-10-29 05:45:55 +080019
Wentong Wuc3f38db2020-05-06 20:54:13 +080020static struct mipi_syst_header log_syst_header;
21static struct mipi_syst_handle log_syst_handle;
22
Daniel Leung4b6eb552022-01-11 15:38:52 -080023#define HEXDUMP_BYTES_IN_LINE 16
24
Daniel Leung4b6eb552022-01-11 15:38:52 -080025#define STRING_BUF_MAX_LEN 128
Daniel Leung4b6eb552022-01-11 15:38:52 -080026
Wentong Wuc3f38db2020-05-06 20:54:13 +080027#if defined(MIPI_SYST_PCFG_ENABLE_PLATFORM_STATE_DATA)
Wentong Wuc85d3532020-10-30 11:33:13 +080028#if defined(CONFIG_MIPI_SYST_STP)
29static mipi_syst_u16 master = 128;
30static mipi_syst_u16 channel = 1;
31
32static struct stp_writer_data writer_state;
33#elif !defined(CONFIG_MIPI_SYST_RAW_DATA)
Wentong Wuc3f38db2020-05-06 20:54:13 +080034static const char pattern[] = "SYS-T RAW DATA: ";
35static const char valToHex[] = "0123456789ABCDEF";
Wentong Wuc85d3532020-10-30 11:33:13 +080036#endif
Wentong Wuc3f38db2020-05-06 20:54:13 +080037
38static int out_func(int c, void *ctx)
39{
40 const struct log_output *out_ctx = (const struct log_output *)ctx;
41
Kumar Galaa1b77fd2020-05-27 11:26:57 -050042 out_ctx->buf[out_ctx->control_block->offset] = (uint8_t)c;
Wentong Wuc3f38db2020-05-06 20:54:13 +080043 out_ctx->control_block->offset++;
44
45 __ASSERT_NO_MSG(out_ctx->control_block->offset <= out_ctx->size);
46
47 if (out_ctx->control_block->offset == out_ctx->size) {
48 log_output_flush(out_ctx);
49 }
50
51 return 0;
52}
53
Wentong Wuc85d3532020-10-30 11:33:13 +080054#if defined(CONFIG_MIPI_SYST_STP)
55static void stp_write_putNibble(struct mipi_syst_handle *systh,
56 struct stp_writer_data *p, mipi_syst_u8 n)
57{
58 p->current |= (n << 4);
59 p->byteDone = !p->byteDone;
60
61 if (p->byteDone) {
62 out_func(p->current, systh->systh_platform.log_output);
63 p->current = 0;
64 } else {
65 p->current >>= 4;
66 }
67}
68
69static void stp_write_flush(struct mipi_syst_handle *systh,
70 struct stp_writer_data *p)
71{
72 if (!p->byteDone) {
73 stp_write_putNibble(systh, p, 0);
74 }
75}
76
77static void stp_write_d4(struct mipi_syst_handle *systh,
78 struct stp_writer_data *p, mipi_syst_u8 v)
79{
80 stp_write_putNibble(systh, p, v);
81}
82
83static void stp_write_payload8(struct mipi_syst_handle *systh,
84 struct stp_writer_data *p, mipi_syst_u8 v)
85{
86 stp_write_d4(systh, p, v);
87 stp_write_d4(systh, p, v>>4);
88}
89
90static void stp_write_payload16(struct mipi_syst_handle *systh,
91 struct stp_writer_data *p, mipi_syst_u16 v)
92{
93 stp_write_payload8(systh, p, (mipi_syst_u8)v);
94 stp_write_payload8(systh, p, (mipi_syst_u8)(v>>8));
95}
96
97static void stp_write_payload32(struct mipi_syst_handle *systh,
98 struct stp_writer_data *p, mipi_syst_u32 v)
99{
100 stp_write_payload16(systh, p, (mipi_syst_u16)v);
101 stp_write_payload16(systh, p, (mipi_syst_u16)(v>>16));
102}
103
104static void stp_write_payload64(struct mipi_syst_handle *systh,
105 struct stp_writer_data *p, mipi_syst_u64 v)
106{
107 stp_write_payload32(systh, p, (mipi_syst_u32)v);
108 stp_write_payload32(systh, p, (mipi_syst_u32)(v>>32));
109}
110
111static mipi_syst_u64 deltaTime(struct stp_writer_data *p)
112{
113 mipi_syst_u64 delta;
114
115 delta = mipi_syst_get_epoch() - p->timestamp;
116 return delta * 60;
117}
118
119static void stp_write_d32mts(struct mipi_syst_handle *systh,
120 struct stp_writer_data *p, mipi_syst_u32 v)
121{
122 stp_write_d4(systh, p, 0xA);
123 stp_write_payload32(systh, p, v);
124
125 stp_write_d4(systh, p, 0xE);
126 stp_write_payload64(systh, p, deltaTime(p));
127}
128
129static void stp_write_d64mts(struct mipi_syst_handle *systh,
130 struct stp_writer_data *p, mipi_syst_u64 v)
131{
132 stp_write_d4(systh, p, 0xB);
133 stp_write_payload64(systh, p, v);
134
135 stp_write_d4(systh, p, 0xE);
136 stp_write_payload64(systh, p, deltaTime(p));
137}
138
139static void stp_write_d32ts(struct mipi_syst_handle *systh,
140 struct stp_writer_data *p, mipi_syst_u32 v)
141{
142 stp_write_d4(systh, p, 0xF);
143 stp_write_d4(systh, p, 0x6);
144
145 stp_write_payload32(systh, p, v);
146
147 stp_write_d4(systh, p, 0xE);
148 stp_write_payload64(systh, p, deltaTime(p));
149}
150
151static void stp_write_d8(struct mipi_syst_handle *systh,
152 struct stp_writer_data *p, mipi_syst_u8 v)
153{
154 stp_write_d4(systh, p, 0x4);
155 stp_write_payload8(systh, p, v);
156}
157
158static void stp_write_d16(struct mipi_syst_handle *systh,
159 struct stp_writer_data *p, mipi_syst_u16 v)
160{
161 stp_write_d4(systh, p, 0x5);
162 stp_write_payload16(systh, p, v);
163}
164
165static void stp_write_d32(struct mipi_syst_handle *systh,
166 struct stp_writer_data *p, mipi_syst_u32 v)
167{
168 stp_write_d4(systh, p, 0x6);
169 stp_write_payload32(systh, p, v);
170}
171
172#if defined(MIPI_SYST_PCFG_ENABLE_64BIT_IO)
173static void stp_write_d64(struct mipi_syst_handle *systh,
174 struct stp_writer_data *p, mipi_syst_u64 v)
175{
176 stp_write_d4(systh, p, 0x7);
177 stp_write_payload64(systh, p, v);
178}
179#endif
180
181static void stp_write_flag(struct mipi_syst_handle *systh,
182 struct stp_writer_data *p)
183{
184 stp_write_d4(systh, p, 0xF);
185 stp_write_d4(systh, p, 0xE);
186}
187
188static void stp_write_async(struct mipi_syst_handle *systh,
189 struct stp_writer_data *p)
190{
191 for (int i = 0; i < 21; ++i) {
192 stp_write_d4(systh, p, 0xF);
193 }
194
195 stp_write_d4(systh, p, 0x0);
196}
197
198static void stp_write_version(struct mipi_syst_handle *systh,
199 struct stp_writer_data *p)
200{
201 stp_write_d4(systh, p, 0xF);
202 stp_write_d4(systh, p, 0x0);
203 stp_write_d4(systh, p, 0x0);
204
205 stp_write_d4(systh, p, 0x3);
206
207 p->master = 0;
208 p->channel = 0;
209}
210
211static void stp_write_freq(struct mipi_syst_handle *systh,
212 struct stp_writer_data *p)
213{
214 stp_write_d4(systh, p, 0xF);
215 stp_write_d4(systh, p, 0x0);
216 stp_write_d4(systh, p, 0x8);
217 stp_write_payload32(systh, p, 60 * 1000 * 1000);
218}
219
220static void stp_write_setMC(struct mipi_syst_handle *systh,
221 struct stp_writer_data *p,
222 mipi_syst_u16 master,
223 mipi_syst_u16 channel)
224{
225 if (!(p->recordCount++ % 20)) {
226 stp_write_async(systh, p);
227 stp_write_version(systh, p);
228 stp_write_freq(systh, p);
229 }
230
231 if (p->master != master) {
232 stp_write_d4(systh, p, 0xF);
233 stp_write_d4(systh, p, 0x1);
234 stp_write_payload16(systh, p, master);
235
236 p->master = master;
237 p->channel = 0;
238 }
239
240 if (p->channel != channel) {
241 stp_write_d4(systh, p, 0xF);
242 stp_write_d4(systh, p, 0x3);
243 stp_write_payload16(systh, p, channel);
244
245 p->channel = channel;
246 }
247}
248#else
Wentong Wuc3f38db2020-05-06 20:54:13 +0800249static void write_raw(struct mipi_syst_handle *systh, const void *p, int n)
250{
251 int i;
Kumar Galaa1b77fd2020-05-27 11:26:57 -0500252 uint8_t c;
Wentong Wuc3f38db2020-05-06 20:54:13 +0800253
254#if defined(MIPI_SYST_BIG_ENDIAN)
255 for (i = n - 1; i >= 0; --i) {
256#else
257 for (i = 0; i < n; ++i) {
258#endif
Kumar Galaa1b77fd2020-05-27 11:26:57 -0500259 c = ((const uint8_t *)p)[i];
Wentong Wuc85d3532020-10-30 11:33:13 +0800260#if defined(CONFIG_MIPI_SYST_RAW_DATA)
261 out_func(c, systh->systh_platform.log_output);
262#else
Wentong Wuc3f38db2020-05-06 20:54:13 +0800263 out_func(valToHex[c >> 0x4], systh->systh_platform.log_output);
264 out_func(valToHex[c & 0xF], systh->systh_platform.log_output);
Wentong Wuc85d3532020-10-30 11:33:13 +0800265#endif
Wentong Wuc3f38db2020-05-06 20:54:13 +0800266 }
267}
Wentong Wuc85d3532020-10-30 11:33:13 +0800268#endif
Wentong Wuc3f38db2020-05-06 20:54:13 +0800269
Kumar Galaa1b77fd2020-05-27 11:26:57 -0500270static void write_d8(struct mipi_syst_handle *systh, uint8_t v)
Wentong Wuc3f38db2020-05-06 20:54:13 +0800271{
Wentong Wuc85d3532020-10-30 11:33:13 +0800272#if defined(CONFIG_MIPI_SYST_STP)
273 struct stp_writer_data *writer =
274 systh->systh_header->systh_platform.stpWriter;
275
276 stp_write_d8(systh, writer, v);
277#else
Wentong Wuc3f38db2020-05-06 20:54:13 +0800278 write_raw(systh, &v, sizeof(v));
Wentong Wuc85d3532020-10-30 11:33:13 +0800279#endif
Wentong Wuc3f38db2020-05-06 20:54:13 +0800280}
281
Kumar Galaa1b77fd2020-05-27 11:26:57 -0500282static void write_d16(struct mipi_syst_handle *systh, uint16_t v)
Wentong Wuc3f38db2020-05-06 20:54:13 +0800283{
Wentong Wuc85d3532020-10-30 11:33:13 +0800284#if defined(CONFIG_MIPI_SYST_STP)
285 struct stp_writer_data *writer =
286 systh->systh_header->systh_platform.stpWriter;
287
288 stp_write_d16(systh, writer, v);
289#else
Wentong Wuc3f38db2020-05-06 20:54:13 +0800290 write_raw(systh, &v, sizeof(v));
Wentong Wuc85d3532020-10-30 11:33:13 +0800291#endif
Wentong Wuc3f38db2020-05-06 20:54:13 +0800292}
293
Kumar Galaa1b77fd2020-05-27 11:26:57 -0500294static void write_d32(struct mipi_syst_handle *systh, uint32_t v)
Wentong Wuc3f38db2020-05-06 20:54:13 +0800295{
Wentong Wuc85d3532020-10-30 11:33:13 +0800296#if defined(CONFIG_MIPI_SYST_STP)
297 struct stp_writer_data *writer =
298 systh->systh_header->systh_platform.stpWriter;
299
300 stp_write_d32(systh, writer, v);
301#else
Wentong Wuc3f38db2020-05-06 20:54:13 +0800302 write_raw(systh, &v, sizeof(v));
Wentong Wuc85d3532020-10-30 11:33:13 +0800303#endif
Wentong Wuc3f38db2020-05-06 20:54:13 +0800304}
305
306#if defined(MIPI_SYST_PCFG_ENABLE_64BIT_IO)
Kumar Galaa1b77fd2020-05-27 11:26:57 -0500307static void write_d64(struct mipi_syst_handle *systh, uint64_t v)
Wentong Wuc3f38db2020-05-06 20:54:13 +0800308{
Wentong Wuc85d3532020-10-30 11:33:13 +0800309#if defined(CONFIG_MIPI_SYST_STP)
310 struct stp_writer_data *writer =
311 systh->systh_header->systh_platform.stpWriter;
312
313 stp_write_d64(systh, writer, v);
314#else
Wentong Wuc3f38db2020-05-06 20:54:13 +0800315 write_raw(systh, &v, sizeof(v));
Wentong Wuc85d3532020-10-30 11:33:13 +0800316#endif
Wentong Wuc3f38db2020-05-06 20:54:13 +0800317}
318#endif
319
Kumar Galaa1b77fd2020-05-27 11:26:57 -0500320static void write_d32ts(struct mipi_syst_handle *systh, uint32_t v)
Wentong Wuc3f38db2020-05-06 20:54:13 +0800321{
Wentong Wuc85d3532020-10-30 11:33:13 +0800322#if defined(CONFIG_MIPI_SYST_STP)
323 struct stp_writer_data *writer =
324 systh->systh_header->systh_platform.stpWriter;
325
326 stp_write_setMC(systh, writer,
327 systh->systh_platform.master,
328 systh->systh_platform.channel);
329 stp_write_d32ts(systh, writer, v);
330#elif defined(CONFIG_MIPI_SYST_RAW_DATA)
331 ARG_UNUSED(systh);
332
333 write_raw(systh, &v, sizeof(v));
334#else
Wentong Wuc3f38db2020-05-06 20:54:13 +0800335 for (int i = 0; i < strlen(pattern); i++) {
336 out_func(pattern[i], systh->systh_platform.log_output);
337 }
338
339 write_raw(systh, &v, sizeof(v));
Wentong Wuc85d3532020-10-30 11:33:13 +0800340#endif
341}
342
343static void write_d32mts(struct mipi_syst_handle *systh, mipi_syst_u32 v)
344{
345#if defined(CONFIG_MIPI_SYST_STP)
346 struct stp_writer_data *writer =
347 systh->systh_header->systh_platform.stpWriter;
348
349 stp_write_setMC(systh, writer,
350 systh->systh_platform.master,
351 systh->systh_platform.channel);
352 stp_write_d32mts(systh, writer, v);
353#else
354 ARG_UNUSED(systh);
355 ARG_UNUSED(v);
356#endif
357}
358
359static void write_d64mts(struct mipi_syst_handle *systh, mipi_syst_u64 v)
360{
361#if defined(CONFIG_MIPI_SYST_STP)
362 struct stp_writer_data *writer =
363 systh->systh_header->systh_platform.stpWriter;
364
365 stp_write_setMC(systh, writer,
366 systh->systh_platform.master,
367 systh->systh_platform.channel);
368 stp_write_d64mts(systh, writer, v);
369#else
370 ARG_UNUSED(systh);
371 ARG_UNUSED(v);
372#endif
Wentong Wuc3f38db2020-05-06 20:54:13 +0800373}
374
375static void write_flag(struct mipi_syst_handle *systh)
376{
Wentong Wuc85d3532020-10-30 11:33:13 +0800377#if defined(CONFIG_MIPI_SYST_STP)
378 struct stp_writer_data *writer =
379 systh->systh_header->systh_platform.stpWriter;
380
381 stp_write_flag(systh, writer);
382 stp_write_flush(systh, writer);
383#elif defined(CONFIG_MIPI_SYST_RAW_DATA)
384 ARG_UNUSED(systh);
385#else
Kumar Galaa1b77fd2020-05-27 11:26:57 -0500386 uint32_t flag = systh->systh_platform.flag;
Wentong Wuc3f38db2020-05-06 20:54:13 +0800387
388 if ((flag & LOG_OUTPUT_FLAG_CRLF_NONE) != 0U) {
389 return;
390 }
391
392 if ((flag & LOG_OUTPUT_FLAG_CRLF_LFONLY) != 0U) {
393 out_func('\n', systh->systh_platform.log_output);
394 } else {
395 out_func('\r', systh->systh_platform.log_output);
396 out_func('\n', systh->systh_platform.log_output);
397 }
Wentong Wuc85d3532020-10-30 11:33:13 +0800398#endif
Wentong Wuc3f38db2020-05-06 20:54:13 +0800399}
400#endif
401
402#if defined(MIPI_SYST_PCFG_ENABLE_TIMESTAMP)
403mipi_syst_u64 mipi_syst_get_epoch(void)
404{
405 return k_uptime_ticks();
406}
407#endif
408
409static void update_systh_platform_data(struct mipi_syst_handle *handle,
410 const struct log_output *log_output,
Kumar Galaa1b77fd2020-05-27 11:26:57 -0500411 uint32_t flag)
Wentong Wuc3f38db2020-05-06 20:54:13 +0800412{
413#if defined(MIPI_SYST_PCFG_ENABLE_PLATFORM_STATE_DATA)
414 handle->systh_platform.flag = (mipi_syst_u32)flag;
415 handle->systh_platform.log_output = (struct log_output *)log_output;
416#endif
417}
418
Krzysztof Chruscinskic5f2cde2022-06-15 07:46:35 +0200419#if defined(CONFIG_LOG_MIPI_SYST_OUTPUT_LOG_MSG_SRC_ID)
Daniel Leung6ec1fcc2022-04-07 16:00:22 -0700420/**
421 * @brief Set module ID in the origin unit of Sys-T message
422 *
423 * Note that this only sets the module ID if
424 * CONFIG_LOG_MIPI_SYST_OUTPUT_LOG_MSG_SRC_ID is enabled.
425 * Otherwise, this is a no-op as the module ID is set to
426 * default at boot time, and no need to be set again.
427 *
428 * @param handle Pointer to mipi_syst_handle struct
429 * @param module_id Module ID to be set (range 0x00 - 0x7F)
430 */
431static void update_handle_origin_unit(struct mipi_syst_handle *handle,
432 int16_t module_id)
433{
434 handle->systh_tag.et_modunit =
435 _MIPI_SYST_MK_MODUNIT_ORIGIN(
436 module_id,
437 CONFIG_LOG_MIPI_SYST_MSG_DEFAULT_UNIT_ID
438 );
439}
440#endif
441
Wentong Wuc3f38db2020-05-06 20:54:13 +0800442#if defined(MIPI_SYST_PCFG_ENABLE_PLATFORM_HANDLE_DATA)
443/*
444 * Platform specific SyS-T handle initialization hook function
445 *
446 * @param systh pointer to the SyS-T handle structure
447 */
448static void platform_handle_init(struct mipi_syst_handle *systh)
449{
Wentong Wuc85d3532020-10-30 11:33:13 +0800450#if defined(CONFIG_MIPI_SYST_STP)
451 if (channel > 127) {
452 ++master;
453 channel = 1;
454 }
455
456 systh->systh_platform.channel = channel++;
457 systh->systh_platform.master = master;
458#endif
459
Wentong Wu450f21f2020-05-06 21:05:38 +0800460#if defined(MIPI_SYST_PCFG_LENGTH_FIELD)
461 MIPI_SYST_ENABLE_HANDLE_LENGTH(systh, 1);
462#endif
Wentong Wuab8392f2020-05-06 21:10:08 +0800463
464#if defined(MIPI_SYST_PCFG_ENABLE_TIMESTAMP)
465 MIPI_SYST_ENABLE_HANDLE_TIMESTAMP(systh, 1);
466#endif
Wentong Wuc3f38db2020-05-06 20:54:13 +0800467}
468
469static void platform_handle_release(struct mipi_syst_handle *systh)
470{
471 ARG_UNUSED(systh);
472}
473#endif
474
475/*
476 * Platform specific global state initialization hook function
477 *
478 * @param systh pointer to the new SyS-T handle structure
479 * @param platform_data user defined data for the init function.
480 *
481 */
482static void mipi_syst_platform_init(struct mipi_syst_header *systh,
483 const void *platform_data)
484{
Wentong Wuc85d3532020-10-30 11:33:13 +0800485#if defined(CONFIG_MIPI_SYST_STP)
486 writer_state.byteDone = 0;
487 writer_state.current = 0;
488 writer_state.master = 0;
489 writer_state.channel = 0;
490 writer_state.recordCount = 0;
491 writer_state.timestamp = mipi_syst_get_epoch();
492
493 systh->systh_platform.stpWriter = &writer_state;
494#endif
495
Wentong Wuc3f38db2020-05-06 20:54:13 +0800496#if defined(MIPI_SYST_PCFG_ENABLE_PLATFORM_HANDLE_DATA)
497 systh->systh_inith = platform_handle_init;
498 systh->systh_releaseh = platform_handle_release;
499#endif
500#if defined(MIPI_SYST_PCFG_ENABLE_PLATFORM_STATE_DATA)
501 systh->systh_platform.write_d8 = write_d8;
502 systh->systh_platform.write_d16 = write_d16;
503 systh->systh_platform.write_d32 = write_d32;
504#if defined(MIPI_SYST_PCFG_ENABLE_64BIT_IO)
505 systh->systh_platform.write_d64 = write_d64;
506#endif
507 systh->systh_platform.write_d32ts = write_d32ts;
Wentong Wuc85d3532020-10-30 11:33:13 +0800508 systh->systh_platform.write_d32mts = write_d32mts;
509 systh->systh_platform.write_d64mts = write_d64mts;
Wentong Wuc3f38db2020-05-06 20:54:13 +0800510 systh->systh_platform.write_flag = write_flag;
511#endif
512}
Wentong Wudd948092019-10-29 05:45:55 +0800513
514/*
515 * 0 MIPI_SYST_SEVERITY_MAX no assigned severity
516 * 1 MIPI_SYST_SEVERITY_FATAL critical error level
517 * 2 MIPI_SYST_SEVERITY_ERROR error message level
518 * 3 MIPI_SYST_SEVERITY_WARNING warning message level
519 * 4 MIPI_SYST_SEVERITY_INFO information message level
520 * 5 MIPI_SYST_SEVERITY_USER1 user defined level 5
521 * 6 MIPI_SYST_SEVERITY_USER2 user defined level 6
522 * 7 MIPI_SYST_SEVERITY_DEBUG debug information level
523 */
Kumar Galaa1b77fd2020-05-27 11:26:57 -0500524static uint32_t level_to_syst_severity(uint32_t level)
Wentong Wudd948092019-10-29 05:45:55 +0800525{
Kumar Galaa1b77fd2020-05-27 11:26:57 -0500526 uint32_t ret;
Wentong Wudd948092019-10-29 05:45:55 +0800527
528 switch (level) {
529 case LOG_LEVEL_NONE:
530 ret = 0U;
531 break;
532 case LOG_LEVEL_ERR:
533 ret = 2U;
534 break;
535 case LOG_LEVEL_WRN:
536 ret = 3U;
537 break;
538 case LOG_LEVEL_INF:
539 ret = 4U;
540 break;
541 case LOG_LEVEL_DBG:
542 ret = 7U;
543 break;
544 default:
545 ret = 7U;
546 break;
547 }
548
549 return ret;
550}
551
Daniel Leung4b6eb552022-01-11 15:38:52 -0800552static void hexdump_line_print(const uint8_t *data, uint32_t length,
553 uint32_t severity)
554{
555 char hexdump_buf[HEXDUMP_BYTES_IN_LINE * 4 + 4];
556
557 hexdump_buf[sizeof(hexdump_buf) - 1] = '\0';
558 char *buf = &hexdump_buf[0];
559
560 for (int i = 0; i < HEXDUMP_BYTES_IN_LINE; i++) {
561 if (i > 0 && !(i % 8)) {
562 *buf = ' ';
563 buf++;
564 }
565
566 if (i < length) {
567 sprintf(buf, "%02x ", data[i]);
568 } else {
569 sprintf(buf, " ");
570 }
571
572 buf += 3;
573 }
574
575 *buf = '|';
576 buf++;
577
578 for (int i = 0; i < HEXDUMP_BYTES_IN_LINE; i++) {
579 if (i > 0 && !(i % 8)) {
580 *buf = ' ';
581 buf++;
582 }
583
584 if (i < length) {
Dennis Sitelew42f9b692022-09-09 12:41:32 +0200585 unsigned char c = (unsigned char)data[i];
Daniel Leung4b6eb552022-01-11 15:38:52 -0800586
587 *buf = isprint((int)c) ? c : '.';
588 } else {
589 *buf = ' ';
590 }
591
592 buf++;
593 }
594
595 MIPI_SYST_PRINTF(&log_syst_handle, severity, "%s", hexdump_buf);
596}
597
Daniel Leung4b6eb552022-01-11 15:38:52 -0800598static void hexdump2_print(const uint8_t *data, uint32_t length,
599 uint32_t severity)
Aastha Grovera89fae72021-10-28 16:07:54 -0700600{
Daniel Leung4b6eb552022-01-11 15:38:52 -0800601 while (length != 0U) {
602 uint32_t part_len = MIN(length, HEXDUMP_BYTES_IN_LINE);
Aastha Grovera89fae72021-10-28 16:07:54 -0700603
Daniel Leung4b6eb552022-01-11 15:38:52 -0800604 hexdump_line_print(data, part_len, severity);
605
606 data += part_len;
607 length -= part_len;
608 }
Aastha Grovera89fae72021-10-28 16:07:54 -0700609}
610
Daniel Leungba488d12022-02-04 13:18:13 -0800611static int mipi_vprintf_formatter(cbprintf_cb out, void *ctx,
Daniel Leung57982972022-02-01 12:46:55 -0800612 const char *fmt, va_list ap)
613{
Krzysztof Chruscinski9833ca62022-06-21 11:51:38 +0200614 struct log_msg *msg = ctx;
615 uint32_t severity = level_to_syst_severity(log_msg_get_level(msg));
Daniel Leung57982972022-02-01 12:46:55 -0800616
617 MIPI_SYST_VPRINTF(&log_syst_handle, severity, fmt, ap);
618
619 return 0;
620}
Daniel Leungba488d12022-02-04 13:18:13 -0800621
Daniel Leung134d26b2022-02-04 13:18:13 -0800622#ifdef CONFIG_LOG_MIPI_SYST_USE_CATALOG
Daniel Leungba488d12022-02-04 13:18:13 -0800623
624#ifdef CONFIG_64BIT
625#define MIPI_SYST_CATMSG_ARGS_COPY MIPI_SYST_CATALOG64_ARGS_COPY
626#else
627#define MIPI_SYST_CATMSG_ARGS_COPY MIPI_SYST_CATALOG32_ARGS_COPY
628#endif
629
630static inline bool is_in_log_strings_section(const void *addr)
631{
Kumar Galacaea9dc2023-03-16 05:55:18 +0000632 TYPE_SECTION_START_EXTERN(const char *, log_strings);
633 TYPE_SECTION_END_EXTERN(const char *, log_strings);
Daniel Leungba488d12022-02-04 13:18:13 -0800634
Kumar Galacaea9dc2023-03-16 05:55:18 +0000635 if (((const char *)addr >= (const char *)TYPE_SECTION_START(log_strings)) &&
636 ((const char *)addr < (const char *)TYPE_SECTION_END(log_strings))) {
Daniel Leungba488d12022-02-04 13:18:13 -0800637 return true;
638 }
639
640 return false;
641}
642
Daniel Leung134d26b2022-02-04 13:18:13 -0800643
Daniel Leungba488d12022-02-04 13:18:13 -0800644static struct k_spinlock payload_lock;
645static uint8_t payload_buf[CONFIG_LOG_MIPI_SYST_CATALOG_ARGS_BUFFER_SIZE];
Daniel Leungba488d12022-02-04 13:18:13 -0800646
Daniel Leung134d26b2022-02-04 13:18:13 -0800647static int mipi_catalog_formatter(cbprintf_cb out, void *ctx,
648 const char *fmt, va_list ap)
Daniel Leungba488d12022-02-04 13:18:13 -0800649{
Krzysztof Chruscinski9833ca62022-06-21 11:51:38 +0200650 struct log_msg *msg = ctx;
651 uint32_t severity = level_to_syst_severity(log_msg_get_level(msg));
Daniel Leung134d26b2022-02-04 13:18:13 -0800652 k_spinlock_key_t key;
Daniel Leungba488d12022-02-04 13:18:13 -0800653
Daniel Leung134d26b2022-02-04 13:18:13 -0800654 union {
655 mipi_syst_u64 v64;
656 mipi_syst_u32 v32;
Daniel Leungba488d12022-02-04 13:18:13 -0800657
Daniel Leung134d26b2022-02-04 13:18:13 -0800658 unsigned int u;
659 unsigned long lu;
660 unsigned long long llu;
Daniel Leungba488d12022-02-04 13:18:13 -0800661
Daniel Leung134d26b2022-02-04 13:18:13 -0800662 double d;
Daniel Leungba488d12022-02-04 13:18:13 -0800663
Daniel Leung134d26b2022-02-04 13:18:13 -0800664 void *p;
665 } val;
Daniel Leungba488d12022-02-04 13:18:13 -0800666
Daniel Leung134d26b2022-02-04 13:18:13 -0800667 const char *s;
668 size_t arg_sz;
Daniel Leungba488d12022-02-04 13:18:13 -0800669
Daniel Leung134d26b2022-02-04 13:18:13 -0800670 uint8_t *argp = payload_buf;
671 const uint8_t * const argEob = payload_buf + sizeof(payload_buf);
Daniel Leungba488d12022-02-04 13:18:13 -0800672
Daniel Leung134d26b2022-02-04 13:18:13 -0800673 size_t payload_sz;
Daniel Leungba488d12022-02-04 13:18:13 -0800674
Daniel Leung134d26b2022-02-04 13:18:13 -0800675 key = k_spin_lock(&payload_lock);
Daniel Leungba488d12022-02-04 13:18:13 -0800676
Daniel Leung134d26b2022-02-04 13:18:13 -0800677 for (int arg_tag = va_arg(ap, int);
678 arg_tag != CBPRINTF_PACKAGE_ARG_TYPE_END;
679 arg_tag = va_arg(ap, int)) {
Daniel Leungba488d12022-02-04 13:18:13 -0800680
Daniel Leung134d26b2022-02-04 13:18:13 -0800681 switch (arg_tag) {
682 case CBPRINTF_PACKAGE_ARG_TYPE_CHAR:
683 __fallthrough;
684 case CBPRINTF_PACKAGE_ARG_TYPE_UNSIGNED_CHAR:
685 __fallthrough;
686 case CBPRINTF_PACKAGE_ARG_TYPE_SHORT:
687 __fallthrough;
688 case CBPRINTF_PACKAGE_ARG_TYPE_UNSIGNED_SHORT:
689 __fallthrough;
690 case CBPRINTF_PACKAGE_ARG_TYPE_INT:
691 __fallthrough;
692 case CBPRINTF_PACKAGE_ARG_TYPE_UNSIGNED_INT:
693 val.u = (unsigned int)va_arg(ap, unsigned int);
694 arg_sz = sizeof(unsigned int);
695 break;
Daniel Leungba488d12022-02-04 13:18:13 -0800696
Daniel Leung134d26b2022-02-04 13:18:13 -0800697 case CBPRINTF_PACKAGE_ARG_TYPE_LONG:
698 __fallthrough;
699 case CBPRINTF_PACKAGE_ARG_TYPE_UNSIGNED_LONG:
700 val.lu = (unsigned long)va_arg(ap, unsigned long);
701 arg_sz = sizeof(unsigned long);
702 break;
Daniel Leungba488d12022-02-04 13:18:13 -0800703
Daniel Leung134d26b2022-02-04 13:18:13 -0800704 case CBPRINTF_PACKAGE_ARG_TYPE_LONG_LONG:
705 __fallthrough;
706 case CBPRINTF_PACKAGE_ARG_TYPE_UNSIGNED_LONG_LONG:
707 val.llu = (unsigned long long)va_arg(ap, unsigned long long);
708 arg_sz = sizeof(unsigned long long);
709 break;
Daniel Leungba488d12022-02-04 13:18:13 -0800710
Daniel Leung134d26b2022-02-04 13:18:13 -0800711 case CBPRINTF_PACKAGE_ARG_TYPE_FLOAT:
712 __fallthrough;
713 case CBPRINTF_PACKAGE_ARG_TYPE_DOUBLE:
714 val.d = (double)va_arg(ap, double);
715 arg_sz = sizeof(double);
716 break;
Daniel Leungba488d12022-02-04 13:18:13 -0800717
Daniel Leung134d26b2022-02-04 13:18:13 -0800718 case CBPRINTF_PACKAGE_ARG_TYPE_LONG_DOUBLE:
719 /* Handle long double as double */
720 val.d = (double)va_arg(ap, long double);
721 arg_sz = sizeof(double);
722 break;
Daniel Leungba488d12022-02-04 13:18:13 -0800723
Daniel Leung134d26b2022-02-04 13:18:13 -0800724 case CBPRINTF_PACKAGE_ARG_TYPE_PTR_VOID:
725 val.p = (void *)va_arg(ap, void *);
726 arg_sz = sizeof(void *);
727 break;
Daniel Leungba488d12022-02-04 13:18:13 -0800728
Daniel Leung134d26b2022-02-04 13:18:13 -0800729 case CBPRINTF_PACKAGE_ARG_TYPE_PTR_CHAR:
730 s = va_arg(ap, char *);
731 while (argp < argEob) {
732 *argp++ = *s;
733 if (*s == 0) {
734 break;
735 }
Daniel Leungba488d12022-02-04 13:18:13 -0800736 s++;
737
Daniel Leung134d26b2022-02-04 13:18:13 -0800738 if (argp == argEob) {
739 goto no_space;
Daniel Leungba488d12022-02-04 13:18:13 -0800740 }
741 }
Daniel Leung134d26b2022-02-04 13:18:13 -0800742 continue;
Daniel Leungba488d12022-02-04 13:18:13 -0800743
Daniel Leung134d26b2022-02-04 13:18:13 -0800744 default:
745 k_spin_unlock(&payload_lock, key);
746 return -EINVAL;
Daniel Leungba488d12022-02-04 13:18:13 -0800747 }
Daniel Leung134d26b2022-02-04 13:18:13 -0800748
749 if (argp + arg_sz >= argEob) {
750 goto no_space;
751 }
752
753 if (arg_sz == sizeof(mipi_syst_u64)) {
754 *((mipi_syst_u64 *)argp) =
755 (mipi_syst_u64)MIPI_SYST_HTOLE64(val.v64);
756 } else {
757 *((mipi_syst_u32 *)argp) =
758 (mipi_syst_u32)MIPI_SYST_HTOLE32(val.v32);
759 }
760 argp += arg_sz;
761 }
762
763 /* Calculate how much buffer has been used */
764 payload_sz = argp - payload_buf;
Daniel Leungba488d12022-02-04 13:18:13 -0800765
766 MIPI_SYST_CATMSG_ARGS_COPY(&log_syst_handle, severity,
767 (uintptr_t)fmt,
768 payload_buf,
Daniel Leung134d26b2022-02-04 13:18:13 -0800769 payload_sz);
770
Daniel Leungba488d12022-02-04 13:18:13 -0800771 k_spin_unlock(&payload_lock, key);
Daniel Leung134d26b2022-02-04 13:18:13 -0800772
773 return 0;
774
775no_space:
776 k_spin_unlock(&payload_lock, key);
777 return -ENOSPC;
Daniel Leungba488d12022-02-04 13:18:13 -0800778}
Daniel Leung134d26b2022-02-04 13:18:13 -0800779#endif /* CONFIG_LOG_MIPI_SYST_USE_CATALOG */
Daniel Leung57982972022-02-01 12:46:55 -0800780
Krzysztof Chruscinski9833ca62022-06-21 11:51:38 +0200781void log_output_msg_syst_process(const struct log_output *output,
782 struct log_msg *msg, uint32_t flag)
Aastha Grovera89fae72021-10-28 16:07:54 -0700783{
Aastha Grovera89fae72021-10-28 16:07:54 -0700784 size_t len, hexdump_len;
785
786 update_systh_platform_data(&log_syst_handle, output, flag);
787
Daniel Leung6ec1fcc2022-04-07 16:00:22 -0700788#ifdef CONFIG_LOG_MIPI_SYST_OUTPUT_LOG_MSG_SRC_ID
Krzysztof Chruscinski9833ca62022-06-21 11:51:38 +0200789 uint8_t level = log_msg_get_level(msg);
Daniel Leung6ec1fcc2022-04-07 16:00:22 -0700790 bool raw_string = (level == LOG_LEVEL_INTERNAL_RAW_STRING);
791 int16_t source_id = CONFIG_LOG_MIPI_SYST_MSG_DEFAULT_MODULE_ID;
792
793 /* Set the log source ID as Sys-T message module ID */
794 if (!raw_string) {
Krzysztof Chruscinski9833ca62022-06-21 11:51:38 +0200795 void *source = (void *)log_msg_get_source(msg);
Daniel Leung6ec1fcc2022-04-07 16:00:22 -0700796
797 if (source != NULL) {
798 source_id = IS_ENABLED(CONFIG_LOG_RUNTIME_FILTERING) ?
799 log_dynamic_source_id(source) :
800 log_const_source_id(source);
801 }
802 }
803
804 update_handle_origin_unit(&log_syst_handle, source_id);
805#endif
806
Krzysztof Chruscinski9833ca62022-06-21 11:51:38 +0200807 uint8_t *data = log_msg_get_package(msg, &len);
Aastha Grovera89fae72021-10-28 16:07:54 -0700808
Daniel Leung4b6eb552022-01-11 15:38:52 -0800809 if (len) {
Daniel Leungba488d12022-02-04 13:18:13 -0800810#ifdef CONFIG_LOG_MIPI_SYST_USE_CATALOG
Daniel Leung134d26b2022-02-04 13:18:13 -0800811 struct cbprintf_package_hdr_ext *pkg_hdr = (void *)data;
812 bool is_cat_msg = false, skip = false;
813
814 if (is_in_log_strings_section(pkg_hdr->fmt)) {
815 if ((pkg_hdr->hdr.desc.pkg_flags & CBPRINTF_PACKAGE_ARGS_ARE_TAGGED) ==
816 CBPRINTF_PACKAGE_ARGS_ARE_TAGGED) {
817 /*
818 * Only if the package has tagged argument and
819 * the format string is in the log strings section,
820 * then we treat it as catalog message, because:
821 *
822 * 1. mipi_catalog_formatter() can only deal with
823 * tagged arguments; and,
824 * 2. the collateral XML file only contains strings
825 * in the log strings section.
826 */
827 is_cat_msg = true;
828 } else {
829 /*
830 * The format string is in log strings section
831 * but the package does not have tagged argument.
832 * This cannot be processed as a catalog message,
833 * and also means we cannot print the message as
834 * it is highly likely that the log strings section
835 * has been stripped from binary and cannot be
836 * accessed.
837 */
838 skip = true;
839 }
840 }
841
842 if (is_cat_msg) {
843 (void)cbpprintf_external(NULL,
844 mipi_catalog_formatter,
845 msg, data);
846 } else if (!skip)
Daniel Leungba488d12022-02-04 13:18:13 -0800847#endif
Daniel Leung134d26b2022-02-04 13:18:13 -0800848 {
849#ifdef CONFIG_CBPRINTF_PACKAGE_HEADER_STORE_CREATION_FLAGS
850 struct cbprintf_package_desc *pkg_hdr = (void *)data;
851
852 CHECKIF((pkg_hdr->pkg_flags & CBPRINTF_PACKAGE_ARGS_ARE_TAGGED) ==
853 CBPRINTF_PACKAGE_ARGS_ARE_TAGGED) {
854 /*
855 * Tagged arguments are to be used with catalog messages,
856 * and should not be used for non-tagged ones.
857 */
858 return;
859 }
860#endif
861
862
863 (void)cbpprintf_external(NULL,
864 mipi_vprintf_formatter,
865 msg, data);
866 }
Aastha Grovera89fae72021-10-28 16:07:54 -0700867 }
868
Krzysztof Chruscinski9833ca62022-06-21 11:51:38 +0200869 data = log_msg_get_data(msg, &hexdump_len);
Aastha Grovera89fae72021-10-28 16:07:54 -0700870 if (hexdump_len) {
Krzysztof Chruscinski9833ca62022-06-21 11:51:38 +0200871 uint32_t severity = level_to_syst_severity(log_msg_get_level(msg));
Daniel Leung57982972022-02-01 12:46:55 -0800872
Daniel Leung4b6eb552022-01-11 15:38:52 -0800873 hexdump2_print(data, hexdump_len, severity);
Aastha Grovera89fae72021-10-28 16:07:54 -0700874 }
875}
Aastha Grovera89fae72021-10-28 16:07:54 -0700876
Gerard Marull-Paretasa5fd0d12022-10-19 09:33:44 +0200877static int syst_init(void)
Wentong Wuc3f38db2020-05-06 20:54:13 +0800878{
Wentong Wuc3f38db2020-05-06 20:54:13 +0800879
880 MIPI_SYST_INIT_STATE(&log_syst_header,
881 mipi_syst_platform_init, (void *)0);
882
883 MIPI_SYST_INIT_HANDLE_STATE(&log_syst_header,
884 &log_syst_handle, NULL);
885
Daniel Leung6ec1fcc2022-04-07 16:00:22 -0700886 log_syst_handle.systh_tag.et_guid = 0;
887
888#ifndef CONFIG_LOG_MIPI_SYST_OUTPUT_LOG_MSG_SRC_ID
889 /* Set the default here once as it won't be modified anymore. */
890 log_syst_handle.systh_tag.et_modunit =
891 _MIPI_SYST_MK_MODUNIT_ORIGIN(
892 CONFIG_LOG_MIPI_SYST_MSG_DEFAULT_MODULE_ID,
893 CONFIG_LOG_MIPI_SYST_MSG_DEFAULT_UNIT_ID
894 );
895#endif
896
Wentong Wuc3f38db2020-05-06 20:54:13 +0800897 return 0;
898}
899
900SYS_INIT(syst_init, POST_KERNEL, 0);