[Pdbg] [PATCH v2 10/24] libsbefifo: Refactor protocol marshalling for register chip-ops

[Amitay Isaacs]

Signed-off-by: Amitay Isaacs <amitay at ozlabs.org>
---
 libsbefifo/cmd_register.c | 116 +++++++++++++++++++++++++++-----------
 1 file changed, 84 insertions(+), 32 deletions(-)

diff --git a/libsbefifo/cmd_register.c b/libsbefifo/cmd_register.c
index ba4815b..8737e7f 100644
--- a/libsbefifo/cmd_register.c
+++ b/libsbefifo/cmd_register.c
@@ -22,77 +22,107 @@
 #include "libsbefifo.h"
 #include "sbefifo_private.h"
 
-int sbefifo_register_get(struct sbefifo_context *sctx, uint8_t core_id, uint8_t thread_id, uint8_t reg_type, uint8_t *reg_id, uint8_t reg_count, uint64_t **value)
+static int sbefifo_register_get_push(uint8_t core_id, uint8_t thread_id, uint8_t reg_type, uint8_t *reg_id, uint8_t reg_count, uint8_t **buf, uint32_t *buflen)
 {
-	uint8_t *out;
-	uint32_t msg[3+reg_count];
-	uint32_t cmd, out_len;
+	uint32_t *msg;
+	uint32_t nwords, cmd;
 	uint32_t r, i;
-	int rc;
 
 	if (reg_count == 0 || reg_count > 64)
 		return EINVAL;
 
+	nwords = 3 + reg_count;
+	*buflen = nwords * sizeof(uint32_t);
+	msg = malloc(*buflen);
+	if (!msg)
+		return ENOMEM;
+
 	cmd = SBEFIFO_CMD_CLASS_REGISTER | SBEFIFO_CMD_GET_REGISTER;
+
 	r = ((uint32_t)(core_id & 0xff) << 16) |
 	    ((uint32_t)(thread_id & 0x3) << 12) |
 	    ((uint32_t)(reg_type & 0x3) << 8) |
 	    ((uint32_t)(reg_count & 0xff));
 
-	msg[0] = htobe32(3+reg_count);	// number of words
+	msg[0] = htobe32(nwords);
 	msg[1] = htobe32(cmd);
 	msg[2] = htobe32(r);
-	for (i=0; i<reg_count; i++) {
+	for (i=0; i<reg_count; i++)
 		msg[3+i] = htobe32(reg_id[i] & 0xff);
-	}
 
-	out_len = reg_count * 8;
-	rc = sbefifo_operation(sctx, (uint8_t *)msg, (3+reg_count) * 4, &out, &out_len);
-	if (rc)
-		return rc;
+	*buf = (uint8_t *)msg;
+	return 0;
+}
 
-	if (out_len != reg_count * 8) {
-		free(out);
+static int sbefifo_register_get_pull(uint8_t *buf, uint32_t buflen, uint8_t reg_count, uint64_t **value)
+{
+	uint32_t i;
+
+	if (buflen != reg_count * 8)
 		return EPROTO;
-	}
 
 	*value = malloc(reg_count * 8);
-	if (! *value) {
-		free(out);
+	if (! *value)
 		return ENOMEM;
-	}
 
 	for (i=0; i<reg_count; i++) {
 		uint32_t val1, val2;
 
-		val1 = be32toh(*(uint32_t *) &out[i*4]);
-		val2 = be32toh(*(uint32_t *) &out[i*4+4]);
+		val1 = be32toh(*(uint32_t *) &buf[i*4]);
+		val2 = be32toh(*(uint32_t *) &buf[i*4+4]);
 
 		(*value)[i] = ((uint64_t)val1 << 32) | (uint64_t)val2;
 	}
 
-	free(out);
 	return 0;
 }
 
-int sbefifo_register_put(struct sbefifo_context *sctx, uint8_t core_id, uint8_t thread_id, uint8_t reg_type, uint8_t *reg_id, uint8_t reg_count, uint64_t *value)
+int sbefifo_register_get(struct sbefifo_context *sctx, uint8_t core_id, uint8_t thread_id, uint8_t reg_type, uint8_t *reg_id, uint8_t reg_count, uint64_t **value)
 {
-	uint8_t *out;
-	uint32_t msg[3+(3*reg_count)];
-	uint32_t cmd, out_len;
-	uint32_t r, i;
+	uint8_t *msg, *out;
+	uint32_t msg_len, out_len;
 	int rc;
 
+	rc = sbefifo_register_get_push(core_id, thread_id, reg_type, reg_id, reg_count, &msg, &msg_len);
+	if (rc)
+		return rc;
+
+	out_len = reg_count * 8;
+	rc = sbefifo_operation(sctx, msg, msg_len, &out, &out_len);
+	free(msg);
+	if (rc)
+		return rc;
+
+	rc = sbefifo_register_get_pull(out, out_len, reg_count, value);
+	if (out)
+		free(out);
+
+	return rc;
+}
+
+static int sbefifo_register_put_push(uint8_t core_id, uint8_t thread_id, uint8_t reg_type, uint8_t *reg_id, uint8_t reg_count, uint64_t *value, uint8_t **buf, uint32_t *buflen)
+{
+	uint32_t *msg;
+	uint32_t nwords, cmd;
+	uint32_t r, i;
+
 	if (reg_count == 0 || reg_count > 64)
 		return EINVAL;
 
+	nwords = 3 + 3*reg_count;
+	*buflen = nwords * sizeof(uint32_t);
+	msg = malloc(*buflen);
+	if (!msg)
+		return ENOMEM;
+
 	cmd = SBEFIFO_CMD_CLASS_REGISTER | SBEFIFO_CMD_PUT_REGISTER;
+
 	r = ((uint32_t)(core_id & 0xff) << 16) |
 	    ((uint32_t)(thread_id & 0x3) << 12) |
 	    ((uint32_t)(reg_type & 0x3) << 8) |
 	    ((uint32_t)(reg_count & 0xff));
 
-	msg[0] = htobe32(3 + 3*reg_count);	// number of words
+	msg[0] = htobe32(nwords);
 	msg[1] = htobe32(cmd);
 	msg[2] = htobe32(r);
 	for (i=0; i<reg_count; i++) {
@@ -101,15 +131,37 @@ int sbefifo_register_put(struct sbefifo_context *sctx, uint8_t core_id, uint8_t
 		msg[3+i*3+2] = htobe32(value[i] & 0xffffffff);
 	}
 
+	*buf = (uint8_t *)msg;
+	return 0;
+}
+
+static int sbefifo_register_put_pull(uint8_t *buf, uint32_t buflen)
+{
+	if (buflen != 0)
+		return EPROTO;
+
+	return 0;
+}
+
+int sbefifo_register_put(struct sbefifo_context *sctx, uint8_t core_id, uint8_t thread_id, uint8_t reg_type, uint8_t *reg_id, uint8_t reg_count, uint64_t *value)
+{
+	uint8_t *msg, *out;
+	uint32_t msg_len, out_len;
+	int rc;
+
+	rc = sbefifo_register_put_push(core_id, thread_id, reg_type, reg_id, reg_count, value, &msg, &msg_len);
+	if (rc)
+		return rc;
+
 	out_len = 0;
-	rc = sbefifo_operation(sctx, (uint8_t *)msg, (3+3*reg_count) * 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_register_put_pull(out, out_len);
+	if (out)
 		free(out);
-		return EPROTO;
-	}
 
-	return 0;
+	return rc;
 }
-- 
2.21.0