[Pdbg] [PATCH 07/10] sbefifo: Add hardware procedeure for thread control

[Amitay Isaacs]

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

diff --git a/libpdbg/hwunit.h b/libpdbg/hwunit.h
index cc94c6d..497e5d7 100644
--- a/libpdbg/hwunit.h
+++ b/libpdbg/hwunit.h
@@ -70,6 +70,7 @@ struct sbefifo {
 	int (*istep)(struct sbefifo *, uint32_t major, uint32_t minor);
 	int (*mem_read)(struct sbefifo *, uint64_t, uint8_t *, uint64_t, bool);
 	int (*mem_write)(struct sbefifo *, uint64_t, uint8_t *, uint64_t, bool);
+	int (*control)(struct sbefifo *, uint32_t core_id, uint32_t thread_id, uint32_t oper);
 	uint32_t (*ffdc_get)(struct sbefifo *, const uint8_t **, uint32_t *);
 	void (*ffdc_dump)(struct sbefifo *);
 	int fd;
diff --git a/libpdbg/sbefifo.c b/libpdbg/sbefifo.c
index 54dc90d..42b33ef 100644
--- a/libpdbg/sbefifo.c
+++ b/libpdbg/sbefifo.c
@@ -436,6 +436,40 @@ static int sbefifo_op_putmem(struct sbefifo *sbefifo,
 	return 0;
 }
 
+/**
+ * operation = 0x0 start
+ *             0x1 stop
+ *             0x2 step
+ *             0x3 sreset
+ */
+static int sbefifo_op_control(struct sbefifo *sbefifo,
+			      uint32_t core_id, uint32_t thread_id,
+			      uint32_t oper)
+{
+	uint8_t *out;
+	uint32_t msg[3];
+	uint32_t cmd, op, out_len, status;
+	int rc;
+
+	PR_NOTICE("sbefifo: control c:0x%x, t=%u\n, op=%u\n", core_id, thread_id, oper);
+
+	op = ((core_id & 0xff) << 8) | ((thread_id & 0x0f) << 4) | (oper & 0x0f);
+	cmd = 0xa701;
+
+	msg[0] = htobe32(3);	// number of words
+	msg[1] = htobe32(cmd);
+	msg[2] = htobe32(op);
+
+	rc = sbefifo_op(sbefifo, msg, sizeof(msg), cmd, &out, &out_len, &status);
+	if (rc)
+		return rc;
+
+	if (out_len > 0)
+		free(out);
+
+	return 0;
+}
+
 static int sbefifo_probe(struct pdbg_target *target)
 {
 	struct sbefifo *sf = target_to_sbefifo(target);
@@ -463,6 +497,7 @@ struct sbefifo kernel_sbefifo = {
 	.istep = sbefifo_op_istep,
 	.mem_read = sbefifo_op_getmem,
 	.mem_write = sbefifo_op_putmem,
+	.control = sbefifo_op_control,
 	.ffdc_get = sbefifo_ffdc_get,
 	.ffdc_dump = sbefifo_ffdc_dump,
 	.fd = -1,
-- 
2.21.0