[Pdbg] [PATCH v4 11/13] sbefifo: Add procedure for custom chipop

[Amitay Isaacs]

Signed-off-by: Amitay Isaacs <amitay at ozlabs.org>
---
 libpdbg/hwunit.h  |  1 +
 libpdbg/sbefifo.c | 33 +++++++++++++++++++++++++++++++++
 2 files changed, 34 insertions(+)

diff --git a/libpdbg/hwunit.h b/libpdbg/hwunit.h
index 501ec74..1bfb872 100644
--- a/libpdbg/hwunit.h
+++ b/libpdbg/hwunit.h
@@ -74,6 +74,7 @@ struct sbefifo {
 	int (*thread_stop)(struct sbefifo *, uint32_t core_id, uint32_t thread_id);
 	int (*thread_step)(struct sbefifo *, uint32_t core_id, uint32_t thread_id);
 	int (*thread_sreset)(struct sbefifo *, uint32_t core_id, uint32_t thread_id);
+	int (*chipop)(struct sbefifo *, uint32_t *, uint32_t, uint8_t **, uint32_t *, uint32_t *);
 	uint32_t (*ffdc_get)(struct sbefifo *, const uint8_t **, uint32_t *);
 	int fd;
 	uint32_t status;
diff --git a/libpdbg/sbefifo.c b/libpdbg/sbefifo.c
index 79a0c46..a2442cd 100644
--- a/libpdbg/sbefifo.c
+++ b/libpdbg/sbefifo.c
@@ -430,6 +430,38 @@ static int sbefifo_op_thread_sreset(struct sbefifo *sbefifo,
 	return sbefifo_op_control(sbefifo, core_id, thread_id, SBEFIFO_INSN_OP_SRESET);
 }
 
+static int sbefifo_op_chipop(struct sbefifo *sbefifo,
+			     uint32_t *msg, uint32_t msg_len,
+			     uint8_t **out, uint32_t *out_len, uint32_t *status)
+{
+	uint32_t len, cmd, op;
+	int rc;
+
+	assert(msg_len > 3);
+
+	len = be32toh(msg[0]);
+	cmd = be32toh(msg[1]);
+	op = be32toh(msg[2]);
+
+	PR_NOTICE("sbefifo: chipop command=%u, op=%u\n", cmd, op);
+
+	if (len != msg_len) {
+		PR_ERROR("sbefifo: chipop: Invalid msg length, expected %u, got %u\n",
+			 len, msg_len);
+		return -1;
+	}
+
+	/* Limit the expected data to 64K */
+	if (*out_len > 0x10000)
+		*out_len = 0x10000;
+
+	rc = sbefifo_op(sbefifo, msg, msg_len, cmd, out, out_len, status);
+	if (rc)
+		return rc;
+
+	return 0;
+}
+
 static int sbefifo_probe(struct pdbg_target *target)
 {
 	struct sbefifo *sf = target_to_sbefifo(target);
@@ -461,6 +493,7 @@ struct sbefifo kernel_sbefifo = {
 	.thread_stop = sbefifo_op_thread_stop,
 	.thread_step = sbefifo_op_thread_step,
 	.thread_sreset = sbefifo_op_thread_sreset,
+	.chipop = sbefifo_op_chipop,
 	.ffdc_get = sbefifo_ffdc_get,
 	.fd = -1,
 };
-- 
2.21.0