1 /*
2 * Copyright (c) 2024 Nordic Semiconductor ASA
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/logging/log_frontend_stmesp_demux.h>
8 #include <zephyr/logging/log_ctrl.h>
9 #include <zephyr/sys/mpsc_pbuf.h>
10 #include <zephyr/sys/__assert.h>
11 #include <zephyr/logging/log_msg.h>
12 #include <zephyr/logging/log.h>
13 LOG_MODULE_REGISTER(stmesp_demux);
14
15 BUILD_ASSERT(sizeof(struct log_frontend_stmesp_demux_log_header) == sizeof(uint32_t),
16 "Must fit in a word");
17
18 #ifndef CONFIG_LOG_FRONTEND_STPESP_TURBO_SOURCE_PORT_ID
19 #define CONFIG_LOG_FRONTEND_STPESP_TURBO_SOURCE_PORT_ID 0
20 #endif
21
22 #ifndef CONFIG_LOG_FRONTEND_STMESP_TURBO_LOG_BASE
23 #define CONFIG_LOG_FRONTEND_STMESP_TURBO_LOG_BASE 0x8000
24 #endif
25
26 #define NUM_OF_ACTIVE CONFIG_LOG_FRONTEND_STMESP_DEMUX_ACTIVE_PACKETS
27 #define M_ID_OFF 16
28 #define M_ID_MASK (BIT_MASK(16) << M_ID_OFF)
29 #define C_ID_MASK BIT_MASK(16)
30
31 #define M_CH_HW_EVENT 0x00800000
32 #define M_CH_INVALID 0xFFFFFFFF
33
34 #define APP_M_ID 0x22
35 #define FLPR_M_ID 0x2D
36 #define PPR_M_ID 0x2E
37
38 struct log_frontend_stmesp_demux_active_entry {
39 sys_snode_t node;
40 uint32_t m_ch;
41 uint32_t ts;
42 struct log_frontend_stmesp_demux_log *packet;
43 int off;
44 };
45
46 struct log_frontend_stmesp_coop_sources {
47 uint32_t m_id;
48 const struct log_source_const_data *log_const;
49 };
50
51 struct log_frontend_stmesp_demux {
52 /* Pool for active entries. */
53 struct k_mem_slab mslab;
54
55 /* List of currently active entries. */
56 sys_slist_t active_entries;
57
58 /* The most recently used entry. */
59 struct log_frontend_stmesp_demux_active_entry *curr;
60
61 struct mpsc_pbuf_buffer pbuf;
62
63 uint32_t curr_m_ch;
64
65 const uint16_t *m_ids;
66
67 uint32_t *source_ids;
68
69 uint16_t m_ids_cnt;
70
71 uint16_t source_id_len;
72
73 uint32_t dropped;
74
75 struct log_frontend_stmesp_coop_sources coop_sources[2];
76 };
77
78 struct log_frontend_stmesp_entry_source_pair {
79 uint16_t entry_id;
80 uint16_t source_id;
81 };
82
83 static uint32_t buffer[CONFIG_LOG_FRONTEND_STMESP_DEMUX_BUFFER_SIZE]
84 __aligned(Z_LOG_MSG_ALIGNMENT);
85
86 static struct log_frontend_stmesp_demux demux;
87 static uint32_t slab_buf[NUM_OF_ACTIVE * sizeof(struct log_frontend_stmesp_demux_active_entry)];
88 static bool skip;
89
notify_drop(const struct mpsc_pbuf_buffer * buffer,const union mpsc_pbuf_generic * packet)90 static void notify_drop(const struct mpsc_pbuf_buffer *buffer,
91 const union mpsc_pbuf_generic *packet)
92 {
93 demux.dropped++;
94 }
95
store_source_id(uint16_t m_id,uint16_t entry_id,uint16_t source_id)96 static void store_source_id(uint16_t m_id, uint16_t entry_id, uint16_t source_id)
97 {
98 uint32_t *source_ids_data = &demux.source_ids[m_id * (demux.source_id_len + 1)];
99 uint32_t *wr_idx = source_ids_data;
100 struct log_frontend_stmesp_entry_source_pair *source_ids =
101 (struct log_frontend_stmesp_entry_source_pair *)&source_ids_data[1];
102
103 source_ids[*wr_idx].entry_id = entry_id;
104 source_ids[*wr_idx].source_id = source_id;
105 *wr_idx = *wr_idx + 1;
106 if (*wr_idx == (demux.source_id_len)) {
107 *wr_idx = 0;
108 }
109 }
110
get_source_id(uint16_t m_id,uint16_t entry_id)111 static uint16_t get_source_id(uint16_t m_id, uint16_t entry_id)
112 {
113 uint32_t *source_ids_data = &demux.source_ids[m_id * (demux.source_id_len + 1)];
114 int32_t rd_idx = source_ids_data[0];
115 uint32_t cnt = demux.source_id_len;
116 struct log_frontend_stmesp_entry_source_pair *source_ids =
117 (struct log_frontend_stmesp_entry_source_pair *)&source_ids_data[1];
118
119 do {
120 rd_idx = (rd_idx == 0) ? (demux.source_id_len - 1) : (rd_idx - 1);
121 if (source_ids[rd_idx].entry_id == entry_id) {
122 return source_ids[rd_idx].source_id;
123 }
124 cnt--;
125 } while (cnt);
126
127 return 0;
128 }
129
calc_wlen(uint32_t total_len)130 static uint32_t calc_wlen(uint32_t total_len)
131 {
132 return DIV_ROUND_UP(ROUND_UP(total_len, Z_LOG_MSG_ALIGNMENT), sizeof(uint32_t));
133 }
134
get_wlen(const union mpsc_pbuf_generic * packet)135 static uint32_t get_wlen(const union mpsc_pbuf_generic *packet)
136 {
137 union log_frontend_stmesp_demux_packet p = {.rgeneric = packet};
138 uint32_t len;
139
140 switch (p.generic_packet->type) {
141 case LOG_FRONTEND_STMESP_DEMUX_TYPE_TRACE_POINT:
142 len = sizeof(struct log_frontend_stmesp_demux_trace_point) / sizeof(uint32_t);
143 break;
144 case LOG_FRONTEND_STMESP_DEMUX_TYPE_HW_EVENT:
145 len = sizeof(struct log_frontend_stmesp_demux_hw_event) / sizeof(uint32_t);
146 break;
147 default:
148 len = calc_wlen(p.log->hdr.total_len +
149 offsetof(struct log_frontend_stmesp_demux_log, data));
150 break;
151 }
152
153 return len;
154 }
155
log_frontend_stmesp_demux_init(const struct log_frontend_stmesp_demux_config * config)156 int log_frontend_stmesp_demux_init(const struct log_frontend_stmesp_demux_config *config)
157 {
158 int err;
159 static const struct mpsc_pbuf_buffer_config pbuf_config = {
160 .buf = buffer,
161 .size = ARRAY_SIZE(buffer),
162 .notify_drop = notify_drop,
163 .get_wlen = get_wlen,
164 .flags = MPSC_PBUF_MODE_OVERWRITE |
165 (IS_ENABLED(CONFIG_LOG_FRONTEND_STMESP_DEMUX_MAX_UTILIZATION) ?
166 MPSC_PBUF_MAX_UTILIZATION : 0)};
167
168 memset(buffer, 0, sizeof(buffer));
169 mpsc_pbuf_init(&demux.pbuf, &pbuf_config);
170
171 sys_slist_init(&demux.active_entries);
172
173 if (config->m_ids_cnt > BIT(3)) {
174 return -EINVAL;
175 }
176
177 demux.m_ids = config->m_ids;
178 demux.m_ids_cnt = config->m_ids_cnt;
179 demux.dropped = 0;
180 demux.curr_m_ch = M_CH_INVALID;
181 demux.curr = NULL;
182 demux.source_ids = config->source_id_buf;
183 demux.source_id_len = config->source_id_buf_len / config->m_ids_cnt - 1;
184
185 err = k_mem_slab_init(&demux.mslab, slab_buf,
186 sizeof(struct log_frontend_stmesp_demux_active_entry),
187 NUM_OF_ACTIVE);
188
189 return err;
190 }
191
log_frontend_stmesp_demux_major(uint16_t id)192 void log_frontend_stmesp_demux_major(uint16_t id)
193 {
194 for (int i = 0; i < demux.m_ids_cnt; i++) {
195 if (id == demux.m_ids[i]) {
196 sys_snode_t *node;
197
198 demux.curr_m_ch = id << M_ID_OFF;
199 demux.curr = NULL;
200
201 SYS_SLIST_FOR_EACH_NODE(&demux.active_entries, node) {
202 struct log_frontend_stmesp_demux_active_entry *entry =
203 CONTAINER_OF(node,
204 struct log_frontend_stmesp_demux_active_entry,
205 node);
206
207 if (entry->m_ch == demux.curr_m_ch) {
208 demux.curr = entry;
209 break;
210 }
211 }
212 skip = false;
213 return;
214 }
215 }
216
217 skip = true;
218 }
219
log_frontend_stmesp_demux_channel(uint16_t id)220 void log_frontend_stmesp_demux_channel(uint16_t id)
221 {
222 if (skip) {
223 return;
224 }
225
226 if (id == CONFIG_LOG_FRONTEND_STMESP_FLUSH_PORT_ID) {
227 /* Flushing data that shall be discarded. */
228 goto bail;
229 }
230
231 demux.curr_m_ch &= ~C_ID_MASK;
232 demux.curr_m_ch |= id;
233
234 sys_snode_t *node;
235
236 SYS_SLIST_FOR_EACH_NODE(&demux.active_entries, node) {
237 struct log_frontend_stmesp_demux_active_entry *entry =
238 CONTAINER_OF(node, struct log_frontend_stmesp_demux_active_entry, node);
239
240 if (entry->m_ch == demux.curr_m_ch) {
241 demux.curr = entry;
242 return;
243 }
244 }
245
246 bail:
247 demux.curr = NULL;
248 }
249
get_major_id(uint16_t m_id)250 static uint8_t get_major_id(uint16_t m_id)
251 {
252 for (int i = 0; i < demux.m_ids_cnt; i++) {
253 if (m_id == demux.m_ids[i]) {
254 return i;
255 }
256 }
257
258 __ASSERT_NO_MSG(0);
259 return 0;
260 }
261
store_turbo_log0(uint16_t m_id,uint16_t id,uint64_t * ts,uint16_t source_id)262 static void store_turbo_log0(uint16_t m_id, uint16_t id, uint64_t *ts, uint16_t source_id)
263 {
264 struct log_frontend_stmesp_demux_trace_point packet = {
265 .valid = 1,
266 .type = LOG_FRONTEND_STMESP_DEMUX_TYPE_TRACE_POINT,
267 .content_invalid = 0,
268 .has_data = 0,
269 .timestamp = ts ? *ts : 0,
270 .major = m_id,
271 .source_id = source_id,
272 .id = id,
273 .data = 0};
274 static const size_t wlen = sizeof(packet) / sizeof(uint32_t);
275
276 mpsc_pbuf_put_data(&demux.pbuf, (const uint32_t *)&packet, wlen);
277 }
278
store_turbo_log1(uint16_t m_id,uint16_t id,uint64_t * ts,uint32_t data)279 static void store_turbo_log1(uint16_t m_id, uint16_t id, uint64_t *ts, uint32_t data)
280 {
281 struct log_frontend_stmesp_demux_trace_point packet = {
282 .valid = 1,
283 .type = LOG_FRONTEND_STMESP_DEMUX_TYPE_TRACE_POINT,
284 .content_invalid = 0,
285 .has_data = 0,
286 .timestamp = ts ? *ts : 0,
287 .major = m_id,
288 .source_id = get_source_id(m_id, id),
289 .id = id,
290 .data = data};
291 static const size_t wlen = sizeof(packet) / sizeof(uint32_t);
292
293 mpsc_pbuf_put_data(&demux.pbuf, (const uint32_t *)&packet, wlen);
294 }
295
store_tracepoint(uint16_t m_id,uint16_t id,uint64_t * ts,uint32_t * data)296 static void store_tracepoint(uint16_t m_id, uint16_t id, uint64_t *ts, uint32_t *data)
297 {
298 struct log_frontend_stmesp_demux_trace_point packet = {.valid = 1,
299 .type = LOG_FRONTEND_STMESP_DEMUX_TYPE_TRACE_POINT,
300 .content_invalid = 0,
301 .has_data = data ? 1 : 0,
302 .timestamp = ts ? *ts : 0,
303 .major = m_id,
304 .id = id,
305 .data = data ? *data : 0};
306 static const size_t wlen = sizeof(packet) / sizeof(uint32_t);
307
308 mpsc_pbuf_put_data(&demux.pbuf, (const uint32_t *)&packet, wlen);
309 }
310
log_frontend_stmesp_demux_hw_event(uint64_t * ts,uint8_t data)311 static void log_frontend_stmesp_demux_hw_event(uint64_t *ts, uint8_t data)
312 {
313 struct log_frontend_stmesp_demux_hw_event packet = {.valid = 1,
314 .type = LOG_FRONTEND_STMESP_DEMUX_TYPE_HW_EVENT,
315 .content_invalid = 0,
316 .timestamp = ts ? *ts : 0,
317 .evt = data};
318 static const size_t wlen = sizeof(packet) / sizeof(uint32_t);
319
320 mpsc_pbuf_put_data(&demux.pbuf, (const uint32_t *)&packet, wlen);
321 }
322
323 /* Check if there are any active messages which are not completed for a significant
324 * amount of time. It may indicate that part of message was lost (due to reset,
325 * fault in the core or fault on the bus). In that case message shall be closed as
326 * incomplete to not block processing of other messages.
327 */
garbage_collector(uint32_t now)328 static void garbage_collector(uint32_t now)
329 {
330 sys_snode_t *node;
331
332 SYS_SLIST_FOR_EACH_NODE(&demux.active_entries, node) {
333 struct log_frontend_stmesp_demux_active_entry *entry =
334 CONTAINER_OF(node, struct log_frontend_stmesp_demux_active_entry, node);
335
336 if ((now - entry->ts) > CONFIG_LOG_FRONTEND_STMESP_DEMUX_GC_TIMEOUT) {
337 union log_frontend_stmesp_demux_packet p = {.log = entry->packet};
338
339 sys_slist_find_and_remove(&demux.active_entries, node);
340 entry->packet->content_invalid = 1;
341 mpsc_pbuf_commit(&demux.pbuf, p.generic);
342 demux.dropped++;
343 k_mem_slab_free(&demux.mslab, entry);
344 /* After removing one we need to stop as removing disrupts
345 * iterating over the list as current node is no longer in
346 * the list.
347 */
348 break;
349 }
350 }
351 }
352
log_frontend_stmesp_demux_log0(uint16_t source_id,uint64_t * ts)353 int log_frontend_stmesp_demux_log0(uint16_t source_id, uint64_t *ts)
354 {
355 if (skip) {
356 return 0;
357 }
358
359 if (demux.curr_m_ch == M_CH_INVALID) {
360 return -EINVAL;
361 }
362
363 if (demux.curr != NULL) {
364 /* Previous package was incompleted. Finish it and potentially
365 * mark as incompleted if not all data is received.
366 */
367 log_frontend_stmesp_demux_packet_end();
368 return -EINVAL;
369 }
370
371 uint16_t ch = demux.curr_m_ch & C_ID_MASK;
372 uint16_t m = get_major_id(demux.curr_m_ch >> M_ID_OFF);
373
374 if (ch < CONFIG_LOG_FRONTEND_STMESP_TURBO_LOG_BASE) {
375 return -EINVAL;
376 }
377
378 store_turbo_log0(m, ch, ts, source_id);
379
380 return 1;
381 }
382
log_frontend_stmesp_demux_source_id(uint16_t data)383 void log_frontend_stmesp_demux_source_id(uint16_t data)
384 {
385 if (skip) {
386 return;
387 }
388
389 if (demux.curr_m_ch == M_CH_INVALID) {
390 return;
391 }
392
393 uint16_t ch = demux.curr_m_ch & C_ID_MASK;
394 uint16_t m = get_major_id(demux.curr_m_ch >> M_ID_OFF);
395
396 store_source_id(m, ch, data);
397 }
398
log_frontend_stmesp_demux_sname_get(uint32_t m_id,uint16_t s_id)399 const char *log_frontend_stmesp_demux_sname_get(uint32_t m_id, uint16_t s_id)
400 {
401 if (!IS_ENABLED(CONFIG_LOG_FRONTEND_STMESP_TURBO_LOG)) {
402 return "";
403 }
404
405 if (demux.m_ids[m_id] == APP_M_ID) {
406 return log_source_name_get(0, s_id);
407 } else if (m_id == demux.coop_sources[0].m_id) {
408 return demux.coop_sources[0].log_const[s_id].name;
409 } else if (m_id == demux.coop_sources[1].m_id) {
410 return demux.coop_sources[1].log_const[s_id].name;
411 }
412
413 return "unknown";
414 }
415
log_frontend_stmesp_demux_packet_start(uint32_t * data,uint64_t * ts)416 int log_frontend_stmesp_demux_packet_start(uint32_t *data, uint64_t *ts)
417 {
418 if (skip) {
419 return 0;
420 }
421
422 struct log_frontend_stmesp_demux_active_entry *entry;
423 union log_frontend_stmesp_demux_packet p;
424 int err;
425
426 if (demux.curr_m_ch == M_CH_INVALID) {
427 return -EINVAL;
428 }
429
430 if (demux.curr_m_ch == M_CH_HW_EVENT) {
431 /* HW event */
432 log_frontend_stmesp_demux_hw_event(ts, (uint8_t)*data);
433
434 return 1;
435 }
436
437 uint16_t ch = demux.curr_m_ch & C_ID_MASK;
438 uint16_t m = get_major_id(demux.curr_m_ch >> M_ID_OFF);
439
440 if (IS_ENABLED(CONFIG_LOG_FRONTEND_STMESP_TURBO_LOG) &&
441 (ch == CONFIG_LOG_FRONTEND_STPESP_TURBO_SOURCE_PORT_ID)) {
442 if (demux.m_ids[m] == FLPR_M_ID) {
443 demux.coop_sources[0].m_id = m;
444 demux.coop_sources[0].log_const =
445 (const struct log_source_const_data *)(uintptr_t)*data;
446 } else if (demux.m_ids[m] == PPR_M_ID) {
447 demux.coop_sources[1].m_id = m;
448 demux.coop_sources[1].log_const =
449 (const struct log_source_const_data *)(uintptr_t)*data;
450 }
451 return 0;
452 }
453
454 if (demux.curr != NULL) {
455 /* Previous package was incompleted. Finish it and potentially
456 * mark as incompleted if not all data is received.
457 */
458 log_frontend_stmesp_demux_packet_end();
459 return -EINVAL;
460 }
461
462 if (ch >= CONFIG_LOG_FRONTEND_STMESP_TP_CHAN_BASE) {
463 /* Trace point */
464 if (ch >= CONFIG_LOG_FRONTEND_STMESP_TURBO_LOG_BASE) {
465 store_turbo_log1(m, ch, ts, *data);
466 } else {
467 store_tracepoint(m, ch, ts, data);
468 }
469
470 return 1;
471 }
472
473 union log_frontend_stmesp_demux_header hdr = {.raw = *data};
474 uint32_t pkt_len = hdr.log.total_len + offsetof(struct log_frontend_stmesp_demux_log, data);
475 uint32_t wlen = calc_wlen(pkt_len);
476 uint32_t now = k_uptime_get_32();
477
478 garbage_collector(now);
479 err = k_mem_slab_alloc(&demux.mslab, (void **)&entry, K_NO_WAIT);
480 if (err < 0) {
481 goto on_nomem;
482 }
483
484 entry->m_ch = demux.curr_m_ch;
485 entry->off = 0;
486 p.generic = mpsc_pbuf_alloc(&demux.pbuf, wlen, K_NO_WAIT);
487 if (p.generic == NULL) {
488 k_mem_slab_free(&demux.mslab, entry);
489 goto on_nomem;
490 }
491
492 entry->packet = p.log;
493 entry->packet->type = LOG_FRONTEND_STMESP_DEMUX_TYPE_LOG;
494 entry->packet->content_invalid = 0;
495 if (ts) {
496 entry->packet->timestamp = *ts;
497 }
498 entry->packet->hdr = hdr.log;
499 entry->packet->hdr.major = m;
500 entry->ts = now;
501 demux.curr = entry;
502 sys_slist_append(&demux.active_entries, &entry->node);
503
504 return 0;
505
506 on_nomem:
507 demux.curr = NULL;
508 demux.dropped++;
509 return -ENOMEM;
510 }
511
log_frontend_stmesp_demux_timestamp(uint64_t ts)512 void log_frontend_stmesp_demux_timestamp(uint64_t ts)
513 {
514 if (demux.curr == NULL) {
515 return;
516 }
517
518 demux.curr->packet->timestamp = ts;
519 }
520
log_frontend_stmesp_demux_data(uint8_t * data,size_t len)521 void log_frontend_stmesp_demux_data(uint8_t *data, size_t len)
522 {
523 if (demux.curr == NULL) {
524 return;
525 }
526
527 if (demux.curr->off + len <= demux.curr->packet->hdr.total_len) {
528 memcpy(&demux.curr->packet->data[demux.curr->off], data, len);
529 demux.curr->off += len;
530 }
531 }
532
log_frontend_stmesp_demux_packet_end(void)533 void log_frontend_stmesp_demux_packet_end(void)
534 {
535 if (demux.curr == NULL) {
536 return;
537 }
538
539 union log_frontend_stmesp_demux_packet p = {.log = demux.curr->packet};
540
541 if (demux.curr->off != demux.curr->packet->hdr.total_len) {
542 demux.curr->packet->content_invalid = 1;
543 demux.dropped++;
544 }
545
546 mpsc_pbuf_commit(&demux.pbuf, p.generic);
547
548 sys_slist_find_and_remove(&demux.active_entries, &demux.curr->node);
549 k_mem_slab_free(&demux.mslab, demux.curr);
550 demux.curr = NULL;
551 }
552
log_frontend_stmesp_demux_get_dropped(void)553 uint32_t log_frontend_stmesp_demux_get_dropped(void)
554 {
555 uint32_t rv = demux.dropped;
556
557 demux.dropped = 0;
558
559 return rv;
560 }
561
log_frontend_stmesp_demux_claim(void)562 union log_frontend_stmesp_demux_packet log_frontend_stmesp_demux_claim(void)
563 {
564 union log_frontend_stmesp_demux_packet p;
565
566 /* Discard any invalid packets. */
567 while ((p.rgeneric = mpsc_pbuf_claim(&demux.pbuf)) != NULL) {
568 if (p.generic_packet->content_invalid) {
569 mpsc_pbuf_free(&demux.pbuf, p.rgeneric);
570 } else {
571 break;
572 }
573 }
574
575 return p;
576 }
577
log_frontend_stmesp_demux_free(union log_frontend_stmesp_demux_packet packet)578 void log_frontend_stmesp_demux_free(union log_frontend_stmesp_demux_packet packet)
579 {
580 mpsc_pbuf_free(&demux.pbuf, packet.rgeneric);
581 }
582
log_frontend_stmesp_demux_reset(void)583 void log_frontend_stmesp_demux_reset(void)
584 {
585 sys_snode_t *node;
586
587 while ((node = sys_slist_get(&demux.active_entries)) != NULL) {
588 struct log_frontend_stmesp_demux_active_entry *entry =
589 CONTAINER_OF(node, struct log_frontend_stmesp_demux_active_entry, node);
590 union log_frontend_stmesp_demux_packet p = {.log = entry->packet};
591
592 entry->packet->content_invalid = 1;
593 mpsc_pbuf_commit(&demux.pbuf, p.generic);
594 demux.dropped++;
595 demux.curr_m_ch = M_CH_INVALID;
596
597 k_mem_slab_free(&demux.mslab, entry);
598 }
599 }
600
log_frontend_stmesp_demux_is_idle(void)601 bool log_frontend_stmesp_demux_is_idle(void)
602 {
603 return sys_slist_is_empty(&demux.active_entries);
604 }
605
log_frontend_stmesp_demux_max_utilization(void)606 int log_frontend_stmesp_demux_max_utilization(void)
607 {
608 uint32_t max;
609 int rv = mpsc_pbuf_get_max_utilization(&demux.pbuf, &max);
610
611 return rv == 0 ? max : rv;
612 }
613
