From 31469c232fb054d722b4a03796e69249e87c157b Mon Sep 17 00:00:00 2001 From: Kasil Date: Thu, 25 Aug 2022 19:02:56 +0200 Subject: [PATCH] Add support for the Interpolator (#371) * Implementation of the interpolator. * corrected formatting * fixed documentation code * add clamp flag to LaneCtrl * addition of an example for the interpolator * put documentation behind /// * rewording comment for clarity * using more idiomatic fn new --- boards/rp-pico/examples/pico_interpolator.rs | 441 +++++++++++++++++++ rp2040-hal/src/sio.rs | 251 ++++++++++- 2 files changed, 689 insertions(+), 3 deletions(-) create mode 100644 boards/rp-pico/examples/pico_interpolator.rs diff --git a/boards/rp-pico/examples/pico_interpolator.rs b/boards/rp-pico/examples/pico_interpolator.rs new file mode 100644 index 0000000..dc96f25 --- /dev/null +++ b/boards/rp-pico/examples/pico_interpolator.rs @@ -0,0 +1,441 @@ +//! # Pico Interpolator Example +//! +//! Example demonstrating the usage of the hardware interpolator. +//! +//! Runs several test programs, outputs the result on LEDs. +//! Green led for successful test connects to GPIO3. +//! Red led for unsuccessful test connects to GPIO4. +//! In case of failure, the system LED blinks the number of the test. +//! In case of success, the system LED stays lit. +//! +//! See the `Cargo.toml` file for Copyright and license details. + +#![no_std] +#![no_main] + +// The macro for our start-up function +use rp_pico::entry; + +// Time handling traits +use embedded_time::rate::*; + +// GPIO traits +use embedded_hal::digital::v2::OutputPin; + +// Ensure we halt the program on panic (if we don't mention this crate it won't +// be linked) +use panic_halt as _; + +// A shorter alias for the Peripheral Access Crate, which provides low-level +// register access +use rp_pico::hal::pac; + +// A shorter alias for the Hardware Abstraction Layer, which provides +// higher-level drivers. +use rp_pico::hal; + +// Pull in any important traits +use rp_pico::hal::prelude::*; + +use rp_pico::hal::sio::{Interp, Interp0, Interp1, Lane, LaneCtrl}; + +/// Entry point to our bare-metal application. +/// +/// The `#[entry]` macro ensures the Cortex-M start-up code calls this function +/// as soon as all global variables are initialised. +/// +/// The function configures the RP2040 peripherals, then just reads the button +/// and sets the LED appropriately. +#[entry] +fn main() -> ! { + // Grab our singleton objects + let mut pac = pac::Peripherals::take().unwrap(); + let core = pac::CorePeripherals::take().unwrap(); + + // Set up the watchdog driver - needed by the clock setup code + let mut watchdog = hal::Watchdog::new(pac.WATCHDOG); + + // Configure the clocks + // + // The default is to generate a 125 MHz system clock + let clocks = hal::clocks::init_clocks_and_plls( + rp_pico::XOSC_CRYSTAL_FREQ, + pac.XOSC, + pac.CLOCKS, + pac.PLL_SYS, + pac.PLL_USB, + &mut pac.RESETS, + &mut watchdog, + ) + .ok() + .unwrap(); + + // The delay object lets us wait for specified amounts of time (in + // milliseconds) + let mut delay = cortex_m::delay::Delay::new(core.SYST, clocks.system_clock.freq().integer()); + + // The single-cycle I/O block controls our GPIO pins + let mut sio = hal::Sio::new(pac.SIO); + + // Set the pins up according to their function on this particular board + let pins = rp_pico::Pins::new( + pac.IO_BANK0, + pac.PADS_BANK0, + sio.gpio_bank0, + &mut pac.RESETS, + ); + + // Our LED outputs + let mut system_led_pin = pins.led.into_push_pull_output(); + let mut green_led_pin = pins.gpio3.into_push_pull_output(); + let mut red_led_pin = pins.gpio4.into_push_pull_output(); + + system_led_pin.set_low().unwrap(); + green_led_pin.set_low().unwrap(); + red_led_pin.set_low().unwrap(); + + let mut choose_led = |index: u32, result: bool| { + if result { + // blink the green led once to indicate success + green_led_pin.set_high().unwrap(); + delay.delay_ms(500); + green_led_pin.set_low().unwrap(); + delay.delay_ms(500); + } else { + // turn the red led on to indicate failure + // and blink the on board led to indicate which test failed, looping forever + red_led_pin.set_high().unwrap(); + loop { + for _ in 0..index { + system_led_pin.set_high().unwrap(); + delay.delay_ms(200); + system_led_pin.set_low().unwrap(); + delay.delay_ms(200); + } + delay.delay_ms(1000); + } + } + }; + + // Run forever, setting the LED according to the button + + choose_led(1, multiplication_table(&mut sio.interp0)); + choose_led(2, moving_mask(&mut sio.interp0)); + choose_led(3, cross_lanes(&mut sio.interp0)); + choose_led(4, simple_blend1(&mut sio.interp0)); + choose_led(5, simple_blend2(&mut sio.interp0)); + choose_led(6, clamp(&mut sio.interp1)); + choose_led(7, texture_mapping(&mut sio.interp0)); + + // turn the on board led on to indicate testing is done + system_led_pin.set_high().unwrap(); + loop { + delay.delay_ms(1000); + } +} + +fn multiplication_table(interp: &mut Interp0) -> bool { + //get the default configuration that just keep adding base into accum + let config = LaneCtrl::new(); + + //write the configuration to the hardware. + interp.get_lane0().set_ctrl(config.encode()); + + //set the accumulator to 0 and the base to 9 + interp.get_lane0().set_accum(0); + interp.get_lane0().set_base(9); + + //the expected output for comparison + let expected = [9, 18, 27, 36, 45, 54, 63, 72, 81, 90]; + + for i in expected { + //returns the value of accum + base and sets accum to the same value + let value = interp.get_lane0().pop(); + + if value != i { + return false; //inform that the interpolator did not return the expected value + } + } + true +} + +fn moving_mask(interp: &mut Interp0) -> bool { + //get the default configuration that just keep adding base into accum + let mut config = LaneCtrl::new(); + + interp.get_lane0().set_accum(0x1234ABCD); + + let expected = [ + 0x0000_000D, + 0x0000_00C0, + 0x0000_0B00, + 0x0000_A000, + 0x0004_0000, + 0x0030_0000, + 0x0200_0000, + 0x1000_0000, + ]; + for i in 0..8 { + // LSB, then MSB. These are inclusive, so 0,31 means "the entire 32 bit register" + config.mask_lsb = i * 4; + config.mask_msb = i * 4 + 3; + interp.get_lane0().set_ctrl(config.encode()); + + // Reading read_raw() returns the lane data + // after shifting, masking and sign extending, without adding base + if interp.get_lane0().read_raw() != expected[i as usize] { + return false; + } + } + + let signed_expected = [ + 0xFFFF_FFFD, + 0xFFFF_FFC0, + 0xFFFF_FB00, + 0xFFFF_A000, + 0x0004_0000, + 0x0030_0000, + 0x0200_0000, + 0x1000_0000, + ]; + + config.signed = true; + for i in 0..8 { + config.mask_lsb = i * 4; + config.mask_msb = i * 4 + 3; + interp.get_lane0().set_ctrl(config.encode()); + + if interp.get_lane0().read_raw() != signed_expected[i as usize] { + return false; + } + } + true +} + +fn cross_lanes(interp: &mut Interp0) -> bool { + // this configuration will at the time of pop() + // when applied to lane0 : set lane0 accumulator to the result from lane1 + // when applied to lane1 : set lane1 accumulator to the result from lane0 + let config = LaneCtrl { + cross_result: true, + ..LaneCtrl::new() + }; + let encoded_config = config.encode(); + + // each lane is used through an accessor, + // as lanes mutate each other, they can not be borrowed at the same time + interp.get_lane0().set_ctrl(encoded_config); + interp.get_lane1().set_ctrl(encoded_config); + + interp.get_lane0().set_accum(123); + interp.get_lane1().set_accum(456); + + // lane0 will add 1 to its result, lane1 will add nothing + interp.get_lane0().set_base(1); + interp.get_lane1().set_base(0); + + let expected = [ + (124, 456), + (457, 124), + (125, 457), + (458, 125), + (126, 458), + (459, 126), + (127, 459), + (460, 127), + (128, 460), + (461, 128), + ]; + + for i in expected { + if i != (interp.get_lane0().peek(), interp.get_lane1().pop()) { + return false; + } + } + true +} + +fn simple_blend1(interp: &mut Interp0) -> bool { + let config = LaneCtrl { + blend: true, + ..LaneCtrl::new() + }; + + //enable blend mode + interp.get_lane0().set_ctrl(config.encode()); + //make sure the default configuration is in lane1 as the value may be shifted and masked. + interp.get_lane1().set_ctrl(LaneCtrl::new().encode()); + + //set the minimum value for interp.get_lane0().set_accum(0) 0/256 + interp.get_lane0().set_base(500); + //set the maximum value which is inaccessible + // as the blend is done between 0/256 and 255/256 + interp.get_lane1().set_base(1000); + + let expected = [500, 582, 666, 748, 832, 914, 998]; + for i in 0..=6 { + interp.get_lane1().set_accum(255 * i / 6); + if expected[i as usize] != interp.get_lane1().peek() { + return false; + } + } + true +} + +fn simple_blend2(interp: &mut Interp0) -> bool { + let config = LaneCtrl { + blend: true, + ..LaneCtrl::new() + }; + //enable blend mode + interp.get_lane0().set_ctrl(config.encode()); + + interp.get_lane0().set_base((-1000i32) as u32); + interp.get_lane1().set_base(1000); + + let mut config1 = LaneCtrl { + signed: true, + ..LaneCtrl::new() + }; + interp.get_lane1().set_ctrl(config1.encode()); + let expected_signed = [-1000, -672, -336, -8, 328, 656, 992]; + for i in 0..=6 { + // write a value between 0 and 256 (exclusive) + interp.get_lane1().set_accum(255 * i / 6); + // reads it as a value between -1000 and 1000 (exclusive) + if interp.get_lane1().peek() as i32 != expected_signed[i as usize] { + return false; + } + } + config1.signed = false; + interp.get_lane1().set_ctrl(config1.encode()); + let expected_unsigned = [ + 0xfffffc18, 0xd5fffd60, 0xaafffeb0, 0x80fffff8, 0x56000148, 0x2c000290, 0x010003e0, + ]; + for i in 0..=6 { + interp.get_lane1().set_accum(255 * i / 6); + // reads a value between 4294966296 and 1000 + if interp.get_lane1().peek() != expected_unsigned[i as usize] { + return false; + } + } + true +} + +///Divides by 4 and clamp the value between 0 and 255 inclusive +fn clamp(interp: &mut Interp1) -> bool { + // Enables Clamp ONLY AVAILABLE ON Interp1 + // shift two bits to the right and mask the two most significant bits + // because sign extension is made after the mask + let config = LaneCtrl { + clamp: true, + shift: 2, + mask_lsb: 0, + mask_msb: 29, + signed: true, + ..LaneCtrl::new() + }; + interp.get_lane0().set_ctrl(config.encode()); + //set minimum value of result + interp.get_lane0().set_base(0); + //set maximum value of result + interp.get_lane1().set_base(255); + let values: [(i32, i32); 9] = [ + (-1024, 0), + (-768, 0), + (-512, 0), + (-256, 0), + (0, 0), + (256, 64), + (512, 128), + (768, 192), + (1024, 255), + ]; + for (arg, result) in values { + interp.get_lane0().set_accum(arg as u32); + if result != interp.get_lane0().peek() as i32 { + return false; + } + } + true +} + +fn texture_mapping(interp: &mut Interp0) -> bool { + #[rustfmt::skip] + let texture: [u8;16] = [ + 0x00, 0x01, 0x02, 0x03, + 0x10, 0x11, 0x12, 0x13, + 0x20, 0x21, 0x22, 0x23, + 0x30, 0x31, 0x32, 0x33, + ]; + + // the position will be given in fixed point with 16 bits + // fractional part + let uv_fractional_bits = 16; + let texture_width_bits = 2; + let texture_height_bits = 2; + + // bits + // 3322222222221111 1111110000000000 + // 1098765432109876 5432109876543210 + // accum0 u axis coordinate xx xxxxxxxxxxxxxxxx 18 bits + // after shift and mask xx + // accum1 v axis xx xxxxxxxxxxxxxxxx 18 bits + // after shift and mask xx + + // add_raw make the interpolator increment the accumulator + // with the base value without masking or shifting + let config0 = LaneCtrl { + add_raw: true, + shift: uv_fractional_bits, + mask_lsb: 0, + mask_msb: texture_width_bits - 1, + ..LaneCtrl::new() + }; + interp.get_lane0().set_ctrl(config0.encode()); + let config1 = LaneCtrl { + add_raw: true, + shift: uv_fractional_bits - texture_width_bits, + mask_lsb: texture_width_bits, + mask_msb: texture_width_bits + texture_height_bits - 1, + ..LaneCtrl::new() + }; + interp.get_lane1().set_ctrl(config1.encode()); + + interp.set_base(0); + + // set starting position to 0x0 + // will move 1/2 a pixel horizontally + // and 1/3 a pixel vertically per call to pop() + interp.get_lane0().set_accum(0); + interp.get_lane0().set_base(65536 / 2); + interp.get_lane1().set_accum(0); + interp.get_lane1().set_base(65536 / 3); + + let expected = [ + 0x00, 0x00, 0x01, 0x01, 0x12, 0x12, 0x13, 0x23, 0x20, 0x20, 0x31, 0x31, + ]; + + for i in expected { + if i != texture[interp.pop() as usize] { + return false; + } + } + + // reset the starting position + interp.get_lane0().set_accum(0); + interp.get_lane1().set_accum(0); + interp.set_base(texture.as_ptr() as u32); + + for i in expected { + // This is unsafe and should be done extremely carefully + // remember to follow memory alignment, + // reading or writing an unaligned address will crash + if i != unsafe { *(interp.pop() as *const u8) } { + return false; + } + } + + true +} +// End of file diff --git a/rp2040-hal/src/sio.rs b/rp2040-hal/src/sio.rs index b841e6d..e675c42 100644 --- a/rp2040-hal/src/sio.rs +++ b/rp2040-hal/src/sio.rs @@ -60,9 +60,10 @@ pub struct Sio { pub hwdivider: HwDivider, /// Inter-core FIFO pub fifo: SioFifo, - // we can hand out other things here, for example: - // interp0 - // interp1 + /// Interpolator 0 + pub interp0: Interp0, + /// Interpolator 1 + pub interp1: Interp1, } impl Sio { @@ -74,6 +75,14 @@ impl Sio { gpio_qspi: SioGpioQspi { _private: () }, fifo: SioFifo { _private: () }, hwdivider: HwDivider { _private: () }, + interp0: Interp0 { + lane0: Interp0Lane0 { _private: () }, + lane1: Interp0Lane1 { _private: () }, + }, + interp1: Interp1 { + lane0: Interp1Lane0 { _private: () }, + lane1: Interp1Lane1 { _private: () }, + }, } } @@ -667,3 +676,239 @@ pub unsafe fn spinlock_reset() { SPINLOCK0_PTR.wrapping_add(i).write_volatile(1); } } + +/// Configuration struct for one lane of the interpolator +pub struct LaneCtrl { + /// Bit 22 - Only present on INTERP1 on each core. If CLAMP mode is enabled: + /// - LANE0 result is shifted and masked ACCUM0, clamped by a lower bound of + /// BASE0 and an upper bound of BASE1. + /// - Signedness of these comparisons is determined by LANE0_CTRL_SIGNED + pub clamp: bool, + /// Bit 21 - Only present on INTERP0 on each core. If BLEND mode is enabled: + /// - LANE1 result is a linear interpolation between BASE0 and BASE1, controlled + /// by the 8 LSBs of lane 1 shift and mask value (a fractional number between + /// 0 and 255/256ths) + /// - LANE0 result does not have BASE0 added (yields only + /// the 8 LSBs of lane 1 shift+mask value) + /// - FULL result does not have lane 1 shift+mask value added (BASE2 + lane 0 shift+mask) + /// LANE1 SIGNED flag controls whether the interpolation is signed or unsigned. + pub blend: bool, + /// Bits 19:20 - ORed into bits 29:28 of the lane result presented to the processor on the bus. + /// No effect on the internal 32-bit datapath. Handy for using a lane to generate sequence + /// of pointers into flash or SRAM. + pub force_msb: u8, + /// Bit 18 - If 1, mask + shift is bypassed for LANE0 result. This does not affect FULL result. + pub add_raw: bool, + /// Bit 17 - If 1, feed the opposite lane's result into this lane's accumulator on POP. + pub cross_result: bool, + /// Bit 16 - If 1, feed the opposite lane's accumulator into this lane's shift + mask hardware. + /// Takes effect even if ADD_RAW is set (the CROSS_INPUT mux is before the shift+mask bypass) + pub cross_input: bool, + /// Bit 15 - If SIGNED is set, the shifted and masked accumulator value is sign-extended to 32 bits + /// before adding to BASE0, and LANE0 PEEK/POP appear extended to 32 bits when read by processor. + pub signed: bool, + /// Bits 10:14 - The most-significant bit allowed to pass by the mask (inclusive) + /// Setting MSB < LSB may cause chip to turn inside-out + pub mask_msb: u8, + /// Bits 5:9 - The least-significant bit allowed to pass by the mask (inclusive) + pub mask_lsb: u8, + /// Bits 0:4 - Logical right-shift applied to accumulator before masking + pub shift: u8, +} + +impl LaneCtrl { + /// Default configuration. Normal operation, unsigned, mask keeps all bits, no shift. + pub const fn new() -> Self { + Self { + clamp: false, + blend: false, + force_msb: 0, + add_raw: false, + cross_result: false, + cross_input: false, + signed: false, + mask_msb: 31, + mask_lsb: 0, + shift: 0, + } + } + + /// encode the configuration to be loaded in the ctrl register of one lane of an interpolator + pub const fn encode(&self) -> u32 { + assert!(!(self.blend && self.clamp)); + assert!(self.force_msb < 0b100); + assert!(self.mask_msb < 0b100000); + assert!(self.mask_lsb < 0b100000); + assert!(self.mask_msb >= self.mask_lsb); + assert!(self.shift < 0b100000); + ((self.clamp as u32) << 22) + | ((self.blend as u32) << 21) + | ((self.force_msb as u32) << 19) + | ((self.add_raw as u32) << 18) + | ((self.cross_result as u32) << 17) + | ((self.cross_input as u32) << 16) + | ((self.signed as u32) << 15) + | ((self.mask_msb as u32) << 10) + | ((self.mask_lsb as u32) << 5) + | (self.shift as u32) + } +} + +///Trait representing the functionnality of a single lane of an interpolator. +pub trait Lane { + ///Read the lane result, and simultaneously write lane results to both accumulators. + fn pop(&mut self) -> u32; + ///Read the lane result without altering any internal state + fn peek(&self) -> u32; + ///Write a value to the accumulator + fn set_accum(&mut self, v: u32); + ///Read the value from the accumulator + fn get_accum(&self) -> u32; + ///Write a value to the base register + fn set_base(&mut self, v: u32); + ///Read the value from the base register + fn get_base(&self) -> u32; + ///Write to the control register + fn set_ctrl(&mut self, v: u32); + ///Read from the control register + fn get_ctrl(&self) -> u32; + ///Add the value to the accumulator register + fn add_accum(&mut self, v: u32); + ///Read the raw shift and mask value (BASE register not added) + fn read_raw(&self) -> u32; +} + +///Trait representing the functionnality of an interpolator. +/// ```no_run +/// use rp2040_hal::sio::{Sio,LaneCtrl,Lane}; +/// use rp2040_hal::pac; +/// let mut peripherals = pac::Peripherals::take().unwrap(); +/// let mut sio = Sio::new(peripherals.SIO); +/// +/// // by having the configuration const, the validity is checked during compilation. +/// const config: u32 = LaneCtrl { +/// mask_msb: 4, // Most significant bit of the mask is bit 4 +/// // By default the least significant bit is bit 0 +/// // this will keep only the 5 least significant bits. +/// // this is equivalent to %32 +/// ..LaneCtrl::new() +/// }.encode(); +/// sio.interp0.get_lane0().set_ctrl(config); +/// sio.interp0.get_lane0().set_accum(0); +/// sio.interp0.get_lane0().set_base(1); // will increment the value by 1 on each call to pop +/// +/// sio.interp0.get_lane0().peek(); // returns 1 +/// sio.interp0.get_lane0().pop(); // returns 1 +/// sio.interp0.get_lane0().pop(); // returns 2 +/// sio.interp0.get_lane0().pop(); // returns 3 +/// ``` +pub trait Interp { + ///Read the interpolator result (Result 2 in the datasheet), and simultaneously write lane results to both accumulators. + fn pop(&mut self) -> u32; + ///Read the interpolator result (Result 2 in the datasheet) without altering any internal state + fn peek(&self) -> u32; + ///Write to the interpolator Base register (Base2 in the datasheet) + fn set_base(&mut self, v: u32); + ///Read the interpolator Base register (Base2 in the datasheet) + fn get_base(&self) -> u32; +} + +macro_rules! interpolators { + ( + $($interp:ident : ( $( [ $lane:ident,$lane_id:expr ] ),+ ) ),+ + ) => { + $crate::paste::paste! { + + + $( + $( + #[doc = "The lane " $lane_id " of " $interp] + pub struct [<$interp $lane>]{ + _private: (), + } + impl Lane for [<$interp $lane>]{ + fn pop(&mut self) ->u32{ + let sio = unsafe { &*pac::SIO::ptr() }; + sio.[<$interp:lower _pop_ $lane:lower>].read().bits() + } + fn peek(&self) ->u32{ + let sio = unsafe { &*pac::SIO::ptr() }; + sio.[<$interp:lower _peek_ $lane:lower>].read().bits() + } + fn set_accum(&mut self,v:u32){ + let sio = unsafe { &*pac::SIO::ptr() }; + sio.[<$interp:lower _accum $lane_id>].write(|w| unsafe { w.bits(v) }); + } + fn get_accum(&self)->u32{ + let sio = unsafe { &*pac::SIO::ptr() }; + sio.[<$interp:lower _accum $lane_id>].read().bits() + } + fn set_base(&mut self, v:u32){ + let sio = unsafe { &*pac::SIO::ptr() }; + sio.[<$interp:lower _base $lane_id>].write(|w| unsafe { w.bits(v) }); + } + fn get_base(&self)->u32{ + let sio = unsafe { &*pac::SIO::ptr() }; + sio.[<$interp:lower _base $lane_id>].read().bits() + } + fn set_ctrl(&mut self, v:u32){ + let sio = unsafe { &*pac::SIO::ptr() }; + sio.[<$interp:lower _ctrl_lane $lane_id>].write(|w| unsafe { w.bits(v) }); + } + fn get_ctrl(&self)->u32{ + let sio = unsafe { &*pac::SIO::ptr() }; + sio.[<$interp:lower _ctrl_lane $lane_id>].read().bits() + } + fn add_accum(&mut self, v:u32){ + let sio = unsafe { &*pac::SIO::ptr() }; + sio.[<$interp:lower _accum $lane_id _add>].write(|w| unsafe { w.bits(v) }); + } + fn read_raw(&self)->u32{ + let sio = unsafe { &*pac::SIO::ptr() }; + sio.[<$interp:lower _accum $lane_id _add>].read().bits() + } + } + )+ + #[doc = "Interpolator " $interp] + pub struct $interp { + $( + [<$lane:lower>]: [<$interp $lane>], + )+ + } + impl $interp{ + $( + /// Lane accessor function + pub fn [](&mut self)->&mut [<$interp $lane>]{ + &mut self.[<$lane:lower>] + } + )+ + } + impl Interp for $interp{ + fn pop(&mut self) ->u32{ + let sio = unsafe { &*pac::SIO::ptr() }; + sio.[<$interp:lower _pop_full>].read().bits() + } + fn peek(&self) ->u32{ + let sio = unsafe { &*pac::SIO::ptr() }; + sio.[<$interp:lower _peek_full>].read().bits() + } + fn set_base(&mut self, v:u32){ + let sio = unsafe { &*pac::SIO::ptr() }; + sio.[<$interp:lower _base2>].write(|w| unsafe { w.bits(v)}); + } + fn get_base(&self)->u32{ + let sio = unsafe { &*pac::SIO::ptr() }; + sio.[<$interp:lower _base2>].read().bits() + } + + } + + )+ + } + } + } + +interpolators!( + Interp0 : ([Lane0,0],[Lane1,1]), + Interp1 : ([Lane0,0],[Lane1,1]) +);