X-Git-Url: https://review.openocd.org/gitweb?a=blobdiff_plain;f=src%2Ftarget%2Faarch64.c;h=ef73afd7e84f4c249f28fe7f1c0ae7a333500e31;hb=d8abda4bd828f40fc76613f4d809d86f4c6f1c97;hp=84e6db3ea7372dbdcdbf2b37441b9e0212548a6b;hpb=b085570de9550bf50938d57f50465d07ba3ed87b;p=openocd.git

diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 84e6db3ea7..ef73afd7e8 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -57,11 +57,41 @@ static int aarch64_restore_system_control_reg(struct target *target)
 
 	if (aarch64->system_control_reg != aarch64->system_control_reg_curr) {
 		aarch64->system_control_reg_curr = aarch64->system_control_reg;
-		retval = aarch64_instr_write_data_r0(armv8->arm.dpm,
-						     0xd5181000,
-						     aarch64->system_control_reg);
+		/* LOG_INFO("cp15_control_reg: %8.8" PRIx32, cortex_v8->cp15_control_reg); */
+
+		switch (armv8->arm.core_mode) {
+			case ARMV8_64_EL0T:
+			case ARMV8_64_EL1T:
+			case ARMV8_64_EL1H:
+				retval = armv8->arm.msr(target, 3, /*op 0*/
+						0, 1,	/* op1, op2 */
+						0, 0,	/* CRn, CRm */
+						aarch64->system_control_reg);
+				if (retval != ERROR_OK)
+					return retval;
+			break;
+			case ARMV8_64_EL2T:
+			case ARMV8_64_EL2H:
+				retval = armv8->arm.msr(target, 3, /*op 0*/
+						4, 1,	/* op1, op2 */
+						0, 0,	/* CRn, CRm */
+						aarch64->system_control_reg);
+				if (retval != ERROR_OK)
+					return retval;
+			break;
+			case ARMV8_64_EL3H:
+			case ARMV8_64_EL3T:
+				retval = armv8->arm.msr(target, 3, /*op 0*/
+						6, 1,	/* op1, op2 */
+						0, 0,	/* CRn, CRm */
+						aarch64->system_control_reg);
+				if (retval != ERROR_OK)
+					return retval;
+			break;
+			default:
+				LOG_DEBUG("unknow cpu state 0x%x" PRIx32, armv8->arm.core_state);
+			}
 	}
-
 	return retval;
 }
 
@@ -82,16 +112,45 @@ static int aarch64_mmu_modify(struct target *target, int enable)
 	int retval = ERROR_OK;
 
 	if (enable) {
-		/*  if mmu enabled at target stop and mmu not enable */
+		/*	if mmu enabled at target stop and mmu not enable */
 		if (!(aarch64->system_control_reg & 0x1U)) {
 			LOG_ERROR("trying to enable mmu on target stopped with mmu disable");
 			return ERROR_FAIL;
 		}
 		if (!(aarch64->system_control_reg_curr & 0x1U)) {
 			aarch64->system_control_reg_curr |= 0x1U;
-			retval = aarch64_instr_write_data_r0(armv8->arm.dpm,
-							     0xd5181000,
-							     aarch64->system_control_reg_curr);
+			switch (armv8->arm.core_mode) {
+				case ARMV8_64_EL0T:
+				case ARMV8_64_EL1T:
+				case ARMV8_64_EL1H:
+					retval = armv8->arm.msr(target, 3, /*op 0*/
+							0, 0,	/* op1, op2 */
+							1, 0,	/* CRn, CRm */
+							aarch64->system_control_reg_curr);
+					if (retval != ERROR_OK)
+						return retval;
+				break;
+				case ARMV8_64_EL2T:
+				case ARMV8_64_EL2H:
+					retval = armv8->arm.msr(target, 3, /*op 0*/
+							4, 0,	/* op1, op2 */
+							1, 0,	/* CRn, CRm */
+							aarch64->system_control_reg_curr);
+					if (retval != ERROR_OK)
+						return retval;
+				break;
+				case ARMV8_64_EL3H:
+				case ARMV8_64_EL3T:
+					retval = armv8->arm.msr(target, 3, /*op 0*/
+							6, 0,	/* op1, op2 */
+							1, 0,	/* CRn, CRm */
+							aarch64->system_control_reg_curr);
+					if (retval != ERROR_OK)
+						return retval;
+				break;
+				default:
+					LOG_DEBUG("unknow cpu state 0x%x" PRIx32, armv8->arm.core_state);
+			}
 		}
 	} else {
 		if (aarch64->system_control_reg_curr & 0x4U) {
@@ -103,9 +162,39 @@ static int aarch64_mmu_modify(struct target *target, int enable)
 		}
 		if ((aarch64->system_control_reg_curr & 0x1U)) {
 			aarch64->system_control_reg_curr &= ~0x1U;
-			retval = aarch64_instr_write_data_r0(armv8->arm.dpm,
-							     0xd5181000,
-							     aarch64->system_control_reg_curr);
+			switch (armv8->arm.core_mode) {
+				case ARMV8_64_EL0T:
+				case ARMV8_64_EL1T:
+				case ARMV8_64_EL1H:
+					retval = armv8->arm.msr(target, 3, /*op 0*/
+							0, 0,	/* op1, op2 */
+							1, 0,	/* CRn, CRm */
+							aarch64->system_control_reg_curr);
+					if (retval != ERROR_OK)
+						return retval;
+					break;
+				case ARMV8_64_EL2T:
+				case ARMV8_64_EL2H:
+					retval = armv8->arm.msr(target, 3, /*op 0*/
+							4, 0,	/* op1, op2 */
+							1, 0,	/* CRn, CRm */
+							aarch64->system_control_reg_curr);
+					if (retval != ERROR_OK)
+						return retval;
+					break;
+				case ARMV8_64_EL3H:
+				case ARMV8_64_EL3T:
+					retval = armv8->arm.msr(target, 3, /*op 0*/
+							6, 0,	/* op1, op2 */
+							1, 0,	/* CRn, CRm */
+							aarch64->system_control_reg_curr);
+					if (retval != ERROR_OK)
+						return retval;
+					break;
+				default:
+					LOG_DEBUG("unknow cpu state 0x%x" PRIx32, armv8->arm.core_state);
+					break;
+			}
 		}
 	}
 	return retval;
@@ -360,10 +449,14 @@ static int aarch64_dpm_prepare(struct arm_dpm *dpm)
 	if (dscr & DSCR_DTR_RX_FULL) {
 		LOG_ERROR("DSCR_DTR_RX_FULL, dscr 0x%08" PRIx32, dscr);
 		/* Clear DCCRX */
-		retval = aarch64_exec_opcode(
-				a8->armv8_common.arm.target,
-				0xd5130400,
-				&dscr);
+		retval = mem_ap_read_u32(a8->armv8_common.debug_ap,
+			a8->armv8_common.debug_base + CPUV8_DBG_DTRRX, &dscr);
+		if (retval != ERROR_OK)
+			return retval;
+
+		/* Clear sticky error */
+		retval = mem_ap_write_u32(a8->armv8_common.debug_ap,
+			a8->armv8_common.debug_base + CPUV8_DBG_DRCR, DRCR_CSE);
 		if (retval != ERROR_OK)
 			return retval;
 	}
@@ -436,7 +529,7 @@ static int aarch64_instr_write_data_r0(struct arm_dpm *dpm,
 
 	retval = aarch64_exec_opcode(
 			a8->armv8_common.arm.target,
-			0xd5330500,
+			ARMV8_MRS(SYSTEM_DBG_DTRRX_EL0, 0),
 			&dscr);
 	if (retval != ERROR_OK)
 		return retval;
@@ -463,7 +556,7 @@ static int aarch64_instr_write_data_r0_64(struct arm_dpm *dpm,
 
 	retval = aarch64_exec_opcode(
 			a8->armv8_common.arm.target,
-			0xd5330400,
+			ARMV8_MRS(SYSTEM_DBG_DBGDTR_EL0, 0),
 			&dscr);
 	if (retval != ERROR_OK)
 		return retval;
@@ -484,7 +577,7 @@ static int aarch64_instr_cpsr_sync(struct arm_dpm *dpm)
 
 	/* "Prefetch flush" after modifying execution status in CPSR */
 	return aarch64_exec_opcode(target,
-			ARMV4_5_MCR(15, 0, 0, 7, 5, 4),
+			DSB_SY,
 			&dscr);
 }
 
@@ -542,7 +635,7 @@ static int aarch64_instr_read_data_r0(struct arm_dpm *dpm,
 	/* write R0 to DCC */
 	retval = aarch64_exec_opcode(
 			a8->armv8_common.arm.target,
-			0xd5130400,  /* msr dbgdtr_el0, x0 */
+			ARMV8_MSR_GP(SYSTEM_DBG_DTRTX_EL0, 0),  /* msr dbgdtr_el0, x0 */
 			&dscr);
 	if (retval != ERROR_OK)
 		return retval;
@@ -568,7 +661,7 @@ static int aarch64_instr_read_data_r0_64(struct arm_dpm *dpm,
 	/* write R0 to DCC */
 	retval = aarch64_exec_opcode(
 			a8->armv8_common.arm.target,
-			0xd5130400,  /* msr dbgdtr_el0, x0 */
+			ARMV8_MSR_GP(SYSTEM_DBG_DBGDTR_EL0, 0),  /* msr dbgdtr_el0, x0 */
 			&dscr);
 	if (retval != ERROR_OK)
 		return retval;
@@ -796,59 +889,49 @@ static int aarch64_halt(struct target *target)
 	uint32_t dscr;
 	struct armv8_common *armv8 = target_to_armv8(target);
 
-	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + 0x10000 + 0, &dscr);
+	/* enable CTI*/
 	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + 0x10000 + 0, 1);
-	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + 0x10000 + 0, &dscr);
+			armv8->cti_base + CTI_CTR, 1);
+	if (retval != ERROR_OK)
+		return retval;
 
-	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + 0x10000 + 0x140, &dscr);
 	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + 0x10000 + 0x140, 6);
-	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + 0x10000 + 0x140, &dscr);
+			armv8->cti_base + CTI_GATE, 3);
+	if (retval != ERROR_OK)
+		return retval;
 
-	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + 0x10000 + 0xa0, &dscr);
 	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + 0x10000 + 0xa0, 5);
-	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + 0x10000 + 0xa0, &dscr);
-
-	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + 0x10000 + 0xa4, &dscr);
-	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + 0x10000 + 0xa4, 2);
-	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + 0x10000 + 0xa4, &dscr);
+			armv8->cti_base + CTI_OUTEN0, 1);
+	if (retval != ERROR_OK)
+		return retval;
 
-	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + 0x10000 + 0x20, &dscr);
 	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + 0x10000 + 0x20, 4);
-	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + 0x10000 + 0x20, &dscr);
+			armv8->cti_base + CTI_OUTEN1, 2);
+	if (retval != ERROR_OK)
+		return retval;
 
 	/*
-	 * enter halting debug mode
+	 * add HDE in halting debug mode
 	 */
 	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
 			armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
 	if (retval != ERROR_OK)
 		return retval;
 
-#	/* STATUS */
-	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + 0x10000 + 0x134, &dscr);
+	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUV8_DBG_DSCR, dscr | DSCR_HDE);
+	if (retval != ERROR_OK)
+		return retval;
 
-	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + 0x10000 + 0x1c, &dscr);
 	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + 0x10000 + 0x1c, 1);
-	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + 0x10000 + 0x1c, &dscr);
+			armv8->cti_base + CTI_APPPULSE, 1);
+	if (retval != ERROR_OK)
+		return retval;
+
+	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+			armv8->cti_base + CTI_INACK, 1);
+	if (retval != ERROR_OK)
+		return retval;
 
 
 	long long then = timeval_ms();
@@ -857,7 +940,7 @@ static int aarch64_halt(struct target *target)
 				armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
 		if (retval != ERROR_OK)
 			return retval;
-		if ((dscr & DSCR_CORE_HALTED) != 0)
+		if ((dscr & DSCRV8_HALT_MASK) != 0)
 			break;
 		if (timeval_ms() > then + 1000) {
 			LOG_ERROR("Timeout waiting for halt");
@@ -969,23 +1052,7 @@ static int aarch64_internal_restart(struct target *target)
 		LOG_ERROR("DSCR InstrCompl must be set before leaving debug!");
 
 	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUV8_DBG_DSCR, dscr & ~DSCR_ITR_EN);
-	if (retval != ERROR_OK)
-		return retval;
-
-	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUV8_DBG_DRCR, DRCR_RESTART |
-			DRCR_CLEAR_EXCEPTIONS);
-	if (retval != ERROR_OK)
-		return retval;
-
-	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + 0x10000 + 0x10, 1);
-	if (retval != ERROR_OK)
-		return retval;
-
-	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + 0x10000 + 0x1c, 2);
+			armv8->cti_base + CTI_APPPULSE, 2);
 	if (retval != ERROR_OK)
 		return retval;
 
@@ -995,7 +1062,7 @@ static int aarch64_internal_restart(struct target *target)
 				armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
 		if (retval != ERROR_OK)
 			return retval;
-		if ((dscr & DSCR_CORE_RESTARTED) != 0)
+		if ((dscr & DSCR_HDE) != 0)
 			break;
 		if (timeval_ms() > then + 1000) {
 			LOG_ERROR("Timeout waiting for resume");
@@ -1061,11 +1128,11 @@ static int aarch64_resume(struct target *target, int current,
 	if (!debug_execution) {
 		target->state = TARGET_RUNNING;
 		target_call_event_callbacks(target, TARGET_EVENT_RESUMED);
-		LOG_DEBUG("target resumed at 0x%" PRIu64, addr);
+		LOG_DEBUG("target resumed at 0x%" PRIx64, addr);
 	} else {
 		target->state = TARGET_DEBUG_RUNNING;
 		target_call_event_callbacks(target, TARGET_EVENT_DEBUG_RESUMED);
-		LOG_DEBUG("target debug resumed at 0x%" PRIu64, addr);
+		LOG_DEBUG("target debug resumed at 0x%" PRIx64, addr);
 	}
 
 	return ERROR_OK;
@@ -1073,49 +1140,45 @@ static int aarch64_resume(struct target *target, int current,
 
 static int aarch64_debug_entry(struct target *target)
 {
-	uint32_t dscr;
 	int retval = ERROR_OK;
 	struct aarch64_common *aarch64 = target_to_aarch64(target);
 	struct armv8_common *armv8 = target_to_armv8(target);
-	uint32_t tmp;
 
 	LOG_DEBUG("dscr = 0x%08" PRIx32, aarch64->cpudbg_dscr);
 
-	/* REVISIT surely we should not re-read DSCR !! */
-	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
-	if (retval != ERROR_OK)
-		return retval;
-
 	/* REVISIT see A8 TRM 12.11.4 steps 2..3 -- make sure that any
 	 * imprecise data aborts get discarded by issuing a Data
 	 * Synchronization Barrier:  ARMV4_5_MCR(15, 0, 0, 7, 10, 4).
 	 */
 
-	/* Enable the ITR execution once we are in debug mode */
-	dscr |= DSCR_ITR_EN;
+	/* make sure to clear all sticky errors */
 	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUV8_DBG_DSCR, dscr);
+			armv8->debug_base + CPUV8_DBG_DRCR, DRCR_CSE);
 	if (retval != ERROR_OK)
 		return retval;
 
 	/* Examine debug reason */
-	arm_dpm_report_dscr(&armv8->dpm, aarch64->cpudbg_dscr);
-	mem_ap_read_atomic_u32(armv8->debug_ap,
-				   armv8->debug_base + CPUV8_DBG_EDESR, &tmp);
-	if ((tmp & 0x7) == 0x4)
-		target->debug_reason = DBG_REASON_SINGLESTEP;
+	armv8_dpm_report_dscr(&armv8->dpm, aarch64->cpudbg_dscr);
 
 	/* save address of instruction that triggered the watchpoint? */
 	if (target->debug_reason == DBG_REASON_WATCHPOINT) {
-		uint32_t wfar;
+		uint32_t tmp;
+		uint64_t wfar = 0;
 
+		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_WFAR1,
+				&tmp);
+		if (retval != ERROR_OK)
+			return retval;
+		wfar = tmp;
+		wfar = (wfar << 32);
 		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
 				armv8->debug_base + CPUV8_DBG_WFAR0,
-				&wfar);
+				&tmp);
 		if (retval != ERROR_OK)
 			return retval;
-		arm_dpm_report_wfar(&armv8->dpm, wfar);
+		wfar |= tmp;
+		armv8_dpm_report_wfar(&armv8->dpm, wfar);
 	}
 
 	retval = armv8_dpm_read_current_registers(&armv8->dpm);
@@ -1133,31 +1196,57 @@ static int aarch64_post_debug_entry(struct target *target)
 {
 	struct aarch64_common *aarch64 = target_to_aarch64(target);
 	struct armv8_common *armv8 = &aarch64->armv8_common;
-	struct armv8_mmu_common *armv8_mmu = &armv8->armv8_mmu;
-	uint32_t sctlr_el1 = 0;
 	int retval;
 
 	mem_ap_write_atomic_u32(armv8->debug_ap,
-				armv8->debug_base + CPUV8_DBG_DRCR, 1<<2);
-	retval = aarch64_instr_read_data_r0(armv8->arm.dpm,
-					    0xd5381000, &sctlr_el1);
-	if (retval != ERROR_OK)
-		return retval;
-
-	LOG_DEBUG("sctlr_el1 = %#8.8x", sctlr_el1);
-	aarch64->system_control_reg = sctlr_el1;
-	aarch64->system_control_reg_curr = sctlr_el1;
-	aarch64->curr_mode = armv8->arm.core_mode;
-
-	armv8_mmu->mmu_enabled = sctlr_el1 & 0x1U ? 1 : 0;
-	armv8_mmu->armv8_cache.d_u_cache_enabled = sctlr_el1 & 0x4U ? 1 : 0;
-	armv8_mmu->armv8_cache.i_cache_enabled = sctlr_el1 & 0x1000U ? 1 : 0;
+				    armv8->debug_base + CPUV8_DBG_DRCR, 1<<2);
+	switch (armv8->arm.core_mode) {
+		case ARMV8_64_EL0T:
+		case ARMV8_64_EL1T:
+		case ARMV8_64_EL1H:
+			retval = armv8->arm.mrs(target, 3, /*op 0*/
+					0, 0,	/* op1, op2 */
+					1, 0,	/* CRn, CRm */
+					&aarch64->system_control_reg);
+			if (retval != ERROR_OK)
+				return retval;
+		break;
+		case ARMV8_64_EL2T:
+		case ARMV8_64_EL2H:
+			retval = armv8->arm.mrs(target, 3, /*op 0*/
+					4, 0,	/* op1, op2 */
+					1, 0,	/* CRn, CRm */
+					&aarch64->system_control_reg);
+			if (retval != ERROR_OK)
+				return retval;
+		break;
+		case ARMV8_64_EL3H:
+		case ARMV8_64_EL3T:
+			retval = armv8->arm.mrs(target, 3, /*op 0*/
+					6, 0,	/* op1, op2 */
+					1, 0,	/* CRn, CRm */
+					&aarch64->system_control_reg);
+			if (retval != ERROR_OK)
+				return retval;
+		break;
+		default:
+			LOG_DEBUG("unknow cpu state 0x%x" PRIx32, armv8->arm.core_state);
+	}
+	LOG_DEBUG("System_register: %8.8" PRIx32, aarch64->system_control_reg);
+	aarch64->system_control_reg_curr = aarch64->system_control_reg;
 
 #if 0
 	if (armv8->armv8_mmu.armv8_cache.ctype == -1)
 		armv8_identify_cache(target);
 #endif
 
+	armv8->armv8_mmu.mmu_enabled =
+			(aarch64->system_control_reg & 0x1U) ? 1 : 0;
+	armv8->armv8_mmu.armv8_cache.d_u_cache_enabled =
+		(aarch64->system_control_reg & 0x4U) ? 1 : 0;
+	armv8->armv8_mmu.armv8_cache.i_cache_enabled =
+		(aarch64->system_control_reg & 0x1000U) ? 1 : 0;
+	aarch64->curr_mode = armv8->arm.core_mode;
 	return ERROR_OK;
 }
 
@@ -1291,8 +1380,7 @@ static int aarch64_set_breakpoint(struct target *target,
 
 	} else if (breakpoint->type == BKPT_SOFT) {
 		uint8_t code[4];
-		buf_set_u32(code, 0, 32, 0xD4400000);
-
+		buf_set_u32(code, 0, 32, ARMV8_BKPT(0x11));
 		retval = target_read_memory(target,
 				breakpoint->address & 0xFFFFFFFFFFFFFFFE,
 				breakpoint->length, 1,
@@ -1348,18 +1436,19 @@ static int aarch64_set_context_breakpoint(struct target *target,
 
 	breakpoint->set = brp_i + 1;
 	control = ((matchmode & 0x7) << 20)
+		| (1 << 13)
 		| (byte_addr_select << 5)
 		| (3 << 1) | 1;
 	brp_list[brp_i].used = 1;
 	brp_list[brp_i].value = (breakpoint->asid);
 	brp_list[brp_i].control = control;
 	retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
-			+ CPUV8_DBG_BVR_BASE + 4 * brp_list[brp_i].BRPn,
+			+ CPUV8_DBG_BVR_BASE + 16 * brp_list[brp_i].BRPn,
 			brp_list[brp_i].value);
 	if (retval != ERROR_OK)
 		return retval;
 	retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
-			+ CPUV8_DBG_BCR_BASE + 4 * brp_list[brp_i].BRPn,
+			+ CPUV8_DBG_BCR_BASE + 16 * brp_list[brp_i].BRPn,
 			brp_list[brp_i].control);
 	if (retval != ERROR_OK)
 		return retval;
@@ -1420,30 +1509,36 @@ static int aarch64_set_hybrid_breakpoint(struct target *target, struct breakpoin
 	brp_list[brp_1].value = (breakpoint->asid);
 	brp_list[brp_1].control = control_CTX;
 	retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
-			+ CPUV8_DBG_BVR_BASE + 4 * brp_list[brp_1].BRPn,
+			+ CPUV8_DBG_BVR_BASE + 16 * brp_list[brp_1].BRPn,
 			brp_list[brp_1].value);
 	if (retval != ERROR_OK)
 		return retval;
 	retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
-			+ CPUV8_DBG_BCR_BASE + 4 * brp_list[brp_1].BRPn,
+			+ CPUV8_DBG_BCR_BASE + 16 * brp_list[brp_1].BRPn,
 			brp_list[brp_1].control);
 	if (retval != ERROR_OK)
 		return retval;
 
 	control_IVA = ((IVA_machmode & 0x7) << 20)
 		| (brp_1 << 16)
+		| (1 << 13)
 		| (IVA_byte_addr_select << 5)
 		| (3 << 1) | 1;
 	brp_list[brp_2].used = 1;
-	brp_list[brp_2].value = (breakpoint->address & 0xFFFFFFFC);
+	brp_list[brp_2].value = breakpoint->address & 0xFFFFFFFFFFFFFFFC;
 	brp_list[brp_2].control = control_IVA;
 	retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
-			+ CPUV8_DBG_BVR_BASE + 4 * brp_list[brp_2].BRPn,
-			brp_list[brp_2].value);
+			+ CPUV8_DBG_BVR_BASE + 16 * brp_list[brp_2].BRPn,
+			brp_list[brp_2].value & 0xFFFFFFFF);
 	if (retval != ERROR_OK)
 		return retval;
 	retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
-			+ CPUV8_DBG_BCR_BASE + 4 * brp_list[brp_2].BRPn,
+			+ CPUV8_DBG_BVR_BASE + 4 + 16 * brp_list[brp_2].BRPn,
+			brp_list[brp_2].value >> 32);
+	if (retval != ERROR_OK)
+		return retval;
+	retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
+			+ CPUV8_DBG_BCR_BASE + 16 * brp_list[brp_2].BRPn,
 			brp_list[brp_2].control);
 	if (retval != ERROR_OK)
 		return retval;
@@ -1511,12 +1606,12 @@ static int aarch64_unset_breakpoint(struct target *target, struct breakpoint *br
 			brp_list[brp_i].value = 0;
 			brp_list[brp_i].control = 0;
 			retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
-					+ CPUV8_DBG_BCR_BASE + 4 * brp_list[brp_i].BRPn,
+					+ CPUV8_DBG_BCR_BASE + 16 * brp_list[brp_i].BRPn,
 					brp_list[brp_i].control);
 			if (retval != ERROR_OK)
 				return retval;
 			retval = aarch64_dap_write_memap_register_u32(target, armv8->debug_base
-					+ CPUV8_DBG_BVR_BASE + 4 * brp_list[brp_i].BRPn,
+					+ CPUV8_DBG_BVR_BASE + 16 * brp_list[brp_i].BRPn,
 					brp_list[brp_i].value);
 			if (retval != ERROR_OK)
 				return retval;
@@ -2309,6 +2404,12 @@ static int aarch64_examine_first(struct target *target)
 	} else
 		armv8->debug_base = target->dbgbase;
 
+	LOG_DEBUG("Target ctibase is 0x%x", target->ctibase);
+	if (target->ctibase == 0)
+		armv8->cti_base = target->ctibase = armv8->debug_base + 0x1000;
+	else
+		armv8->cti_base = target->ctibase;
+
 	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
 			armv8->debug_base + CPUV8_DBG_LOCKACCESS, 0xC5ACCE55);
 	if (retval != ERROR_OK) {