94 #endif |
94 #endif |
95 GET_RWRO_TID(,r8); // r8 = User RO Thread ID |
95 GET_RWRO_TID(,r8); // r8 = User RO Thread ID |
96 GET_RWRW_TID(,r9); // r9 = User RW Thread ID |
96 GET_RWRW_TID(,r9); // r9 = User RW Thread ID |
97 #ifdef __CPU_HAS_VFP |
97 #ifdef __CPU_HAS_VFP |
98 VFP_FMRX(,0,VFP_XREG_FPEXC); // r0 = FPEXC |
98 VFP_FMRX(,0,VFP_XREG_FPEXC); // r0 = FPEXC |
99 asm("bic r0, #%a0" : : "i" ((TInt)VFP_FPEXC_EN) ); // Store FPEXC with VFP disabled in case this thread runs on a different core next time |
99 asm("bic r0, r0, #%a0" : : "i" ((TInt)VFP_FPEXC_EN) ); // Store FPEXC with VFP disabled in case this thread runs on a different core next time |
100 #else |
100 #else |
101 asm("mov r0, #0 "); |
101 asm("mov r0, #0 "); |
102 #endif |
102 #endif |
103 GET_CAR(, r1); // r1 = CAR |
103 GET_CAR(, r1); // r1 = CAR |
104 asm("mrc p15, 0, r12, c3, c0, 0 "); // r12 = DACR |
104 asm("mrc p15, 0, r12, c3, c0, 0 "); // r12 = DACR |
674 #ifdef __CPU_HAS_VFP |
674 #ifdef __CPU_HAS_VFP |
675 // Do the actual VFP context save |
675 // Do the actual VFP context save |
676 __NAKED__ void VfpContextSave(void*) |
676 __NAKED__ void VfpContextSave(void*) |
677 { |
677 { |
678 VFP_FMRX(,1,VFP_XREG_FPEXC); |
678 VFP_FMRX(,1,VFP_XREG_FPEXC); |
679 asm("tst r1, #%a0" : : "i" ((TInt)VFP_FPEXC_EN) ); // Check to see if VFP in use |
679 asm("tst r1, #%a0" : : "i" ((TInt)VFP_FPEXC_EN) ); // Check to see if VFP in use |
680 __JUMP(eq, lr); // Return immediately if not |
680 __JUMP(eq, lr); // Return immediately if not |
|
681 asm("tst r1, #%a0" : : "i" ((TInt)VFP_FPEXC_EX) ); // Check to see if an exception has occurred |
|
682 asm("beq 1f "); // Skip ahead if not |
|
683 asm("bic r1, r1, #%a0" : : "i" ((TInt)VFP_FPEXC_EX)); |
|
684 VFP_FMXR(,VFP_XREG_FPEXC,1); // Reset exception flag |
|
685 asm("orr r1, r1, #%a0" : : "i" ((TInt)VFP_FPEXC_EX)); // But store it for later |
|
686 asm("1: "); |
|
687 |
681 |
688 |
682 VFP_FMRX(,2,VFP_XREG_FPSCR); |
689 VFP_FMRX(,2,VFP_XREG_FPSCR); |
683 asm("stmia r0!, {r2} "); // Save FPSCR |
690 asm("stmia r0!, {r2} "); // Save FPSCR |
684 |
691 |
685 #ifndef __VFP_V3 |
692 #ifndef __VFP_V3 |
698 asm("tst r2, #%a0" : : "i" ((TInt)VFP_CPACR_D32DIS)); // Check to see if access to the upper 16 registers is disabled |
705 asm("tst r2, #%a0" : : "i" ((TInt)VFP_CPACR_D32DIS)); // Check to see if access to the upper 16 registers is disabled |
699 VFP_FSTMIADW(CC_EQ,0,16,16); // If not then save D16 - D31 |
706 VFP_FSTMIADW(CC_EQ,0,16,16); // If not then save D16 - D31 |
700 #endif |
707 #endif |
701 |
708 |
702 asm("0: "); |
709 asm("0: "); |
703 asm("bic r1, #%a0" : : "i" ((TInt)VFP_FPEXC_EN) ); |
710 asm("bic r1, r1, #%a0" : : "i" ((TInt)VFP_FPEXC_EN) ); |
704 VFP_FMXR(,VFP_XREG_FPEXC,1); // Disable VFP |
711 VFP_FMXR(,VFP_XREG_FPEXC,1); // Disable VFP |
705 |
712 |
706 __JUMP(,lr); |
713 __JUMP(,lr); |
707 } |
714 } |
708 #endif |
715 #endif |