1/*
2 * Copyright (c) 2006-2022, RT-Thread Development Team
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 *
6 * Change Logs:
7 * Date           Author       Notes
8 * 2014-11-07     weety    first version
9 */
10
11#include <rtconfig.h>
12
13#include "armv6.h"
14
15//#define DEBUG
16
17.macro  PRINT, str
18#ifdef DEBUG
19    stmfd   sp!, {r0-r3, ip, lr}
20    add r0, pc, #4
21    bl  rt_kprintf
22    b   1f
23    .asciz  "UNDEF: \str\n"
24    .balign 4
251:  ldmfd   sp!, {r0-r3, ip, lr}
26#endif
27    .endm
28
29.macro  PRINT1, str, arg
30#ifdef DEBUG
31    stmfd   sp!, {r0-r3, ip, lr}
32    mov r1, \arg
33    add r0, pc, #4
34    bl  rt_kprintf
35    b   1f
36    .asciz  "UNDEF: \str\n"
37    .balign 4
381:  ldmfd   sp!, {r0-r3, ip, lr}
39#endif
40    .endm
41
42.macro  PRINT3, str, arg1, arg2, arg3
43#ifdef DEBUG
44    stmfd   sp!, {r0-r3, ip, lr}
45    mov r3, \arg3
46    mov r2, \arg2
47    mov r1, \arg1
48    add r0, pc, #4
49    bl  rt_kprintf
50    b   1f
51    .asciz  "UNDEF: \str\n"
52    .balign 4
531:  ldmfd   sp!, {r0-r3, ip, lr}
54#endif
55    .endm
56
57.macro  get_current_thread, rd
58    ldr \rd, .current_thread
59    ldr \rd, [\rd]
60    .endm
61
62.current_thread:
63    .word   rt_current_thread
64
65#ifdef RT_USING_NEON
66    .align  6
67
68/* is the neon instuction on arm mode? */
69.neon_opcode:
70    .word   0xfe000000          @ mask
71    .word   0xf2000000          @ opcode
72
73    .word   0xff100000          @ mask
74    .word   0xf4000000          @ opcode
75
76    .word   0x00000000          @ end mask
77    .word   0x00000000          @ end opcode
78#endif
79
80/* undefined instruction exception processing */
81.globl undef_entry
82undef_entry:
83    PRINT1 "r0=0x%08x", r0
84    PRINT1 "r2=0x%08x", r2
85    PRINT1 "r9=0x%08x", r9
86    PRINT1 "sp=0x%08x", sp
87
88#ifdef RT_USING_NEON
89    ldr r6, .neon_opcode
90__check_neon_instruction:
91    ldr r7, [r6], #4        @ load mask value
92    cmp r7, #0              @ end mask?
93    beq __check_vfp_instruction
94    and r8, r0, r7
95    ldr r7, [r6], #4        @ load opcode value
96    cmp r8, r7              @ is NEON instruction?
97    bne __check_neon_instruction
98    b   vfp_entry
99__check_vfp_instruction:
100#endif
101    tst r0, #0x08000000         @ only CDP/CPRT/LDC/STC instruction has bit 27
102    tstne   r0, #0x04000000     @ bit 26 set on both ARM and Thumb-2 instruction
103    moveq   pc, lr              @ no vfp coprocessor instruction, return
104    get_current_thread r10
105    and r8, r0, #0x00000f00     @ get coprocessor number
106    PRINT1 "CP=0x%08x", r8
107    add pc, pc, r8, lsr #6
108    nop
109    mov pc, lr              @ CP0
110    mov pc, lr              @ CP1
111    mov pc, lr              @ CP2
112    mov pc, lr              @ CP3
113    mov pc, lr              @ CP4
114    mov pc, lr              @ CP5
115    mov pc, lr              @ CP6
116    mov pc, lr              @ CP7
117    mov pc, lr              @ CP8
118    mov pc, lr              @ CP9
119    mov pc, lr              @ CP10 VFP
120    mov pc, lr              @ CP11 VFP
121    mov pc, lr              @ CP12
122    mov pc, lr              @ CP13
123    mov pc, lr              @ CP14 DEBUG
124    mov pc, lr              @ CP15 SYS CONTROL
125
126
127