From 4a779f5131dea2ebcf95cc275df3ab23850ac6c0 Mon Sep 17 00:00:00 2001 From: Huang bo Date: Tue, 23 Nov 2021 16:28:44 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E4=BD=BF=E7=94=A8C=E8=AF=AD?= =?UTF-8?q?=E8=A8=80=E6=9E=84=E5=BB=BA=E4=BB=BB=E5=8A=A1=E6=A0=88=E5=B8=A7?= =?UTF-8?q?=EF=BC=8C=E6=B8=85=E9=99=A4fls=E5=92=8Cffs=E5=AF=B9?= =?UTF-8?q?=E6=96=87=E4=BB=B6=E7=9A=84=E4=BE=9D=E8=B5=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- libcpu/ti-dsp/c6x/c66xx.h | 18 +++-- libcpu/ti-dsp/c6x/context.asm | 34 ++++++++++ libcpu/ti-dsp/c6x/cpuport.c | 2 +- libcpu/ti-dsp/c6x/interrupt.c | 2 +- libcpu/ti-dsp/c6x/intexc.asm | 4 +- libcpu/ti-dsp/c6x/stack.asm | 107 ------------------------------ libcpu/ti-dsp/c6x/stack.c | 120 ++++++++++++++++++++++++++++++++++ libcpu/ti-dsp/c6x/trap.c | 27 ++++---- libcpu/ti-dsp/c6x/trap.h | 47 +++++++++++-- 9 files changed, 228 insertions(+), 133 deletions(-) delete mode 100644 libcpu/ti-dsp/c6x/stack.asm create mode 100644 libcpu/ti-dsp/c6x/stack.c diff --git a/libcpu/ti-dsp/c6x/c66xx.h b/libcpu/ti-dsp/c6x/c66xx.h index ea204907d8..903edbfaf1 100644 --- a/libcpu/ti-dsp/c6x/c66xx.h +++ b/libcpu/ti-dsp/c6x/c66xx.h @@ -11,7 +11,15 @@ #ifndef __C66XX_H__ #define __C66XX_H__ -#include +extern cregister volatile unsigned int IERR; /* Internal Exception Report Register */ +extern cregister volatile unsigned int ECR; /* Exception Clear Register */ +extern cregister volatile unsigned int EFR; /* Exception Flag Register */ +extern cregister volatile unsigned int TSR; /* Task State Register */ +extern cregister volatile unsigned int ITSR; /* Interrupt Task State Register */ +extern cregister volatile unsigned int NTSR; /* NMI/exception Task State Register */ +extern cregister volatile unsigned int TSCL; /* Time Stamp Counter Register - Low Half */ +extern cregister volatile unsigned int TSCH; /* Time Stamp Counter Register - High Half */ +extern cregister volatile unsigned int DNUM; /* Core number */ #ifdef _BIG_ENDIAN #define RT_REG_PAIR(odd, even) unsigned long odd; unsigned long even @@ -37,7 +45,7 @@ struct rt_hw_register RT_REG_PAIR(b9, b8); RT_REG_PAIR(b11, b10); RT_REG_PAIR(b13, b12); - + RT_REG_PAIR(a17, a16); RT_REG_PAIR(a19, a18); RT_REG_PAIR(a21, a20); @@ -58,14 +66,14 @@ struct rt_hw_register RT_REG_PAIR(a15, a14); RT_REG_PAIR(sp, dp); }; - -struct rt_hw_exp_stack_register + +typedef struct rt_hw_exp_stack_register { RT_REG_PAIR(tsr, orig_a4); RT_REG_PAIR(rilc, ilc); RT_REG_PAIR(pc, csr); struct rt_hw_register hw_register; -}; +} rt_hw_thread_stack_register; #define __dint() asm(" DINT") #define __rint() asm(" RINT") diff --git a/libcpu/ti-dsp/c6x/context.asm b/libcpu/ti-dsp/c6x/context.asm index b3146d1eb3..2a57e9414d 100644 --- a/libcpu/ti-dsp/c6x/context.asm +++ b/libcpu/ti-dsp/c6x/context.asm @@ -79,6 +79,40 @@ rt_hw_interrupt_enable: NOP 5 ;} +;----------------------------------------------------------- +; rt_uint32_t rt_hw_get_current_dp(void) +;----------------------------------------------------------- + .global rt_hw_get_current_dp +rt_hw_get_current_dp: +;{ + B B3 + MV B14, A4 + NOP 4 +;} + +;----------------------------------------------------------- +; rt_int32_t __fls(rt_int32_t val) +;----------------------------------------------------------- + .global __fls +__fls: +;{ + B B3 + LMBD .L1 1,A4,A4 + NOP 4 +;} + +;----------------------------------------------------------- +; rt_int32_t __ffs(rt_int32_t val) +;----------------------------------------------------------- + .global __ffs +__ffs: +;{ + BITR .M1 A4,A4 + B B3 + LMBD .L1 1,A4,A4 + NOP 4 +;} + ; ;----------------------------------------------------------- ; diff --git a/libcpu/ti-dsp/c6x/cpuport.c b/libcpu/ti-dsp/c6x/cpuport.c index 367b884a60..3f755a5c7c 100644 --- a/libcpu/ti-dsp/c6x/cpuport.c +++ b/libcpu/ti-dsp/c6x/cpuport.c @@ -158,7 +158,7 @@ void hw_int11_handler(void) -----------------------------------------------------------------------------*/ void hw_int12_handler(void) { - + } /*------------ hw_int13_handler() function ------------------------------------ diff --git a/libcpu/ti-dsp/c6x/interrupt.c b/libcpu/ti-dsp/c6x/interrupt.c index 1bfc19cbdd..2be2f88313 100644 --- a/libcpu/ti-dsp/c6x/interrupt.c +++ b/libcpu/ti-dsp/c6x/interrupt.c @@ -94,6 +94,6 @@ void rt_hw_interrupt_clear(int vector) { if (vector < 0 || vector >= MAX_HANDLERS) { - return; + return; } } diff --git a/libcpu/ti-dsp/c6x/intexc.asm b/libcpu/ti-dsp/c6x/intexc.asm index 4485c6579c..2490977331 100644 --- a/libcpu/ti-dsp/c6x/intexc.asm +++ b/libcpu/ti-dsp/c6x/intexc.asm @@ -81,7 +81,9 @@ RT_INTERRUPT_ENTRY .macro .endm RT_CALL_INT .macro __isr - CALLP __isr,B3 + B __isr + ADDKPC $1 ,B3,4 +$1: B .S1 rt_interrupt_context_restore NOP 5 .endm diff --git a/libcpu/ti-dsp/c6x/stack.asm b/libcpu/ti-dsp/c6x/stack.asm deleted file mode 100644 index f9b31dec93..0000000000 --- a/libcpu/ti-dsp/c6x/stack.asm +++ /dev/null @@ -1,107 +0,0 @@ -; -; Copyright (c) 2021, Shenzhen Academy of Aerospace Technology -; -; SPDX-License-Identifier: Apache-2.0 -; -; Change Logs: -; Date Author Notes -; 2021-11-16 Dystopia the first version -; - -;----------------------------------------------------------- -; build thread stack for C6000 DSP -;----------------------------------------------------------- - -;----------------------------------------------------------- -; macro definition -;----------------------------------------------------------- -ADDRESS_MSK .set 0xFFFFFFF0 - -; -;----------------------------------------------------------- -; - - .sect ".text" -; -;----------------------------------------------------------- -; - -; -; rt_uint8_t *rt_hw_stack_init(void *tentry, void *parameter, rt_uint8_t *stack_addr, void *texit) -; tentry --> A4 -; parameter --> B4 -; stack_addr --> A6 -; texit --> B6 -;{ - .global rt_hw_stack_init -rt_hw_stack_init: - SUB A6,1,B1 ; - MVKL .S1 ADDRESS_MSK,A1 ; - MVKH .S1 ADDRESS_MSK,A1 ; Build address mask - MVC .S2 CSR,B0 ; - AND -2,B0,B0 ; Clear GIE bit - OR 2,B0,B0 ; Set PGIE bit for interrupt return - AND A1,B1,B1 ; Ensure alignment -; -; Actually build the stack frame. -; - MV .S1 B1,A3 - MV .S1 B14,A2 - STDW .D2T1 A3:A2,*--B1[1] ; Initial B15:B14 - SUBAW .D2 B1,2,B1 - ZERO A2 - ZERO A3 ; Clear value - STDW .D2T1 A3:A2,*B1--[1] ; Initial A15:A14 - STDW .D2T1 A3:A2,*B1--[1] ; Initial A13:A12 - STDW .D2T1 A3:A2,*B1--[1] ; Initial A11:A10 - STDW .D2T1 A3:A2,*B1--[1] ; Initial A9:A8 - STDW .D2T1 A3:A2,*B1--[1] ; Initial A7:A6 - MV .S1 B4,A2 - STDW .D2T1 A3:A2,*B1--[1] ; Initial A5:A4 - ZERO A2 - STDW .D2T1 A3:A2,*B1--[1] ; Initial A3:A2 - STDW .D2T1 A3:A2,*B1--[1] ; Initial A1:A0 - - STDW .D2T1 A3:A2,*B1--[1] ; Initial A31:A30 - STDW .D2T1 A3:A2,*B1--[1] ; Initial A29:A28 - STDW .D2T1 A3:A2,*B1--[1] ; Initial A27:A26 - STDW .D2T1 A3:A2,*B1--[1] ; Initial A25:A24 - STDW .D2T1 A3:A2,*B1--[1] ; Initial A23:A22 - STDW .D2T1 A3:A2,*B1--[1] ; Initial A21:A20 - STDW .D2T1 A3:A2,*B1--[1] ; Initial A19:A18 - STDW .D2T1 A3:A2,*B1--[1] ; Initial A17:A16 - - STDW .D2T1 A3:A2,*B1--[1] ; Initial B13:B12 - STDW .D2T1 A3:A2,*B1--[1] ; Initial B11:B10 - STDW .D2T1 A3:A2,*B1--[1] ; Initial B9:B8 - STDW .D2T1 A3:A2,*B1--[1] ; Initial B7:B6 - STDW .D2T1 A3:A2,*B1--[1] ; Initial B5:B4 - MV .S1 B6,A3 - STDW .D2T1 A3:A2,*B1--[1] ; Initial B3:B2 - ZERO A3 - STDW .D2T1 A3:A2,*B1--[1] ; Initial B1:B0 - - STDW .D2T1 A3:A2,*B1--[1] ; Initial B31:B30 - STDW .D2T1 A3:A2,*B1--[1] ; Initial B29:B28 - STDW .D2T1 A3:A2,*B1--[1] ; Initial B27:B26 - STDW .D2T1 A3:A2,*B1--[1] ; Initial B25:B24 - STDW .D2T1 A3:A2,*B1--[1] ; Initial B23:B22 - STDW .D2T1 A3:A2,*B1--[1] ; Initial B21:B20 - STDW .D2T1 A3:A2,*B1--[1] ; Initial B19:B18 - STDW .D2T1 A3:A2,*B1--[1] ; Initial B17:B16 - - MV .S1 A4,A3 - MV .S1 B0,A2 - STDW .D2T1 A3:A2,*B1--[1] ; Initial PC:CSR - - ZERO A2 - ZERO A3 - STDW .D2T1 A3:A2,*B1--[1] ; Initial ILC:RILC - B B3 - MVKL .S2 0x3,B0 - MV .S1 B0,A3 - MVKL .S1 1,A2 - STDW .D2T1 A3:A2,*B1--[1] ; Initial TSR:stack type - MV .S1 B1,A4 ; Save to TCB -;} - .end diff --git a/libcpu/ti-dsp/c6x/stack.c b/libcpu/ti-dsp/c6x/stack.c new file mode 100644 index 0000000000..3fe3377a1c --- /dev/null +++ b/libcpu/ti-dsp/c6x/stack.c @@ -0,0 +1,120 @@ +/* + * Copyright (c) 2021, Shenzhen Academy of Aerospace Technology + * + * SPDX-License-Identifier: Apache-2.0 + * + * Change Logs: + * Date Author Notes + * 2021-11-16 Dystopia the first version + */ + +#include +#include + +extern rt_uint32_t rt_hw_get_current_dp(void); + +/** + * @addtogroup C66xx + */ +/*@{*/ + +/** + * This function will initialize thread stack + * + * @param tentry the entry of thread + * @param parameter the parameter of entry + * @param stack_addr the beginning stack address + * @param texit the function will be called when thread exit + * + * @return stack address + */ +rt_uint8_t *rt_hw_stack_init(void *tentry, void *parameter, + rt_uint8_t *stack_addr, void *texit) +{ + rt_hw_thread_stack_register *thread_context; + rt_uint32_t stk; + + stack_addr += sizeof(rt_uint32_t); + stack_addr = (rt_uint8_t *)RT_ALIGN_DOWN((rt_uint32_t)stack_addr, 8); + stk = (rt_uint32_t)stack_addr; + thread_context = (rt_hw_thread_stack_register *)(stk - sizeof(rt_hw_thread_stack_register)); + + thread_context->pc = (rt_uint32_t)tentry; + thread_context->csr = 0x0103; + thread_context->tsr = 0x3; + thread_context->orig_a4 = 1; + thread_context->ilc = 0; + thread_context->rilc = 0; + + thread_context->hw_register.b17 = 0xB17; + thread_context->hw_register.b16 = 0xB16; + thread_context->hw_register.b19 = 0xB19; + thread_context->hw_register.b18 = 0xB18; + thread_context->hw_register.b21 = 0xB21; + thread_context->hw_register.b20 = 0xB20; + thread_context->hw_register.b23 = 0xB23; + thread_context->hw_register.b22 = 0xB22; + thread_context->hw_register.b25 = 0xB25; + thread_context->hw_register.b24 = 0xB24; + thread_context->hw_register.b27 = 0xB27; + thread_context->hw_register.b26 = 0xB26; + thread_context->hw_register.b29 = 0xB29; + thread_context->hw_register.b28 = 0xB28; + thread_context->hw_register.b31 = 0xB31; + thread_context->hw_register.b30 = 0xB30; + + thread_context->hw_register.b1 = 0xB01; + thread_context->hw_register.b0 = 0xB00; + thread_context->hw_register.b3 = (rt_uint32_t)texit; + thread_context->hw_register.b2 = 0xB02; + thread_context->hw_register.b5 = 0xB05; + thread_context->hw_register.b4 = 0xB04; + thread_context->hw_register.b7 = 0xB07; + thread_context->hw_register.b6 = 0xB06; + thread_context->hw_register.b9 = 0xB09; + thread_context->hw_register.b8 = 0xB08; + thread_context->hw_register.b11 = 0xB11; + thread_context->hw_register.b10 = 0xB10; + thread_context->hw_register.b13 = 0xB13; + thread_context->hw_register.b12 = 0xB12; + + thread_context->hw_register.a17 = 0xA17; + thread_context->hw_register.a16 = 0xA16; + thread_context->hw_register.a19 = 0xA19; + thread_context->hw_register.a18 = 0xA18; + thread_context->hw_register.a21 = 0xA21; + thread_context->hw_register.a20 = 0xA20; + thread_context->hw_register.a23 = 0xA23; + thread_context->hw_register.a22 = 0xA22; + thread_context->hw_register.a25 = 0xA25; + thread_context->hw_register.a24 = 0xA24; + thread_context->hw_register.a27 = 0xA27; + thread_context->hw_register.a26 = 0xA26; + thread_context->hw_register.a29 = 0xA29; + thread_context->hw_register.a28 = 0xA28; + thread_context->hw_register.a31 = 0xA31; + thread_context->hw_register.a30 = 0xA30; + + thread_context->hw_register.a1 = 0xA01; + thread_context->hw_register.a0 = 0xA00; + thread_context->hw_register.a3 = 0xA03; + thread_context->hw_register.a2 = 0xA02; + thread_context->hw_register.a5 = 0xA05; + thread_context->hw_register.a4 = (rt_uint32_t)parameter; + thread_context->hw_register.a7 = 0xA07; + thread_context->hw_register.a6 = 0xA06; + thread_context->hw_register.a9 = 0xA09; + thread_context->hw_register.a8 = 0xA08; + thread_context->hw_register.a11 = 0xA11; + thread_context->hw_register.a10 = 0xA10; + thread_context->hw_register.a13 = 0xA13; + thread_context->hw_register.a12 = 0xA12; + + thread_context->hw_register.a15 = 0xA15; + thread_context->hw_register.a14 = 0xA14; + thread_context->hw_register.dp = rt_hw_get_current_dp(); + thread_context->hw_register.sp = (rt_uint32_t)stk; + + /* return task's current stack address */ + return (rt_uint8_t *)thread_context - 8; +} diff --git a/libcpu/ti-dsp/c6x/trap.c b/libcpu/ti-dsp/c6x/trap.c index 634e486396..9f8f80d5ed 100644 --- a/libcpu/ti-dsp/c6x/trap.c +++ b/libcpu/ti-dsp/c6x/trap.c @@ -17,7 +17,6 @@ #define RT_SYS_STACK_SIZE 4096 -extern void rt_hw_enable_exception(void); rt_uint8_t rt_system_stack[RT_SYS_STACK_SIZE]; rt_uint8_t *rt_system_stack_top; @@ -117,14 +116,14 @@ void do_trap(struct rt_exception_info *except_info, struct rt_hw_exp_stack_regis static struct rt_exception_info iexcept_table[10] = { { " - instruction fetch", ABORT_TYPE_BUS, ABORT_BUS_ADDRERR }, - { " - fetch packet", ABORT_TYPE_BUS, ABORT_BUS_ADDRERR }, - { " - execute packet", ABORT_TYPE_UNDDEF, ABORT_OPCODE_ILL }, + { " - fetch packet", ABORT_TYPE_BUS, ABORT_BUS_ADDRERR }, + { " - execute packet", ABORT_TYPE_UNDDEF, ABORT_OPCODE_ILL }, { " - undefined instruction", ABORT_TYPE_UNDDEF, ABORT_OPCODE_ILL }, { " - resource conflict", ABORT_TYPE_UNDDEF, ABORT_OPCODE_ILL }, - { " - resource access", ABORT_TYPE_UNDDEF, ABORT_PRVREG_ILL }, + { " - resource access", ABORT_TYPE_UNDDEF, ABORT_PRVREG_ILL }, { " - privilege", ABORT_TYPE_UNDDEF, ABORT_PRVOPC_ILL }, - { " - loops buffer", ABORT_TYPE_UNDDEF, ABORT_PRVOPC_ILL }, - { " - software exception", ABORT_TYPE_UNDDEF, ABORT_ILLTRP_ILL }, + { " - loops buffer", ABORT_TYPE_UNDDEF, ABORT_PRVOPC_ILL }, + { " - software exception", ABORT_TYPE_UNDDEF, ABORT_ILLTRP_ILL }, { " - unknown exception", ABORT_TYPE_UNDDEF, ABORT_PRVOPC_ILL } }; @@ -140,7 +139,7 @@ static int process_iexcept(struct rt_hw_exp_stack_register *regs) while(iexcept_report) { - iexcept_num = __ffs(iexcept_report); + iexcept_num = ffs(iexcept_report); iexcept_report &= ~(1 << iexcept_num); set_iexcept(iexcept_report); @@ -160,7 +159,7 @@ static int process_iexcept(struct rt_hw_exp_stack_register *regs) return 0; } -static struct rt_exception_info except_table[128] = { +static struct rt_exception_info eexcept_table[128] = { { " - external exception", ABORT_TYPE_BUS, ABORT_BUS_ADDRERR }, { " - external exception", ABORT_TYPE_BUS, ABORT_BUS_ADDRERR }, { " - external exception", ABORT_TYPE_BUS, ABORT_BUS_ADDRERR }, @@ -298,7 +297,7 @@ static struct rt_exception_info except_table[128] = { /* * Process an external exception (maskable) */ -static void process_except(struct rt_hw_exp_stack_register *regs) +static void process_eexcept(struct rt_hw_exp_stack_register *regs) { int except_num = 0; int bank = 0; @@ -309,10 +308,10 @@ static void process_except(struct rt_hw_exp_stack_register *regs) while (INTC_MEXPMASK[i]) { __dint(); - except_num = __ffs(INTC_MEXPMASK[i]); + except_num = ffs(INTC_MEXPMASK[i]); INTC_MEXPMASK[i] &= ~(1 << except_num); /* ack the external exception */ __rint(); - do_trap(&except_table[except_num + (bank << 5)], regs); + do_trap(&eexcept_table[except_num + (bank << 5)], regs); } bank++; } @@ -331,11 +330,11 @@ int rt_hw_process_exception(struct rt_hw_exp_stack_register *regs) int ie_num = 9; /* default is unknown exception */ while ((type = get_except_type()) != 0) { - type_num = __fls(type) - 1; + type_num = fls(type) - 1; switch(type_num) { case EXCEPT_TYPE_NXF: /* NMI exception */ - ack_exception(EXCEPT_TYPE_NXF); /* clear exception */ + ack_exception(EXCEPT_TYPE_NXF); /* clear exception */ if (hw_nmi_handler != RT_NULL) { hw_nmi_handler(regs); @@ -350,7 +349,7 @@ int rt_hw_process_exception(struct rt_hw_exp_stack_register *regs) break; case EXCEPT_TYPE_EXC: /* external exception */ - process_except(regs); + process_eexcept(regs); break; case EXCEPT_TYPE_SXF: /* software exception */ diff --git a/libcpu/ti-dsp/c6x/trap.h b/libcpu/ti-dsp/c6x/trap.h index 9c8f52dd14..12734df4a7 100644 --- a/libcpu/ti-dsp/c6x/trap.h +++ b/libcpu/ti-dsp/c6x/trap.h @@ -67,10 +67,49 @@ struct rt_exception_info { #define BKPT_OPCODE 0x56454314 /* illegal opcode */ #define INTC_MEXPMASK __SYSREGA(0x018000e0, unsigned int) -#define __ffs(a) (_lmbd(1, _bitr(a))) -#define __fls(a) (!(a) ? 0 : (32 - _lmbd(1, (a)))) - extern void rt_trap_init(void); +extern void rt_hw_enable_exception(void); +extern int __fls(int val); +extern int __ffs(int val); -#endif /* __TRAP_H__ */ +/* + * ffz - find first zero in word. + * @word: The word to search + * + * Undefined if no zero exists, so code should check against ~0UL first. + */ +#define ffz(x) __ffs(~(x)) + +/** + * fls - find last (most-significant) bit set + * @x: the word to search + * + * This is defined the same way as ffs. + * Note fls(0) = 0, fls(1) = 1, fls(0x80000000) = 32. + */ +static inline int fls(int x) +{ + if (!x) + return 0; + + return 32 - __fls(x); +} + +/** + * ffs - find first bit set + * @x: the word to search + * + * This is defined the same way as + * the libc and compiler builtin ffs routines, therefore + * differs in spirit from the above ffz (man ffs). + * Note ffs(0) = 0, ffs(1) = 1, ffs(0x80000000) = 32. + */ +static inline int ffs(int x) +{ + if (!x) + return 0; + return __ffs(x) + 1; +} + +#endif /* __TRAP_H__ */ -- GitLab