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
major_cb(uint64_t id,uint64_t ts)260 static void major_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_MAJOR, 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, major_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, major_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 uint32_t *src32 = (uint32_t *)&src[src_noff / 2];
465 uint32_t *dst32 = (uint32_t *)&dst[dst_noff / 2];
466
467 if (nlen == 16) {
468 /* dst must be aligned. */
469 if (src_ba) {
470 dst32[0] = src32[0];
471 dst32[1] = src32[1];
472 } else {
473 uint64_t src0 = src32[0] | ((uint64_t)src32[1] << 32);
474 uint64_t src1 = src32[2] | ((uint64_t)src32[3] << 32);
475 uint64_t part_a = src0 >> 4;
476 uint64_t part_b = src1 << 60;
477 uint64_t out = part_a | part_b;
478
479 dst32[0] = (uint32_t)out;
480 dst32[1] = (uint32_t)(out >> 32);
481 }
482 return;
483 }
484
485 uint64_t src0 = src32[0] | ((uint64_t)src32[1] << 32);
486 uint64_t mask = BIT64_MASK(nlen * 4) << (src_ba ? 0 : 4);
487 uint64_t src_d = src0 & mask;
488
489 if (((src_noff ^ dst_noff) & 0x1UL) == 0) {
490 /* nothing */
491 } else if (dst_ba) {
492 src_d >>= 4;
493 } else {
494 src_d <<= 4;
495 }
496
497 dst32[0] |= (uint32_t)src_d;
498 dst32[1] |= (uint32_t)(src_d >> 32);
499 }
500
501 /* Function performs getting nibbles in less efficient way but does not use unaligned
502 * access which may not be supported by some platforms.
503 */
get_nibbles_unaligned(const uint8_t * src,size_t src_noff,uint8_t * dst,size_t dst_noff,size_t nlen)504 static void get_nibbles_unaligned(const uint8_t *src, size_t src_noff, uint8_t *dst,
505 size_t dst_noff, size_t nlen)
506 {
507 for (size_t i = 0; i < nlen; i++) {
508 size_t idx = (src_noff + i) / 2;
509 size_t ni = (src_noff + i) & 0x1;
510 uint8_t n = src[idx] >> (ni ? 4 : 0);
511 size_t d_idx = (dst_noff + i) / 2;
512 size_t dni = (dst_noff + i) & 0x1;
513
514 if (dni == 0) {
515 dst[d_idx] = n;
516 } else {
517 dst[d_idx] |= n << 4;
518 }
519 }
520 }
521
get_nibbles(const uint8_t * src,size_t src_noff,uint8_t * dst,size_t dst_noff,size_t nlen)522 static inline void get_nibbles(const uint8_t *src, size_t src_noff, uint8_t *dst, size_t dst_noff,
523 size_t nlen)
524 {
525 if (!UNALIGNED_ACCESS_SUPPORTED) {
526 get_nibbles_unaligned(src, src_noff, dst, dst_noff, nlen);
527 return;
528 }
529
530 bool src_ba = (src_noff & 0x1UL) == 0;
531 bool dst_ba = (dst_noff & 0x1UL) == 0;
532 uint32_t *src32 = (uint32_t *)&src[src_noff / 2];
533 uint32_t *dst32 = (uint32_t *)&dst[dst_noff / 2];
534
535 if (nlen > 8) {
536 get_nibbles64(src, src_noff, dst, dst_noff, nlen);
537 return;
538 } else if (nlen == 8) {
539 /* dst must be aligned. */
540 if (src_ba) {
541 dst32[0] = src32[0];
542 } else {
543 uint32_t part_a = src32[0] >> 4;
544 uint32_t part_b = src32[1] << 28;
545
546 dst32[0] = part_a | part_b;
547 }
548 return;
549 }
550
551 uint32_t mask = BIT_MASK(nlen * 4) << (src_ba ? 0 : 4);
552 uint32_t src_d = src32[0] & mask;
553
554 if (((src_noff ^ dst_noff) & 0x1UL) == 0) {
555 dst32[0] |= src_d;
556 } else if (dst_ba) {
557 dst32[0] |= (src_d >> 4);
558 } else {
559 dst32[0] |= (src_d << 4);
560 }
561 }
562
563 /* Function swaps nibbles in a byte. */
swap8(uint8_t byte)564 static uint8_t swap8(uint8_t byte)
565 {
566 return (byte << 4) | (byte >> 4);
567 }
568
569 /* Function swaps nibbles in a 16 bit variable. */
swap16(uint16_t halfword)570 static uint16_t swap16(uint16_t halfword)
571 {
572 halfword = __builtin_bswap16(halfword);
573 uint16_t d1 = (halfword & 0xf0f0) >> 4;
574 uint16_t d2 = (halfword & 0x0f0f) << 4;
575
576 return d1 | d2;
577 }
578
579 /* Function swaps nibbles in a 32 bit word. */
swap32(uint32_t word)580 static uint32_t swap32(uint32_t word)
581 {
582 word = __builtin_bswap32(word);
583 uint32_t d1 = (word & 0xf0f0f0f0) >> 4;
584 uint32_t d2 = (word & 0x0f0f0f0f) << 4;
585
586 return d1 | d2;
587 }
588
589 /* Function swaps nibbles in a 64 bit word. */
swap64(uint64_t dword)590 static uint64_t swap64(uint64_t dword)
591 {
592 uint32_t l = (uint32_t)dword;
593 uint32_t u = (uint32_t)(dword >> 32);
594
595 return ((uint64_t)swap32(l) << 32) | (uint64_t)swap32(u);
596 }
597
swap_n(uint8_t * data,uint32_t n)598 static void swap_n(uint8_t *data, uint32_t n)
599 {
600 switch (n) {
601 case 2:
602 *data = swap8(*data);
603 break;
604 case 4:
605 *(uint16_t *)data = swap16(*(uint16_t *)data);
606 break;
607 case 8:
608 *(uint32_t *)data = swap32(*(uint32_t *)data);
609 break;
610 case 16:
611 *(uint64_t *)data = swap64(*(uint64_t *)data);
612 break;
613 default:
614 *(uint64_t *)data = swap64(*(uint64_t *)data);
615 *(uint64_t *)data >>= (4 * (16 - n));
616 break;
617 }
618 }
619
get_op(const uint8_t * data,size_t * noff,size_t * nlen,size_t * ncnt,size_t * ntotal)620 static enum stp_id get_op(const uint8_t *data, size_t *noff, size_t *nlen, size_t *ncnt,
621 size_t *ntotal)
622 {
623 uint8_t op = 0;
624
625 op = get_nibble(data, *noff);
626
627 *noff += 1;
628 *ncnt += 1;
629 if (*ntotal == 0 && *ncnt == 1) {
630 /* Starting to read op. */
631 /* op code has only 1 nibble. */
632 if (op != 0xF) {
633 return (enum stp_id)op;
634 }
635 } else if (*ncnt == 2) {
636 if (op == 0xF) {
637 /* ASYNC*/
638 *ntotal = 6;
639 } else if (op != 0) {
640 return (enum stp_id)(STP_TAG_2NIBBLE_OP - 1 + op);
641 }
642 } else if (*ncnt == 3) {
643 if (op != 0xf) {
644 return (enum stp_id)(STP_TAG_3NIBBLE_OP + op);
645 } else if (*ntotal == 0) {
646 *ntotal = 4;
647 }
648 } else if (*ncnt == *ntotal) {
649 if (*ntotal == 4) {
650 return (enum stp_id)(STP_TAG_4NIBBLE_OP + op);
651 } else {
652 return STP_ASYNC;
653 }
654 }
655
656 return STP_INVALID;
657 }
658
mipi_stp_decoder_sync_loss(void)659 void mipi_stp_decoder_sync_loss(void)
660 {
661 state = STP_STATE_OUT_OF_SYNC;
662 ncnt = 0;
663 ntotal = 0;
664 }
665
mipi_stp_decoder_decode(const uint8_t * data,size_t len)666 int mipi_stp_decoder_decode(const uint8_t *data, size_t len)
667 {
668 static enum stp_id curr_id = STP_INVALID;
669 static uint8_t data_buf[8] __aligned(sizeof(uint64_t));
670 static uint8_t ts_buf[8] __aligned(sizeof(uint64_t));
671 uint64_t *data64 = (uint64_t *)data_buf;
672 uint64_t *ts64 = (uint64_t *)ts_buf;
673 size_t nlen = 2 * len;
674
675 do {
676 switch (state) {
677 case STP_STATE_OUT_OF_SYNC: {
678 uint8_t b = get_nibble(data, noff);
679
680 noff++;
681 if (ncnt < 21 && b == 0xF) {
682 ncnt++;
683 } else if (ncnt == 21 && b == 0) {
684 curr_id = STP_INVALID;
685 ncnt = 0;
686
687 items[STP_ASYNC].cb(0, 0);
688 state = STP_STATE_OP;
689 } else {
690 ncnt = 0;
691 }
692 break;
693 }
694 case STP_STATE_OP: {
695 curr_id = get_op(data, &noff, &nlen, &ncnt, &ntotal);
696 if (curr_id != STP_INVALID) {
697 ntotal = items[curr_id].d_ncnt;
698 ncnt = 0;
699 if (ntotal > 0) {
700 state = STP_STATE_DATA;
701 data64[0] = 0;
702 } else if (items[curr_id].has_ts) {
703 state = STP_STATE_TS;
704 } else {
705 /* item without data and ts, notify. */
706 items[curr_id].cb(0, 0);
707 curr_id = STP_INVALID;
708 }
709 }
710 break;
711 }
712 case STP_STATE_DATA: {
713 size_t ncpy = MIN(ntotal - ncnt, nlen - noff);
714
715 get_nibbles(data, noff, data_buf, ncnt, ncpy);
716
717 ncnt += ncpy;
718 noff += ncpy;
719 if (ncnt == ntotal) {
720 swap_n(data_buf, ntotal);
721 ncnt = 0;
722 if (items[curr_id].has_ts) {
723 ncnt = 0;
724 ntotal = 0;
725 state = STP_STATE_TS;
726 } else {
727 items[curr_id].cb(*data64, 0);
728 curr_id = STP_INVALID;
729 state = STP_STATE_OP;
730 ntotal = 0;
731 ncnt = 0;
732 }
733 }
734 break;
735 }
736 case STP_STATE_TS:
737 if (ntotal == 0 && ncnt == 0) {
738 /* TS to be read but length is unknown yet */
739 *ts64 = 0;
740 ntotal = get_nibble(data, noff);
741 noff++;
742 /* Values up to 12 represents number of nibbles on which
743 * timestamp is encoded. Above are the exceptions:
744 * - 13 => 14 nibbles
745 * - 14 => 16 nibbles
746 */
747 if (ntotal > 12) {
748 if (ntotal == 13) {
749 ntotal = 14;
750 base_ts = ~BIT64_MASK(4 * ntotal) & prev_ts;
751 } else {
752 ntotal = 16;
753 base_ts = 0;
754 }
755 } else {
756 base_ts = ~BIT64_MASK(4 * ntotal) & prev_ts;
757 }
758
759 } else {
760 size_t ncpy = MIN(ntotal - ncnt, nlen - noff);
761
762 get_nibbles(data, noff, ts_buf, ncnt, ncpy);
763 ncnt += ncpy;
764 noff += ncpy;
765 if (ncnt == ntotal) {
766 swap_n(ts_buf, ntotal);
767 prev_ts = base_ts | *ts64;
768 items[curr_id].cb(*data64, prev_ts);
769 curr_id = STP_INVALID;
770 state = STP_STATE_OP;
771 ntotal = 0;
772 ncnt = 0;
773 }
774 }
775 break;
776
777 default:
778 break;
779 }
780 } while (noff < nlen);
781
782 noff = 0;
783
784 return 0;
785 }
786
mipi_stp_decoder_init(const struct mipi_stp_decoder_config * config)787 int mipi_stp_decoder_init(const struct mipi_stp_decoder_config *config)
788 {
789 state = config->start_out_of_sync ? STP_STATE_OUT_OF_SYNC : STP_STATE_OP;
790 ntotal = 0;
791 ncnt = 0;
792 cfg = *config;
793 prev_ts = 0;
794 base_ts = 0;
795 noff = 0;
796
797 return 0;
798 }
799
