+
+ if ((output = fopen(args[0], "w")) == NULL)
+ {
+ LOG_DEBUG("error opening mmu content file");
+ return ERROR_OK;
+ }
+
+ for (i = 0; i < 16; i++)
+ regs_p[i] = ®s[i];
+
+ /* disable MMU and Caches */
+ arm920t_read_cp15_physical(target, ARM920T_CP15_PHYS_ADDR(0, 0x1, 0), &cp15_ctrl);
+ if ((retval = jtag_execute_queue()) != ERROR_OK)
+ {
+ return retval;
+ }
+ cp15_ctrl_saved = cp15_ctrl;
+ cp15_ctrl &= ~(ARMV4_5_MMU_ENABLED | ARMV4_5_D_U_CACHE_ENABLED | ARMV4_5_I_CACHE_ENABLED);
+ arm920t_write_cp15_physical(target, ARM920T_CP15_PHYS_ADDR(0, 0x1, 0), cp15_ctrl);
+
+ /* read CP15 test state register */
+ arm920t_read_cp15_physical(target, ARM920T_CP15_PHYS_ADDR(0, 0xf, 0), &cp15c15);
+ if ((retval = jtag_execute_queue()) != ERROR_OK)
+ {
+ return retval;
+ }
+
+ /* prepare reading D TLB content
+ * */
+
+ /* set interpret mode */
+ cp15c15 |= 0x1;
+ arm920t_write_cp15_physical(target, ARM920T_CP15_PHYS_ADDR(0, 0xf, 0), cp15c15);
+
+ /* Read D TLB lockdown */
+ arm920t_execute_cp15(target, ARMV4_5_MRC(15,0,0,10,0,0), ARMV4_5_LDR(1, 0));
+
+ /* clear interpret mode */
+ cp15c15 &= ~0x1;
+ arm920t_write_cp15_physical(target, 0x1e, cp15c15);
+
+ /* read D TLB lockdown stored to r1 */
+ arm9tdmi_read_core_regs(target, 0x2, regs_p);
+ if ((retval = jtag_execute_queue()) != ERROR_OK)
+ {
+ return retval;
+ }
+ Dlockdown = regs[1];
+
+ for (victim = 0; victim < 64; victim += 8)
+ {
+ /* new lockdown value: base[31:26]:victim[25:20]:SBZ[19:1]:p[0]
+ * base remains unchanged, victim goes through entries 0 to 63 */
+ regs[1] = (Dlockdown & 0xfc000000) | (victim << 20);
+ arm9tdmi_write_core_regs(target, 0x2, regs);
+
+ /* set interpret mode */
+ cp15c15 |= 0x1;
+ arm920t_write_cp15_physical(target, ARM920T_CP15_PHYS_ADDR(0, 0xf, 0), cp15c15);
+
+ /* Write D TLB lockdown */
+ arm920t_execute_cp15(target, ARMV4_5_MCR(15,0,0,10,0,0), ARMV4_5_STR(1, 0));
+
+ /* Read D TLB CAM */
+ arm920t_execute_cp15(target, ARMV4_5_MCR(15,4,0,15,6,4), ARMV4_5_LDMIA(0, 0x3fc, 0, 0));
+
+ /* clear interpret mode */
+ cp15c15 &= ~0x1;
+ arm920t_write_cp15_physical(target, 0x1e, cp15c15);
+
+ /* read D TLB CAM content stored to r2-r9 */
+ arm9tdmi_read_core_regs(target, 0x3fc, regs_p);
+ if ((retval = jtag_execute_queue()) != ERROR_OK)
+ {
+ return retval;
+ }
+
+ for (i = 0; i < 8; i++)
+ d_tlb[victim + i].cam = regs[i + 2];
+ }
+
+ for (victim = 0; victim < 64; victim++)
+ {
+ /* new lockdown value: base[31:26]:victim[25:20]:SBZ[19:1]:p[0]
+ * base remains unchanged, victim goes through entries 0 to 63 */
+ regs[1] = (Dlockdown & 0xfc000000) | (victim << 20);
+ arm9tdmi_write_core_regs(target, 0x2, regs);
+
+ /* set interpret mode */
+ cp15c15 |= 0x1;
+ arm920t_write_cp15_physical(target, ARM920T_CP15_PHYS_ADDR(0, 0xf, 0), cp15c15);
+
+ /* Write D TLB lockdown */
+ arm920t_execute_cp15(target, ARMV4_5_MCR(15,0,0,10,0,0), ARMV4_5_STR(1, 0));
+
+ /* Read D TLB RAM1 */
+ arm920t_execute_cp15(target, ARMV4_5_MCR(15,4,0,15,10,4), ARMV4_5_LDR(2,0));
+
+ /* Read D TLB RAM2 */
+ arm920t_execute_cp15(target, ARMV4_5_MCR(15,4,0,15,2,5), ARMV4_5_LDR(3,0));
+
+ /* clear interpret mode */
+ cp15c15 &= ~0x1;
+ arm920t_write_cp15_physical(target, 0x1e, cp15c15);
+
+ /* read D TLB RAM content stored to r2 and r3 */
+ arm9tdmi_read_core_regs(target, 0xc, regs_p);
+ if ((retval = jtag_execute_queue()) != ERROR_OK)
+ {
+ return retval;
+ }
+
+ d_tlb[victim].ram1 = regs[2];
+ d_tlb[victim].ram2 = regs[3];
+ }
+
+ /* restore D TLB lockdown */
+ regs[1] = Dlockdown;
+ arm9tdmi_write_core_regs(target, 0x2, regs);
+
+ /* Write D TLB lockdown */
+ arm920t_execute_cp15(target, ARMV4_5_MCR(15,0,0,10,0,0), ARMV4_5_STR(1, 0));
+
+ /* prepare reading I TLB content
+ * */
+
+ /* set interpret mode */
+ cp15c15 |= 0x1;
+ arm920t_write_cp15_physical(target, ARM920T_CP15_PHYS_ADDR(0, 0xf, 0), cp15c15);
+
+ /* Read I TLB lockdown */
+ arm920t_execute_cp15(target, ARMV4_5_MRC(15,0,0,10,0,1), ARMV4_5_LDR(1, 0));
+
+ /* clear interpret mode */
+ cp15c15 &= ~0x1;
+ arm920t_write_cp15_physical(target, 0x1e, cp15c15);
+
+ /* read I TLB lockdown stored to r1 */
+ arm9tdmi_read_core_regs(target, 0x2, regs_p);
+ if ((retval = jtag_execute_queue()) != ERROR_OK)
+ {
+ return retval;
+ }
+ Ilockdown = regs[1];
+
+ for (victim = 0; victim < 64; victim += 8)
+ {
+ /* new lockdown value: base[31:26]:victim[25:20]:SBZ[19:1]:p[0]
+ * base remains unchanged, victim goes through entries 0 to 63 */
+ regs[1] = (Ilockdown & 0xfc000000) | (victim << 20);
+ arm9tdmi_write_core_regs(target, 0x2, regs);
+
+ /* set interpret mode */
+ cp15c15 |= 0x1;
+ arm920t_write_cp15_physical(target, ARM920T_CP15_PHYS_ADDR(0, 0xf, 0), cp15c15);
+
+ /* Write I TLB lockdown */
+ arm920t_execute_cp15(target, ARMV4_5_MCR(15,0,0,10,0,1), ARMV4_5_STR(1, 0));
+
+ /* Read I TLB CAM */
+ arm920t_execute_cp15(target, ARMV4_5_MCR(15,4,0,15,5,4), ARMV4_5_LDMIA(0, 0x3fc, 0, 0));
+
+ /* clear interpret mode */
+ cp15c15 &= ~0x1;
+ arm920t_write_cp15_physical(target, 0x1e, cp15c15);
+
+ /* read I TLB CAM content stored to r2-r9 */
+ arm9tdmi_read_core_regs(target, 0x3fc, regs_p);
+ if ((retval = jtag_execute_queue()) != ERROR_OK)
+ {
+ return retval;
+ }
+
+ for (i = 0; i < 8; i++)
+ i_tlb[i + victim].cam = regs[i + 2];
+ }
+
+ for (victim = 0; victim < 64; victim++)
+ {
+ /* new lockdown value: base[31:26]:victim[25:20]:SBZ[19:1]:p[0]
+ * base remains unchanged, victim goes through entries 0 to 63 */
+ regs[1] = (Dlockdown & 0xfc000000) | (victim << 20);
+ arm9tdmi_write_core_regs(target, 0x2, regs);
+
+ /* set interpret mode */
+ cp15c15 |= 0x1;
+ arm920t_write_cp15_physical(target, ARM920T_CP15_PHYS_ADDR(0, 0xf, 0), cp15c15);
+
+ /* Write I TLB lockdown */
+ arm920t_execute_cp15(target, ARMV4_5_MCR(15,0,0,10,0,1), ARMV4_5_STR(1, 0));
+
+ /* Read I TLB RAM1 */
+ arm920t_execute_cp15(target, ARMV4_5_MCR(15,4,0,15,9,4), ARMV4_5_LDR(2,0));
+
+ /* Read I TLB RAM2 */
+ arm920t_execute_cp15(target, ARMV4_5_MCR(15,4,0,15,1,5), ARMV4_5_LDR(3,0));
+
+ /* clear interpret mode */
+ cp15c15 &= ~0x1;
+ arm920t_write_cp15_physical(target, 0x1e, cp15c15);
+
+ /* read I TLB RAM content stored to r2 and r3 */
+ arm9tdmi_read_core_regs(target, 0xc, regs_p);
+ if ((retval = jtag_execute_queue()) != ERROR_OK)
+ {
+ return retval;
+ }
+
+ i_tlb[victim].ram1 = regs[2];
+ i_tlb[victim].ram2 = regs[3];
+ }
+
+ /* restore I TLB lockdown */
+ regs[1] = Ilockdown;
+ arm9tdmi_write_core_regs(target, 0x2, regs);
+
+ /* Write I TLB lockdown */
+ arm920t_execute_cp15(target, ARMV4_5_MCR(15,0,0,10,0,1), ARMV4_5_STR(1, 0));
+
+ /* restore CP15 MMU and Cache settings */
+ arm920t_write_cp15_physical(target, ARM920T_CP15_PHYS_ADDR(0, 0x1, 0), cp15_ctrl_saved);
+
+ /* output data to file */
+ fprintf(output, "D TLB content:\n");
+ for (i = 0; i < 64; i++)
+ {
+ fprintf(output, "%i: 0x%8.8" PRIx32 " 0x%8.8" PRIx32 " 0x%8.8" PRIx32 " %s\n", i, d_tlb[i].cam, d_tlb[i].ram1, d_tlb[i].ram2, (d_tlb[i].cam & 0x20) ? "(valid)" : "(invalid)");
+ }
+
+ fprintf(output, "\n\nI TLB content:\n");
+ for (i = 0; i < 64; i++)
+ {
+ fprintf(output, "%i: 0x%8.8" PRIx32 " 0x%8.8" PRIx32 " 0x%8.8" PRIx32 " %s\n", i, i_tlb[i].cam, i_tlb[i].ram1, i_tlb[i].ram2, (i_tlb[i].cam & 0x20) ? "(valid)" : "(invalid)");
+ }
+
+ command_print(cmd_ctx, "mmu content successfully output to %s", args[0]);
+
+ fclose(output);
+
+ if (armv4_5_mode_to_number(armv4_5->core_mode)==-1)
+ return ERROR_FAIL;
+
+ /* mark registers dirty */
+ ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 0).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 0).valid;
+ ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 1).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 1).valid;
+ ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 2).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 2).valid;
+ ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 3).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 3).valid;
+ ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 4).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 4).valid;
+ ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 5).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 5).valid;
+ ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 6).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 6).valid;
+ ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 7).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 7).valid;
+ ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 8).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 8).valid;
+ ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 9).dirty = ARMV4_5_CORE_REG_MODE(armv4_5->core_cache, armv4_5->core_mode, 9).valid;
+
+ return ERROR_OK;
+}
+
+COMMAND_HANDLER(arm920t_handle_cp15_command)
+{
+ int retval;
+ target_t *target = get_current_target(cmd_ctx);
+ struct arm920t_common *arm920t = target_to_arm920(target);
+
+ retval = arm920t_verify_pointer(cmd_ctx, arm920t);
+ if (retval != ERROR_OK)
+ return retval;
+