Semihosting 2 of 3: Refactor magic sequence detection

Refactor (clean up) the code in riscv_semihosting.c by moving
the magic sequence detection to its own function.

Change-Id: I3a3ce991336ceeeff023d459d0e28558059554e0
Signed-off-by: Jan Matyas <[email protected]>

Author: JanMatCodasip

src/target/riscv/riscv_semihosting.c

@@ -37,6 +37,51 @@
 static int riscv_semihosting_setup(struct target *target, int enable);
 static int riscv_semihosting_post_result(struct target *target);
 
+static int riscv_semihosting_detect_magic_sequence(struct target *target,
+	const target_addr_t pc, bool *sequence_found)
+{
+	assert(sequence_found);
+
+	const uint32_t magic[] = {
+		0x01f01013,	/* slli    zero,zero,0x1f */
+		0x00100073,	/* ebreak */
+		0x40705013	/* srai    zero,zero,0x7 */
+	};
+
+	LOG_TARGET_DEBUG(target, "Checking for RISC-V semihosting sequence "
+		"at PC = 0x%" TARGET_PRIxADDR, pc);
+
+	/* Read three uncompressed instructions:
+	 * The previous, the current one (pointed to by PC) and the next one. */
+	for (int i = 0; i < 3; i++) {
+		uint8_t buf[4];
+
+		/* Instruction memories may not support arbitrary read size.
+		 * Use any size that will work. */
+		const target_addr_t address = (pc - 4) + 4 * i;
+		int result = riscv_read_by_any_size(target, address, 4, buf);
+		if (result != ERROR_OK) {
+			*sequence_found = false;
+			return result;
+		}
+
+		const uint32_t value = target_buffer_get_u32(target, buf);
+
+		LOG_TARGET_DEBUG(target, "compare 0x%08x from 0x%" PRIx64 " against 0x%08x",
+			value, address, magic[i]);
+		if (value != magic[i]) {
+			LOG_TARGET_DEBUG(target, "Not a RISC-V semihosting sequence");
+			*sequence_found = false;
+			return ERROR_OK;
+		}
+	}
+
+	LOG_TARGET_DEBUG(target, "RISC-V semihosting sequence found "
+		"at PC = 0x%" TARGET_PRIxADDR, pc);
+	*sequence_found = true;
+	return ERROR_OK;
+}
+
 /**
  * Initialize RISC-V semihosting. Use common ARM code.
  */
@@ -69,33 +114,19 @@ enum semihosting_result riscv_semihosting(struct target *target, int *retval)
 	if (result != ERROR_OK)
 		return SEMIHOSTING_ERROR;
 
-	/*
-	 * The instructions that trigger a semihosting call,
-	 * always uncompressed, should look like:
-	 */
-	uint32_t magic[] = {
-		0x01f01013,	/* slli    zero,zero,0x1f */
-		0x00100073,	/* ebreak */
-		0x40705013	/* srai    zero,zero,0x7 */
-	};
+	bool sequence_found;
+	*retval = riscv_semihosting_detect_magic_sequence(target, pc, &sequence_found);
+	if (*retval != ERROR_OK)
+		return SEMIHOSTING_ERROR;
 
-	/* Read three uncompressed instructions: The previous, the current one (pointed to by PC) and the next one */
-	for (int i = 0; i < 3; i++) {
-		uint8_t buf[4];
-		/* Instruction memories may not support arbitrary read size. Use any size that will work. */
-		target_addr_t address = (pc - 4) + 4 * i;
-		*retval = riscv_read_by_any_size(target, address, 4, buf);
-		if (*retval != ERROR_OK)
-			return SEMIHOSTING_ERROR;
-		uint32_t value = target_buffer_get_u32(target, buf);
-		LOG_TARGET_DEBUG(target, "compare 0x%08x from 0x%" PRIx64 " against 0x%08x",
-			value, address, magic[i]);
-		if (value != magic[i]) {
-			LOG_TARGET_DEBUG(target, "   -> NONE (no magic)");
-			return SEMIHOSTING_NONE;
-		}
+	if (!sequence_found) {
+		LOG_TARGET_DEBUG(target, "   -> NONE (no magic)");
+		return SEMIHOSTING_NONE;
 	}
 
+	/* Otherwise we have a semihosting call (and semihosting is enabled).
+	 * Proceed with the semihosting. */
+
 	/*
 	 * Perform semihosting call if we are not waiting on a fileio
 	 * operation to complete.