From: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
Date: Fri, 16 Sep 2016 09:43:27 +0000 (+0200)
Subject: aarch64: fix reading of translation table registers
X-Git-Url: https://git.sur5r.net/?a=commitdiff_plain;h=7eb95b1d72c0e4aea3499caf33312f8846bab262;p=openocd

aarch64: fix reading of translation table registers

Correctly access and parse aarch64 ttbcr.

Change-Id: I1b1652791a6b5200f58033925286292d838e8410
Signed-off-by: Matthias Welwarsky <matthias.welwarsky@sysgo.com>
---

diff --git a/src/target/armv8.c b/src/target/armv8.c
index 537dadb2..5a96d965 100644
--- a/src/target/armv8.c
+++ b/src/target/armv8.c
@@ -266,31 +266,114 @@ static void armv8_show_fault_registers(struct target *target)
 	/* TODO */
 }
 
+static uint8_t armv8_pa_size(uint32_t ps)
+{
+	uint8_t ret = 0;
+	switch (ps) {
+		case 0:
+			ret = 32;
+			break;
+		case 1:
+			ret = 36;
+			break;
+		case 2:
+			ret = 40;
+			break;
+		case 3:
+			ret = 42;
+			break;
+		case 4:
+			ret = 44;
+			break;
+		case 5:
+			ret = 48;
+			break;
+		default:
+			LOG_INFO("Unknow physicall address size");
+			break;
+	}
+	return ret;
+}
+
 static int armv8_read_ttbcr(struct target *target)
 {
 	struct armv8_common *armv8 = target_to_armv8(target);
 	struct arm_dpm *dpm = armv8->arm.dpm;
+	struct arm *arm = &armv8->arm;
 	uint32_t ttbcr;
+	uint64_t ttbcr_64;
+
 	int retval = dpm->prepare(dpm);
 	if (retval != ERROR_OK)
 		goto done;
-	/*  MRC p15,0,<Rt>,c2,c0,2 ; Read CP15 Translation Table Base Control Register*/
-	retval = dpm->instr_read_data_r0(dpm,
-			ARMV4_5_MRC(15, 0, 0, 2, 0, 2),
-			&ttbcr);
+
+	/* claaer ttrr1_used and ttbr0_mask */
+	memset(&armv8->armv8_mmu.ttbr1_used, 0, sizeof(armv8->armv8_mmu.ttbr1_used));
+	memset(&armv8->armv8_mmu.ttbr0_mask, 0, sizeof(armv8->armv8_mmu.ttbr0_mask));
+
+	switch (arm->core_mode) {
+		case ARMV8_64_EL3H:
+		case ARMV8_64_EL3T:
+			retval = dpm->instr_read_data_r0(dpm,
+					ARMV8_MRS(SYSTEM_TCR_EL3, 0),
+					&ttbcr);
+			retval += dpm->instr_read_data_r0_64(dpm,
+					ARMV8_MRS(SYSTEM_TTBR0_EL3, 0),
+					&armv8->ttbr_base);
+			if (retval != ERROR_OK)
+				goto done;
+			armv8->va_size = 64 - (ttbcr & 0x3F);
+			armv8->pa_size = armv8_pa_size((ttbcr >> 16) & 7);
+			armv8->page_size = (ttbcr >> 14) & 3;
+			break;
+		case ARMV8_64_EL2T:
+		case ARMV8_64_EL2H:
+			retval = dpm->instr_read_data_r0(dpm,
+					ARMV8_MRS(SYSTEM_TCR_EL2, 0),
+					&ttbcr);
+			retval += dpm->instr_read_data_r0_64(dpm,
+					ARMV8_MRS(SYSTEM_TTBR0_EL2, 0),
+					&armv8->ttbr_base);
+			if (retval != ERROR_OK)
+				goto done;
+			armv8->va_size = 64 - (ttbcr & 0x3F);
+			armv8->pa_size = armv8_pa_size((ttbcr >> 16) & 7);
+			armv8->page_size = (ttbcr >> 14) & 3;
+			break;
+		case ARMV8_64_EL0T:
+		case ARMV8_64_EL1T:
+		case ARMV8_64_EL1H:
+			retval = dpm->instr_read_data_r0_64(dpm,
+					ARMV8_MRS(SYSTEM_TCR_EL1, 0),
+					&ttbcr_64);
+			armv8->va_size = 64 - (ttbcr_64 & 0x3F);
+			armv8->pa_size = armv8_pa_size((ttbcr_64 >> 32) & 7);
+			armv8->page_size = (ttbcr_64 >> 14) & 3;
+			armv8->armv8_mmu.ttbr1_used = (((ttbcr_64 >> 16) & 0x3F) != 0) ? 1 : 0;
+			armv8->armv8_mmu.ttbr0_mask  = 0x0000FFFFFFFFFFFF;
+			retval += dpm->instr_read_data_r0_64(dpm,
+					ARMV8_MRS(SYSTEM_TTBR0_EL1 | (armv8->armv8_mmu.ttbr1_used), 0),
+					&armv8->ttbr_base);
+			if (retval != ERROR_OK)
+				goto done;
+			break;
+		default:
+			LOG_ERROR("unknow core state");
+			retval = ERROR_FAIL;
+			break;
+	}
 	if (retval != ERROR_OK)
 		goto done;
-	armv8->armv8_mmu.ttbr1_used = ((ttbcr & 0x7) != 0) ? 1 : 0;
-	armv8->armv8_mmu.ttbr0_mask  = 7 << (32 - ((ttbcr & 0x7)));
+
 #if 0
-	LOG_INFO("ttb1 %s ,ttb0_mask %x",
+	LOG_INFO("ttb1 %s ,ttb0_mask %llx",
 		armv8->armv8_mmu.ttbr1_used ? "used" : "not used",
 		armv8->armv8_mmu.ttbr0_mask);
 #endif
 	if (armv8->armv8_mmu.ttbr1_used == 1) {
-		LOG_INFO("SVC access above %" PRIx32,
-			 (uint32_t)(0xffffffff & armv8->armv8_mmu.ttbr0_mask));
-		armv8->armv8_mmu.os_border = 0xffffffff & armv8->armv8_mmu.ttbr0_mask;
+		LOG_INFO("TTBR0 access above %" PRIx64,
+			 (uint64_t)(armv8->armv8_mmu.ttbr0_mask));
+		armv8->armv8_mmu.os_border = armv8->armv8_mmu.ttbr0_mask;
 	} else {
 		/*  fix me , default is hard coded LINUX border  */
 		armv8->armv8_mmu.os_border = 0xc0000000;
diff --git a/src/target/armv8.h b/src/target/armv8.h
index 9c6ccce4..b9e3f12e 100644
--- a/src/target/armv8.h
+++ b/src/target/armv8.h
@@ -113,7 +113,7 @@ struct armv8_cache_common {
 struct armv8_mmu_common {
 	/* following field mmu working way */
 	int32_t ttbr1_used; /*  -1 not initialized, 0 no ttbr1 1 ttbr1 used and  */
-	uint32_t ttbr0_mask;/*  masked to be used  */
+	uint64_t ttbr0_mask;/*  masked to be used  */
 	uint32_t os_border;
 
 	int (*read_physical_memory)(struct target *target, target_addr_t address,
@@ -141,6 +141,12 @@ struct armv8_common {
 	uint8_t cpu_id;
 	bool is_armv7r;
 
+	/* armv8 aarch64 need below information for page translation */
+	uint8_t va_size;
+	uint8_t pa_size;
+	uint32_t page_size;
+	uint64_t ttbr_base;
+
 	/* cache specific to V7 Memory Management Unit compatible with v4_5*/
 	struct armv8_mmu_common armv8_mmu;