target/riscv: improve error handling in write_memory_progbuf()

Change-Id: I8a100b686e594855fbf34acf5ccf0e1550f18869
Signed-off-by: Evgeniy Naydanov <[email protected]>

src/target/riscv/batch.c

@@ -230,3 +230,13 @@ size_t riscv_batch_available_scans(struct riscv_batch *batch)
 {
 	return batch->allocated_scans - batch->used_scans - 4;
 }
+
+bool riscv_batch_dmi_busy_encountered(const struct riscv_batch *batch)
+{
+	if (!batch->used_scans)
+		return false;
+	assert(batch->last_scan == RISCV_SCAN_TYPE_NOP);
+	const struct scan_field *field = batch->fields + batch->used_scans - 1;
+	const uint64_t in = buf_get_u64(field->in_value, 0, field->num_bits);
+	return get_field(in, DTM_DMI_OP) == DTM_DMI_OP_BUSY;
+}

src/target/riscv/batch.h

@@ -75,4 +75,6 @@ void riscv_batch_add_nop(struct riscv_batch *batch);
 /* Returns the number of available scans. */
 size_t riscv_batch_available_scans(struct riscv_batch *batch);
 
+bool riscv_batch_dmi_busy_encountered(const struct riscv_batch *batch);
+
 #endif

src/target/riscv/program.c

@@ -99,6 +99,23 @@ int riscv_program_sbr(struct riscv_program *p, enum gdb_regno d, enum gdb_regno
 	return riscv_program_insert(p, sb(d, b, offset));
 }
 
+int riscv_program_store(struct riscv_program *p, enum gdb_regno d, enum gdb_regno b, int offset,
+		unsigned int size)
+{
+	switch (size) {
+	case 1:
+		return riscv_program_sbr(p, d, b, offset);
+	case 2:
+		return riscv_program_shr(p, d, b, offset);
+	case 4:
+		return riscv_program_swr(p, d, b, offset);
+	case 8:
+		return riscv_program_sdr(p, d, b, offset);
+	}
+	assert(false && "Unsupported size");
+	return ERROR_FAIL;
+}
+
 int riscv_program_ldr(struct riscv_program *p, enum gdb_regno d, enum gdb_regno b, int offset)
 {
 	return riscv_program_insert(p, ld(d, b, offset));

src/target/riscv/program.h

@@ -58,6 +58,8 @@ int riscv_program_sdr(struct riscv_program *p, enum gdb_regno s, enum gdb_regno
 int riscv_program_swr(struct riscv_program *p, enum gdb_regno s, enum gdb_regno a, int o);
 int riscv_program_shr(struct riscv_program *p, enum gdb_regno s, enum gdb_regno a, int o);
 int riscv_program_sbr(struct riscv_program *p, enum gdb_regno s, enum gdb_regno a, int o);
+int riscv_program_store(struct riscv_program *p, enum gdb_regno d, enum gdb_regno b, int o,
+		unsigned int s);
 
 int riscv_program_csrrsi(struct riscv_program *p, enum gdb_regno d, unsigned int z, enum gdb_regno csr);
 int riscv_program_csrrci(struct riscv_program *p, enum gdb_regno d, unsigned int z, enum gdb_regno csr);