subsys/debug/mipi_stp_decoder.c

/*
 * Copyright (c) 2024 Nordic Semiconductor ASA.
 *
 * SPDX-License-Identifier: Apache-2.0
 */
#include <zephyr/debug/mipi_stp_decoder.h>
#include <string.h>

#if defined(CONFIG_CPU_CORTEX_M) && \
	!defined(CONFIG_CPU_CORTEX_M0) && \
	!defined(CONFIG_CPU_CORTEX_M0PLUS)
#define UNALIGNED_ACCESS_SUPPORTED 1
#else
#define UNALIGNED_ACCESS_SUPPORTED 0
#endif

enum stp_state {
	STP_STATE_OP,
	STP_STATE_DATA,
	STP_STATE_TS,
	STP_STATE_OUT_OF_SYNC,
};

enum stp_id {
	STP_NULL,
	STP_M8,
	STP_MERR,
	STP_C8,
	STP_D8,
	STP_D16,
	STP_D32,
	STP_D64,
	STP_D8MTS,
	STP_D16MTS,
	STP_D32MTS,
	STP_D64MTS,
	STP_D4,
	STP_D4MTS,
	STP_FLAG_TS,
	STP_VERSION,
	STP_TAG_3NIBBLE_OP = STP_VERSION,
	STP_NULL_TS,
	STP_USER,
	STP_USER_TS,
	STP_TIME,
	STP_TIME_TS,
	STP_TRIG,
	STP_TRIG_TS,
	STP_FREQ,
	STP_FREQ_TS,
	STP_XSYNC,
	STP_XSYNC_TS,
	STP_FREQ_40,
	STP_TAG_4NIBBLE_OP = STP_FREQ_40,
	STP_FREQ_40_TS,
	STP_DIP,
	STP_M16,
	STP_TAG_2NIBBLE_OP = STP_M16,
	STP_GERR,
	STP_C16,
	STP_D8TS,
	STP_D16TS,
	STP_D32TS,
	STP_D64TS,
	STP_D8M,
	STP_D16M,
	STP_D32M,
	STP_D64M,
	STP_D4TS,
	STP_D4M,
	STP_FLAG,
	STP_ASYNC,
	STP_INVALID,
	STP_OP_MAX
};

#define STP_LONG_OP_ID 0xF
#define STP_2B_OP_ID   0xF0

#define STP_VAR_DATA 0xff

typedef void (*stp_cb)(uint64_t data, uint64_t ts);

struct stp_item {
	const char *name;
	enum stp_id type;
	uint8_t id[3];
	uint8_t id_ncnt;
	uint8_t d_ncnt;
	bool has_ts;
	stp_cb cb;
};

#define STP_ITEM(_type, _id, _id_ncnt, _d_ncnt, _has_ts, _cb)                                      \
	{                                                                                          \
		.name = STRINGIFY(_type), .type = _type, .id = {__DEBRACKET _id},                  \
					  .id_ncnt = _id_ncnt, .d_ncnt = _d_ncnt,                  \
					  .has_ts = _has_ts, .cb = (stp_cb)_cb                     \
	}

static struct mipi_stp_decoder_config cfg;
static uint64_t prev_ts;
static uint64_t base_ts;
static enum stp_state state;
static size_t ntotal;
static size_t ncnt;
static size_t noff;
static uint16_t curr_ch;

static void data4_cb(uint64_t data, uint64_t ts)
{
	ARG_UNUSED(ts);
	union mipi_stp_decoder_data d = {.data = data};

	cfg.cb(STP_DATA4, d, NULL, false);
}

static void data8_cb(uint64_t data, uint64_t ts)
{
	ARG_UNUSED(ts);
	union mipi_stp_decoder_data d = {.data = data};

	cfg.cb(STP_DATA8, d, NULL, false);
}

static void data16_cb(uint64_t data, uint64_t ts)
{
	ARG_UNUSED(ts);
	union mipi_stp_decoder_data d = {.data = data};

	cfg.cb(STP_DATA16, d, NULL, false);
}

static void data32_cb(uint64_t data, uint64_t ts)
{
	ARG_UNUSED(ts);
	union mipi_stp_decoder_data d = {.data = data};

	cfg.cb(STP_DATA32, d, NULL, false);
}

static void data64_cb(uint64_t data, uint64_t ts)
{
	ARG_UNUSED(ts);
	union mipi_stp_decoder_data d = {.data = data};

	cfg.cb(STP_DATA64, d, NULL, false);
}

static void data4_m_cb(uint64_t data, uint64_t ts)
{
	ARG_UNUSED(ts);
	union mipi_stp_decoder_data d = {.data = data};

	cfg.cb(STP_DATA4, d, NULL, true);
}

static void data8_m_cb(uint64_t data, uint64_t ts)
{
	ARG_UNUSED(ts);
	union mipi_stp_decoder_data d = {.data = data};

	cfg.cb(STP_DATA8, d, NULL, true);
}

static void data16_m_cb(uint64_t data, uint64_t ts)
{
	ARG_UNUSED(ts);
	union mipi_stp_decoder_data d = {.data = data};

	cfg.cb(STP_DATA16, d, NULL, true);
}

static void data32_m_cb(uint64_t data, uint64_t ts)
{
	ARG_UNUSED(ts);
	union mipi_stp_decoder_data d = {.data = data};

	cfg.cb(STP_DATA32, d, NULL, true);
}

static void data64_m_cb(uint64_t data, uint64_t ts)
{
	ARG_UNUSED(ts);
	union mipi_stp_decoder_data d = {.data = data};

	cfg.cb(STP_DATA64, d, NULL, true);
}

static void data4_ts_cb(uint64_t data, uint64_t ts)
{
	union mipi_stp_decoder_data d = {.data = data};

	cfg.cb(STP_DATA4, d, &ts, false);
}

static void data8_ts_cb(uint64_t data, uint64_t ts)
{
	union mipi_stp_decoder_data d = {.data = data};

	cfg.cb(STP_DATA8, d, &ts, false);
}

static void data16_ts_cb(uint64_t data, uint64_t ts)
{
	union mipi_stp_decoder_data d = {.data = data};

	cfg.cb(STP_DATA16, d, &ts, false);
}

static void data32_ts_cb(uint64_t data, uint64_t ts)
{
	union mipi_stp_decoder_data d = {.data = data};

	cfg.cb(STP_DATA32, d, &ts, false);
}

static void data64_ts_cb(uint64_t data, uint64_t ts)
{
	union mipi_stp_decoder_data d = {.data = data};

	cfg.cb(STP_DATA64, d, &ts, false);
}

static void data4_mts_cb(uint64_t data, uint64_t ts)
{
	union mipi_stp_decoder_data d = {.data = data};

	cfg.cb(STP_DATA4, d, &ts, true);
}

static void data8_mts_cb(uint64_t data, uint64_t ts)
{
	union mipi_stp_decoder_data d = {.data = data};

	cfg.cb(STP_DATA8, d, &ts, true);
}

static void data16_mts_cb(uint64_t data, uint64_t ts)
{
	union mipi_stp_decoder_data d = {.data = data};

	cfg.cb(STP_DATA16, d, &ts, true);
}

static void data32_mts_cb(uint64_t data, uint64_t ts)
{
	union mipi_stp_decoder_data d = {.data = data};

	cfg.cb(STP_DATA32, d, &ts, true);
}

static void data64_mts_cb(uint64_t data, uint64_t ts)
{
	union mipi_stp_decoder_data d = {.data = data};

	cfg.cb(STP_DATA64, d, &ts, true);
}

static void major_cb(uint64_t id, uint64_t ts)
{
	ARG_UNUSED(ts);
	uint16_t m_id = (uint16_t)id;
	union mipi_stp_decoder_data data = {.id = m_id};

	curr_ch = 0;

	cfg.cb(STP_DECODER_MAJOR, data, NULL, false);
}

static void channel16_cb(uint64_t id, uint64_t ts)
{
	uint16_t ch = (uint16_t)id;

	curr_ch = 0xFF00 & ch;

	ARG_UNUSED(ts);
	union mipi_stp_decoder_data data = {.id = ch};

	cfg.cb(STP_DECODER_CHANNEL, data, NULL, false);
}
static void channel_cb(uint64_t id, uint64_t ts)
{
	uint16_t ch = (uint16_t)id;

	ch |= curr_ch;

	ARG_UNUSED(ts);
	union mipi_stp_decoder_data data = {.id = ch};

	cfg.cb(STP_DECODER_CHANNEL, data, NULL, false);
}

static void merror_cb(uint64_t err, uint64_t ts)
{
	ARG_UNUSED(ts);
	union mipi_stp_decoder_data data = {.err = (uint32_t)err};

	cfg.cb(STP_DECODER_MERROR, data, NULL, false);
}

static void gerror_cb(uint64_t err, uint64_t ts)
{
	ARG_UNUSED(ts);
	union mipi_stp_decoder_data data = {.err = (uint32_t)err};

	cfg.cb(STP_DECODER_GERROR, data, NULL, false);
}

static void flag_cb(uint64_t data, uint64_t ts)
{
	ARG_UNUSED(ts);
	ARG_UNUSED(data);
	union mipi_stp_decoder_data dummy = {.dummy = 0};

	cfg.cb(STP_DECODER_FLAG, dummy, NULL, false);
}

static void flag_ts_cb(uint64_t unused, uint64_t ts)
{
	ARG_UNUSED(unused);
	union mipi_stp_decoder_data data = {.dummy = 0};

	cfg.cb(STP_DECODER_FLAG, data, &ts, false);
}

static void version_cb(uint64_t version, uint64_t ts)
{
	ARG_UNUSED(ts);

	curr_ch = 0;

	union mipi_stp_decoder_data data = {.ver = version};

	cfg.cb(STP_DECODER_VERSION, data, NULL, false);
}

static void notsup_cb(uint64_t data, uint64_t ts)
{
	ARG_UNUSED(ts);
	ARG_UNUSED(data);

	union mipi_stp_decoder_data dummy = {.dummy = 0};

	cfg.cb(STP_DECODER_NOT_SUPPORTED, dummy, NULL, false);
}

static void freq_cb(uint64_t freq, uint64_t ts)
{
	ARG_UNUSED(ts);

	union mipi_stp_decoder_data data = {.freq = freq};

	cfg.cb(STP_DECODER_FREQ, data, NULL, false);
}

static void freq_ts_cb(uint64_t freq, uint64_t ts)
{
	union mipi_stp_decoder_data data = {.freq = freq};

	cfg.cb(STP_DECODER_FREQ, data, &ts, false);
}

static void null_cb(uint64_t data, uint64_t ts)
{
	ARG_UNUSED(ts);
	ARG_UNUSED(data);

	union mipi_stp_decoder_data dummy = {.dummy = 0};

	cfg.cb(STP_DECODER_NULL, dummy, NULL, false);
}

static void async_cb(uint64_t data, uint64_t ts)
{
	ARG_UNUSED(ts);
	ARG_UNUSED(data);

	union mipi_stp_decoder_data dummy = {.dummy = 0};

	cfg.cb(STP_DECODER_ASYNC, dummy, NULL, false);
}

static const struct stp_item items[] = {
	STP_ITEM(STP_NULL, (0x0), 1, 0, false, null_cb),
	STP_ITEM(STP_M8, (0x1), 1, 2, false, major_cb),
	STP_ITEM(STP_MERR, (0x2), 1, 2, false, merror_cb),
	STP_ITEM(STP_C8, (0x3), 1, 2, false, channel_cb),
	STP_ITEM(STP_D8, (0x4), 1, 2, false, data8_cb),
	STP_ITEM(STP_D16, (0x5), 1, 4, false, data16_cb),
	STP_ITEM(STP_D32, (0x6), 1, 8, false, data32_cb),
	STP_ITEM(STP_D64, (0x7), 1, 16, false, data64_cb),
	STP_ITEM(STP_D8MTS, (0x8), 1, 2, true, data8_mts_cb),
	STP_ITEM(STP_D16MTS, (0x9), 1, 4, true, data16_mts_cb),
	STP_ITEM(STP_D32MTS, (0xa), 1, 8, true, data32_mts_cb),
	STP_ITEM(STP_D64MTS, (0xb), 1, 16, true, data64_mts_cb),
	STP_ITEM(STP_D4, (0xc), 1, 1, false, data4_cb),
	STP_ITEM(STP_D4MTS, (0xd), 1, 1, true, data4_mts_cb),
	STP_ITEM(STP_FLAG_TS, (0xe), 1, 0, true, flag_ts_cb),
	STP_ITEM(STP_VERSION, (0xf0, 0x00), 3, 1, false, version_cb),
	STP_ITEM(STP_NULL_TS, (0xf0, 0x01), 3, 0, true, notsup_cb),
	STP_ITEM(STP_USER, (0xf0, 0x02), 3, 0, false, notsup_cb),
	STP_ITEM(STP_USER_TS, (0xf0, 0x03), 3, 0, true, notsup_cb),
	STP_ITEM(STP_TIME, (0xf0, 0x04), 3, 0, false, notsup_cb),
	STP_ITEM(STP_TIME_TS, (0xf0, 0x05), 3, 0, true, notsup_cb),
	STP_ITEM(STP_TRIG, (0xf0, 0x06), 3, 0, false, notsup_cb),
	STP_ITEM(STP_TRIG_TS, (0xf0, 0x07), 3, 0, true, notsup_cb),
	STP_ITEM(STP_FREQ, (0xf0, 0x08), 3, 8, false, freq_cb),
	STP_ITEM(STP_FREQ_TS, (0xf0, 0x09), 3, 8, true, freq_ts_cb),
	STP_ITEM(STP_XSYNC, (0xf0, 0x0a), 3, 0, false, notsup_cb),
	STP_ITEM(STP_XSYNC_TS, (0xf0, 0x0b), 3, 0, true, notsup_cb),
	STP_ITEM(STP_FREQ_40, (0xf0, 0xf0), 4, 10, false, freq_cb),
	STP_ITEM(STP_FREQ_40_TS, (0xf0, 0xf1), 4, 0, true, notsup_cb),
	STP_ITEM(STP_DIP, (0xf0, 0xf2), 4, 0, false, notsup_cb),
	STP_ITEM(STP_M16, (0xf1), 2, 4, false, major_cb),
	STP_ITEM(STP_GERR, (0xf2), 2, 2, false, gerror_cb),
	STP_ITEM(STP_C16, (0xf3), 2, 4, false, channel16_cb),
	STP_ITEM(STP_D8TS, (0xf4), 2, 2, true, data8_ts_cb),
	STP_ITEM(STP_D16TS, (0xf5), 2, 4, true, data16_ts_cb),
	STP_ITEM(STP_D32TS, (0xf6), 2, 8, true, data32_ts_cb),
	STP_ITEM(STP_D64TS, (0xf7), 2, 16, true, data64_ts_cb),
	STP_ITEM(STP_D8M, (0xf8), 2, 2, false, data8_m_cb),
	STP_ITEM(STP_D16M, (0xf9), 2, 4, false, data16_m_cb),
	STP_ITEM(STP_D32M, (0xfa), 2, 8, false, data32_m_cb),
	STP_ITEM(STP_D64M, (0xfb), 2, 16, false, data64_m_cb),
	STP_ITEM(STP_D4TS, (0xfc), 2, 1, true, data4_ts_cb),
	STP_ITEM(STP_D4M, (0xfd), 2, 1, false, data4_m_cb),
	STP_ITEM(STP_FLAG, (0xfe), 2, 0, false, flag_cb),
	STP_ITEM(STP_ASYNC, (0xff, 0xff, 0xff), 6, 16, false, async_cb),
	STP_ITEM(STP_INVALID, (0x0), 0, 0, false, NULL),
};

/** @brief Decode a nibble and read opcode from the stream.
 *
 * Function reads a nibble and continues or starts decoding of a STP opcode.
 *
 * @param	  data		Pointer to the stream.
 * @param[in,out] noff		Offset (in nibbles).
 * @param	  nlen		Length (in nibbles).
 * @param[in,out] ncnt		Current number of nibbles
 * @param[in,out] ntotal	Number of nibbles in the opcode.
 *
 * @retval STP_INVALID	Opcode decoding is in progress.
 * @retval opcode	Decoded opcode.
 */
static inline uint8_t get_nibble(const uint8_t *data, size_t noff)
{
	uint8_t ret = data[noff / 2];

	if (noff & 0x1UL) {
		ret >>= 4;
	}

	ret &= 0x0F;

	return ret;
}

static inline void get_nibbles64(const uint8_t *src, size_t src_noff, uint8_t *dst, size_t dst_noff,
				 size_t nlen)
{
	bool src_ba = (src_noff & 0x1UL) == 0;
	bool dst_ba = (dst_noff & 0x1UL) == 0;
	uint32_t *src32 = (uint32_t *)&src[src_noff / 2];
	uint32_t *dst32 = (uint32_t *)&dst[dst_noff / 2];

	if (nlen == 16) {
		/* dst must be aligned. */
		if (src_ba) {
			dst32[0] = src32[0];
			dst32[1] = src32[1];
		} else {
			uint64_t src0 = src32[0] | ((uint64_t)src32[1] << 32);
			uint64_t src1 = src32[2] | ((uint64_t)src32[3] << 32);
			uint64_t part_a = src0 >> 4;
			uint64_t part_b = src1 << 60;
			uint64_t out = part_a | part_b;

			dst32[0] = (uint32_t)out;
			dst32[1] = (uint32_t)(out >> 32);
		}
		return;
	}

	uint64_t src0 = src32[0] | ((uint64_t)src32[1] << 32);
	uint64_t mask = BIT64_MASK(nlen * 4) << (src_ba ? 0 : 4);
	uint64_t src_d = src0 & mask;

	if (((src_noff ^ dst_noff) & 0x1UL) == 0) {
		/* nothing */
	} else if (dst_ba) {
		src_d >>= 4;
	} else {
		src_d <<= 4;
	}

	dst32[0] |= (uint32_t)src_d;
	dst32[1] |= (uint32_t)(src_d >> 32);
}

/* Function performs getting nibbles in less efficient way but does not use unaligned
 * access which may not be supported by some platforms.
 */
static void get_nibbles_unaligned(const uint8_t *src, size_t src_noff, uint8_t *dst,
				  size_t dst_noff, size_t nlen)
{
	for (size_t i = 0; i < nlen; i++) {
		size_t idx = (src_noff + i) / 2;
		size_t ni = (src_noff + i) & 0x1;
		uint8_t n = src[idx] >> (ni ? 4 : 0);
		size_t d_idx = (dst_noff + i) / 2;
		size_t dni = (dst_noff + i) & 0x1;

		if (dni == 0) {
			dst[d_idx] = n;
		} else {
			dst[d_idx] |= n << 4;
		}
	}
}

static inline void get_nibbles(const uint8_t *src, size_t src_noff, uint8_t *dst, size_t dst_noff,
			       size_t nlen)
{
	if (!UNALIGNED_ACCESS_SUPPORTED) {
		get_nibbles_unaligned(src, src_noff, dst, dst_noff, nlen);
		return;
	}

	bool src_ba = (src_noff & 0x1UL) == 0;
	bool dst_ba = (dst_noff & 0x1UL) == 0;
	uint32_t *src32 = (uint32_t *)&src[src_noff / 2];
	uint32_t *dst32 = (uint32_t *)&dst[dst_noff / 2];

	if (nlen > 8) {
		get_nibbles64(src, src_noff, dst, dst_noff, nlen);
		return;
	} else if (nlen == 8) {
		/* dst must be aligned. */
		if (src_ba) {
			dst32[0] = src32[0];
		} else {
			uint32_t part_a = src32[0] >> 4;
			uint32_t part_b = src32[1] << 28;

			dst32[0] = part_a | part_b;
		}
		return;
	}

	uint32_t mask = BIT_MASK(nlen * 4) << (src_ba ? 0 : 4);
	uint32_t src_d = src32[0] & mask;

	if (((src_noff ^ dst_noff) & 0x1UL) == 0) {
		dst32[0] |= src_d;
	} else if (dst_ba) {
		dst32[0] |= (src_d >> 4);
	} else {
		dst32[0] |= (src_d << 4);
	}
}

/* Function swaps nibbles in a byte. */
static uint8_t swap8(uint8_t byte)
{
	return (byte << 4) | (byte >> 4);
}

/* Function swaps nibbles in a 16 bit variable. */
static uint16_t swap16(uint16_t halfword)
{
	halfword = __builtin_bswap16(halfword);
	uint16_t d1 = (halfword & 0xf0f0) >> 4;
	uint16_t d2 = (halfword & 0x0f0f) << 4;

	return d1 | d2;
}

/* Function swaps nibbles in a 32 bit word. */
static uint32_t swap32(uint32_t word)
{
	word = __builtin_bswap32(word);
	uint32_t d1 = (word & 0xf0f0f0f0) >> 4;
	uint32_t d2 = (word & 0x0f0f0f0f) << 4;

	return d1 | d2;
}

/* Function swaps nibbles in a 64 bit word. */
static uint64_t swap64(uint64_t dword)
{
	uint32_t l = (uint32_t)dword;
	uint32_t u = (uint32_t)(dword >> 32);

	return ((uint64_t)swap32(l) << 32) | (uint64_t)swap32(u);
}

static void swap_n(uint8_t *data, uint32_t n)
{
	switch (n) {
	case 2:
		*data = swap8(*data);
		break;
	case 4:
		*(uint16_t *)data = swap16(*(uint16_t *)data);
		break;
	case 8:
		*(uint32_t *)data = swap32(*(uint32_t *)data);
		break;
	case 16:
		*(uint64_t *)data = swap64(*(uint64_t *)data);
		break;
	default:
		*(uint64_t *)data = swap64(*(uint64_t *)data);
		*(uint64_t *)data >>= (4 * (16 - n));
		break;
	}
}

static enum stp_id get_op(const uint8_t *data, size_t *noff, size_t *nlen, size_t *ncnt,
			  size_t *ntotal)
{
	uint8_t op = 0;

	op = get_nibble(data, *noff);

	*noff += 1;
	*ncnt += 1;
	if (*ntotal == 0 && *ncnt == 1) {
		/* Starting to read op. */
		/* op code has only 1 nibble. */
		if (op != 0xF) {
			return (enum stp_id)op;
		}
	} else if (*ncnt == 2) {
		if (op == 0xF) {
			/* ASYNC*/
			*ntotal = 6;
		} else if (op != 0) {
			return (enum stp_id)(STP_TAG_2NIBBLE_OP - 1 + op);
		}
	} else if (*ncnt == 3) {
		if (op != 0xf) {
			return (enum stp_id)(STP_TAG_3NIBBLE_OP + op);
		} else if (*ntotal == 0) {
			*ntotal = 4;
		}
	} else if (*ncnt == *ntotal) {
		if (*ntotal == 4) {
			return (enum stp_id)(STP_TAG_4NIBBLE_OP + op);
		} else {
			return STP_ASYNC;
		}
	}

	return STP_INVALID;
}

void mipi_stp_decoder_sync_loss(void)
{
	state = STP_STATE_OUT_OF_SYNC;
	ncnt = 0;
	ntotal = 0;
}

int mipi_stp_decoder_decode(const uint8_t *data, size_t len)
{
	static enum stp_id curr_id = STP_INVALID;
	static uint8_t data_buf[8] __aligned(sizeof(uint64_t));
	static uint8_t ts_buf[8] __aligned(sizeof(uint64_t));
	uint64_t *data64 = (uint64_t *)data_buf;
	uint64_t *ts64 = (uint64_t *)ts_buf;
	size_t nlen = 2 * len;

	do {
		switch (state) {
		case STP_STATE_OUT_OF_SYNC: {
			uint8_t b = get_nibble(data, noff);

			noff++;
			if (ncnt < 21 && b == 0xF) {
				ncnt++;
			} else if (ncnt == 21 && b == 0) {
				curr_id = STP_INVALID;
				ncnt = 0;

				items[STP_ASYNC].cb(0, 0);
				state = STP_STATE_OP;
			} else {
				ncnt = 0;
			}
			break;
		}
		case STP_STATE_OP: {
			curr_id = get_op(data, &noff, &nlen, &ncnt, &ntotal);
			if (curr_id != STP_INVALID) {
				ntotal = items[curr_id].d_ncnt;
				ncnt = 0;
				if (ntotal > 0) {
					state = STP_STATE_DATA;
					data64[0] = 0;
				} else if (items[curr_id].has_ts) {
					state = STP_STATE_TS;
				} else {
					/* item without data and ts, notify. */
					items[curr_id].cb(0, 0);
					curr_id = STP_INVALID;
				}
			}
			break;
		}
		case STP_STATE_DATA: {
			size_t ncpy = MIN(ntotal - ncnt, nlen - noff);

			get_nibbles(data, noff, data_buf, ncnt, ncpy);

			ncnt += ncpy;
			noff += ncpy;
			if (ncnt == ntotal) {
				swap_n(data_buf, ntotal);
				ncnt = 0;
				if (items[curr_id].has_ts) {
					ncnt = 0;
					ntotal = 0;
					state = STP_STATE_TS;
				} else {
					items[curr_id].cb(*data64, 0);
					curr_id = STP_INVALID;
					state = STP_STATE_OP;
					ntotal = 0;
					ncnt = 0;
				}
			}
			break;
		}
		case STP_STATE_TS:
			if (ntotal == 0 && ncnt == 0) {
				/* TS to be read but length is unknown yet */
				*ts64 = 0;
				ntotal = get_nibble(data, noff);
				noff++;
				/* Values up to 12 represents number of nibbles on which
				 * timestamp is encoded. Above are the exceptions:
				 * - 13 => 14 nibbles
				 * - 14 => 16 nibbles
				 */
				if (ntotal > 12) {
					if (ntotal == 13) {
						ntotal = 14;
						base_ts = ~BIT64_MASK(4 * ntotal) & prev_ts;
					} else {
						ntotal = 16;
						base_ts = 0;
					}
				} else {
					base_ts = ~BIT64_MASK(4 * ntotal) & prev_ts;
				}

			} else {
				size_t ncpy = MIN(ntotal - ncnt, nlen - noff);

				get_nibbles(data, noff, ts_buf, ncnt, ncpy);
				ncnt += ncpy;
				noff += ncpy;
				if (ncnt == ntotal) {
					swap_n(ts_buf, ntotal);
					prev_ts = base_ts | *ts64;
					items[curr_id].cb(*data64, prev_ts);
					curr_id = STP_INVALID;
					state = STP_STATE_OP;
					ntotal = 0;
					ncnt = 0;
				}
			}
			break;

		default:
			break;
		}
	} while (noff < nlen);

	noff = 0;

	return 0;
}

int mipi_stp_decoder_init(const struct mipi_stp_decoder_config *config)
{
	state = config->start_out_of_sync ? STP_STATE_OUT_OF_SYNC : STP_STATE_OP;
	ntotal = 0;
	ncnt = 0;
	cfg = *config;
	prev_ts = 0;
	base_ts = 0;
	noff = 0;

	return 0;
}