1 /*
2 * @ : Copyright (c) 2021 Phytium Information Technology, Inc.
3 *
4 * SPDX-License-Identifier: Apache-2.0.
5 *
6 * @Date: 2021-04-20 11:32:32
7 * @LastEditTime: 2021-05-25 10:51:19
8 * @Description: Description of file
9 * @Modify History:
10 * * * Ver Who Date Changes
11 * * ----- ------ -------- --------------------------------------
12 */
13
14 #include "ft_parameters.h"
15 #include "ft_cpu.h"
16 #include "ft_assert.h"
17 #include "ft_printf.h"
18
19 #ifdef FT_SMP_EN
20
21 typedef union
22 {
23 u32 Slock;
24 struct ArchTicket
25 {
26 u16 owner;
27 u16 next;
28 } Tickets;
29 } FCpu_Lock_t;
30
31 struct FCpu
32 {
33 u32 IsReady;
34 FCpu_Lock_t Clock;
35 };
36
37 struct FCpu FCpu_Lock = {0};
38
39 const u32 SoftAffiTable[4] = {0, 1, 0x100, 0x101};
40
41 /**
42 * @name: FCpu_IdGet
43 * @msg: In a multiprocessor system, provides an additional PE identification mechanism for scheduling
44 purposes.
45 * @return {Aff0} Affinity level 0. The most significant affinity level field, for this PE in the system.
46 */
FCpu_IdGet(void)47 s32 FCpu_IdGet(void)
48 {
49 s32 cpu_id;
50 __asm__ volatile(
51 "mrc p15, 0, %0, c0, c0, 5"
52 : "=r"(cpu_id));
53 // Ft_printf("error cpu_id %x \r\n", cpu_id);
54 // cpu_id &= 0xf;
55
56 switch ((cpu_id & 0xfff))
57 {
58 case 1:
59 return 1;
60 case 0x100:
61 return 2;
62 case 0x101:
63 return 3;
64 default:
65 return (cpu_id & 0xf);
66 }
67 }
68
FCpu_AffinityGet(void)69 s32 FCpu_AffinityGet(void)
70 {
71 s32 AffinityId;
72 __asm__ volatile(
73 "mrc p15, 0, %0, c0, c0, 5"
74 : "=r"(AffinityId));
75
76 return AffinityId & 0xfff;
77 }
78
FCpu_SpinLockInit(void)79 void FCpu_SpinLockInit(void)
80 {
81 FCpu_Lock.Clock.Slock = 0;
82 FCpu_Lock.IsReady = FT_COMPONENT_IS_READLY;
83 }
84
FCpu_SpinLock(void)85 void FCpu_SpinLock(void)
86 {
87 u32 Tmp;
88 u32 Newval;
89 FCpu_Lock_t LockVal;
90 Ft_assertVoid(FCpu_Lock.IsReady == FT_COMPONENT_IS_READLY);
91
92 __asm__ __volatile__(
93 "pld [%0]" ::"r"(&FCpu_Lock.Clock.Slock));
94
95 __asm__ __volatile__(
96 "1: ldrex %0, [%3]\n"
97 " add %1, %0, %4\n"
98 " strex %2, %1, [%3]\n"
99 " teq %2, #0\n"
100 " bne 1b"
101 : "=&r"(LockVal), "=&r"(Newval), "=&r"(Tmp)
102 : "r"(&FCpu_Lock.Clock.Slock), "I"(1 << 16)
103 : "cc");
104
105 while (LockVal.Tickets.next != LockVal.Tickets.owner)
106 {
107 __asm__ __volatile__("wfe" ::
108 : "memory");
109 LockVal.Tickets.owner = *(volatile unsigned short *)(&FCpu_Lock.Clock.Tickets.owner);
110 }
111
112 __asm__ volatile("dmb" ::
113 : "memory");
114 }
115
FCpu_SpinUnlock(void)116 void FCpu_SpinUnlock(void)
117 {
118 Ft_assertVoid(FCpu_Lock.IsReady == FT_COMPONENT_IS_READLY);
119 __asm__ volatile("dmb" ::
120 : "memory");
121 FCpu_Lock.Clock.Tickets.owner++;
122 __asm__ volatile("dsb ishst\nsev" ::
123 : "memory");
124 }
125
126 #else /*RT_USING_SMP*/
127
FCpu_IdGet(void)128 s32 FCpu_IdGet(void)
129 {
130 return 0;
131 }
FCpu_SpinLockInit(void)132 void FCpu_SpinLockInit(void)
133 {
134 return;
135 }
FCpu_SpinLock(void)136 void FCpu_SpinLock(void)
137 {
138 return;
139 }
FCpu_SpinUnlock(void)140 void FCpu_SpinUnlock(void)
141 {
142 return;
143 }
144
145 #endif
146