1 2 #include <libcflat.h> 3 #include "smp.h" 4 #include "apic.h" 5 #include "fwcfg.h" 6 #include "desc.h" 7 8 #define IPI_VECTOR 0x20 9 10 typedef void (*ipi_function_type)(void *data); 11 12 static struct spinlock ipi_lock; 13 static volatile ipi_function_type ipi_function; 14 static void *volatile ipi_data; 15 static volatile int ipi_done; 16 static volatile bool ipi_wait; 17 static int _cpu_count; 18 19 static __attribute__((used)) void ipi() 20 { 21 void (*function)(void *data) = ipi_function; 22 void *data = ipi_data; 23 bool wait = ipi_wait; 24 25 if (!wait) { 26 ipi_done = 1; 27 apic_write(APIC_EOI, 0); 28 } 29 function(data); 30 if (wait) { 31 ipi_done = 1; 32 apic_write(APIC_EOI, 0); 33 } 34 } 35 36 asm ( 37 "ipi_entry: \n" 38 " call ipi \n" 39 #ifndef __x86_64__ 40 " iret" 41 #else 42 " iretq" 43 #endif 44 ); 45 46 void spin_lock(struct spinlock *lock) 47 { 48 int v = 1; 49 50 do { 51 asm volatile ("xchg %1, %0" : "+m"(lock->v), "+r"(v)); 52 } while (v); 53 asm volatile ("" : : : "memory"); 54 } 55 56 void spin_unlock(struct spinlock *lock) 57 { 58 asm volatile ("" : : : "memory"); 59 lock->v = 0; 60 } 61 62 int cpu_count(void) 63 { 64 return _cpu_count; 65 } 66 67 int smp_id(void) 68 { 69 unsigned id; 70 71 asm ("mov %%gs:0, %0" : "=r"(id)); 72 return id; 73 } 74 75 static void setup_smp_id(void *data) 76 { 77 asm ("mov %0, %%gs:0" : : "r"(apic_id()) : "memory"); 78 } 79 80 static void __on_cpu(int cpu, void (*function)(void *data), void *data, 81 int wait) 82 { 83 spin_lock(&ipi_lock); 84 if (cpu == smp_id()) 85 function(data); 86 else { 87 ipi_done = 0; 88 ipi_function = function; 89 ipi_data = data; 90 ipi_wait = wait; 91 apic_icr_write(APIC_INT_ASSERT | APIC_DEST_PHYSICAL | APIC_DM_FIXED 92 | IPI_VECTOR, 93 cpu); 94 while (!ipi_done) 95 ; 96 } 97 spin_unlock(&ipi_lock); 98 } 99 100 void on_cpu(int cpu, void (*function)(void *data), void *data) 101 { 102 __on_cpu(cpu, function, data, 1); 103 } 104 105 void on_cpu_async(int cpu, void (*function)(void *data), void *data) 106 { 107 __on_cpu(cpu, function, data, 0); 108 } 109 110 111 void smp_init(void) 112 { 113 int i; 114 void ipi_entry(void); 115 116 _cpu_count = fwcfg_get_nb_cpus(); 117 118 setup_idt(); 119 set_idt_entry(IPI_VECTOR, ipi_entry, 0); 120 121 setup_smp_id(0); 122 for (i = 1; i < cpu_count(); ++i) 123 on_cpu(i, setup_smp_id, 0); 124 125 } 126