Index: sys/powerpc/aim/mp_cpudep.c =================================================================== --- sys/powerpc/aim/mp_cpudep.c +++ sys/powerpc/aim/mp_cpudep.c @@ -91,7 +91,7 @@ mtspr(SPR_LPID, 0); isync(); - mtspr(SPR_LPCR, LPCR_LPES); + mtspr(SPR_LPCR, LPCR_LPES | LPCR_PECE_SET); isync(); break; } Index: sys/powerpc/aim/trap_subr64.S =================================================================== --- sys/powerpc/aim/trap_subr64.S +++ sys/powerpc/aim/trap_subr64.S @@ -298,10 +298,24 @@ * once the MMU is turned on. */ .globl CNAME(rstcode), CNAME(rstcodeend), CNAME(cpu_reset_handler) + .globl CNAME(cpu_wakeup_handler) .p2align 3 CNAME(rstcode): + /* + * Check if this is software reset or + * processor is waking up from power saving mode + * It is software reset when 46:47 = 0b00 + */ + mfsrr1 %r9 /* Load SRR1 into r9 */ + andis. %r9,%r9,0x3 /* Logic AND with 0x30000 */ + beq 2f /* Branch if software reset */ + bl 1f + .llong cpu_wakeup_handler + + /* It is software reset */ + /* Explicitly set MSR[SF] */ - mfmsr %r9 +2: mfmsr %r9 li %r8,1 insrdi %r9,%r8,1,0 mtmsrd %r9 @@ -309,6 +323,7 @@ bl 1f .llong cpu_reset_handler /* Make sure to maintain 8-byte alignment */ + 1: mflr %r9 ld %r9,0(%r9) mtlr %r9 @@ -350,6 +365,51 @@ 9: b 9b +cpu_wakeup_handler: + /* Turn on MMU after return from interrupt */ + mfsrr1 %r3 + ori %r3,%r3,(PSL_IR | PSL_DR) + mtsrr1 %r3 + + /* Turn on MMU (needed to access PCB) */ + mfmsr %r3 + ori %r3,%r3,(PSL_IR | PSL_DR) + mtmsr %r3 + isync + + mfsprg0 %r3 + + ld %r3,PC_CURTHREAD(%r3) /* Get current thread */ + ld %r3,TD_PCB(%r3) /* Get PCB of current thread */ + ld %r12,PCB_CONTEXT(%r3) /* Load the non-volatile GP regs. */ + ld %r13,PCB_CONTEXT+1*8(%r3) + ld %r14,PCB_CONTEXT+2*8(%r3) + ld %r15,PCB_CONTEXT+3*8(%r3) + ld %r16,PCB_CONTEXT+4*8(%r3) + ld %r17,PCB_CONTEXT+5*8(%r3) + ld %r18,PCB_CONTEXT+6*8(%r3) + ld %r19,PCB_CONTEXT+7*8(%r3) + ld %r20,PCB_CONTEXT+8*8(%r3) + ld %r21,PCB_CONTEXT+9*8(%r3) + ld %r22,PCB_CONTEXT+10*8(%r3) + ld %r23,PCB_CONTEXT+11*8(%r3) + ld %r24,PCB_CONTEXT+12*8(%r3) + ld %r25,PCB_CONTEXT+13*8(%r3) + ld %r26,PCB_CONTEXT+14*8(%r3) + ld %r27,PCB_CONTEXT+15*8(%r3) + ld %r28,PCB_CONTEXT+16*8(%r3) + ld %r29,PCB_CONTEXT+17*8(%r3) + ld %r30,PCB_CONTEXT+18*8(%r3) + ld %r31,PCB_CONTEXT+19*8(%r3) + ld %r5,PCB_CR(%r3) /* Load the condition register */ + mtcr %r5 + ld %r5,PCB_LR(%r3) /* Load the link register */ + mtsrr0 %r5 + ld %r1,PCB_SP(%r3) /* Load the stack pointer */ + ld %r2,PCB_TOC(%r3) /* Load the TOC pointer */ + + rfid + /* * This code gets copied to all the trap vectors * (except ISI/DSI, ALI, and the interrupts). Has to fit in 8 instructions! @@ -872,3 +932,67 @@ blrl /* Branch to generictrap */ CNAME(dbend): #endif /* KDB */ + + .globl CNAME(power_save_sequence) + .p2align 3 +ENTRY(enter_power_save) + mfsprg0 %r3 /* Get the pcpu pointer */ + ld %r3,PC_CURTHREAD(%r3) /* Get current thread */ + ld %r3,TD_PCB(%r3) /* Get PCB of current thread */ + std %r12,PCB_CONTEXT(%r3) /* Save the non-volatile GP regs. */ + std %r13,PCB_CONTEXT+1*8(%r3) + std %r14,PCB_CONTEXT+2*8(%r3) + std %r15,PCB_CONTEXT+3*8(%r3) + std %r16,PCB_CONTEXT+4*8(%r3) + std %r17,PCB_CONTEXT+5*8(%r3) + std %r18,PCB_CONTEXT+6*8(%r3) + std %r19,PCB_CONTEXT+7*8(%r3) + std %r20,PCB_CONTEXT+8*8(%r3) + std %r21,PCB_CONTEXT+9*8(%r3) + std %r22,PCB_CONTEXT+10*8(%r3) + std %r23,PCB_CONTEXT+11*8(%r3) + std %r24,PCB_CONTEXT+12*8(%r3) + std %r25,PCB_CONTEXT+13*8(%r3) + std %r26,PCB_CONTEXT+14*8(%r3) + std %r27,PCB_CONTEXT+15*8(%r3) + std %r28,PCB_CONTEXT+16*8(%r3) + std %r29,PCB_CONTEXT+17*8(%r3) + std %r30,PCB_CONTEXT+18*8(%r3) + std %r31,PCB_CONTEXT+19*8(%r3) + + mfcr %r16 /* Save the condition register */ + std %r16,PCB_CR(%r3) + mflr %r16 /* Save the link register */ + std %r16,PCB_LR(%r3) + std %r1,PCB_SP(%r3) /* Save the stack pointer */ + std %r2,PCB_TOC(%r3) /* Save the TOC pointer */ + + /* Set where we want to jump */ + bl 1f + .llong power_save_sequence /* Remember about 8 byte alignment */ +1: mflr %r3 + ld %r3,0(%r3) + mtsrr0 %r3 + + /* Set MSR */ + li %r3,0 + ori %r3,%r3,(PSL_ME | PSL_RI) + li %r8,0x9 /* PSL_SF and PSL_HV */ + insrdi %r3,%r8,4,0 + mtsrr1 %r3 + + rfid + +CNAME(power_save_sequence): + bl 1f + .llong 0x0 /* Playground for power-save sequence */ +1: mflr %r3 + + /* Start power-save sequence */ + std %r2,0(%r3) + ptesync + ld %r2,0(%r3) +2: cmpd %r2,%r2 + bne 2b + nap + b . Index: sys/powerpc/include/spr.h =================================================================== --- sys/powerpc/include/spr.h +++ sys/powerpc/include/spr.h @@ -201,6 +201,13 @@ #define SPR_LPCR 0x13e /* Logical Partitioning Control */ #define LPCR_LPES 0x008 /* Bit 60 */ +#define LPCR_PECE_DRBL (1ULL << 16) /* Directed Privileged Doorbell */ +#define LPCR_PECE_HDRBL (1ULL << 15) /* Directed Hypervisor Doorbell */ +#define LPCR_PECE_EXT (1ULL << 14) /* External exceptions */ +#define LPCR_PECE_DECR (1ULL << 13) /* Decrementer exceptions */ +#define LPCR_PECE_ME (1ULL << 12) /* Machine Check and Hypervisor */ + /* Maintenance exceptions */ +#define LPCR_PECE_SET (LPCR_PECE_EXT | LPCR_PECE_DECR | LPCR_PECE_ME) #define SPR_EPCR 0x133 #define EPCR_EXTGS 0x80000000 Index: sys/powerpc/powernv/platform_powernv.c =================================================================== --- sys/powerpc/powernv/platform_powernv.c +++ sys/powerpc/powernv/platform_powernv.c @@ -34,6 +34,7 @@ #include #include #include +#include #include #include #include @@ -55,6 +56,8 @@ #include "platform_if.h" #include "opal.h" +extern void enter_power_save(void); + static int powernv_probe(platform_t); static int powernv_attach(platform_t); void powernv_mem_regions(platform_t, struct mem_region *phys, int *physsz, @@ -146,11 +149,11 @@ mtspr(SPR_LPID, 0); isync(); - mtmsr(msr); - - mtspr(SPR_LPCR, LPCR_LPES); + mtspr(SPR_LPCR, LPCR_LPES | LPCR_PECE_SET); isync(); + mtmsr(msr); + /* Init CPU bits */ powernv_smp_ap_init(plat); @@ -459,4 +462,11 @@ static void powernv_cpu_idle(sbintime_t sbt) { + + if (sched_runnable()) + return; + + spinlock_enter(); + enter_power_save(); + spinlock_exit(); }