1 /*
2  * Copyright (c) 2024 Nordic Semiconductor ASA.
3  *
4  * SPDX-License-Identifier: Apache-2.0
5  */
6 #include <zephyr/debug/coresight/cs_trace_defmt.h>
7 
8 static cs_trace_defmt_cb callback;
9 static uint8_t curr_id;
10 
cs_trace_defmt_init(cs_trace_defmt_cb cb)11 int cs_trace_defmt_init(cs_trace_defmt_cb cb)
12 {
13 	callback = cb;
14 	return 0;
15 }
16 
cs_trace_defmt_process(const uint8_t * data,size_t len)17 int cs_trace_defmt_process(const uint8_t *data, size_t len)
18 {
19 	uint8_t buf[15];
20 	size_t cnt = 0;
21 
22 	if (len != 16) {
23 		return -EINVAL;
24 	}
25 
26 	uint8_t aux = data[15];
27 	uint8_t d_id;
28 	uint8_t cb_id;
29 	bool do_cb = false;
30 
31 	for (int i = 0; i < 8; i++) {
32 		d_id = data[2 * i];
33 		if (d_id & 0x1) {
34 			if (cnt != 0) {
35 				cb_id = curr_id;
36 				if ((aux >> i) & 0x1) {
37 					/* next byte belongs to the old stream */
38 					do_cb = true;
39 				} else {
40 					callback(cb_id, buf, cnt);
41 					cnt = 0;
42 				}
43 			}
44 			curr_id = d_id >> 1;
45 		} else {
46 			buf[cnt++] = d_id | ((aux >> i) & 0x1);
47 		}
48 		if (i < 7) {
49 			buf[cnt++] = data[2 * i + 1];
50 			if (do_cb) {
51 				do_cb = false;
52 				callback(cb_id, buf, cnt);
53 				cnt = 0;
54 			}
55 		}
56 	}
57 
58 	if (cnt) {
59 		callback(curr_id, buf, cnt);
60 	}
61 
62 	return 0;
63 }
64