1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
|
#ifndef PSR_H
#define PSR_H
#include <stdint.h>
enum execution_mode {
MODE_USER = 0b10000,
MODE_FIQ = 0b10001,
MODE_IRQ = 0b10010,
MODE_SUPERVISOR = 0b10011,
MODE_MONITOR = 0b10110,
MODE_ABORT = 0b10111,
MODE_HYPERVISOR = 0b11010,
MODE_UNDEFINED = 0b11011,
MODE_SYSTEM = 0b11111,
};
typedef union
{
uint32_t raw;
struct
{
uint32_t M_4_0 : 5; // bits 4:0
uint32_t T : 1; // bit 5
uint32_t F : 1; // bit 6
uint32_t I : 1; // bit 7
uint32_t A : 1; // bit 8
uint32_t E : 1; // bit 9
uint32_t IT_7_2 : 6; // bits 15:10
uint32_t GE_3_0 : 4; // bits 19:16
uint32_t Bits_23_20 : 4; // bits 23:20
uint32_t J : 1; // bit 24
uint32_t IT_1_0 : 2; // bits 26:25
uint32_t Q : 1; // bit 27
uint32_t V : 1; // bit 28
uint32_t C : 1; // bit 29
uint32_t Z : 1; // bit 30
uint32_t N : 1; // bit 31
#define PSR_MODE_4_0 M_4_0
#define PSR_THUMB_BIT T
#define PSR_FIQ_MASKK_BIT F
#define PSR_IRQ_MASK_BIT I
#define PSR_ASYNC_ABORT_MASK_BIT A
#define PSR_ENDIANNESS_BIT E
#define PSR_IF_THEN_STATE_7_2 IT_7_2
#define PSR_GREATER_THAN_OR_EQUAL_FLAGS GE_3_0
// bits 23:20 are reserved
#define PSR_JAZELLE_BIT J
#define PSR_IF_THEN_STATE_1_0 IT_1_0
#define PSR_CUMULATIVE_SATURATION_BIT Q
#define PSR_OVERFLOW_CONDITION_BIT V
#define PSR_CARRY_CONDITION_BIT C
#define PSR_ZERO_CONDITION_BIT Z
#define PSR_NEGATIVE_CONDITION_BIT N
} fields;
} PSR_t;
inline static PSR_t read_CPSR(void)
{
PSR_t CPSR;
// get content of current program status register
asm("mrs %0, cpsr" : "=r" (CPSR.raw) :: "memory");
return CPSR;
}
//// Write function not working for some reason... Assembly from gcc
//// looks ok. Needs checking on real hw.
//inline static void write_CPSR(PSR_t CPSR)
//{
// write to current program status register and synchronize context
// asm("msr cpsr, %0\n\r"
// "isb" :: "r" (CPSR.raw) : "memory");
//}
inline static void set_system_mode(void)
{
// hack to fix an unexplained bug; volatile needed in case of
// compilation with optimizations
volatile PSR_t CPSR __attribute__((unused)) = read_CPSR();
//// there are 2 ways of changing mode, both with the same
//// problem (see the long comment below)
//// way 1
// CPSR.fields.M_4_0 = MODE_SYSTEM;
// write to current program status register and synchronize context
// asm("msr cpsr, %0\n\r"
// "isb":: "r" (CPSR.raw) : "memory");
//// way 2
asm("cps #0b11111\n\r"
"isb" ::: "memory");
}
// The thing with writing to cpsr is weird. I used to have a single
// function that would set the system mode by:
// 1. reading the cpsr
// 2. modifying the value using bit shifts and logical or
// 3. writing the value back
// When introducing structs and bitfields I wanted to have separate
// functions for reading and writing the cpsr and have code up the
// call stack handle value modification. For some reason this didn't
// work - all would just hang when writing the CPSR. It turned out
// everything works if i call read_CPSR() from the same function, in
// which i write to cpsr. And I don't even need to use the value
// I read. I can just discard it (we're compiling without
// optimizations, so read_CPSR() is still called) and use the value
// passed as argument. I noticed an even weirder thing: the 2nd way
// of changing mode (cps instruction) doesn't work normally, but,
// just as msr, it does work when I call read_CPSR() before it. Even
// weirder - I cannot replace read_CPSR() with the actual assembly it
// does, because it stops working again! Asm generated by gcc looks
// ok, so Idk, maybe its some qemu issue (haven't tested on real RPi
// yet). I seem to have experienced uart printing inserted here and
// there do the same magic read_CPSR() does (but, again, not always).
// Unfortunately, nop repeated several hundreds times (or a recursive
// nop function, that just calls itself a given number of times)
// doesn't have this magical property :(
#endif // PSR_H
|
