xref: /kvm-unit-tests/lib/x86/smp.c (revision a4f9d79d66b6c110c20eef4b77ee4cda91370fa5)
17d36db35SAvi Kivity 
27d36db35SAvi Kivity #include <libcflat.h>
37d36db35SAvi Kivity #include "smp.h"
47d36db35SAvi Kivity #include "apic.h"
57d36db35SAvi Kivity #include "fwcfg.h"
6*a4f9d79dSJan Kiszka #include "desc.h"
77d36db35SAvi Kivity 
87d36db35SAvi Kivity #define IPI_VECTOR 0x20
97d36db35SAvi Kivity 
10fa816346SAvi Kivity typedef void (*ipi_function_type)(void *data);
11fa816346SAvi Kivity 
127d36db35SAvi Kivity static struct spinlock ipi_lock;
13fa816346SAvi Kivity static volatile ipi_function_type ipi_function;
14*a4f9d79dSJan Kiszka static void *volatile ipi_data;
157d36db35SAvi Kivity static volatile int ipi_done;
16e7bc5602SAvi Kivity static volatile bool ipi_wait;
17e7bc5602SAvi Kivity static int _cpu_count;
187d36db35SAvi Kivity 
197d36db35SAvi Kivity static __attribute__((used)) void ipi()
207d36db35SAvi Kivity {
21e7bc5602SAvi Kivity     void (*function)(void *data) = ipi_function;
22e7bc5602SAvi Kivity     void *data = ipi_data;
23e7bc5602SAvi Kivity     bool wait = ipi_wait;
24e7bc5602SAvi Kivity 
25e7bc5602SAvi Kivity     if (!wait) {
267d36db35SAvi Kivity 	ipi_done = 1;
27e7bc5602SAvi Kivity 	apic_write(APIC_EOI, 0);
28e7bc5602SAvi Kivity     }
29e7bc5602SAvi Kivity     function(data);
30e7bc5602SAvi Kivity     if (wait) {
31e7bc5602SAvi Kivity 	ipi_done = 1;
32e7bc5602SAvi Kivity 	apic_write(APIC_EOI, 0);
33e7bc5602SAvi Kivity     }
347d36db35SAvi Kivity }
357d36db35SAvi Kivity 
367d36db35SAvi Kivity asm (
377d36db35SAvi Kivity      "ipi_entry: \n"
387d36db35SAvi Kivity      "   call ipi \n"
397d36db35SAvi Kivity #ifndef __x86_64__
407d36db35SAvi Kivity      "   iret"
417d36db35SAvi Kivity #else
427d36db35SAvi Kivity      "   iretq"
437d36db35SAvi Kivity #endif
447d36db35SAvi Kivity      );
457d36db35SAvi Kivity 
467d36db35SAvi Kivity void spin_lock(struct spinlock *lock)
477d36db35SAvi Kivity {
487d36db35SAvi Kivity     int v = 1;
497d36db35SAvi Kivity 
507d36db35SAvi Kivity     do {
517d36db35SAvi Kivity 	asm volatile ("xchg %1, %0" : "+m"(lock->v), "+r"(v));
527d36db35SAvi Kivity     } while (v);
537d36db35SAvi Kivity     asm volatile ("" : : : "memory");
547d36db35SAvi Kivity }
557d36db35SAvi Kivity 
567d36db35SAvi Kivity void spin_unlock(struct spinlock *lock)
577d36db35SAvi Kivity {
587d36db35SAvi Kivity     asm volatile ("" : : : "memory");
597d36db35SAvi Kivity     lock->v = 0;
607d36db35SAvi Kivity }
617d36db35SAvi Kivity 
627d36db35SAvi Kivity int cpu_count(void)
637d36db35SAvi Kivity {
64a9f949c7SAvi Kivity     return _cpu_count;
657d36db35SAvi Kivity }
667d36db35SAvi Kivity 
677d36db35SAvi Kivity int smp_id(void)
687d36db35SAvi Kivity {
697d36db35SAvi Kivity     unsigned id;
707d36db35SAvi Kivity 
717d36db35SAvi Kivity     asm ("mov %%gs:0, %0" : "=r"(id));
727d36db35SAvi Kivity     return id;
737d36db35SAvi Kivity }
747d36db35SAvi Kivity 
757d36db35SAvi Kivity static void setup_smp_id(void *data)
767d36db35SAvi Kivity {
777d36db35SAvi Kivity     asm ("mov %0, %%gs:0" : : "r"(apic_id()) : "memory");
787d36db35SAvi Kivity }
797d36db35SAvi Kivity 
807d36db35SAvi Kivity static void __on_cpu(int cpu, void (*function)(void *data), void *data,
817d36db35SAvi Kivity                      int wait)
827d36db35SAvi Kivity {
837d36db35SAvi Kivity     spin_lock(&ipi_lock);
847d36db35SAvi Kivity     if (cpu == smp_id())
857d36db35SAvi Kivity 	function(data);
867d36db35SAvi Kivity     else {
877d36db35SAvi Kivity 	ipi_done = 0;
887d36db35SAvi Kivity 	ipi_function = function;
897d36db35SAvi Kivity 	ipi_data = data;
90e7bc5602SAvi Kivity 	ipi_wait = wait;
917d36db35SAvi Kivity 	apic_icr_write(APIC_INT_ASSERT | APIC_DEST_PHYSICAL | APIC_DM_FIXED
927d36db35SAvi Kivity                        | IPI_VECTOR,
937d36db35SAvi Kivity                        cpu);
947d36db35SAvi Kivity 	while (!ipi_done)
957d36db35SAvi Kivity 	    ;
967d36db35SAvi Kivity     }
977d36db35SAvi Kivity     spin_unlock(&ipi_lock);
987d36db35SAvi Kivity }
997d36db35SAvi Kivity 
1007d36db35SAvi Kivity void on_cpu(int cpu, void (*function)(void *data), void *data)
1017d36db35SAvi Kivity {
1027d36db35SAvi Kivity     __on_cpu(cpu, function, data, 1);
1037d36db35SAvi Kivity }
1047d36db35SAvi Kivity 
1057d36db35SAvi Kivity void on_cpu_async(int cpu, void (*function)(void *data), void *data)
1067d36db35SAvi Kivity {
1077d36db35SAvi Kivity     __on_cpu(cpu, function, data, 0);
1087d36db35SAvi Kivity }
1097d36db35SAvi Kivity 
1107d36db35SAvi Kivity 
1117d36db35SAvi Kivity void smp_init(void)
1127d36db35SAvi Kivity {
1137d36db35SAvi Kivity     int i;
1147d36db35SAvi Kivity     void ipi_entry(void);
1157d36db35SAvi Kivity 
116a9f949c7SAvi Kivity     _cpu_count = fwcfg_get_nb_cpus();
117a9f949c7SAvi Kivity 
1182bb987f3SAvi Kivity     setup_idt();
1192bb987f3SAvi Kivity     set_idt_entry(IPI_VECTOR, ipi_entry, 0);
1207d36db35SAvi Kivity 
1217d36db35SAvi Kivity     setup_smp_id(0);
1227d36db35SAvi Kivity     for (i = 1; i < cpu_count(); ++i)
1237d36db35SAvi Kivity         on_cpu(i, setup_smp_id, 0);
1247d36db35SAvi Kivity 
1257d36db35SAvi Kivity }
126