diff --git a/bsp/ls1cdev/drivers/board.c b/bsp/ls1cdev/drivers/board.c index f9c7454a16..29d7fa7142 100644 --- a/bsp/ls1cdev/drivers/board.c +++ b/bsp/ls1cdev/drivers/board.c @@ -51,6 +51,31 @@ void rt_hw_timer_init(void) write_c0_count(0); } + +/** + * init hardware FPU + */ +void rt_hw_fpu_init(void) +{ + rt_uint32_t c0_status = 0; + rt_uint32_t c1_status = 0; + + // 使能协处理器1--FPU + c0_status = read_c0_status(); + c0_status |= (ST0_CU1 | ST0_FR); + write_c0_status(c0_status); + + // 配置FPU + c1_status = read_c1_status(); + c1_status |= (FPU_CSR_FS | FPU_CSR_FO | FPU_CSR_FN); // set FS, FO, FN + c1_status &= ~(FPU_CSR_ALL_E); // disable exception + c1_status = (c1_status & (~FPU_CSR_RM)) | FPU_CSR_RN; // set RN + write_c1_status(c1_status); + + return ; +} + + /** * This function will initial sam7s64 board. */ @@ -69,6 +94,9 @@ void rt_hw_board_init(void) /* init operating system timer */ rt_hw_timer_init(); + /* init hardware fpu */ + rt_hw_fpu_init(); + rt_kprintf("current sr: 0x%08x\n", read_c0_status()); } diff --git a/libcpu/mips/common/mipsregs.h b/libcpu/mips/common/mipsregs.h index 888d4b13b5..22e5754b8b 100644 --- a/libcpu/mips/common/mipsregs.h +++ b/libcpu/mips/common/mipsregs.h @@ -116,6 +116,70 @@ #define CP1_REVISION $0 #define CP1_STATUS $31 +/* + * FPU Status Register Values + */ +/* + * Status Register Values + */ + +#define FPU_CSR_FLUSH 0x01000000 /* flush denormalised results to 0 */ +#define FPU_CSR_COND 0x00800000 /* $fcc0 */ +#define FPU_CSR_COND0 0x00800000 /* $fcc0 */ +#define FPU_CSR_COND1 0x02000000 /* $fcc1 */ +#define FPU_CSR_COND2 0x04000000 /* $fcc2 */ +#define FPU_CSR_COND3 0x08000000 /* $fcc3 */ +#define FPU_CSR_COND4 0x10000000 /* $fcc4 */ +#define FPU_CSR_COND5 0x20000000 /* $fcc5 */ +#define FPU_CSR_COND6 0x40000000 /* $fcc6 */ +#define FPU_CSR_COND7 0x80000000 /* $fcc7 */ + + +/* FS/FO/FN */ +#define FPU_CSR_FS 0x01000000 +#define FPU_CSR_FO 0x00400000 +#define FPU_CSR_FN 0x00200000 + +/* + * Bits 18 - 20 of the FPU Status Register will be read as 0, + * and should be written as zero. + */ +#define FPU_CSR_RSVD 0x001c0000 + +/* + * X the exception cause indicator + * E the exception enable + * S the sticky/flag bit +*/ +#define FPU_CSR_ALL_X 0x0003f000 +#define FPU_CSR_UNI_X 0x00020000 +#define FPU_CSR_INV_X 0x00010000 +#define FPU_CSR_DIV_X 0x00008000 +#define FPU_CSR_OVF_X 0x00004000 +#define FPU_CSR_UDF_X 0x00002000 +#define FPU_CSR_INE_X 0x00001000 + +#define FPU_CSR_ALL_E 0x00000f80 +#define FPU_CSR_INV_E 0x00000800 +#define FPU_CSR_DIV_E 0x00000400 +#define FPU_CSR_OVF_E 0x00000200 +#define FPU_CSR_UDF_E 0x00000100 +#define FPU_CSR_INE_E 0x00000080 + +#define FPU_CSR_ALL_S 0x0000007c +#define FPU_CSR_INV_S 0x00000040 +#define FPU_CSR_DIV_S 0x00000020 +#define FPU_CSR_OVF_S 0x00000010 +#define FPU_CSR_UDF_S 0x00000008 +#define FPU_CSR_INE_S 0x00000004 + +/* Bits 0 and 1 of FPU Status Register specify the rounding mode */ +#define FPU_CSR_RM 0x00000003 +#define FPU_CSR_RN 0x0 /* nearest */ +#define FPU_CSR_RZ 0x1 /* towards zero */ +#define FPU_CSR_RU 0x2 /* towards +Infinity */ +#define FPU_CSR_RD 0x3 /* towards -Infinity */ + /* * R4x00 interrupt enable / cause bits @@ -610,6 +674,32 @@ do { \ #define write_c0_config3(val) __write_32bit_c0_register($16, 3, val) +/* + * Macros to access the floating point coprocessor control registers + */ +#define read_32bit_cp1_register(source) \ +({ int __res; \ + __asm__ __volatile__( \ + ".set\tpush\n\t" \ + ".set\treorder\n\t" \ + /* gas fails to assemble cfc1 for some archs (octeon).*/ \ + ".set\tmips1\n\t" \ + "cfc1\t%0,"STR(source)"\n\t" \ + ".set\tpop" \ + : "=r" (__res)); \ + __res;}) +#define write_32bit_cp1_register(register, value) \ +do { \ + __asm__ __volatile__( \ + "ctc1\t%z0, "STR(register)"\n\t" \ + : : "Jr" ((unsigned int)(value))); \ +} while (0) + +#define read_c1_status() read_32bit_cp1_register(CP1_STATUS) +#define read_c1_revision() read_32bit_cp1_register(CP1_REVISION); +#define write_c1_status(val) write_32bit_cp1_register(CP1_STATUS, val) + + #endif /* end of __ASSEMBLY__ */ #endif /* end of __MIPSREGS_H__ */ diff --git a/libcpu/mips/loongson_1c/context_gcc.S b/libcpu/mips/loongson_1c/context_gcc.S index 1ac2747747..fbffd34866 100644 --- a/libcpu/mips/loongson_1c/context_gcc.S +++ b/libcpu/mips/loongson_1c/context_gcc.S @@ -16,6 +16,7 @@ #include "../common/mips.inc" #include "../common/stackframe.h" +#include "stackframe_fpu.h" .section ".text", "ax" .set noreorder @@ -56,10 +57,12 @@ rt_hw_interrupt_enable: rt_hw_context_switch: mtc0 ra, CP0_EPC SAVE_ALL + SAVE_FPU sw sp, 0(a0) /* store sp in preempted tasks TCB */ lw sp, 0(a1) /* get new task stack pointer */ + RESTORE_FPU RESTORE_ALL_AND_RET /* @@ -70,6 +73,7 @@ rt_hw_context_switch: rt_hw_context_switch_to: lw sp, 0(a0) /* get new task stack pointer */ + RESTORE_FPU RESTORE_ALL_AND_RET /* @@ -103,6 +107,7 @@ _reswitch: .globl mips_irq_handle mips_irq_handle: SAVE_ALL + SAVE_FPU mfc0 t0, CP0_CAUSE and t1, t0, 0xff @@ -151,6 +156,7 @@ mips_irq_handle: nop spurious_interrupt: + RESTORE_FPU RESTORE_ALL_AND_RET .set reorder diff --git a/libcpu/mips/loongson_1c/stackframe_fpu.h b/libcpu/mips/loongson_1c/stackframe_fpu.h new file mode 100644 index 0000000000..58e9271833 --- /dev/null +++ b/libcpu/mips/loongson_1c/stackframe_fpu.h @@ -0,0 +1,94 @@ +/* + * ls1c FPU's stackframe + * 最开始本想，将代码加入到stackframe.h中的SAVE_ALL, RESTORE_ALL和RESTORE_ALL_AND_RET中， + * 但考虑到源文件"stackframe.h"位于目录"libcpu\mips\common"内，怕影响到其它mips cpu + * 所以，另外新建本源文件 + */ +#ifndef __OPENLOONGSON_STACKFRAME_FPU_H +#define __OPENLOONGSON_STACKFRAME_FPU_H + + +#include "../common/asm.h" +#include "../common/mipsregs.h" +#include "../common/stackframe.h" + + + +#define PT_FPU_R0 (0) +#define PT_FPU_R2 ((PT_FPU_R0) + 2*LONGSIZE) +#define PT_FPU_R4 ((PT_FPU_R2) + 2*LONGSIZE) +#define PT_FPU_R6 ((PT_FPU_R4) + 2*LONGSIZE) +#define PT_FPU_R8 ((PT_FPU_R6) + 2*LONGSIZE) +#define PT_FPU_R10 ((PT_FPU_R8) + 2*LONGSIZE) +#define PT_FPU_R12 ((PT_FPU_R10) + 2*LONGSIZE) +#define PT_FPU_R14 ((PT_FPU_R12) + 2*LONGSIZE) +#define PT_FPU_R16 ((PT_FPU_R14) + 2*LONGSIZE) +#define PT_FPU_R18 ((PT_FPU_R16) + 2*LONGSIZE) +#define PT_FPU_R20 ((PT_FPU_R18) + 2*LONGSIZE) +#define PT_FPU_R22 ((PT_FPU_R20) + 2*LONGSIZE) +#define PT_FPU_R24 ((PT_FPU_R22) + 2*LONGSIZE) +#define PT_FPU_R26 ((PT_FPU_R24) + 2*LONGSIZE) +#define PT_FPU_R28 ((PT_FPU_R26) + 2*LONGSIZE) +#define PT_FPU_R30 ((PT_FPU_R28) + 2*LONGSIZE) + +#define PT_FPU_SIZE ((((PT_FPU_R30) + 2*LONGSIZE) + (2*PTRSIZE-1)) & ~(2*PTRSIZE-1)) + + + .macro SAVE_FPU + .set push + .set noreorder + move k1, sp /* 保存现场 */ + and k0, k1, 0xFFFFFFF8 /* 8字节对齐 */ + PTR_SUBU sp, k0, PT_FPU_SIZE /* 计算栈底 */ + s.d $f0, PT_FPU_R0(sp) + s.d $f2, PT_FPU_R2(sp) + s.d $f4, PT_FPU_R4(sp) + s.d $f6, PT_FPU_R6(sp) + s.d $f8, PT_FPU_R8(sp) + s.d $f10, PT_FPU_R10(sp) + s.d $f12, PT_FPU_R12(sp) + s.d $f14, PT_FPU_R14(sp) + s.d $f16, PT_FPU_R16(sp) + s.d $f18, PT_FPU_R18(sp) + s.d $f20, PT_FPU_R20(sp) + s.d $f22, PT_FPU_R22(sp) + s.d $f24, PT_FPU_R24(sp) + s.d $f26, PT_FPU_R26(sp) + s.d $f28, PT_FPU_R28(sp) + s.d $f30, PT_FPU_R30(sp) + move sp, k1 /* 恢复现场 */ + .set reorder + .set pop + .endm + + + .macro RESTORE_FPU + .set push + .set noreorder + move k1, sp /* 保存现场 */ + and k0, k1, 0xFFFFFFF8 /* 8字节对齐 */ + PTR_SUBU sp, k0, PT_FPU_SIZE /* 计算栈底*/ + l.d $f0, PT_FPU_R0(sp) + l.d $f2, PT_FPU_R2(sp) + l.d $f4, PT_FPU_R4(sp) + l.d $f6, PT_FPU_R6(sp) + l.d $f8, PT_FPU_R8(sp) + l.d $f10, PT_FPU_R10(sp) + l.d $f12, PT_FPU_R12(sp) + l.d $f14, PT_FPU_R14(sp) + l.d $f16, PT_FPU_R16(sp) + l.d $f18, PT_FPU_R18(sp) + l.d $f20, PT_FPU_R20(sp) + l.d $f22, PT_FPU_R22(sp) + l.d $f24, PT_FPU_R24(sp) + l.d $f26, PT_FPU_R26(sp) + l.d $f28, PT_FPU_R28(sp) + l.d $f30, PT_FPU_R30(sp) + move sp, k1 /* 恢复现场 */ + .set reorder + .set pop + .endm + + +#endif +