1 /*
2 * Copyright (c) 2019 Intel corporation
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/init.h>
8 #include <stdio.h>
9 #include <ctype.h>
10 #include <zephyr/kernel.h>
11 #include <mipi_syst.h>
12 #include <zephyr/spinlock.h>
13 #include <zephyr/sys/__assert.h>
14 #include <zephyr/sys/check.h>
15 #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>
19
20 static struct mipi_syst_header log_syst_header;
21 static struct mipi_syst_handle log_syst_handle;
22
23 #define HEXDUMP_BYTES_IN_LINE 16
24
25 #define STRING_BUF_MAX_LEN 128
26
27 #if defined(MIPI_SYST_PCFG_ENABLE_PLATFORM_STATE_DATA)
28 #if defined(CONFIG_MIPI_SYST_STP)
29 static mipi_syst_u16 master = 128;
30 static mipi_syst_u16 channel = 1;
31
32 static struct stp_writer_data writer_state;
33 #elif !defined(CONFIG_MIPI_SYST_RAW_DATA)
34 static const char pattern[] = "SYS-T RAW DATA: ";
35 static const char valToHex[] = "0123456789ABCDEF";
36 #endif
37
out_func(int c,void * ctx)38 static int out_func(int c, void *ctx)
39 {
40 const struct log_output *out_ctx = (const struct log_output *)ctx;
41
42 out_ctx->buf[out_ctx->control_block->offset] = (uint8_t)c;
43 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
54 #if defined(CONFIG_MIPI_SYST_STP)
stp_write_putNibble(struct mipi_syst_handle * systh,struct stp_writer_data * p,mipi_syst_u8 n)55 static 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
stp_write_flush(struct mipi_syst_handle * systh,struct stp_writer_data * p)69 static 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
stp_write_d4(struct mipi_syst_handle * systh,struct stp_writer_data * p,mipi_syst_u8 v)77 static 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
stp_write_payload8(struct mipi_syst_handle * systh,struct stp_writer_data * p,mipi_syst_u8 v)83 static 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
stp_write_payload16(struct mipi_syst_handle * systh,struct stp_writer_data * p,mipi_syst_u16 v)90 static 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
stp_write_payload32(struct mipi_syst_handle * systh,struct stp_writer_data * p,mipi_syst_u32 v)97 static 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
stp_write_payload64(struct mipi_syst_handle * systh,struct stp_writer_data * p,mipi_syst_u64 v)104 static 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
deltaTime(struct stp_writer_data * p)111 static 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
stp_write_d32mts(struct mipi_syst_handle * systh,struct stp_writer_data * p,mipi_syst_u32 v)119 static 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
stp_write_d64mts(struct mipi_syst_handle * systh,struct stp_writer_data * p,mipi_syst_u64 v)129 static 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
stp_write_d32ts(struct mipi_syst_handle * systh,struct stp_writer_data * p,mipi_syst_u32 v)139 static 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
stp_write_d8(struct mipi_syst_handle * systh,struct stp_writer_data * p,mipi_syst_u8 v)151 static 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
stp_write_d16(struct mipi_syst_handle * systh,struct stp_writer_data * p,mipi_syst_u16 v)158 static 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
stp_write_d32(struct mipi_syst_handle * systh,struct stp_writer_data * p,mipi_syst_u32 v)165 static 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)
stp_write_d64(struct mipi_syst_handle * systh,struct stp_writer_data * p,mipi_syst_u64 v)173 static 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
stp_write_flag(struct mipi_syst_handle * systh,struct stp_writer_data * p)181 static 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
stp_write_async(struct mipi_syst_handle * systh,struct stp_writer_data * p)188 static 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
stp_write_version(struct mipi_syst_handle * systh,struct stp_writer_data * p)198 static 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
stp_write_freq(struct mipi_syst_handle * systh,struct stp_writer_data * p)211 static 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
stp_write_setMC(struct mipi_syst_handle * systh,struct stp_writer_data * p,mipi_syst_u16 master,mipi_syst_u16 channel)220 static 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
write_raw(struct mipi_syst_handle * systh,const void * p,int n)249 static void write_raw(struct mipi_syst_handle *systh, const void *p, int n)
250 {
251 int i;
252 uint8_t c;
253
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
259 c = ((const uint8_t *)p)[i];
260 #if defined(CONFIG_MIPI_SYST_RAW_DATA)
261 out_func(c, systh->systh_platform.log_output);
262 #else
263 out_func(valToHex[c >> 0x4], systh->systh_platform.log_output);
264 out_func(valToHex[c & 0xF], systh->systh_platform.log_output);
265 #endif
266 }
267 }
268 #endif
269
270 static void write_d8(struct mipi_syst_handle *systh, uint8_t v)
271 {
272 #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
278 write_raw(systh, &v, sizeof(v));
279 #endif
280 }
281
282 static void write_d16(struct mipi_syst_handle *systh, uint16_t v)
283 {
284 #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
290 write_raw(systh, &v, sizeof(v));
291 #endif
292 }
293
294 static void write_d32(struct mipi_syst_handle *systh, uint32_t v)
295 {
296 #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
302 write_raw(systh, &v, sizeof(v));
303 #endif
304 }
305
306 #if defined(MIPI_SYST_PCFG_ENABLE_64BIT_IO)
307 static void write_d64(struct mipi_syst_handle *systh, uint64_t v)
308 {
309 #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
315 write_raw(systh, &v, sizeof(v));
316 #endif
317 }
318 #endif
319
320 static void write_d32ts(struct mipi_syst_handle *systh, uint32_t v)
321 {
322 #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
335 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));
340 #endif
341 }
342
343 static 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
359 static 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
373 }
374
375 static void write_flag(struct mipi_syst_handle *systh)
376 {
377 #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
386 uint32_t flag = systh->systh_platform.flag;
387
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 }
398 #endif
399 }
400 #endif
401
402 #if defined(MIPI_SYST_PCFG_ENABLE_TIMESTAMP)
403 mipi_syst_u64 mipi_syst_get_epoch(void)
404 {
405 return k_uptime_ticks();
406 }
407 #endif
408
409 static void update_systh_platform_data(struct mipi_syst_handle *handle,
410 const struct log_output *log_output,
411 uint32_t flag)
412 {
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
419 #if defined(CONFIG_LOG_MIPI_SYST_OUTPUT_LOG_MSG_SRC_ID)
420 /**
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 */
431 static 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
442 #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 */
448 static void platform_handle_init(struct mipi_syst_handle *systh)
449 {
450 #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
460 #if defined(MIPI_SYST_PCFG_LENGTH_FIELD)
461 MIPI_SYST_ENABLE_HANDLE_LENGTH(systh, 1);
462 #endif
463
464 #if defined(MIPI_SYST_PCFG_ENABLE_TIMESTAMP)
465 MIPI_SYST_ENABLE_HANDLE_TIMESTAMP(systh, 1);
466 #endif
467 }
468
469 static 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 */
482 static void mipi_syst_platform_init(struct mipi_syst_header *systh,
483 const void *platform_data)
484 {
485 #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
496 #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;
508 systh->systh_platform.write_d32mts = write_d32mts;
509 systh->systh_platform.write_d64mts = write_d64mts;
510 systh->systh_platform.write_flag = write_flag;
511 #endif
512 }
513
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 */
524 static uint32_t level_to_syst_severity(uint32_t level)
525 {
526 uint32_t ret;
527
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
552 static 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) {
585 unsigned char c = (unsigned char)data[i];
586
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
598 static void hexdump2_print(const uint8_t *data, uint32_t length,
599 uint32_t severity)
600 {
601 while (length != 0U) {
602 uint32_t part_len = MIN(length, HEXDUMP_BYTES_IN_LINE);
603
604 hexdump_line_print(data, part_len, severity);
605
606 data += part_len;
607 length -= part_len;
608 }
609 }
610
611 static int mipi_vprintf_formatter(cbprintf_cb out, void *ctx,
612 const char *fmt, va_list ap)
613 {
614 struct log_msg *msg = ctx;
615 uint32_t severity = level_to_syst_severity(log_msg_get_level(msg));
616
617 MIPI_SYST_VPRINTF(&log_syst_handle, severity, fmt, ap);
618
619 return 0;
620 }
621
622 #ifdef CONFIG_LOG_MIPI_SYST_USE_CATALOG
623
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
630 static inline bool is_in_log_strings_section(const void *addr)
631 {
632 TYPE_SECTION_START_EXTERN(const char *, log_strings);
633 TYPE_SECTION_END_EXTERN(const char *, log_strings);
634
635 if (((const char *)addr >= (const char *)TYPE_SECTION_START(log_strings)) &&
636 ((const char *)addr < (const char *)TYPE_SECTION_END(log_strings))) {
637 return true;
638 }
639
640 return false;
641 }
642
643
644 static struct k_spinlock payload_lock;
645 static uint8_t payload_buf[CONFIG_LOG_MIPI_SYST_CATALOG_ARGS_BUFFER_SIZE];
646
647 static int mipi_catalog_formatter(cbprintf_cb out, void *ctx,
648 const char *fmt, va_list ap)
649 {
650 struct log_msg *msg = ctx;
651 uint32_t severity = level_to_syst_severity(log_msg_get_level(msg));
652 k_spinlock_key_t key;
653
654 union {
655 mipi_syst_u64 v64;
656 mipi_syst_u32 v32;
657
658 unsigned int u;
659 unsigned long lu;
660 unsigned long long llu;
661
662 double d;
663
664 void *p;
665 } val;
666
667 const char *s;
668 size_t arg_sz;
669
670 uint8_t *argp = payload_buf;
671 const uint8_t * const argEob = payload_buf + sizeof(payload_buf);
672
673 size_t payload_sz;
674
675 key = k_spin_lock(&payload_lock);
676
677 for (int arg_tag = va_arg(ap, int);
678 arg_tag != CBPRINTF_PACKAGE_ARG_TYPE_END;
679 arg_tag = va_arg(ap, int)) {
680
681 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;
696
697 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;
703
704 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;
710
711 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;
717
718 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;
723
724 case CBPRINTF_PACKAGE_ARG_TYPE_PTR_VOID:
725 val.p = (void *)va_arg(ap, void *);
726 arg_sz = sizeof(void *);
727 break;
728
729 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 }
736 s++;
737
738 if (argp == argEob) {
739 goto no_space;
740 }
741 }
742 continue;
743
744 default:
745 k_spin_unlock(&payload_lock, key);
746 return -EINVAL;
747 }
748
749 if (argp + arg_sz >= argEob) {
750 goto no_space;
751 }
752
753 if (arg_sz == sizeof(mipi_syst_u64)) {
754 val.v64 = MIPI_SYST_HTOLE64(val.v64);
755 memcpy(argp, &val.v64, sizeof(val.v64));
756 } else {
757 val.v32 = MIPI_SYST_HTOLE32(val.v32);
758 memcpy(argp, &val.v32, sizeof(val.v32));
759 }
760 argp += arg_sz;
761 }
762
763 /* Calculate how much buffer has been used */
764 payload_sz = argp - payload_buf;
765
766 MIPI_SYST_CATMSG_ARGS_COPY(&log_syst_handle, severity,
767 (uintptr_t)fmt,
768 payload_buf,
769 payload_sz);
770
771 k_spin_unlock(&payload_lock, key);
772
773 return 0;
774
775 no_space:
776 k_spin_unlock(&payload_lock, key);
777 return -ENOSPC;
778 }
779 #endif /* CONFIG_LOG_MIPI_SYST_USE_CATALOG */
780
781 void log_output_msg_syst_process(const struct log_output *output,
782 struct log_msg *msg, uint32_t flag)
783 {
784 size_t len, hexdump_len;
785
786 update_systh_platform_data(&log_syst_handle, output, flag);
787
788 #ifdef CONFIG_LOG_MIPI_SYST_OUTPUT_LOG_MSG_SRC_ID
789 uint8_t level = log_msg_get_level(msg);
790 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) {
795 void *source = (void *)log_msg_get_source(msg);
796
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
807 uint8_t *data = log_msg_get_package(msg, &len);
808
809 if (len) {
810 #ifdef CONFIG_LOG_MIPI_SYST_USE_CATALOG
811 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)
847 #endif
848 {
849 #ifdef CONFIG_CBPRINTF_PACKAGE_HEADER_STORE_CREATION_FLAGS
850 struct cbprintf_package_desc *pkg_desc = (void *)data;
851
852 CHECKIF((pkg_desc->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 }
867 }
868
869 data = log_msg_get_data(msg, &hexdump_len);
870 if (hexdump_len) {
871 uint32_t severity = level_to_syst_severity(log_msg_get_level(msg));
872
873 hexdump2_print(data, hexdump_len, severity);
874 }
875 }
876
877 static int syst_init(void)
878 {
879
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
886 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
897 return 0;
898 }
899
900 SYS_INIT(syst_init, POST_KERNEL, 0);
901
