diff --git a/src/kernel/cpu.h b/src/kernel/cpu.h index dd158ea..2e94e1f 100644 --- a/src/kernel/cpu.h +++ b/src/kernel/cpu.h @@ -36,4 +36,31 @@ void cpu_setCPUOPM(uint32_t CPUOPM); /* cpu_getCPUOPM(): Get the CPU OperatioN Mode register */ uint32_t cpu_getCPUOPM(void); +//--- +// Status Register +//--- + +/* Status Register bits */ +typedef lword_union(sr_t, + uint32_t :1; + uint32_t MD :1; + uint32_t RB :1; + uint32_t BL :1; + uint32_t RC :12; + uint32_t :3; + uint32_t DSP :1; + uint32_t DMY :1; + uint32_t DMX :1; + uint32_t M :1; + uint32_t Q :1; + uint32_t IMASK :4; + uint32_t RF :2; + uint32_t S :1; + uint32_t T :1; +); + +/* Get and set sr through the sr_t type */ +sr_t cpu_getSR(void); +void cpu_setSR(sr_t sr); + #endif /* GINT_CORE_CPU */ diff --git a/src/kernel/cpu.s b/src/kernel/cpu.s index 3bb17a6..54d06fc 100644 --- a/src/kernel/cpu.s +++ b/src/kernel/cpu.s @@ -5,21 +5,23 @@ .global _cpu_setVBR .global _cpu_setCPUOPM .global _cpu_getCPUOPM +.global _cpu_getSR +.global _cpu_setSR /* cpu_setVBR(): Change VBR address */ .section .gint.mapped, "ax" _cpu_setVBR_reloc: mov.l r8, @-r15 + mov.l r9, @-r15 sts.l pr, @-r15 - stc.l sr, @-r15 /* Block all interrupts by setting IMASK=15 */ - mov #0xf, r1 - shll2 r1 - shll2 r1 + mov #0xf, r9 + shll2 r9 + shll2 r9 stc sr, r0 - or r1, r0 + or r9, r0 ldc r0, sr /* Set the new VBR address */ @@ -30,10 +32,17 @@ _cpu_setVBR_reloc: jsr @r5 mov r6, r4 - /* Enable interrupts again by restoring the status register */ - ldc.l @r15+, sr - lds.l @r15+, pr + /* Enable interrupts again */ + stc sr, r0 + not r9, r9 + and r9, r0 + ldc r0, sr + + /* Return the previous VBR address */ mov r8, r0 + + lds.l @r15+, pr + mov.l @r15+, r9 rts mov.l @r15+, r8 @@ -68,5 +77,16 @@ _cpu_getCPUOPM: mov.l @r0, r0 .align 4 - 1: .long 0xff2f0000 + +/* cpu_getSR(): Get status register */ +_cpu_getSR: + stc sr, r0 + rts + nop + +/* cpu_setSR(): Set status register */ +_cpu_setSR: + ldc r4, sr + rts + nop diff --git a/src/kernel/kernel.c b/src/kernel/kernel.c index c97203f..f8c4877 100644 --- a/src/kernel/kernel.c +++ b/src/kernel/kernel.c @@ -22,6 +22,7 @@ static void kinit_cpu(void); typedef struct { + sr_t SR; uint32_t VBR; uint32_t CPUOPM; } ctx_t; @@ -31,11 +32,19 @@ GBSS static ctx_t sys_ctx, gint_ctx; static void ctx_save(ctx_t *ctx) { - if(isSH4()) ctx->CPUOPM = cpu_getCPUOPM(); + if(isSH4()) + { + ctx->CPUOPM = cpu_getCPUOPM(); + ctx->SR = cpu_getSR(); + } } static void ctx_restore(ctx_t *ctx) { - if(isSH4()) cpu_setCPUOPM(ctx->CPUOPM); + if(isSH4()) + { + cpu_setCPUOPM(ctx->CPUOPM); + cpu_setSR(ctx->SR); + } } //--- @@ -99,7 +108,15 @@ static void drivers_switch(int who) static void kinit_cpu(void) { - if(isSH4()) cpu_setCPUOPM(cpu_getCPUOPM() | 0x00000008); + if(isSH4()) + { + cpu_setCPUOPM(cpu_getCPUOPM() | 0x00000008); + + /* Enable DSP mode on the CPU */ + sr_t SR = cpu_getSR(); + SR.DSP = 1; + cpu_setSR(SR); + } } /* kinit(): Install and start gint */