1 /*
2  * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
3  *
4  * SPDX-License-Identifier: BSD-3-Clause
5  */
6 
7 #include "pico/multicore.h"
8 #include "hardware/sync.h"
9 #include "hardware/irq.h"
10 #include "pico/runtime_init.h"
11 #ifdef __riscv
12 #include "hardware/riscv.h"
13 #else
14 #include "hardware/structs/scb.h"
15 #endif
16 #include "hardware/structs/sio.h"
17 #include "hardware/regs/psm.h"
18 #include "hardware/claim.h"
19 
20 #if !PICO_RP2040
21 #ifndef __riscv
22 #include "hardware/structs/m33.h"
23 #endif
24 #endif
25 
26 // note that these are not reset by core reset, however for now, I think people resetting cores
27 // and then relying on multicore_lockout for that core without re-initializing, is probably
28 // something we can live with breaking.
29 //
30 // whilst we could clear this in core 1 reset path, that doesn't necessarily catch all,
31 // and means pulling in this array even if multicore_lockout is not used.
32 static bool lockout_victim_initialized[NUM_CORES];
33 
multicore_fifo_push_blocking(uint32_t data)34 void multicore_fifo_push_blocking(uint32_t data) {
35     multicore_fifo_push_blocking_inline(data);
36 }
37 
multicore_fifo_push_timeout_us(uint32_t data,uint64_t timeout_us)38 bool multicore_fifo_push_timeout_us(uint32_t data, uint64_t timeout_us) {
39     absolute_time_t end_time = make_timeout_time_us(timeout_us);
40     // We wait for the fifo to have some space
41     while (!multicore_fifo_wready()) {
42         tight_loop_contents();
43         if (time_reached(end_time)) return false;
44     }
45 
46     sio_hw->fifo_wr = data;
47 
48     // Fire off an event to the other core
49     __sev();
50     return true;
51 }
52 
multicore_fifo_pop_blocking(void)53 uint32_t multicore_fifo_pop_blocking(void) {
54     return multicore_fifo_pop_blocking_inline();
55 }
56 
multicore_fifo_pop_timeout_us(uint64_t timeout_us,uint32_t * out)57 bool multicore_fifo_pop_timeout_us(uint64_t timeout_us, uint32_t *out) {
58     absolute_time_t end_time = make_timeout_time_us(timeout_us);
59     // If nothing there yet, we wait for an event first,
60     // to try and avoid too much busy waiting
61     while (!multicore_fifo_rvalid()) {
62         if (best_effort_wfe_or_timeout(end_time)) return false;
63     }
64 
65     *out = sio_hw->fifo_rd;
66     return true;
67 }
68 
69 #if PICO_CORE1_STACK_SIZE > 0
70 // Default stack for core1 ... if multicore_launch_core1 is not included then .stack1 section will be garbage collected
71 static uint32_t __attribute__((section(".stack1"))) core1_stack[PICO_CORE1_STACK_SIZE / sizeof(uint32_t)];
72 #endif
73 
core1_trampoline(void)74 static void __attribute__ ((naked)) core1_trampoline(void) {
75 #ifdef __riscv
76     // Do not add function calls here, because we want to preserve the return
77     // address pointing back to the bootrom launch routine.
78     pico_default_asm(
79         "lw a0, 0(sp)\n"
80         "lw a1, 4(sp)\n"
81         "lw a2, 8(sp)\n"
82         "lw gp, 12(sp)\n"
83         "addi sp, sp, 16\n"
84         "jr a2\n"
85     );
86 #else
87     pico_default_asm("pop {r0, r1, pc}");
88 #endif
89 }
90 
core1_wrapper(int (* entry)(void),void * stack_base)91 int core1_wrapper(int (*entry)(void), void *stack_base) {
92 #if !PICO_RUNTIME_SKIP_INIT_PER_CORE_INSTALL_STACK_GUARD
93     // install core1 stack guard
94     runtime_init_per_core_install_stack_guard(stack_base);
95 #else
96     (void)stack_base;
97 #endif
98     runtime_run_per_core_initializers();
99     return (*entry)();
100 }
101 
multicore_reset_core1(void)102 void multicore_reset_core1(void) {
103     // Use atomic aliases just in case core 1 is also manipulating some PSM state
104     io_rw_32 *power_off = (io_rw_32 *) (PSM_BASE + PSM_FRCE_OFF_OFFSET);
105     io_rw_32 *power_off_set = hw_set_alias(power_off);
106     io_rw_32 *power_off_clr = hw_clear_alias(power_off);
107 
108     // Hard-reset core 1.
109     // Reading back confirms the core 1 reset is in the correct state, but also
110     // forces APB IO bridges to fence on any internal store buffering
111     *power_off_set = PSM_FRCE_OFF_PROC1_BITS;
112     while (!(*power_off & PSM_FRCE_OFF_PROC1_BITS)) tight_loop_contents();
113 
114     // Allow for the fact that the caller may have already enabled the FIFO IRQ for their
115     // own purposes (expecting FIFO content after core 1 is launched). We must disable
116     // the IRQ during the handshake, then restore afterward
117     uint irq_num = SIO_FIFO_IRQ_NUM(0);
118     bool enabled = pico_irq_is_enabled(irq_num);
119     irq_set_enabled(irq_num, false);
120 
121     // Bring core 1 back out of reset. It will drain its own mailbox FIFO, then push
122     // a 0 to our mailbox to tell us it has done this.
123     *power_off_clr = PSM_FRCE_OFF_PROC1_BITS;
124 
125     // check the pushed value
126     uint32_t value = multicore_fifo_pop_blocking();
127     assert(value == 0);
128     (void) value; // silence warning
129 
130     // restore interrupt state
131     irq_set_enabled(irq_num, enabled);
132 }
133 
multicore_launch_core1_with_stack(void (* entry)(void),uint32_t * stack_bottom,size_t stack_size_bytes)134 void multicore_launch_core1_with_stack(void (*entry)(void), uint32_t *stack_bottom, size_t stack_size_bytes) {
135     assert(!(stack_size_bytes & 3u));
136     uint32_t *stack_ptr = stack_bottom + stack_size_bytes / sizeof(uint32_t);
137     // Push values onto top of stack for core1_trampoline
138 #ifdef __riscv
139     // On RISC-V we also need to initialise the global pointer
140     stack_ptr -= 4;
141     uint32_t vector_table = riscv_read_csr(mtvec);
142     asm volatile ("mv %0, gp" : "=r"(stack_ptr[3]));
143 #else
144     stack_ptr -= 3;
145     uint32_t vector_table = scb_hw->vtor;
146 #endif
147     stack_ptr[0] = (uintptr_t) entry;
148     stack_ptr[1] = (uintptr_t) stack_bottom;
149     stack_ptr[2] = (uintptr_t) core1_wrapper;
150 #if PICO_VTABLE_PER_CORE
151     #warning PICO_VTABLE_PER_CORE==1 is not currently supported in pico_multicore
152     panic_unsupported();
153 #endif
154     multicore_launch_core1_raw(core1_trampoline, stack_ptr, vector_table);
155 }
156 
multicore_launch_core1(void (* entry)(void))157 void multicore_launch_core1(void (*entry)(void)) {
158 #if PICO_CORE1_STACK_SIZE > 0
159     extern uint32_t __StackOneBottom;
160     uint32_t *stack_limit = (uint32_t *) &__StackOneBottom;
161     // hack to reference core1_stack although that pointer is wrong.... core1_stack should always be <= stack_limit, if not boom!
162     uint32_t *stack = core1_stack <= stack_limit ? stack_limit : (uint32_t *) -1;
163     multicore_launch_core1_with_stack(entry, stack, sizeof(core1_stack));
164 #else
165     panic("multicore_launch_core1() can't be used when PICO_CORE1_STACK_SIZE == 0");
166 #endif
167 }
168 
multicore_launch_core1_raw(void (* entry)(void),uint32_t * sp,uint32_t vector_table)169 void multicore_launch_core1_raw(void (*entry)(void), uint32_t *sp, uint32_t vector_table) {
170     // Allow for the fact that the caller may have already enabled the FIFO IRQ for their
171     // own purposes (expecting FIFO content after core 1 is launched). We must disable
172     // the IRQ during the handshake, then restore afterwards.
173     uint irq_num = SIO_FIFO_IRQ_NUM(0);
174     bool enabled = pico_irq_is_enabled(irq_num);
175     irq_set_enabled(irq_num, false);
176 
177     // Values to be sent in order over the FIFO from core 0 to core 1
178     //
179     // vector_table is value for VTOR register
180     // sp is initial stack pointer (SP)
181     // entry is the initial program counter (PC) (don't forget to set the thumb bit!)
182     const uint32_t cmd_sequence[] =
183             {0, 0, 1, (uintptr_t) vector_table, (uintptr_t) sp, (uintptr_t) entry};
184 
185     uint seq = 0;
186     do {
187         uint cmd = cmd_sequence[seq];
188         // Always drain the READ FIFO (from core 1) before sending a 0
189         if (!cmd) {
190             multicore_fifo_drain();
191             // Execute a SEV as core 1 may be waiting for FIFO space via WFE
192             __sev();
193         }
194         multicore_fifo_push_blocking(cmd);
195         uint32_t response = multicore_fifo_pop_blocking();
196         // Move to next state on correct response (echo-d value) otherwise start over
197         seq = cmd == response ? seq + 1 : 0;
198     } while (seq < count_of(cmd_sequence));
199 
200     irq_set_enabled(irq_num, enabled);
201 }
202 
203 #define LOCKOUT_MAGIC_START 0x73a8831eu
204 #define LOCKOUT_MAGIC_END (~LOCKOUT_MAGIC_START)
205 
206 static mutex_t lockout_mutex;
207 static bool lockout_in_progress;
208 
209 // note this method is in RAM because lockout is used when writing to flash
210 // it only makes inline calls
__not_in_flash_func(multicore_lockout_handler)211 static void __isr __not_in_flash_func(multicore_lockout_handler)(void) {
212     multicore_fifo_clear_irq();
213     while (multicore_fifo_rvalid()) {
214         if (sio_hw->fifo_rd == LOCKOUT_MAGIC_START) {
215             uint32_t save = save_and_disable_interrupts();
216             multicore_fifo_push_blocking_inline(LOCKOUT_MAGIC_START);
217             while (multicore_fifo_pop_blocking_inline() != LOCKOUT_MAGIC_END) {
218                 tight_loop_contents(); // not tight but endless potentially
219             }
220             restore_interrupts_from_disabled(save);
221             multicore_fifo_push_blocking_inline(LOCKOUT_MAGIC_END);
222         }
223     }
224 }
225 
check_lockout_mutex_init(void)226 static void check_lockout_mutex_init(void) {
227     // use known available lock - we only need it briefly
228     uint32_t save = hw_claim_lock();
229     if (!mutex_is_initialized(&lockout_mutex)) {
230         mutex_init(&lockout_mutex);
231     }
232     hw_claim_unlock(save);
233 }
234 
multicore_lockout_victim_init(void)235 void multicore_lockout_victim_init(void) {
236     check_lockout_mutex_init();
237     // On platforms other than RP2040, these are actually the same IRQ number
238     // (each core only sees its own IRQ, always at the same IRQ number).
239     uint core_num = get_core_num();
240     uint fifo_irq_this_core = SIO_FIFO_IRQ_NUM(core_num);
241     irq_set_exclusive_handler(fifo_irq_this_core, multicore_lockout_handler);
242     irq_set_enabled(fifo_irq_this_core, true);
243     lockout_victim_initialized[core_num] = true;
244 }
245 
multicore_lockout_handshake(uint32_t magic,absolute_time_t until)246 static bool multicore_lockout_handshake(uint32_t magic, absolute_time_t until) {
247     uint irq_num = SIO_FIFO_IRQ_NUM(get_core_num());
248     bool enabled = pico_irq_is_enabled(irq_num);
249     if (enabled) irq_set_enabled(irq_num, false);
250     bool rc = false;
251     do {
252         int64_t next_timeout_us = absolute_time_diff_us(get_absolute_time(), until);
253         if (next_timeout_us < 0) {
254             break;
255         }
256         multicore_fifo_push_timeout_us(magic, (uint64_t)next_timeout_us);
257         next_timeout_us = absolute_time_diff_us(get_absolute_time(), until);
258         if (next_timeout_us < 0) {
259             break;
260         }
261         uint32_t word = 0;
262         if (!multicore_fifo_pop_timeout_us((uint64_t)next_timeout_us, &word)) {
263             break;
264         }
265         if (word == magic) {
266             rc = true;
267         }
268     } while (!rc);
269     if (enabled) irq_set_enabled(irq_num, true);
270     return rc;
271 }
272 
multicore_lockout_start_block_until(absolute_time_t until)273 static bool multicore_lockout_start_block_until(absolute_time_t until) {
274     check_lockout_mutex_init();
275     if (!mutex_enter_block_until(&lockout_mutex, until)) {
276         return false;
277     }
278     hard_assert(!lockout_in_progress);
279     bool rc = multicore_lockout_handshake(LOCKOUT_MAGIC_START, until);
280     lockout_in_progress = rc;
281     mutex_exit(&lockout_mutex);
282     return rc;
283 }
284 
multicore_lockout_start_timeout_us(uint64_t timeout_us)285 bool multicore_lockout_start_timeout_us(uint64_t timeout_us) {
286     return multicore_lockout_start_block_until(make_timeout_time_us(timeout_us));
287 }
288 
multicore_lockout_start_blocking(void)289 void multicore_lockout_start_blocking(void) {
290     multicore_lockout_start_block_until(at_the_end_of_time);
291 }
292 
multicore_lockout_end_block_until(absolute_time_t until)293 static bool multicore_lockout_end_block_until(absolute_time_t until) {
294     assert(mutex_is_initialized(&lockout_mutex));
295     if (!mutex_enter_block_until(&lockout_mutex, until)) {
296         return false;
297     }
298     assert(lockout_in_progress);
299     bool rc = multicore_lockout_handshake(LOCKOUT_MAGIC_END, until);
300     if (rc) {
301         lockout_in_progress = false;
302     }
303     mutex_exit(&lockout_mutex);
304     return rc;
305 }
306 
multicore_lockout_end_timeout_us(uint64_t timeout_us)307 bool multicore_lockout_end_timeout_us(uint64_t timeout_us) {
308     return multicore_lockout_end_block_until(make_timeout_time_us(timeout_us));
309 }
310 
multicore_lockout_end_blocking(void)311 void multicore_lockout_end_blocking(void) {
312     multicore_lockout_end_block_until(at_the_end_of_time);
313 }
314 
multicore_lockout_victim_is_initialized(uint core_num)315 bool multicore_lockout_victim_is_initialized(uint core_num) {
316     return lockout_victim_initialized[core_num];
317 }
318 
319 #if NUM_DOORBELLS
320 
321 static uint8_t doorbell_claimed[NUM_CORES][(NUM_DOORBELLS + 7) >> 3];
322 
is_bit_claimed(const uint8_t * bits,uint bit_index)323 static inline bool is_bit_claimed(const uint8_t *bits, uint bit_index) {
324     return (bits[bit_index >> 3u] & (1u << (bit_index & 7u)));
325 }
326 
set_claimed_bit(uint8_t * bits,uint bit_index)327 static inline void set_claimed_bit(uint8_t *bits, uint bit_index) {
328     bits[bit_index >> 3u] |= ( uint8_t ) ( 1u << ( bit_index & 7u ));
329 }
330 
clear_claimed_bit(uint8_t * bits,uint bit_index)331 static inline void clear_claimed_bit(uint8_t *bits, uint bit_index) {
332     bits[bit_index >> 3u] &= ( uint8_t ) ~( 1u << ( bit_index & 7u ));
333 }
334 
multicore_doorbell_claim_under_lock(uint doorbell_num,uint core_mask,bool required)335 static bool multicore_doorbell_claim_under_lock(uint doorbell_num, uint core_mask, bool required) {
336     static_assert(NUM_CORES == 2, "");
337     uint claimed_cores_for_doorbell = (uint) (is_bit_claimed(doorbell_claimed[0], doorbell_num) |
338                                               (is_bit_claimed(doorbell_claimed[1], doorbell_num + 1u) << 1));
339     if (claimed_cores_for_doorbell & core_mask) {
340         if (required) {
341             panic( "Multicoore doorbell %d already claimed on core mask 0x%x; requested core mask 0x%x\n",
342                    claimed_cores_for_doorbell, core_mask);
343         }
344         return false;
345     } else {
346         for(uint i=0; i<NUM_CORES; i++) {
347             if (core_mask & (1u << i)) {
348                 set_claimed_bit(doorbell_claimed[i], doorbell_num);
349             }
350         }
351         return true;
352     }
353 }
354 
multicore_doorbell_claim(uint doorbell_num,uint core_mask)355 void multicore_doorbell_claim(uint doorbell_num, uint core_mask) {
356     check_doorbell_num_param(doorbell_num);
357     uint32_t save = hw_claim_lock();
358     multicore_doorbell_claim_under_lock(doorbell_num, core_mask, true);
359     hw_claim_unlock(save);
360 }
361 
multicore_doorbell_claim_unused(uint core_mask,bool required)362 int multicore_doorbell_claim_unused(uint core_mask, bool required) {
363     int rc = PICO_ERROR_INSUFFICIENT_RESOURCES;
364     uint32_t save = hw_claim_lock();
365     for(int i=NUM_DOORBELLS-1; i>=0; i--) {
366         if (multicore_doorbell_claim_under_lock((uint) i, core_mask, false)) {
367             rc = i;
368             break;
369         }
370     }
371     if (required && rc < 0) {
372         panic("No free doorbells");
373     }
374     hw_claim_unlock(save);
375     return rc;
376 }
377 
multicore_doorbell_unclaim(uint doorbell_num,uint core_mask)378 void multicore_doorbell_unclaim(uint doorbell_num, uint core_mask) {
379     check_doorbell_num_param(doorbell_num);
380     uint32_t save = hw_claim_lock();
381     for(uint i=0; i < NUM_CORES; i++) {
382         if (core_mask & (1u << i)) {
383             assert(is_bit_claimed(doorbell_claimed[i], doorbell_num));
384             clear_claimed_bit(doorbell_claimed[i], doorbell_num);
385         }
386     }
387     hw_claim_unlock(save);
388 }
389 
390 
391 #endif