use crate::barrier::dsb;
use crate::cache::{clean_invalidate_range, clean_range, invalidate_range};
use crate::mailbox::Mailbox;
use crate::mmu;
use crate::soc::PERIPHERAL_BASE;
use crate::timer::Timer;
use core::mem::offset_of;
const SLOT_SIZE: usize = 4096;
const SLOT_MASK: u32 = SLOT_SIZE as u32 - 1;
const MAX_SLOTS_PER_SIDE: usize = 64;
const SLOT_QUEUE_MASK: u32 = MAX_SLOTS_PER_SIDE as u32 - 1;
const MAX_SLOTS: usize = 128;
const DEBUG_ENTRIES: usize = 11;
const MAGIC: u32 = u32::from_be_bytes(*b"VCHI");
const VERSION: u16 = 8;
const VERSION_MIN: u16 = 3;
const MSG_PADDING: u32 = 0;
const MSG_CONNECT: u32 = 1;
const MSG_OPEN: u32 = 2;
const MSG_OPENACK: u32 = 3;
const MSG_CLOSE: u32 = 4;
const MSG_DATA: u32 = 5;
const MSG_BULK_RX: u32 = 6;
const MSG_BULK_TX: u32 = 7;
const MSG_BULK_RX_DONE: u32 = 8;
const MSG_BULK_TX_DONE: u32 = 9;
const TYPE_SHIFT: u32 = 24;
const HEADER_SIZE: u32 = 8;
const PAGE_SIZE: u32 = 4096;
const MAX_BULKS: usize = 4;
const MAX_SERVICES: usize = 4;
pub const MAX_MESSAGE_SIZE: usize = SLOT_SIZE - HEADER_SIZE as usize;
const DATA_SLOTS: usize = 64;
const MAX_FRAGMENTS: usize = 64;
const FRAGMENT_SIZE: usize = 128;
const PAGELIST_SIZE: usize = 64;
const PAGELIST_COUNT: usize = 2 * MAX_BULKS;
const FRAGMENTS_OFFSET: usize = (SLOT_ZERO_SLOTS + DATA_SLOTS) * SLOT_SIZE;
const PAGELISTS_OFFSET: usize = FRAGMENTS_OFFSET + MAX_FRAGMENTS * FRAGMENT_SIZE;
const SLOT_ZERO_SLOTS: usize = size_of::<SlotZero>().div_ceil(SLOT_SIZE);
const SHARED_MEMORY_SIZE: usize = 0x20_0000;
const _: () = assert!(size_of::<SharedState>() == 372);
const _: () = assert!(size_of::<SlotZero>() == 1288);
const _: () = assert!(SHARED_MEMORY_SIZE >= PAGELISTS_OFFSET + PAGELIST_COUNT * PAGELIST_SIZE);
const _: () = assert!(PAGELIST_SIZE >= size_of::<PageList>());
const _: () = assert!(SHARED_MEMORY_SIZE.is_multiple_of(mmu::UNCACHED_GRANULE));
#[repr(C)]
struct RemoteEvent {
armed: u32,
fired: u32,
_unused: u32,
}
#[repr(C)]
struct SharedState {
initialised: u32,
slot_first: u32,
slot_last: u32,
slot_sync: u32,
trigger: RemoteEvent,
tx_pos: u32,
recycle: RemoteEvent,
slot_queue_recycle: u32,
sync_trigger: RemoteEvent,
sync_release: RemoteEvent,
slot_queue: [u32; MAX_SLOTS_PER_SIDE],
debug: [u32; DEBUG_ENTRIES],
}
#[repr(C)]
struct SlotInfo {
use_count: u16,
release_count: u16,
}
#[repr(C)]
struct SlotZero {
magic: u32,
version: u16,
version_min: u16,
slot_zero_size: u32,
slot_size: u32,
max_slots: u32,
max_slots_per_side: u32,
platform_data: [u32; 2],
master: SharedState,
slave: SharedState,
slots: [SlotInfo; MAX_SLOTS],
}
#[repr(C)]
struct PageList {
length: u32,
kind: u16,
offset: u16,
addrs: [u32; 1],
}
const PAGELIST_WRITE: u16 = 0;
const PAGELIST_READ: u16 = 1;
#[repr(C, align(0x20_0000))]
pub struct SharedMemory {
#[allow(dead_code)]
bytes: [u8; SHARED_MEMORY_SIZE],
}
impl SharedMemory {
pub const fn new() -> Self {
Self {
bytes: [0; SHARED_MEMORY_SIZE],
}
}
}
impl Default for SharedMemory {
fn default() -> Self {
Self::new()
}
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum Error {
Mailbox(crate::mailbox::Error),
SlotsRejected(u32),
FirmwareNotReady,
Mmu(mmu::Error),
Timeout,
NotConnected,
TooManyServices,
ServiceRefused,
MessageTooLong,
OutOfSlots,
TooManyBulks {
receive: bool,
},
Protocol,
}
impl From<crate::mailbox::Error> for Error {
fn from(error: crate::mailbox::Error) -> Self {
Error::Mailbox(error)
}
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub struct ServiceId(u16);
#[derive(Clone, Copy, Debug, Default, PartialEq, Eq)]
pub struct Stats {
pub messages_sent: u32,
pub messages_received: u32,
pub data_received: u32,
pub bulk_transmits: u32,
pub bulk_transmits_done: u32,
pub bulk_receives: u32,
pub bulk_receives_done: u32,
pub messages_skipped: u32,
pub last_skipped_type: u32,
pub slots_recycled: u32,
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum Event {
Connected,
ServiceOpened(ServiceId),
ServiceClosed(ServiceId),
Message {
service: ServiceId,
len: usize,
},
BulkTransmitDone {
context: u32,
actual: usize,
},
BulkReceiveDone {
context: u32,
actual: usize,
},
}
#[derive(Clone, Copy, PartialEq, Eq)]
enum ServiceState {
Free,
Opening,
Open,
Closed,
}
#[derive(Clone, Copy)]
struct Service {
state: ServiceState,
remote_port: u16,
}
#[derive(Clone, Copy)]
struct Bulk {
address: u32,
len: usize,
context: u32,
}
pub struct Vchiq {
base: u32,
rx_pos: u32,
local_tx_pos: u32,
tx_data: u32,
rx_data: u32,
rx_slot: usize,
slot_queue_available: u32,
connected: bool,
services: [Service; MAX_SERVICES],
bulk_tx: [Option<Bulk>; MAX_BULKS],
bulk_rx: [Option<Bulk>; MAX_BULKS],
stats: Stats,
_memory: &'static mut SharedMemory,
}
impl Vchiq {
pub fn new(
memory: &'static mut SharedMemory,
mailbox: &mut Mailbox,
timer: &Timer,
) -> Result<Self, Error> {
let base = memory as *mut SharedMemory as u32;
unsafe { mmu::set_uncached(base as usize, SHARED_MEMORY_SIZE) }.map_err(Error::Mmu)?;
let mut vchiq = Self {
base,
rx_pos: 0,
local_tx_pos: 0,
tx_data: 0,
rx_data: 0,
rx_slot: 0,
slot_queue_available: 0,
connected: false,
services: [Service {
state: ServiceState::Free,
remote_port: 0,
}; MAX_SERVICES],
bulk_tx: [None; MAX_BULKS],
bulk_rx: [None; MAX_BULKS],
stats: Stats::default(),
_memory: memory,
};
vchiq.init_slot_zero();
dsb();
let status = mailbox.vchiq_init(to_bus(base))?;
if status != 0 {
return Err(Error::SlotsRejected(status));
}
let deadline = timer.now_micros() + 1_000_000;
while vchiq.read(vchiq.remote() + offset_of!(SharedState, initialised) as u32) == 0 {
if timer.now_micros() > deadline {
return Err(Error::FirmwareNotReady);
}
}
Ok(vchiq)
}
pub fn connect(&mut self, timer: &Timer) -> Result<(), Error> {
self.queue_message(MSG_CONNECT << TYPE_SHIFT, &[])?;
let deadline = timer.now_micros() + 1_000_000;
let mut scratch = [0u8; 0];
while !self.connected {
self.poll(&mut scratch)?;
if timer.now_micros() > deadline {
return Err(Error::Timeout);
}
}
Ok(())
}
pub fn open_service(
&mut self,
fourcc: [u8; 4],
version: u16,
version_min: u16,
timer: &Timer,
) -> Result<ServiceId, Error> {
if !self.connected {
return Err(Error::NotConnected);
}
let port = self
.services
.iter()
.position(|service| service.state == ServiceState::Free)
.ok_or(Error::TooManyServices)?;
self.services[port] = Service {
state: ServiceState::Opening,
remote_port: 0,
};
let mut payload = [0u8; 12];
payload[0..4].copy_from_slice(&u32::from_be_bytes(fourcc).to_le_bytes());
payload[4..8].copy_from_slice(&1u32.to_le_bytes());
payload[8..10].copy_from_slice(&version.to_le_bytes());
payload[10..12].copy_from_slice(&version_min.to_le_bytes());
self.queue_message((MSG_OPEN << TYPE_SHIFT) | ((port as u32) << 12), &payload)?;
let deadline = timer.now_micros() + 1_000_000;
let mut scratch = [0u8; 0];
loop {
match self.services[port].state {
ServiceState::Open => return Ok(ServiceId(port as u16)),
ServiceState::Closed => {
self.services[port].state = ServiceState::Free;
return Err(Error::ServiceRefused);
}
_ => {}
}
self.poll(&mut scratch)?;
if timer.now_micros() > deadline {
self.services[port].state = ServiceState::Free;
return Err(Error::Timeout);
}
}
}
pub fn close_service(&mut self, service: ServiceId) -> Result<(), Error> {
let port = service.0 as usize;
let remote = self.services[port].remote_port;
self.services[port].state = ServiceState::Closed;
self.queue_message(
(MSG_CLOSE << TYPE_SHIFT) | ((port as u32) << 12) | u32::from(remote),
&[],
)
}
pub fn send(&mut self, service: ServiceId, data: &[u8]) -> Result<(), Error> {
let port = service.0 as usize;
if self.services[port].state != ServiceState::Open {
return Err(Error::NotConnected);
}
if data.len() > MAX_MESSAGE_SIZE {
return Err(Error::MessageTooLong);
}
let remote = self.services[port].remote_port;
self.queue_message(
(MSG_DATA << TYPE_SHIFT) | ((port as u32) << 12) | u32::from(remote),
data,
)
}
pub unsafe fn bulk_transmit(
&mut self,
service: ServiceId,
address: u32,
len: usize,
context: u32,
) -> Result<(), Error> {
clean_range(address, len);
self.queue_bulk(service, address, len, context, PAGELIST_WRITE)
}
pub unsafe fn bulk_receive(
&mut self,
service: ServiceId,
address: u32,
len: usize,
context: u32,
) -> Result<(), Error> {
clean_invalidate_range(address, len);
self.queue_bulk(service, address, len, context, PAGELIST_READ)
}
pub fn poll(&mut self, buffer: &mut [u8]) -> Result<Option<Event>, Error> {
let recycle = self.read(self.local() + offset_of!(SharedState, slot_queue_recycle) as u32);
self.slot_queue_available = recycle;
loop {
let tx_pos = self.read(self.remote() + offset_of!(SharedState, tx_pos) as u32);
if self.rx_pos == tx_pos {
return Ok(None);
}
if self.rx_data == 0 {
let queue_index = (self.rx_pos / SLOT_SIZE as u32) & SLOT_QUEUE_MASK;
let slot = self.read(
self.remote() + offset_of!(SharedState, slot_queue) as u32 + queue_index * 4,
) as usize;
self.rx_slot = slot;
self.rx_data = self.base + (slot * SLOT_SIZE) as u32;
self.write_slot_info(slot, 1, 0);
}
let header = self.rx_data + (self.rx_pos & SLOT_MASK);
let msgid = self.read(header);
let size = self.read(header + 4);
let event = self.parse(msgid, size, header + HEADER_SIZE, buffer)?;
self.rx_pos = self.rx_pos.wrapping_add(stride(size));
if self.rx_pos & SLOT_MASK == 0 {
self.release_slot(self.rx_slot);
self.rx_data = 0;
}
if event.is_some() {
return Ok(event);
}
}
}
pub fn stats(&self) -> Stats {
self.stats
}
fn parse(
&mut self,
msgid: u32,
size: u32,
data: u32,
buffer: &mut [u8],
) -> Result<Option<Event>, Error> {
let message_type = msgid >> TYPE_SHIFT;
let source_port = ((msgid >> 12) & 0xFFF) as u16;
let destination_port = (msgid & 0xFFF) as usize;
self.stats.messages_received += 1;
match message_type {
MSG_CONNECT => {
self.connected = true;
Ok(Some(Event::Connected))
}
MSG_OPENACK => {
let Some(service) = self.services.get_mut(destination_port) else {
return Ok(None);
};
if service.state != ServiceState::Opening {
return Ok(None);
}
service.remote_port = source_port;
service.state = ServiceState::Open;
Ok(Some(Event::ServiceOpened(ServiceId(
destination_port as u16,
))))
}
MSG_CLOSE => {
let Some(service) = self.services.get_mut(destination_port) else {
return Ok(None);
};
let was_open = service.state == ServiceState::Open;
service.state = ServiceState::Closed;
if was_open {
self.queue_message(
(MSG_CLOSE << TYPE_SHIFT)
| ((destination_port as u32) << 12)
| u32::from(source_port),
&[],
)?;
}
Ok(Some(Event::ServiceClosed(ServiceId(
destination_port as u16,
))))
}
MSG_DATA => {
if self.services.get(destination_port).map(|s| s.state) != Some(ServiceState::Open)
{
return Ok(None);
}
let len = size as usize;
let copied = len.min(buffer.len());
for (index, byte) in buffer[..copied].iter_mut().enumerate() {
*byte = self.read_byte(data + index as u32);
}
self.stats.data_received += 1;
Ok(Some(Event::Message {
service: ServiceId(destination_port as u16),
len,
}))
}
MSG_BULK_TX_DONE | MSG_BULK_RX_DONE => {
let receive = message_type == MSG_BULK_RX_DONE;
let queue = if receive {
&mut self.bulk_rx
} else {
&mut self.bulk_tx
};
let bulk = queue[0].take().ok_or(Error::Protocol)?;
queue.rotate_left(1);
let actual = self.read(data) as i32;
let actual = if actual < 0 { 0 } else { actual as usize };
if receive {
self.stats.bulk_receives_done += 1;
invalidate_range(bulk.address, bulk.len);
Ok(Some(Event::BulkReceiveDone {
context: bulk.context,
actual,
}))
} else {
self.stats.bulk_transmits_done += 1;
Ok(Some(Event::BulkTransmitDone {
context: bulk.context,
actual,
}))
}
}
MSG_BULK_TX | MSG_BULK_RX => Err(Error::Protocol),
_ => {
self.stats.messages_skipped += 1;
self.stats.last_skipped_type = message_type;
Ok(None)
}
}
}
fn queue_bulk(
&mut self,
service: ServiceId,
address: u32,
len: usize,
context: u32,
kind: u16,
) -> Result<(), Error> {
let port = service.0 as usize;
if self.services[port].state != ServiceState::Open {
return Err(Error::NotConnected);
}
let receive = kind == PAGELIST_READ;
let queue = if receive {
&mut self.bulk_rx
} else {
&mut self.bulk_tx
};
let index = queue
.iter()
.position(|slot| slot.is_none())
.ok_or(Error::TooManyBulks { receive })?;
queue[index] = Some(Bulk {
address,
len,
context,
});
if receive {
self.stats.bulk_receives += 1;
} else {
self.stats.bulk_transmits += 1;
}
let list_index = if receive { MAX_BULKS + index } else { index };
let list = self.base + (PAGELISTS_OFFSET + list_index * PAGELIST_SIZE) as u32;
let offset = address & (PAGE_SIZE - 1);
let pages = (len as u32 + offset).div_ceil(PAGE_SIZE);
self.write(list + offset_of!(PageList, length) as u32, len as u32);
self.write(
list + offset_of!(PageList, kind) as u32,
u32::from(kind) | (offset << 16),
);
self.write(
list + offset_of!(PageList, addrs) as u32,
to_bus(address & !(PAGE_SIZE - 1)) | (pages - 1),
);
let remote = self.services[port].remote_port;
let message_type = if receive { MSG_BULK_RX } else { MSG_BULK_TX };
let mut payload = [0u8; 8];
payload[0..4].copy_from_slice(&to_bus(list).to_le_bytes());
payload[4..8].copy_from_slice(&(len as u32).to_le_bytes());
self.queue_message(
(message_type << TYPE_SHIFT) | ((port as u32) << 12) | u32::from(remote),
&payload,
)
}
fn queue_message(&mut self, msgid: u32, data: &[u8]) -> Result<(), Error> {
let space = stride(data.len() as u32);
let header = self.reserve_space(space)?;
for (index, byte) in data.iter().enumerate() {
self.write_byte(header + HEADER_SIZE + index as u32, *byte);
}
self.write(header, msgid);
self.write(header + 4, data.len() as u32);
dsb();
self.write(
self.local() + offset_of!(SharedState, tx_pos) as u32,
self.local_tx_pos,
);
dsb();
self.signal_remote(offset_of!(SharedState, trigger) as u32);
self.stats.messages_sent += 1;
Ok(())
}
fn reserve_space(&mut self, space: u32) -> Result<u32, Error> {
let mut tx_pos = self.local_tx_pos;
let slot_space = SLOT_SIZE as u32 - (tx_pos & SLOT_MASK);
if space > slot_space {
let header = self.tx_data + (tx_pos & SLOT_MASK);
self.write(header, MSG_PADDING << TYPE_SHIFT);
self.write(header + 4, slot_space - HEADER_SIZE);
tx_pos = tx_pos.wrapping_add(slot_space);
}
if tx_pos & SLOT_MASK == 0 {
if tx_pos / SLOT_SIZE as u32 == self.slot_queue_available {
self.write(
self.local() + offset_of!(SharedState, tx_pos) as u32,
self.local_tx_pos,
);
dsb();
self.signal_remote(offset_of!(SharedState, trigger) as u32);
return Err(Error::OutOfSlots);
}
let queue_index = (tx_pos / SLOT_SIZE as u32) & SLOT_QUEUE_MASK;
let slot = self
.read(self.local() + offset_of!(SharedState, slot_queue) as u32 + queue_index * 4)
as usize;
self.tx_data = self.base + (slot * SLOT_SIZE) as u32;
}
self.local_tx_pos = tx_pos.wrapping_add(space);
Ok(self.tx_data + (tx_pos & SLOT_MASK))
}
fn release_slot(&mut self, slot: usize) {
let info = self.base + offset_of!(SlotZero, slots) as u32 + (slot * 4) as u32;
let use_count = self.read_half(info);
let release_count = self.read_half(info + 2) + 1;
self.write_half(info + 2, release_count);
if release_count != use_count {
return;
}
let recycle_field = self.remote() + offset_of!(SharedState, slot_queue_recycle) as u32;
let recycle = self.read(recycle_field);
self.write(
self.remote()
+ offset_of!(SharedState, slot_queue) as u32
+ (recycle & SLOT_QUEUE_MASK) * 4,
slot as u32,
);
self.write(recycle_field, recycle.wrapping_add(1));
dsb();
self.signal_remote(offset_of!(SharedState, recycle) as u32);
self.stats.slots_recycled += 1;
}
fn signal_remote(&mut self, event_offset: u32) {
let event = self.remote() + event_offset;
self.write(event + offset_of!(RemoteEvent, fired) as u32, 1);
dsb();
if self.read(event + offset_of!(RemoteEvent, armed) as u32) != 0 {
unsafe { ((PERIPHERAL_BASE + DOORBELL_OFFSET) as *mut u32).write_volatile(0) };
}
}
fn init_slot_zero(&mut self) {
for offset in (0..size_of::<SlotZero>() as u32).step_by(4) {
self.write(self.base + offset, 0);
}
self.write(self.base + offset_of!(SlotZero, magic) as u32, MAGIC);
self.write(
self.base + offset_of!(SlotZero, version) as u32,
u32::from(VERSION) | (u32::from(VERSION_MIN) << 16),
);
self.write(
self.base + offset_of!(SlotZero, slot_zero_size) as u32,
size_of::<SlotZero>() as u32,
);
self.write(
self.base + offset_of!(SlotZero, slot_size) as u32,
SLOT_SIZE as u32,
);
self.write(
self.base + offset_of!(SlotZero, max_slots) as u32,
MAX_SLOTS as u32,
);
self.write(
self.base + offset_of!(SlotZero, max_slots_per_side) as u32,
MAX_SLOTS_PER_SIDE as u32,
);
self.write(
self.base + offset_of!(SlotZero, platform_data) as u32,
to_bus(self.base + FRAGMENTS_OFFSET as u32),
);
self.write(
self.base + offset_of!(SlotZero, platform_data) as u32 + 4,
MAX_FRAGMENTS as u32,
);
let first_data_slot = SLOT_ZERO_SLOTS as u32;
let half = DATA_SLOTS as u32 / 2;
let master = self.remote();
self.write(
master + offset_of!(SharedState, slot_sync) as u32,
first_data_slot,
);
self.write(
master + offset_of!(SharedState, slot_first) as u32,
first_data_slot + 1,
);
self.write(
master + offset_of!(SharedState, slot_last) as u32,
first_data_slot + half - 1,
);
let local = self.local();
let slave_sync = first_data_slot + half;
self.write(
local + offset_of!(SharedState, slot_sync) as u32,
slave_sync,
);
self.write(
local + offset_of!(SharedState, slot_first) as u32,
slave_sync + 1,
);
self.write(
local + offset_of!(SharedState, slot_last) as u32,
first_data_slot + DATA_SLOTS as u32 - 1,
);
for (position, slot) in (slave_sync + 1..first_data_slot + DATA_SLOTS as u32).enumerate() {
self.write(
local + offset_of!(SharedState, slot_queue) as u32 + (position as u32) * 4,
slot,
);
self.slot_queue_available += 1;
}
self.write(
local + offset_of!(SharedState, slot_queue_recycle) as u32,
self.slot_queue_available,
);
self.write(
self.base + (slave_sync * SLOT_SIZE as u32),
MSG_PADDING << TYPE_SHIFT,
);
self.write(
local
+ offset_of!(SharedState, sync_release) as u32
+ offset_of!(RemoteEvent, fired) as u32,
1,
);
dsb();
self.write(local + offset_of!(SharedState, initialised) as u32, 1);
}
fn local(&self) -> u32 {
self.base + offset_of!(SlotZero, slave) as u32
}
fn remote(&self) -> u32 {
self.base + offset_of!(SlotZero, master) as u32
}
fn write_slot_info(&mut self, slot: usize, use_count: u16, release_count: u16) {
let info = self.base + offset_of!(SlotZero, slots) as u32 + (slot * 4) as u32;
self.write(
info,
u32::from(use_count) | (u32::from(release_count) << 16),
);
}
fn read(&self, address: u32) -> u32 {
unsafe { (address as *const u32).read_volatile() }
}
fn write(&mut self, address: u32, value: u32) {
unsafe { (address as *mut u32).write_volatile(value) };
}
fn read_half(&self, address: u32) -> u16 {
unsafe { (address as *const u16).read_volatile() }
}
fn write_half(&mut self, address: u32, value: u16) {
unsafe { (address as *mut u16).write_volatile(value) };
}
fn read_byte(&self, address: u32) -> u8 {
unsafe { (address as *const u8).read_volatile() }
}
fn write_byte(&mut self, address: u32, value: u8) {
unsafe { (address as *mut u8).write_volatile(value) };
}
}
const DOORBELL_OFFSET: u32 = 0x0000_B848;
fn stride(size: u32) -> u32 {
(size + HEADER_SIZE + HEADER_SIZE - 1) & !(HEADER_SIZE - 1)
}
fn to_bus(physical_address: u32) -> u32 {
(physical_address & 0x3FFF_FFFF) | 0xC000_0000
}