diff --git a/sys/arm/arm/swtch-v6.S b/sys/arm/arm/swtch-v6.S --- a/sys/arm/arm/swtch-v6.S +++ b/sys/arm/arm/swtch-v6.S @@ -109,6 +109,10 @@ .word PCPU_SIZE .Lblocked_lock: .word _C_LABEL(blocked_lock) +#ifdef VFP +.Lvfp_exists: + .word _C_LABEL(vfp_exists) +#endif ENTRY(cpu_context_switch) DSB @@ -169,7 +173,12 @@ mov r11, r1 /* r11 = newtd */ #ifdef VFP /* This thread is dying, disable */ + ldr r9, .Lvfp_exists + ldr r9, [r9] + cmp r9, #0 + beq 1f bl _C_LABEL(vfp_discard) /* VFP without preserving state. */ +1: #endif GET_PCPU(r8, r9) /* r8 = current pcpu */ ldr r4, [r8, #PC_CPUID] /* r4 = current cpu id */ @@ -319,10 +328,15 @@ #ifdef VFP + ldr r0, .Lvfp_exists + ldr r0, [r0] + cmp r0, #0 + beq 1f ldr r3, [r10, #(TD_PCB)] mov r1, r3 mov r0, r10 bl _C_LABEL(vfp_save_state) +1: #endif /* diff --git a/sys/arm/arm/vfp.c b/sys/arm/arm/vfp.c --- a/sys/arm/arm/vfp.c +++ b/sys/arm/arm/vfp.c @@ -139,6 +139,23 @@ coproc |= COPROC10 | COPROC11; set_coprocessorACR(coproc); + /* + * If the coprocessor access bits reset to 0 after the write, then + * the coprocessor does not exist in the system + */ + coproc = get_coprocessorACR(); + if ((coproc & COPROC10) == 0 || (coproc & COPROC11) == 0) { + /* + * If either CP10 or CP11 were able to be enabled, + * disable them, as we should not have one and not the other. + */ + if ((coproc & (COPROC10 | COPROC11)) != 0) { + coproc &= ~(COPROC10 | COPROC11); + set_coprocessorACR(coproc); + } + return; + } + fpsid = fmrx(fpsid); /* read the vfp system id */ if (!(fpsid & VFPSID_HARDSOFT_IMP)) {