[Pdbg] [PATCH 09/17] libsbefifo: Refactor protocol marshalling for instruction chip-op

[Amitay Isaacs]

Signed-off-by: Amitay Isaacs <amitay at ozlabs.org>
---
 libsbefifo/cmd_instruction.c | 49 ++++++++++++++++++++++++++++--------
 libsbefifo/libsbefifo.h      |  3 +++
 2 files changed, 41 insertions(+), 11 deletions(-)

diff --git a/libsbefifo/cmd_instruction.c b/libsbefifo/cmd_instruction.c
index c83cdd7..a7a225d 100644
--- a/libsbefifo/cmd_instruction.c
+++ b/libsbefifo/cmd_instruction.c
@@ -22,33 +22,60 @@
 #include "libsbefifo.h"
 #include "sbefifo_private.h"
 
-int sbefifo_control_insn(struct sbefifo_context *sctx, uint8_t core_id, uint8_t thread_id, uint8_t thread_op, uint8_t mode)
+int sbefifo_control_insn_push(uint8_t core_id, uint8_t thread_id, uint8_t thread_op, uint8_t mode, uint8_t **buf, uint32_t *buflen)
 {
-	uint8_t *out;
-	uint32_t msg[3];
-	uint32_t cmd, out_len;
+	uint32_t *msg;
+	uint32_t nwords, cmd;
 	uint32_t oper;
-	int rc;
+
+	nwords = 3;
+	*buflen = nwords * sizeof(uint32_t);
+	msg = malloc(*buflen);
+	if (!msg)
+		return ENOMEM;
 
 	cmd = SBEFIFO_CMD_CLASS_INSTRUCTION | SBEFIFO_CMD_CONTROL_INSN;
+
 	oper = ((uint32_t)(mode & 0xf) << 16) |
 		((uint32_t)(core_id & 0xff) << 8) |
 		((uint32_t)(thread_id & 0xf) << 4) |
 		((uint32_t)(thread_op & 0xf));
 
-	msg[0] = htobe32(3);	// number of words
+	msg[0] = htobe32(nwords);
 	msg[1] = htobe32(cmd);
 	msg[2] = htobe32(oper);
 
+	*buf = (uint8_t *)msg;
+	return 0;
+}
+
+int sbefifo_control_insn_pull(uint8_t *buf, uint32_t buflen)
+{
+	if (buflen != 0)
+		return EPROTO;
+
+	return 0;
+}
+
+int sbefifo_control_insn(struct sbefifo_context *sctx, uint8_t core_id, uint8_t thread_id, uint8_t thread_op, uint8_t mode)
+{
+	uint8_t *msg, *out;
+	uint32_t msg_len, out_len;
+	int rc;
+
+	rc = sbefifo_control_insn_push(core_id, thread_id, thread_op, mode, &msg, &msg_len);
+	if (rc)
+		return rc;
+
 	out_len = 0;
-	rc = sbefifo_operation(sctx, (uint8_t *)msg, 3 * 4, &out, &out_len);
+	rc = sbefifo_operation(sctx, msg, msg_len, &out, &out_len);
+	free(msg);
 	if (rc)
 		return rc;
 
-	if (out_len != 0) {
+	rc = sbefifo_control_insn_pull(out, out_len);
+	if (out)
 		free(out);
-		return EPROTO;
-	}
 
-	return 0;
+	return rc;
 }
diff --git a/libsbefifo/libsbefifo.h b/libsbefifo/libsbefifo.h
index 78082b0..7617f13 100644
--- a/libsbefifo/libsbefifo.h
+++ b/libsbefifo/libsbefifo.h
@@ -149,6 +149,9 @@ int sbefifo_control_trace_array(struct sbefifo_context *sctx, uint16_t target_ty
 #define SBEFIFO_INSN_OP_STEP             0x2
 #define SBEFIFO_INSN_OP_SRESET           0x3
 
+int sbefifo_control_insn_push(uint8_t core_id, uint8_t thread_id, uint8_t thread_op, uint8_t mode, uint8_t **buf, uint32_t *buflen);
+int sbefifo_control_insn_pull(uint8_t *buf, uint32_t buflen);
+
 int sbefifo_control_insn(struct sbefifo_context *sctx, uint8_t core_id, uint8_t thread_id, uint8_t thread_op, uint8_t mode);
 
 int sbefifo_get_ffdc(struct sbefifo_context *sctx, uint8_t **ffdc, uint32_t *ffdc_len);
-- 
2.21.0