Merge pull request #1029 from MrAlexei/add_decode_wp_rvc

Add functions to decode RVC load and store instructions for watchpoints

1 files changed, 467 insertions, 30 deletions

diff --git a/src/target/riscv/riscv.c b/src/target/riscv/riscv.c
index d610b33..26fd365 100644
--- a/src/target/riscv/riscv.c
+++ b/src/target/riscv/riscv.c
@@ -1617,6 +1617,440 @@ static int riscv_hit_trigger_hit_bit(struct target *target, uint32_t *unique_id)
 	return ERROR_OK;
 }
 
+/**
+ * These functions are needed to extract individual bits (for offset)
+ * from the instruction
+ */
+// c.lwsp rd_n0 c_uimm8sphi c_uimm8splo - offset[5] offset[4:2|7:6]
+static uint16_t get_offset_clwsp(riscv_insn_t instruction)
+{
+	uint16_t offset_4to2and7to6_bits =
+		get_field32(instruction, INSN_FIELD_C_UIMM8SPLO);
+	uint16_t offset_4to2_bits = offset_4to2and7to6_bits >> 2;
+	uint16_t offset_7to6_bits = offset_4to2and7to6_bits & 0x3;
+	uint16_t offset_5_bit = get_field32(instruction, INSN_FIELD_C_UIMM8SPHI);
+	return (offset_4to2_bits << 2) + (offset_5_bit << 5)
+		   + (offset_7to6_bits << 6);
+}
+
+// c.ldsp rd_n0 c_uimm9sphi c_uimm9splo - offset[5] offset[4:3|8:6]
+static uint16_t get_offset_cldsp(riscv_insn_t instruction)
+{
+	uint16_t offset_4to3and8to6_bits =
+		get_field32(instruction, INSN_FIELD_C_UIMM9SPLO);
+	uint16_t offset_4to3_bits = offset_4to3and8to6_bits >> 3;
+	uint16_t offset_8to6_bits = offset_4to3and8to6_bits & 0x7;
+	uint16_t offset_5_bit = get_field32(instruction, INSN_FIELD_C_UIMM9SPHI);
+	return (offset_4to3_bits << 3) + (offset_5_bit << 5)
+		   + (offset_8to6_bits << 6);
+}
+
+// c.swsp c_rs2 c_uimm8sp_s - offset[5:2|7:6]
+static uint16_t get_offset_cswsp(riscv_insn_t instruction)
+{
+	uint16_t offset_5to2and7to6_bits =
+		get_field32(instruction, INSN_FIELD_C_UIMM8SP_S);
+	uint16_t offset_5to2_bits = offset_5to2and7to6_bits >> 2;
+	uint16_t offset_7to6_bits = offset_5to2and7to6_bits & 0x3;
+	return (offset_5to2_bits << 2) + (offset_7to6_bits << 6);
+}
+
+// c.sdsp c_rs2 c_uimm9sp_s - offset[5:3|8:6]
+static uint16_t get_offset_csdsp(riscv_insn_t instruction)
+{
+	uint16_t offset_5to3and8to6_bits =
+		get_field32(instruction, INSN_FIELD_C_UIMM9SP_S);
+	uint16_t offset_5to3_bits = offset_5to3and8to6_bits >> 3;
+	uint16_t offset_8to6_bits = offset_5to3and8to6_bits & 0x7;
+	return (offset_5to3_bits << 3) + (offset_8to6_bits << 6);
+}
+
+// c.lw rd_p rs1_p c_uimm7lo c_uimm7hi - offset[2|6] offset[5:3]
+static uint16_t get_offset_clw(riscv_insn_t instruction)
+{
+	uint16_t offset_2and6_bits = get_field32(instruction, INSN_FIELD_C_UIMM7LO);
+	uint16_t offset_2_bit = offset_2and6_bits >> 1;
+	uint16_t offset_6_bit = offset_2and6_bits & 0x1;
+	uint16_t offset_5to3_bits = get_field32(instruction, INSN_FIELD_C_UIMM7HI);
+	return (offset_2_bit << 2) + (offset_5to3_bits << 3) + (offset_6_bit << 6);
+}
+
+// c.ld rd_p rs1_p c_uimm8lo c_uimm8hi - offset[7:6] offset[5:3]
+static uint16_t get_offset_cld(riscv_insn_t instruction)
+{
+	uint16_t offset_7to6_bits = get_field32(instruction, INSN_FIELD_C_UIMM8LO);
+	uint16_t offset_5to3_bits = get_field32(instruction, INSN_FIELD_C_UIMM8HI);
+	return (offset_5to3_bits << 3) + (offset_7to6_bits << 6);
+}
+
+// c.lq rd_p rs1_p c_uimm9lo c_uimm9hi - offset[7:6] offset[5|4|8]
+static uint16_t get_offset_clq(riscv_insn_t instruction)
+{
+	uint16_t offset_7to6_bits = get_field32(instruction, INSN_FIELD_C_UIMM9LO);
+	uint16_t offset_5to4and8_bits =
+		get_field32(instruction, INSN_FIELD_C_UIMM9HI);
+	uint16_t offset_5to4_bits = offset_5to4and8_bits >> 1;
+	uint16_t offset_8_bit = offset_5to4and8_bits & 0x1;
+	return (offset_5to4_bits << 4) + (offset_7to6_bits << 6)
+		   + (offset_8_bit << 8);
+}
+
+// c.lqsp rd_n0 c_uimm10sphi c_uimm10splo - offset[5] offset[4|9:6]
+static uint16_t get_offset_clqsp(riscv_insn_t instruction)
+{
+	uint16_t offset_4and9to6_bits =
+		get_field32(instruction, INSN_FIELD_C_UIMM10SPLO);
+	uint16_t offset_4_bit = offset_4and9to6_bits >> 4;
+	uint16_t offset_9to6_bits = offset_4and9to6_bits & 0xf;
+	uint16_t offset_5_bit = get_field32(instruction, INSN_FIELD_C_UIMM10SPHI);
+	return (offset_4_bit << 4) + (offset_5_bit << 5) + (offset_9to6_bits << 6);
+}
+
+// c.sqsp c_rs2 c_uimm10sp_s - offset[5:4|9:6]
+static uint16_t get_offset_csqsp(riscv_insn_t instruction)
+{
+	uint16_t offset_5to4and9to6_bits =
+		get_field32(instruction, INSN_FIELD_C_UIMM10SP_S);
+	uint16_t offset_5to4_biits = offset_5to4and9to6_bits >> 4;
+	uint16_t offset_9to6_bits = offset_5to4and9to6_bits & 0xf;
+	return (offset_5to4_biits << 4) + (offset_9to6_bits << 6);
+}
+
+/**
+ * Decode rs1' register num for RVC.
+ * See "Table: Registers specified by the three-bit rs1′, rs2′, and rd′ fields
+ * of the CIW, CL, CS, CA, and CB formats" in "The RISC-V Instruction Set Manual
+ * Volume I: Unprivileged ISA".
+ * */
+static uint32_t get_rs1_c(riscv_insn_t instruction)
+{
+	return GDB_REGNO_S0 + get_field32(instruction, INSN_FIELD_C_SREG1);
+}
+
+static uint32_t get_opcode(const riscv_insn_t instruction)
+{
+	// opcode is first 7 bits of the instruction
+	uint32_t opcode = instruction & INSN_FIELD_OPCODE;
+	if ((instruction & 0x03) < 0x03) { // opcode size RVC
+		// RVC MASK_C = 0xe003 for load/store instructions
+		opcode = instruction & MASK_C_LD;
+	}
+	return opcode;
+}
+
+static int get_loadstore_membase_regno(struct target *target,
+		const riscv_insn_t instruction, int *regid)
+{
+	uint32_t opcode = get_opcode(instruction);
+	int rs;
+
+	switch (opcode) {
+	case MATCH_LB:
+	case MATCH_FLH & ~INSN_FIELD_FUNCT3:
+	case MATCH_SB:
+	case MATCH_FSH & ~INSN_FIELD_FUNCT3:
+		rs = get_field32(instruction, INSN_FIELD_RS1);
+		break;
+
+	case MATCH_C_LWSP:
+	case MATCH_C_LDSP: // if xlen >= 64 or MATCH_C_FLWSP:
+	case MATCH_C_FLDSP: // or MATCH_C_LQSP if xlen == 128
+	case MATCH_C_SWSP:
+	case MATCH_C_SDSP: // if xlen >= 64 or MATCH_C_FSWSP:
+	case MATCH_C_FSDSP: // or MATCH_C_SQSP if xlen == 128
+		rs = GDB_REGNO_SP;
+		break;
+
+	case MATCH_C_LW:
+	case MATCH_C_FLW: // or MATCH_C_LD if xlen >= 64
+	case MATCH_C_FLD: // or MATCH_C_LQ if xlen == 128
+	case MATCH_C_SW:
+	case MATCH_C_FSW: // or MATCH_C_SD if xlen >= 64
+	case MATCH_C_FSD: // or MATCH_C_SQ if xlen == 128
+		rs = get_rs1_c(instruction);
+		break;
+
+	default:
+		LOG_TARGET_DEBUG(target, "0x%" PRIx32 " is not a RV32I or \"C\" load or"
+						 " store", instruction);
+		return ERROR_FAIL;
+	}
+	*regid = rs;
+	return ERROR_OK;
+}
+
+static int get_loadstore_memoffset(struct target *target,
+		const riscv_insn_t instruction, int16_t *memoffset)
+{
+	uint32_t opcode = get_opcode(instruction);
+	int16_t offset;
+
+	switch (opcode) {
+	case MATCH_LB:
+	case MATCH_FLH & ~INSN_FIELD_FUNCT3:
+	case MATCH_SB:
+	case MATCH_FSH & ~INSN_FIELD_FUNCT3:
+		if (opcode == MATCH_SB || opcode == (MATCH_FSH & ~INSN_FIELD_FUNCT3)) {
+			offset = get_field32(instruction, INSN_FIELD_IMM12LO) |
+				  (get_field32(instruction, INSN_FIELD_IMM12HI) << 5);
+		} else if (opcode == MATCH_LB ||
+				   opcode == (MATCH_FLH & ~INSN_FIELD_FUNCT3)) {
+			offset = get_field32(instruction, INSN_FIELD_IMM12);
+		} else {
+			assert(false);
+		}
+		/* sign extend 12-bit imm to 16-bits */
+		if (offset & (1 << 11))
+			offset |= 0xf000;
+		break;
+
+	case MATCH_C_LWSP:
+		offset = get_offset_clwsp(instruction);
+		break;
+
+	case MATCH_C_LDSP: // if xlen >= 64 or MATCH_C_FLWSP:
+		if (riscv_xlen(target) > 32) { // MATCH_C_LDSP
+			offset = get_offset_cldsp(instruction);
+		} else { // MATCH_C_FLWSP
+			offset = get_offset_clwsp(instruction);
+		}
+		break;
+
+	case MATCH_C_FLDSP: // or MATCH_C_LQSP if xlen == 128
+		if (riscv_xlen(target) == 128) { // MATCH_C_LQSP
+			offset = get_offset_clqsp(instruction);
+		} else { // MATCH_C_FLDSP
+			offset = get_offset_cldsp(instruction);
+		}
+		break;
+
+	case MATCH_C_SWSP:
+		offset = get_offset_cswsp(instruction);
+		break;
+
+	case MATCH_C_SDSP: // if xlen >= 64 or MATCH_C_FSWSP:
+		if (riscv_xlen(target) > 32) { // MATCH_C_SDSP
+			offset = get_offset_csdsp(instruction);
+		} else { // MATCH_C_FSWSP
+			offset = get_offset_cswsp(instruction);
+		}
+		break;
+
+	case MATCH_C_FSDSP: // or MATCH_C_SQSP if xlen == 128
+		if (riscv_xlen(target) == 128) { // MATCH_C_SQSP
+			offset = get_offset_csqsp(instruction);
+		} else { // MATCH_C_FSDSP
+			offset = get_offset_csdsp(instruction); // same as C.SDSP
+		}
+		break;
+
+	case MATCH_C_LW:
+		offset = get_offset_clw(instruction);
+		break;
+
+	case MATCH_C_FLW: // or MATCH_C_LD if xlen >= 64
+		if (riscv_xlen(target) > 32) { // MATCH_C_LD
+			offset = get_offset_cld(instruction);
+		} else { // MATCH_C_FLW
+			offset = get_offset_clw(instruction); // same as C.FLW
+		}
+		break;
+
+	case MATCH_C_FLD: // or MATCH_C_LQ if xlen == 128
+		if (riscv_xlen(target) == 128) { // MATCH_C_LQ
+			offset = get_offset_clq(instruction);
+		} else { // MATCH_C_FLD
+			offset = get_offset_cld(instruction); // same as C.LD
+		}
+		break;
+
+	case MATCH_C_SW:
+		offset = get_offset_clw(instruction); // same as C.LW
+		break;
+
+	case MATCH_C_FSW: // or MATCH_C_SD if xlen >= 64
+		if (riscv_xlen(target) > 32) { // MATCH_C_SD
+			offset = get_offset_cld(instruction); // same as C.LD
+		} else { // MATCH_C_FSW
+			offset = get_offset_clw(instruction); // same as C.LW
+		}
+		break;
+
+	case MATCH_C_FSD: // or MATCH_C_SQ if xlen == 128
+		if (riscv_xlen(target) == 128) { // MATCH_C_SQ
+			offset = get_offset_clq(instruction); // same as C.LQ
+		} else { // MATCH_C_FSD
+			offset = get_offset_cld(instruction); // same as C.LD
+		}
+		break;
+
+	default:
+		LOG_TARGET_DEBUG(target, "0x%" PRIx32 " is not a RV32I or \"C\" load or"
+						 " store", instruction);
+		return ERROR_FAIL;
+	}
+	*memoffset = offset;
+	return ERROR_OK;
+}
+
+static int verify_loadstore(struct target *target,
+		const riscv_insn_t instruction, bool *is_read)
+{
+	uint32_t opcode = get_opcode(instruction);
+	bool misa_f = riscv_supports_extension(target, 'F');
+	bool misa_d = riscv_supports_extension(target, 'D');
+	enum watchpoint_rw rw;
+
+	switch (opcode) {
+	case MATCH_LB:
+	case MATCH_FLH & ~INSN_FIELD_FUNCT3:
+		rw = WPT_READ;
+		break;
+
+	case MATCH_SB:
+	case MATCH_FSH & ~INSN_FIELD_FUNCT3:
+		rw = WPT_WRITE;
+		break;
+
+	case MATCH_C_LWSP:
+		if (get_field32(instruction, INSN_FIELD_RD) == 0) {
+			LOG_TARGET_DEBUG(target,
+				"The code points with rd = x0 are reserved for C.LWSP");
+			return ERROR_FAIL;
+		}
+		rw = WPT_READ;
+		break;
+
+	case MATCH_C_LDSP: // if xlen >= 64 or MATCH_C_FLWSP:
+		if (riscv_xlen(target) > 32) { // MATCH_C_LDSP
+			if (get_field32(instruction, INSN_FIELD_RD) == 0) {
+				LOG_TARGET_DEBUG(target,
+					"The code points with rd = x0 are reserved for C.LDSP");
+				return ERROR_FAIL;
+			}
+		} else { // MATCH_C_FLWSP
+			if (!misa_f) {
+				LOG_TARGET_DEBUG(target, "Matched C.FLWSP but target doesn\'t "
+								 "have the \"F\" extension");
+				return ERROR_FAIL;
+			}
+		}
+		rw = WPT_READ;
+		break;
+
+	case MATCH_C_FLDSP: // or MATCH_C_LQSP if xlen == 128
+		if (riscv_xlen(target) == 128) { // MATCH_C_LQSP
+			if (get_field32(instruction, INSN_FIELD_RD) == 0) {
+				LOG_TARGET_DEBUG(target,
+					"The code points with rd = x0 are reserved for C.LQSP");
+				return ERROR_FAIL;
+			}
+		} else { // MATCH_C_FLDSP
+			if (!misa_d) {
+				LOG_TARGET_DEBUG(target, "Matched C.FLDSP but target doesn\'t "
+								 "have the \"D\" extension");
+				return ERROR_FAIL;
+			}
+		}
+		rw = WPT_READ;
+		break;
+
+	case MATCH_C_SWSP:
+		rw = WPT_WRITE;
+		break;
+
+	case MATCH_C_SDSP: // if xlen >= 64 or MATCH_C_FSWSP:
+		if (riscv_xlen(target) == 32) { // MATCH_C_FSWSP
+			if (!misa_f) {
+				LOG_TARGET_DEBUG(target, "Matched C.FSWSP but target doesn\'t "
+								 "have the \"F\" extension");
+				return ERROR_FAIL;
+			}
+		}
+		rw = WPT_WRITE;
+		break;
+
+	case MATCH_C_FSDSP: // or MATCH_C_SQSP if xlen == 128
+		if (riscv_xlen(target) != 128) { // MATCH_C_SQSP
+			if (!misa_d) {
+				LOG_TARGET_DEBUG(target, "Matched C.FSDSP but target doesn\'t "
+								 "have the \"D\" extension");
+				return ERROR_FAIL;
+			}
+		}
+		rw = WPT_WRITE;
+		break;
+
+	case MATCH_C_LW:
+		rw = WPT_READ;
+		break;
+
+	case MATCH_C_FLW: // or MATCH_C_LD if xlen >= 64
+		if (riscv_xlen(target) == 32) { // MATCH_C_FLW
+			if (!misa_f) {
+				LOG_TARGET_DEBUG(target, "Matched C.FLW but target doesn\'t "
+								 "have the \"F\" extension");
+				return ERROR_FAIL;
+			}
+		}
+		rw = WPT_READ;
+		break;
+
+	case MATCH_C_FLD: // or MATCH_C_LQ if xlen == 128
+		if (riscv_xlen(target) != 128) { // MATCH_C_FLD
+			if (!misa_d) {
+				LOG_TARGET_DEBUG(target, "Matched C.FLD but target doesn\'t "
+								 "have the \"D\" extension");
+				return ERROR_FAIL;
+			}
+		}
+		rw = WPT_READ;
+		break;
+
+	case MATCH_C_SW:
+		rw = WPT_WRITE;
+		break;
+
+	case MATCH_C_FSW: // or MATCH_C_SD if xlen >= 64
+		if (riscv_xlen(target) == 32) { // MATCH_C_FSW
+			if (!misa_f) {
+				LOG_TARGET_DEBUG(target, "Matched C.FSW but target doesn\'t "
+								 "have the \"F\" extension");
+				return ERROR_FAIL;
+			}
+		}
+		rw = WPT_WRITE;
+		break;
+
+	case MATCH_C_FSD: // or MATCH_C_SQ if xlen == 128
+		if (riscv_xlen(target) != 128) { // MATCH_C_FSD
+			if (!misa_d) {
+				LOG_TARGET_DEBUG(target, "Matched C.FSD but target doesn\'t "
+								 "have the \"D\" extension");
+				return ERROR_FAIL;
+			}
+		}
+		rw = WPT_WRITE;
+		break;
+
+	default:
+		LOG_TARGET_DEBUG(target, "0x%" PRIx32 " is not a RV32I or \"C\" load or"
+						 " store", instruction);
+		return ERROR_FAIL;
+	}
+
+	if (rw == WPT_WRITE) {
+		*is_read = false;
+		LOG_TARGET_DEBUG(target, "0x%" PRIx32 " is store instruction",
+						 instruction);
+	} else {
+		*is_read = true;
+		LOG_TARGET_DEBUG(target, "0x%" PRIx32 " is load instruction",
+						 instruction);
+	}
+	return ERROR_OK;
+}
+
 /* Sets *hit_watchpoint to the first watchpoint identified as causing the
  * current halt.
  *
@@ -1639,18 +2073,20 @@ static int riscv_hit_watchpoint(struct target *target, struct watchpoint **hit_w
 	}
 
 	riscv_reg_t dpc;
-	riscv_get_register(target, &dpc, GDB_REGNO_DPC);
+	if (riscv_get_register(target, &dpc, GDB_REGNO_DPC) != ERROR_OK)
+		return ERROR_FAIL;
 	const uint8_t length = 4;
 	LOG_TARGET_DEBUG(target, "dpc is 0x%" PRIx64, dpc);
 
 	/* fetch the instruction at dpc */
 	uint8_t buffer[length];
 	if (target_read_buffer(target, dpc, length, buffer) != ERROR_OK) {
-		LOG_TARGET_ERROR(target, "Failed to read instruction at dpc 0x%" PRIx64, dpc);
+		LOG_TARGET_ERROR(target, "Failed to read instruction at dpc 0x%" PRIx64,
+						 dpc);
 		return ERROR_FAIL;
 	}
 
-	uint32_t instruction = 0;
+	riscv_insn_t instruction = 0;
 
 	for (int i = 0; i < length; i++) {
 		LOG_TARGET_DEBUG(target, "Next byte is %x", buffer[i]);
@@ -1658,40 +2094,39 @@ static int riscv_hit_watchpoint(struct target *target, struct watchpoint **hit_w
 	}
 	LOG_TARGET_DEBUG(target, "Full instruction is %x", instruction);
 
-	/* find out which memory address is accessed by the instruction at dpc */
-	/* opcode is first 7 bits of the instruction */
-	uint8_t opcode = instruction & 0x7F;
-	uint32_t rs1;
-	int16_t imm;
-	riscv_reg_t mem_addr;
+	int rs;
+	target_addr_t mem_addr;
+	int16_t memoffset;
+
+	if (get_loadstore_membase_regno(target, instruction, &rs) != ERROR_OK)
+		return ERROR_FAIL;
+	if (riscv_get_register(target, &mem_addr, rs) != ERROR_OK)
+		return ERROR_FAIL;
+	if (get_loadstore_memoffset(target, instruction, &memoffset) != ERROR_OK)
+		return ERROR_FAIL;
 
-	if (opcode == MATCH_LB || opcode == MATCH_SB) {
-		rs1 = (instruction & 0xf8000) >> 15;
-		riscv_get_register(target, &mem_addr, rs1);
+	mem_addr += memoffset;
+	bool is_load;
 
-		if (opcode == MATCH_SB) {
-			LOG_TARGET_DEBUG(target, "%x is store instruction", instruction);
-			imm = ((instruction & 0xf80) >> 7) | ((instruction & 0xfe000000) >> 20);
-		} else {
-			LOG_TARGET_DEBUG(target, "%x is load instruction", instruction);
-			imm = (instruction & 0xfff00000) >> 20;
-		}
-		/* sign extend 12-bit imm to 16-bits */
-		if (imm & (1 << 11))
-			imm |= 0xf000;
-		mem_addr += imm;
-		LOG_TARGET_DEBUG(target, "Memory address=0x%" PRIx64, mem_addr);
-	} else {
-		LOG_TARGET_DEBUG(target, "%x is not a RV32I load or store", instruction);
+	if (verify_loadstore(target, instruction, &is_load) != ERROR_OK)
 		return ERROR_FAIL;
-	}
 
 	struct watchpoint *wp = target->watchpoints;
 	while (wp) {
-		/*TODO support length/mask */
-		if (wp->address == mem_addr) {
+		/* TODO support mask and check read/write/access */
+		/* TODO check for intersection of the access range and watchpoint range
+				Recommended matching:
+				if (intersects(mem_addr, mem_addr + ref_size, wp->address, wp->address + wp->length))
+		*/
+		if (mem_addr >= wp->address &&
+			mem_addr < (wp->address + wp->length)) {
 			*hit_watchpoint = wp;
-			LOG_TARGET_DEBUG(target, "Hit address=%" TARGET_PRIxADDR, wp->address);
+			LOG_TARGET_DEBUG(target, "WP hit found: %s 0x%" TARGET_PRIxADDR
+				" covered by %s wp at address 0x%" TARGET_PRIxADDR,
+				is_load ? "Load from" : "Store to", mem_addr,
+				(wp->rw == WPT_READ ?
+					"read" : (wp->rw == WPT_WRITE ? "write" : "access")),
+				wp->address);
 			return ERROR_OK;
 		}
 		wp = wp->next;
@@ -1702,6 +2137,8 @@ static int riscv_hit_watchpoint(struct target *target, struct watchpoint **hit_w
 	 *
 	 * OpenOCD will behave as if this function had never been implemented i.e.
 	 * report the halt to GDB with no address information. */
+	LOG_TARGET_DEBUG(target, "No watchpoint found that would cover %s 0x%"
+		TARGET_PRIxADDR, is_load ? "load from" : "store to", mem_addr);
 	return ERROR_FAIL;
 }