aarch64: handle semihosting in aarch32 state

Change-Id: I0e868d617db126a2b258e27b11979b75b5bb72f5
Signed-off-by: Tarek BOCHKATI <tarek.bouchkati@gmail.com>
Reviewed-on: http://openocd.zylin.com/5860
Tested-by: jenkins
Reviewed-by: Matthias Welwarsky <matthias@welwarsky.de>
Reviewed-by: Antonio Borneo <borneo.antonio@gmail.com>

src/target/arm_semihosting.c

@@ -123,6 +123,22 @@ static int post_result(struct target *target)
 			uint64_t pc = buf_get_u64(arm->core_cache->reg_list[32].value, 0, 64);
 			buf_set_u64(arm->pc->value, 0, 64, pc + 4);
 			arm->pc->dirty = true;
+		}  else if (arm->core_state == ARM_STATE_ARM) {
+			/* return value in R0 */
+			buf_set_u32(arm->core_cache->reg_list[0].value, 0, 32, target->semihosting->result);
+			arm->core_cache->reg_list[0].dirty = true;
+
+			uint32_t pc = buf_get_u32(arm->core_cache->reg_list[32].value, 0, 32);
+			buf_set_u32(arm->pc->value, 0, 32, pc + 4);
+			arm->pc->dirty = true;
+		} else if (arm->core_state == ARM_STATE_THUMB) {
+			/* return value in R0 */
+			buf_set_u32(arm->core_cache->reg_list[0].value, 0, 32, target->semihosting->result);
+			arm->core_cache->reg_list[0].dirty = true;
+
+			uint32_t pc = buf_get_u32(arm->core_cache->reg_list[32].value, 0, 32);
+			buf_set_u32(arm->pc->value, 0, 32, pc + 2);
+			arm->pc->dirty = true;
 		}
 	} else {
 		/* resume execution, this will be pc+2 to skip over the
@@ -275,6 +291,16 @@ int arm_semihosting(struct target *target, int *retval)
 		if (target->debug_reason != DBG_REASON_BREAKPOINT)
 			return 0;
 
+		/* According to ARM Semihosting for AArch32 and AArch64:
+		 * The HLT encodings are new in version 2.0 of the semihosting specification.
+		 * Where possible, have semihosting callers continue to use the previously
+		 * existing trap instructions to ensure compatibility with legacy semihosting
+		 * implementations.
+		 * These trap instructions are HLT for A64, SVC on A+R profile A32 or T32,
+		 * and BKPT on M profile.
+		 * However, it is necessary to change from SVC to HLT instructions to support
+		 * AArch32 semihosting properly in a mixed AArch32/AArch64 system. */
+
 		if (arm->core_state == ARM_STATE_AARCH64) {
 			uint32_t insn = 0;
 			r = arm->pc;
@@ -284,9 +310,38 @@ int arm_semihosting(struct target *target, int *retval)
 			if (*retval != ERROR_OK)
 				return 1;
 
-			/* bkpt 0xAB */
+			/* HLT 0xF000 */
 			if (insn != 0xD45E0000)
 				return 0;
+		} else if (arm->core_state == ARM_STATE_ARM) {
+			r = arm->pc;
+			pc = buf_get_u32(arm->pc->value, 0, 32);
+
+			/* A32 instruction => check for HLT 0xF000 (0xE10F0070) */
+			uint32_t insn = 0;
+
+			*retval = target_read_u32(target, pc, &insn);
+
+			if (*retval != ERROR_OK)
+				return 1;
+
+			/* HLT 0xF000*/
+			if (insn != 0xE10F0070)
+				return 0;
+		} else if (arm->core_state == ARM_STATE_THUMB) {
+			r = arm->pc;
+			pc = buf_get_u32(arm->pc->value, 0, 32);
+
+			/* T32 instruction => check for HLT 0x3C (0xBABC) */
+			uint16_t insn = 0;
+			*retval = target_read_u16(target, pc, &insn);
+
+			if (*retval != ERROR_OK)
+				return 1;
+
+			/* HLT 0x3C*/
+			if (insn != 0xBABC)
+				return 0;
 		} else
 			return 1;
 	} else {

src/target/armv8.c

@@ -1098,13 +1098,6 @@ int armv8_handle_cache_info_command(struct command_invocation *cmd,
 
 static int armv8_setup_semihosting(struct target *target, int enable)
 {
-	struct arm *arm = target_to_arm(target);
-
-	if (arm->core_state != ARM_STATE_AARCH64) {
-		LOG_ERROR("semihosting only supported in AArch64 state\n");
-		return ERROR_FAIL;
-	}
-
 	return ERROR_OK;
 }