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
123
124
125
126
127
128
129
130 | --
-- Copyright (c) 2022-2023, German Rivera
--
--
-- SPDX-License-Identifier: Apache-2.0
--
--
-- @summary HiRTOS multi-core CPU interface for ARMv8-R architecture
--
with System.Machine_Code;
with Memory_Utils;
package body HiRTOS_Cpu_Multi_Core_Interface is
MPIDR_Core_Id_Mask : constant := 2#1111_1111#;
type Atomic_Operator_Type is (Test_Set,
Fetch_Add,
Fetch_Sub,
Fetch_Or,
Fetch_And);
function Get_Cpu_Id return Valid_Cpu_Core_Id_Type is
use type Interfaces.Unsigned_32;
Reg_Value : Interfaces.Unsigned_32;
begin
System.Machine_Code.Asm (
"mrc p15, 0, %0, c0, c0, 5", -- read MPIDR
Outputs => Interfaces.Unsigned_32'Asm_Output ("=r", Reg_Value), -- %0
Volatile => True);
Reg_Value := @ and MPIDR_Core_Id_Mask;
return Valid_Cpu_Core_Id_Type (Reg_Value);
end Get_Cpu_Id;
function Atomic_Operation (Atomic_Operator : Atomic_Operator_Type;
Atomic_Counter : in out Atomic_Counter_Type;
Value : Cpu_Register_Type)
return Cpu_Register_Type
with Inline_Always
is
Old_Value : Cpu_Register_Type;
New_Value : Cpu_Register_Type;
begin
loop
-- NOTE: Invalidate cache line to support multi-core processors without cache coherence
Memory_Utils.Invalidate_Data_Cache_Range (Atomic_Counter'Address, Cache_Line_Size_Bytes);
Old_Value := Ldrex_Word (Atomic_Counter.Counter'Address);
case Atomic_Operator is
when Test_Set =>
if Old_Value = Value then
return Old_Value;
end if;
New_Value := Value;
when Fetch_Add =>
New_Value := Old_Value + Value;
when Fetch_Sub =>
New_Value := Old_Value - Value;
when Fetch_Or =>
New_Value := Old_Value or Value;
when Fetch_And =>
New_Value := Old_Value and Value;
end case;
exit when Strex_Word (Atomic_Counter.Counter'Address, New_Value);
end loop;
-- NOTE: Flush cache line to support multi-core processors without cache coherence
Memory_Utils.Flush_Data_Cache_Range (Atomic_Counter'Address, Cache_Line_Size_Bytes);
return Old_Value;
end Atomic_Operation;
function Atomic_Test_Set (Atomic_Counter : in out Atomic_Counter_Type; Value : Cpu_Register_Type)
return Cpu_Register_Type is
(Atomic_Operation (Test_Set, Atomic_Counter, Value));
function Atomic_Fetch_Add (Atomic_Counter : in out Atomic_Counter_Type; Value : Cpu_Register_Type)
return Cpu_Register_Type is
(Atomic_Operation (Fetch_Add, Atomic_Counter, Value));
function Atomic_Fetch_Sub (Atomic_Counter : in out Atomic_Counter_Type; Value : Cpu_Register_Type)
return Cpu_Register_Type is
(Atomic_Operation (Fetch_Sub, Atomic_Counter, Value));
function Atomic_Fetch_Or (Atomic_Counter : in out Atomic_Counter_Type; Value : Cpu_Register_Type)
return Cpu_Register_Type is
(Atomic_Operation (Fetch_Or, Atomic_Counter, Value));
function Atomic_Fetch_And (Atomic_Counter : in out Atomic_Counter_Type; Value : Cpu_Register_Type)
return Cpu_Register_Type is
(Atomic_Operation (Fetch_And, Atomic_Counter, Value));
function Atomic_Load (Atomic_Counter : Atomic_Counter_Type)
return Cpu_Register_Type
is
begin
-- NOTE: Invalidate cache line to support multi-core processors without cache coherence
Memory_Utils.Invalidate_Data_Cache_Range (Atomic_Counter'Address, Cache_Line_Size_Bytes);
return Atomic_Counter.Counter;
end Atomic_Load;
procedure Atomic_Store (Atomic_Counter : out Atomic_Counter_Type; Value : Cpu_Register_Type)
is
begin
Atomic_Counter.Counter := Value;
-- NOTE: Flush cache line to support multi-core processors without cache coherence
Memory_Utils.Flush_Data_Cache_Range (Atomic_Counter'Address, Cache_Line_Size_Bytes);
end Atomic_Store;
procedure Spinlock_Acquire (Spinlock : in out Spinlock_Type) is
begin
while Atomic_Test_Set (Atomic_Counter_Type (Spinlock), 1) = 1 loop
HiRTOS_Cpu_Arch_Interface.Wait_For_Multicore_Event;
end loop;
HiRTOS_Cpu_Arch_Interface.Memory_Barrier;
end Spinlock_Acquire;
procedure Spinlock_Release (Spinlock : in out Spinlock_Type) is
begin
Atomic_Store (Atomic_Counter_Type (Spinlock), 0);
HiRTOS_Cpu_Arch_Interface.Memory_Barrier;
HiRTOS_Cpu_Arch_Interface.Send_Multicore_Event;
end Spinlock_Release;
end HiRTOS_Cpu_Multi_Core_Interface;
|