[PATCH 03/13] hw/riscv: add n-trace encoder with ProgTraceSync message

Introduce N-Trace format message encoder, based on "RISC-V n-trace
(Nexus-based Trace) Specificationand" Version 1.0.

Add generation, encoding and sending logic for ProgTraceSync message

Signed-off-by: Konstantin Semichastnov <[email protected]>
---
 hw/riscv/rv-trace-messages.c | 154 +++++++++++++++++++++++++++++++++++++++++++
 hw/riscv/rv-trace-messages.h |   1 +
 hw/riscv/trace-encoder.c     |  29 +++++++-
 hw/riscv/trace-encoder.h     |   1 +
 4 files changed, 184 insertions(+), 1 deletion(-)

diff --git a/hw/riscv/rv-trace-messages.c b/hw/riscv/rv-trace-messages.c
index 932f3ae499..0b90d6a437 100644
--- a/hw/riscv/rv-trace-messages.c
+++ b/hw/riscv/rv-trace-messages.c
@@ -338,3 +338,157 @@ size_t rv_etrace_gen_encoded_format1(uint8_t *buf,
 
     return HEADER_SIZE + header.length;
 }
+
+#define MDO_BITS_NUM  6
+#define MSEO_BITS_NUM 2
+
+#define MSEO_VAR_FIELD_FINISHED 0b01
+#define MSEO_END_OF_MESSAGE     0b11
+
+typedef enum {
+    FIELD_END = 0,
+    FIELD_FIXED,
+    FIELD_VAR,
+} NTraceFieldType;
+
+typedef enum {
+    WIDTH_TCODE  = 6,
+    WIDTH_SYNC   = 4,
+    WIDTH_ICNT   = 22,
+    WIDTH_FADDR  = 63,
+} NTraceFieldWidth;
+
+typedef enum {
+    TCODE_PROG_TRACE_SYNC        = 0x9,
+} NTraceTcodeFieldValues;
+
+typedef enum {
+    SYNC_PERIODIC      = 2,
+    SYNC_TRACE_ENABLED = 5,
+} NTraceSyncFieldValues;
+
+typedef struct NTraceFieldPayload {
+    NTraceFieldType type;
+    NTraceFieldWidth width;
+    uint64_t value;
+} NTraceFieldPayload;
+
+#define PAYLOAD_TCODE(val)    \
+    {                         \
+        .type = FIELD_FIXED,  \
+        .width = WIDTH_TCODE, \
+        .value = (val),       \
+    }
+
+#define PAYLOAD_SYNC(val)    \
+    {                        \
+        .type = FIELD_FIXED, \
+        .width = WIDTH_SYNC, \
+        .value = (val),      \
+    }
+
+#define PAYLOAD_ICNT(val)    \
+    {                        \
+        .type = FIELD_VAR,   \
+        .width = WIDTH_ICNT, \
+        .value = (val),      \
+    }
+
+#define PAYLOAD_FADDR(val)    \
+    {                         \
+        .type = FIELD_VAR,    \
+        .width = WIDTH_FADDR, \
+        .value = (val),       \
+    }
+#define PAYLOAD_END()      \
+    {                      \
+        .type = FIELD_END, \
+        .width = 0,        \
+        .value = 0,        \
+    }
+
+static size_t rv_ntrace_gen_encoded_msg(uint8_t *buf,
+                                        const NTraceFieldPayload *payload)
+{
+    uint8_t buf_byte   = 0;
+    uint8_t packed_bit = 0;
+    uint8_t packed_cur = 0;
+    uint8_t nr_move    = 0;
+
+    /*
+     * as described in "Chapter 3. N-Trace Transmission Protocol" of N-Trace
+     * spec each byte has only bits[7:2] filled with data, called MDO bits.
+     * Lowest 2 bits[1:0] are called MSEO bits and used to signal about
+     * boundaries between message fields and messages
+     */
+
+    while (payload->type != FIELD_END) {
+        for (uint8_t offset = 0; offset < payload->width; offset += nr_move) {
+            nr_move = MIN(MDO_BITS_NUM - packed_bit, payload->width - offset);
+            uint8_t chunk = extract64(payload->value, offset, nr_move);
+
+            packed_cur = deposit32(packed_cur, packed_bit, nr_move, chunk);
+            packed_bit += nr_move;
+
+            if (packed_bit == MDO_BITS_NUM) {
+                buf[buf_byte++] = packed_cur << MSEO_BITS_NUM;
+                packed_bit = 0;
+                packed_cur = 0;
+            }
+        }
+
+        /*
+         * if field has variable length - last byte of this field should have
+         * MSEO (lowest 2) bits set to 01, and zero filled to end of byte
+         *
+         * N-Trace spec: 3.1. MSEO Sequences.
+         */
+        if (payload->type == FIELD_VAR) {
+            if (packed_bit != 0) {
+                /* last chunk was not finished, leave remaining bits cleared */
+                buf[buf_byte++] = packed_cur << MSEO_BITS_NUM;
+                packed_bit = 0;
+                packed_cur = 0;
+            }
+
+            /* correct last chunk MSEO bits */
+            buf[buf_byte - 1] = deposit32(buf[buf_byte - 1], 0,
+                                          MSEO_BITS_NUM,
+                                          MSEO_VAR_FIELD_FINISHED);
+        }
+
+        payload++;
+    }
+
+    if (packed_bit != 0) {
+        /* last chunk was not finished, so finish it now */
+        buf[buf_byte++] = packed_cur;
+    }
+
+    /*
+     * last byte of message should have MSEO (lowest 2) bits set to 11
+     *
+     * N-Trace spec: 3.1. MSEO Sequences.
+     */
+    buf[buf_byte - 1] = deposit32(buf[buf_byte - 1], 0,
+                                  MSEO_BITS_NUM,
+                                  MSEO_END_OF_MESSAGE);
+
+    return buf_byte;
+}
+
+size_t rv_ntrace_gen_encoded_prog_trace_sync_msg(uint8_t *buf, uint64_t faddr)
+{
+    const NTraceFieldPayload msg_fields[] = {
+        PAYLOAD_TCODE(TCODE_PROG_TRACE_SYNC),
+        /* SRC field is not supported for now */
+        PAYLOAD_SYNC(SYNC_TRACE_ENABLED),
+        PAYLOAD_ICNT(0),
+        PAYLOAD_FADDR(faddr),
+        /* TSTAMP field is not supported for now */
+        PAYLOAD_END(),
+    };
+
+    return rv_ntrace_gen_encoded_msg(buf, msg_fields);
+}
+
diff --git a/hw/riscv/rv-trace-messages.h b/hw/riscv/rv-trace-messages.h
index 6f700eac8e..635e498393 100644
--- a/hw/riscv/rv-trace-messages.h
+++ b/hw/riscv/rv-trace-messages.h
@@ -37,4 +37,5 @@ size_t rv_etrace_gen_encoded_format1(uint8_t *buf,
                                      uint64_t addr,
                                      bool notify, bool updiscon);
 
+size_t rv_ntrace_gen_encoded_prog_trace_sync_msg(uint8_t *buf, uint64_t faddr);
 #endif
diff --git a/hw/riscv/trace-encoder.c b/hw/riscv/trace-encoder.c
index 34c8f60ce1..10e52e207e 100644
--- a/hw/riscv/trace-encoder.c
+++ b/hw/riscv/trace-encoder.c
@@ -255,7 +255,13 @@ static void trencoder_te_ctrl_postw(RegisterInfo *reg, uint64_t val)
     if (!te->trace_running && trTeInstTracing) {
         /* Starting trace. Ask the CPU for the first trace insn */
         if (te->ntrace) {
-            /* send ProgTraceSync message */
+            /*
+             * N-Trace require to trace from very first instruction,
+             * not from next instruction, after enabling the trace,
+             * like E-Trace. So we need to send message here, instead of
+             * using te->trace_next_insn
+             */
+            trencoder_send_prog_trace_sync(OBJECT(te), env->pc);
         } else {
             te->trace_next_insn = true;
         }
@@ -508,6 +514,27 @@ void trencoder_set_first_trace_insn(Object *trencoder_obj, uint64_t pc)
     trencoder_send_message_smem(trencoder, msg, msg_size);
 }
 
+static uint64_t rv_ntrace_encode_faddr(uint64_t pc)
+{
+    return pc >> 1ul;
+}
+
+void trencoder_send_prog_trace_sync(Object *trencoder_obj, uint64_t pc)
+{
+    TraceEncoder *trencoder = TRACE_ENCODER(trencoder_obj);
+    g_autofree uint8_t *msg = g_malloc0(TRACE_MSG_MAX_SIZE);
+    uint8_t msg_size;
+
+    uint64_t faddr = rv_ntrace_encode_faddr(pc);
+
+    g_assert(trencoder->ntrace);
+
+    msg_size = rv_ntrace_gen_encoded_prog_trace_sync_msg(msg, faddr);
+
+    trencoder_send_message_smem(trencoder, msg, msg_size);
+}
+
+
 void trencoder_trace_trap_insn(Object *trencoder_obj,
                                uint64_t pc, uint32_t ecause,
                                bool is_interrupt,
diff --git a/hw/riscv/trace-encoder.h b/hw/riscv/trace-encoder.h
index fed0ef9a48..96324d2b9a 100644
--- a/hw/riscv/trace-encoder.h
+++ b/hw/riscv/trace-encoder.h
@@ -53,6 +53,7 @@ struct TraceEncoder {
 OBJECT_DECLARE_SIMPLE_TYPE(TraceEncoder, TRACE_ENCODER)
 
 void trencoder_set_first_trace_insn(Object *trencoder_obj, uint64_t pc);
+void trencoder_send_prog_trace_sync(Object *trencoder_obj, uint64_t pc);
 void trencoder_trace_trap_insn(Object *trencoder_obj,
                                uint64_t pc, uint32_t ecause,
                                bool is_interrupt,

-- 
2.43.0