1
2 #include <stdbool.h>
3 #include <stdint.h>
4 #include <string.h>
5
6 #include <sys/kassert.h>
7 #include <sys/kconfig.h>
8 #include <sys/kdebug.h>
9 #include <sys/kmem.h>
10 #include <sys/ktime.h>
11 #include <sys/mp.h>
12
13 #include <machine/amd64.h>
14 #include <machine/amd64op.h>
15 #include <machine/pmap.h>
16 #include <machine/lapic.h>
17 #include <machine/mp.h>
18 #include <machine/trap.h>
19
20 extern uint8_t mpstart_begin[];
21 extern uint8_t mpstart_end[];
22
23 extern AS systemAS;
24
25 #define MP_WAITTIME 250000000ULL
26
27 typedef struct CrossCallFrame {
28 CrossCallCB cb;
29 void *arg;
30 volatile int count;
31 volatile int done[MAX_CPUS];
32 volatile int status[MAX_CPUS];
33 } CrossCallFrame;
34
35 const char *CPUStateToString[] = {
36 "NOT PRESENT",
37 "BOOTED",
38 "HALTED",
39 };
40
41 typedef struct CPUState {
42 int state;
43 UnixEpochNS heartbeat;
44 CrossCallFrame *frame;
45 } CPUState;
46
47 volatile static bool booted;
48 volatile static int lastCPU;
49 volatile static CPUState cpus[MAX_CPUS];
50
51 static int
MPBootAP(int procNo)52 MPBootAP(int procNo)
53 {
54 UnixEpochNS startTS, stopTS;
55 /*
56 * Arguments to mpstart are stored at 0x6F00
57 * arg[0] = CR3
58 * arg[1] = RSP
59 */
60 volatile uint64_t *args = (uint64_t *)DMPA2VA(0x6F00);
61
62 kprintf("Starting processor %d\n", procNo);
63 memcpy((void *)DMPA2VA(0x7000), mpstart_begin, mpstart_end - mpstart_begin);
64
65 args[0] = DMVA2PA((uint64_t)systemAS.root);
66 args[1] = PGSIZE + (uint64_t)PAlloc_AllocPage();
67
68 kprintf("CR3: %016llx RSP: %016llx\n", args[0], args[1]);
69
70 booted = 0;
71 LAPIC_StartAP(procNo, 0x7000);
72
73 startTS = KTime_GetEpochNS();
74 while (1) {
75 if (booted == 1)
76 break;
77
78 stopTS = KTime_GetEpochNS();
79 if ((stopTS - startTS) > MP_WAITTIME) {
80 kprintf("Processor %d did not respond in %d ms\n",
81 procNo, MP_WAITTIME / 1000000ULL);
82 PAlloc_Release((void *)(args[1] - PGSIZE));
83 return -1;
84 }
85 }
86
87 return 0;
88 }
89
90 void
MP_Init()91 MP_Init()
92 {
93 int i;
94 kprintf("Booting on CPU %u\n", CPU());
95
96 cpus[CPU()].state = CPUSTATE_BOOTED;
97 cpus[CPU()].frame = NULL;
98
99 for (i = 1; i < MAX_CPUS; i++) {
100 cpus[i].state = CPUSTATE_NOT_PRESENT;
101 cpus[i].frame = NULL;
102 }
103
104 /*
105 * XXX: We really should read from the MP Table, but this appears to be
106 * reliable for now.
107 */
108 lastCPU = 0;
109 for (i = 1; i < MAX_CPUS; i++) {
110 if (MPBootAP(i) < 0)
111 break;
112
113 lastCPU = i;
114 }
115 lastCPU++;
116 }
117
118 void
MP_InitAP()119 MP_InitAP()
120 {
121 kprintf("AP %d booted!\n", CPU());
122 cpus[CPU()].state = CPUSTATE_BOOTED;
123 booted = 1;
124 }
125
126 void
MP_SetState(int state)127 MP_SetState(int state)
128 {
129 ASSERT(state > 0 && state <= CPUSTATE_MAX);
130 cpus[CPU()].state = state;
131 }
132
133 int
MP_GetCPUs()134 MP_GetCPUs()
135 {
136 return lastCPU;
137 }
138
139 void
MP_CrossCallTrap()140 MP_CrossCallTrap()
141 {
142 int c;
143
144 cpuid(0, 0, 0, 0, 0);
145
146 Critical_Enter();
147
148 for (c = 0; c <= lastCPU; c++) {
149 CrossCallFrame *frame = cpus[c].frame;
150 if (frame == NULL)
151 continue;
152
153 if (frame->done[CPU()] == 1)
154 continue;
155
156 frame->status[CPU()] = (frame->cb)(frame->arg);
157 frame->done[CPU()] = 1;
158
159 // Increment
160 __sync_add_and_fetch(&frame->count, 1);
161 }
162
163 Critical_Exit();
164 }
165
166 // XXX: The thread should not be migrated in the middle of this call.
167 int
MP_CrossCall(CrossCallCB cb,void * arg)168 MP_CrossCall(CrossCallCB cb, void *arg)
169 {
170 volatile CrossCallFrame frame;
171
172 // Setup frame
173 memset((void *)&frame, 0, sizeof(frame));
174 frame.cb = cb;
175 frame.arg = arg;
176 frame.count = 1;
177
178 Critical_Enter();
179
180 cpus[CPU()].frame = (CrossCallFrame *)&frame;
181 cpuid(0, 0, 0, 0, 0);
182
183 if (LAPIC_Broadcast(T_CROSSCALL) < 0)
184 return -1;
185
186 // Run on the local CPU
187 frame.status[CPU()] = cb(arg);
188 frame.done[CPU()] = 1;
189
190 // Wait for all to respond
191 while (frame.count < lastCPU) {
192 // Check for timeout
193
194 // XXX: Should dump the crosscall frame
195 }
196 cpus[CPU()].frame = NULL;
197 cpuid(0, 0, 0, 0, 0);
198
199 Critical_Exit();
200
201 return 0;
202 }
203
204 static int
MPPing(void * arg)205 MPPing(void *arg)
206 {
207 //kprintf("CPU %d Ack\n", CPU());
208 return 0;
209 }
210
211 static void
Debug_CrossCall(int argc,const char * argv[])212 Debug_CrossCall(int argc, const char *argv[])
213 {
214 int i;
215 UnixEpochNS startTS, stopTS;
216
217 startTS = KTime_GetEpochNS();
218 for (i = 0; i < 32; i++) {
219 MP_CrossCall(&MPPing, NULL);
220 }
221 stopTS = KTime_GetEpochNS();
222
223 // XXX: Print min and max
224 kprintf("Average CrossCall Latency: %llu ns\n",
225 (stopTS - startTS) / 32ULL);
226
227 return;
228 }
229
230 REGISTER_DBGCMD(crosscall, "Ping crosscall", Debug_CrossCall);
231
232 static void
Debug_CPUS(int argc,const char * argv[])233 Debug_CPUS(int argc, const char *argv[])
234 {
235 int c;
236
237 for (c = 0; c < MAX_CPUS; c++) {
238 if (cpus[c].state != CPUSTATE_NOT_PRESENT) {
239 kprintf("CPU %d: %s\n", c, CPUStateToString[cpus[c].state]);
240 }
241 }
242 }
243
244 REGISTER_DBGCMD(cpus, "Show MP information", Debug_CPUS);
245
246 static void
Debug_CPU(int argc,const char * argv[])247 Debug_CPU(int argc, const char *argv[])
248 {
249 kprintf("CPU %d\n", CPU());
250 }
251
252 REGISTER_DBGCMD(cpu, "Current CPU number", Debug_CPU);
253
254