|
| 1 | +use core::{ |
| 2 | + panic::PanicInfo, |
| 3 | + sync::atomic::{AtomicBool, Ordering}, |
| 4 | +}; |
| 5 | +use r3::kernel::{cfg::CfgBuilder, Kernel, StartupHook}; |
| 6 | +use r3_support_rp2040::usbstdio; |
| 7 | + |
| 8 | +/// The separators for our multiplexing protocol |
| 9 | +pub mod mux { |
| 10 | + pub const BEGIN_MAIN: &str = "\x171"; |
| 11 | + pub const BEGIN_LOG: &str = "\x172"; |
| 12 | +} |
| 13 | + |
| 14 | +pub const SYSTICK_FREQUENCY: u64 = 48_000_000; |
| 15 | + |
| 16 | +#[panic_handler] |
| 17 | +fn panic(info: &PanicInfo) -> ! { |
| 18 | + // Disable IRQ |
| 19 | + cortex_m::interrupt::disable(); |
| 20 | + |
| 21 | + r3_support_rp2040::sprintln!("{}{}", mux::BEGIN_MAIN, info); |
| 22 | + |
| 23 | + enter_poll_loop(); |
| 24 | +} |
| 25 | + |
| 26 | +/// Start polling USB so that we can deliver the test result and reset the |
| 27 | +/// device when requested. |
| 28 | +pub fn enter_poll_loop() -> ! { |
| 29 | + loop { |
| 30 | + usbstdio::poll::<Options>(); |
| 31 | + } |
| 32 | +} |
| 33 | + |
| 34 | +/// Implements `kernel_benchmarks::Driver::performance_time`. |
| 35 | +pub fn performance_time() -> u32 { |
| 36 | + let timerawl = unsafe { (0x40054028 as *const u32).read_volatile() }; |
| 37 | + timerawl.wrapping_mul(2) // scale by `tick.cycles` |
| 38 | +} |
| 39 | + |
| 40 | +struct Logger; |
| 41 | + |
| 42 | +impl log::Log for Logger { |
| 43 | + fn enabled(&self, _: &log::Metadata) -> bool { |
| 44 | + true |
| 45 | + } |
| 46 | + |
| 47 | + fn log(&self, record: &log::Record) { |
| 48 | + r3_support_rp2040::sprintln!( |
| 49 | + "{}[{:5} {}] {}", |
| 50 | + mux::BEGIN_LOG, |
| 51 | + record.level(), |
| 52 | + record.target(), |
| 53 | + record.args() |
| 54 | + ); |
| 55 | + } |
| 56 | + |
| 57 | + fn flush(&self) {} |
| 58 | +} |
| 59 | + |
| 60 | +pub const fn configure<System: Kernel>(b: &mut CfgBuilder<System>) { |
| 61 | + StartupHook::build() |
| 62 | + .start(|_| { |
| 63 | + // Set the correct vector table address |
| 64 | + unsafe { |
| 65 | + let p = cortex_m::Peripherals::steal(); |
| 66 | + p.SCB.vtor.write(0x20000000); |
| 67 | + } |
| 68 | + |
| 69 | + // Configure peripherals |
| 70 | + let p = unsafe { rp2040::Peripherals::steal() }; |
| 71 | + r3_support_rp2040::clock::init_clock( |
| 72 | + &p.CLOCKS, |
| 73 | + &p.XOSC, |
| 74 | + &p.PLL_SYS, |
| 75 | + &p.PLL_USB, |
| 76 | + &p.RESETS, |
| 77 | + &p.WATCHDOG, |
| 78 | + ); |
| 79 | + |
| 80 | + // clk_ref → clk_sys = 48MHz |
| 81 | + p.CLOCKS.clk_sys_ctrl.modify(|_, w| w.src().clk_ref()); |
| 82 | + |
| 83 | + // Supply clk_ref / 2 = 24MHz to SysTick, watchdog, and timer |
| 84 | + // because we want to measure times at high precision in |
| 85 | + // benchmarks. Setting `cycles = 1` would be ideal but doesn't work |
| 86 | + // for some reason. |
| 87 | + p.WATCHDOG |
| 88 | + .tick |
| 89 | + .write(|b| unsafe { b.cycles().bits(2).enable().set_bit() }); |
| 90 | + |
| 91 | + // Reset the timer used by `performance_time` |
| 92 | + p.RESETS.reset.modify(|_, w| w.timer().set_bit()); |
| 93 | + p.RESETS.reset.modify(|_, w| w.timer().clear_bit()); |
| 94 | + while p.RESETS.reset_done.read().timer().bit_is_clear() {} |
| 95 | + |
| 96 | + // Reset and enable IO bank 0 |
| 97 | + p.RESETS |
| 98 | + .reset |
| 99 | + .modify(|_, w| w.pads_bank0().set_bit().io_bank0().set_bit()); |
| 100 | + p.RESETS |
| 101 | + .reset |
| 102 | + .modify(|_, w| w.pads_bank0().clear_bit().io_bank0().clear_bit()); |
| 103 | + while p.RESETS.reset_done.read().pads_bank0().bit_is_clear() {} |
| 104 | + while p.RESETS.reset_done.read().io_bank0().bit_is_clear() {} |
| 105 | + |
| 106 | + // Note: CM0 don't support CAS atomics. This is why we need to use |
| 107 | + // `set_logger_racy` here. |
| 108 | + // Safety: There are no other threads calling `set_logger_racy` at the |
| 109 | + // same time. |
| 110 | + unsafe { log::set_logger_racy(&Logger).unwrap() }; |
| 111 | + log::set_max_level(log::LevelFilter::Trace); |
| 112 | + }) |
| 113 | + .finish(b); |
| 114 | + |
| 115 | + usbstdio::configure::<_, Options>(b); |
| 116 | +} |
| 117 | + |
| 118 | +static SHOULD_PAUSE_OUTPUT: AtomicBool = AtomicBool::new(true); |
| 119 | + |
| 120 | +struct Options; |
| 121 | + |
| 122 | +impl usbstdio::Options for Options { |
| 123 | + /// Handle USB serial input data. |
| 124 | + fn handle_input(s: &[u8]) { |
| 125 | + let mut should_unpause_output = false; |
| 126 | + for &b in s.iter() { |
| 127 | + match b { |
| 128 | + b'r' => { |
| 129 | + // Restart RP2040 in BOOTSEL mode |
| 130 | + let gpio_activity_pin_mask = 1 << 25; // Use GP25 as an "activity light" |
| 131 | + let disable_interface_mask = 1; // enable only PICOBOOT (disable USB MSD) |
| 132 | + BootromHdr::global() |
| 133 | + .reset_to_usb_boot(gpio_activity_pin_mask, disable_interface_mask); |
| 134 | + } |
| 135 | + b'g' => { |
| 136 | + should_unpause_output = true; |
| 137 | + } |
| 138 | + _ => {} |
| 139 | + } |
| 140 | + } |
| 141 | + |
| 142 | + if should_unpause_output && SHOULD_PAUSE_OUTPUT.load(Ordering::Relaxed) { |
| 143 | + SHOULD_PAUSE_OUTPUT.store(false, Ordering::Relaxed); |
| 144 | + |
| 145 | + // Flush the transmission buffer. |
| 146 | + usbstdio::poll::<Options>(); |
| 147 | + } |
| 148 | + } |
| 149 | + |
| 150 | + fn product_name() -> &'static str { |
| 151 | + "R3 Test Driver Port" |
| 152 | + } |
| 153 | + |
| 154 | + fn should_pause_output() -> bool { |
| 155 | + SHOULD_PAUSE_OUTPUT.load(Ordering::Relaxed) |
| 156 | + } |
| 157 | +} |
| 158 | + |
| 159 | +#[repr(C)] |
| 160 | +struct BootromHdr { |
| 161 | + // The first field is excluded because we don't want upset LLVM by a |
| 162 | + // null pointer |
| 163 | + // _initial_boot_stack_ptr: usize, |
| 164 | + _reset_handler: unsafe extern "C" fn(), |
| 165 | + _nmi_handler: unsafe extern "C" fn(), |
| 166 | + _hard_fault_handler: unsafe extern "C" fn(), |
| 167 | + _magic: [u8; 3], |
| 168 | + _version: u8, |
| 169 | + rom_func_table: BootromHalfPtr<BootromFnTablePtr>, |
| 170 | + rom_data_table: BootromHalfPtr<usize>, |
| 171 | + rom_table_lookup: BootromHalfPtr<extern "C" fn(BootromFnTablePtr, u32) -> *const ()>, |
| 172 | +} |
| 173 | + |
| 174 | +impl BootromHdr { |
| 175 | + fn global() -> &'static Self { |
| 176 | + unsafe { &*(4 as *const BootromHdr) } |
| 177 | + } |
| 178 | + |
| 179 | + unsafe fn lookup_func<T>(&self, c1: u8, c2: u8) -> Option<T> { |
| 180 | + let value = self.rom_table_lookup.get()( |
| 181 | + self.rom_func_table.get(), |
| 182 | + u32::from_le_bytes([c1, c2, 0, 0]), |
| 183 | + ); |
| 184 | + unsafe { core::mem::transmute_copy(&value) } |
| 185 | + } |
| 186 | + |
| 187 | + fn reset_to_usb_boot(&self, gpio_activity_pin_mask: u32, disable_interface_mask: u32) -> ! { |
| 188 | + unsafe { |
| 189 | + self.lookup_func::<extern "C" fn(u32, u32) -> !>(b'U', b'B') |
| 190 | + .expect("could not locate `reset_to_usb_boot`")( |
| 191 | + gpio_activity_pin_mask, |
| 192 | + disable_interface_mask, |
| 193 | + ) |
| 194 | + } |
| 195 | + } |
| 196 | +} |
| 197 | + |
| 198 | +#[repr(transparent)] |
| 199 | +struct BootromHalfPtr<T>(u16, core::marker::PhantomData<T>); |
| 200 | + |
| 201 | +impl<T> BootromHalfPtr<T> { |
| 202 | + fn get(&self) -> T { |
| 203 | + unsafe { core::mem::transmute_copy(&(self.0 as usize)) } |
| 204 | + } |
| 205 | +} |
| 206 | + |
| 207 | +#[repr(transparent)] |
| 208 | +struct BootromFnTablePtr(usize); |
0 commit comments