diff options
Diffstat (limited to 'src/target/mips32_pracc.c')
-rw-r--r-- | src/target/mips32_pracc.c | 106 |
1 files changed, 105 insertions, 1 deletions
diff --git a/src/target/mips32_pracc.c b/src/target/mips32_pracc.c index 22edf6a..aaf3875 100644 --- a/src/target/mips32_pracc.c +++ b/src/target/mips32_pracc.c @@ -588,6 +588,26 @@ int mips32_cp0_write(struct mips_ejtag *ejtag_info, uint32_t val, uint32_t cp0_r return ctx.retval; } +int mips32_cp1_control_read(struct mips_ejtag *ejtag_info, uint32_t *val, uint32_t cp1_c_reg) +{ + struct pracc_queue_info ctx = {.ejtag_info = ejtag_info}; + pracc_queue_init(&ctx); + + pracc_add(&ctx, 0, MIPS32_LUI(ctx.isa, 15, PRACC_UPPER_BASE_ADDR)); /* $15 = MIPS32_PRACC_BASE_ADDR */ + pracc_add(&ctx, 0, MIPS32_EHB(ctx.isa)); + pracc_add(&ctx, 0, MIPS32_CFC1(ctx.isa, 8, cp1_c_reg)); /* move cp1c reg to $8 */ + pracc_add(&ctx, MIPS32_PRACC_PARAM_OUT, + MIPS32_SW(ctx.isa, 8, PRACC_OUT_OFFSET, 15)); /* store $8 to pracc_out */ + pracc_add(&ctx, 0, MIPS32_MFC0(ctx.isa, 15, 31, 0)); /* restore $15 from DeSave */ + pracc_add(&ctx, 0, MIPS32_LUI(ctx.isa, 8, UPPER16(ejtag_info->reg8))); /* restore upper 16 bits of $8 */ + pracc_add(&ctx, 0, MIPS32_B(ctx.isa, NEG16((ctx.code_count + 1) << ctx.isa))); /* jump to start */ + pracc_add(&ctx, 0, MIPS32_ORI(ctx.isa, 8, 8, LOWER16(ejtag_info->reg8))); /* restore lower 16 bits of $8 */ + + ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, val, 1); + pracc_queue_free(&ctx); + return ctx.retval; +} + /** * \b mips32_pracc_sync_cache * @@ -856,6 +876,9 @@ int mips32_pracc_write_regs(struct mips32_common *mips32) struct pracc_queue_info ctx = {.ejtag_info = ejtag_info}; uint32_t *gprs = mips32->core_regs.gpr; uint32_t *c0rs = mips32->core_regs.cp0; + bool fpu_in_64bit = ((c0rs[0] & BIT(MIPS32_CP0_STATUS_FR_SHIFT)) != 0); + bool fp_enabled = ((c0rs[0] & BIT(MIPS32_CP0_STATUS_CU1_SHIFT)) != 0); + uint32_t rel = (ejtag_info->config[0] & MIPS32_CONFIG0_AR_MASK) >> MIPS32_CONFIG0_AR_SHIFT; pracc_queue_init(&ctx); @@ -895,6 +918,31 @@ int mips32_pracc_write_regs(struct mips32_common *mips32) if (mips32_cpu_support_hazard_barrier(ejtag_info)) pracc_add(&ctx, 0, MIPS32_EHB(ctx.isa)); + + /* store FPRs */ + if (mips32->fp_imp && fp_enabled) { + uint64_t *fprs = mips32->core_regs.fpr; + if (fpu_in_64bit) { + for (int i = 0; i != MIPS32_REG_FP_COUNT; i++) { + uint32_t fp_lo = fprs[i] & 0xffffffff; + uint32_t fp_hi = (fprs[i] >> 32) & 0xffffffff; + pracc_add_li32(&ctx, 2, fp_lo, 0); + pracc_add_li32(&ctx, 3, fp_hi, 0); + pracc_add(&ctx, 0, MIPS32_MTC1(ctx.isa, 2, i)); + pracc_add(&ctx, 0, MIPS32_MTHC1(ctx.isa, 3, i)); + } + } else { + for (int i = 0; i != MIPS32_REG_FP_COUNT; i++) { + uint32_t fp_lo = fprs[i] & 0xffffffff; + pracc_add_li32(&ctx, 2, fp_lo, 0); + pracc_add(&ctx, 0, MIPS32_MTC1(ctx.isa, 2, i)); + } + } + + if (rel > MIPS32_RELEASE_1) + pracc_add(&ctx, 0, MIPS32_EHB(ctx.isa)); + } + /* load registers 2 to 31 with li32, optimize */ for (int i = 2; i < 32; i++) pracc_add_li32(&ctx, i, gprs[i], 1); @@ -1014,6 +1062,9 @@ int mips32_pracc_read_regs(struct mips32_common *mips32) struct mips32_core_regs *core_regs = &mips32->core_regs; unsigned int offset_gpr = ((uint8_t *)&core_regs->gpr[0]) - (uint8_t *)core_regs; unsigned int offset_cp0 = ((uint8_t *)&core_regs->cp0[0]) - (uint8_t *)core_regs; + unsigned int offset_fpr = ((uint8_t *)&core_regs->fpr[0]) - (uint8_t *)core_regs; + unsigned int offset_fpcr = ((uint8_t *)&core_regs->fpcr[0]) - (uint8_t *)core_regs; + bool fp_enabled; /* * This procedure has to be in 2 distinctive steps, because we can @@ -1040,11 +1091,64 @@ int mips32_pracc_read_regs(struct mips32_common *mips32) ejtag_info->reg8 = mips32->core_regs.gpr[8]; ejtag_info->reg9 = mips32->core_regs.gpr[9]; + if (ctx.retval != ERROR_OK) + return ctx.retval; + /* we only care if FP is actually impl'd and if cp1 is enabled */ /* since we already read cp0 in the prev step */ /* now we know what's in cp0.status */ - /* TODO: Read FPRs */ + fp_enabled = (mips32->core_regs.cp0[0] & BIT(MIPS32_CP0_STATUS_CU1_SHIFT)) != 0; + if (mips32->fp_imp && fp_enabled) { + pracc_queue_init(&ctx); + mips32_pracc_store_regs_set_base_addr(&ctx); + + /* FCSR */ + pracc_add(&ctx, 0, MIPS32_CFC1(ctx.isa, 8, 31)); + pracc_add(&ctx, MIPS32_PRACC_PARAM_OUT + offset_fpcr, + MIPS32_SW(ctx.isa, 8, PRACC_OUT_OFFSET + offset_fpcr, 1)); + + /* FIR */ + pracc_add(&ctx, 0, MIPS32_CFC1(ctx.isa, 8, 0)); + pracc_add(&ctx, MIPS32_PRACC_PARAM_OUT + offset_fpcr + 4, + MIPS32_SW(ctx.isa, 8, PRACC_OUT_OFFSET + offset_fpcr + 4, 1)); + + /* f0 to f31 */ + if (mips32->fpu_in_64bit) { + for (int i = 0; i != 32; i++) { + size_t offset = offset_fpr + (i * 8); + /* current pracc implementation (or EJTAG itself) only supports 32b access */ + /* so there is no way to use SDC1 */ + + /* lower half */ + pracc_add(&ctx, 0, MIPS32_MFC1(ctx.isa, 8, i)); + pracc_add(&ctx, MIPS32_PRACC_PARAM_OUT + offset, + MIPS32_SW(ctx.isa, 8, PRACC_OUT_OFFSET + offset, 1)); + + /* upper half */ + pracc_add(&ctx, 0, MIPS32_MFHC1(ctx.isa, 8, i)); + pracc_add(&ctx, MIPS32_PRACC_PARAM_OUT + offset + 4, + MIPS32_SW(ctx.isa, 8, PRACC_OUT_OFFSET + offset + 4, 1)); + } + } else { + for (int i = 0; i != 32; i++) { + size_t offset = offset_fpr + (i * 8); + pracc_add(&ctx, MIPS32_PRACC_PARAM_OUT + offset, + MIPS32_SWC1(ctx.isa, i, PRACC_OUT_OFFSET + offset, 1)); + } + } + + mips32_pracc_store_regs_restore(&ctx); + + /* jump to start */ + pracc_add(&ctx, 0, MIPS32_B(ctx.isa, NEG16((ctx.code_count + 1) << ctx.isa))); + /* load $15 in DeSave */ + pracc_add(&ctx, 0, MIPS32_MTC0(ctx.isa, 15, 31, 0)); + + ctx.retval = mips32_pracc_queue_exec(ejtag_info, &ctx, (uint32_t *)&mips32->core_regs, 1); + + pracc_queue_free(&ctx); + } return ctx.retval; } |