kopia lustrzana https://github.com/majbthrd/pico-debug
59 wiersze
1.7 KiB
C
59 wiersze
1.7 KiB
C
/*
|
|
* Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
|
|
*
|
|
* SPDX-License-Identifier: BSD-3-Clause
|
|
*/
|
|
|
|
#include <rp2040.h>
|
|
|
|
static inline bool multicore_fifo_rvalid() {
|
|
return !!(sio_hw->fifo_st & SIO_FIFO_ST_VLD_BITS);
|
|
}
|
|
|
|
static inline void multicore_fifo_drain() {
|
|
while (multicore_fifo_rvalid())
|
|
(void) sio_hw->fifo_rd;
|
|
}
|
|
|
|
static inline bool multicore_fifo_wready() {
|
|
return !!(sio_hw->fifo_st & SIO_FIFO_ST_RDY_BITS);
|
|
}
|
|
|
|
static inline void multicore_fifo_push_blocking(uint32_t data) {
|
|
// We wait for the fifo to have some space
|
|
while (!multicore_fifo_wready())
|
|
tight_loop_contents();
|
|
|
|
sio_hw->fifo_wr = data;
|
|
|
|
// Fire off an event to the other core
|
|
__SEV();
|
|
}
|
|
|
|
static inline uint32_t multicore_fifo_pop_blocking(void) {
|
|
// If nothing there yet, we wait for an event first,
|
|
// to try and avoid too much busy waiting
|
|
while (!multicore_fifo_rvalid())
|
|
__WFE();
|
|
|
|
return sio_hw->fifo_rd;
|
|
}
|
|
|
|
__attribute__ (( section(".bootc") )) void multicore_launch_core1_raw(void (*entry)(void), uint32_t *sp, uint32_t vector_table) {
|
|
uint32_t cmd_sequence[] = {0, 0, 1, (uintptr_t) vector_table, (uintptr_t) sp, (uintptr_t) entry};
|
|
|
|
uint seq = 0;
|
|
do {
|
|
uint cmd = cmd_sequence[seq];
|
|
// we drain before sending a 0
|
|
if (!cmd) {
|
|
multicore_fifo_drain();
|
|
__SEV(); // core 1 may be waiting for fifo space
|
|
}
|
|
multicore_fifo_push_blocking(cmd);
|
|
uint32_t response = multicore_fifo_pop_blocking();
|
|
// move to next state on correct response otherwise start over
|
|
seq = cmd == response ? seq + 1 : 0;
|
|
} while (seq < (sizeof(cmd_sequence) / sizeof(*cmd_sequence)));
|
|
}
|