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