From 23706e6ffae2d19a36ae48c95fa3fe59318b96c0 Mon Sep 17 00:00:00 2001 From: Hussein Hazem Date: Wed, 26 Jun 2024 14:35:35 +0200 Subject: [PATCH 1/6] #11 add support for listen only mode --- src/config.rs | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/config.rs b/src/config.rs index 37a893c..457dbf1 100644 --- a/src/config.rs +++ b/src/config.rs @@ -234,6 +234,8 @@ pub enum RequestMode { InternalLoopback, /// External loop back mode ExternalLoopback, + /// Listen only mode + ListenOnly, /// CAN 2.0 mode, possible error frames on CAN FD frames NormalCAN2_0, } @@ -250,6 +252,7 @@ impl RequestMode { RequestMode::NormalCANFD => OperationMode::NormalCANFD, RequestMode::InternalLoopback => OperationMode::InternalLoopback, RequestMode::ExternalLoopback => OperationMode::ExternalLoopback, + RequestMode::ListenOnly => OperationMode::ListenOnly, RequestMode::NormalCAN2_0 => OperationMode::NormalCAN2_0, } } From a043e498e7c508a5cd529d146536441641e5c8b4 Mon Sep 17 00:00:00 2001 From: Hussein Hazem Date: Wed, 26 Jun 2024 17:04:51 +0200 Subject: [PATCH 2/6] #11 add TX/RX using internal loopback mode in example crate --- example/Cargo.toml | 5 +++- example/src/main.rs | 70 +++++++++++++++++++++++++++++++++++++++++++-- 2 files changed, 72 insertions(+), 3 deletions(-) diff --git a/example/Cargo.toml b/example/Cargo.toml index fcb2b9e..8ec674f 100644 --- a/example/Cargo.toml +++ b/example/Cargo.toml @@ -21,6 +21,9 @@ cortex-m = "0.7.7" cortex-m-rt = "0.7.3" rp-pico = "0.9.0" rp2040-boot2 = "0.3.0" +embedded-can = "0.4.1" +bytes = { version = "1.6.0", default-features = false } +log = "0.4.21" [patch.crates-io] -bytes = { git = "https://github.com/atlas-aero/rt-bytes.git", branch = "cfg_target_has_atomic_v1.6.0" } \ No newline at end of file +bytes = { git = "https://github.com/atlas-aero/rt-bytes.git", branch = "cfg_target_has_atomic_v1.6.0" } diff --git a/example/src/main.rs b/example/src/main.rs index d98c186..84694df 100644 --- a/example/src/main.rs +++ b/example/src/main.rs @@ -7,10 +7,21 @@ pub mod mutex; use crate::clock::SystemClock; use crate::heap::Heap; +use bytes::Bytes; +use cortex_m::asm::delay; +use embedded_can::{Id, StandardId}; +use embedded_hal::delay::DelayNs; use hal::clocks::Clock; use hal::fugit::RateExtU32; use hal::pac; +use log::info; use mcp2517::can::Controller; +use mcp2517::config::{ + ClockConfiguration, ClockOutputDivisor, Configuration, FifoConfiguration, PLLSetting, PayloadSize, RequestMode, + RetransmissionAttempts, SystemClockDivisor, +}; +use mcp2517::filter::Filter; +use mcp2517::message::{Can20, TxMessage}; use panic_halt as _; use rp2040_hal as hal; @@ -56,10 +67,65 @@ fn main() -> ! { embedded_hal::spi::MODE_0, ); + let mut timer = hal::Timer::new(pac.TIMER, &mut pac.RESETS, &clocks); + let sys_clk = SystemClock::default(); + sys_clk.initialize(timer); + // Configure GPIO5 as an CS pin let pin_cs = pins.gpio5.into_push_pull_output(); - let _controller: Controller<_, _, SystemClock> = Controller::new(spi, pin_cs); + let mut can_controller: Controller<_, _, SystemClock> = Controller::new(spi, pin_cs); + + // Setup clk config + let clk_config = ClockConfiguration { + clock_output: ClockOutputDivisor::DivideBy1, + system_clock: SystemClockDivisor::DivideBy1, + pll: PLLSetting::DirectXTALOscillator, + disable_clock: false, + }; + + // Setup fifo config + let fifo_config = FifoConfiguration { + rx_size: 1, + tx_size: 1, + pl_size: PayloadSize::EightBytes, + tx_priority: 32, + tx_enable: true, + tx_attempts: RetransmissionAttempts::Unlimited, + }; + + // Setup CAN Controller config + let config = Configuration { + clock: clk_config, + fifo: fifo_config, + mode: RequestMode::InternalLoopback, + }; + + let _ = can_controller.configure(&config, &sys_clk); + + let can_id = Id::Standard(StandardId::new(0x55).unwrap()); + + // Create filter object for RX + let filter = Filter::new(can_id, 0).unwrap(); + let _ = can_controller.set_filter_object(filter); + + // Create message frame + let message_type = Can20 {}; + let payload = [1, 2, 3, 4, 5, 6, 7, 8]; + let pl_bytes = bytes::Bytes::copy_from_slice(&payload); + let can_message = TxMessage::new(message_type, pl_bytes, can_id).unwrap(); + + let mut receive_buffer = [0u8; 8]; + + loop { + can_controller.transmit(&can_message).unwrap(); + info!("Message sent"); + + match can_controller.receive(&mut receive_buffer).unwrap() { + Ok(_) => info!("message received {receive_buffer:?}"), + Err(e) => info!("Error while attempting to read message: {e:?}"), + } - loop {} + timer.delay_ms(500); + } } From 0088eebc2d34d4b3c284f2c7ba0ba4a917a5484c Mon Sep 17 00:00:00 2001 From: Hussein Hazem Date: Wed, 26 Jun 2024 17:06:40 +0200 Subject: [PATCH 3/6] #11 renove reduntant prefix --- example/src/main.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/example/src/main.rs b/example/src/main.rs index 84694df..fb5357d 100644 --- a/example/src/main.rs +++ b/example/src/main.rs @@ -112,7 +112,7 @@ fn main() -> ! { // Create message frame let message_type = Can20 {}; let payload = [1, 2, 3, 4, 5, 6, 7, 8]; - let pl_bytes = bytes::Bytes::copy_from_slice(&payload); + let pl_bytes = Bytes::copy_from_slice(&payload); let can_message = TxMessage::new(message_type, pl_bytes, can_id).unwrap(); let mut receive_buffer = [0u8; 8]; From a5361a7232626c37e6e35db9157fc5cd6640c2f2 Mon Sep 17 00:00:00 2001 From: Hussein Hazem Date: Fri, 30 Aug 2024 12:02:01 +0200 Subject: [PATCH 4/6] #11 fix bugs with impl of transmission and reception of CAN messages --- Cargo.toml | 1 + example/.cargo/config.toml | 24 +++++++++ example/Cargo.toml | 7 +++ example/memory.x | 15 ++++++ example/src/main.rs | 104 +++++++++++++++++++++++-------------- src/can.rs | 31 +++++++++-- src/config.rs | 6 ++- src/registers.rs | 2 +- src/tests/can.rs | 38 ++++++++------ src/tests/config.rs | 12 ++--- src/tests/filter.rs | 46 ++++++++++++++++ 11 files changed, 217 insertions(+), 69 deletions(-) create mode 100644 example/.cargo/config.toml create mode 100644 example/memory.x diff --git a/Cargo.toml b/Cargo.toml index 31e7a84..4eb77df 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -12,6 +12,7 @@ readme = "README.md" documentation = "https://docs.rs/mcp2517" [dependencies] +byteorder = { version = "1.5.0", default-features = false } bytes = { version = "1.6.0", default-features = false } embedded-can = "0.4.1" embedded-hal = { version = "0.2.7", features = ["unproven"] } diff --git a/example/.cargo/config.toml b/example/.cargo/config.toml new file mode 100644 index 0000000..f15cc44 --- /dev/null +++ b/example/.cargo/config.toml @@ -0,0 +1,24 @@ +[build] +# Set the default target to match the Cortex-M0+ in the RP2040 +target = "thumbv6m-none-eabi" + +# Target specific options +[target.thumbv6m-none-eabi] +# Pass some extra options to rustc, some of which get passed on to the linker. +# +# * linker argument --nmagic turns off page alignment of sections (which saves +# flash space) +# * linker argument -Tlink.x tells the linker to use link.x as the linker +# script. This is usually provided by the cortex-m-rt crate, and by default +# the version in that crate will include a file called `memory.x` which +# describes the particular memory layout for your specific chip. +# * inline-threshold=5 makes the compiler more aggressive and inlining functions +# * no-vectorize-loops turns off the loop vectorizer (seeing as the M0+ doesn't +# have SIMD) +rustflags = [ + "-C", "link-arg=--nmagic", + "-C", "link-arg=-Tlink.x", + "-C", "no-vectorize-loops", +] + +runner = "elf2uf2-rs" diff --git a/example/Cargo.toml b/example/Cargo.toml index 8ec674f..706996b 100644 --- a/example/Cargo.toml +++ b/example/Cargo.toml @@ -24,6 +24,13 @@ rp2040-boot2 = "0.3.0" embedded-can = "0.4.1" bytes = { version = "1.6.0", default-features = false } log = "0.4.21" +usb-device = "0.3.2" +usbd-serial = "0.2.2" +defmt = "0.3.8" +defmt-serial = "0.10.0" +static_cell = "2.1.0" +embedded-serial = "0.5.0" +fugit = { version = "0.3.7", features = ["defmt"] } [patch.crates-io] bytes = { git = "https://github.com/atlas-aero/rt-bytes.git", branch = "cfg_target_has_atomic_v1.6.0" } diff --git a/example/memory.x b/example/memory.x new file mode 100644 index 0000000..070eac7 --- /dev/null +++ b/example/memory.x @@ -0,0 +1,15 @@ +MEMORY { + BOOT2 : ORIGIN = 0x10000000, LENGTH = 0x100 + FLASH : ORIGIN = 0x10000100, LENGTH = 2048K - 0x100 + RAM : ORIGIN = 0x20000000, LENGTH = 256K +} + +EXTERN(BOOT2_FIRMWARE) + +SECTIONS { + /* ### Boot loader */ + .boot2 ORIGIN(BOOT2) : + { + KEEP(*(.boot2)); + } > BOOT2 +} INSERT BEFORE .text; \ No newline at end of file diff --git a/example/src/main.rs b/example/src/main.rs index fb5357d..229843f 100644 --- a/example/src/main.rs +++ b/example/src/main.rs @@ -1,5 +1,6 @@ #![no_std] #![no_main] +extern crate alloc; pub mod clock; pub mod heap; @@ -8,38 +9,45 @@ pub mod mutex; use crate::clock::SystemClock; use crate::heap::Heap; use bytes::Bytes; -use cortex_m::asm::delay; +use core::fmt::Write; use embedded_can::{Id, StandardId}; use embedded_hal::delay::DelayNs; -use hal::clocks::Clock; -use hal::fugit::RateExtU32; -use hal::pac; -use log::info; +use embedded_hal::digital::OutputPin; +use fugit::RateExtU32; use mcp2517::can::Controller; use mcp2517::config::{ - ClockConfiguration, ClockOutputDivisor, Configuration, FifoConfiguration, PLLSetting, PayloadSize, RequestMode, - RetransmissionAttempts, SystemClockDivisor, + ClockConfiguration, ClockOutputDivisor, Configuration, FifoConfiguration, PLLSetting, RequestMode, + SystemClockDivisor, }; use mcp2517::filter::Filter; use mcp2517::message::{Can20, TxMessage}; -use panic_halt as _; -use rp2040_hal as hal; - -#[link_section = ".boot2"] -#[used] -pub static BOOT2: [u8; 256] = rp2040_boot2::BOOT_LOADER_GENERIC_03H; +use panic_halt as panic; +use rp_pico as bsp; + +use bsp::{ + entry, + hal::{ + clocks::{init_clocks_and_plls, Clock}, + gpio::FunctionSpi, + pac, + sio::Sio, + uart::*, + watchdog::Watchdog, + Spi, Timer, + }, +}; const XTAL_FREQ_HZ: u32 = 12_000_000u32; -#[rp2040_hal::entry] +#[entry] fn main() -> ! { Heap::init(); let mut pac = pac::Peripherals::take().unwrap(); - let mut watchdog = hal::Watchdog::new(pac.WATCHDOG); + let mut watchdog = Watchdog::new(pac.WATCHDOG); // Configure the clocks - let clocks = hal::clocks::init_clocks_and_plls( + let clocks = init_clocks_and_plls( XTAL_FREQ_HZ, pac.XOSC, pac.CLOCKS, @@ -50,49 +58,55 @@ fn main() -> ! { ) .unwrap(); - let sio = hal::Sio::new(pac.SIO); + let sio = Sio::new(pac.SIO); - let pins = hal::gpio::Pins::new(pac.IO_BANK0, pac.PADS_BANK0, sio.gpio_bank0, &mut pac.RESETS); + let pins = bsp::Pins::new(pac.IO_BANK0, pac.PADS_BANK0, sio.gpio_bank0, &mut pac.RESETS); - let spi_mosi = pins.gpio7.into_function::(); - let spi_miso = pins.gpio4.into_function::(); - let spi_sclk = pins.gpio6.into_function::(); - let spi = hal::spi::Spi::<_, _, _, 8>::new(pac.SPI0, (spi_mosi, spi_miso, spi_sclk)); + let spi_mosi = pins.gpio11.into_function::(); + let spi_miso = pins.gpio12.into_function::(); + let spi_sclk = pins.gpio10.into_function::(); + let spi = Spi::<_, _, _, 8>::new(pac.SPI1, (spi_mosi, spi_miso, spi_sclk)); // Exchange the uninitialised SPI driver for an initialised one let spi = spi.init( &mut pac.RESETS, clocks.peripheral_clock.freq(), - 16.MHz(), + 1.MHz(), embedded_hal::spi::MODE_0, ); - let mut timer = hal::Timer::new(pac.TIMER, &mut pac.RESETS, &clocks); + let mut led_pin = pins.led.into_push_pull_output(); + + let mut timer = Timer::new(pac.TIMER, &mut pac.RESETS, &clocks); let sys_clk = SystemClock::default(); sys_clk.initialize(timer); // Configure GPIO5 as an CS pin - let pin_cs = pins.gpio5.into_push_pull_output(); + let pin_cs = pins.gpio13.into_push_pull_output(); + + let mut uart = bsp::hal::uart::UartPeripheral::new( + pac.UART0, + (pins.gpio0.into_function(), pins.gpio1.into_function()), + &mut pac.RESETS, + ) + .enable( + UartConfig::new(9600.Hz(), DataBits::Eight, None, StopBits::One), + clocks.peripheral_clock.freq(), + ) + .unwrap(); let mut can_controller: Controller<_, _, SystemClock> = Controller::new(spi, pin_cs); // Setup clk config let clk_config = ClockConfiguration { clock_output: ClockOutputDivisor::DivideBy1, - system_clock: SystemClockDivisor::DivideBy1, + system_clock: SystemClockDivisor::DivideBy2, pll: PLLSetting::DirectXTALOscillator, disable_clock: false, }; // Setup fifo config - let fifo_config = FifoConfiguration { - rx_size: 1, - tx_size: 1, - pl_size: PayloadSize::EightBytes, - tx_priority: 32, - tx_enable: true, - tx_attempts: RetransmissionAttempts::Unlimited, - }; + let fifo_config = FifoConfiguration::default(); // Setup CAN Controller config let config = Configuration { @@ -101,7 +115,9 @@ fn main() -> ! { mode: RequestMode::InternalLoopback, }; - let _ = can_controller.configure(&config, &sys_clk); + if let Err(_) = can_controller.configure(&config, &sys_clk) { + panic!() + } let can_id = Id::Standard(StandardId::new(0x55).unwrap()); @@ -119,11 +135,21 @@ fn main() -> ! { loop { can_controller.transmit(&can_message).unwrap(); - info!("Message sent"); + uart.write_raw(b"can message sent\n\r").unwrap(); + + timer.delay_ms(500); + + match can_controller.receive(&mut receive_buffer) { + Ok(_) => { + uart.write_fmt(format_args!("can message received\n\r")).unwrap(); + + for val in receive_buffer { + uart.write_fmt(format_args!("{val}\n\r")).unwrap(); + } - match can_controller.receive(&mut receive_buffer).unwrap() { - Ok(_) => info!("message received {receive_buffer:?}"), - Err(e) => info!("Error while attempting to read message: {e:?}"), + led_pin.set_high().unwrap(); + } + Err(e) => uart.write_fmt(format_args!("error reading message {:?}\n\r", e)).unwrap(), } timer.delay_ms(500); diff --git a/src/can.rs b/src/can.rs index bcd33b1..3f2f9c8 100644 --- a/src/can.rs +++ b/src/can.rs @@ -5,6 +5,7 @@ use crate::filter::Filter; use crate::message::{MessageType, TxMessage}; use crate::registers::{FifoControlReg1, FifoStatusReg0}; use crate::status::{OperationMode, OperationStatus, OscillatorStatus}; +use byteorder::{BigEndian, ByteOrder, LittleEndian}; use core::marker::PhantomData; use embedded_hal::blocking::spi::Transfer; use embedded_hal::digital::v2::OutputPin; @@ -102,7 +103,7 @@ impl, CS: OutputPin, CLK: Clock> Controller { self.write_register( Self::fifo_control_register(FIFO_RX_INDEX) + 3, - config.fifo.as_rx_register(), + config.fifo.as_rx_register_3(), )?; self.write_register( @@ -186,6 +187,7 @@ impl, CS: OutputPin, CLK: Clock> Controller { // Set FLTENm to enable filter self.write_register(filter_control_reg, (1 << 7) | fifo_index)?; + Ok(()) } @@ -193,6 +195,7 @@ impl, CS: OutputPin, CLK: Clock> Controller { pub fn disable_filter(&mut self, filter_index: u8) -> Result<(), BusError> { let filter_reg = Self::filter_control_register_byte(filter_index); self.write_register(filter_reg, 0x00)?; + Ok(()) } @@ -207,8 +210,13 @@ impl, CS: OutputPin, CLK: Clock> Controller { let mask_value = u32::from(filter.mask_bits); self.write32(filter_object_reg, filter_value)?; + self.write32(filter_mask_reg, mask_value)?; + let filter_control_reg = Self::filter_control_register_byte(filter.index); + + self.write_register(filter_control_reg, (1 << 7) | 1)?; + Ok(()) } @@ -273,7 +281,11 @@ impl, CS: OutputPin, CLK: Clock> Controller { } // get address in which to write next message in TX FIFO (should not be read in configuration mode) - let address = self.read32(Self::fifo_user_address_register(FIFO_TX_INDEX))?; + let user_address = self.read32(Self::fifo_user_address_register(FIFO_TX_INDEX))?; + + // calculate address of next Message Object according to + // Equation 4-1 in MCP251XXFD Family Reference Manual + let address = user_address + 0x400; // get address of TX FIFO control register byte 1 let fifo_control_reg1 = Self::fifo_control_register(FIFO_TX_INDEX) + 1; @@ -309,7 +321,10 @@ impl, CS: OutputPin, CLK: Clock> Controller { rxfifo_status_reg0 = FifoStatusReg0::from(rxfifo_status_byte0); } - let address = self.read32(Self::fifo_user_address_register(FIFO_RX_INDEX))?; + let user_address = self.read32(Self::fifo_user_address_register(FIFO_RX_INDEX))?; + + let address = 0x400 + user_address; + // read message object self.read_fifo(address as u16, data)?; @@ -335,12 +350,17 @@ impl, CS: OutputPin, CLK: Clock> Controller { // copy message data into mutable buffer let mut data = [0u8; L]; - data.copy_from_slice(message.buff.as_ref()); + data.copy_from_slice(&message.buff); buffer[0] = (command >> 8) as u8; buffer[1] = (command & 0xFF) as u8; buffer[2..].copy_from_slice(&message.header.into_bytes()); + for word in buffer[2..].chunks_exact_mut(4) { + let num = BigEndian::read_u32(word); + LittleEndian::write_u32(word, num); + } + self.pin_cs.set_low().map_err(CSError)?; self.bus.transfer(&mut buffer).map_err(TransferError)?; self.bus.transfer(&mut data).map_err(TransferError)?; @@ -351,8 +371,9 @@ impl, CS: OutputPin, CLK: Clock> Controller { /// Read message from RX FIFO fn read_fifo(&mut self, register: u16, data: &mut [u8]) -> Result<(), Error> { + let payload_address = register + 8; let mut buffer = [0u8; 2]; - let command = (register & 0x0FFF) | ((Operation::Read as u16) << 12); + let command = (payload_address & 0x0FFF) | ((Operation::Read as u16) << 12); buffer[0] = (command >> 8) as u8; buffer[1] = (command & 0xFF) as u8; diff --git a/src/config.rs b/src/config.rs index 457dbf1..725c634 100644 --- a/src/config.rs +++ b/src/config.rs @@ -182,13 +182,15 @@ impl Default for FifoConfiguration { } impl FifoConfiguration { - /// Encodes the configuration to RX FIFO configuration register byte - pub(crate) fn as_rx_register(&self) -> u8 { + /// Encodes the configuration for the third RX fifo control register byte + pub(crate) fn as_rx_register_3(&self) -> u8 { (Self::limit_size(self.rx_size) - 1) | ((self.pl_size as u8) << 5) } /// Encodes the configuration for the first TX configuration register byte pub(crate) fn as_tx_register_0(&self) -> u8 { + // bit 7 -> tx enable + // bit 0 -> tx fifo not full interrupt flag enable match self.tx_enable { true => 0b1000_0000, false => 0b0000_0000, diff --git a/src/registers.rs b/src/registers.rs index ef15bea..e505b88 100644 --- a/src/registers.rs +++ b/src/registers.rs @@ -137,7 +137,7 @@ pub struct FilterObjectReg { __: B1, /// Extended ID enable bit /// If MIDE 1, setting this bit matches Extended ID only - /// if MIDE 1, clearing this bit matches Standard ID only + /// If MIDE 0, clearing this bit matches Standard ID only pub exide: bool, /// Standard ID filter bit pub sid11: bool, diff --git a/src/tests/can.rs b/src/tests/can.rs index 68f29eb..b770946 100644 --- a/src/tests/can.rs +++ b/src/tests/can.rs @@ -3,10 +3,11 @@ use crate::config::{ ClockConfiguration, ClockOutputDivisor, Configuration, FifoConfiguration, PLLSetting, PayloadSize, RequestMode, RetransmissionAttempts, SystemClockDivisor, }; -use crate::message::{Can20, CanFd, RxHeader, TxMessage}; +use crate::message::{Can20, CanFd, TxMessage}; use crate::mocks::{MockPin, MockSPIBus, TestClock}; use crate::status::OperationMode; use alloc::vec; +use byteorder::{BigEndian, ByteOrder, LittleEndian}; use bytes::Bytes; use embedded_can::{ExtendedId, Id}; use mockall::Sequence; @@ -210,10 +211,16 @@ fn test_transmit_can20() { .times(1) .returning(move |data| { let mut cmd_and_header_buffer = [0u8; 10]; - cmd_and_header_buffer[0] = 0x24; + cmd_and_header_buffer[0] = 0x28; cmd_and_header_buffer[1] = 0xA2; + cmd_and_header_buffer[2..].copy_from_slice(&tx_message.header.into_bytes()); + for chunk in cmd_and_header_buffer[2..].chunks_exact_mut(4) { + let num = BigEndian::read_u32(chunk); + LittleEndian::write_u32(chunk, num); + } + assert_eq!(cmd_and_header_buffer, data); Ok(&[0u8; 10]) }) @@ -311,10 +318,15 @@ fn test_transmit_can_fd() { .times(1) .returning(move |data| { let mut cmd_and_header_buffer = [0u8; 10]; - cmd_and_header_buffer[0] = 0x24; + cmd_and_header_buffer[0] = 0x28; cmd_and_header_buffer[1] = 0xA2; cmd_and_header_buffer[2..].copy_from_slice(&tx_message.header.into_bytes()); + for chunk in cmd_and_header_buffer[2..].chunks_exact_mut(4) { + let num = BigEndian::read_u32(chunk); + LittleEndian::write_u32(chunk, num); + } + assert_eq!(cmd_and_header_buffer, data); Ok(&[0u8; 10]) }) @@ -376,14 +388,9 @@ fn test_transmit_can_fd() { fn test_receive() { let mut mocks = Mocks::default(); - let id = ExtendedId::new(EXTENDED_ID).unwrap(); - let mut seq = Sequence::new(); - // custom Rx message header for testing - let message_header = RxHeader::new_test_cfg(Id::Extended(id)); - - let mut message_buff = [0u8; 16]; + let mut message_buff = [0u8; 8]; // status register read (wait till fifo not empty flag is set) mocks.mock_register_read::<0b0000_0000>([0x30, 0x60], &mut seq); @@ -394,7 +401,7 @@ fn test_receive() { // user address register read mocks.mock_read32::<0x00_00_04_7C>([0x30, 0x64], &mut seq); - // Message read from RAM address 0x47C + // Message read from RAM address (0x47C+8) to start reading received message object payload // transfer cmd+address mocks .pin_cs @@ -407,7 +414,7 @@ fn test_receive() { .expect_transfer() .times(1) .returning(move |data| { - assert_eq!([0x34, 0x7C], data); + assert_eq!([0x38, 0x84], data); Ok(&[0u8; 2]) }) .in_sequence(&mut seq); @@ -418,9 +425,9 @@ fn test_receive() { .expect_transfer() .times(1) .returning(|data| { - assert_eq!([0u8; 16], data); - data.copy_from_slice(&[0x09, 0x51, 0x5D, 0x32, 0u8, 0u8, 0u8, 0x18, 1, 2, 3, 4, 5, 6, 7, 8]); - Ok(&[0x09, 0x51, 0x5D, 0x32, 0u8, 0u8, 0u8, 0x18, 1, 2, 3, 4, 5, 6, 7, 8]) + assert_eq!([0u8; 8], data); + data.copy_from_slice(&[1, 2, 3, 4, 5, 6, 7, 8]); + Ok(&[1, 2, 3, 4, 5, 6, 7, 8]) }) .in_sequence(&mut seq); mocks @@ -457,8 +464,7 @@ fn test_receive() { assert!(result.is_ok()); - assert_eq!(message_buff[..8], message_header.into_bytes()); - assert_eq!(message_buff[8..], [1, 2, 3, 4, 5, 6, 7, 8]); + assert_eq!(message_buff, [1, 2, 3, 4, 5, 6, 7, 8]); } #[test] diff --git a/src/tests/config.rs b/src/tests/config.rs index 7a15ccb..24d3dc0 100644 --- a/src/tests/config.rs +++ b/src/tests/config.rs @@ -94,14 +94,14 @@ fn test_clock_configuration_to_register() { #[test] fn test_fifo_configuration_as_rx_register() { - assert_eq!(0b0000_0000, fifo_rx_config(0).as_rx_register()); - assert_eq!(0b0000_0000, fifo_rx_config(1).as_rx_register()); + assert_eq!(0b0000_0000, fifo_rx_config(0).as_rx_register_3()); + assert_eq!(0b0000_0000, fifo_rx_config(1).as_rx_register_3()); - assert_eq!(0b0000_0001, fifo_rx_config(2).as_rx_register()); - assert_eq!(0b0000_1011, fifo_rx_config(12).as_rx_register()); + assert_eq!(0b0000_0001, fifo_rx_config(2).as_rx_register_3()); + assert_eq!(0b0000_1011, fifo_rx_config(12).as_rx_register_3()); - assert_eq!(0b0001_1111, fifo_rx_config(32).as_rx_register()); - assert_eq!(0b0001_1111, fifo_rx_config(33).as_rx_register()); + assert_eq!(0b0001_1111, fifo_rx_config(32).as_rx_register_3()); + assert_eq!(0b0001_1111, fifo_rx_config(33).as_rx_register_3()); } #[test] diff --git a/src/tests/filter.rs b/src/tests/filter.rs index 2b3e565..f39be39 100644 --- a/src/tests/filter.rs +++ b/src/tests/filter.rs @@ -90,6 +90,29 @@ fn test_set_filter_object_standard_id() { .return_const(Ok(())) .in_sequence(&mut seq); + // enable filter + mocks + .pin_cs + .expect_set_low() + .times(1) + .return_const(Ok(())) + .in_sequence(&mut seq); + mocks + .bus + .expect_transfer() + .times(1) + .returning(move |data| { + assert_eq!([0x21, 0xD1, 0x81], data); + Ok(&[0u8; 6]) + }) + .in_sequence(&mut seq); + mocks + .pin_cs + .expect_set_high() + .times(1) + .return_const(Ok(())) + .in_sequence(&mut seq); + let result = mocks.into_controller().set_filter_object(filter); assert!(result.is_ok()); @@ -176,6 +199,29 @@ fn test_set_filter_object_extended_id() { .return_const(Ok(())) .in_sequence(&mut seq); + // enable filter + mocks + .pin_cs + .expect_set_low() + .times(1) + .return_const(Ok(())) + .in_sequence(&mut seq); + mocks + .bus + .expect_transfer() + .times(1) + .returning(move |data| { + assert_eq!([0x21, 0xD0, 0x81], data); + Ok(&[0u8; 6]) + }) + .in_sequence(&mut seq); + mocks + .pin_cs + .expect_set_high() + .times(1) + .return_const(Ok(())) + .in_sequence(&mut seq); + let result_extended = mocks.into_controller().set_filter_object(filter); assert!(result_extended.is_ok()); From 2d18baed10e47bceb925848259b88f10bbd1e47d Mon Sep 17 00:00:00 2001 From: Hussein Hazem Date: Mon, 2 Sep 2024 10:21:34 +0200 Subject: [PATCH 5/6] #11 remove debugging code using led --- example/src/main.rs | 5 ----- 1 file changed, 5 deletions(-) diff --git a/example/src/main.rs b/example/src/main.rs index 229843f..53eacb1 100644 --- a/example/src/main.rs +++ b/example/src/main.rs @@ -12,7 +12,6 @@ use bytes::Bytes; use core::fmt::Write; use embedded_can::{Id, StandardId}; use embedded_hal::delay::DelayNs; -use embedded_hal::digital::OutputPin; use fugit::RateExtU32; use mcp2517::can::Controller; use mcp2517::config::{ @@ -75,8 +74,6 @@ fn main() -> ! { embedded_hal::spi::MODE_0, ); - let mut led_pin = pins.led.into_push_pull_output(); - let mut timer = Timer::new(pac.TIMER, &mut pac.RESETS, &clocks); let sys_clk = SystemClock::default(); sys_clk.initialize(timer); @@ -146,8 +143,6 @@ fn main() -> ! { for val in receive_buffer { uart.write_fmt(format_args!("{val}\n\r")).unwrap(); } - - led_pin.set_high().unwrap(); } Err(e) => uart.write_fmt(format_args!("error reading message {:?}\n\r", e)).unwrap(), } From a878b3cc3a3bd391d867f71407a1da1594383d81 Mon Sep 17 00:00:00 2001 From: Hussein Hazem Date: Mon, 2 Sep 2024 10:29:04 +0200 Subject: [PATCH 6/6] #11 adjust/add comments for example code --- example/src/main.rs | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/example/src/main.rs b/example/src/main.rs index 53eacb1..bba4c25 100644 --- a/example/src/main.rs +++ b/example/src/main.rs @@ -78,9 +78,10 @@ fn main() -> ! { let sys_clk = SystemClock::default(); sys_clk.initialize(timer); - // Configure GPIO5 as an CS pin + // Configure GPIO13 as an CS pin let pin_cs = pins.gpio13.into_push_pull_output(); + // Enable uart to print to terminal let mut uart = bsp::hal::uart::UartPeripheral::new( pac.UART0, (pins.gpio0.into_function(), pins.gpio1.into_function()),