Index: head/sys/powerpc/include/pcb.h =================================================================== --- head/sys/powerpc/include/pcb.h +++ head/sys/powerpc/include/pcb.h @@ -46,14 +46,16 @@ register_t pcb_sp; /* stack pointer */ register_t pcb_toc; /* toc pointer */ register_t pcb_lr; /* link register */ + register_t pcb_dscr; /* dscr value */ struct pmap *pcb_pm; /* pmap of our vmspace */ jmp_buf *pcb_onfault; /* For use during copyin/copyout */ int pcb_flags; -#define PCB_FPU 1 /* Process uses FPU */ -#define PCB_FPREGS 2 /* Process had FPU registers initialized */ -#define PCB_VEC 4 /* Process had Altivec initialized */ -#define PCB_VSX 8 /* Process had VSX initialized */ +#define PCB_FPU 0x1 /* Process uses FPU */ +#define PCB_FPREGS 0x2 /* Process had FPU registers initialized */ +#define PCB_VEC 0x4 /* Process had Altivec initialized */ +#define PCB_VSX 0x8 /* Process had VSX initialized */ +#define PCB_CDSCR 0x10 /* Process had Custom DSCR initialized */ struct fpu { union { double fpr; Index: head/sys/powerpc/include/spr.h =================================================================== --- head/sys/powerpc/include/spr.h +++ head/sys/powerpc/include/spr.h @@ -97,6 +97,7 @@ #define SPR_RTCL_R 0x005 /* .6. 601 RTC Lower - Read */ #define SPR_LR 0x008 /* 468 Link Register */ #define SPR_CTR 0x009 /* 468 Count Register */ +#define SPR_DSCR 0x011 /* Data Stream Control Register */ #define SPR_DSISR 0x012 /* .68 DSI exception source */ #define DSISR_DIRECT 0x80000000 /* Direct-store error exception */ #define DSISR_NOTFOUND 0x40000000 /* Translation not found */ Index: head/sys/powerpc/powerpc/exec_machdep.c =================================================================== --- head/sys/powerpc/powerpc/exec_machdep.c +++ head/sys/powerpc/powerpc/exec_machdep.c @@ -1021,11 +1021,46 @@ td->td_retval[1] = 0; } +static int +emulate_mfspr(int spr, int reg, struct trapframe *frame){ + struct thread *td; + + td = curthread; + + if (spr == SPR_DSCR) { + // If DSCR was never set, get the default DSCR + if ((td->td_pcb->pcb_flags & PCB_CDSCR) == 0) + td->td_pcb->pcb_dscr = mfspr(SPR_DSCR); + + frame->fixreg[reg] = td->td_pcb->pcb_dscr; + frame->srr0 += 4; + return 0; + } else + return SIGILL; +} + +static int +emulate_mtspr(int spr, int reg, struct trapframe *frame){ + struct thread *td; + + td = curthread; + + if (spr == SPR_DSCR) { + td->td_pcb->pcb_flags |= PCB_CDSCR; + td->td_pcb->pcb_dscr = frame->fixreg[reg]; + frame->srr0 += 4; + return 0; + } else + return SIGILL; +} + +#define XFX 0xFC0007FF int ppc_instr_emulate(struct trapframe *frame, struct pcb *pcb) { uint32_t instr; int reg, sig; + int rs, spr; instr = fuword32((void *)frame->srr0); sig = SIGILL; @@ -1035,9 +1070,15 @@ frame->fixreg[reg] = mfpvr(); frame->srr0 += 4; return (0); - } - - if ((instr & 0xfc000ffe) == 0x7c0004ac) { /* various sync */ + } else if ((instr & XFX) == 0x7c0002a6) { /* mfspr */ + rs = (instr & 0x3e00000) >> 21; + spr = (instr & 0x1ff800) >> 16; + return emulate_mfspr(spr, rs, frame); + } else if ((instr & XFX) == 0x7c0003a6) { /* mtspr */ + rs = (instr & 0x3e00000) >> 21; + spr = (instr & 0x1ff800) >> 16; + return emulate_mtspr(spr, rs, frame); + } else if ((instr & 0xfc000ffe) == 0x7c0004ac) { /* various sync */ powerpc_sync(); /* Do a heavy-weight sync */ frame->srr0 += 4; return (0); Index: head/sys/powerpc/powerpc/genassym.c =================================================================== --- head/sys/powerpc/powerpc/genassym.c +++ head/sys/powerpc/powerpc/genassym.c @@ -195,6 +195,7 @@ ASSYM(PCB_CONTEXT, offsetof(struct pcb, pcb_context)); ASSYM(PCB_CR, offsetof(struct pcb, pcb_cr)); +ASSYM(PCB_DSCR, offsetof(struct pcb, pcb_dscr)); ASSYM(PCB_SP, offsetof(struct pcb, pcb_sp)); ASSYM(PCB_TOC, offsetof(struct pcb, pcb_toc)); ASSYM(PCB_LR, offsetof(struct pcb, pcb_lr)); @@ -202,6 +203,7 @@ ASSYM(PCB_FLAGS, offsetof(struct pcb, pcb_flags)); ASSYM(PCB_FPU, PCB_FPU); ASSYM(PCB_VEC, PCB_VEC); +ASSYM(PCB_CDSCR, PCB_CDSCR); ASSYM(PCB_AIM_USR_VSID, offsetof(struct pcb, pcb_cpu.aim.usr_vsid)); ASSYM(PCB_BOOKE_DBCR0, offsetof(struct pcb, pcb_cpu.booke.dbcr0)); Index: head/sys/powerpc/powerpc/swtch64.S =================================================================== --- head/sys/powerpc/powerpc/swtch64.S +++ head/sys/powerpc/powerpc/swtch64.S @@ -62,6 +62,7 @@ #include #include +#include #include #include @@ -124,6 +125,14 @@ stdu %r1,-48(%r1) + lwz %r7, PCB_FLAGS(%r17) + andi. %r7, %r7, PCB_CDSCR + beq .L0 + /* Custom DSCR was set. Reseting it to enter kernel */ + li %r7, 0x0 + mtspr SPR_DSCR, %r7 + +.L0: lwz %r7,PCB_FLAGS(%r17) /* Save FPU context if needed */ andi. %r7, %r7, PCB_FPU @@ -188,10 +197,18 @@ lwz %r6, PCB_FLAGS(%r17) /* Restore Altivec context if needed */ andi. %r6, %r6, PCB_VEC - beq .L4 + beq .L31 mr %r3,%r13 /* Pass curthread to enable_vec */ bl enable_vec nop + +.L31: + lwz %r6, PCB_FLAGS(%r17) + /* Restore Custom DSCR if needed */ + andi. %r6, %r6, PCB_CDSCR + beq .L4 + ld %r6, PCB_DSCR(%r17) /* Load the DSCR register*/ + mtspr SPR_DSCR, %r6 /* thread to restore is in r3 */ .L4: