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