diff --git a/proxyclient/proxy.py b/proxyclient/proxy.py index 720b84b0..d3957dff 100755 --- a/proxyclient/proxy.py +++ b/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") diff --git a/src/exception.c b/src/exception.c index 3463ed71..a2fdbae9 100644 --- a/src/exception.c +++ b/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); } diff --git a/src/proxy.c b/src/proxy.c index eea8b828..19d064a6 100644 --- a/src/proxy.c +++ b/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; diff --git a/src/proxy.h b/src/proxy.h index da153b61..1b88082b 100644 --- a/src/proxy.h +++ b/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 diff --git a/src/smp.c b/src/smp.c index 537f6e3e..19b16a7a 100644 --- a/src/smp.c +++ b/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; +} diff --git a/src/smp.h b/src/smp.h index 5c4f6c7e..ed310d4e 100644 --- a/src/smp.h +++ b/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 diff --git a/src/startup.c b/src/startup.c index 6b67df81..c8319f2c 100644 --- a/src/startup.c +++ b/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(); }