src/arch/riscv/smp.c

   1 /* SPDX-License-Identifier: GPL-2.0-only */
   2
   3 #include <arch/barrier.h>
   4 #include <arch/encoding.h>
   5 #include <arch/smp/smp.h>
   6 #include <arch/smp/atomic.h>
   7 #include <console/console.h>
   8 #include <mcall.h>
   9
  10 void smp_pause(int working_hartid)
  11 {
  12 #define SYNCA (OTHER_HLS(working_hartid)->entry.sync_a)
  13 #define SYNCB (OTHER_HLS(working_hartid)->entry.sync_b)
  14
  15         int hartid = read_csr(mhartid);
  16
  17         if (hartid != working_hartid) {
  18                 /* waiting for work hart */
  19                 do {
  20                         barrier();
  21                 } while (atomic_read(&SYNCA) != 0x01234567);
  22
  23                 clear_csr(mstatus, MSTATUS_MIE);
  24                 write_csr(mie, MIP_MSIP);
  25
  26                 /* count how many cores enter the halt */
  27                 atomic_add(&SYNCB, 1);
  28
  29                 do {
  30                         barrier();
  31                         __asm__ volatile ("wfi");
  32                 } while ((read_csr(mip) & MIP_MSIP) == 0);
  33                 set_msip(hartid, 0);
  34                 HLS()->entry.fn(HLS()->entry.arg);
  35         } else {
  36                 /* Initialize the counter and
  37                  * mark the work hart into smp_pause */
  38                 atomic_set(&SYNCB, 0);
  39                 atomic_set(&SYNCA, 0x01234567);
  40
  41                 /* waiting for other Hart to enter the halt */
  42                 do {
  43                         barrier();
  44                 } while (atomic_read(&SYNCB) + 1 < CONFIG_MAX_CPUS);
  45
  46                 /* initialize for the next call */
  47                 atomic_set(&SYNCA, 0);
  48                 atomic_set(&SYNCB, 0);
  49         }
  50 #undef SYNCA
  51 #undef SYNCB
  52 }
  53
  54 void smp_resume(void (*fn)(void *), void *arg)
  55 {
  56         int hartid = read_csr(mhartid);
  57
  58         if (fn == NULL)
  59                 die("must pass a non-null function pointer\n");
  60
  61         for (int i = 0; i < CONFIG_MAX_CPUS; i++) {
  62                 OTHER_HLS(i)->entry.fn = fn;
  63                 OTHER_HLS(i)->entry.arg = arg;
  64         }
  65
  66         for (int i = 0; i < CONFIG_MAX_CPUS; i++)
  67                 if (i != hartid)
  68                         set_msip(i, 1);
  69
  70         if (HLS()->entry.fn == NULL)
  71                 die("entry fn not set\n");
  72
  73         HLS()->entry.fn(HLS()->entry.arg);
  74 }