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