|
|
|
@ -9,30 +9,41 @@
|
|
|
|
|
#![allow(dead_code)]
|
|
|
|
|
|
|
|
|
|
use core::mem::{uninitialized, transmute};
|
|
|
|
|
use core::ptr::write_volatile;
|
|
|
|
|
use r0::zero_bss;
|
|
|
|
|
use compiler_builtins as _;
|
|
|
|
|
use smoltcp::wire::{EthernetAddress, IpAddress, IpCidr};
|
|
|
|
|
use smoltcp::iface::{NeighborCache, EthernetInterfaceBuilder, EthernetInterface};
|
|
|
|
|
use smoltcp::time::Instant;
|
|
|
|
|
use smoltcp::socket::SocketSet;
|
|
|
|
|
use mailbox::{MAILBOX_FROM_CORE0, MAILBOX_FROM_CORE1};
|
|
|
|
|
|
|
|
|
|
mod regs;
|
|
|
|
|
mod cortex_a9;
|
|
|
|
|
mod clocks;
|
|
|
|
|
mod mailbox;
|
|
|
|
|
mod mpcore;
|
|
|
|
|
mod mutex;
|
|
|
|
|
mod slcr;
|
|
|
|
|
mod uart;
|
|
|
|
|
mod stdio;
|
|
|
|
|
mod eth;
|
|
|
|
|
|
|
|
|
|
use crate::regs::{RegisterR, RegisterW};
|
|
|
|
|
use crate::regs::{RegisterR, RegisterW, RegisterRW};
|
|
|
|
|
use crate::cortex_a9::{asm, regs::*, mmu};
|
|
|
|
|
|
|
|
|
|
extern "C" {
|
|
|
|
|
static mut __bss_start: u32;
|
|
|
|
|
static mut __bss_end: u32;
|
|
|
|
|
static mut __stack_start: u32;
|
|
|
|
|
static mut __stack_start: u32; // refers to the stack for core 0
|
|
|
|
|
static mut __stack1_start: u32; // refers to the stack for core 1
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// program address as u32, for execution after setting up core 1
|
|
|
|
|
static mut START_ADDR_CORE1: u32 = 0;
|
|
|
|
|
// initial stack pointer for starting core 1
|
|
|
|
|
static mut INITIAL_SP_CORE1: u32 = 0; // must be zero (as a flag)
|
|
|
|
|
|
|
|
|
|
#[link_section = ".text.boot"]
|
|
|
|
|
#[no_mangle]
|
|
|
|
|
#[naked]
|
|
|
|
@ -41,13 +52,24 @@ pub unsafe extern "C" fn _boot_cores() -> ! {
|
|
|
|
|
|
|
|
|
|
match MPIDR.read() & CORE_MASK {
|
|
|
|
|
0 => {
|
|
|
|
|
// executing on core 0
|
|
|
|
|
SP.write(&mut __stack_start as *mut _ as u32);
|
|
|
|
|
boot_core0();
|
|
|
|
|
}
|
|
|
|
|
_ => loop {
|
|
|
|
|
// if not core0, infinitely wait for events
|
|
|
|
|
asm::wfe();
|
|
|
|
|
},
|
|
|
|
|
_ => {
|
|
|
|
|
// executing on core 1 (as there are only cores 0 and 1)
|
|
|
|
|
while INITIAL_SP_CORE1 == 0 {
|
|
|
|
|
// NOTE: This wfe and its loop can be removed as long
|
|
|
|
|
// as the regular boot loader remains in place
|
|
|
|
|
// (i.e. this program is not written into ROM).
|
|
|
|
|
asm::wfe();
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// the following requires a stack (at least later, for the
|
|
|
|
|
// function for setting up the MMU)
|
|
|
|
|
SP.write(INITIAL_SP_CORE1);
|
|
|
|
|
boot_core1();
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
@ -55,16 +77,59 @@ pub unsafe extern "C" fn _boot_cores() -> ! {
|
|
|
|
|
#[inline(never)]
|
|
|
|
|
unsafe fn boot_core0() -> ! {
|
|
|
|
|
l1_cache_init();
|
|
|
|
|
|
|
|
|
|
// Invalidate SCU, for all cores
|
|
|
|
|
mpcore::RegisterBlock::new().scu_invalidate.write(0xffff);
|
|
|
|
|
|
|
|
|
|
zero_bss(&mut __bss_start, &mut __bss_end);
|
|
|
|
|
|
|
|
|
|
let mmu_table = mmu::L1Table::get()
|
|
|
|
|
.setup_flat_layout();
|
|
|
|
|
mmu::with_mmu(mmu_table, || {
|
|
|
|
|
// start SCU
|
|
|
|
|
mpcore::RegisterBlock::new().scu_control.modify(
|
|
|
|
|
|_, w| w.enable(true)
|
|
|
|
|
);
|
|
|
|
|
// enable SMP (for starting correct SCU operation)
|
|
|
|
|
ACTLR.modify(|_, w|
|
|
|
|
|
w.smp(true) // SMP mode
|
|
|
|
|
.fw(true) // cache and TLB maintenance broadcast on
|
|
|
|
|
);
|
|
|
|
|
asm::dmb();
|
|
|
|
|
asm::dsb();
|
|
|
|
|
main();
|
|
|
|
|
panic!("return from main");
|
|
|
|
|
});
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
#[naked]
|
|
|
|
|
#[inline(never)]
|
|
|
|
|
unsafe fn boot_core1() -> ! {
|
|
|
|
|
l1_cache_init();
|
|
|
|
|
|
|
|
|
|
// Invalidate SCU, for core1 only
|
|
|
|
|
mpcore::RegisterBlock::new().scu_invalidate.write(0x00f0);
|
|
|
|
|
|
|
|
|
|
// use the MMU L1 Table already set up by core 0
|
|
|
|
|
let mmu_table = mmu::L1Table::get();
|
|
|
|
|
mmu::with_mmu(mmu_table, || {
|
|
|
|
|
// enable SMP (for correct SCU operation)
|
|
|
|
|
ACTLR.modify(|_, w|
|
|
|
|
|
w.smp(true) // SMP mode
|
|
|
|
|
.fw(true) // cache and TLB maintenance broadcast
|
|
|
|
|
);
|
|
|
|
|
|
|
|
|
|
asm::dmb();
|
|
|
|
|
asm::dsb();
|
|
|
|
|
|
|
|
|
|
// now that the MMU is active using the same table as active
|
|
|
|
|
// on the other core, one can branch to any normal memory
|
|
|
|
|
// location in which the code may reside
|
|
|
|
|
asm!("bx r1" :: "{r1}"(START_ADDR_CORE1) :: "volatile");
|
|
|
|
|
unreachable!();
|
|
|
|
|
});
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
fn l1_cache_init() {
|
|
|
|
|
// Invalidate TLBs
|
|
|
|
|
tlbiall();
|
|
|
|
@ -73,13 +138,123 @@ fn l1_cache_init() {
|
|
|
|
|
// Invalidate Branch Predictor Array
|
|
|
|
|
bpiall();
|
|
|
|
|
// Invalidate D-Cache
|
|
|
|
|
dccisw();
|
|
|
|
|
//
|
|
|
|
|
// Note: Do use dcisw rather than dccisw to only invalidate rather
|
|
|
|
|
// than also clear (which may write values back into the
|
|
|
|
|
// underlying L2 cache or memory!)
|
|
|
|
|
//
|
|
|
|
|
// use the "made-up instruction" (see definition) dciall()
|
|
|
|
|
dciall();
|
|
|
|
|
|
|
|
|
|
asm::dsb();
|
|
|
|
|
asm::isb();
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
fn stop_core1() {
|
|
|
|
|
slcr::RegisterBlock::unlocked(|slcr| {
|
|
|
|
|
slcr.a9_cpu_rst_ctrl.modify(|_, w| {
|
|
|
|
|
w.a9_rst1(true)
|
|
|
|
|
});
|
|
|
|
|
slcr.a9_cpu_rst_ctrl.modify(|_, w| {
|
|
|
|
|
w.a9_clkstop1(true)
|
|
|
|
|
});
|
|
|
|
|
slcr.a9_cpu_rst_ctrl.modify(|_, w| {
|
|
|
|
|
w.a9_rst1(false)
|
|
|
|
|
});
|
|
|
|
|
});
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Execute f on core 1 using the given stack. Note that these
|
|
|
|
|
// semantics are inherently unsafe as the stack needs to live longer
|
|
|
|
|
// than Rust semantics dictate...hence this method is marked as unsafe
|
|
|
|
|
// to remind the caller to take special care (but also many operations
|
|
|
|
|
// performed would otherwise require `unsafe` blocks).
|
|
|
|
|
unsafe fn run_on_core1(f: fn() -> !, stack: &mut [u32]) {
|
|
|
|
|
// reset and stop core 1 (this is safe to repeat, if the caller
|
|
|
|
|
// has already performed this)
|
|
|
|
|
stop_core1();
|
|
|
|
|
|
|
|
|
|
// ensure any mailbox access finishes before the mailbox reset
|
|
|
|
|
asm::dmb();
|
|
|
|
|
// reset the mailbox for sending messages
|
|
|
|
|
MAILBOX_FROM_CORE0.reset_discard();
|
|
|
|
|
MAILBOX_FROM_CORE1.reset_discard();
|
|
|
|
|
// determine address of f and save it as start address for core 1
|
|
|
|
|
write_volatile(
|
|
|
|
|
&mut START_ADDR_CORE1,
|
|
|
|
|
f as *const () as u32
|
|
|
|
|
);
|
|
|
|
|
write_volatile(
|
|
|
|
|
&mut INITIAL_SP_CORE1,
|
|
|
|
|
&mut stack[stack.len() - 1] as *const _ as u32
|
|
|
|
|
);
|
|
|
|
|
// ensure the above is written to cache before it is cleaned
|
|
|
|
|
asm::dmb();
|
|
|
|
|
// TODO: Is the following necessary, considering that the SCU
|
|
|
|
|
// should take care of coherency of all (normal) memory?
|
|
|
|
|
//
|
|
|
|
|
// clean cache lines containing START_ADDR_CORE1 and
|
|
|
|
|
// INITIAL_SP_CORE1
|
|
|
|
|
dccmvac(&START_ADDR_CORE1 as *const _ as u32);
|
|
|
|
|
dccmvac(&INITIAL_SP_CORE1 as *const _ as u32);
|
|
|
|
|
|
|
|
|
|
// clean cache lines containing mailboxes
|
|
|
|
|
dccmvac(&MAILBOX_FROM_CORE0 as *const _ as u32);
|
|
|
|
|
dccmvac(&MAILBOX_FROM_CORE1 as *const _ as u32);
|
|
|
|
|
|
|
|
|
|
// restart core 1
|
|
|
|
|
slcr::RegisterBlock::unlocked(|slcr| {
|
|
|
|
|
slcr.a9_cpu_rst_ctrl.modify(|_, w| {
|
|
|
|
|
w.a9_rst1(false)
|
|
|
|
|
});
|
|
|
|
|
slcr.a9_cpu_rst_ctrl.modify(|_, w| {
|
|
|
|
|
w.a9_clkstop1(false)
|
|
|
|
|
});
|
|
|
|
|
});
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
fn main_core1() -> ! {
|
|
|
|
|
let mut data: [u32; 2] = [42, 42];
|
|
|
|
|
println!("Core 1 SP: 0x{:X}", SP.read());
|
|
|
|
|
loop {
|
|
|
|
|
// effectively perform something similar to `println!("from
|
|
|
|
|
// core 1");` by passing a message to core 0 and having core 0
|
|
|
|
|
// output it via the println! macro
|
|
|
|
|
unsafe {
|
|
|
|
|
println!("sending from core 1");
|
|
|
|
|
MAILBOX_FROM_CORE1.send(&data as *const _ as usize);
|
|
|
|
|
while !MAILBOX_FROM_CORE1.acknowledged() {
|
|
|
|
|
println!("core 1 waiting for acknowledgement from core 0");
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// change data to make it more interesting
|
|
|
|
|
data[1] += 1;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
fn main_core1_program2() -> ! {
|
|
|
|
|
let mut data: [u32; 2] = [4200, 4200];
|
|
|
|
|
println!("Core 1 SP: 0x{:X}", SP.read());
|
|
|
|
|
loop {
|
|
|
|
|
unsafe {
|
|
|
|
|
MAILBOX_FROM_CORE1.send(&data as *const _ as usize);
|
|
|
|
|
while !MAILBOX_FROM_CORE1.acknowledged() {}
|
|
|
|
|
}
|
|
|
|
|
// change data to make it more interesting
|
|
|
|
|
data[0] -= 1;
|
|
|
|
|
data[1] += 1;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// reserve some memory as stack for core1
|
|
|
|
|
static mut STACK_CORE1: [u32; 256] = [0; 256];
|
|
|
|
|
|
|
|
|
|
const HWADDR: [u8; 6] = [0, 0x23, 0xde, 0xea, 0xbe, 0xef];
|
|
|
|
|
|
|
|
|
|
fn main() {
|
|
|
|
|
println!("Main.");
|
|
|
|
|
println!("Core 0 SP: 0x{:X}", SP.read());
|
|
|
|
|
let clocks = clocks::CpuClocks::get();
|
|
|
|
|
println!("Clocks: {:?}", clocks);
|
|
|
|
|
println!("CPU speeds: {}/{}/{}/{} MHz",
|
|
|
|
@ -92,6 +267,52 @@ fn main() {
|
|
|
|
|
println!("Eth on");
|
|
|
|
|
eth.reset_phy();
|
|
|
|
|
|
|
|
|
|
// start executing main_core1() on core 1
|
|
|
|
|
unsafe {
|
|
|
|
|
run_on_core1(main_core1, &mut STACK_CORE1[..]);
|
|
|
|
|
}
|
|
|
|
|
println!("Started main_core1() on core 1");
|
|
|
|
|
for _ in 0..5 {
|
|
|
|
|
// wait for data
|
|
|
|
|
while unsafe { !MAILBOX_FROM_CORE1.available() } {}
|
|
|
|
|
// receive data
|
|
|
|
|
let data_ptr = unsafe { MAILBOX_FROM_CORE1.receive() };
|
|
|
|
|
println!(
|
|
|
|
|
"Received via mailbox from core 1: data {} and {} at address 0x{:X}",
|
|
|
|
|
unsafe { (*(data_ptr as *const [u32; 2]))[0] },
|
|
|
|
|
unsafe { (*(data_ptr as *const [u32; 2]))[1] },
|
|
|
|
|
data_ptr
|
|
|
|
|
);
|
|
|
|
|
unsafe {
|
|
|
|
|
MAILBOX_FROM_CORE1.acknowledge(data_ptr);
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
stop_core1();
|
|
|
|
|
println!("Stopped core 1.");
|
|
|
|
|
|
|
|
|
|
// start executing main_core1_program2() on core 1
|
|
|
|
|
unsafe {
|
|
|
|
|
run_on_core1(main_core1_program2, &mut STACK_CORE1[..]);
|
|
|
|
|
}
|
|
|
|
|
println!("Started main_core1_program2() on core 1");
|
|
|
|
|
for _ in 0..5 {
|
|
|
|
|
// wait for data
|
|
|
|
|
while unsafe { !MAILBOX_FROM_CORE1.available() } {}
|
|
|
|
|
// receive data
|
|
|
|
|
let data_ptr = unsafe { MAILBOX_FROM_CORE1.receive() };
|
|
|
|
|
println!(
|
|
|
|
|
"Received via mailbox from core 1: data {} and {} at address 0x{:X}",
|
|
|
|
|
unsafe { (*(data_ptr as *const [u32; 2]))[0] },
|
|
|
|
|
unsafe { (*(data_ptr as *const [u32; 2]))[1] },
|
|
|
|
|
data_ptr
|
|
|
|
|
);
|
|
|
|
|
unsafe {
|
|
|
|
|
MAILBOX_FROM_CORE1.acknowledge(data_ptr);
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
stop_core1();
|
|
|
|
|
println!("Stopped core 1.");
|
|
|
|
|
|
|
|
|
|
const RX_LEN: usize = 1;
|
|
|
|
|
let mut rx_descs: [eth::rx::DescEntry; RX_LEN] = unsafe { uninitialized() };
|
|
|
|
|
let mut rx_buffers = [[0u8; eth::MTU]; RX_LEN];
|
|
|
|
|