cortex-a8: more MMU support

+ virt2phys() can now convert virtual address to real
+ read_memory() and write_memory() are renamed to read_phys_memory()
and write_phys_memory()
+ new read_memory() and write_memory() try to resolve real address if
mmu is enambled than perform real address reading/writing
   + if address is bellow 0xc000000 than TTB0 is used for page table
dereference, if above - than TTB1. Linux style of user/kernel address
separation
   + if above fails (i.e address is unspecified) than mode is checked
whether it is Supervisor (than TTB1) or User (than TTB0)
- Software breakpoints doesn't work. You should invoke
"gdb_breakpoint_override hard" before you start debugging
+ cortex_a8_mmu(), cortex_a8_enable_mmu_caches(),
cortex_a8_disable_mmu_caches() are implemented

Signed-off-by: Øyvind Harboe <oyvind.harboe@zylin.com>

src/target/cortex_a8.c

@@ -51,6 +51,16 @@ static int cortex_a8_dap_read_coreregister_u32(struct target *target,
 		uint32_t *value, int regnum);
 static int cortex_a8_dap_write_coreregister_u32(struct target *target,
 		uint32_t value, int regnum);
+static int cortex_a8_mmu(struct target *target, int *enabled);
+static int cortex_a8_virt2phys(struct target *target,
+                uint32_t virt, uint32_t *phys);
+static void cortex_a8_disable_mmu_caches(struct target *target, int mmu,
+                int d_u_cache, int i_cache);
+static void cortex_a8_enable_mmu_caches(struct target *target, int mmu,
+                int d_u_cache, int i_cache);
+static uint32_t cortex_a8_get_ttb(struct target *target);
+
+
 /*
  * FIXME do topology discovery using the ROM; don't
  * assume this is an OMAP3.   Also, allow for multiple ARMv7-A
@@ -1269,8 +1279,9 @@ static int cortex_a8_deassert_reset(struct target *target)
  * ap number for every access.
  */
 
-static int cortex_a8_read_memory(struct target *target, uint32_t address,
+static int cortex_a8_read_phys_memory(struct target *target,
-		uint32_t size, uint32_t count, uint8_t *buffer)
+                uint32_t address, uint32_t size,
+                uint32_t count, uint8_t *buffer)
 {
         struct armv7a_common *armv7a = target_to_armv7a(target);
         struct adiv5_dap *swjdp = &armv7a->dap;
@@ -1279,7 +1290,7 @@ static int cortex_a8_read_memory(struct target *target, uint32_t address,
         /* cortex_a8 handles unaligned memory access */
 
 // ???  dap_ap_select(swjdp, swjdp_memoryap);
-
+        LOG_DEBUG("Reading memory at real address 0x%x; size %d; count %d", address, size, count);
         if (count && buffer) {
                 switch (size) {
                 case 4:
@@ -1297,8 +1308,31 @@ static int cortex_a8_read_memory(struct target *target, uint32_t address,
         return retval;
 }
 
-static int cortex_a8_write_memory(struct target *target, uint32_t address,
+static int cortex_a8_read_memory(struct target *target, uint32_t address,
 		uint32_t size, uint32_t count, uint8_t *buffer)
+{
+        int enabled = 0;
+        uint32_t virt, phys;
+
+	/* cortex_a8 handles unaligned memory access */
+
+// ???	dap_ap_select(swjdp, swjdp_memoryap);
+        LOG_DEBUG("Reading memory at address 0x%x; size %d; count %d", address, size, count);
+        cortex_a8_mmu(target, &enabled);
+        if(enabled)
+        {
+            virt = address;
+            cortex_a8_virt2phys(target, virt, &phys);
+            LOG_DEBUG("Reading at virtual address. Translating v:0x%x to r:0x%x", virt, phys);
+            address = phys;
+        }
+
+        return cortex_a8_read_phys_memory(target, address, size, count, buffer);
+}
+
+static int cortex_a8_write_phys_memory(struct target *target,
+                uint32_t address, uint32_t size,
+                uint32_t count, uint8_t *buffer)
 {
         struct armv7a_common *armv7a = target_to_armv7a(target);
         struct adiv5_dap *swjdp = &armv7a->dap;
@@ -1306,6 +1340,7 @@ static int cortex_a8_write_memory(struct target *target, uint32_t address,
 
 // ???  dap_ap_select(swjdp, swjdp_memoryap);
 
+        LOG_DEBUG("Writing memory to real address 0x%x; size %d; count %d", address, size, count);
         if (count && buffer) {
                 switch (size) {
                 case 4:
@@ -1376,6 +1411,28 @@ static int cortex_a8_write_memory(struct target *target, uint32_t address,
         return retval;
 }
 
+static int cortex_a8_write_memory(struct target *target, uint32_t address,
+                uint32_t size, uint32_t count, uint8_t *buffer)
+{
+        int enabled = 0;
+        uint32_t virt, phys;
+
+// ???  dap_ap_select(swjdp, swjdp_memoryap);
+
+        LOG_DEBUG("Writing memory to address 0x%x; size %d; count %d", address, size, count);
+        cortex_a8_mmu(target, &enabled);
+        if(enabled)
+        {
+            virt = address;
+            cortex_a8_virt2phys(target, virt, &phys);
+            LOG_DEBUG("Writing to virtual address. Translating v:0x%x to r:0x%x", virt, phys);
+            address = phys;
+        }
+
+        return cortex_a8_write_phys_memory(target, address, size, 
+                count, buffer);
+}
+
 static int cortex_a8_bulk_write_memory(struct target *target, uint32_t address,
 		uint32_t count, uint8_t *buffer)
 {
@@ -1580,6 +1637,9 @@ static int cortex_a8_init_arch_info(struct target *target,
 
 	cortex_a8->fast_reg_read = 0;
 
+	/* Set default value */
+	cortex_a8->current_address_mode = ARM_MODE_ANY;
+
 	/* register arch-specific functions */
 	armv7a->examine_debug_reason = NULL;
 
@@ -1587,11 +1647,11 @@ static int cortex_a8_init_arch_info(struct target *target,
 
 	armv7a->pre_restore_context = NULL;
 	armv7a->armv4_5_mmu.armv4_5_cache.ctype = -1;
-//	armv7a->armv4_5_mmu.get_ttb = armv7a_get_ttb;
+	armv7a->armv4_5_mmu.get_ttb = cortex_a8_get_ttb;
-	armv7a->armv4_5_mmu.read_memory = cortex_a8_read_memory;
+	armv7a->armv4_5_mmu.read_memory = cortex_a8_read_phys_memory;
-	armv7a->armv4_5_mmu.write_memory = cortex_a8_write_memory;
+	armv7a->armv4_5_mmu.write_memory = cortex_a8_write_phys_memory;
-//	armv7a->armv4_5_mmu.disable_mmu_caches = armv7a_disable_mmu_caches;
+	armv7a->armv4_5_mmu.disable_mmu_caches = cortex_a8_disable_mmu_caches;
-//	armv7a->armv4_5_mmu.enable_mmu_caches = armv7a_enable_mmu_caches;
+	armv7a->armv4_5_mmu.enable_mmu_caches = cortex_a8_enable_mmu_caches;
 	armv7a->armv4_5_mmu.has_tiny_pages = 1;
 	armv7a->armv4_5_mmu.mmu_enabled = 0;
 
@@ -1616,6 +1676,160 @@ static int cortex_a8_target_create(struct target *target, Jim_Interp *interp)
 	return ERROR_OK;
 }
 
+static uint32_t cortex_a8_get_ttb(struct target *target)
+{
+	struct cortex_a8_common *cortex_a8 = target_to_cortex_a8(target);
+    struct armv7a_common *armv7a = &cortex_a8->armv7a_common;
+    uint32_t ttb = 0, retval = ERROR_OK;
+
+    /* current_address_mode is set inside cortex_a8_virt2phys()
+       where we can determine if address belongs to user or kernel */
+    if(cortex_a8->current_address_mode == ARM_MODE_SVC)
+    {
+        /* MRC p15,0,<Rt>,c1,c0,0 ; Read CP15 System Control Register */
+        retval = armv7a->armv4_5_common.mrc(target, 15,
+                    0, 1,   /* op1, op2 */
+                    2, 0,   /* CRn, CRm */
+                    &ttb);
+    }
+    else if(cortex_a8->current_address_mode == ARM_MODE_USR)
+    {
+        /* MRC p15,0,<Rt>,c1,c0,0 ; Read CP15 System Control Register */
+        retval = armv7a->armv4_5_common.mrc(target, 15,
+                    0, 0,   /* op1, op2 */
+                    2, 0,   /* CRn, CRm */
+                    &ttb);
+    }
+    /* we don't know whose address is: user or kernel
+       we assume that if we are in kernel mode then
+       address belongs to kernel else if in user mode
+       - to user */
+    else if(armv7a->armv4_5_common.core_mode == ARM_MODE_SVC)
+    {
+        /* MRC p15,0,<Rt>,c1,c0,0 ; Read CP15 System Control Register */
+        retval = armv7a->armv4_5_common.mrc(target, 15,
+                    0, 1,   /* op1, op2 */
+                    2, 0,   /* CRn, CRm */
+                    &ttb);
+    }
+    else if(armv7a->armv4_5_common.core_mode == ARM_MODE_USR)
+    {
+        /* MRC p15,0,<Rt>,c1,c0,0 ; Read CP15 System Control Register */
+        retval = armv7a->armv4_5_common.mrc(target, 15,
+                    0, 0,   /* op1, op2 */
+                    2, 0,   /* CRn, CRm */
+                    &ttb);
+    }
+    /* finaly we don't know whose ttb to use: user or kernel */
+    else
+        LOG_ERROR("Don't know how to get ttb for current mode!!!");
+
+    ttb &= 0xffffc000;
+
+    return ttb;
+}
+
+static void cortex_a8_disable_mmu_caches(struct target *target, int mmu,
+                int d_u_cache, int i_cache)
+{
+    struct cortex_a8_common *cortex_a8 = target_to_cortex_a8(target);
+    struct armv7a_common *armv7a = &cortex_a8->armv7a_common;
+    uint32_t cp15_control;
+
+    /* read cp15 control register */
+    armv7a->armv4_5_common.mrc(target, 15,
+                    0, 0,   /* op1, op2 */
+                    1, 0,   /* CRn, CRm */
+                    &cp15_control);
+
+
+    if (mmu)
+            cp15_control &= ~0x1U;
+
+    if (d_u_cache)
+            cp15_control &= ~0x4U;
+
+    if (i_cache)
+            cp15_control &= ~0x1000U;
+
+    armv7a->armv4_5_common.mcr(target, 15,
+                    0, 0,   /* op1, op2 */
+                    1, 0,   /* CRn, CRm */
+                    cp15_control);
+}
+
+static void cortex_a8_enable_mmu_caches(struct target *target, int mmu,
+                int d_u_cache, int i_cache)
+{
+    struct cortex_a8_common *cortex_a8 = target_to_cortex_a8(target);
+    struct armv7a_common *armv7a = &cortex_a8->armv7a_common;
+    uint32_t cp15_control;
+
+    /* read cp15 control register */
+    armv7a->armv4_5_common.mrc(target, 15,
+                    0, 0,   /* op1, op2 */
+                    1, 0,   /* CRn, CRm */
+                    &cp15_control);
+
+    if (mmu)
+            cp15_control |= 0x1U;
+
+    if (d_u_cache)
+            cp15_control |= 0x4U;
+
+    if (i_cache)
+            cp15_control |= 0x1000U;
+
+    armv7a->armv4_5_common.mcr(target, 15,
+                    0, 0,   /* op1, op2 */
+                    1, 0,   /* CRn, CRm */
+                    cp15_control);
+}
+
+
+static int cortex_a8_mmu(struct target *target, int *enabled)
+{
+	if (target->state != TARGET_HALTED) {
+		LOG_ERROR("%s: target not halted", __func__);
+		return ERROR_TARGET_INVALID;
+	}
+
+	*enabled = target_to_cortex_a8(target)->armv7a_common.armv4_5_mmu.mmu_enabled;
+	return ERROR_OK;
+}
+
+static int cortex_a8_virt2phys(struct target *target,
+		uint32_t virt, uint32_t *phys)
+{
+	int type;
+	uint32_t cb;
+	int domain;
+	uint32_t ap;
+	struct cortex_a8_common *cortex_a8 = target_to_cortex_a8(target);
+	// struct armv7a_common *armv7a = &cortex_a8->armv7a_common;
+	struct armv7a_common *armv7a = target_to_armv7a(target);
+
+    /* We assume that virtual address is separated
+       between user and kernel in Linux style:
+       0x00000000-0xbfffffff - User space
+       0xc0000000-0xffffffff - Kernel space */
+    if( virt < 0xc0000000 ) /* Linux user space */
+        cortex_a8->current_address_mode = ARM_MODE_USR;
+    else /* Linux kernel */
+        cortex_a8->current_address_mode = ARM_MODE_SVC;
+	uint32_t ret = armv4_5_mmu_translate_va(target,
+			&armv7a->armv4_5_mmu, virt, &type, &cb, &domain, &ap);
+    /* Reset the flag. We don't want someone else to use it by error */
+    cortex_a8->current_address_mode = ARM_MODE_ANY;
+
+	if (type == -1)
+	{
+		return ret;
+	}
+	*phys = ret;
+	return ERROR_OK;
+}
+
 COMMAND_HANDLER(cortex_a8_handle_cache_info_command)
 {
 	struct target *target = get_current_target(CMD_CTX);
@@ -1703,4 +1917,10 @@ struct target_type cortexa8_target = {
 	.target_create = cortex_a8_target_create,
 	.init_target = cortex_a8_init_target,
 	.examine = cortex_a8_examine,
+
+	.read_phys_memory = cortex_a8_read_phys_memory,
+	.write_phys_memory = cortex_a8_write_phys_memory,
+	.mmu = cortex_a8_mmu,
+	.virt2phys = cortex_a8_virt2phys,
+
 };

src/target/cortex_a8.h

@@ -72,6 +72,9 @@ struct cortex_a8_common
 	/* Use cortex_a8_read_regs_through_mem for fast register reads */
 	int fast_reg_read;
 
+	/* Flag that helps to resolve what ttb to use: user or kernel */
+	int current_address_mode;
+
 	struct armv7a_common armv7a_common;
 };