libpdbg/adu.c: Implement a basic putmem

Signed-off-by: Alistair Popple <alistair@popple.id.au>

3 files changed, 109 insertions, 0 deletions

diff --git a/libpdbg/adu.c b/libpdbg/adu.c
index 75cd829..4e7211e 100644
--- a/libpdbg/adu.c
+++ b/libpdbg/adu.c
@@ -47,6 +47,7 @@
 /* ALTD_CONTROL_REG fields */
 #define FBC_ALTD_TTYPE		PPC_BITMASK(0, 5)
 #define TTYPE_TREAD 		PPC_BIT(6)
+#define TTYPE_TWRITE 		0
 #define FBC_ALTD_TSIZE		PPC_BITMASK(7, 13)
 #define FBC_ALTD_ADDRESS	PPC_BITMASK(14, 63)
 
@@ -171,7 +172,75 @@ int adu_getmem(struct target *target, uint64_t start_addr, uint8_t *output, uint
 			memcpy(output, &data, 8);
 			output += 8;
 		}
+	}
+
+	adu_unlock(target);
+
+	return rc;
+}
+
+int adu_putmem(struct target *target, uint64_t start_addr, uint8_t *input, uint64_t size)
+{
+	int rc = 0, tsize;
+	uint64_t addr, cmd_reg, ctrl_reg, val, data, end_addr;
+
+	CHECK_ERR(adu_lock(target));
+
+	ctrl_reg = TTYPE_TWRITE;
+	ctrl_reg = SETFIELD(FBC_ALTD_TTYPE, ctrl_reg, TTYPE_DMA_PARTIAL_WRITE);
+
+	CHECK_ERR(read_target(target, ALTD_CMD_REG, &cmd_reg));
+	cmd_reg |= FBC_ALTD_START_OP;
+	cmd_reg = SETFIELD(FBC_ALTD_SCOPE, cmd_reg, SCOPE_SYSTEM);
+	cmd_reg = SETFIELD(FBC_ALTD_DROP_PRIORITY, cmd_reg, DROP_PRIORITY_MEDIUM);
+
+	end_addr = start_addr + size;
+	for (addr = start_addr; addr < end_addr; addr += tsize, input += tsize) {
+		if ((addr % 8) || (addr + 8 > end_addr)) {
+			/* If the address is not 64-bit aligned we
+			 * copy in a byte at a time until it is. */
+			tsize = 1;
+
+			/* Copy the input data in with correct alignment */
+			data = ((uint64_t) *input) << 8*(8 - (addr % 8) - 1);
+		} else {
+			tsize = 8;
+			memcpy(&data, input, sizeof(data));
+			data = __builtin_bswap64(data);
+		}
+		ctrl_reg = SETFIELD(FBC_ALTD_TSIZE, ctrl_reg, tsize);
+
+	retry:
+		/* Clear status bits */
+		CHECK_ERR(adu_reset(target));
 
+		/* Set the address */
+		ctrl_reg = SETFIELD(FBC_ALTD_ADDRESS, ctrl_reg, addr);
+		CHECK_ERR(write_target(target, ALTD_CONTROL_REG, ctrl_reg));
+
+		/* Write the data */
+		CHECK_ERR(write_target(target, ALTD_DATA_REG, data));
+
+		/* Start the command */
+		CHECK_ERR(write_target(target, ALTD_CMD_REG, cmd_reg));
+
+		/* Wait for completion */
+		do {
+			CHECK_ERR(read_target(target, ALTD_STATUS_REG, &val));
+		} while (!val);
+
+		if( !(val & FBC_ALTD_ADDR_DONE) ||
+		    !(val & FBC_ALTD_DATA_DONE)) {
+			/* PBINIT_MISSING is expected occasionally so just retry */
+			if (val & FBC_ALTD_PBINIT_MISSING)
+				goto retry;
+			else {
+				PR_ERROR("Unable to write memory. "	\
+					 "ALTD_STATUS_REG = 0x%016llx\n", val);
+				rc = -1;
+				break;
+			}
+		}
 	}
 
 	adu_unlock(target);
diff --git a/libpdbg/operations.h b/libpdbg/operations.h
index cb20a75..a60a4ea 100644
--- a/libpdbg/operations.h
+++ b/libpdbg/operations.h
@@ -41,6 +41,7 @@
 
 /* Alter display unit functions */
 int adu_getmem(struct target *target, uint64_t addr, uint8_t *output, uint64_t size);
+int adu_putmem(struct target *target, uint64_t start_addr, uint8_t *input, uint64_t size);
 
 /* Functions to ram instructions */
 #define THREAD_STATUS_ACTIVE	PPC_BIT(63)
diff --git a/src/main.c b/src/main.c
index 9289bdf..7134fa6 100644
--- a/src/main.c
+++ b/src/main.c
@@ -113,6 +113,7 @@ static void print_usage(char *pname)
 	printf("\tgetscom <address>\n");
 	printf("\tputscom <address> <value>\n");
 	printf("\tgetmem <address> <count>\n");
+	printf("\tputmem <address>\n");
 	printf("\tgetvmem <virtual address>\n");
 	printf("\tgetgpr <gpr>\n");
 	printf("\tputgpr <gpr> <value>\n");
@@ -144,6 +145,9 @@ enum command parse_cmd(char *optarg)
 	} else if (strcmp(optarg, "getmem") == 0) {
 		cmd = GETMEM;
 		cmd_arg_count = 2;
+	} else if (strcmp(optarg, "putmem") == 0) {
+		cmd = PUTMEM;
+		cmd_arg_count = 1;
 	} else if (strcmp(optarg, "getgpr") == 0) {
 		cmd = GETGPR;
 		cmd_arg_count = 1;
@@ -710,6 +714,36 @@ static int getreg(struct target *thread, uint64_t reg, uint64_t *data)
 	return 0;
 }
 
+#define PUTMEM_BUF_SIZE 1024
+static int putmem(uint64_t addr)
+{
+	uint8_t *buf;
+	int read_size, rc = 0;
+	struct target *target;
+
+	/* It doesn't matter which processor we execute on so
+	 * just use the primary one */
+	if (list_empty(&processors.targets))
+		return 0;
+
+	target = list_entry(processors.targets.n.next, struct target, class_link);
+	buf = malloc(PUTMEM_BUF_SIZE);
+	assert(buf);
+	do {
+		read_size = read(STDIN_FILENO, buf, PUTMEM_BUF_SIZE);
+		if (adu_putmem(target, addr, buf, read_size)) {
+			rc = 0;
+			PR_ERROR("Unable to write memory.\n");
+			break;
+		}
+
+		rc += read_size;
+	} while (read_size > 0);
+
+	free(buf);
+	return rc;
+}
+
 int main(int argc, char *argv[])
 {
 	int rc = 0;
@@ -744,6 +778,7 @@ int main(int argc, char *argv[])
 
 		target = list_entry(processors.targets.n.next, struct target, class_link);
 		buf = malloc(cmd_args[1]);
+		assert(buf);
 		if (!adu_getmem(target, cmd_args[0], buf, cmd_args[1])) {
 			rc = 1;
 			write(STDOUT_FILENO, buf, cmd_args[1]);
@@ -752,6 +787,10 @@ int main(int argc, char *argv[])
 
 		free(buf);
 		break;
+	case PUTMEM:
+		rc = putmem(cmd_args[0]);
+		printf("Wrote %d bytes starting at 0x%016llx\n", rc, cmd_args[0]);
+		break;
 	case GETGPR:
 		rc = for_each_class_call(&threads, NULL, getreg, NULL, cmd_args[0], &value);
 		break;