1 /*
2 * Copyright (c) 2024 Nordic Semiconductor ASA.
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6 #include <zephyr/debug/mipi_stp_decoder.h>
7 #include <string.h>
8
9 #if defined(CONFIG_CPU_CORTEX_M) && \
10 !defined(CONFIG_CPU_CORTEX_M0) && \
11 !defined(CONFIG_CPU_CORTEX_M0PLUS)
12 #define UNALIGNED_ACCESS_SUPPORTED 1
13 #else
14 #define UNALIGNED_ACCESS_SUPPORTED 0
15 #endif
16
17 enum stp_state {
18 STP_STATE_OP,
19 STP_STATE_DATA,
20 STP_STATE_TS,
21 STP_STATE_OUT_OF_SYNC,
22 };
23
24 enum stp_id {
25 STP_NULL,
26 STP_M8,
27 STP_MERR,
28 STP_C8,
29 STP_D8,
30 STP_D16,
31 STP_D32,
32 STP_D64,
33 STP_D8MTS,
34 STP_D16MTS,
35 STP_D32MTS,
36 STP_D64MTS,
37 STP_D4,
38 STP_D4MTS,
39 STP_FLAG_TS,
40 STP_VERSION,
41 STP_TAG_3NIBBLE_OP = STP_VERSION,
42 STP_NULL_TS,
43 STP_USER,
44 STP_USER_TS,
45 STP_TIME,
46 STP_TIME_TS,
47 STP_TRIG,
48 STP_TRIG_TS,
49 STP_FREQ,
50 STP_FREQ_TS,
51 STP_XSYNC,
52 STP_XSYNC_TS,
53 STP_FREQ_40,
54 STP_TAG_4NIBBLE_OP = STP_FREQ_40,
55 STP_FREQ_40_TS,
56 STP_DIP,
57 STP_M16,
58 STP_TAG_2NIBBLE_OP = STP_M16,
59 STP_GERR,
60 STP_C16,
61 STP_D8TS,
62 STP_D16TS,
63 STP_D32TS,
64 STP_D64TS,
65 STP_D8M,
66 STP_D16M,
67 STP_D32M,
68 STP_D64M,
69 STP_D4TS,
70 STP_D4M,
71 STP_FLAG,
72 STP_ASYNC,
73 STP_INVALID,
74 STP_OP_MAX
75 };
76
77 #define STP_LONG_OP_ID 0xF
78 #define STP_2B_OP_ID 0xF0
79
80 #define STP_VAR_DATA 0xff
81
82 typedef void (*stp_cb)(uint64_t data, uint64_t ts);
83
84 struct stp_item {
85 const char *name;
86 enum stp_id type;
87 uint8_t id[3];
88 uint8_t id_ncnt;
89 uint8_t d_ncnt;
90 bool has_ts;
91 stp_cb cb;
92 };
93
94 #define STP_ITEM(_type, _id, _id_ncnt, _d_ncnt, _has_ts, _cb) \
95 { \
96 .name = STRINGIFY(_type), .type = _type, .id = {__DEBRACKET _id}, \
97 .id_ncnt = _id_ncnt, .d_ncnt = _d_ncnt, \
98 .has_ts = _has_ts, .cb = (stp_cb)_cb \
99 }
100
101 static struct mipi_stp_decoder_config cfg;
102 static uint64_t prev_ts;
103 static uint64_t base_ts;
104 static enum stp_state state;
105 static size_t ntotal;
106 static size_t ncnt;
107 static size_t noff;
108 static uint16_t curr_ch;
109
data4_cb(uint64_t data,uint64_t ts)110 static void data4_cb(uint64_t data, uint64_t ts)
111 {
112 ARG_UNUSED(ts);
113 union mipi_stp_decoder_data d = {.data = data};
114
115 cfg.cb(STP_DATA4, d, NULL, false);
116 }
117
data8_cb(uint64_t data,uint64_t ts)118 static void data8_cb(uint64_t data, uint64_t ts)
119 {
120 ARG_UNUSED(ts);
121 union mipi_stp_decoder_data d = {.data = data};
122
123 cfg.cb(STP_DATA8, d, NULL, false);
124 }
125
data16_cb(uint64_t data,uint64_t ts)126 static void data16_cb(uint64_t data, uint64_t ts)
127 {
128 ARG_UNUSED(ts);
129 union mipi_stp_decoder_data d = {.data = data};
130
131 cfg.cb(STP_DATA16, d, NULL, false);
132 }
133
data32_cb(uint64_t data,uint64_t ts)134 static void data32_cb(uint64_t data, uint64_t ts)
135 {
136 ARG_UNUSED(ts);
137 union mipi_stp_decoder_data d = {.data = data};
138
139 cfg.cb(STP_DATA32, d, NULL, false);
140 }
141
data64_cb(uint64_t data,uint64_t ts)142 static void data64_cb(uint64_t data, uint64_t ts)
143 {
144 ARG_UNUSED(ts);
145 union mipi_stp_decoder_data d = {.data = data};
146
147 cfg.cb(STP_DATA64, d, NULL, false);
148 }
149
data4_m_cb(uint64_t data,uint64_t ts)150 static void data4_m_cb(uint64_t data, uint64_t ts)
151 {
152 ARG_UNUSED(ts);
153 union mipi_stp_decoder_data d = {.data = data};
154
155 cfg.cb(STP_DATA4, d, NULL, true);
156 }
157
data8_m_cb(uint64_t data,uint64_t ts)158 static void data8_m_cb(uint64_t data, uint64_t ts)
159 {
160 ARG_UNUSED(ts);
161 union mipi_stp_decoder_data d = {.data = data};
162
163 cfg.cb(STP_DATA8, d, NULL, true);
164 }
165
data16_m_cb(uint64_t data,uint64_t ts)166 static void data16_m_cb(uint64_t data, uint64_t ts)
167 {
168 ARG_UNUSED(ts);
169 union mipi_stp_decoder_data d = {.data = data};
170
171 cfg.cb(STP_DATA16, d, NULL, true);
172 }
173
data32_m_cb(uint64_t data,uint64_t ts)174 static void data32_m_cb(uint64_t data, uint64_t ts)
175 {
176 ARG_UNUSED(ts);
177 union mipi_stp_decoder_data d = {.data = data};
178
179 cfg.cb(STP_DATA32, d, NULL, true);
180 }
181
data64_m_cb(uint64_t data,uint64_t ts)182 static void data64_m_cb(uint64_t data, uint64_t ts)
183 {
184 ARG_UNUSED(ts);
185 union mipi_stp_decoder_data d = {.data = data};
186
187 cfg.cb(STP_DATA64, d, NULL, true);
188 }
189
data4_ts_cb(uint64_t data,uint64_t ts)190 static void data4_ts_cb(uint64_t data, uint64_t ts)
191 {
192 union mipi_stp_decoder_data d = {.data = data};
193
194 cfg.cb(STP_DATA4, d, &ts, false);
195 }
196
data8_ts_cb(uint64_t data,uint64_t ts)197 static void data8_ts_cb(uint64_t data, uint64_t ts)
198 {
199 union mipi_stp_decoder_data d = {.data = data};
200
201 cfg.cb(STP_DATA8, d, &ts, false);
202 }
203
data16_ts_cb(uint64_t data,uint64_t ts)204 static void data16_ts_cb(uint64_t data, uint64_t ts)
205 {
206 union mipi_stp_decoder_data d = {.data = data};
207
208 cfg.cb(STP_DATA16, d, &ts, false);
209 }
210
data32_ts_cb(uint64_t data,uint64_t ts)211 static void data32_ts_cb(uint64_t data, uint64_t ts)
212 {
213 union mipi_stp_decoder_data d = {.data = data};
214
215 cfg.cb(STP_DATA32, d, &ts, false);
216 }
217
data64_ts_cb(uint64_t data,uint64_t ts)218 static void data64_ts_cb(uint64_t data, uint64_t ts)
219 {
220 union mipi_stp_decoder_data d = {.data = data};
221
222 cfg.cb(STP_DATA64, d, &ts, false);
223 }
224
data4_mts_cb(uint64_t data,uint64_t ts)225 static void data4_mts_cb(uint64_t data, uint64_t ts)
226 {
227 union mipi_stp_decoder_data d = {.data = data};
228
229 cfg.cb(STP_DATA4, d, &ts, true);
230 }
231
data8_mts_cb(uint64_t data,uint64_t ts)232 static void data8_mts_cb(uint64_t data, uint64_t ts)
233 {
234 union mipi_stp_decoder_data d = {.data = data};
235
236 cfg.cb(STP_DATA8, d, &ts, true);
237 }
238
data16_mts_cb(uint64_t data,uint64_t ts)239 static void data16_mts_cb(uint64_t data, uint64_t ts)
240 {
241 union mipi_stp_decoder_data d = {.data = data};
242
243 cfg.cb(STP_DATA16, d, &ts, true);
244 }
245
data32_mts_cb(uint64_t data,uint64_t ts)246 static void data32_mts_cb(uint64_t data, uint64_t ts)
247 {
248 union mipi_stp_decoder_data d = {.data = data};
249
250 cfg.cb(STP_DATA32, d, &ts, true);
251 }
252
data64_mts_cb(uint64_t data,uint64_t ts)253 static void data64_mts_cb(uint64_t data, uint64_t ts)
254 {
255 union mipi_stp_decoder_data d = {.data = data};
256
257 cfg.cb(STP_DATA64, d, &ts, true);
258 }
259
master_cb(uint64_t id,uint64_t ts)260 static void master_cb(uint64_t id, uint64_t ts)
261 {
262 ARG_UNUSED(ts);
263 uint16_t m_id = (uint16_t)id;
264 union mipi_stp_decoder_data data = {.id = m_id};
265
266 curr_ch = 0;
267
268 cfg.cb(STP_DECODER_MASTER, data, NULL, false);
269 }
270
channel16_cb(uint64_t id,uint64_t ts)271 static void channel16_cb(uint64_t id, uint64_t ts)
272 {
273 uint16_t ch = (uint16_t)id;
274
275 curr_ch = 0xFF00 & ch;
276
277 ARG_UNUSED(ts);
278 union mipi_stp_decoder_data data = {.id = ch};
279
280 cfg.cb(STP_DECODER_CHANNEL, data, NULL, false);
281 }
channel_cb(uint64_t id,uint64_t ts)282 static void channel_cb(uint64_t id, uint64_t ts)
283 {
284 uint16_t ch = (uint16_t)id;
285
286 ch |= curr_ch;
287
288 ARG_UNUSED(ts);
289 union mipi_stp_decoder_data data = {.id = ch};
290
291 cfg.cb(STP_DECODER_CHANNEL, data, NULL, false);
292 }
293
merror_cb(uint64_t err,uint64_t ts)294 static void merror_cb(uint64_t err, uint64_t ts)
295 {
296 ARG_UNUSED(ts);
297 union mipi_stp_decoder_data data = {.err = (uint32_t)err};
298
299 cfg.cb(STP_DECODER_MERROR, data, NULL, false);
300 }
301
gerror_cb(uint64_t err,uint64_t ts)302 static void gerror_cb(uint64_t err, uint64_t ts)
303 {
304 ARG_UNUSED(ts);
305 union mipi_stp_decoder_data data = {.err = (uint32_t)err};
306
307 cfg.cb(STP_DECODER_GERROR, data, NULL, false);
308 }
309
flag_cb(uint64_t data,uint64_t ts)310 static void flag_cb(uint64_t data, uint64_t ts)
311 {
312 ARG_UNUSED(ts);
313 ARG_UNUSED(data);
314 union mipi_stp_decoder_data dummy = {.dummy = 0};
315
316 cfg.cb(STP_DECODER_FLAG, dummy, NULL, false);
317 }
318
flag_ts_cb(uint64_t unused,uint64_t ts)319 static void flag_ts_cb(uint64_t unused, uint64_t ts)
320 {
321 ARG_UNUSED(unused);
322 union mipi_stp_decoder_data data = {.dummy = 0};
323
324 cfg.cb(STP_DECODER_FLAG, data, &ts, false);
325 }
326
version_cb(uint64_t version,uint64_t ts)327 static void version_cb(uint64_t version, uint64_t ts)
328 {
329 ARG_UNUSED(ts);
330
331 curr_ch = 0;
332
333 union mipi_stp_decoder_data data = {.ver = version};
334
335 cfg.cb(STP_DECODER_VERSION, data, NULL, false);
336 }
337
notsup_cb(uint64_t data,uint64_t ts)338 static void notsup_cb(uint64_t data, uint64_t ts)
339 {
340 ARG_UNUSED(ts);
341 ARG_UNUSED(data);
342
343 union mipi_stp_decoder_data dummy = {.dummy = 0};
344
345 cfg.cb(STP_DECODER_NOT_SUPPORTED, dummy, NULL, false);
346 }
347
freq_cb(uint64_t freq,uint64_t ts)348 static void freq_cb(uint64_t freq, uint64_t ts)
349 {
350 ARG_UNUSED(ts);
351
352 union mipi_stp_decoder_data data = {.freq = freq};
353
354 cfg.cb(STP_DECODER_FREQ, data, NULL, false);
355 }
356
freq_ts_cb(uint64_t freq,uint64_t ts)357 static void freq_ts_cb(uint64_t freq, uint64_t ts)
358 {
359 union mipi_stp_decoder_data data = {.freq = freq};
360
361 cfg.cb(STP_DECODER_FREQ, data, &ts, false);
362 }
363
null_cb(uint64_t data,uint64_t ts)364 static void null_cb(uint64_t data, uint64_t ts)
365 {
366 ARG_UNUSED(ts);
367 ARG_UNUSED(data);
368
369 union mipi_stp_decoder_data dummy = {.dummy = 0};
370
371 cfg.cb(STP_DECODER_NULL, dummy, NULL, false);
372 }
373
async_cb(uint64_t data,uint64_t ts)374 static void async_cb(uint64_t data, uint64_t ts)
375 {
376 ARG_UNUSED(ts);
377 ARG_UNUSED(data);
378
379 union mipi_stp_decoder_data dummy = {.dummy = 0};
380
381 cfg.cb(STP_DECODER_ASYNC, dummy, NULL, false);
382 }
383
384 static const struct stp_item items[] = {
385 STP_ITEM(STP_NULL, (0x0), 1, 0, false, null_cb),
386 STP_ITEM(STP_M8, (0x1), 1, 2, false, master_cb),
387 STP_ITEM(STP_MERR, (0x2), 1, 2, false, merror_cb),
388 STP_ITEM(STP_C8, (0x3), 1, 2, false, channel_cb),
389 STP_ITEM(STP_D8, (0x4), 1, 2, false, data8_cb),
390 STP_ITEM(STP_D16, (0x5), 1, 4, false, data16_cb),
391 STP_ITEM(STP_D32, (0x6), 1, 8, false, data32_cb),
392 STP_ITEM(STP_D64, (0x7), 1, 16, false, data64_cb),
393 STP_ITEM(STP_D8MTS, (0x8), 1, 2, true, data8_mts_cb),
394 STP_ITEM(STP_D16MTS, (0x9), 1, 4, true, data16_mts_cb),
395 STP_ITEM(STP_D32MTS, (0xa), 1, 8, true, data32_mts_cb),
396 STP_ITEM(STP_D64MTS, (0xb), 1, 16, true, data64_mts_cb),
397 STP_ITEM(STP_D4, (0xc), 1, 1, false, data4_cb),
398 STP_ITEM(STP_D4MTS, (0xd), 1, 1, true, data4_mts_cb),
399 STP_ITEM(STP_FLAG_TS, (0xe), 1, 0, true, flag_ts_cb),
400 STP_ITEM(STP_VERSION, (0xf0, 0x00), 3, 1, false, version_cb),
401 STP_ITEM(STP_NULL_TS, (0xf0, 0x01), 3, 0, true, notsup_cb),
402 STP_ITEM(STP_USER, (0xf0, 0x02), 3, 0, false, notsup_cb),
403 STP_ITEM(STP_USER_TS, (0xf0, 0x03), 3, 0, true, notsup_cb),
404 STP_ITEM(STP_TIME, (0xf0, 0x04), 3, 0, false, notsup_cb),
405 STP_ITEM(STP_TIME_TS, (0xf0, 0x05), 3, 0, true, notsup_cb),
406 STP_ITEM(STP_TRIG, (0xf0, 0x06), 3, 0, false, notsup_cb),
407 STP_ITEM(STP_TRIG_TS, (0xf0, 0x07), 3, 0, true, notsup_cb),
408 STP_ITEM(STP_FREQ, (0xf0, 0x08), 3, 8, false, freq_cb),
409 STP_ITEM(STP_FREQ_TS, (0xf0, 0x09), 3, 8, true, freq_ts_cb),
410 STP_ITEM(STP_XSYNC, (0xf0, 0x0a), 3, 0, false, notsup_cb),
411 STP_ITEM(STP_XSYNC_TS, (0xf0, 0x0b), 3, 0, true, notsup_cb),
412 STP_ITEM(STP_FREQ_40, (0xf0, 0xf0), 4, 10, false, freq_cb),
413 STP_ITEM(STP_FREQ_40_TS, (0xf0, 0xf1), 4, 0, true, notsup_cb),
414 STP_ITEM(STP_DIP, (0xf0, 0xf2), 4, 0, false, notsup_cb),
415 STP_ITEM(STP_M16, (0xf1), 2, 4, false, master_cb),
416 STP_ITEM(STP_GERR, (0xf2), 2, 2, false, gerror_cb),
417 STP_ITEM(STP_C16, (0xf3), 2, 4, false, channel16_cb),
418 STP_ITEM(STP_D8TS, (0xf4), 2, 2, true, data8_ts_cb),
419 STP_ITEM(STP_D16TS, (0xf5), 2, 4, true, data16_ts_cb),
420 STP_ITEM(STP_D32TS, (0xf6), 2, 8, true, data32_ts_cb),
421 STP_ITEM(STP_D64TS, (0xf7), 2, 16, true, data64_ts_cb),
422 STP_ITEM(STP_D8M, (0xf8), 2, 2, false, data8_m_cb),
423 STP_ITEM(STP_D16M, (0xf9), 2, 4, false, data16_m_cb),
424 STP_ITEM(STP_D32M, (0xfa), 2, 8, false, data32_m_cb),
425 STP_ITEM(STP_D64M, (0xfb), 2, 16, false, data64_m_cb),
426 STP_ITEM(STP_D4TS, (0xfc), 2, 1, true, data4_ts_cb),
427 STP_ITEM(STP_D4M, (0xfd), 2, 1, false, data4_m_cb),
428 STP_ITEM(STP_FLAG, (0xfe), 2, 0, false, flag_cb),
429 STP_ITEM(STP_ASYNC, (0xff, 0xff, 0xff), 6, 16, false, async_cb),
430 STP_ITEM(STP_INVALID, (0x0), 0, 0, false, NULL),
431 };
432
433 /** @brief Decode a nibble and read opcode from the stream.
434 *
435 * Function reads a nibble and continues or starts decoding of a STP opcode.
436 *
437 * @param data Pointer to the stream.
438 * @param[in,out] noff Offset (in nibbles).
439 * @param nlen Length (in nibbles).
440 * @param[in,out] ncnt Current number of nibbles
441 * @param[in,out] ntotal Number of nibbles in the opcode.
442 *
443 * @retval STP_INVALID Opcode decoding is in progress.
444 * @retval opcode Decoded opcode.
445 */
get_nibble(const uint8_t * data,size_t noff)446 static inline uint8_t get_nibble(const uint8_t *data, size_t noff)
447 {
448 uint8_t ret = data[noff / 2];
449
450 if (noff & 0x1UL) {
451 ret >>= 4;
452 }
453
454 ret &= 0x0F;
455
456 return ret;
457 }
458
get_nibbles64(const uint8_t * src,size_t src_noff,uint8_t * dst,size_t dst_noff,size_t nlen)459 static inline void get_nibbles64(const uint8_t *src, size_t src_noff, uint8_t *dst, size_t dst_noff,
460 size_t nlen)
461 {
462 bool src_ba = (src_noff & 0x1UL) == 0;
463 bool dst_ba = (dst_noff & 0x1UL) == 0;
464 uint64_t *src64 = (uint64_t *)&src[src_noff / 2];
465 uint64_t *dst64 = (uint64_t *)&dst[dst_noff / 2];
466
467 if (nlen == 16) {
468 /* dst must be aligned. */
469 if (src_ba) {
470 uint32_t *s32 = (uint32_t *)src64;
471 uint32_t *d32 = (uint32_t *)dst64;
472
473 d32[0] = s32[0];
474 d32[1] = s32[1];
475 } else {
476 uint64_t part_a = src64[0] >> 4;
477 uint64_t part_b = src64[1] << 60;
478
479 dst64[0] = part_a | part_b;
480 }
481 return;
482 }
483
484 uint64_t mask = BIT64_MASK(nlen * 4) << (src_ba ? 0 : 4);
485 uint64_t src_d = src64[0] & mask;
486
487 if (((src_noff ^ dst_noff) & 0x1UL) == 0) {
488 dst64[0] |= src_d;
489 } else if (dst_ba) {
490 dst64[0] |= (src_d >> 4);
491 } else {
492 dst64[0] |= (src_d << 4);
493 }
494 }
495
496 /* Function performs getting nibbles in less efficient way but does not use unaligned
497 * access which may not be supported by some platforms.
498 */
get_nibbles_unaligned(const uint8_t * src,size_t src_noff,uint8_t * dst,size_t dst_noff,size_t nlen)499 static void get_nibbles_unaligned(const uint8_t *src, size_t src_noff, uint8_t *dst,
500 size_t dst_noff, size_t nlen)
501 {
502 for (size_t i = 0; i < nlen; i++) {
503 size_t idx = (src_noff + i) / 2;
504 size_t ni = (src_noff + i) & 0x1;
505 uint8_t n = src[idx] >> (ni ? 4 : 0);
506 size_t d_idx = (dst_noff + i) / 2;
507 size_t dni = (dst_noff + i) & 0x1;
508
509 if (dni == 0) {
510 dst[d_idx] = n;
511 } else {
512 dst[d_idx] |= n << 4;
513 }
514 }
515 }
516
get_nibbles(const uint8_t * src,size_t src_noff,uint8_t * dst,size_t dst_noff,size_t nlen)517 static inline void get_nibbles(const uint8_t *src, size_t src_noff, uint8_t *dst, size_t dst_noff,
518 size_t nlen)
519 {
520 if (!UNALIGNED_ACCESS_SUPPORTED) {
521 get_nibbles_unaligned(src, src_noff, dst, dst_noff, nlen);
522 return;
523 }
524
525 bool src_ba = (src_noff & 0x1UL) == 0;
526 bool dst_ba = (dst_noff & 0x1UL) == 0;
527 uint32_t *src32 = (uint32_t *)&src[src_noff / 2];
528 uint32_t *dst32 = (uint32_t *)&dst[dst_noff / 2];
529
530 if (nlen > 8) {
531 get_nibbles64(src, src_noff, dst, dst_noff, nlen);
532 return;
533 } else if (nlen == 8) {
534 /* dst must be aligned. */
535 if (src_ba) {
536 dst32[0] = src32[0];
537 } else {
538 uint32_t part_a = src32[0] >> 4;
539 uint32_t part_b = src32[1] << 28;
540
541 dst32[0] = part_a | part_b;
542 }
543 return;
544 }
545
546 uint32_t mask = BIT_MASK(nlen * 4) << (src_ba ? 0 : 4);
547 uint32_t src_d = src32[0] & mask;
548
549 if (((src_noff ^ dst_noff) & 0x1UL) == 0) {
550 dst32[0] |= src_d;
551 } else if (dst_ba) {
552 dst32[0] |= (src_d >> 4);
553 } else {
554 dst32[0] |= (src_d << 4);
555 }
556 }
557
558 /* Function swaps nibbles in a byte. */
swap8(uint8_t byte)559 static uint8_t swap8(uint8_t byte)
560 {
561 return (byte << 4) | (byte >> 4);
562 }
563
564 /* Function swaps nibbles in a 16 bit variable. */
swap16(uint16_t halfword)565 static uint16_t swap16(uint16_t halfword)
566 {
567 halfword = __builtin_bswap16(halfword);
568 uint16_t d1 = (halfword & 0xf0f0) >> 4;
569 uint16_t d2 = (halfword & 0x0f0f) << 4;
570
571 return d1 | d2;
572 }
573
574 /* Function swaps nibbles in a 32 bit word. */
swap32(uint32_t word)575 static uint32_t swap32(uint32_t word)
576 {
577 word = __builtin_bswap32(word);
578 uint32_t d1 = (word & 0xf0f0f0f0) >> 4;
579 uint32_t d2 = (word & 0x0f0f0f0f) << 4;
580
581 return d1 | d2;
582 }
583
584 /* Function swaps nibbles in a 64 bit word. */
swap64(uint64_t dword)585 static uint64_t swap64(uint64_t dword)
586 {
587 uint32_t l = (uint32_t)dword;
588 uint32_t u = (uint32_t)(dword >> 32);
589
590 return ((uint64_t)swap32(l) << 32) | (uint64_t)swap32(u);
591 }
592
swap_n(uint8_t * data,uint32_t n)593 static void swap_n(uint8_t *data, uint32_t n)
594 {
595 switch (n) {
596 case 2:
597 *data = swap8(*data);
598 break;
599 case 4:
600 *(uint16_t *)data = swap16(*(uint16_t *)data);
601 break;
602 case 8:
603 *(uint32_t *)data = swap32(*(uint32_t *)data);
604 break;
605 case 16:
606 *(uint64_t *)data = swap64(*(uint64_t *)data);
607 break;
608 default:
609 *(uint64_t *)data = swap64(*(uint64_t *)data);
610 *(uint64_t *)data >>= (4 * (16 - n));
611 break;
612 }
613 }
614
get_op(const uint8_t * data,size_t * noff,size_t * nlen,size_t * ncnt,size_t * ntotal)615 static enum stp_id get_op(const uint8_t *data, size_t *noff, size_t *nlen, size_t *ncnt,
616 size_t *ntotal)
617 {
618 uint8_t op = 0;
619
620 op = get_nibble(data, *noff);
621
622 *noff += 1;
623 *ncnt += 1;
624 if (*ntotal == 0 && *ncnt == 1) {
625 /* Starting to read op. */
626 /* op code has only 1 nibble. */
627 if (op != 0xF) {
628 return (enum stp_id)op;
629 }
630 } else if (*ncnt == 2) {
631 if (op == 0xF) {
632 /* ASYNC*/
633 *ntotal = 6;
634 } else if (op != 0) {
635 return (enum stp_id)(STP_TAG_2NIBBLE_OP - 1 + op);
636 }
637 } else if (*ncnt == 3) {
638 if (op != 0xf) {
639 return (enum stp_id)(STP_TAG_3NIBBLE_OP + op);
640 } else if (*ntotal == 0) {
641 *ntotal = 4;
642 }
643 } else if (*ncnt == *ntotal) {
644 if (*ntotal == 4) {
645 return (enum stp_id)(STP_TAG_4NIBBLE_OP + op);
646 } else {
647 return STP_ASYNC;
648 }
649 }
650
651 return STP_INVALID;
652 }
653
mipi_stp_decoder_sync_loss(void)654 void mipi_stp_decoder_sync_loss(void)
655 {
656 state = STP_STATE_OUT_OF_SYNC;
657 ncnt = 0;
658 ntotal = 0;
659 }
660
mipi_stp_decoder_decode(const uint8_t * data,size_t len)661 int mipi_stp_decoder_decode(const uint8_t *data, size_t len)
662 {
663 static enum stp_id curr_id = STP_INVALID;
664 static uint8_t data_buf[8] __aligned(sizeof(uint64_t));
665 static uint8_t ts_buf[8] __aligned(sizeof(uint64_t));
666 uint64_t *data64 = (uint64_t *)data_buf;
667 uint64_t *ts64 = (uint64_t *)ts_buf;
668 size_t nlen = 2 * len;
669
670 do {
671 switch (state) {
672 case STP_STATE_OUT_OF_SYNC: {
673 uint8_t b = get_nibble(data, noff);
674
675 noff++;
676 if (ncnt < 21 && b == 0xF) {
677 ncnt++;
678 } else if (ncnt == 21 && b == 0) {
679 curr_id = STP_INVALID;
680 ncnt = 0;
681
682 items[STP_ASYNC].cb(0, 0);
683 state = STP_STATE_OP;
684 } else {
685 ncnt = 0;
686 }
687 break;
688 }
689 case STP_STATE_OP: {
690 curr_id = get_op(data, &noff, &nlen, &ncnt, &ntotal);
691 if (curr_id != STP_INVALID) {
692 ntotal = items[curr_id].d_ncnt;
693 ncnt = 0;
694 if (ntotal > 0) {
695 state = STP_STATE_DATA;
696 data64[0] = 0;
697 } else if (items[curr_id].has_ts) {
698 state = STP_STATE_TS;
699 } else {
700 /* item without data and ts, notify. */
701 items[curr_id].cb(0, 0);
702 curr_id = STP_INVALID;
703 }
704 }
705 break;
706 }
707 case STP_STATE_DATA: {
708 size_t ncpy = MIN(ntotal - ncnt, nlen - noff);
709
710 get_nibbles(data, noff, data_buf, ncnt, ncpy);
711
712 ncnt += ncpy;
713 noff += ncpy;
714 if (ncnt == ntotal) {
715 swap_n(data_buf, ntotal);
716 ncnt = 0;
717 if (items[curr_id].has_ts) {
718 ncnt = 0;
719 ntotal = 0;
720 state = STP_STATE_TS;
721 } else {
722 items[curr_id].cb(*data64, 0);
723 curr_id = STP_INVALID;
724 state = STP_STATE_OP;
725 ntotal = 0;
726 ncnt = 0;
727 }
728 }
729 break;
730 }
731 case STP_STATE_TS:
732 if (ntotal == 0 && ncnt == 0) {
733 /* TS to be read but length is unknown yet */
734 *ts64 = 0;
735 ntotal = get_nibble(data, noff);
736 noff++;
737 /* Values up to 12 represents number of nibbles on which
738 * timestamp is encoded. Above are the exceptions:
739 * - 13 => 14 nibbles
740 * - 14 => 16 nibbles
741 */
742 if (ntotal > 12) {
743 if (ntotal == 13) {
744 ntotal = 14;
745 base_ts = ~BIT64_MASK(4 * ntotal) & prev_ts;
746 } else {
747 ntotal = 16;
748 base_ts = 0;
749 }
750 } else {
751 base_ts = ~BIT64_MASK(4 * ntotal) & prev_ts;
752 }
753
754 } else {
755 size_t ncpy = MIN(ntotal - ncnt, nlen - noff);
756
757 get_nibbles(data, noff, ts_buf, ncnt, ncpy);
758 ncnt += ncpy;
759 noff += ncpy;
760 if (ncnt == ntotal) {
761 swap_n(ts_buf, ntotal);
762 prev_ts = base_ts | *ts64;
763 items[curr_id].cb(*data64, prev_ts);
764 curr_id = STP_INVALID;
765 state = STP_STATE_OP;
766 ntotal = 0;
767 ncnt = 0;
768 }
769 }
770 break;
771
772 default:
773 break;
774 }
775 } while (noff < nlen);
776
777 noff = 0;
778
779 return 0;
780 }
781
mipi_stp_decoder_init(const struct mipi_stp_decoder_config * config)782 int mipi_stp_decoder_init(const struct mipi_stp_decoder_config *config)
783 {
784 state = config->start_out_of_sync ? STP_STATE_OUT_OF_SYNC : STP_STATE_OP;
785 ntotal = 0;
786 ncnt = 0;
787 cfg = *config;
788 prev_ts = 0;
789 base_ts = 0;
790 noff = 0;
791
792 return 0;
793 }
794
