sawyer/picocalc-os-rs
1
1
Fork 0
mirror of https://github.com/LegitCamper/picocalc-os-rs.git synced 2025-12-27 07:45:28 +00:00
Code Issues Packages Projects Releases Wiki Activity

add pimoroni2w support

Browse Source
This commit is contained in:
sawyer bristol
2025-10-26 20:21:34 -06:00
parent 31a3772bcb
commit 5414725241
13 changed files with 533 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
kernel/Cargo.toml
View File

@@ -12,6 +12,7 @@ bench = false
[features]
default = ["rp235x", "defmt"]
pimoroni2w = ["rp235x"]
rp2040 = ["embassy-rp/rp2040"]
rp235x = ["embassy-rp/rp235xb"]
trouble = ["dep:bt-hci", "dep:cyw43", "dep:cyw43-pio", "dep:trouble-host"]
@@ -89,6 +90,7 @@ spin = "0.10.0"
num_enum = { version = "0.7.4", default-features = false }
goblin = { version = "0.10.1", default-features = false, features = ["elf32"] }
talc = "4.4.3"
embedded-alloc = "0.6.0"
bumpalo = "3.19.0"
abi_sys = { path = "../abi_sys" }

7
kernel/build.rs
View File

@@ -13,13 +13,18 @@ use std::fs::File;
use std::io::Write;
use std::path::PathBuf;
#[cfg(all(feature = "rp235x", not(feature = "pimoroni2w")))]
const MEMORY: &'static [u8] = include_bytes!("rp2350.x");
#[cfg(feature = "pimoroni2w")]
const MEMORY: &'static [u8] = include_bytes!("rp2350.x");
fn main() {
// Put `memory.x` in our output directory and ensure it's
// on the linker search path.
let out = &PathBuf::from(env::var_os("OUT_DIR").unwrap());
File::create(out.join("memory.x"))
.unwrap()
.write_all(include_bytes!("memory.x"))
.write_all(MEMORY)
.unwrap();
println!("cargo:rustc-link-search={}", out.display());

60
kernel/pimoroni2w.x Normal file
View File

@@ -0,0 +1,60 @@
MEMORY {
FLASH : ORIGIN = 0x10000000, LENGTH = 16M - 4K
RAM : ORIGIN = 0x20000000, LENGTH = 512K
SRAM4 : ORIGIN = 0x20080000, LENGTH = 4K
SRAM5 : ORIGIN = 0x20081000, LENGTH = 4K
}
SECTIONS {
/* ### Boot ROM info
*
* Goes after .vector_table, to keep it in the first 4K of flash
* where the Boot ROM (and picotool) can find it
*/
.start_block : ALIGN(4)
{
__start_block_addr = .;
KEEP(*(.start_block));
KEEP(*(.boot_info));
} > FLASH
} INSERT AFTER .vector_table;
/* move .text to start /after/ the boot info */
_stext = ADDR(.start_block) + SIZEOF(.start_block);
SECTIONS {
/* ### Picotool 'Binary Info' Entries
*
* Picotool looks through this block (as we have pointers to it in our
* header) to find interesting information.
*/
.bi_entries : ALIGN(4)
{
/* We put this in the header */
__bi_entries_start = .;
/* Here are the entries */
KEEP(*(.bi_entries));
/* Keep this block a nice round size */
. = ALIGN(4);
/* We put this in the header */
__bi_entries_end = .;
} > FLASH
} INSERT AFTER .text;
SECTIONS {
/* ### Boot ROM extra info
*
* Goes after everything in our program, so it can contain a signature.
*/
.end_block : ALIGN(4)
{
__end_block_addr = .;
KEEP(*(.end_block));
} > FLASH
} INSERT AFTER .uninit;
PROVIDE(start_to_end = __end_block_addr - __start_block_addr);
PROVIDE(end_to_start = __start_block_addr - __end_block_addr);

0
kernel/memory.x → kernel/rp2350.x
View File

6
kernel/src/abi.rs
View File

@@ -20,9 +20,11 @@ pub extern "C" fn print(ptr: *const u8, len: usize) {
// SAFETY: caller guarantees `ptr` is valid for `len` bytes
let slice = unsafe { core::slice::from_raw_parts(ptr, len) };
if let Ok(msg) = core::str::from_utf8(slice) {
defmt::info!("print: {}", msg);
if let Ok(_msg) = core::str::from_utf8(slice) {
#[cfg(feature = "defmt")]
defmt::info!("print: {}", _msg);
} else {
#[cfg(feature = "defmt")]
defmt::warn!("print: <invalid utf8>");
}
}

132
kernel/src/heap.rs Normal file
View File

@@ -0,0 +1,132 @@
// This whole file was taken from:
// https://github.com/wezterm/picocalc-wezterm/blob/main/src/heap.rs
use core::alloc::{GlobalAlloc, Layout};
use core::mem::MaybeUninit;
use core::sync::atomic::{AtomicUsize, Ordering};
use embedded_alloc::LlffHeap as Heap;
#[global_allocator]
pub static HEAP: DualHeap = DualHeap::empty();
const HEAP_SIZE: usize = 64 * 1024;
static mut HEAP_MEM: [MaybeUninit<u8>; HEAP_SIZE] = [MaybeUninit::uninit(); HEAP_SIZE];
struct Region {
start: AtomicUsize,
size: AtomicUsize,
}
impl Region {
const fn default() -> Self {
Self {
start: AtomicUsize::new(0),
size: AtomicUsize::new(0),
}
}
fn contains(&self, address: usize) -> bool {
let start = self.start.load(Ordering::Relaxed);
let end = self.start.load(Ordering::Relaxed);
(start..start + end).contains(&address)
}
fn new(start: usize, size: usize) -> Self {
Self {
start: AtomicUsize::new(start),
size: AtomicUsize::new(size),
}
}
}
/// This is an allocator that combines two regions of memory.
/// The intent is to use some of the directly connected RAM
/// for this, and if we find some XIP capable PSRAM, add that
/// as a secondary region.
/// Allocation from the primary region is always preferred,
/// as it is expected to be a bit faster than PSRAM.
/// FIXME: PSRAM-allocated memory isn't compatible with
/// CAS atomics, so we might need a bit of a think about this!
pub struct DualHeap {
primary: Heap,
primary_region: Region,
secondary: Heap,
}
impl DualHeap {
pub const fn empty() -> Self {
Self {
primary: Heap::empty(),
primary_region: Region::default(),
secondary: Heap::empty(),
}
}
unsafe fn add_primary(&self, region: Region) {
let start = region.start.load(Ordering::SeqCst);
let size = region.size.load(Ordering::SeqCst);
unsafe {
self.primary.init(start, size);
}
self.primary_region.start.store(start, Ordering::SeqCst);
self.primary_region.size.store(size, Ordering::SeqCst);
}
unsafe fn add_secondary(&self, region: Region) {
let start = region.start.load(Ordering::SeqCst);
let size = region.size.load(Ordering::SeqCst);
unsafe {
self.secondary.init(start, size);
}
}
pub fn used(&self) -> usize {
self.primary.used() + self.secondary.used()
}
pub fn free(&self) -> usize {
self.primary.free() + self.secondary.free()
}
}
unsafe impl GlobalAlloc for DualHeap {
unsafe fn alloc(&self, layout: Layout) -> *mut u8 {
unsafe {
let ptr = self.primary.alloc(layout);
if !ptr.is_null() {
return ptr;
}
// start using secondary area when primary heap is full
self.secondary.alloc(layout)
}
}
unsafe fn dealloc(&self, ptr: *mut u8, layout: Layout) {
unsafe {
let ptr_usize = ptr as usize;
if self.primary_region.contains(ptr_usize) {
self.primary.dealloc(ptr, layout);
} else {
self.secondary.dealloc(ptr, layout);
}
}
}
}
pub fn init_heap() {
let primary_start = &raw mut HEAP_MEM as usize;
unsafe { HEAP.add_primary(Region::new(primary_start, HEAP_SIZE)) }
}
pub fn init_qmi_psram_heap(size: u32) {
unsafe { HEAP.add_secondary(Region::new(0x11000000, size as usize)) }
}
pub async fn free_command(_args: &[&str]) {
let ram_used = HEAP.primary.used();
let ram_free = HEAP.primary.free();
let ram_total = ram_used + ram_free;
let qmi_used = HEAP.secondary.used();
let qmi_free = HEAP.secondary.free();
let qmi_total = qmi_used + qmi_free;
}

25
kernel/src/main.rs
View File

@@ -10,6 +10,8 @@ mod abi;
mod display;
mod elf;
mod framebuffer;
#[cfg(feature = "pimoroni2w")]
mod heap;
mod peripherals;
mod psram;
mod scsi;
@@ -18,6 +20,9 @@ mod ui;
mod usb;
mod utils;
#[cfg(feature = "pimoroni2w")]
use crate::{heap::init_qmi_psram_heap, psram::init_psram_qmi};
use crate::{
abi::{KEY_CACHE, MS_SINCE_LAUNCH},
display::{FRAMEBUFFER, display_handler, init_display},
@@ -40,7 +45,6 @@ use embedded_graphics::{
use {defmt_rtt as _, panic_probe as _};
use core::sync::atomic::{AtomicBool, Ordering};
use defmt::unwrap;
use embassy_executor::{Executor, Spawner};
use embassy_futures::{join::join, select::select};
use embassy_rp::{
@@ -76,8 +80,10 @@ static mut CORE1_STACK: Stack<16384> = Stack::new();
static EXECUTOR0: StaticCell<Executor> = StaticCell::new();
static EXECUTOR1: StaticCell<Executor> = StaticCell::new();
#[cfg(not(feature = "pimoroni2w"))]
static mut ARENA: [u8; 200 * 1024] = [0; 200 * 1024];
#[cfg(not(feature = "pimoroni2w"))]
#[global_allocator]
static ALLOCATOR: Talck<spin::Mutex<()>, ClaimOnOom> =
Talc::new(unsafe { ClaimOnOom::new(Span::from_array(core::ptr::addr_of!(ARENA).cast_mut())) })
@@ -95,7 +101,7 @@ async fn main(_spawner: Spawner) {
unsafe { &mut *core::ptr::addr_of_mut!(CORE1_STACK) },
move || {
let executor1 = EXECUTOR1.init(Executor::new());
executor1.run(|spawner| unwrap!(spawner.spawn(userland_task())));
executor1.run(|spawner| spawner.spawn(userland_task()).unwrap());
},
);
@@ -134,7 +140,9 @@ async fn main(_spawner: Spawner) {
};
let executor0 = EXECUTOR0.init(Executor::new());
executor0.run(|spawner| {
unwrap!(spawner.spawn(kernel_task(spawner, display, sd, psram, mcu, p.USB)))
spawner
.spawn(kernel_task(spawner, display, sd, psram, mcu, p.USB))
.unwrap()
});
}
@@ -159,6 +167,7 @@ async fn userland_task() {
}
unsafe { MS_SINCE_LAUNCH = Some(Instant::now()) };
#[cfg(feature = "defmt")]
defmt::info!("Executing Binary");
entry();
@@ -238,11 +247,21 @@ async fn setup_psram(psram: Psram) {
)
.await;
#[cfg(feature = "defmt")]
defmt::info!("psram size: {}", psram.size);
if psram.size == 0 {
#[cfg(feature = "defmt")]
defmt::info!("\u{1b}[1mExternal PSRAM was NOT found!\u{1b}[0m");
}
#[cfg(feature = "pimoroni2w")]
{
let psram_qmi_size = init_psram_qmi(&embassy_rp::pac::QMI, &embassy_rp::pac::XIP_CTRL);
if psram_qmi_size > 0 {
init_qmi_psram_heap(psram_qmi_size);
}
}
}
async fn setup_sd(sd: Sd) {

242
kernel/src/psram.rs
View File

@@ -4,6 +4,7 @@
use crate::Irqs;
use embassy_futures::yield_now;
use embassy_rp::Peri;
use embassy_rp::clocks::clk_peri_freq;
use embassy_rp::gpio::{Drive, SlewRate};
use embassy_rp::peripherals::{DMA_CH3, DMA_CH4, PIN_2, PIN_3, PIN_20, PIN_21, PIO0};
use embassy_rp::pio::program::pio_asm;
@@ -20,6 +21,10 @@ use embassy_time::{Duration, Instant, Timer};
// RAM_IO2 - PIN_4 SIO2 (QPI Mode)
// RAM_IO3 - PIN_5 SIO3 (QPI Mode)
#[allow(unused)]
const PSRAM_CMD_QUAD_END: u8 = 0xf5;
#[allow(unused)]
const PSRAM_CMD_QUAD_ENABLE: u8 = 0x35;
#[allow(unused)]
const PSRAM_CMD_READ_ID: u8 = 0x9F;
const PSRAM_CMD_RSTEN: u8 = 0x66;
@@ -27,11 +32,15 @@ const PSRAM_CMD_RST: u8 = 0x99;
const PSRAM_CMD_WRITE: u8 = 0x02;
const PSRAM_CMD_FAST_READ: u8 = 0x0B;
#[allow(unused)]
const PSRAM_CMD_QUAD_READ: u8 = 0xEB;
#[allow(unused)]
const PSRAM_CMD_QUAD_WRITE: u8 = 0x38;
#[allow(unused)]
const PSRAM_CMD_NOOP: u8 = 0xFF;
#[allow(unused)]
const PSRAM_KNOWN_GOOD_DIE_PASS: u8 = 0x5d;
const SPEED: u32 = 133_000_000;
const MAX_PSRAM_FREQ: u32 = 133_000_000;
pub struct PsRam {
sm: embassy_rp::pio::StateMachine<'static, PIO0, 0>,
@@ -179,7 +188,7 @@ pub async fn init_psram(
) -> PsRam {
let mut pio = Pio::new(pio, Irqs);
let divider = calculate_pio_clock_divider(SPEED);
let divider = calculate_pio_clock_divider(MAX_PSRAM_FREQ);
// This pio program was taken from
// <https://github.com/polpo/rp2040-psram/blob/7786c93ec8d02dbb4f94a2e99645b25fb4abc2db/psram_spi.pio>
@@ -257,6 +266,7 @@ done:
psram.send_command(&[8, 0, PSRAM_CMD_RST], &mut []).await;
Timer::after(Duration::from_micros(100)).await;
#[cfg(feature = "defmt")]
defmt::info!("Verifying 1 byte write and read...");
for i in 0..10u8 {
psram.write8(i as u32, i).await;
@@ -265,18 +275,22 @@ done:
let n = psram.read8(i as u32).await;
if n as u32 != i {}
}
#[cfg(feature = "defmt")]
defmt::info!("testing read again @ 0");
let mut got = [0u8; 8];
psram.read(0, &mut got).await;
const EXPECT: &[u8] = &[0, 1, 2, 3, 4, 5, 6, 7];
if got != EXPECT {
#[cfg(feature = "defmt")]
defmt::warn!("Got Read error");
}
const DEADBEEF: &[u8] = &[0xd, 0xe, 0xa, 0xd, 0xb, 0xe, 0xe, 0xf];
#[cfg(feature = "defmt")]
defmt::info!("testing write of deadbeef at 0");
psram.write(0, DEADBEEF).await;
#[cfg(feature = "defmt")]
defmt::info!("testing read of deadbeef from 0");
psram.read(0, &mut got).await;
if got != DEADBEEF {
@@ -284,6 +298,7 @@ done:
let bad = got[addr];
if bad != DEADBEEF[addr] {
let x = psram.read8(addr as u32).await;
#[cfg(feature = "defmt")]
defmt::info!("read addr: {}, got: {:X}", addr, x);
}
}
@@ -299,9 +314,11 @@ done:
if got != TEST_STRING {}
#[cfg(feature = "defmt")]
defmt::info!("PSRAM test complete");
let id = psram.read_id().await;
#[cfg(feature = "defmt")]
defmt::info!("psram id: {}", id);
// id: [d, 5d, 53, 15, 49, e3, 7c, 7b]
// id[0] -- manufacturer id
@@ -355,6 +372,7 @@ pub async fn test_psram(psram: &mut PsRam) -> bool {
}
let writes_took = start.elapsed();
#[cfg(feature = "defmt")]
defmt::info!("Starting reads...");
Timer::after(Duration::from_millis(200)).await;
@@ -383,3 +401,223 @@ pub async fn test_psram(psram: &mut PsRam) -> bool {
bad_count == 0
}
// The origin of the code in this file is:
// <https://github.com/Altaflux/rp2350-psram-test/blob/ae50a819fef96486f6d962a609984cde4b4dd4cc/src/psram.rs#L1>
// which is MIT/Apache-2 licensed.
#[unsafe(link_section = ".data")]
#[inline(never)]
pub fn detect_psram_qmi(qmi: &embassy_rp::pac::qmi::Qmi) -> u32 {
const GPIO_FUNC_XIP_CS1: u8 = 9;
const XIP_CS_PIN: usize = 47;
embassy_rp::pac::PADS_BANK0.gpio(XIP_CS_PIN).modify(|w| {
w.set_iso(true);
});
embassy_rp::pac::PADS_BANK0.gpio(XIP_CS_PIN).modify(|w| {
w.set_ie(true);
w.set_od(false);
});
embassy_rp::pac::IO_BANK0
.gpio(XIP_CS_PIN)
.ctrl()
.write(|w| w.set_funcsel(GPIO_FUNC_XIP_CS1));
embassy_rp::pac::PADS_BANK0.gpio(XIP_CS_PIN).modify(|w| {
w.set_iso(false);
});
critical_section::with(|_cs| {
// Try and read the PSRAM ID via direct_csr.
qmi.direct_csr().write(|w| {
w.set_clkdiv(30);
w.set_en(true);
});
// Need to poll for the cooldown on the last XIP transfer to expire
// (via direct-mode BUSY flag) before it is safe to perform the first
// direct-mode operation
while qmi.direct_csr().read().busy() {
// rp235x_hal::arch::nop();
}
// Exit out of QMI in case we've inited already
qmi.direct_csr().modify(|w| w.set_assert_cs1n(true));
// Transmit the command to exit QPI quad mode - read ID as standard SPI
// Transmit as quad.
qmi.direct_tx().write(|w| {
w.set_oe(true);
w.set_iwidth(embassy_rp::pac::qmi::vals::Iwidth::Q);
w.set_data(PSRAM_CMD_QUAD_END.into());
});
while qmi.direct_csr().read().busy() {
// rp235x_hal::arch::nop();
}
let _ = qmi.direct_rx().read();
qmi.direct_csr().modify(|w| {
w.set_assert_cs1n(false);
});
// Read the id
qmi.direct_csr().modify(|w| {
w.set_assert_cs1n(true);
});
// kgd is "known good die"
let mut kgd: u16 = 0;
let mut eid: u16 = 0;
for i in 0usize..7 {
qmi.direct_tx().write(|w| {
w.set_data(if i == 0 {
PSRAM_CMD_READ_ID.into()
} else {
PSRAM_CMD_NOOP.into()
})
});
while !qmi.direct_csr().read().txempty() {
// rp235x_hal::arch::nop();
}
while qmi.direct_csr().read().busy() {
// rp235x_hal::arch::nop();
}
let value = qmi.direct_rx().read().direct_rx();
match i {
5 => {
kgd = value;
}
6 => {
eid = value;
}
_ => {}
}
}
qmi.direct_csr().modify(|w| {
w.set_assert_cs1n(false);
w.set_en(false);
});
let mut param_size: u32 = 0;
if kgd == PSRAM_KNOWN_GOOD_DIE_PASS as u16 {
param_size = 1024 * 1024;
let size_id = eid >> 5;
if eid == 0x26 || size_id == 2 {
param_size *= 8;
} else if size_id == 0 {
param_size *= 2;
} else if size_id == 1 {
param_size *= 4;
}
}
param_size
})
}
#[unsafe(link_section = ".data")]
#[inline(never)]
pub fn init_psram_qmi(
qmi: &embassy_rp::pac::qmi::Qmi,
xip: &embassy_rp::pac::xip_ctrl::XipCtrl,
) -> u32 {
let psram_size = detect_psram_qmi(qmi);
if psram_size == 0 {
return 0;
}
// Set PSRAM timing for APS6404
//
// Using an rxdelay equal to the divisor isn't enough when running the APS6404 close to 133MHz.
// So: don't allow running at divisor 1 above 100MHz (because delay of 2 would be too late),
// and add an extra 1 to the rxdelay if the divided clock is > 100MHz (i.e. sys clock > 200MHz).
const MAX_PSRAM_FREQ: u32 = 133_000_000;
let clock_hz = clk_peri_freq();
let mut divisor: u32 = (clock_hz + MAX_PSRAM_FREQ - 1) / MAX_PSRAM_FREQ;
if divisor == 1 && clock_hz > 100_000_000 {
divisor = 2;
}
let mut rxdelay: u32 = divisor;
if clock_hz / divisor > 100_000_000 {
rxdelay += 1;
}
// - Max select must be <= 8us. The value is given in multiples of 64 system clocks.
// - Min deselect must be >= 18ns. The value is given in system clock cycles - ceil(divisor / 2).
let clock_period_fs: u64 = 1_000_000_000_000_000_u64 / u64::from(clock_hz);
let max_select: u8 = ((125 * 1_000_000) / clock_period_fs) as u8;
let min_deselect: u32 = ((18 * 1_000_000 + (clock_period_fs - 1)) / clock_period_fs
- u64::from(divisor + 1) / 2) as u32;
#[cfg(feature = "defmt")]
defmt::info!(
"clock_period_fs={} max_select={} min_deselect={}",
clock_period_fs,
max_select,
min_deselect
);
qmi.direct_csr().write(|w| {
w.set_clkdiv(10);
w.set_en(true);
w.set_auto_cs1n(true);
});
while qmi.direct_csr().read().busy() {
// rp235x_hal::arch::nop();
}
qmi.direct_tx().write(|w| {
w.set_nopush(true);
w.0 = 0x35;
});
while qmi.direct_csr().read().busy() {
// rp235x_hal::arch::nop();
}
qmi.mem(1).timing().write(|w| {
w.set_cooldown(1);
w.set_pagebreak(embassy_rp::pac::qmi::vals::Pagebreak::_1024);
w.set_max_select(max_select as u8);
w.set_min_deselect(min_deselect as u8);
w.set_rxdelay(rxdelay as u8);
w.set_clkdiv(divisor as u8);
});
// // Set PSRAM commands and formats
qmi.mem(1).rfmt().write(|w| {
w.set_prefix_width(embassy_rp::pac::qmi::vals::PrefixWidth::Q);
w.set_addr_width(embassy_rp::pac::qmi::vals::AddrWidth::Q);
w.set_suffix_width(embassy_rp::pac::qmi::vals::SuffixWidth::Q);
w.set_dummy_width(embassy_rp::pac::qmi::vals::DummyWidth::Q);
w.set_data_width(embassy_rp::pac::qmi::vals::DataWidth::Q);
w.set_prefix_len(embassy_rp::pac::qmi::vals::PrefixLen::_8);
w.set_dummy_len(embassy_rp::pac::qmi::vals::DummyLen::_24);
});
qmi.mem(1).rcmd().write(|w| w.0 = 0xEB);
qmi.mem(1).wfmt().write(|w| {
w.set_prefix_width(embassy_rp::pac::qmi::vals::PrefixWidth::Q);
w.set_addr_width(embassy_rp::pac::qmi::vals::AddrWidth::Q);
w.set_suffix_width(embassy_rp::pac::qmi::vals::SuffixWidth::Q);
w.set_dummy_width(embassy_rp::pac::qmi::vals::DummyWidth::Q);
w.set_data_width(embassy_rp::pac::qmi::vals::DataWidth::Q);
w.set_prefix_len(embassy_rp::pac::qmi::vals::PrefixLen::_8);
});
qmi.mem(1).wcmd().write(|w| w.0 = 0x38);
// Disable direct mode
qmi.direct_csr().write(|w| w.0 = 0);
// Enable writes to PSRAM
xip.ctrl().modify(|w| w.set_writable_m1(true));
psram_size
}

3
kernel/src/scsi/mod.rs
View File

@@ -54,6 +54,7 @@ impl<'d, 's, D: Driver<'d>> MassStorageClass<'d, D> {
select(self.handle_cbw(), MSC_SHUTDOWN.wait()).await;
if MSC_SHUTDOWN.signaled() {
#[cfg(feature = "defmt")]
defmt::info!("MSC shutting down");
if self.temp_sd.is_some() {
@@ -80,6 +81,7 @@ impl<'d, 's, D: Driver<'d>> MassStorageClass<'d, D> {
if let Some(sd) = guard.take() {
self.temp_sd = Some(sd);
} else {
#[cfg(feature = "defmt")]
defmt::warn!("Tried to take SDCARD but it was already taken");
return;
}
@@ -363,6 +365,7 @@ impl<'d, 's, D: Driver<'d>> MassStorageClass<'d, D> {
}
pub async fn send_csw_fail(&mut self, tag: u32) {
#[cfg(feature = "defmt")]
defmt::error!("Command Failed: {}", tag);
self.send_csw(tag, 0x01, 0).await; // 0x01 = Command Failed
}