From ae4ce45419f908cf884d3fdc37f5706972068d34 Mon Sep 17 00:00:00 2001 From: James Hogan Date: Tue, 4 Mar 2014 10:20:43 +0000 Subject: [PATCH] MIPS: traps: Add CPU PM callback for trap configuration Implement a CPU power management callback for restoring trap related CPU configuration after CPU power up from a low power state. The following state is restored: - Status register - HWREna register - Exception vector configuration registers - Context/XContext register Signed-off-by: James Hogan Signed-off-by: Paul Burton --- arch/mips/include/asm/mmu_context.h | 15 ++++-- arch/mips/kernel/traps.c | 93 ++++++++++++++++++++++++++++--------- 2 files changed, 81 insertions(+), 27 deletions(-) diff --git a/arch/mips/include/asm/mmu_context.h b/arch/mips/include/asm/mmu_context.h index e277bbad2871..ecae1dc260fd 100644 --- a/arch/mips/include/asm/mmu_context.h +++ b/arch/mips/include/asm/mmu_context.h @@ -31,11 +31,15 @@ do { \ } while (0) #ifdef CONFIG_MIPS_PGD_C0_CONTEXT + +#define TLBMISS_HANDLER_RESTORE() \ + write_c0_xcontext((unsigned long) smp_processor_id() << \ + SMP_CPUID_REGSHIFT) + #define TLBMISS_HANDLER_SETUP() \ do { \ TLBMISS_HANDLER_SETUP_PGD(swapper_pg_dir); \ - write_c0_xcontext((unsigned long) smp_processor_id() << \ - SMP_CPUID_REGSHIFT); \ + TLBMISS_HANDLER_RESTORE(); \ } while (0) #else /* !CONFIG_MIPS_PGD_C0_CONTEXT: using pgd_current*/ @@ -47,9 +51,12 @@ do { \ */ extern unsigned long pgd_current[]; -#define TLBMISS_HANDLER_SETUP() \ +#define TLBMISS_HANDLER_RESTORE() \ write_c0_context((unsigned long) smp_processor_id() << \ - SMP_CPUID_REGSHIFT); \ + SMP_CPUID_REGSHIFT) + +#define TLBMISS_HANDLER_SETUP() \ + TLBMISS_HANDLER_RESTORE(); \ back_to_back_c0_hazard(); \ TLBMISS_HANDLER_SETUP_PGD(swapper_pg_dir) #endif /* CONFIG_MIPS_PGD_C0_CONTEXT*/ diff --git a/arch/mips/kernel/traps.c b/arch/mips/kernel/traps.c index 074e857ced28..9651f68c2116 100644 --- a/arch/mips/kernel/traps.c +++ b/arch/mips/kernel/traps.c @@ -15,6 +15,7 @@ #include #include #include +#include #include #include #include @@ -1865,32 +1866,16 @@ static int __init ulri_disable(char *s) } __setup("noulri", ulri_disable); -void per_cpu_trap_init(bool is_boot_cpu) +/* configure STATUS register */ +static void configure_status(void) { - unsigned int cpu = smp_processor_id(); - unsigned int status_set = ST0_CU0; - unsigned int hwrena = cpu_hwrena_impl_bits; -#ifdef CONFIG_MIPS_MT_SMTC - int secondaryTC = 0; - int bootTC = (cpu == 0); - - /* - * Only do per_cpu_trap_init() for first TC of Each VPE. - * Note that this hack assumes that the SMTC init code - * assigns TCs consecutively and in ascending order. - */ - - if (((read_c0_tcbind() & TCBIND_CURTC) != 0) && - ((read_c0_tcbind() & TCBIND_CURVPE) == cpu_data[cpu - 1].vpe_id)) - secondaryTC = 1; -#endif /* CONFIG_MIPS_MT_SMTC */ - /* * Disable coprocessors and select 32-bit or 64-bit addressing * and the 16/32 or 32/32 FPR register model. Reset the BEV * flag that some firmware may have left set and the TS bit (for * IP27). Set XX for ISA IV code to work. */ + unsigned int status_set = ST0_CU0; #ifdef CONFIG_64BIT status_set |= ST0_FR|ST0_KX|ST0_SX|ST0_UX; #endif @@ -1901,6 +1886,12 @@ void per_cpu_trap_init(bool is_boot_cpu) change_c0_status(ST0_CU|ST0_MX|ST0_RE|ST0_FR|ST0_BEV|ST0_TS|ST0_KX|ST0_SX|ST0_UX, status_set); +} + +/* configure HWRENA register */ +static void configure_hwrena(void) +{ + unsigned int hwrena = cpu_hwrena_impl_bits; if (cpu_has_mips_r2) hwrena |= 0x0000000f; @@ -1910,11 +1901,10 @@ void per_cpu_trap_init(bool is_boot_cpu) if (hwrena) write_c0_hwrena(hwrena); +} -#ifdef CONFIG_MIPS_MT_SMTC - if (!secondaryTC) { -#endif /* CONFIG_MIPS_MT_SMTC */ - +static void configure_exception_vector(void) +{ if (cpu_has_veic || cpu_has_vint) { unsigned long sr = set_c0_status(ST0_BEV); write_c0_ebase(ebase); @@ -1930,6 +1920,34 @@ void per_cpu_trap_init(bool is_boot_cpu) } else set_c0_cause(CAUSEF_IV); } +} + +void per_cpu_trap_init(bool is_boot_cpu) +{ + unsigned int cpu = smp_processor_id(); +#ifdef CONFIG_MIPS_MT_SMTC + int secondaryTC = 0; + int bootTC = (cpu == 0); + + /* + * Only do per_cpu_trap_init() for first TC of Each VPE. + * Note that this hack assumes that the SMTC init code + * assigns TCs consecutively and in ascending order. + */ + + if (((read_c0_tcbind() & TCBIND_CURTC) != 0) && + ((read_c0_tcbind() & TCBIND_CURVPE) == cpu_data[cpu - 1].vpe_id)) + secondaryTC = 1; +#endif /* CONFIG_MIPS_MT_SMTC */ + + configure_status(); + configure_hwrena(); + +#ifdef CONFIG_MIPS_MT_SMTC + if (!secondaryTC) { +#endif /* CONFIG_MIPS_MT_SMTC */ + + configure_exception_vector(); /* * Before R2 both interrupt numbers were fixed to 7, so on R2 only: @@ -2185,3 +2203,32 @@ void __init trap_init(void) cu2_notifier(default_cu2_call, 0x80000000); /* Run last */ } + +static int trap_pm_notifier(struct notifier_block *self, unsigned long cmd, + void *v) +{ + switch (cmd) { + case CPU_PM_ENTER_FAILED: + case CPU_PM_EXIT: + configure_status(); + configure_hwrena(); + configure_exception_vector(); + + /* Restore register with CPU number for TLB handlers */ + TLBMISS_HANDLER_RESTORE(); + + break; + } + + return NOTIFY_OK; +} + +static struct notifier_block trap_pm_notifier_block = { + .notifier_call = trap_pm_notifier, +}; + +static int __init trap_pm_init(void) +{ + return cpu_pm_register_notifier(&trap_pm_notifier_block); +} +arch_initcall(trap_pm_init); -- 2.11.0