415 lines
12 KiB
Rust
415 lines
12 KiB
Rust
use hal::Syscon;
|
|
use hal::peripherals::syscon::ClockControl;
|
|
|
|
use crate::{hal, pac};
|
|
use core::cell::UnsafeCell;
|
|
use core::convert::Infallible;
|
|
use core::ptr::copy_nonoverlapping;
|
|
use core::sync::atomic::{AtomicUsize, Ordering, compiler_fence};
|
|
|
|
pub const DMA0_FLEXCOMM7_TX: u8 = 19;
|
|
|
|
#[repr(C)]
|
|
#[derive(Copy, Clone)]
|
|
pub struct DmaDescriptor {
|
|
pub xfercfg: u32,
|
|
pub src_end: *const u8,
|
|
pub dst_end: *mut u32,
|
|
pub next: *const DmaDescriptor,
|
|
}
|
|
|
|
impl defmt::Format for DmaDescriptor {
|
|
fn format(&self, fmt: defmt::Formatter) {
|
|
defmt::write!(
|
|
fmt,
|
|
"xfercfg={:x} src_end={:x} dst_end={:x} next={:x}",
|
|
self.xfercfg,
|
|
self.src_end,
|
|
self.dst_end,
|
|
self.next
|
|
)
|
|
}
|
|
}
|
|
|
|
// Channel descriptor table; linked from SRAMBASE
|
|
#[repr(C, align(512))]
|
|
pub struct DescriptorTable {
|
|
pub d: [DmaDescriptor; 32],
|
|
}
|
|
// Our ring that we will transition to once the transfer begins
|
|
#[repr(C)]
|
|
pub struct RingDescriptors<const N: usize> {
|
|
pub d: [DmaDescriptor; N],
|
|
}
|
|
|
|
#[derive(Debug, Copy, Clone, Eq, PartialEq)]
|
|
pub struct PushResult {
|
|
pub written: usize,
|
|
pub dropped: usize,
|
|
}
|
|
|
|
#[derive(Debug, Copy, Clone, Eq, PartialEq)]
|
|
pub enum ConfigError {
|
|
SlotTooLarge,
|
|
SlotTooSmall,
|
|
SlotNotAligned,
|
|
UnsupportedWidth,
|
|
}
|
|
|
|
#[derive(Debug)]
|
|
pub enum DmaError {
|
|
Underrun,
|
|
}
|
|
impl core::error::Error for DmaError {}
|
|
impl core::fmt::Display for DmaError {
|
|
fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
|
|
f.write_str("DmaUnderrun")
|
|
}
|
|
}
|
|
|
|
/// Slot-based DMA ring
|
|
pub struct DmaRing<const N: usize, const MAX_SLOT_BYTES: usize> {
|
|
dma: pac::DMA0,
|
|
dst_reg: *mut u32,
|
|
|
|
pub(crate) channel_desc: UnsafeCell<DescriptorTable>,
|
|
pub(crate) desc: UnsafeCell<RingDescriptors<N>>,
|
|
slots: UnsafeCell<[[u8; MAX_SLOT_BYTES]; N]>,
|
|
|
|
/// Effective bytes per slot (atomic for interior mutability)
|
|
slot_bytes: AtomicUsize,
|
|
word_bytes: usize,
|
|
|
|
write_slot: UnsafeCell<usize>,
|
|
write_off: UnsafeCell<usize>,
|
|
|
|
produced: AtomicUsize,
|
|
consumed: AtomicUsize,
|
|
|
|
safety_gap: usize,
|
|
pub produced_bytes: AtomicUsize,
|
|
pub consumed_bytes: AtomicUsize,
|
|
}
|
|
|
|
impl<const N: usize, const MAX_SLOT_BYTES: usize> DmaRing<N, MAX_SLOT_BYTES> {
|
|
/// Construct using PAC DMA0 + &mut SYSCON + a destination FIFO register.
|
|
pub fn new(
|
|
dma: pac::DMA0,
|
|
syscon: &mut Syscon,
|
|
dst_reg: *mut u32,
|
|
word_bytes: usize,
|
|
) -> Result<Self, ConfigError> {
|
|
if word_bytes != 1 && word_bytes != 2 && word_bytes != 4 {
|
|
return Err(ConfigError::UnsupportedWidth);
|
|
}
|
|
// Start the DMA0 clock
|
|
dma.enable_clock(syscon);
|
|
|
|
Ok(Self {
|
|
dma,
|
|
dst_reg: dst_reg,
|
|
channel_desc: UnsafeCell::new(DescriptorTable {
|
|
d: [DmaDescriptor {
|
|
xfercfg: 0,
|
|
src_end: core::ptr::null(),
|
|
dst_end: core::ptr::null_mut(),
|
|
next: core::ptr::null(),
|
|
}; 32],
|
|
}),
|
|
desc: UnsafeCell::new(RingDescriptors {
|
|
d: [DmaDescriptor {
|
|
xfercfg: 0,
|
|
src_end: core::ptr::null(),
|
|
dst_end: core::ptr::null_mut(),
|
|
next: core::ptr::null(),
|
|
}; N],
|
|
}),
|
|
slots: UnsafeCell::new([[0u8; MAX_SLOT_BYTES]; N]),
|
|
slot_bytes: AtomicUsize::new(MAX_SLOT_BYTES),
|
|
word_bytes,
|
|
write_slot: UnsafeCell::new(0),
|
|
write_off: UnsafeCell::new(0),
|
|
produced: AtomicUsize::new(0),
|
|
consumed: AtomicUsize::new(0),
|
|
safety_gap: 1,
|
|
produced_bytes: AtomicUsize::new(0),
|
|
consumed_bytes: AtomicUsize::new(0),
|
|
})
|
|
}
|
|
|
|
/// Optional: adjust safety gap (defaults to 1 empty slot).
|
|
pub fn set_safety_gap(&mut self, gap_slots: usize) {
|
|
self.safety_gap = gap_slots.min(N);
|
|
}
|
|
pub fn slot_size(&self) -> usize {
|
|
self.slot_bytes.load(Ordering::Acquire)
|
|
}
|
|
|
|
pub fn set_slot_size(&self, slot_bytes: usize) -> Result<(), ConfigError> {
|
|
if slot_bytes == 0 {
|
|
return Err(ConfigError::SlotTooSmall);
|
|
}
|
|
if slot_bytes > MAX_SLOT_BYTES {
|
|
return Err(ConfigError::SlotTooLarge);
|
|
}
|
|
if slot_bytes % self.word_bytes != 0 {
|
|
return Err(ConfigError::SlotNotAligned);
|
|
}
|
|
|
|
// Update atomic size
|
|
self.slot_bytes.store(slot_bytes, Ordering::Release);
|
|
|
|
// Re-initialize descriptors and reset producer state safely through internal mutability
|
|
self.init_descriptors();
|
|
|
|
Ok(())
|
|
}
|
|
|
|
/// Producer: copy into ring; commits whole slots; reports overflow by returning dropped bytes.
|
|
pub fn push(&self, mut data: &[u8]) -> PushResult {
|
|
let mut written = 0usize;
|
|
|
|
let write_slot = unsafe { &mut *self.write_slot.get() };
|
|
let write_off = unsafe { &mut *self.write_off.get() };
|
|
|
|
let slots = unsafe { &mut *self.slots.get() };
|
|
defmt::debug!(
|
|
"produced={} consumed={} fill={}",
|
|
self.produced(),
|
|
self.consumed(),
|
|
self.fill_slots()
|
|
);
|
|
while !data.is_empty() {
|
|
if self.is_full_for_producer() {
|
|
break;
|
|
}
|
|
|
|
let slot_bytes = self.slot_bytes.load(Ordering::Acquire);
|
|
let cap = slot_bytes - *write_off;
|
|
let n = core::cmp::min(cap, data.len());
|
|
|
|
unsafe {
|
|
let dst = slots[*write_slot].as_mut_ptr().add(*write_off);
|
|
copy_nonoverlapping(data.as_ptr(), dst, n);
|
|
}
|
|
|
|
*write_off += n;
|
|
written += n;
|
|
data = &data[n..];
|
|
|
|
if *write_off == slot_bytes {
|
|
// publish completed slot
|
|
compiler_fence(Ordering::Release);
|
|
self.produced.fetch_add(1, Ordering::Release);
|
|
|
|
*write_slot = (*write_slot + 1) % N;
|
|
*write_off = 0;
|
|
}
|
|
}
|
|
|
|
self.produced_bytes.fetch_add(written, Ordering::Release);
|
|
|
|
PushResult {
|
|
written,
|
|
dropped: data.len(),
|
|
}
|
|
}
|
|
|
|
/// Call from DMA IRQ bookkeeping when a slot has been consumed.
|
|
pub fn advance_consumed(&self, slots: usize) -> Result<(), DmaError> {
|
|
let produced = self.produced.load(Ordering::Acquire);
|
|
let consumed = self.consumed.load(Ordering::Relaxed);
|
|
if consumed < produced {
|
|
self.consumed.fetch_add(slots, Ordering::Release);
|
|
let slot_bytes = self.slot_bytes.load(Ordering::Acquire);
|
|
self.consumed_bytes
|
|
.fetch_add(slots * slot_bytes, Ordering::Relaxed);
|
|
Ok(())
|
|
} else {
|
|
defmt::error!("DMA underrun!");
|
|
Err(DmaError::Underrun)
|
|
}
|
|
}
|
|
|
|
pub fn produced(&self) -> usize {
|
|
self.produced.load(Ordering::Acquire)
|
|
}
|
|
pub fn produced_bytes(&self) -> usize {
|
|
self.produced_bytes.load(Ordering::Acquire)
|
|
}
|
|
pub fn consumed(&self) -> usize {
|
|
self.consumed.load(Ordering::Acquire)
|
|
}
|
|
pub fn consumed_bytes(&self) -> usize {
|
|
loop {
|
|
let consumed_start = self.consumed.load(Ordering::Acquire);
|
|
|
|
let reg_1 = (self.dma.channel19.xfercfg.read().bits() >> 16) & 0x3FF;
|
|
let reg_2 = (self.dma.channel19.xfercfg.read().bits() >> 16) & 0x3FF;
|
|
|
|
let consumed_end = self.consumed.load(Ordering::Acquire);
|
|
|
|
if consumed_start == consumed_end && reg_1 == reg_2 {
|
|
let remaining_bytes = if reg_1 == 0x3FF {
|
|
0
|
|
} else {
|
|
(reg_1 as usize + 1) * self.word_bytes
|
|
};
|
|
|
|
// Active slot consumed calculation accounts for dynamic slot size
|
|
let slot_bytes = self.slot_bytes.load(Ordering::Acquire);
|
|
let active_slot_consumed = slot_bytes.saturating_sub(remaining_bytes);
|
|
|
|
return consumed_start * slot_bytes + active_slot_consumed;
|
|
}
|
|
}
|
|
}
|
|
|
|
pub fn fill_slots(&self) -> usize {
|
|
self.produced().wrapping_sub(self.consumed())
|
|
}
|
|
|
|
pub fn init(&self) {
|
|
self.init_descriptors();
|
|
|
|
// Descriptor table base
|
|
let desc = unsafe { &*self.desc.get() };
|
|
let base = self.channel_desc.get() as u32;
|
|
self.dma.srambase.write(|w| unsafe { w.bits(base) });
|
|
self.dma
|
|
.channel19
|
|
.cfg
|
|
.write(|w| w.periphreqen().enabled().hwtrigen().disabled());
|
|
self.dma
|
|
.channel19
|
|
.xfercfg
|
|
.write(|w| unsafe { w.bits(desc.d[0].xfercfg) });
|
|
|
|
self.dma.enableclr0.write(|w| unsafe { w.bits(1 << 19) });
|
|
self.dma.ctrl.write(|w| w.enable().enabled());
|
|
self.dma.setvalid0.write(|w| unsafe { w.bits(1 << 19) });
|
|
self.dma.intenset0.write(|w| unsafe { w.bits(1 << 19) });
|
|
|
|
self.dma.settrig0.write(|w| unsafe { w.bits(1 << 19) });
|
|
}
|
|
|
|
pub fn run(&self) {
|
|
self.dma.enableset0.write(|w| unsafe { w.bits(1 << 19) });
|
|
}
|
|
|
|
pub fn stop(&self) {
|
|
self.dma.enableclr0.write(|w| unsafe { w.bits(1 << 19) });
|
|
nb::block!(if (self.dma.busy0.read().bits() & 1 << 19) == 0 {
|
|
Ok(())
|
|
} else {
|
|
Err(nb::Error::<Infallible>::WouldBlock)
|
|
});
|
|
self.dma.abort0.write(|w| unsafe { w.bits(1 << 19) });
|
|
self.reset_producer();
|
|
}
|
|
|
|
fn reset_producer(&self) {
|
|
unsafe {
|
|
*self.write_slot.get() = 0;
|
|
*self.write_off.get() = 0;
|
|
}
|
|
self.produced.store(0, Ordering::Relaxed);
|
|
self.produced_bytes.store(0, Ordering::Relaxed);
|
|
self.consumed.store(0, Ordering::Relaxed);
|
|
self.consumed_bytes.store(0, Ordering::Relaxed);
|
|
}
|
|
|
|
fn is_full_for_producer(&self) -> bool {
|
|
let fill = self.fill_slots();
|
|
fill >= N.wrapping_sub(self.safety_gap)
|
|
}
|
|
|
|
fn init_descriptors(&self) {
|
|
let slot_bytes = self.slot_bytes.load(Ordering::Acquire);
|
|
let slots = unsafe { &mut *self.slots.get() };
|
|
let desc = unsafe { &mut *self.desc.get() };
|
|
let chan_desc = unsafe { &mut *self.channel_desc.get() };
|
|
|
|
// Pre-fill active slot regions with silence
|
|
for i in 0..N {
|
|
slots[i][..slot_bytes].fill(0);
|
|
}
|
|
|
|
let transfers = (slot_bytes / self.word_bytes) as u32;
|
|
|
|
for i in 0..N {
|
|
let src_start = slots[i].as_ptr() as usize;
|
|
let src_end = (src_start + slot_bytes - self.word_bytes) as *const u8;
|
|
let next = &desc.d[(i + 1) % N] as *const DmaDescriptor;
|
|
|
|
desc.d[i] = DmaDescriptor {
|
|
xfercfg: encode_xfercfg(
|
|
true, // valid
|
|
true, // reload
|
|
false, // swtrig
|
|
false, // clrtrig
|
|
true, // intA
|
|
false, // intB
|
|
self.word_bytes as u32,
|
|
1, // src_inc
|
|
0, // dst_inc
|
|
transfers,
|
|
),
|
|
src_end,
|
|
dst_end: self.dst_reg,
|
|
next,
|
|
};
|
|
}
|
|
|
|
// Ensure memory writes complete before reloading DMA hardware pointers
|
|
compiler_fence(Ordering::Release);
|
|
|
|
chan_desc.d[19] = desc.d[0];
|
|
chan_desc.d[19].xfercfg = 0;
|
|
|
|
self.reset_producer();
|
|
}
|
|
}
|
|
|
|
unsafe impl<const N: usize, const MAX_SLOT_BYTES: usize> Sync for DmaRing<N, MAX_SLOT_BYTES> {}
|
|
|
|
/// XFERCFG encoding follows the common LPC DMA layout:
|
|
/// - SETINTA at bit4, SETINTB at bit5
|
|
/// - WIDTH at bits 9:8
|
|
/// - SRCINC at bits 13:12
|
|
/// - DSTINC at bits 15:14
|
|
/// - XFERCOUNT at bits 25:16
|
|
/// This layout is shown in LPC DMA examples. [5](https://www.kernel.org/doc/html/latest/core-api/dma-api-howto.html)
|
|
fn encode_xfercfg(
|
|
cfgvalid: bool,
|
|
reload: bool,
|
|
swtrig: bool,
|
|
clrtrig: bool,
|
|
inta: bool,
|
|
intb: bool,
|
|
width_bytes: u32,
|
|
src_inc: u32,
|
|
dst_inc: u32,
|
|
transfers: u32,
|
|
) -> u32 {
|
|
let width_code = match width_bytes {
|
|
1 => 0,
|
|
2 => 1,
|
|
4 => 2,
|
|
_ => 0,
|
|
};
|
|
|
|
let count_field = transfers.saturating_sub(1) & 0x3FF;
|
|
|
|
((cfgvalid as u32) << 0)
|
|
| ((reload as u32) << 1)
|
|
| ((swtrig as u32) << 2)
|
|
| ((clrtrig as u32) << 3)
|
|
| ((inta as u32) << 4)
|
|
| ((intb as u32) << 5)
|
|
| ((width_code & 0x3) << 8)
|
|
| ((src_inc & 0x3) << 12)
|
|
| ((dst_inc & 0x3) << 14)
|
|
| (count_field << 16)
|
|
}
|