Blind implementation of write_memory.

src/target/riscv/opcodes.h

@@ -1,6 +1,7 @@
 #include "encoding.h"
 
 #define ZERO	0
+#define T0      5
 #define S0      8
 #define S1      9
 
@@ -36,6 +37,24 @@ static uint32_t sw(unsigned int src, unsigned int base, uint16_t offset)
     MATCH_SW;
 }
 
+static uint32_t sh(unsigned int src, unsigned int base, uint16_t offset)
+{
+  return (bits(offset, 11, 5) << 25) |
+    (src << 20) |
+    (base << 15) |
+    (bits(offset, 4, 0) << 7) |
+    MATCH_SH;
+}
+
+static uint32_t sb(unsigned int src, unsigned int base, uint16_t offset)
+{
+  return (bits(offset, 11, 5) << 25) |
+    (src << 20) |
+    (base << 15) |
+    (bits(offset, 4, 0) << 7) |
+    MATCH_SB;
+}
+
 static uint32_t lw(unsigned int rd, unsigned int base, uint16_t offset)
 {
   return (bits(offset, 11, 0) << 20) |
@@ -86,6 +105,14 @@ static uint32_t csrw(unsigned int source, unsigned int csr) {
   return (csr << 20) | (source << 15) | MATCH_CSRRW;
 }
 
+static uint32_t addi(unsigned int dest, unsigned int src, uint16_t imm)
+{
+  return (bits(imm, 11, 0) << 20) |
+    (src << 15) |
+    (dest << 7) |
+    MATCH_ADDI;
+}
+
 /*
 static uint32_t csrr(unsigned int rd, unsigned int csr) {
   return (csr << 20) | (rd << 7) | MATCH_CSRRS;
@@ -96,24 +123,6 @@ static uint32_t fence_i(void)
   return MATCH_FENCE_I;
 }
 
-static uint32_t sb(unsigned int src, unsigned int base, uint16_t offset)
-{
-  return (bits(offset, 11, 5) << 25) |
-    (src << 20) |
-    (base << 15) |
-    (bits(offset, 4, 0) << 7) |
-    MATCH_SB;
-}
-
-static uint32_t sh(unsigned int src, unsigned int base, uint16_t offset)
-{
-  return (bits(offset, 11, 5) << 25) |
-    (src << 20) |
-    (base << 15) |
-    (bits(offset, 4, 0) << 7) |
-    MATCH_SH;
-}
-
 static uint32_t sd(unsigned int src, unsigned int base, uint16_t offset)
 {
   return (bits(offset, 11, 5) << 25) |
@@ -149,14 +158,6 @@ static uint32_t fld(unsigned int src, unsigned int base, uint16_t offset)
     MATCH_FLD;
 }
 
-static uint32_t addi(unsigned int dest, unsigned int src, uint16_t imm)
-{
-  return (bits(imm, 11, 0) << 20) |
-    (src << 15) |
-    (dest << 7) |
-    MATCH_ADDI;
-}
-
 static uint32_t ori(unsigned int dest, unsigned int src, uint16_t imm)
 {
   return (bits(imm, 11, 0) << 20) |

src/target/riscv/riscv.c

@@ -541,7 +541,7 @@ static int riscv_read_memory(struct target *target, uint32_t address,
 			dram_write32(target, 2, sw(S1, ZERO, DEBUG_RAM_START + 16), false);
 			break;
 		default:
-			LOG_ERROR("Unsupported size for read_memory: %d", size);
+			LOG_ERROR("Unsupported size: %d", size);
 			return ERROR_FAIL;
 	}
 	dram_write_jump(target, 3, false);
@@ -587,6 +587,82 @@ static int riscv_read_memory(struct target *target, uint32_t address,
 	return ERROR_OK;
 }
 
+static int riscv_write_memory(struct target *target, uint32_t address,
+		uint32_t size, uint32_t count, const uint8_t *buffer)
+{
+	// TODO: save/restore T0
+
+	// Set up the address.
+	dram_write32(target, 0, lw(T0, ZERO, DEBUG_RAM_START + 16), false);
+	dram_write_jump(target, 1, false);
+	dram_write32(target, 4, address, true);
+
+	if (wait_for_debugint_clear(target) != ERROR_OK) {
+		LOG_ERROR("Debug interrupt didn't clear.");
+		return ERROR_FAIL;
+	}
+
+	switch (size) {
+		case 1:
+			dram_write32(target, 0, lb(S0, ZERO, DEBUG_RAM_START + 16), false);
+			dram_write32(target, 1, sb(S0, T0, 0), false);
+			break;
+		case 2:
+			dram_write32(target, 0, lh(S0, ZERO, DEBUG_RAM_START + 16), false);
+			dram_write32(target, 1, sh(S0, T0, 0), false);
+			break;
+		case 4:
+			dram_write32(target, 0, lw(S0, ZERO, DEBUG_RAM_START + 16), false);
+			dram_write32(target, 1, sw(S0, T0, 0), false);
+			break;
+		default:
+			LOG_ERROR("Unsupported size: %d", size);
+			return ERROR_FAIL;
+	}
+	dram_write32(target, 2, addi(T0, T0, size), false);
+	dram_write_jump(target, 3, false);
+
+	uint32_t i = 0;
+	while (i < count) {
+		// Write the next value and set interrupt.
+		uint32_t value;
+		uint32_t offset = size * i;
+		switch (size) {
+			case 1:
+				value = buffer[offset];
+				break;
+			case 2:
+				value = buffer[offset] |
+					(buffer[offset+1] << 8);
+				break;
+			case 4:
+				value = buffer[offset] |
+					((uint32_t) buffer[offset+1] << 8) |
+					((uint32_t) buffer[offset+2] << 16) |
+					((uint32_t) buffer[offset+3] << 24);
+				break;
+			default:
+				return ERROR_FAIL;
+		}
+
+		dbus_status_t status = dbus_scan(target, NULL, DBUS_OP_CONDITIONAL_WRITE,
+				4, DMCONTROL_HALTNOT | DMCONTROL_INTERRUPT | value);
+		if (status == DBUS_STATUS_SUCCESS) {
+			i++;
+		} else if (status == DBUS_STATUS_NO_WRITE) {
+			// Need to retry the access that failed, which was the previous one.
+			i--;
+		} else if (status == DBUS_STATUS_BUSY) {
+			// This operation may still complete. Retry the current access.
+		} else if (status == DBUS_STATUS_FAILED) {
+			LOG_ERROR("dbus write failed!");
+			return ERROR_FAIL;
+		}
+	}
+
+	return ERROR_OK;
+}
+
 struct target_type riscv_target = {
 	.name = "riscv",
 
@@ -605,7 +681,7 @@ struct target_type riscv_target = {
 	.deassert_reset = riscv_deassert_reset,
 
 	.read_memory = riscv_read_memory,
+	.write_memory = riscv_write_memory,
 
-	/* TODO: */
+	//.get_gdb_reg_list = riscv_get_gdb_reg_list,
-	/* .virt2phys = riscv_virt2phys, */
 };