Basic CrossCall implementation

sys/amd64/include/amd64op.h

@@ -21,6 +21,11 @@ static INLINE void hlt()
     asm volatile("hlt");
 }
 
+static INLINE void pause()
+{
+    asm volatile("pause");
+}
+
 static INLINE void breakpoint()
 {
     asm volatile("int3");

sys/amd64/include/lapic.h

@@ -9,6 +9,7 @@ void LAPIC_Init();
 uint32_t LAPIC_CPU();
 void LAPIC_SendEOI();
 void LAPIC_StartAP(uint8_t apicid, uint32_t addr);
+int LAPIC_Broadcast(int vector);
 void LAPIC_Periodic(uint64_t rate);
 
 #endif /* __LAPIC_H__ */

sys/amd64/include/mp.h

@@ -5,6 +5,11 @@
 void MP_Init();
 void MP_InitAP();
 
+/* Cross Calls */
+typedef int (*CrossCallCB)(void *);
+void MP_CrossCallTrap();
+int MP_CrossCall(CrossCallCB cb, void *arg);
+
 uint32_t LAPIC_CPU();
 #define THISCPU	    LAPIC_CPU
 

sys/amd64/include/trap.h

@@ -40,6 +40,7 @@
 #define T_IRQ_THERMAL	(T_IRQ_BASE + 26)
 
 #define T_SYSCALL	60	/* System Call */
+#define T_CROSSCALL	61	/* Cross Call (IPI) */
 
 #define T_UNKNOWN	63	/* Unknown Trap */
 

sys/amd64/lapic.c

@@ -138,6 +138,24 @@ LAPIC_StartAP(uint8_t apicid, uint32_t addr)
     // XXX: Delay
 }
 
+int
+LAPIC_Broadcast(int vector)
+{
+    int i = 0;
+    LAPIC_Write(LAPIC_ICR_LO, LAPIC_ICR_EXCSELF | vector);
+
+    while ((LAPIC_Read(LAPIC_ICR_LO) & LAPIC_ICR_DELIVERY_PENDING) != 0) {
+	pause();
+
+	if (i > 1000000) {
+	    kprintf("IPI not delivered?\n");
+	    return -1;
+	}
+    }
+
+    return 0;
+}
+
 void
 LAPIC_Init()
 {

sys/amd64/mp.c

@@ -11,8 +11,11 @@
 #include <sys/mp.h>
 
 #include <machine/amd64.h>
+#include <machine/amd64op.h>
 #include <machine/pmap.h>
 #include <machine/lapic.h>
+#include <machine/mp.h>
+#include <machine/trap.h>
 
 extern uint8_t mpstart_begin[];
 extern uint8_t mpstart_end[];
@@ -21,15 +24,25 @@ extern AS systemAS;
 
 #define MP_WAITTIME	250000000ULL
 
+typedef struct CrossCallFrame {
+    CrossCallCB		cb;
+    void		*arg;
+    volatile int	count;
+    volatile int	done[MAX_CPUS];
+    volatile int	status[MAX_CPUS];
+} CrossCallFrame;
+
 #define CPUSTATE_NOT_PRESENT	0
 #define CPUSTATE_BOOTED		1
 
 typedef struct CPUState {
     int			state;
     UnixEpochNS		heartbeat;
+    CrossCallFrame	*frame;
 } CPUState;
 
 volatile static bool booted;
+volatile static int lastCPU;
 volatile static CPUState cpus[MAX_CPUS];
 
 static int
@@ -76,16 +89,25 @@ MP_Init()
     int i;
     kprintf("Booting on CPU %u\n", CPU());
 
+    cpus[CPU()].state = CPUSTATE_BOOTED;
+    cpus[CPU()].frame = NULL;
+
+    for (i = 1; i < MAX_CPUS; i++) {
+	cpus[i].state = CPUSTATE_NOT_PRESENT;
+	cpus[i].frame = NULL;
+    }
+
     /*
      * XXX: We really should read from the MP Table, but this appears to be 
      * reliable for now.
      */
+    lastCPU = 0;
     for (i = 1; i < MAX_CPUS; i++) {
 	if (MPBootAP(i) < 0)
 	    break;
-    }
 
-    cpus[CPU()].state = CPUSTATE_BOOTED;
+	lastCPU = i;
+    }
 }
 
 void
@@ -96,6 +118,98 @@ MP_InitAP()
     booted = 1;
 }
 
+void
+MP_CrossCallTrap()
+{
+    int c;
+
+    cpuid(0, 0, 0, 0, 0);
+
+    Critical_Enter();
+
+    for (c = 0; c <= lastCPU; c++) {
+	CrossCallFrame *frame = cpus[c].frame;
+	if (frame == NULL)
+	    continue;
+
+	if (frame->done[CPU()] == 1)
+	    continue;
+
+	frame->status[CPU()] = (frame->cb)(frame->arg);
+	frame->done[CPU()] = 1;
+
+	// Increment
+	__sync_add_and_fetch(&frame->count, 1);
+    }
+
+    Critical_Exit();
+}
+
+// XXX: The thread should not be migrated in the middle of this call.
+int
+MP_CrossCall(CrossCallCB cb, void *arg)
+{
+    CrossCallFrame frame;
+
+    // Setup frame
+    memset(&frame, 0, sizeof(frame));
+    frame.cb = cb;
+    frame.arg = arg;
+
+    Critical_Enter();
+
+    cpus[CPU()].frame = &frame;
+    cpuid(0, 0, 0, 0, 0);
+
+    if (LAPIC_Broadcast(T_CROSSCALL) < 0)
+	return -1;
+
+    // Run on the local CPU
+    frame.status[CPU()] = cb(arg);
+    frame.done[CPU()] = 1;
+
+    // Wait for all to respond
+    while (frame.count != lastCPU) {
+	// Check for timeout
+
+	// XXX: Should dump the crosscall frame
+    }
+    cpus[CPU()].frame = NULL;
+    cpuid(0, 0, 0, 0, 0);
+
+    Critical_Exit();
+
+    return 0;
+}
+
+static int
+MPPing(void *arg)
+{
+    //kprintf("CPU %d Ack\n", CPU());
+    return 0;
+}
+
+static void
+Debug_CrossCall(int argc, const char *argv[])
+{
+    int i;
+    UnixEpochNS startTS, stopTS;
+
+    startTS = KTime_GetEpochNS();
+    for (i = 0; i < 32; i++) {
+	MP_CrossCall(&MPPing, NULL);
+    }
+    stopTS = KTime_GetEpochNS();
+
+    // XXX: Print min and max
+    kprintf("Average CrossCall Latency: %llu ns\n",
+	    (stopTS - startTS) / 32ULL);
+
+    return;
+}
+
+REGISTER_DBGCMD(crosscall, "Ping crosscall", Debug_CrossCall);
+
 static void
 Debug_CPUS(int argc, const char *argv[])
 {

sys/amd64/trap.c

@@ -15,6 +15,7 @@
 #include <machine/amd64.h>
 #include <machine/lapic.h>
 #include <machine/trap.h>
+#include <machine/mp.h>
 
 #include <sys/thread.h>
 
@@ -224,6 +225,14 @@ trap_entry(TrapFrame *tf)
         return;
     }
 
+    // Cross calls
+    if (tf->vector == T_CROSSCALL)
+    {
+	MP_CrossCallTrap();
+	LAPIC_SendEOI();
+	return;
+    }
+
     // LAPIC Special Vectors
     if (tf->vector == T_IRQ_SPURIOUS)
     {