smp: add scaffolding and proxy funcs for cross-CPU calls

Signed-off-by: Hector Martin <marcan@marcan.st>

proxyclient/proxy.py

@@ -303,6 +303,10 @@ class M1N1Proxy:
     P_XZDEC = 0x400
     P_GZDEC = 0x401
 
+    P_SMP_START_SECONDARIES = 0x500
+    P_SMP_CALL = 0x501
+    P_SMP_CALL_SYNC = 0x502
+
     def __init__(self, iface, debug=False):
         self.debug = debug
         self.iface = iface
@@ -497,6 +501,19 @@ class M1N1Proxy:
         return self.request(self.P_GZDEC, inbuf, insize, outbuf,
                             outsize, signed=True)
 
+    def smp_start_secondaries(self):
+        self.request(self.P_SMP_START_SECONDARIES)
+
+    def smp_call(self, cpu, addr, *args):
+        if len(args) > 4:
+            raise ValueError("Too many arguments")
+        self.request(self.P_SMP_CALL, cpu, addr, *args)
+
+    def smp_call_sync(self, cpu, addr, *args):
+        if len(args) > 4:
+            raise ValueError("Too many arguments")
+        return self.request(self.P_SMP_CALL_SYNC, cpu, addr, *args)
+
 if __name__ == "__main__":
     import serial
     uartdev = os.environ.get("M1N1DEVICE", "/dev/ttyUSB0")

src/exception.c

@@ -7,8 +7,6 @@ extern char _vectors_start[0];
 
 void exception_initialize(void)
 {
-    printf("Initializing exceptions...\n");
-
     msr(vbar_el2, _vectors_start);
 }
 

src/proxy.c

@@ -1,6 +1,7 @@
 #include "proxy.h"
 #include "memory.h"
 #include "minilzlib/minlzma.h"
+#include "smp.h"
 #include "tinf/tinf.h"
 #include "types.h"
 #include "uart.h"
@@ -193,6 +194,19 @@ int proxy_process(ProxyRequest *request, ProxyReply *reply)
             break;
         }
 
+        case P_SMP_START_SECONDARIES:
+            smp_start_secondaries();
+            break;
+        case P_SMP_CALL:
+            smp_call4(request->args[0], (void *)request->args[1], request->args[2],
+                      request->args[3], request->args[4], request->args[5]);
+            break;
+        case P_SMP_CALL_SYNC:
+            smp_call4(request->args[0], (void *)request->args[1], request->args[2],
+                      request->args[3], request->args[4], request->args[5]);
+            reply->retval = smp_wait(request->args[0]);
+            break;
+
         default:
             reply->status = S_BADCMD;
             break;

src/proxy.h

@@ -54,9 +54,13 @@ typedef enum {
     P_DC_CVAU,
     P_DC_CIVAC,
 
-    P_XZDEC = 0x400,
+    P_XZDEC = 0x400, // Decompression and data processing ops
     P_GZDEC,
 
+    P_SMP_START_SECONDARIES = 0x500, // SMP and system management ops
+    P_SMP_CALL,
+    P_SMP_CALL_SYNC,
+
 } ProxyOp;
 
 #define S_OK     0

src/smp.c

@@ -13,14 +13,16 @@
 struct spin_table {
     u64 flag;
     u64 target;
+    u64 args[4];
+    u64 retval;
 };
 
 void *_reset_stack;
 
-u8 secondary_stacks[MAX_CPUS][SECONDARY_STACK_SIZE] ALIGNED(64);
+static u8 secondary_stacks[MAX_CPUS][SECONDARY_STACK_SIZE] ALIGNED(64);
 
-int target_cpu;
-struct spin_table spin_table[MAX_CPUS];
+static int target_cpu;
+static struct spin_table spin_table[MAX_CPUS];
 
 extern u8 _vectors_start[0];
 
@@ -38,10 +40,13 @@ void smp_secondary_entry(void)
             sysop("wfe");
         }
         sysop("dmb sy");
-        me->target = 0;
         me->flag++;
         sysop("dmb sy");
-        ((void (*)(void))target)();
+        me->retval = ((u64(*)(u64 a, u64 b, u64 c, u64 d))target)(me->args[0], me->args[1],
+                                                                  me->args[2], me->args[3]);
+        sysop("dmb sy");
+        me->target = 0;
+        sysop("dmb sy");
     }
 }
 
@@ -49,9 +54,12 @@ static void smp_start_cpu(int index, int cluster, int core, u64 rvbar, u64 cpu_s
 {
     int i;
 
+    if (spin_table[index].flag)
+        return;
+
     printf("Starting CPU %d (%d:%d)... ", index, cluster, core);
 
-    spin_table[index].flag = 0;
+    memset(&spin_table[index], 0, sizeof(struct spin_table));
 
     target_cpu = index;
     _reset_stack = secondary_stacks[index] + SECONDARY_STACK_SIZE;
@@ -133,3 +141,33 @@ void smp_start_secondaries(void)
         smp_start_cpu(i, reg >> 8, reg & 0xff, cpu_impl_reg[0], pmgr_reg + CPU_START_OFF);
     }
 }
+
+void smp_call4(int cpu, void *func, u64 arg0, u64 arg1, u64 arg2, u64 arg3)
+{
+    struct spin_table *target = &spin_table[cpu];
+
+    if (cpu == 0)
+        return;
+
+    u64 flag = target->flag;
+    target->args[0] = arg0;
+    target->args[1] = arg1;
+    target->args[2] = arg2;
+    target->args[3] = arg3;
+    sysop("dmb sy");
+    target->target = (u64)func;
+    sysop("dmb sy");
+    sysop("sev");
+    while (target->flag == flag)
+        sysop("dmb sy");
+}
+
+u64 smp_wait(int cpu)
+{
+    struct spin_table *target = &spin_table[cpu];
+
+    while (target->target)
+        sysop("dmb sy");
+
+    return target->retval;
+}

src/smp.h

@@ -3,10 +3,21 @@
 #ifndef __SMP_H__
 #define __SMP_H__
 
+#include "types.h"
+
 #define MAX_CPUS 8
 
 void smp_secondary_entry(void);
 
 void smp_start_secondaries(void);
 
+#define smp_call0(i, f)          smp_call(i, f, 0, 0, 0, 0)
+#define smp_call1(i, f, a)       smp_call(i, f, a, 0, 0, 0)
+#define smp_call2(i, f, a, b)    smp_call(i, f, a, b, 0, 0)
+#define smp_call3(i, f, a, b, c) smp_call(i, f, a, b, c, 0)
+
+void smp_call4(int cpu, void *func, u64 arg0, u64 arg1, u64 arg2, u64 arg3);
+
+u64 smp_wait(int cpu);
+
 #endif

src/startup.c

@@ -98,5 +98,6 @@ void _cpu_reset_c(void *stack)
     const char *type = init_cpu();
     printf("  CPU: %s\n", type);
 
+    exception_initialize();
     smp_secondary_entry();
 }