Set haltreq when we're halted.

I'd missed a case when a hart halts almost instantly after resume

Change-Id: I68cbb04c16e1443aa318faa2d30475c61c71701f

src/target/riscv/riscv-013.c

@@ -200,6 +200,12 @@ typedef struct {
 	bool abstract_read_fpr_supported;
 	bool abstract_write_fpr_supported;
 
+	/* Track whether the hart is currently halted. This is more precise than
+	 * target->state, because it'll reflect halted status even if we're
+	 * pretending to the client that the hart is running (ie. during
+	 * examine()). */
+	bool really_halted[RISCV_MAX_HARTS];
+
 	/* When a function returns some error due to a failure indicated by the
 	 * target in cmderr, the caller can look here to see what that error was.
 	 * (Compare with errno.) */
@@ -408,7 +414,7 @@ static void select_dmi(struct target *target)
 	jtag_add_ir_scan(target->tap, &field, TAP_IDLE);
 }
 
-static uint32_t dtmcontrol_scan(struct target *target, uint32_t out)
+static int dtmcontrol_scan(struct target *target, uint32_t *in, uint32_t out)
 {
 	struct scan_field field;
 	uint8_t in_value[4];
@@ -432,13 +438,22 @@ static uint32_t dtmcontrol_scan(struct target *target, uint32_t out)
 		return retval;
 	}
 
-	uint32_t in = buf_get_u32(field.in_value, 0, 32);
-	LOG_DEBUG("DTMCS: 0x%x -> 0x%x", out, in);
+	uint32_t dtmcontrol = buf_get_u32(field.in_value, 0, 32);
+	LOG_DEBUG("DTMCS: 0x%x -> 0x%x", out, dtmcontrol);
 
-	return in;
+	if (dtmcontrol == 0) {
+		LOG_ERROR("TDO seems to be stuck low; target might be held in "
+				"reset, or there might be a connection failure");
+		return ERROR_FAIL;
+	}
+
+	if (in)
+		*in = dtmcontrol;
+
+	return ERROR_OK;
 }
 
-static void increase_dmi_busy_delay(struct target *target)
+static int increase_dmi_busy_delay(struct target *target)
 {
 	riscv013_info_t *info = get_info(target);
 	info->dmi_busy_delay += info->dmi_busy_delay / 10 + 1;
@@ -446,7 +461,7 @@ static void increase_dmi_busy_delay(struct target *target)
 			info->dtmcontrol_idle, info->dmi_busy_delay,
 			info->ac_busy_delay);
 
-	dtmcontrol_scan(target, DTM_DTMCS_DMIRESET);
+	return dtmcontrol_scan(target, NULL, DTM_DTMCS_DMIRESET | DTM_DTMCS_DMIHARDRESET);
 }
 
 /**
@@ -506,10 +521,14 @@ static dmi_status_t dmi_scan(struct target *target, uint32_t *address_in,
 static int dmi_op_timeout(struct target *target, uint32_t *data_in, int dmi_op,
 		uint32_t address, uint32_t data_out, int timeout_sec)
 {
+	riscv013_info_t *info = get_info(target);
 	select_dmi(target);
 
 	dmi_status_t status;
 	uint32_t address_in;
+	uint32_t data;
+	if (data_in == NULL)
+		data_in = &data;
 
 	const char *op_name;
 	switch (dmi_op) {
@@ -531,9 +550,15 @@ static int dmi_op_timeout(struct target *target, uint32_t *data_in, int dmi_op,
 	/* This first loop performs the request.  Note that if for some reason this
 	 * stays busy, it is actually due to the previous access. */
 	while (1) {
-		status = dmi_scan(target, NULL, NULL, dmi_op, address, data_out,
-				false);
-		if (status == DMI_STATUS_BUSY) {
+		status = dmi_scan(target, &address_in, data_in, dmi_op, address,
+				data_out, false);
+		LOG_DEBUG("address_in=0x%x; data_in=0x%x", address_in, *data_in);
+		if (status == 3 && address_in == (1U << info->abits) - 1 &&
+				*data_in == 0xffffffff) {
+			LOG_ERROR("TDO seems to be stuck high; target might be held in "
+					"reset, or there might be a connection failure");
+			return ERROR_FAIL;
+		} else if (status == DMI_STATUS_BUSY) {
 			increase_dmi_busy_delay(target);
 		} else if (status == DMI_STATUS_SUCCESS) {
 			break;
@@ -556,7 +581,12 @@ static int dmi_op_timeout(struct target *target, uint32_t *data_in, int dmi_op,
 	while (1) {
 		status = dmi_scan(target, &address_in, data_in, DMI_OP_NOP, address, 0,
 				false);
-		if (status == DMI_STATUS_BUSY) {
+		if (status == 3 && address_in == (1U << info->abits) - 1 &&
+				*data_in == 0xffffffff) {
+			LOG_ERROR("TDO seems to be stuck high; target might be held in "
+					"reset, or there might be a connection failure");
+			return ERROR_FAIL;
+		} else if (status == DMI_STATUS_BUSY) {
 			increase_dmi_busy_delay(target);
 		} else if (status == DMI_STATUS_SUCCESS) {
 			break;
@@ -1344,7 +1374,9 @@ static int examine(struct target *target)
 {
 	/* Don't need to select dbus, since the first thing we do is read dtmcontrol. */
 
-	uint32_t dtmcontrol = dtmcontrol_scan(target, 0);
+	uint32_t dtmcontrol;
+	if (dtmcontrol_scan(target, &dtmcontrol, 0) != ERROR_OK)
+		return ERROR_FAIL;
 	LOG_DEBUG("dtmcontrol=0x%x", dtmcontrol);
 	LOG_DEBUG("  dmireset=%d", get_field(dtmcontrol, DTM_DTMCS_DMIRESET));
 	LOG_DEBUG("  idle=%d", get_field(dtmcontrol, DTM_DTMCS_IDLE));
@@ -2828,16 +2860,16 @@ static int riscv013_set_register(struct target *target, int hid, int rid, uint64
 static int riscv013_select_current_hart(struct target *target)
 {
 	RISCV_INFO(r);
+	RISCV013_INFO(info);
 
 	dm013_info_t *dm = get_dm(target);
 	if (r->current_hartid == dm->current_hartid)
 		return ERROR_OK;
 
-	uint32_t dmcontrol;
-	/* TODO: can't we just "dmcontrol = DMI_DMACTIVE"? */
-	if (dmi_read(target, &dmcontrol, DMI_DMCONTROL) != ERROR_OK)
-		return ERROR_FAIL;
+	uint32_t dmcontrol = DMI_DMCONTROL_DMACTIVE;
 	dmcontrol = set_hartsel(dmcontrol, r->current_hartid);
+	if (info->really_halted[r->current_hartid])
+		dmcontrol |= DMI_DMCONTROL_HALTREQ;
 	int result = dmi_write(target, DMI_DMCONTROL, dmcontrol);
 	dm->current_hartid = r->current_hartid;
 	return result;
@@ -2846,15 +2878,16 @@ static int riscv013_select_current_hart(struct target *target)
 static int riscv013_halt_current_hart(struct target *target)
 {
 	RISCV_INFO(r);
+	RISCV013_INFO(info);
+
 	LOG_DEBUG("halting hart %d", r->current_hartid);
 	if (riscv_is_halted(target))
 		LOG_ERROR("Hart %d is already halted!", r->current_hartid);
 
 	/* Issue the halt command, and then wait for the current hart to halt. */
-	uint32_t dmcontrol;
-	if (dmi_read(target, &dmcontrol, DMI_DMCONTROL) != ERROR_OK)
-		return ERROR_FAIL;
-	dmcontrol = set_field(dmcontrol, DMI_DMCONTROL_HALTREQ, 1);
+	uint32_t dmcontrol = DMI_DMCONTROL_DMACTIVE;
+	dmcontrol |= DMI_DMCONTROL_HALTREQ;
+	dmcontrol = set_hartsel(dmcontrol, r->current_hartid);
 	dmi_write(target, DMI_DMCONTROL, dmcontrol);
 	for (size_t i = 0; i < 256; ++i)
 		if (riscv_is_halted(target))
@@ -2873,8 +2906,7 @@ static int riscv013_halt_current_hart(struct target *target)
 		return ERROR_FAIL;
 	}
 
-	dmcontrol = set_field(dmcontrol, DMI_DMCONTROL_HALTREQ, 0);
-	dmi_write(target, DMI_DMCONTROL, dmcontrol);
+	info->really_halted[r->current_hartid] = true;
 
 	return ERROR_OK;
 }
@@ -2906,6 +2938,8 @@ static int riscv013_on_halt(struct target *target)
 
 static bool riscv013_is_halted(struct target *target)
 {
+	RISCV013_INFO(info);
+
 	uint32_t dmstatus;
 	if (dmstatus_read(target, &dmstatus, true) != ERROR_OK)
 		return false;
@@ -2913,8 +2947,8 @@ static bool riscv013_is_halted(struct target *target)
 		LOG_ERROR("Hart %d is unavailable.", riscv_current_hartid(target));
 	if (get_field(dmstatus, DMI_DMSTATUS_ANYNONEXISTENT))
 		LOG_ERROR("Hart %d doesn't exist.", riscv_current_hartid(target));
+	int hartid = riscv_current_hartid(target);
 	if (get_field(dmstatus, DMI_DMSTATUS_ANYHAVERESET)) {
-		int hartid = riscv_current_hartid(target);
 		LOG_INFO("Hart %d unexpectedly reset!", hartid);
 		/* TODO: Can we make this more obvious to eg. a gdb user? */
 		uint32_t dmcontrol = DMI_DMCONTROL_DMACTIVE |
@@ -2929,6 +2963,7 @@ static bool riscv013_is_halted(struct target *target)
 			dmcontrol |= DMI_DMCONTROL_HALTREQ;
 		dmi_write(target, DMI_DMCONTROL, dmcontrol);
 	}
+	info->really_halted[hartid] = get_field(dmstatus, DMI_DMSTATUS_ALLHALTED);
 	return get_field(dmstatus, DMI_DMSTATUS_ALLHALTED);
 }
 
@@ -3394,6 +3429,8 @@ static int riscv013_on_step_or_resume(struct target *target, bool step)
 static int riscv013_step_or_resume_current_hart(struct target *target, bool step)
 {
 	RISCV_INFO(r);
+	RISCV013_INFO(info);
+
 	LOG_DEBUG("resuming hart %d (for step?=%d)", r->current_hartid, step);
 	if (!riscv_is_halted(target)) {
 		LOG_ERROR("Hart %d is not halted!", r->current_hartid);
@@ -3418,6 +3455,12 @@ static int riscv013_step_or_resume_current_hart(struct target *target, bool step
 		if (step && get_field(dmstatus, DMI_DMSTATUS_ALLHALTED) == 0)
 			continue;
 
+		if (get_field(dmstatus, DMI_DMSTATUS_ANYHALTED)) {
+			dmcontrol |= DMI_DMCONTROL_HALTREQ;
+			info->really_halted[r->current_hartid] = true;
+		} else {
+			info->really_halted[r->current_hartid] = false;
+		}
 		dmi_write(target, DMI_DMCONTROL, dmcontrol);
 		return ERROR_OK;
 	}