diff --git a/ci.sh b/ci.sh index 1eafda3a..8a3669f0 100755 --- a/ci.sh +++ b/ci.sh @@ -5,6 +5,10 @@ set -euo pipefail export RUSTFLAGS=-Dwarnings export DEFMT_LOG=trace +# needed by wifi examples +export WIFI_NETWORK=x +export WIFI_PASSWORD=x + TARGET=$(rustc -vV | sed -n 's|host: ||p') BUILD_EXTRA="" @@ -82,6 +86,13 @@ cargo batch \ --- build --release --manifest-path embassy-stm32/Cargo.toml --target thumbv7m-none-eabi --features nightly,stm32f100c4,defmt,exti,time-driver-any,unstable-traits \ --- build --release --manifest-path embassy-stm32/Cargo.toml --target thumbv7m-none-eabi --features nightly,stm32h503rb,defmt,exti,time-driver-any,unstable-traits \ --- build --release --manifest-path embassy-stm32/Cargo.toml --target thumbv7m-none-eabi --features nightly,stm32h562ag,defmt,exti,time-driver-any,unstable-traits \ + --- build --release --manifest-path cyw43/Cargo.toml --target thumbv6m-none-eabi --features ''\ + --- build --release --manifest-path cyw43/Cargo.toml --target thumbv6m-none-eabi --features 'log' \ + --- build --release --manifest-path cyw43/Cargo.toml --target thumbv6m-none-eabi --features 'defmt' \ + --- build --release --manifest-path cyw43/Cargo.toml --target thumbv6m-none-eabi --features 'log,firmware-logs' \ + --- build --release --manifest-path cyw43/Cargo.toml --target thumbv6m-none-eabi --features 'defmt,firmware-logs' \ + --- build --release --manifest-path cyw43-pio/Cargo.toml --target thumbv6m-none-eabi --features '' \ + --- build --release --manifest-path cyw43-pio/Cargo.toml --target thumbv6m-none-eabi --features 'overclock' \ --- build --release --manifest-path embassy-boot/nrf/Cargo.toml --target thumbv7em-none-eabi --features embassy-nrf/nrf52840,nightly \ --- build --release --manifest-path embassy-boot/nrf/Cargo.toml --target thumbv8m.main-none-eabihf --features embassy-nrf/nrf9160-ns,nightly \ --- build --release --manifest-path embassy-boot/rp/Cargo.toml --target thumbv6m-none-eabi --features nightly \ diff --git a/cyw43-firmware/43439A0.bin b/cyw43-firmware/43439A0.bin new file mode 100755 index 00000000..b46b3bef Binary files /dev/null and b/cyw43-firmware/43439A0.bin differ diff --git a/cyw43-firmware/43439A0_clm.bin b/cyw43-firmware/43439A0_clm.bin new file mode 100755 index 00000000..6e3ba786 Binary files /dev/null and b/cyw43-firmware/43439A0_clm.bin differ diff --git a/cyw43-firmware/LICENSE-permissive-binary-license-1.0.txt b/cyw43-firmware/LICENSE-permissive-binary-license-1.0.txt new file mode 100644 index 00000000..cbb51f9c --- /dev/null +++ b/cyw43-firmware/LICENSE-permissive-binary-license-1.0.txt @@ -0,0 +1,49 @@ +Permissive Binary License + +Version 1.0, July 2019 + +Redistribution. Redistribution and use in binary form, without +modification, are permitted provided that the following conditions are +met: + +1) Redistributions must reproduce the above copyright notice and the + following disclaimer in the documentation and/or other materials + provided with the distribution. + +2) Unless to the extent explicitly permitted by law, no reverse + engineering, decompilation, or disassembly of this software is + permitted. + +3) Redistribution as part of a software development kit must include the + accompanying file named �DEPENDENCIES� and any dependencies listed in + that file. + +4) Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +Limited patent license. The copyright holders (and contributors) grant a +worldwide, non-exclusive, no-charge, royalty-free patent license to +make, have made, use, offer to sell, sell, import, and otherwise +transfer this software, where such license applies only to those patent +claims licensable by the copyright holders (and contributors) that are +necessarily infringed by this software. This patent license shall not +apply to any combinations that include this software. No hardware is +licensed hereunder. + +If you institute patent litigation against any entity (including a +cross-claim or counterclaim in a lawsuit) alleging that the software +itself infringes your patent(s), then your rights granted under this +license shall terminate as of the date such litigation is filed. + +DISCLAIMER. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND +CONTRIBUTORS "AS IS." ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT +NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED +TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR +PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF +LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING +NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. \ No newline at end of file diff --git a/cyw43-firmware/README.md b/cyw43-firmware/README.md new file mode 100644 index 00000000..7381fdc5 --- /dev/null +++ b/cyw43-firmware/README.md @@ -0,0 +1,5 @@ +# WiFi firmware + +Firmware obtained from https://github.com/Infineon/wifi-host-driver/tree/master/WiFi_Host_Driver/resources/firmware/COMPONENT_43439 + +Licensed under the [Infineon Permissive Binary License](./LICENSE-permissive-binary-license-1.0.txt) \ No newline at end of file diff --git a/cyw43-pio/Cargo.toml b/cyw43-pio/Cargo.toml new file mode 100644 index 00000000..6e9e784a --- /dev/null +++ b/cyw43-pio/Cargo.toml @@ -0,0 +1,17 @@ +[package] +name = "cyw43-pio" +version = "0.1.0" +edition = "2021" + +[features] +# If disabled, SPI runs at 31.25MHz +# If enabled, SPI runs at 62.5MHz, which is 25% higher than 50Mhz which is the maximum according to the CYW43439 datasheet. +overclock = [] + +[dependencies] +cyw43 = { version = "0.1.0", path = "../cyw43" } +embassy-rp = { version = "0.1.0", path = "../embassy-rp" } +pio-proc = "0.2" +pio = "0.2.1" +fixed = "1.23.1" +defmt = { version = "0.3", optional = true } diff --git a/cyw43-pio/src/lib.rs b/cyw43-pio/src/lib.rs new file mode 100644 index 00000000..dca30c74 --- /dev/null +++ b/cyw43-pio/src/lib.rs @@ -0,0 +1,229 @@ +#![no_std] +#![allow(incomplete_features)] +#![feature(async_fn_in_trait)] + +use core::slice; + +use cyw43::SpiBusCyw43; +use embassy_rp::dma::Channel; +use embassy_rp::gpio::{Drive, Level, Output, Pin, Pull, SlewRate}; +use embassy_rp::pio::{Common, Config, Direction, Instance, Irq, PioPin, ShiftDirection, StateMachine}; +use embassy_rp::relocate::RelocatedProgram; +use embassy_rp::{pio_instr_util, Peripheral, PeripheralRef}; +use fixed::FixedU32; +use pio_proc::pio_asm; + +pub struct PioSpi<'d, CS: Pin, PIO: Instance, const SM: usize, DMA> { + cs: Output<'d, CS>, + sm: StateMachine<'d, PIO, SM>, + irq: Irq<'d, PIO, 0>, + dma: PeripheralRef<'d, DMA>, + wrap_target: u8, +} + +impl<'d, CS, PIO, const SM: usize, DMA> PioSpi<'d, CS, PIO, SM, DMA> +where + DMA: Channel, + CS: Pin, + PIO: Instance, +{ + pub fn new( + common: &mut Common<'d, PIO>, + mut sm: StateMachine<'d, PIO, SM>, + irq: Irq<'d, PIO, 0>, + cs: Output<'d, CS>, + dio: DIO, + clk: CLK, + dma: impl Peripheral

+ 'd, + ) -> Self + where + DIO: PioPin, + CLK: PioPin, + { + #[cfg(feature = "overclock")] + let program = pio_asm!( + ".side_set 1" + + ".wrap_target" + // write out x-1 bits + "lp:" + "out pins, 1 side 0" + "jmp x-- lp side 1" + // switch directions + "set pindirs, 0 side 0" + "nop side 1" // necessary for clkdiv=1. + "nop side 0" + // read in y-1 bits + "lp2:" + "in pins, 1 side 1" + "jmp y-- lp2 side 0" + + // wait for event and irq host + "wait 1 pin 0 side 0" + "irq 0 side 0" + + ".wrap" + ); + #[cfg(not(feature = "overclock"))] + let program = pio_asm!( + ".side_set 1" + + ".wrap_target" + // write out x-1 bits + "lp:" + "out pins, 1 side 0" + "jmp x-- lp side 1" + // switch directions + "set pindirs, 0 side 0" + "nop side 0" + // read in y-1 bits + "lp2:" + "in pins, 1 side 1" + "jmp y-- lp2 side 0" + + // wait for event and irq host + "wait 1 pin 0 side 0" + "irq 0 side 0" + + ".wrap" + ); + + let relocated = RelocatedProgram::new(&program.program); + + let mut pin_io: embassy_rp::pio::Pin = common.make_pio_pin(dio); + pin_io.set_pull(Pull::None); + pin_io.set_schmitt(true); + pin_io.set_input_sync_bypass(true); + pin_io.set_drive_strength(Drive::_12mA); + pin_io.set_slew_rate(SlewRate::Fast); + + let mut pin_clk = common.make_pio_pin(clk); + pin_clk.set_drive_strength(Drive::_12mA); + pin_clk.set_slew_rate(SlewRate::Fast); + + let mut cfg = Config::default(); + cfg.use_program(&common.load_program(&relocated), &[&pin_clk]); + cfg.set_out_pins(&[&pin_io]); + cfg.set_in_pins(&[&pin_io]); + cfg.set_set_pins(&[&pin_io]); + cfg.shift_out.direction = ShiftDirection::Left; + cfg.shift_out.auto_fill = true; + //cfg.shift_out.threshold = 32; + cfg.shift_in.direction = ShiftDirection::Left; + cfg.shift_in.auto_fill = true; + //cfg.shift_in.threshold = 32; + + #[cfg(feature = "overclock")] + { + // 125mhz Pio => 62.5Mhz SPI Freq. 25% higher than theoretical maximum according to + // data sheet, but seems to work fine. + cfg.clock_divider = FixedU32::from_bits(0x0100); + } + + #[cfg(not(feature = "overclock"))] + { + // same speed as pico-sdk, 62.5Mhz + // This is actually the fastest we can go without overclocking. + // According to data sheet, the theoretical maximum is 100Mhz Pio => 50Mhz SPI Freq. + // However, the PIO uses a fractional divider, which works by introducing jitter when + // the divider is not an integer. It does some clocks at 125mhz and others at 62.5mhz + // so that it averages out to the desired frequency of 100mhz. The 125mhz clock cycles + // violate the maximum from the data sheet. + cfg.clock_divider = FixedU32::from_bits(0x0200); + } + + sm.set_config(&cfg); + + sm.set_pin_dirs(Direction::Out, &[&pin_clk, &pin_io]); + sm.set_pins(Level::Low, &[&pin_clk, &pin_io]); + + Self { + cs, + sm, + irq, + dma: dma.into_ref(), + wrap_target: relocated.wrap().target, + } + } + + pub async fn write(&mut self, write: &[u32]) -> u32 { + self.sm.set_enable(false); + let write_bits = write.len() * 32 - 1; + let read_bits = 31; + + #[cfg(feature = "defmt")] + defmt::trace!("write={} read={}", write_bits, read_bits); + + unsafe { + pio_instr_util::set_x(&mut self.sm, write_bits as u32); + pio_instr_util::set_y(&mut self.sm, read_bits as u32); + pio_instr_util::set_pindir(&mut self.sm, 0b1); + pio_instr_util::exec_jmp(&mut self.sm, self.wrap_target); + } + + self.sm.set_enable(true); + + self.sm.tx().dma_push(self.dma.reborrow(), write).await; + + let mut status = 0; + self.sm + .rx() + .dma_pull(self.dma.reborrow(), slice::from_mut(&mut status)) + .await; + status + } + + pub async fn cmd_read(&mut self, cmd: u32, read: &mut [u32]) -> u32 { + self.sm.set_enable(false); + let write_bits = 31; + let read_bits = read.len() * 32 + 32 - 1; + + #[cfg(feature = "defmt")] + defmt::trace!("write={} read={}", write_bits, read_bits); + + unsafe { + pio_instr_util::set_y(&mut self.sm, read_bits as u32); + pio_instr_util::set_x(&mut self.sm, write_bits as u32); + pio_instr_util::set_pindir(&mut self.sm, 0b1); + pio_instr_util::exec_jmp(&mut self.sm, self.wrap_target); + } + + // self.cs.set_low(); + self.sm.set_enable(true); + + self.sm.tx().dma_push(self.dma.reborrow(), slice::from_ref(&cmd)).await; + self.sm.rx().dma_pull(self.dma.reborrow(), read).await; + + let mut status = 0; + self.sm + .rx() + .dma_pull(self.dma.reborrow(), slice::from_mut(&mut status)) + .await; + status + } +} + +impl<'d, CS, PIO, const SM: usize, DMA> SpiBusCyw43 for PioSpi<'d, CS, PIO, SM, DMA> +where + CS: Pin, + PIO: Instance, + DMA: Channel, +{ + async fn cmd_write(&mut self, write: &[u32]) -> u32 { + self.cs.set_low(); + let status = self.write(write).await; + self.cs.set_high(); + status + } + + async fn cmd_read(&mut self, write: u32, read: &mut [u32]) -> u32 { + self.cs.set_low(); + let status = self.cmd_read(write, read).await; + self.cs.set_high(); + status + } + + async fn wait_for_event(&mut self) { + self.irq.wait().await; + } +} diff --git a/cyw43/Cargo.toml b/cyw43/Cargo.toml new file mode 100644 index 00000000..c7f8816f --- /dev/null +++ b/cyw43/Cargo.toml @@ -0,0 +1,28 @@ +[package] +name = "cyw43" +version = "0.1.0" +edition = "2021" + +[features] +defmt = ["dep:defmt"] +log = ["dep:log"] + +# Fetch console logs from the WiFi firmware and forward them to `log` or `defmt`. +firmware-logs = [] + +[dependencies] +embassy-time = { version = "0.1.0", path = "../embassy-time"} +embassy-sync = { version = "0.2.0", path = "../embassy-sync"} +embassy-futures = { version = "0.1.0", path = "../embassy-futures"} +embassy-net-driver-channel = { version = "0.1.0", path = "../embassy-net-driver-channel"} +atomic-polyfill = "0.1.5" + +defmt = { version = "0.3", optional = true } +log = { version = "0.4.17", optional = true } + +cortex-m = "0.7.6" +cortex-m-rt = "0.7.0" +futures = { version = "0.3.17", default-features = false, features = ["async-await", "cfg-target-has-atomic", "unstable"] } + +embedded-hal-1 = { package = "embedded-hal", version = "1.0.0-alpha.10" } +num_enum = { version = "0.5.7", default-features = false } diff --git a/cyw43/README.md b/cyw43/README.md new file mode 100644 index 00000000..defea489 --- /dev/null +++ b/cyw43/README.md @@ -0,0 +1,57 @@ +# cyw43 + +WIP driver for the CYW43439 wifi chip, used in the Raspberry Pi Pico W. Implementation based on [Infineon/wifi-host-driver](https://github.com/Infineon/wifi-host-driver). + +## Current status + +Working: + +- Station mode (joining an AP). +- AP mode (creating an AP) +- Scanning +- Sending and receiving Ethernet frames. +- Using the default MAC address. +- [`embassy-net`](https://embassy.dev) integration. +- RP2040 PIO driver for the nonstandard half-duplex SPI used in the Pico W. +- Using IRQ for device events +- GPIO support (for LED on the Pico W) + +TODO: + +- Setting a custom MAC address. +- Bus sleep (unclear what the benefit is. Is it needed for IRQs? or is it just power consumption optimization?) + +## Running the examples + +- `cargo install probe-rs-cli` +- `cd examples/rpi-pico-w` +### Example 1: Scan the wifi stations +- `cargo run --release --bin wifi_scan` +### Example 2: Create an access point (IP and credentials in the code) +- `cargo run --release --bin tcp_server_ap` +### Example 3: Connect to an existing network and create a server +- `WIFI_NETWORK=MyWifiNetwork WIFI_PASSWORD=MyWifiPassword cargo run --release` + +After a few seconds, you should see that DHCP picks up an IP address like this +``` +11.944489 DEBUG Acquired IP configuration: +11.944517 DEBUG IP address: 192.168.0.250/24 +11.944620 DEBUG Default gateway: 192.168.0.33 +11.944722 DEBUG DNS server 0: 192.168.0.33 +``` +This example implements a TCP echo server on port 1234. You can try connecting to it with: +``` +nc 192.168.0.250 1234 +``` +Send it some data, you should see it echoed back and printed in the firmware's logs. + +## License + +This work is licensed under either of + +- Apache License, Version 2.0 ([LICENSE-APACHE](LICENSE-APACHE) or + ) +- MIT license ([LICENSE-MIT](LICENSE-MIT) or ) + +at your option. + diff --git a/cyw43/src/bus.rs b/cyw43/src/bus.rs new file mode 100644 index 00000000..e26f1112 --- /dev/null +++ b/cyw43/src/bus.rs @@ -0,0 +1,328 @@ +use embassy_futures::yield_now; +use embassy_time::{Duration, Timer}; +use embedded_hal_1::digital::OutputPin; +use futures::FutureExt; + +use crate::consts::*; +use crate::slice8_mut; + +/// Custom Spi Trait that _only_ supports the bus operation of the cyw43 +/// Implementors are expected to hold the CS pin low during an operation. +pub trait SpiBusCyw43 { + /// Issues a write command on the bus + /// First 32 bits of `word` are expected to be a cmd word + async fn cmd_write(&mut self, write: &[u32]) -> u32; + + /// Issues a read command on the bus + /// `write` is expected to be a 32 bit cmd word + /// `read` will contain the response of the device + /// Backplane reads have a response delay that produces one extra unspecified word at the beginning of `read`. + /// Callers that want to read `n` word from the backplane, have to provide a slice that is `n+1` words long. + async fn cmd_read(&mut self, write: u32, read: &mut [u32]) -> u32; + + /// Wait for events from the Device. A typical implementation would wait for the IRQ pin to be high. + /// The default implementation always reports ready, resulting in active polling of the device. + async fn wait_for_event(&mut self) { + yield_now().await; + } +} + +pub(crate) struct Bus { + backplane_window: u32, + pwr: PWR, + spi: SPI, + status: u32, +} + +impl Bus +where + PWR: OutputPin, + SPI: SpiBusCyw43, +{ + pub(crate) fn new(pwr: PWR, spi: SPI) -> Self { + Self { + backplane_window: 0xAAAA_AAAA, + pwr, + spi, + status: 0, + } + } + + pub async fn init(&mut self) { + // Reset + self.pwr.set_low().unwrap(); + Timer::after(Duration::from_millis(20)).await; + self.pwr.set_high().unwrap(); + Timer::after(Duration::from_millis(250)).await; + + while self + .read32_swapped(REG_BUS_TEST_RO) + .inspect(|v| trace!("{:#x}", v)) + .await + != FEEDBEAD + {} + + self.write32_swapped(REG_BUS_TEST_RW, TEST_PATTERN).await; + let val = self.read32_swapped(REG_BUS_TEST_RW).await; + trace!("{:#x}", val); + assert_eq!(val, TEST_PATTERN); + + let val = self.read32_swapped(REG_BUS_CTRL).await; + trace!("{:#010b}", (val & 0xff)); + + // 32-bit word length, little endian (which is the default endianess). + self.write32_swapped( + REG_BUS_CTRL, + WORD_LENGTH_32 | HIGH_SPEED | INTERRUPT_HIGH | WAKE_UP | STATUS_ENABLE | INTERRUPT_WITH_STATUS, + ) + .await; + + let val = self.read8(FUNC_BUS, REG_BUS_CTRL).await; + trace!("{:#b}", val); + + let val = self.read32(FUNC_BUS, REG_BUS_TEST_RO).await; + trace!("{:#x}", val); + assert_eq!(val, FEEDBEAD); + let val = self.read32(FUNC_BUS, REG_BUS_TEST_RW).await; + trace!("{:#x}", val); + assert_eq!(val, TEST_PATTERN); + } + + pub async fn wlan_read(&mut self, buf: &mut [u32], len_in_u8: u32) { + let cmd = cmd_word(READ, INC_ADDR, FUNC_WLAN, 0, len_in_u8); + let len_in_u32 = (len_in_u8 as usize + 3) / 4; + + self.status = self.spi.cmd_read(cmd, &mut buf[..len_in_u32]).await; + } + + pub async fn wlan_write(&mut self, buf: &[u32]) { + let cmd = cmd_word(WRITE, INC_ADDR, FUNC_WLAN, 0, buf.len() as u32 * 4); + //TODO try to remove copy? + let mut cmd_buf = [0_u32; 513]; + cmd_buf[0] = cmd; + cmd_buf[1..][..buf.len()].copy_from_slice(buf); + + self.status = self.spi.cmd_write(&cmd_buf).await; + } + + #[allow(unused)] + pub async fn bp_read(&mut self, mut addr: u32, mut data: &mut [u8]) { + // It seems the HW force-aligns the addr + // to 2 if data.len() >= 2 + // to 4 if data.len() >= 4 + // To simplify, enforce 4-align for now. + assert!(addr % 4 == 0); + + // Backplane read buffer has one extra word for the response delay. + let mut buf = [0u32; BACKPLANE_MAX_TRANSFER_SIZE / 4 + 1]; + + while !data.is_empty() { + // Ensure transfer doesn't cross a window boundary. + let window_offs = addr & BACKPLANE_ADDRESS_MASK; + let window_remaining = BACKPLANE_WINDOW_SIZE - window_offs as usize; + + let len = data.len().min(BACKPLANE_MAX_TRANSFER_SIZE).min(window_remaining); + + self.backplane_set_window(addr).await; + + let cmd = cmd_word(READ, INC_ADDR, FUNC_BACKPLANE, window_offs, len as u32); + + // round `buf` to word boundary, add one extra word for the response delay + self.status = self.spi.cmd_read(cmd, &mut buf[..(len + 3) / 4 + 1]).await; + + // when writing out the data, we skip the response-delay byte + data[..len].copy_from_slice(&slice8_mut(&mut buf[1..])[..len]); + + // Advance ptr. + addr += len as u32; + data = &mut data[len..]; + } + } + + pub async fn bp_write(&mut self, mut addr: u32, mut data: &[u8]) { + // It seems the HW force-aligns the addr + // to 2 if data.len() >= 2 + // to 4 if data.len() >= 4 + // To simplify, enforce 4-align for now. + assert!(addr % 4 == 0); + + let mut buf = [0u32; BACKPLANE_MAX_TRANSFER_SIZE / 4 + 1]; + + while !data.is_empty() { + // Ensure transfer doesn't cross a window boundary. + let window_offs = addr & BACKPLANE_ADDRESS_MASK; + let window_remaining = BACKPLANE_WINDOW_SIZE - window_offs as usize; + + let len = data.len().min(BACKPLANE_MAX_TRANSFER_SIZE).min(window_remaining); + slice8_mut(&mut buf[1..])[..len].copy_from_slice(&data[..len]); + + self.backplane_set_window(addr).await; + + let cmd = cmd_word(WRITE, INC_ADDR, FUNC_BACKPLANE, window_offs, len as u32); + buf[0] = cmd; + + self.status = self.spi.cmd_write(&buf[..(len + 3) / 4 + 1]).await; + + // Advance ptr. + addr += len as u32; + data = &data[len..]; + } + } + + pub async fn bp_read8(&mut self, addr: u32) -> u8 { + self.backplane_readn(addr, 1).await as u8 + } + + pub async fn bp_write8(&mut self, addr: u32, val: u8) { + self.backplane_writen(addr, val as u32, 1).await + } + + pub async fn bp_read16(&mut self, addr: u32) -> u16 { + self.backplane_readn(addr, 2).await as u16 + } + + #[allow(unused)] + pub async fn bp_write16(&mut self, addr: u32, val: u16) { + self.backplane_writen(addr, val as u32, 2).await + } + + #[allow(unused)] + pub async fn bp_read32(&mut self, addr: u32) -> u32 { + self.backplane_readn(addr, 4).await + } + + pub async fn bp_write32(&mut self, addr: u32, val: u32) { + self.backplane_writen(addr, val, 4).await + } + + async fn backplane_readn(&mut self, addr: u32, len: u32) -> u32 { + self.backplane_set_window(addr).await; + + let mut bus_addr = addr & BACKPLANE_ADDRESS_MASK; + if len == 4 { + bus_addr |= BACKPLANE_ADDRESS_32BIT_FLAG + } + self.readn(FUNC_BACKPLANE, bus_addr, len).await + } + + async fn backplane_writen(&mut self, addr: u32, val: u32, len: u32) { + self.backplane_set_window(addr).await; + + let mut bus_addr = addr & BACKPLANE_ADDRESS_MASK; + if len == 4 { + bus_addr |= BACKPLANE_ADDRESS_32BIT_FLAG + } + self.writen(FUNC_BACKPLANE, bus_addr, val, len).await + } + + async fn backplane_set_window(&mut self, addr: u32) { + let new_window = addr & !BACKPLANE_ADDRESS_MASK; + + if (new_window >> 24) as u8 != (self.backplane_window >> 24) as u8 { + self.write8( + FUNC_BACKPLANE, + REG_BACKPLANE_BACKPLANE_ADDRESS_HIGH, + (new_window >> 24) as u8, + ) + .await; + } + if (new_window >> 16) as u8 != (self.backplane_window >> 16) as u8 { + self.write8( + FUNC_BACKPLANE, + REG_BACKPLANE_BACKPLANE_ADDRESS_MID, + (new_window >> 16) as u8, + ) + .await; + } + if (new_window >> 8) as u8 != (self.backplane_window >> 8) as u8 { + self.write8( + FUNC_BACKPLANE, + REG_BACKPLANE_BACKPLANE_ADDRESS_LOW, + (new_window >> 8) as u8, + ) + .await; + } + self.backplane_window = new_window; + } + + pub async fn read8(&mut self, func: u32, addr: u32) -> u8 { + self.readn(func, addr, 1).await as u8 + } + + pub async fn write8(&mut self, func: u32, addr: u32, val: u8) { + self.writen(func, addr, val as u32, 1).await + } + + pub async fn read16(&mut self, func: u32, addr: u32) -> u16 { + self.readn(func, addr, 2).await as u16 + } + + #[allow(unused)] + pub async fn write16(&mut self, func: u32, addr: u32, val: u16) { + self.writen(func, addr, val as u32, 2).await + } + + pub async fn read32(&mut self, func: u32, addr: u32) -> u32 { + self.readn(func, addr, 4).await + } + + #[allow(unused)] + pub async fn write32(&mut self, func: u32, addr: u32, val: u32) { + self.writen(func, addr, val, 4).await + } + + async fn readn(&mut self, func: u32, addr: u32, len: u32) -> u32 { + let cmd = cmd_word(READ, INC_ADDR, func, addr, len); + let mut buf = [0; 2]; + // if we are reading from the backplane, we need an extra word for the response delay + let len = if func == FUNC_BACKPLANE { 2 } else { 1 }; + + self.status = self.spi.cmd_read(cmd, &mut buf[..len]).await; + + // if we read from the backplane, the result is in the second word, after the response delay + if func == FUNC_BACKPLANE { + buf[1] + } else { + buf[0] + } + } + + async fn writen(&mut self, func: u32, addr: u32, val: u32, len: u32) { + let cmd = cmd_word(WRITE, INC_ADDR, func, addr, len); + + self.status = self.spi.cmd_write(&[cmd, val]).await; + } + + async fn read32_swapped(&mut self, addr: u32) -> u32 { + let cmd = cmd_word(READ, INC_ADDR, FUNC_BUS, addr, 4); + let cmd = swap16(cmd); + let mut buf = [0; 1]; + + self.status = self.spi.cmd_read(cmd, &mut buf).await; + + swap16(buf[0]) + } + + async fn write32_swapped(&mut self, addr: u32, val: u32) { + let cmd = cmd_word(WRITE, INC_ADDR, FUNC_BUS, addr, 4); + let buf = [swap16(cmd), swap16(val)]; + + self.status = self.spi.cmd_write(&buf).await; + } + + pub async fn wait_for_event(&mut self) { + self.spi.wait_for_event().await; + } + + pub fn status(&self) -> u32 { + self.status + } +} + +fn swap16(x: u32) -> u32 { + x.rotate_left(16) +} + +fn cmd_word(write: bool, incr: bool, func: u32, addr: u32, len: u32) -> u32 { + (write as u32) << 31 | (incr as u32) << 30 | (func & 0b11) << 28 | (addr & 0x1FFFF) << 11 | (len & 0x7FF) +} diff --git a/cyw43/src/consts.rs b/cyw43/src/consts.rs new file mode 100644 index 00000000..1f655158 --- /dev/null +++ b/cyw43/src/consts.rs @@ -0,0 +1,318 @@ +#![allow(unused)] + +pub(crate) const FUNC_BUS: u32 = 0; +pub(crate) const FUNC_BACKPLANE: u32 = 1; +pub(crate) const FUNC_WLAN: u32 = 2; +pub(crate) const FUNC_BT: u32 = 3; + +pub(crate) const REG_BUS_CTRL: u32 = 0x0; +pub(crate) const REG_BUS_INTERRUPT: u32 = 0x04; // 16 bits - Interrupt status +pub(crate) const REG_BUS_INTERRUPT_ENABLE: u32 = 0x06; // 16 bits - Interrupt mask +pub(crate) const REG_BUS_STATUS: u32 = 0x8; +pub(crate) const REG_BUS_TEST_RO: u32 = 0x14; +pub(crate) const REG_BUS_TEST_RW: u32 = 0x18; +pub(crate) const REG_BUS_RESP_DELAY: u32 = 0x1c; +pub(crate) const WORD_LENGTH_32: u32 = 0x1; +pub(crate) const HIGH_SPEED: u32 = 0x10; +pub(crate) const INTERRUPT_HIGH: u32 = 1 << 5; +pub(crate) const WAKE_UP: u32 = 1 << 7; +pub(crate) const STATUS_ENABLE: u32 = 1 << 16; +pub(crate) const INTERRUPT_WITH_STATUS: u32 = 1 << 17; + +// SPI_STATUS_REGISTER bits +pub(crate) const STATUS_DATA_NOT_AVAILABLE: u32 = 0x00000001; +pub(crate) const STATUS_UNDERFLOW: u32 = 0x00000002; +pub(crate) const STATUS_OVERFLOW: u32 = 0x00000004; +pub(crate) const STATUS_F2_INTR: u32 = 0x00000008; +pub(crate) const STATUS_F3_INTR: u32 = 0x00000010; +pub(crate) const STATUS_F2_RX_READY: u32 = 0x00000020; +pub(crate) const STATUS_F3_RX_READY: u32 = 0x00000040; +pub(crate) const STATUS_HOST_CMD_DATA_ERR: u32 = 0x00000080; +pub(crate) const STATUS_F2_PKT_AVAILABLE: u32 = 0x00000100; +pub(crate) const STATUS_F2_PKT_LEN_MASK: u32 = 0x000FFE00; +pub(crate) const STATUS_F2_PKT_LEN_SHIFT: u32 = 9; +pub(crate) const STATUS_F3_PKT_AVAILABLE: u32 = 0x00100000; +pub(crate) const STATUS_F3_PKT_LEN_MASK: u32 = 0xFFE00000; +pub(crate) const STATUS_F3_PKT_LEN_SHIFT: u32 = 21; + +pub(crate) const REG_BACKPLANE_GPIO_SELECT: u32 = 0x10005; +pub(crate) const REG_BACKPLANE_GPIO_OUTPUT: u32 = 0x10006; +pub(crate) const REG_BACKPLANE_GPIO_ENABLE: u32 = 0x10007; +pub(crate) const REG_BACKPLANE_FUNCTION2_WATERMARK: u32 = 0x10008; +pub(crate) const REG_BACKPLANE_DEVICE_CONTROL: u32 = 0x10009; +pub(crate) const REG_BACKPLANE_BACKPLANE_ADDRESS_LOW: u32 = 0x1000A; +pub(crate) const REG_BACKPLANE_BACKPLANE_ADDRESS_MID: u32 = 0x1000B; +pub(crate) const REG_BACKPLANE_BACKPLANE_ADDRESS_HIGH: u32 = 0x1000C; +pub(crate) const REG_BACKPLANE_FRAME_CONTROL: u32 = 0x1000D; +pub(crate) const REG_BACKPLANE_CHIP_CLOCK_CSR: u32 = 0x1000E; +pub(crate) const REG_BACKPLANE_PULL_UP: u32 = 0x1000F; +pub(crate) const REG_BACKPLANE_READ_FRAME_BC_LOW: u32 = 0x1001B; +pub(crate) const REG_BACKPLANE_READ_FRAME_BC_HIGH: u32 = 0x1001C; +pub(crate) const REG_BACKPLANE_WAKEUP_CTRL: u32 = 0x1001E; +pub(crate) const REG_BACKPLANE_SLEEP_CSR: u32 = 0x1001F; + +pub(crate) const BACKPLANE_WINDOW_SIZE: usize = 0x8000; +pub(crate) const BACKPLANE_ADDRESS_MASK: u32 = 0x7FFF; +pub(crate) const BACKPLANE_ADDRESS_32BIT_FLAG: u32 = 0x08000; +pub(crate) const BACKPLANE_MAX_TRANSFER_SIZE: usize = 64; +// Active Low Power (ALP) clock constants +pub(crate) const BACKPLANE_ALP_AVAIL_REQ: u8 = 0x08; +pub(crate) const BACKPLANE_ALP_AVAIL: u8 = 0x40; + +// Broadcom AMBA (Advanced Microcontroller Bus Architecture) Interconnect +// (AI) pub (crate) constants +pub(crate) const AI_IOCTRL_OFFSET: u32 = 0x408; +pub(crate) const AI_IOCTRL_BIT_FGC: u8 = 0x0002; +pub(crate) const AI_IOCTRL_BIT_CLOCK_EN: u8 = 0x0001; +pub(crate) const AI_IOCTRL_BIT_CPUHALT: u8 = 0x0020; + +pub(crate) const AI_RESETCTRL_OFFSET: u32 = 0x800; +pub(crate) const AI_RESETCTRL_BIT_RESET: u8 = 1; + +pub(crate) const AI_RESETSTATUS_OFFSET: u32 = 0x804; + +pub(crate) const TEST_PATTERN: u32 = 0x12345678; +pub(crate) const FEEDBEAD: u32 = 0xFEEDBEAD; + +// SPI_INTERRUPT_REGISTER and SPI_INTERRUPT_ENABLE_REGISTER Bits +pub(crate) const IRQ_DATA_UNAVAILABLE: u16 = 0x0001; // Requested data not available; Clear by writing a "1" +pub(crate) const IRQ_F2_F3_FIFO_RD_UNDERFLOW: u16 = 0x0002; +pub(crate) const IRQ_F2_F3_FIFO_WR_OVERFLOW: u16 = 0x0004; +pub(crate) const IRQ_COMMAND_ERROR: u16 = 0x0008; // Cleared by writing 1 +pub(crate) const IRQ_DATA_ERROR: u16 = 0x0010; // Cleared by writing 1 +pub(crate) const IRQ_F2_PACKET_AVAILABLE: u16 = 0x0020; +pub(crate) const IRQ_F3_PACKET_AVAILABLE: u16 = 0x0040; +pub(crate) const IRQ_F1_OVERFLOW: u16 = 0x0080; // Due to last write. Bkplane has pending write requests +pub(crate) const IRQ_MISC_INTR0: u16 = 0x0100; +pub(crate) const IRQ_MISC_INTR1: u16 = 0x0200; +pub(crate) const IRQ_MISC_INTR2: u16 = 0x0400; +pub(crate) const IRQ_MISC_INTR3: u16 = 0x0800; +pub(crate) const IRQ_MISC_INTR4: u16 = 0x1000; +pub(crate) const IRQ_F1_INTR: u16 = 0x2000; +pub(crate) const IRQ_F2_INTR: u16 = 0x4000; +pub(crate) const IRQ_F3_INTR: u16 = 0x8000; + +pub(crate) const IOCTL_CMD_UP: u32 = 2; +pub(crate) const IOCTL_CMD_DOWN: u32 = 3; +pub(crate) const IOCTL_CMD_SET_SSID: u32 = 26; +pub(crate) const IOCTL_CMD_SET_CHANNEL: u32 = 30; +pub(crate) const IOCTL_CMD_ANTDIV: u32 = 64; +pub(crate) const IOCTL_CMD_SET_AP: u32 = 118; +pub(crate) const IOCTL_CMD_SET_VAR: u32 = 263; +pub(crate) const IOCTL_CMD_GET_VAR: u32 = 262; +pub(crate) const IOCTL_CMD_SET_PASSPHRASE: u32 = 268; + +pub(crate) const CHANNEL_TYPE_CONTROL: u8 = 0; +pub(crate) const CHANNEL_TYPE_EVENT: u8 = 1; +pub(crate) const CHANNEL_TYPE_DATA: u8 = 2; + +// CYW_SPID command structure constants. +pub(crate) const WRITE: bool = true; +pub(crate) const READ: bool = false; +pub(crate) const INC_ADDR: bool = true; +pub(crate) const FIXED_ADDR: bool = false; + +pub(crate) const AES_ENABLED: u32 = 0x0004; +pub(crate) const WPA2_SECURITY: u32 = 0x00400000; + +pub(crate) const MIN_PSK_LEN: usize = 8; +pub(crate) const MAX_PSK_LEN: usize = 64; + +// Security type (authentication and encryption types are combined using bit mask) +#[allow(non_camel_case_types)] +#[derive(Copy, Clone, PartialEq)] +#[repr(u32)] +pub(crate) enum Security { + OPEN = 0, + WPA2_AES_PSK = WPA2_SECURITY | AES_ENABLED, +} + +#[allow(non_camel_case_types)] +#[derive(Copy, Clone)] +#[repr(u8)] +pub enum EStatus { + /// operation was successful + SUCCESS = 0, + /// operation failed + FAIL = 1, + /// operation timed out + TIMEOUT = 2, + /// failed due to no matching network found + NO_NETWORKS = 3, + /// operation was aborted + ABORT = 4, + /// protocol failure: packet not ack'd + NO_ACK = 5, + /// AUTH or ASSOC packet was unsolicited + UNSOLICITED = 6, + /// attempt to assoc to an auto auth configuration + ATTEMPT = 7, + /// scan results are incomplete + PARTIAL = 8, + /// scan aborted by another scan + NEWSCAN = 9, + /// scan aborted due to assoc in progress + NEWASSOC = 10, + /// 802.11h quiet period started + _11HQUIET = 11, + /// user disabled scanning (WLC_SET_SCANSUPPRESS) + SUPPRESS = 12, + /// no allowable channels to scan + NOCHANS = 13, + /// scan aborted due to CCX fast roam + CCXFASTRM = 14, + /// abort channel select + CS_ABORT = 15, +} + +impl PartialEq for u32 { + fn eq(&self, other: &EStatus) -> bool { + *self == *other as Self + } +} + +#[allow(dead_code)] +pub(crate) struct FormatStatus(pub u32); + +#[cfg(feature = "defmt")] +impl defmt::Format for FormatStatus { + fn format(&self, fmt: defmt::Formatter) { + macro_rules! implm { + ($($name:ident),*) => { + $( + if self.0 & $name > 0 { + defmt::write!(fmt, " | {}", &stringify!($name)[7..]); + } + )* + }; + } + + implm!( + STATUS_DATA_NOT_AVAILABLE, + STATUS_UNDERFLOW, + STATUS_OVERFLOW, + STATUS_F2_INTR, + STATUS_F3_INTR, + STATUS_F2_RX_READY, + STATUS_F3_RX_READY, + STATUS_HOST_CMD_DATA_ERR, + STATUS_F2_PKT_AVAILABLE, + STATUS_F3_PKT_AVAILABLE + ); + } +} + +#[cfg(feature = "log")] +impl core::fmt::Debug for FormatStatus { + fn fmt(&self, fmt: &mut core::fmt::Formatter) -> core::fmt::Result { + macro_rules! implm { + ($($name:ident),*) => { + $( + if self.0 & $name > 0 { + core::write!(fmt, " | {}", &stringify!($name)[7..])?; + } + )* + }; + } + + implm!( + STATUS_DATA_NOT_AVAILABLE, + STATUS_UNDERFLOW, + STATUS_OVERFLOW, + STATUS_F2_INTR, + STATUS_F3_INTR, + STATUS_F2_RX_READY, + STATUS_F3_RX_READY, + STATUS_HOST_CMD_DATA_ERR, + STATUS_F2_PKT_AVAILABLE, + STATUS_F3_PKT_AVAILABLE + ); + Ok(()) + } +} + +#[cfg(feature = "log")] +impl core::fmt::Display for FormatStatus { + fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result { + core::fmt::Debug::fmt(self, f) + } +} + +#[allow(dead_code)] +pub(crate) struct FormatInterrupt(pub u16); + +#[cfg(feature = "defmt")] +impl defmt::Format for FormatInterrupt { + fn format(&self, fmt: defmt::Formatter) { + macro_rules! implm { + ($($name:ident),*) => { + $( + if self.0 & $name > 0 { + defmt::write!(fmt, " | {}", &stringify!($name)[4..]); + } + )* + }; + } + + implm!( + IRQ_DATA_UNAVAILABLE, + IRQ_F2_F3_FIFO_RD_UNDERFLOW, + IRQ_F2_F3_FIFO_WR_OVERFLOW, + IRQ_COMMAND_ERROR, + IRQ_DATA_ERROR, + IRQ_F2_PACKET_AVAILABLE, + IRQ_F3_PACKET_AVAILABLE, + IRQ_F1_OVERFLOW, + IRQ_MISC_INTR0, + IRQ_MISC_INTR1, + IRQ_MISC_INTR2, + IRQ_MISC_INTR3, + IRQ_MISC_INTR4, + IRQ_F1_INTR, + IRQ_F2_INTR, + IRQ_F3_INTR + ); + } +} + +#[cfg(feature = "log")] +impl core::fmt::Debug for FormatInterrupt { + fn fmt(&self, fmt: &mut core::fmt::Formatter) -> core::fmt::Result { + macro_rules! implm { + ($($name:ident),*) => { + $( + if self.0 & $name > 0 { + core::write!(fmt, " | {}", &stringify!($name)[7..])?; + } + )* + }; + } + + implm!( + IRQ_DATA_UNAVAILABLE, + IRQ_F2_F3_FIFO_RD_UNDERFLOW, + IRQ_F2_F3_FIFO_WR_OVERFLOW, + IRQ_COMMAND_ERROR, + IRQ_DATA_ERROR, + IRQ_F2_PACKET_AVAILABLE, + IRQ_F3_PACKET_AVAILABLE, + IRQ_F1_OVERFLOW, + IRQ_MISC_INTR0, + IRQ_MISC_INTR1, + IRQ_MISC_INTR2, + IRQ_MISC_INTR3, + IRQ_MISC_INTR4, + IRQ_F1_INTR, + IRQ_F2_INTR, + IRQ_F3_INTR + ); + Ok(()) + } +} + +#[cfg(feature = "log")] +impl core::fmt::Display for FormatInterrupt { + fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result { + core::fmt::Debug::fmt(self, f) + } +} diff --git a/cyw43/src/control.rs b/cyw43/src/control.rs new file mode 100644 index 00000000..6919d569 --- /dev/null +++ b/cyw43/src/control.rs @@ -0,0 +1,457 @@ +use core::cmp::{max, min}; + +use ch::driver::LinkState; +use embassy_net_driver_channel as ch; +use embassy_time::{Duration, Timer}; + +pub use crate::bus::SpiBusCyw43; +use crate::consts::*; +use crate::events::{Event, EventSubscriber, Events}; +use crate::fmt::Bytes; +use crate::ioctl::{IoctlState, IoctlType}; +use crate::structs::*; +use crate::{countries, events, PowerManagementMode}; + +#[derive(Debug)] +pub struct Error { + pub status: u32, +} + +pub struct Control<'a> { + state_ch: ch::StateRunner<'a>, + events: &'a Events, + ioctl_state: &'a IoctlState, +} + +impl<'a> Control<'a> { + pub(crate) fn new(state_ch: ch::StateRunner<'a>, event_sub: &'a Events, ioctl_state: &'a IoctlState) -> Self { + Self { + state_ch, + events: event_sub, + ioctl_state, + } + } + + pub async fn init(&mut self, clm: &[u8]) { + const CHUNK_SIZE: usize = 1024; + + debug!("Downloading CLM..."); + + let mut offs = 0; + for chunk in clm.chunks(CHUNK_SIZE) { + let mut flag = DOWNLOAD_FLAG_HANDLER_VER; + if offs == 0 { + flag |= DOWNLOAD_FLAG_BEGIN; + } + offs += chunk.len(); + if offs == clm.len() { + flag |= DOWNLOAD_FLAG_END; + } + + let header = DownloadHeader { + flag, + dload_type: DOWNLOAD_TYPE_CLM, + len: chunk.len() as _, + crc: 0, + }; + let mut buf = [0; 8 + 12 + CHUNK_SIZE]; + buf[0..8].copy_from_slice(b"clmload\x00"); + buf[8..20].copy_from_slice(&header.to_bytes()); + buf[20..][..chunk.len()].copy_from_slice(&chunk); + self.ioctl(IoctlType::Set, IOCTL_CMD_SET_VAR, 0, &mut buf[..8 + 12 + chunk.len()]) + .await; + } + + // check clmload ok + assert_eq!(self.get_iovar_u32("clmload_status").await, 0); + + debug!("Configuring misc stuff..."); + + // Disable tx gloming which transfers multiple packets in one request. + // 'glom' is short for "conglomerate" which means "gather together into + // a compact mass". + self.set_iovar_u32("bus:txglom", 0).await; + self.set_iovar_u32("apsta", 1).await; + + // read MAC addr. + let mut mac_addr = [0; 6]; + assert_eq!(self.get_iovar("cur_etheraddr", &mut mac_addr).await, 6); + debug!("mac addr: {:02x}", Bytes(&mac_addr)); + + let country = countries::WORLD_WIDE_XX; + let country_info = CountryInfo { + country_abbrev: [country.code[0], country.code[1], 0, 0], + country_code: [country.code[0], country.code[1], 0, 0], + rev: if country.rev == 0 { -1 } else { country.rev as _ }, + }; + self.set_iovar("country", &country_info.to_bytes()).await; + + // set country takes some time, next ioctls fail if we don't wait. + Timer::after(Duration::from_millis(100)).await; + + // Set antenna to chip antenna + self.ioctl_set_u32(IOCTL_CMD_ANTDIV, 0, 0).await; + + self.set_iovar_u32("bus:txglom", 0).await; + Timer::after(Duration::from_millis(100)).await; + //self.set_iovar_u32("apsta", 1).await; // this crashes, also we already did it before...?? + //Timer::after(Duration::from_millis(100)).await; + self.set_iovar_u32("ampdu_ba_wsize", 8).await; + Timer::after(Duration::from_millis(100)).await; + self.set_iovar_u32("ampdu_mpdu", 4).await; + Timer::after(Duration::from_millis(100)).await; + //self.set_iovar_u32("ampdu_rx_factor", 0).await; // this crashes + + //Timer::after(Duration::from_millis(100)).await; + + // evts + let mut evts = EventMask { + iface: 0, + events: [0xFF; 24], + }; + + // Disable spammy uninteresting events. + evts.unset(Event::RADIO); + evts.unset(Event::IF); + evts.unset(Event::PROBREQ_MSG); + evts.unset(Event::PROBREQ_MSG_RX); + evts.unset(Event::PROBRESP_MSG); + evts.unset(Event::PROBRESP_MSG); + evts.unset(Event::ROAM); + + self.set_iovar("bsscfg:event_msgs", &evts.to_bytes()).await; + + Timer::after(Duration::from_millis(100)).await; + + // set wifi up + self.ioctl(IoctlType::Set, IOCTL_CMD_UP, 0, &mut []).await; + + Timer::after(Duration::from_millis(100)).await; + + self.ioctl_set_u32(110, 0, 1).await; // SET_GMODE = auto + self.ioctl_set_u32(142, 0, 0).await; // SET_BAND = any + + Timer::after(Duration::from_millis(100)).await; + + self.state_ch.set_ethernet_address(mac_addr); + + debug!("INIT DONE"); + } + + pub async fn set_power_management(&mut self, mode: PowerManagementMode) { + // power save mode + let mode_num = mode.mode(); + if mode_num == 2 { + self.set_iovar_u32("pm2_sleep_ret", mode.sleep_ret_ms() as u32).await; + self.set_iovar_u32("bcn_li_bcn", mode.beacon_period() as u32).await; + self.set_iovar_u32("bcn_li_dtim", mode.dtim_period() as u32).await; + self.set_iovar_u32("assoc_listen", mode.assoc() as u32).await; + } + self.ioctl_set_u32(86, 0, mode_num).await; + } + + pub async fn join_open(&mut self, ssid: &str) -> Result<(), Error> { + self.set_iovar_u32("ampdu_ba_wsize", 8).await; + + self.ioctl_set_u32(134, 0, 0).await; // wsec = open + self.set_iovar_u32x2("bsscfg:sup_wpa", 0, 0).await; + self.ioctl_set_u32(20, 0, 1).await; // set_infra = 1 + self.ioctl_set_u32(22, 0, 0).await; // set_auth = open (0) + + let mut i = SsidInfo { + len: ssid.len() as _, + ssid: [0; 32], + }; + i.ssid[..ssid.len()].copy_from_slice(ssid.as_bytes()); + + self.wait_for_join(i).await + } + + pub async fn join_wpa2(&mut self, ssid: &str, passphrase: &str) -> Result<(), Error> { + self.set_iovar_u32("ampdu_ba_wsize", 8).await; + + self.ioctl_set_u32(134, 0, 4).await; // wsec = wpa2 + self.set_iovar_u32x2("bsscfg:sup_wpa", 0, 1).await; + self.set_iovar_u32x2("bsscfg:sup_wpa2_eapver", 0, 0xFFFF_FFFF).await; + self.set_iovar_u32x2("bsscfg:sup_wpa_tmo", 0, 2500).await; + + Timer::after(Duration::from_millis(100)).await; + + let mut pfi = PassphraseInfo { + len: passphrase.len() as _, + flags: 1, + passphrase: [0; 64], + }; + pfi.passphrase[..passphrase.len()].copy_from_slice(passphrase.as_bytes()); + self.ioctl(IoctlType::Set, IOCTL_CMD_SET_PASSPHRASE, 0, &mut pfi.to_bytes()) + .await; // WLC_SET_WSEC_PMK + + self.ioctl_set_u32(20, 0, 1).await; // set_infra = 1 + self.ioctl_set_u32(22, 0, 0).await; // set_auth = 0 (open) + self.ioctl_set_u32(165, 0, 0x80).await; // set_wpa_auth + + let mut i = SsidInfo { + len: ssid.len() as _, + ssid: [0; 32], + }; + i.ssid[..ssid.len()].copy_from_slice(ssid.as_bytes()); + + self.wait_for_join(i).await + } + + async fn wait_for_join(&mut self, i: SsidInfo) -> Result<(), Error> { + self.events.mask.enable(&[Event::SET_SSID, Event::AUTH]); + let mut subscriber = self.events.queue.subscriber().unwrap(); + // the actual join operation starts here + // we make sure to enable events before so we don't miss any + + // set_ssid + self.ioctl(IoctlType::Set, IOCTL_CMD_SET_SSID, 0, &mut i.to_bytes()) + .await; + + // to complete the join, we wait for a SET_SSID event + // we also save the AUTH status for the user, it may be interesting + let mut auth_status = 0; + let status = loop { + let msg = subscriber.next_message_pure().await; + if msg.header.event_type == Event::AUTH && msg.header.status != EStatus::SUCCESS { + auth_status = msg.header.status; + } else if msg.header.event_type == Event::SET_SSID { + // join operation ends with SET_SSID event + break msg.header.status; + } + }; + + self.events.mask.disable_all(); + if status == EStatus::SUCCESS { + // successful join + self.state_ch.set_link_state(LinkState::Up); + debug!("JOINED"); + Ok(()) + } else { + warn!("JOIN failed with status={} auth={}", status, auth_status); + Err(Error { status }) + } + } + + pub async fn gpio_set(&mut self, gpio_n: u8, gpio_en: bool) { + assert!(gpio_n < 3); + self.set_iovar_u32x2("gpioout", 1 << gpio_n, if gpio_en { 1 << gpio_n } else { 0 }) + .await + } + + pub async fn start_ap_open(&mut self, ssid: &str, channel: u8) { + self.start_ap(ssid, "", Security::OPEN, channel).await; + } + + pub async fn start_ap_wpa2(&mut self, ssid: &str, passphrase: &str, channel: u8) { + self.start_ap(ssid, passphrase, Security::WPA2_AES_PSK, channel).await; + } + + async fn start_ap(&mut self, ssid: &str, passphrase: &str, security: Security, channel: u8) { + if security != Security::OPEN + && (passphrase.as_bytes().len() < MIN_PSK_LEN || passphrase.as_bytes().len() > MAX_PSK_LEN) + { + panic!("Passphrase is too short or too long"); + } + + // Temporarily set wifi down + self.ioctl(IoctlType::Set, IOCTL_CMD_DOWN, 0, &mut []).await; + + // Turn off APSTA mode + self.set_iovar_u32("apsta", 0).await; + + // Set wifi up again + self.ioctl(IoctlType::Set, IOCTL_CMD_UP, 0, &mut []).await; + + // Turn on AP mode + self.ioctl_set_u32(IOCTL_CMD_SET_AP, 0, 1).await; + + // Set SSID + let mut i = SsidInfoWithIndex { + index: 0, + ssid_info: SsidInfo { + len: ssid.as_bytes().len() as _, + ssid: [0; 32], + }, + }; + i.ssid_info.ssid[..ssid.as_bytes().len()].copy_from_slice(ssid.as_bytes()); + self.set_iovar("bsscfg:ssid", &i.to_bytes()).await; + + // Set channel number + self.ioctl_set_u32(IOCTL_CMD_SET_CHANNEL, 0, channel as u32).await; + + // Set security + self.set_iovar_u32x2("bsscfg:wsec", 0, (security as u32) & 0xFF).await; + + if security != Security::OPEN { + self.set_iovar_u32x2("bsscfg:wpa_auth", 0, 0x0084).await; // wpa_auth = WPA2_AUTH_PSK | WPA_AUTH_PSK + + Timer::after(Duration::from_millis(100)).await; + + // Set passphrase + let mut pfi = PassphraseInfo { + len: passphrase.as_bytes().len() as _, + flags: 1, // WSEC_PASSPHRASE + passphrase: [0; 64], + }; + pfi.passphrase[..passphrase.as_bytes().len()].copy_from_slice(passphrase.as_bytes()); + self.ioctl(IoctlType::Set, IOCTL_CMD_SET_PASSPHRASE, 0, &mut pfi.to_bytes()) + .await; + } + + // Change mutlicast rate from 1 Mbps to 11 Mbps + self.set_iovar_u32("2g_mrate", 11000000 / 500000).await; + + // Start AP + self.set_iovar_u32x2("bss", 0, 1).await; // bss = BSS_UP + } + + async fn set_iovar_u32x2(&mut self, name: &str, val1: u32, val2: u32) { + let mut buf = [0; 8]; + buf[0..4].copy_from_slice(&val1.to_le_bytes()); + buf[4..8].copy_from_slice(&val2.to_le_bytes()); + self.set_iovar(name, &buf).await + } + + async fn set_iovar_u32(&mut self, name: &str, val: u32) { + self.set_iovar(name, &val.to_le_bytes()).await + } + + async fn get_iovar_u32(&mut self, name: &str) -> u32 { + let mut buf = [0; 4]; + let len = self.get_iovar(name, &mut buf).await; + assert_eq!(len, 4); + u32::from_le_bytes(buf) + } + + async fn set_iovar(&mut self, name: &str, val: &[u8]) { + self.set_iovar_v::<64>(name, val).await + } + + async fn set_iovar_v(&mut self, name: &str, val: &[u8]) { + debug!("set {} = {:02x}", name, Bytes(val)); + + let mut buf = [0; BUFSIZE]; + buf[..name.len()].copy_from_slice(name.as_bytes()); + buf[name.len()] = 0; + buf[name.len() + 1..][..val.len()].copy_from_slice(val); + + let total_len = name.len() + 1 + val.len(); + self.ioctl(IoctlType::Set, IOCTL_CMD_SET_VAR, 0, &mut buf[..total_len]) + .await; + } + + // TODO this is not really working, it always returns all zeros. + async fn get_iovar(&mut self, name: &str, res: &mut [u8]) -> usize { + debug!("get {}", name); + + let mut buf = [0; 64]; + buf[..name.len()].copy_from_slice(name.as_bytes()); + buf[name.len()] = 0; + + let total_len = max(name.len() + 1, res.len()); + let res_len = self + .ioctl(IoctlType::Get, IOCTL_CMD_GET_VAR, 0, &mut buf[..total_len]) + .await; + + let out_len = min(res.len(), res_len); + res[..out_len].copy_from_slice(&buf[..out_len]); + out_len + } + + async fn ioctl_set_u32(&mut self, cmd: u32, iface: u32, val: u32) { + let mut buf = val.to_le_bytes(); + self.ioctl(IoctlType::Set, cmd, iface, &mut buf).await; + } + + async fn ioctl(&mut self, kind: IoctlType, cmd: u32, iface: u32, buf: &mut [u8]) -> usize { + struct CancelOnDrop<'a>(&'a IoctlState); + + impl CancelOnDrop<'_> { + fn defuse(self) { + core::mem::forget(self); + } + } + + impl Drop for CancelOnDrop<'_> { + fn drop(&mut self) { + self.0.cancel_ioctl(); + } + } + + let ioctl = CancelOnDrop(self.ioctl_state); + + ioctl.0.do_ioctl(kind, cmd, iface, buf).await; + let resp_len = ioctl.0.wait_complete().await; + + ioctl.defuse(); + + resp_len + } + + /// Start a wifi scan + /// + /// Returns a `Stream` of networks found by the device + /// + /// # Note + /// Device events are currently implemented using a bounded queue. + /// To not miss any events, you should make sure to always await the stream. + pub async fn scan(&mut self) -> Scanner<'_> { + const SCANTYPE_PASSIVE: u8 = 1; + + let scan_params = ScanParams { + version: 1, + action: 1, + sync_id: 1, + ssid_len: 0, + ssid: [0; 32], + bssid: [0xff; 6], + bss_type: 2, + scan_type: SCANTYPE_PASSIVE, + nprobes: !0, + active_time: !0, + passive_time: !0, + home_time: !0, + channel_num: 0, + channel_list: [0; 1], + }; + + self.events.mask.enable(&[Event::ESCAN_RESULT]); + let subscriber = self.events.queue.subscriber().unwrap(); + self.set_iovar_v::<256>("escan", &scan_params.to_bytes()).await; + + Scanner { + subscriber, + events: &self.events, + } + } +} + +pub struct Scanner<'a> { + subscriber: EventSubscriber<'a>, + events: &'a Events, +} + +impl Scanner<'_> { + /// wait for the next found network + pub async fn next(&mut self) -> Option { + let event = self.subscriber.next_message_pure().await; + if event.header.status != EStatus::PARTIAL { + self.events.mask.disable_all(); + return None; + } + + if let events::Payload::BssInfo(bss) = event.payload { + Some(bss) + } else { + None + } + } +} + +impl Drop for Scanner<'_> { + fn drop(&mut self) { + self.events.mask.disable_all(); + } +} diff --git a/cyw43/src/countries.rs b/cyw43/src/countries.rs new file mode 100644 index 00000000..fa1e8cac --- /dev/null +++ b/cyw43/src/countries.rs @@ -0,0 +1,481 @@ +#![allow(unused)] + +pub struct Country { + pub code: [u8; 2], + pub rev: u16, +} + +/// AF Afghanistan +pub const AFGHANISTAN: Country = Country { code: *b"AF", rev: 0 }; +/// AL Albania +pub const ALBANIA: Country = Country { code: *b"AL", rev: 0 }; +/// DZ Algeria +pub const ALGERIA: Country = Country { code: *b"DZ", rev: 0 }; +/// AS American_Samoa +pub const AMERICAN_SAMOA: Country = Country { code: *b"AS", rev: 0 }; +/// AO Angola +pub const ANGOLA: Country = Country { code: *b"AO", rev: 0 }; +/// AI Anguilla +pub const ANGUILLA: Country = Country { code: *b"AI", rev: 0 }; +/// AG Antigua_and_Barbuda +pub const ANTIGUA_AND_BARBUDA: Country = Country { code: *b"AG", rev: 0 }; +/// AR Argentina +pub const ARGENTINA: Country = Country { code: *b"AR", rev: 0 }; +/// AM Armenia +pub const ARMENIA: Country = Country { code: *b"AM", rev: 0 }; +/// AW Aruba +pub const ARUBA: Country = Country { code: *b"AW", rev: 0 }; +/// AU Australia +pub const AUSTRALIA: Country = Country { code: *b"AU", rev: 0 }; +/// AT Austria +pub const AUSTRIA: Country = Country { code: *b"AT", rev: 0 }; +/// AZ Azerbaijan +pub const AZERBAIJAN: Country = Country { code: *b"AZ", rev: 0 }; +/// BS Bahamas +pub const BAHAMAS: Country = Country { code: *b"BS", rev: 0 }; +/// BH Bahrain +pub const BAHRAIN: Country = Country { code: *b"BH", rev: 0 }; +/// 0B Baker_Island +pub const BAKER_ISLAND: Country = Country { code: *b"0B", rev: 0 }; +/// BD Bangladesh +pub const BANGLADESH: Country = Country { code: *b"BD", rev: 0 }; +/// BB Barbados +pub const BARBADOS: Country = Country { code: *b"BB", rev: 0 }; +/// BY Belarus +pub const BELARUS: Country = Country { code: *b"BY", rev: 0 }; +/// BE Belgium +pub const BELGIUM: Country = Country { code: *b"BE", rev: 0 }; +/// BZ Belize +pub const BELIZE: Country = Country { code: *b"BZ", rev: 0 }; +/// BJ Benin +pub const BENIN: Country = Country { code: *b"BJ", rev: 0 }; +/// BM Bermuda +pub const BERMUDA: Country = Country { code: *b"BM", rev: 0 }; +/// BT Bhutan +pub const BHUTAN: Country = Country { code: *b"BT", rev: 0 }; +/// BO Bolivia +pub const BOLIVIA: Country = Country { code: *b"BO", rev: 0 }; +/// BA Bosnia_and_Herzegovina +pub const BOSNIA_AND_HERZEGOVINA: Country = Country { code: *b"BA", rev: 0 }; +/// BW Botswana +pub const BOTSWANA: Country = Country { code: *b"BW", rev: 0 }; +/// BR Brazil +pub const BRAZIL: Country = Country { code: *b"BR", rev: 0 }; +/// IO British_Indian_Ocean_Territory +pub const BRITISH_INDIAN_OCEAN_TERRITORY: Country = Country { code: *b"IO", rev: 0 }; +/// BN Brunei_Darussalam +pub const BRUNEI_DARUSSALAM: Country = Country { code: *b"BN", rev: 0 }; +/// BG Bulgaria +pub const BULGARIA: Country = Country { code: *b"BG", rev: 0 }; +/// BF Burkina_Faso +pub const BURKINA_FASO: Country = Country { code: *b"BF", rev: 0 }; +/// BI Burundi +pub const BURUNDI: Country = Country { code: *b"BI", rev: 0 }; +/// KH Cambodia +pub const CAMBODIA: Country = Country { code: *b"KH", rev: 0 }; +/// CM Cameroon +pub const CAMEROON: Country = Country { code: *b"CM", rev: 0 }; +/// CA Canada +pub const CANADA: Country = Country { code: *b"CA", rev: 0 }; +/// CA Canada Revision 950 +pub const CANADA_REV950: Country = Country { code: *b"CA", rev: 950 }; +/// CV Cape_Verde +pub const CAPE_VERDE: Country = Country { code: *b"CV", rev: 0 }; +/// KY Cayman_Islands +pub const CAYMAN_ISLANDS: Country = Country { code: *b"KY", rev: 0 }; +/// CF Central_African_Republic +pub const CENTRAL_AFRICAN_REPUBLIC: Country = Country { code: *b"CF", rev: 0 }; +/// TD Chad +pub const CHAD: Country = Country { code: *b"TD", rev: 0 }; +/// CL Chile +pub const CHILE: Country = Country { code: *b"CL", rev: 0 }; +/// CN China +pub const CHINA: Country = Country { code: *b"CN", rev: 0 }; +/// CX Christmas_Island +pub const CHRISTMAS_ISLAND: Country = Country { code: *b"CX", rev: 0 }; +/// CO Colombia +pub const COLOMBIA: Country = Country { code: *b"CO", rev: 0 }; +/// KM Comoros +pub const COMOROS: Country = Country { code: *b"KM", rev: 0 }; +/// CG Congo +pub const CONGO: Country = Country { code: *b"CG", rev: 0 }; +/// CD Congo,_The_Democratic_Republic_Of_The +pub const CONGO_THE_DEMOCRATIC_REPUBLIC_OF_THE: Country = Country { code: *b"CD", rev: 0 }; +/// CR Costa_Rica +pub const COSTA_RICA: Country = Country { code: *b"CR", rev: 0 }; +/// CI Cote_D'ivoire +pub const COTE_DIVOIRE: Country = Country { code: *b"CI", rev: 0 }; +/// HR Croatia +pub const CROATIA: Country = Country { code: *b"HR", rev: 0 }; +/// CU Cuba +pub const CUBA: Country = Country { code: *b"CU", rev: 0 }; +/// CY Cyprus +pub const CYPRUS: Country = Country { code: *b"CY", rev: 0 }; +/// CZ Czech_Republic +pub const CZECH_REPUBLIC: Country = Country { code: *b"CZ", rev: 0 }; +/// DK Denmark +pub const DENMARK: Country = Country { code: *b"DK", rev: 0 }; +/// DJ Djibouti +pub const DJIBOUTI: Country = Country { code: *b"DJ", rev: 0 }; +/// DM Dominica +pub const DOMINICA: Country = Country { code: *b"DM", rev: 0 }; +/// DO Dominican_Republic +pub const DOMINICAN_REPUBLIC: Country = Country { code: *b"DO", rev: 0 }; +/// AU G'Day mate! +pub const DOWN_UNDER: Country = Country { code: *b"AU", rev: 0 }; +/// EC Ecuador +pub const ECUADOR: Country = Country { code: *b"EC", rev: 0 }; +/// EG Egypt +pub const EGYPT: Country = Country { code: *b"EG", rev: 0 }; +/// SV El_Salvador +pub const EL_SALVADOR: Country = Country { code: *b"SV", rev: 0 }; +/// GQ Equatorial_Guinea +pub const EQUATORIAL_GUINEA: Country = Country { code: *b"GQ", rev: 0 }; +/// ER Eritrea +pub const ERITREA: Country = Country { code: *b"ER", rev: 0 }; +/// EE Estonia +pub const ESTONIA: Country = Country { code: *b"EE", rev: 0 }; +/// ET Ethiopia +pub const ETHIOPIA: Country = Country { code: *b"ET", rev: 0 }; +/// FK Falkland_Islands_(Malvinas) +pub const FALKLAND_ISLANDS_MALVINAS: Country = Country { code: *b"FK", rev: 0 }; +/// FO Faroe_Islands +pub const FAROE_ISLANDS: Country = Country { code: *b"FO", rev: 0 }; +/// FJ Fiji +pub const FIJI: Country = Country { code: *b"FJ", rev: 0 }; +/// FI Finland +pub const FINLAND: Country = Country { code: *b"FI", rev: 0 }; +/// FR France +pub const FRANCE: Country = Country { code: *b"FR", rev: 0 }; +/// GF French_Guina +pub const FRENCH_GUINA: Country = Country { code: *b"GF", rev: 0 }; +/// PF French_Polynesia +pub const FRENCH_POLYNESIA: Country = Country { code: *b"PF", rev: 0 }; +/// TF French_Southern_Territories +pub const FRENCH_SOUTHERN_TERRITORIES: Country = Country { code: *b"TF", rev: 0 }; +/// GA Gabon +pub const GABON: Country = Country { code: *b"GA", rev: 0 }; +/// GM Gambia +pub const GAMBIA: Country = Country { code: *b"GM", rev: 0 }; +/// GE Georgia +pub const GEORGIA: Country = Country { code: *b"GE", rev: 0 }; +/// DE Germany +pub const GERMANY: Country = Country { code: *b"DE", rev: 0 }; +/// E0 European_Wide Revision 895 +pub const EUROPEAN_WIDE_REV895: Country = Country { code: *b"E0", rev: 895 }; +/// GH Ghana +pub const GHANA: Country = Country { code: *b"GH", rev: 0 }; +/// GI Gibraltar +pub const GIBRALTAR: Country = Country { code: *b"GI", rev: 0 }; +/// GR Greece +pub const GREECE: Country = Country { code: *b"GR", rev: 0 }; +/// GD Grenada +pub const GRENADA: Country = Country { code: *b"GD", rev: 0 }; +/// GP Guadeloupe +pub const GUADELOUPE: Country = Country { code: *b"GP", rev: 0 }; +/// GU Guam +pub const GUAM: Country = Country { code: *b"GU", rev: 0 }; +/// GT Guatemala +pub const GUATEMALA: Country = Country { code: *b"GT", rev: 0 }; +/// GG Guernsey +pub const GUERNSEY: Country = Country { code: *b"GG", rev: 0 }; +/// GN Guinea +pub const GUINEA: Country = Country { code: *b"GN", rev: 0 }; +/// GW Guinea-bissau +pub const GUINEA_BISSAU: Country = Country { code: *b"GW", rev: 0 }; +/// GY Guyana +pub const GUYANA: Country = Country { code: *b"GY", rev: 0 }; +/// HT Haiti +pub const HAITI: Country = Country { code: *b"HT", rev: 0 }; +/// VA Holy_See_(Vatican_City_State) +pub const HOLY_SEE_VATICAN_CITY_STATE: Country = Country { code: *b"VA", rev: 0 }; +/// HN Honduras +pub const HONDURAS: Country = Country { code: *b"HN", rev: 0 }; +/// HK Hong_Kong +pub const HONG_KONG: Country = Country { code: *b"HK", rev: 0 }; +/// HU Hungary +pub const HUNGARY: Country = Country { code: *b"HU", rev: 0 }; +/// IS Iceland +pub const ICELAND: Country = Country { code: *b"IS", rev: 0 }; +/// IN India +pub const INDIA: Country = Country { code: *b"IN", rev: 0 }; +/// ID Indonesia +pub const INDONESIA: Country = Country { code: *b"ID", rev: 0 }; +/// IR Iran,_Islamic_Republic_Of +pub const IRAN_ISLAMIC_REPUBLIC_OF: Country = Country { code: *b"IR", rev: 0 }; +/// IQ Iraq +pub const IRAQ: Country = Country { code: *b"IQ", rev: 0 }; +/// IE Ireland +pub const IRELAND: Country = Country { code: *b"IE", rev: 0 }; +/// IL Israel +pub const ISRAEL: Country = Country { code: *b"IL", rev: 0 }; +/// IT Italy +pub const ITALY: Country = Country { code: *b"IT", rev: 0 }; +/// JM Jamaica +pub const JAMAICA: Country = Country { code: *b"JM", rev: 0 }; +/// JP Japan +pub const JAPAN: Country = Country { code: *b"JP", rev: 0 }; +/// JE Jersey +pub const JERSEY: Country = Country { code: *b"JE", rev: 0 }; +/// JO Jordan +pub const JORDAN: Country = Country { code: *b"JO", rev: 0 }; +/// KZ Kazakhstan +pub const KAZAKHSTAN: Country = Country { code: *b"KZ", rev: 0 }; +/// KE Kenya +pub const KENYA: Country = Country { code: *b"KE", rev: 0 }; +/// KI Kiribati +pub const KIRIBATI: Country = Country { code: *b"KI", rev: 0 }; +/// KR Korea,_Republic_Of +pub const KOREA_REPUBLIC_OF: Country = Country { code: *b"KR", rev: 1 }; +/// 0A Kosovo +pub const KOSOVO: Country = Country { code: *b"0A", rev: 0 }; +/// KW Kuwait +pub const KUWAIT: Country = Country { code: *b"KW", rev: 0 }; +/// KG Kyrgyzstan +pub const KYRGYZSTAN: Country = Country { code: *b"KG", rev: 0 }; +/// LA Lao_People's_Democratic_Repubic +pub const LAO_PEOPLES_DEMOCRATIC_REPUBIC: Country = Country { code: *b"LA", rev: 0 }; +/// LV Latvia +pub const LATVIA: Country = Country { code: *b"LV", rev: 0 }; +/// LB Lebanon +pub const LEBANON: Country = Country { code: *b"LB", rev: 0 }; +/// LS Lesotho +pub const LESOTHO: Country = Country { code: *b"LS", rev: 0 }; +/// LR Liberia +pub const LIBERIA: Country = Country { code: *b"LR", rev: 0 }; +/// LY Libyan_Arab_Jamahiriya +pub const LIBYAN_ARAB_JAMAHIRIYA: Country = Country { code: *b"LY", rev: 0 }; +/// LI Liechtenstein +pub const LIECHTENSTEIN: Country = Country { code: *b"LI", rev: 0 }; +/// LT Lithuania +pub const LITHUANIA: Country = Country { code: *b"LT", rev: 0 }; +/// LU Luxembourg +pub const LUXEMBOURG: Country = Country { code: *b"LU", rev: 0 }; +/// MO Macao +pub const MACAO: Country = Country { code: *b"MO", rev: 0 }; +/// MK Macedonia,_Former_Yugoslav_Republic_Of +pub const MACEDONIA_FORMER_YUGOSLAV_REPUBLIC_OF: Country = Country { code: *b"MK", rev: 0 }; +/// MG Madagascar +pub const MADAGASCAR: Country = Country { code: *b"MG", rev: 0 }; +/// MW Malawi +pub const MALAWI: Country = Country { code: *b"MW", rev: 0 }; +/// MY Malaysia +pub const MALAYSIA: Country = Country { code: *b"MY", rev: 0 }; +/// MV Maldives +pub const MALDIVES: Country = Country { code: *b"MV", rev: 0 }; +/// ML Mali +pub const MALI: Country = Country { code: *b"ML", rev: 0 }; +/// MT Malta +pub const MALTA: Country = Country { code: *b"MT", rev: 0 }; +/// IM Man,_Isle_Of +pub const MAN_ISLE_OF: Country = Country { code: *b"IM", rev: 0 }; +/// MQ Martinique +pub const MARTINIQUE: Country = Country { code: *b"MQ", rev: 0 }; +/// MR Mauritania +pub const MAURITANIA: Country = Country { code: *b"MR", rev: 0 }; +/// MU Mauritius +pub const MAURITIUS: Country = Country { code: *b"MU", rev: 0 }; +/// YT Mayotte +pub const MAYOTTE: Country = Country { code: *b"YT", rev: 0 }; +/// MX Mexico +pub const MEXICO: Country = Country { code: *b"MX", rev: 0 }; +/// FM Micronesia,_Federated_States_Of +pub const MICRONESIA_FEDERATED_STATES_OF: Country = Country { code: *b"FM", rev: 0 }; +/// MD Moldova,_Republic_Of +pub const MOLDOVA_REPUBLIC_OF: Country = Country { code: *b"MD", rev: 0 }; +/// MC Monaco +pub const MONACO: Country = Country { code: *b"MC", rev: 0 }; +/// MN Mongolia +pub const MONGOLIA: Country = Country { code: *b"MN", rev: 0 }; +/// ME Montenegro +pub const MONTENEGRO: Country = Country { code: *b"ME", rev: 0 }; +/// MS Montserrat +pub const MONTSERRAT: Country = Country { code: *b"MS", rev: 0 }; +/// MA Morocco +pub const MOROCCO: Country = Country { code: *b"MA", rev: 0 }; +/// MZ Mozambique +pub const MOZAMBIQUE: Country = Country { code: *b"MZ", rev: 0 }; +/// MM Myanmar +pub const MYANMAR: Country = Country { code: *b"MM", rev: 0 }; +/// NA Namibia +pub const NAMIBIA: Country = Country { code: *b"NA", rev: 0 }; +/// NR Nauru +pub const NAURU: Country = Country { code: *b"NR", rev: 0 }; +/// NP Nepal +pub const NEPAL: Country = Country { code: *b"NP", rev: 0 }; +/// NL Netherlands +pub const NETHERLANDS: Country = Country { code: *b"NL", rev: 0 }; +/// AN Netherlands_Antilles +pub const NETHERLANDS_ANTILLES: Country = Country { code: *b"AN", rev: 0 }; +/// NC New_Caledonia +pub const NEW_CALEDONIA: Country = Country { code: *b"NC", rev: 0 }; +/// NZ New_Zealand +pub const NEW_ZEALAND: Country = Country { code: *b"NZ", rev: 0 }; +/// NI Nicaragua +pub const NICARAGUA: Country = Country { code: *b"NI", rev: 0 }; +/// NE Niger +pub const NIGER: Country = Country { code: *b"NE", rev: 0 }; +/// NG Nigeria +pub const NIGERIA: Country = Country { code: *b"NG", rev: 0 }; +/// NF Norfolk_Island +pub const NORFOLK_ISLAND: Country = Country { code: *b"NF", rev: 0 }; +/// MP Northern_Mariana_Islands +pub const NORTHERN_MARIANA_ISLANDS: Country = Country { code: *b"MP", rev: 0 }; +/// NO Norway +pub const NORWAY: Country = Country { code: *b"NO", rev: 0 }; +/// OM Oman +pub const OMAN: Country = Country { code: *b"OM", rev: 0 }; +/// PK Pakistan +pub const PAKISTAN: Country = Country { code: *b"PK", rev: 0 }; +/// PW Palau +pub const PALAU: Country = Country { code: *b"PW", rev: 0 }; +/// PA Panama +pub const PANAMA: Country = Country { code: *b"PA", rev: 0 }; +/// PG Papua_New_Guinea +pub const PAPUA_NEW_GUINEA: Country = Country { code: *b"PG", rev: 0 }; +/// PY Paraguay +pub const PARAGUAY: Country = Country { code: *b"PY", rev: 0 }; +/// PE Peru +pub const PERU: Country = Country { code: *b"PE", rev: 0 }; +/// PH Philippines +pub const PHILIPPINES: Country = Country { code: *b"PH", rev: 0 }; +/// PL Poland +pub const POLAND: Country = Country { code: *b"PL", rev: 0 }; +/// PT Portugal +pub const PORTUGAL: Country = Country { code: *b"PT", rev: 0 }; +/// PR Pueto_Rico +pub const PUETO_RICO: Country = Country { code: *b"PR", rev: 0 }; +/// QA Qatar +pub const QATAR: Country = Country { code: *b"QA", rev: 0 }; +/// RE Reunion +pub const REUNION: Country = Country { code: *b"RE", rev: 0 }; +/// RO Romania +pub const ROMANIA: Country = Country { code: *b"RO", rev: 0 }; +/// RU Russian_Federation +pub const RUSSIAN_FEDERATION: Country = Country { code: *b"RU", rev: 0 }; +/// RW Rwanda +pub const RWANDA: Country = Country { code: *b"RW", rev: 0 }; +/// KN Saint_Kitts_and_Nevis +pub const SAINT_KITTS_AND_NEVIS: Country = Country { code: *b"KN", rev: 0 }; +/// LC Saint_Lucia +pub const SAINT_LUCIA: Country = Country { code: *b"LC", rev: 0 }; +/// PM Saint_Pierre_and_Miquelon +pub const SAINT_PIERRE_AND_MIQUELON: Country = Country { code: *b"PM", rev: 0 }; +/// VC Saint_Vincent_and_The_Grenadines +pub const SAINT_VINCENT_AND_THE_GRENADINES: Country = Country { code: *b"VC", rev: 0 }; +/// WS Samoa +pub const SAMOA: Country = Country { code: *b"WS", rev: 0 }; +/// MF Sanit_Martin_/_Sint_Marteen +pub const SANIT_MARTIN_SINT_MARTEEN: Country = Country { code: *b"MF", rev: 0 }; +/// ST Sao_Tome_and_Principe +pub const SAO_TOME_AND_PRINCIPE: Country = Country { code: *b"ST", rev: 0 }; +/// SA Saudi_Arabia +pub const SAUDI_ARABIA: Country = Country { code: *b"SA", rev: 0 }; +/// SN Senegal +pub const SENEGAL: Country = Country { code: *b"SN", rev: 0 }; +/// RS Serbia +pub const SERBIA: Country = Country { code: *b"RS", rev: 0 }; +/// SC Seychelles +pub const SEYCHELLES: Country = Country { code: *b"SC", rev: 0 }; +/// SL Sierra_Leone +pub const SIERRA_LEONE: Country = Country { code: *b"SL", rev: 0 }; +/// SG Singapore +pub const SINGAPORE: Country = Country { code: *b"SG", rev: 0 }; +/// SK Slovakia +pub const SLOVAKIA: Country = Country { code: *b"SK", rev: 0 }; +/// SI Slovenia +pub const SLOVENIA: Country = Country { code: *b"SI", rev: 0 }; +/// SB Solomon_Islands +pub const SOLOMON_ISLANDS: Country = Country { code: *b"SB", rev: 0 }; +/// SO Somalia +pub const SOMALIA: Country = Country { code: *b"SO", rev: 0 }; +/// ZA South_Africa +pub const SOUTH_AFRICA: Country = Country { code: *b"ZA", rev: 0 }; +/// ES Spain +pub const SPAIN: Country = Country { code: *b"ES", rev: 0 }; +/// LK Sri_Lanka +pub const SRI_LANKA: Country = Country { code: *b"LK", rev: 0 }; +/// SR Suriname +pub const SURINAME: Country = Country { code: *b"SR", rev: 0 }; +/// SZ Swaziland +pub const SWAZILAND: Country = Country { code: *b"SZ", rev: 0 }; +/// SE Sweden +pub const SWEDEN: Country = Country { code: *b"SE", rev: 0 }; +/// CH Switzerland +pub const SWITZERLAND: Country = Country { code: *b"CH", rev: 0 }; +/// SY Syrian_Arab_Republic +pub const SYRIAN_ARAB_REPUBLIC: Country = Country { code: *b"SY", rev: 0 }; +/// TW Taiwan,_Province_Of_China +pub const TAIWAN_PROVINCE_OF_CHINA: Country = Country { code: *b"TW", rev: 0 }; +/// TJ Tajikistan +pub const TAJIKISTAN: Country = Country { code: *b"TJ", rev: 0 }; +/// TZ Tanzania,_United_Republic_Of +pub const TANZANIA_UNITED_REPUBLIC_OF: Country = Country { code: *b"TZ", rev: 0 }; +/// TH Thailand +pub const THAILAND: Country = Country { code: *b"TH", rev: 0 }; +/// TG Togo +pub const TOGO: Country = Country { code: *b"TG", rev: 0 }; +/// TO Tonga +pub const TONGA: Country = Country { code: *b"TO", rev: 0 }; +/// TT Trinidad_and_Tobago +pub const TRINIDAD_AND_TOBAGO: Country = Country { code: *b"TT", rev: 0 }; +/// TN Tunisia +pub const TUNISIA: Country = Country { code: *b"TN", rev: 0 }; +/// TR Turkey +pub const TURKEY: Country = Country { code: *b"TR", rev: 0 }; +/// TM Turkmenistan +pub const TURKMENISTAN: Country = Country { code: *b"TM", rev: 0 }; +/// TC Turks_and_Caicos_Islands +pub const TURKS_AND_CAICOS_ISLANDS: Country = Country { code: *b"TC", rev: 0 }; +/// TV Tuvalu +pub const TUVALU: Country = Country { code: *b"TV", rev: 0 }; +/// UG Uganda +pub const UGANDA: Country = Country { code: *b"UG", rev: 0 }; +/// UA Ukraine +pub const UKRAINE: Country = Country { code: *b"UA", rev: 0 }; +/// AE United_Arab_Emirates +pub const UNITED_ARAB_EMIRATES: Country = Country { code: *b"AE", rev: 0 }; +/// GB United_Kingdom +pub const UNITED_KINGDOM: Country = Country { code: *b"GB", rev: 0 }; +/// US United_States +pub const UNITED_STATES: Country = Country { code: *b"US", rev: 0 }; +/// US United_States Revision 4 +pub const UNITED_STATES_REV4: Country = Country { code: *b"US", rev: 4 }; +/// Q1 United_States Revision 931 +pub const UNITED_STATES_REV931: Country = Country { code: *b"Q1", rev: 931 }; +/// Q2 United_States_(No_DFS) +pub const UNITED_STATES_NO_DFS: Country = Country { code: *b"Q2", rev: 0 }; +/// UM United_States_Minor_Outlying_Islands +pub const UNITED_STATES_MINOR_OUTLYING_ISLANDS: Country = Country { code: *b"UM", rev: 0 }; +/// UY Uruguay +pub const URUGUAY: Country = Country { code: *b"UY", rev: 0 }; +/// UZ Uzbekistan +pub const UZBEKISTAN: Country = Country { code: *b"UZ", rev: 0 }; +/// VU Vanuatu +pub const VANUATU: Country = Country { code: *b"VU", rev: 0 }; +/// VE Venezuela +pub const VENEZUELA: Country = Country { code: *b"VE", rev: 0 }; +/// VN Viet_Nam +pub const VIET_NAM: Country = Country { code: *b"VN", rev: 0 }; +/// VG Virgin_Islands,_British +pub const VIRGIN_ISLANDS_BRITISH: Country = Country { code: *b"VG", rev: 0 }; +/// VI Virgin_Islands,_U.S. +pub const VIRGIN_ISLANDS_US: Country = Country { code: *b"VI", rev: 0 }; +/// WF Wallis_and_Futuna +pub const WALLIS_AND_FUTUNA: Country = Country { code: *b"WF", rev: 0 }; +/// 0C West_Bank +pub const WEST_BANK: Country = Country { code: *b"0C", rev: 0 }; +/// EH Western_Sahara +pub const WESTERN_SAHARA: Country = Country { code: *b"EH", rev: 0 }; +/// Worldwide Locale Revision 983 +pub const WORLD_WIDE_XV_REV983: Country = Country { code: *b"XV", rev: 983 }; +/// Worldwide Locale (passive Ch12-14) +pub const WORLD_WIDE_XX: Country = Country { code: *b"XX", rev: 0 }; +/// Worldwide Locale (passive Ch12-14) Revision 17 +pub const WORLD_WIDE_XX_REV17: Country = Country { code: *b"XX", rev: 17 }; +/// YE Yemen +pub const YEMEN: Country = Country { code: *b"YE", rev: 0 }; +/// ZM Zambia +pub const ZAMBIA: Country = Country { code: *b"ZM", rev: 0 }; +/// ZW Zimbabwe +pub const ZIMBABWE: Country = Country { code: *b"ZW", rev: 0 }; diff --git a/cyw43/src/events.rs b/cyw43/src/events.rs new file mode 100644 index 00000000..a94c49a0 --- /dev/null +++ b/cyw43/src/events.rs @@ -0,0 +1,400 @@ +#![allow(dead_code)] +#![allow(non_camel_case_types)] + +use core::cell::RefCell; + +use embassy_sync::blocking_mutex::raw::NoopRawMutex; +use embassy_sync::pubsub::{PubSubChannel, Subscriber}; + +use crate::structs::BssInfo; + +#[derive(Debug, Clone, Copy, PartialEq, Eq, num_enum::FromPrimitive)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +#[repr(u8)] +pub enum Event { + #[num_enum(default)] + Unknown = 0xFF, + /// indicates status of set SSID + SET_SSID = 0, + /// differentiates join IBSS from found (START) IBSS + JOIN = 1, + /// STA founded an IBSS or AP started a BSS + START = 2, + /// 802.11 AUTH request + AUTH = 3, + /// 802.11 AUTH indication + AUTH_IND = 4, + /// 802.11 DEAUTH request + DEAUTH = 5, + /// 802.11 DEAUTH indication + DEAUTH_IND = 6, + /// 802.11 ASSOC request + ASSOC = 7, + /// 802.11 ASSOC indication + ASSOC_IND = 8, + /// 802.11 REASSOC request + REASSOC = 9, + /// 802.11 REASSOC indication + REASSOC_IND = 10, + /// 802.11 DISASSOC request + DISASSOC = 11, + /// 802.11 DISASSOC indication + DISASSOC_IND = 12, + /// 802.11h Quiet period started + QUIET_START = 13, + /// 802.11h Quiet period ended + QUIET_END = 14, + /// BEACONS received/lost indication + BEACON_RX = 15, + /// generic link indication + LINK = 16, + /// TKIP MIC error occurred + MIC_ERROR = 17, + /// NDIS style link indication + NDIS_LINK = 18, + /// roam attempt occurred: indicate status & reason + ROAM = 19, + /// change in dot11FailedCount (txfail) + TXFAIL = 20, + /// WPA2 pmkid cache indication + PMKID_CACHE = 21, + /// current AP's TSF value went backward + RETROGRADE_TSF = 22, + /// AP was pruned from join list for reason + PRUNE = 23, + /// report AutoAuth table entry match for join attempt + AUTOAUTH = 24, + /// Event encapsulating an EAPOL message + EAPOL_MSG = 25, + /// Scan results are ready or scan was aborted + SCAN_COMPLETE = 26, + /// indicate to host addts fail/success + ADDTS_IND = 27, + /// indicate to host delts fail/success + DELTS_IND = 28, + /// indicate to host of beacon transmit + BCNSENT_IND = 29, + /// Send the received beacon up to the host + BCNRX_MSG = 30, + /// indicate to host loss of beacon + BCNLOST_MSG = 31, + /// before attempting to roam + ROAM_PREP = 32, + /// PFN network found event + PFN_NET_FOUND = 33, + /// PFN network lost event + PFN_NET_LOST = 34, + RESET_COMPLETE = 35, + JOIN_START = 36, + ROAM_START = 37, + ASSOC_START = 38, + IBSS_ASSOC = 39, + RADIO = 40, + /// PSM microcode watchdog fired + PSM_WATCHDOG = 41, + /// CCX association start + CCX_ASSOC_START = 42, + /// CCX association abort + CCX_ASSOC_ABORT = 43, + /// probe request received + PROBREQ_MSG = 44, + SCAN_CONFIRM_IND = 45, + /// WPA Handshake + PSK_SUP = 46, + COUNTRY_CODE_CHANGED = 47, + /// WMMAC excedded medium time + EXCEEDED_MEDIUM_TIME = 48, + /// WEP ICV error occurred + ICV_ERROR = 49, + /// Unsupported unicast encrypted frame + UNICAST_DECODE_ERROR = 50, + /// Unsupported multicast encrypted frame + MULTICAST_DECODE_ERROR = 51, + TRACE = 52, + /// BT-AMP HCI event + BTA_HCI_EVENT = 53, + /// I/F change (for wlan host notification) + IF = 54, + /// P2P Discovery listen state expires + P2P_DISC_LISTEN_COMPLETE = 55, + /// indicate RSSI change based on configured levels + RSSI = 56, + /// PFN best network batching event + PFN_BEST_BATCHING = 57, + EXTLOG_MSG = 58, + /// Action frame reception + ACTION_FRAME = 59, + /// Action frame Tx complete + ACTION_FRAME_COMPLETE = 60, + /// assoc request received + PRE_ASSOC_IND = 61, + /// re-assoc request received + PRE_REASSOC_IND = 62, + /// channel adopted (xxx: obsoleted) + CHANNEL_ADOPTED = 63, + /// AP started + AP_STARTED = 64, + /// AP stopped due to DFS + DFS_AP_STOP = 65, + /// AP resumed due to DFS + DFS_AP_RESUME = 66, + /// WAI stations event + WAI_STA_EVENT = 67, + /// event encapsulating an WAI message + WAI_MSG = 68, + /// escan result event + ESCAN_RESULT = 69, + /// action frame off channel complete + ACTION_FRAME_OFF_CHAN_COMPLETE = 70, + /// probe response received + PROBRESP_MSG = 71, + /// P2P Probe request received + P2P_PROBREQ_MSG = 72, + DCS_REQUEST = 73, + /// credits for D11 FIFOs. [AC0,AC1,AC2,AC3,BC_MC,ATIM] + FIFO_CREDIT_MAP = 74, + /// Received action frame event WITH wl_event_rx_frame_data_t header + ACTION_FRAME_RX = 75, + /// Wake Event timer fired, used for wake WLAN test mode + WAKE_EVENT = 76, + /// Radio measurement complete + RM_COMPLETE = 77, + /// Synchronize TSF with the host + HTSFSYNC = 78, + /// request an overlay IOCTL/iovar from the host + OVERLAY_REQ = 79, + CSA_COMPLETE_IND = 80, + /// excess PM Wake Event to inform host + EXCESS_PM_WAKE_EVENT = 81, + /// no PFN networks around + PFN_SCAN_NONE = 82, + /// last found PFN network gets lost + PFN_SCAN_ALLGONE = 83, + GTK_PLUMBED = 84, + /// 802.11 ASSOC indication for NDIS only + ASSOC_IND_NDIS = 85, + /// 802.11 REASSOC indication for NDIS only + REASSOC_IND_NDIS = 86, + ASSOC_REQ_IE = 87, + ASSOC_RESP_IE = 88, + /// association recreated on resume + ASSOC_RECREATED = 89, + /// rx action frame event for NDIS only + ACTION_FRAME_RX_NDIS = 90, + /// authentication request received + AUTH_REQ = 91, + /// fast assoc recreation failed + SPEEDY_RECREATE_FAIL = 93, + /// port-specific event and payload (e.g. NDIS) + NATIVE = 94, + /// event for tx pkt delay suddently jump + PKTDELAY_IND = 95, + /// AWDL AW period starts + AWDL_AW = 96, + /// AWDL Master/Slave/NE master role event + AWDL_ROLE = 97, + /// Generic AWDL event + AWDL_EVENT = 98, + /// NIC AF txstatus + NIC_AF_TXS = 99, + /// NAN event + NAN = 100, + BEACON_FRAME_RX = 101, + /// desired service found + SERVICE_FOUND = 102, + /// GAS fragment received + GAS_FRAGMENT_RX = 103, + /// GAS sessions all complete + GAS_COMPLETE = 104, + /// New device found by p2p offload + P2PO_ADD_DEVICE = 105, + /// device has been removed by p2p offload + P2PO_DEL_DEVICE = 106, + /// WNM event to notify STA enter sleep mode + WNM_STA_SLEEP = 107, + /// Indication of MAC tx failures (exhaustion of 802.11 retries) exceeding threshold(s) + TXFAIL_THRESH = 108, + /// Proximity Detection event + PROXD = 109, + /// AWDL RX Probe response + AWDL_RX_PRB_RESP = 111, + /// AWDL RX Action Frames + AWDL_RX_ACT_FRAME = 112, + /// AWDL Wowl nulls + AWDL_WOWL_NULLPKT = 113, + /// AWDL Phycal status + AWDL_PHYCAL_STATUS = 114, + /// AWDL OOB AF status + AWDL_OOB_AF_STATUS = 115, + /// Interleaved Scan status + AWDL_SCAN_STATUS = 116, + /// AWDL AW Start + AWDL_AW_START = 117, + /// AWDL AW End + AWDL_AW_END = 118, + /// AWDL AW Extensions + AWDL_AW_EXT = 119, + AWDL_PEER_CACHE_CONTROL = 120, + CSA_START_IND = 121, + CSA_DONE_IND = 122, + CSA_FAILURE_IND = 123, + /// CCA based channel quality report + CCA_CHAN_QUAL = 124, + /// to report change in BSSID while roaming + BSSID = 125, + /// tx error indication + TX_STAT_ERROR = 126, + /// credit check for BCMC supported + BCMC_CREDIT_SUPPORT = 127, + /// psta primary interface indication + PSTA_PRIMARY_INTF_IND = 128, + /// Handover Request Initiated + BT_WIFI_HANDOVER_REQ = 130, + /// Southpaw TxInhibit notification + SPW_TXINHIBIT = 131, + /// FBT Authentication Request Indication + FBT_AUTH_REQ_IND = 132, + /// Enhancement addition for RSSI + RSSI_LQM = 133, + /// Full probe/beacon (IEs etc) results + PFN_GSCAN_FULL_RESULT = 134, + /// Significant change in rssi of bssids being tracked + PFN_SWC = 135, + /// a STA been authroized for traffic + AUTHORIZED = 136, + /// probe req with wl_event_rx_frame_data_t header + PROBREQ_MSG_RX = 137, + /// PFN completed scan of network list + PFN_SCAN_COMPLETE = 138, + /// RMC Event + RMC_EVENT = 139, + /// DPSTA interface indication + DPSTA_INTF_IND = 140, + /// RRM Event + RRM = 141, + /// ULP entry event + ULP = 146, + /// TCP Keep Alive Offload Event + TKO = 151, + /// authentication request received + EXT_AUTH_REQ = 187, + /// authentication request received + EXT_AUTH_FRAME_RX = 188, + /// mgmt frame Tx complete + MGMT_FRAME_TXSTATUS = 189, + /// highest val + 1 for range checking + LAST = 190, +} + +// TODO this PubSub can probably be replaced with shared memory to make it a bit more efficient. +pub type EventQueue = PubSubChannel; +pub type EventSubscriber<'a> = Subscriber<'a, NoopRawMutex, Message, 2, 1, 1>; + +pub struct Events { + pub queue: EventQueue, + pub mask: SharedEventMask, +} + +impl Events { + pub fn new() -> Self { + Self { + queue: EventQueue::new(), + mask: SharedEventMask::default(), + } + } +} + +#[derive(Clone, Copy)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +pub struct Status { + pub event_type: Event, + pub status: u32, +} + +#[derive(Clone, Copy)] +pub enum Payload { + None, + BssInfo(BssInfo), +} + +#[derive(Clone, Copy)] + +pub struct Message { + pub header: Status, + pub payload: Payload, +} + +impl Message { + pub fn new(status: Status, payload: Payload) -> Self { + Self { + header: status, + payload, + } + } +} + +#[derive(Default)] +struct EventMask { + mask: [u32; Self::WORD_COUNT], +} + +impl EventMask { + const WORD_COUNT: usize = ((Event::LAST as u32 + (u32::BITS - 1)) / u32::BITS) as usize; + + fn enable(&mut self, event: Event) { + let n = event as u32; + let word = n / u32::BITS; + let bit = n % u32::BITS; + + self.mask[word as usize] |= 1 << bit; + } + + fn disable(&mut self, event: Event) { + let n = event as u32; + let word = n / u32::BITS; + let bit = n % u32::BITS; + + self.mask[word as usize] &= !(1 << bit); + } + + fn is_enabled(&self, event: Event) -> bool { + let n = event as u32; + let word = n / u32::BITS; + let bit = n % u32::BITS; + + self.mask[word as usize] & (1 << bit) > 0 + } +} + +#[derive(Default)] + +pub struct SharedEventMask { + mask: RefCell, +} + +impl SharedEventMask { + pub fn enable(&self, events: &[Event]) { + let mut mask = self.mask.borrow_mut(); + for event in events { + mask.enable(*event); + } + } + + #[allow(dead_code)] + pub fn disable(&self, events: &[Event]) { + let mut mask = self.mask.borrow_mut(); + for event in events { + mask.disable(*event); + } + } + + pub fn disable_all(&self) { + let mut mask = self.mask.borrow_mut(); + mask.mask = Default::default(); + } + + pub fn is_enabled(&self, event: Event) -> bool { + let mask = self.mask.borrow(); + mask.is_enabled(event) + } +} diff --git a/cyw43/src/fmt.rs b/cyw43/src/fmt.rs new file mode 100644 index 00000000..5730447b --- /dev/null +++ b/cyw43/src/fmt.rs @@ -0,0 +1,257 @@ +#![macro_use] +#![allow(unused_macros)] + +use core::fmt::{Debug, Display, LowerHex}; + +#[cfg(all(feature = "defmt", feature = "log"))] +compile_error!("You may not enable both `defmt` and `log` features."); + +macro_rules! assert { + ($($x:tt)*) => { + { + #[cfg(not(feature = "defmt"))] + ::core::assert!($($x)*); + #[cfg(feature = "defmt")] + ::defmt::assert!($($x)*); + } + }; +} + +macro_rules! assert_eq { + ($($x:tt)*) => { + { + #[cfg(not(feature = "defmt"))] + ::core::assert_eq!($($x)*); + #[cfg(feature = "defmt")] + ::defmt::assert_eq!($($x)*); + } + }; +} + +macro_rules! assert_ne { + ($($x:tt)*) => { + { + #[cfg(not(feature = "defmt"))] + ::core::assert_ne!($($x)*); + #[cfg(feature = "defmt")] + ::defmt::assert_ne!($($x)*); + } + }; +} + +macro_rules! debug_assert { + ($($x:tt)*) => { + { + #[cfg(not(feature = "defmt"))] + ::core::debug_assert!($($x)*); + #[cfg(feature = "defmt")] + ::defmt::debug_assert!($($x)*); + } + }; +} + +macro_rules! debug_assert_eq { + ($($x:tt)*) => { + { + #[cfg(not(feature = "defmt"))] + ::core::debug_assert_eq!($($x)*); + #[cfg(feature = "defmt")] + ::defmt::debug_assert_eq!($($x)*); + } + }; +} + +macro_rules! debug_assert_ne { + ($($x:tt)*) => { + { + #[cfg(not(feature = "defmt"))] + ::core::debug_assert_ne!($($x)*); + #[cfg(feature = "defmt")] + ::defmt::debug_assert_ne!($($x)*); + } + }; +} + +macro_rules! todo { + ($($x:tt)*) => { + { + #[cfg(not(feature = "defmt"))] + ::core::todo!($($x)*); + #[cfg(feature = "defmt")] + ::defmt::todo!($($x)*); + } + }; +} + +macro_rules! unreachable { + ($($x:tt)*) => { + { + #[cfg(not(feature = "defmt"))] + ::core::unreachable!($($x)*); + #[cfg(feature = "defmt")] + ::defmt::unreachable!($($x)*); + } + }; +} + +macro_rules! panic { + ($($x:tt)*) => { + { + #[cfg(not(feature = "defmt"))] + ::core::panic!($($x)*); + #[cfg(feature = "defmt")] + ::defmt::panic!($($x)*); + } + }; +} + +macro_rules! trace { + ($s:literal $(, $x:expr)* $(,)?) => { + { + #[cfg(feature = "log")] + ::log::trace!($s $(, $x)*); + #[cfg(feature = "defmt")] + ::defmt::trace!($s $(, $x)*); + #[cfg(not(any(feature = "log", feature="defmt")))] + let _ = ($( & $x ),*); + } + }; +} + +macro_rules! debug { + ($s:literal $(, $x:expr)* $(,)?) => { + { + #[cfg(feature = "log")] + ::log::debug!($s $(, $x)*); + #[cfg(feature = "defmt")] + ::defmt::debug!($s $(, $x)*); + #[cfg(not(any(feature = "log", feature="defmt")))] + let _ = ($( & $x ),*); + } + }; +} + +macro_rules! info { + ($s:literal $(, $x:expr)* $(,)?) => { + { + #[cfg(feature = "log")] + ::log::info!($s $(, $x)*); + #[cfg(feature = "defmt")] + ::defmt::info!($s $(, $x)*); + #[cfg(not(any(feature = "log", feature="defmt")))] + let _ = ($( & $x ),*); + } + }; +} + +macro_rules! warn { + ($s:literal $(, $x:expr)* $(,)?) => { + { + #[cfg(feature = "log")] + ::log::warn!($s $(, $x)*); + #[cfg(feature = "defmt")] + ::defmt::warn!($s $(, $x)*); + #[cfg(not(any(feature = "log", feature="defmt")))] + let _ = ($( & $x ),*); + } + }; +} + +macro_rules! error { + ($s:literal $(, $x:expr)* $(,)?) => { + { + #[cfg(feature = "log")] + ::log::error!($s $(, $x)*); + #[cfg(feature = "defmt")] + ::defmt::error!($s $(, $x)*); + #[cfg(not(any(feature = "log", feature="defmt")))] + let _ = ($( & $x ),*); + } + }; +} + +#[cfg(feature = "defmt")] +macro_rules! unwrap { + ($($x:tt)*) => { + ::defmt::unwrap!($($x)*) + }; +} + +#[cfg(not(feature = "defmt"))] +macro_rules! unwrap { + ($arg:expr) => { + match $crate::fmt::Try::into_result($arg) { + ::core::result::Result::Ok(t) => t, + ::core::result::Result::Err(e) => { + ::core::panic!("unwrap of `{}` failed: {:?}", ::core::stringify!($arg), e); + } + } + }; + ($arg:expr, $($msg:expr),+ $(,)? ) => { + match $crate::fmt::Try::into_result($arg) { + ::core::result::Result::Ok(t) => t, + ::core::result::Result::Err(e) => { + ::core::panic!("unwrap of `{}` failed: {}: {:?}", ::core::stringify!($arg), ::core::format_args!($($msg,)*), e); + } + } + } +} + +#[cfg(feature = "defmt-timestamp-uptime")] +defmt::timestamp! {"{=u64:us}", crate::time::Instant::now().as_micros() } + +#[derive(Debug, Copy, Clone, Eq, PartialEq)] +pub struct NoneError; + +pub trait Try { + type Ok; + type Error; + fn into_result(self) -> Result; +} + +impl Try for Option { + type Ok = T; + type Error = NoneError; + + #[inline] + fn into_result(self) -> Result { + self.ok_or(NoneError) + } +} + +impl Try for Result { + type Ok = T; + type Error = E; + + #[inline] + fn into_result(self) -> Self { + self + } +} + +pub struct Bytes<'a>(pub &'a [u8]); + +impl<'a> Debug for Bytes<'a> { + fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result { + write!(f, "{:#02x?}", self.0) + } +} + +impl<'a> Display for Bytes<'a> { + fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result { + write!(f, "{:#02x?}", self.0) + } +} + +impl<'a> LowerHex for Bytes<'a> { + fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result { + write!(f, "{:#02x?}", self.0) + } +} + +#[cfg(feature = "defmt")] +impl<'a> defmt::Format for Bytes<'a> { + fn format(&self, fmt: defmt::Formatter) { + defmt::write!(fmt, "{:02x}", self.0) + } +} diff --git a/cyw43/src/ioctl.rs b/cyw43/src/ioctl.rs new file mode 100644 index 00000000..61524c27 --- /dev/null +++ b/cyw43/src/ioctl.rs @@ -0,0 +1,126 @@ +use core::cell::{Cell, RefCell}; +use core::future::poll_fn; +use core::task::{Poll, Waker}; + +use embassy_sync::waitqueue::WakerRegistration; + +use crate::fmt::Bytes; + +#[derive(Clone, Copy)] +pub enum IoctlType { + Get = 0, + Set = 2, +} + +#[derive(Clone, Copy)] +pub struct PendingIoctl { + pub buf: *mut [u8], + pub kind: IoctlType, + pub cmd: u32, + pub iface: u32, +} + +#[derive(Clone, Copy)] +enum IoctlStateInner { + Pending(PendingIoctl), + Sent { buf: *mut [u8] }, + Done { resp_len: usize }, +} + +struct Wakers { + control: WakerRegistration, + runner: WakerRegistration, +} + +impl Default for Wakers { + fn default() -> Self { + Self { + control: WakerRegistration::new(), + runner: WakerRegistration::new(), + } + } +} + +pub struct IoctlState { + state: Cell, + wakers: RefCell, +} + +impl IoctlState { + pub fn new() -> Self { + Self { + state: Cell::new(IoctlStateInner::Done { resp_len: 0 }), + wakers: Default::default(), + } + } + + fn wake_control(&self) { + self.wakers.borrow_mut().control.wake(); + } + + fn register_control(&self, waker: &Waker) { + self.wakers.borrow_mut().control.register(waker); + } + + fn wake_runner(&self) { + self.wakers.borrow_mut().runner.wake(); + } + + fn register_runner(&self, waker: &Waker) { + self.wakers.borrow_mut().runner.register(waker); + } + + pub async fn wait_complete(&self) -> usize { + poll_fn(|cx| { + if let IoctlStateInner::Done { resp_len } = self.state.get() { + Poll::Ready(resp_len) + } else { + self.register_control(cx.waker()); + Poll::Pending + } + }) + .await + } + + pub async fn wait_pending(&self) -> PendingIoctl { + let pending = poll_fn(|cx| { + if let IoctlStateInner::Pending(pending) = self.state.get() { + Poll::Ready(pending) + } else { + self.register_runner(cx.waker()); + Poll::Pending + } + }) + .await; + + self.state.set(IoctlStateInner::Sent { buf: pending.buf }); + pending + } + + pub fn cancel_ioctl(&self) { + self.state.set(IoctlStateInner::Done { resp_len: 0 }); + } + + pub async fn do_ioctl(&self, kind: IoctlType, cmd: u32, iface: u32, buf: &mut [u8]) -> usize { + self.state + .set(IoctlStateInner::Pending(PendingIoctl { buf, kind, cmd, iface })); + self.wake_runner(); + self.wait_complete().await + } + + pub fn ioctl_done(&self, response: &[u8]) { + if let IoctlStateInner::Sent { buf } = self.state.get() { + trace!("IOCTL Response: {:02x}", Bytes(response)); + + // TODO fix this + (unsafe { &mut *buf }[..response.len()]).copy_from_slice(response); + + self.state.set(IoctlStateInner::Done { + resp_len: response.len(), + }); + self.wake_control(); + } else { + warn!("IOCTL Response but no pending Ioctl"); + } + } +} diff --git a/cyw43/src/lib.rs b/cyw43/src/lib.rs new file mode 100644 index 00000000..fd11f367 --- /dev/null +++ b/cyw43/src/lib.rs @@ -0,0 +1,236 @@ +#![no_std] +#![no_main] +#![allow(incomplete_features)] +#![feature(async_fn_in_trait, type_alias_impl_trait, concat_bytes)] +#![deny(unused_must_use)] + +// This mod MUST go first, so that the others see its macros. +pub(crate) mod fmt; + +mod bus; +mod consts; +mod countries; +mod events; +mod ioctl; +mod structs; + +mod control; +mod nvram; +mod runner; + +use core::slice; + +use embassy_net_driver_channel as ch; +use embedded_hal_1::digital::OutputPin; +use events::Events; +use ioctl::IoctlState; + +use crate::bus::Bus; +pub use crate::bus::SpiBusCyw43; +pub use crate::control::{Control, Error as ControlError}; +pub use crate::runner::Runner; +pub use crate::structs::BssInfo; + +const MTU: usize = 1514; + +#[allow(unused)] +#[derive(Clone, Copy, PartialEq, Eq)] +enum Core { + WLAN = 0, + SOCSRAM = 1, + SDIOD = 2, +} + +impl Core { + fn base_addr(&self) -> u32 { + match self { + Self::WLAN => CHIP.arm_core_base_address, + Self::SOCSRAM => CHIP.socsram_wrapper_base_address, + Self::SDIOD => CHIP.sdiod_core_base_address, + } + } +} + +#[allow(unused)] +struct Chip { + arm_core_base_address: u32, + socsram_base_address: u32, + socsram_wrapper_base_address: u32, + sdiod_core_base_address: u32, + pmu_base_address: u32, + chip_ram_size: u32, + atcm_ram_base_address: u32, + socram_srmem_size: u32, + chanspec_band_mask: u32, + chanspec_band_2g: u32, + chanspec_band_5g: u32, + chanspec_band_shift: u32, + chanspec_bw_10: u32, + chanspec_bw_20: u32, + chanspec_bw_40: u32, + chanspec_bw_mask: u32, + chanspec_bw_shift: u32, + chanspec_ctl_sb_lower: u32, + chanspec_ctl_sb_upper: u32, + chanspec_ctl_sb_none: u32, + chanspec_ctl_sb_mask: u32, +} + +const WRAPPER_REGISTER_OFFSET: u32 = 0x100000; + +// Data for CYW43439 +const CHIP: Chip = Chip { + arm_core_base_address: 0x18003000 + WRAPPER_REGISTER_OFFSET, + socsram_base_address: 0x18004000, + socsram_wrapper_base_address: 0x18004000 + WRAPPER_REGISTER_OFFSET, + sdiod_core_base_address: 0x18002000, + pmu_base_address: 0x18000000, + chip_ram_size: 512 * 1024, + atcm_ram_base_address: 0, + socram_srmem_size: 64 * 1024, + chanspec_band_mask: 0xc000, + chanspec_band_2g: 0x0000, + chanspec_band_5g: 0xc000, + chanspec_band_shift: 14, + chanspec_bw_10: 0x0800, + chanspec_bw_20: 0x1000, + chanspec_bw_40: 0x1800, + chanspec_bw_mask: 0x3800, + chanspec_bw_shift: 11, + chanspec_ctl_sb_lower: 0x0000, + chanspec_ctl_sb_upper: 0x0100, + chanspec_ctl_sb_none: 0x0000, + chanspec_ctl_sb_mask: 0x0700, +}; + +pub struct State { + ioctl_state: IoctlState, + ch: ch::State, + events: Events, +} + +impl State { + pub fn new() -> Self { + Self { + ioctl_state: IoctlState::new(), + ch: ch::State::new(), + events: Events::new(), + } + } +} + +#[derive(Debug, Clone, Copy, PartialEq, Eq)] +pub enum PowerManagementMode { + /// Custom, officially unsupported mode. Use at your own risk. + /// All power-saving features set to their max at only a marginal decrease in power consumption + /// as oppposed to `Aggressive`. + SuperSave, + + /// Aggressive power saving mode. + Aggressive, + + /// The default mode. + PowerSave, + + /// Performance is prefered over power consumption but still some power is conserved as opposed to + /// `None`. + Performance, + + /// Unlike all the other PM modes, this lowers the power consumption at all times at the cost of + /// a much lower throughput. + ThroughputThrottling, + + /// No power management is configured. This consumes the most power. + None, +} + +impl Default for PowerManagementMode { + fn default() -> Self { + Self::PowerSave + } +} + +impl PowerManagementMode { + fn sleep_ret_ms(&self) -> u16 { + match self { + PowerManagementMode::SuperSave => 2000, + PowerManagementMode::Aggressive => 2000, + PowerManagementMode::PowerSave => 200, + PowerManagementMode::Performance => 20, + PowerManagementMode::ThroughputThrottling => 0, // value doesn't matter + PowerManagementMode::None => 0, // value doesn't matter + } + } + + fn beacon_period(&self) -> u8 { + match self { + PowerManagementMode::SuperSave => 255, + PowerManagementMode::Aggressive => 1, + PowerManagementMode::PowerSave => 1, + PowerManagementMode::Performance => 1, + PowerManagementMode::ThroughputThrottling => 0, // value doesn't matter + PowerManagementMode::None => 0, // value doesn't matter + } + } + + fn dtim_period(&self) -> u8 { + match self { + PowerManagementMode::SuperSave => 255, + PowerManagementMode::Aggressive => 1, + PowerManagementMode::PowerSave => 1, + PowerManagementMode::Performance => 1, + PowerManagementMode::ThroughputThrottling => 0, // value doesn't matter + PowerManagementMode::None => 0, // value doesn't matter + } + } + + fn assoc(&self) -> u8 { + match self { + PowerManagementMode::SuperSave => 255, + PowerManagementMode::Aggressive => 10, + PowerManagementMode::PowerSave => 10, + PowerManagementMode::Performance => 1, + PowerManagementMode::ThroughputThrottling => 0, // value doesn't matter + PowerManagementMode::None => 0, // value doesn't matter + } + } + + fn mode(&self) -> u32 { + match self { + PowerManagementMode::ThroughputThrottling => 1, + PowerManagementMode::None => 0, + _ => 2, + } + } +} + +pub type NetDriver<'a> = ch::Device<'a, MTU>; + +pub async fn new<'a, PWR, SPI>( + state: &'a mut State, + pwr: PWR, + spi: SPI, + firmware: &[u8], +) -> (NetDriver<'a>, Control<'a>, Runner<'a, PWR, SPI>) +where + PWR: OutputPin, + SPI: SpiBusCyw43, +{ + let (ch_runner, device) = ch::new(&mut state.ch, [0; 6]); + let state_ch = ch_runner.state_runner(); + + let mut runner = Runner::new(ch_runner, Bus::new(pwr, spi), &state.ioctl_state, &state.events); + + runner.init(firmware).await; + + ( + device, + Control::new(state_ch, &state.events, &state.ioctl_state), + runner, + ) +} + +fn slice8_mut(x: &mut [u32]) -> &mut [u8] { + let len = x.len() * 4; + unsafe { slice::from_raw_parts_mut(x.as_mut_ptr() as _, len) } +} diff --git a/cyw43/src/nvram.rs b/cyw43/src/nvram.rs new file mode 100644 index 00000000..964a3128 --- /dev/null +++ b/cyw43/src/nvram.rs @@ -0,0 +1,54 @@ +macro_rules! nvram { + ($($s:literal,)*) => { + concat_bytes!($($s, b"\x00",)* b"\x00\x00") + }; +} + +pub static NVRAM: &'static [u8] = &*nvram!( + b"NVRAMRev=$Rev$", + b"manfid=0x2d0", + b"prodid=0x0727", + b"vendid=0x14e4", + b"devid=0x43e2", + b"boardtype=0x0887", + b"boardrev=0x1100", + b"boardnum=22", + b"macaddr=00:A0:50:b5:59:5e", + b"sromrev=11", + b"boardflags=0x00404001", + b"boardflags3=0x04000000", + b"xtalfreq=37400", + b"nocrc=1", + b"ag0=255", + b"aa2g=1", + b"ccode=ALL", + b"pa0itssit=0x20", + b"extpagain2g=0", + b"pa2ga0=-168,6649,-778", + b"AvVmid_c0=0x0,0xc8", + b"cckpwroffset0=5", + b"maxp2ga0=84", + b"txpwrbckof=6", + b"cckbw202gpo=0", + b"legofdmbw202gpo=0x66111111", + b"mcsbw202gpo=0x77711111", + b"propbw202gpo=0xdd", + b"ofdmdigfilttype=18", + b"ofdmdigfilttypebe=18", + b"papdmode=1", + b"papdvalidtest=1", + b"pacalidx2g=45", + b"papdepsoffset=-30", + b"papdendidx=58", + b"ltecxmux=0", + b"ltecxpadnum=0x0102", + b"ltecxfnsel=0x44", + b"ltecxgcigpio=0x01", + b"il0macaddr=00:90:4c:c5:12:38", + b"wl0id=0x431b", + b"deadman_to=0xffffffff", + b"muxenab=0x100", + b"spurconfig=0x3", + b"glitch_based_crsmin=1", + b"btc_mode=1", +); diff --git a/cyw43/src/runner.rs b/cyw43/src/runner.rs new file mode 100644 index 00000000..5706696b --- /dev/null +++ b/cyw43/src/runner.rs @@ -0,0 +1,575 @@ +use embassy_futures::select::{select3, Either3}; +use embassy_net_driver_channel as ch; +use embassy_sync::pubsub::PubSubBehavior; +use embassy_time::{block_for, Duration, Timer}; +use embedded_hal_1::digital::OutputPin; + +use crate::bus::Bus; +pub use crate::bus::SpiBusCyw43; +use crate::consts::*; +use crate::events::{Event, Events, Status}; +use crate::fmt::Bytes; +use crate::ioctl::{IoctlState, IoctlType, PendingIoctl}; +use crate::nvram::NVRAM; +use crate::structs::*; +use crate::{events, slice8_mut, Core, CHIP, MTU}; + +#[cfg(feature = "firmware-logs")] +struct LogState { + addr: u32, + last_idx: usize, + buf: [u8; 256], + buf_count: usize, +} + +#[cfg(feature = "firmware-logs")] +impl Default for LogState { + fn default() -> Self { + Self { + addr: Default::default(), + last_idx: Default::default(), + buf: [0; 256], + buf_count: Default::default(), + } + } +} + +pub struct Runner<'a, PWR, SPI> { + ch: ch::Runner<'a, MTU>, + bus: Bus, + + ioctl_state: &'a IoctlState, + ioctl_id: u16, + sdpcm_seq: u8, + sdpcm_seq_max: u8, + + events: &'a Events, + + #[cfg(feature = "firmware-logs")] + log: LogState, +} + +impl<'a, PWR, SPI> Runner<'a, PWR, SPI> +where + PWR: OutputPin, + SPI: SpiBusCyw43, +{ + pub(crate) fn new( + ch: ch::Runner<'a, MTU>, + bus: Bus, + ioctl_state: &'a IoctlState, + events: &'a Events, + ) -> Self { + Self { + ch, + bus, + ioctl_state, + ioctl_id: 0, + sdpcm_seq: 0, + sdpcm_seq_max: 1, + events, + #[cfg(feature = "firmware-logs")] + log: LogState::default(), + } + } + + pub(crate) async fn init(&mut self, firmware: &[u8]) { + self.bus.init().await; + + // Init ALP (Active Low Power) clock + self.bus + .write8(FUNC_BACKPLANE, REG_BACKPLANE_CHIP_CLOCK_CSR, BACKPLANE_ALP_AVAIL_REQ) + .await; + debug!("waiting for clock..."); + while self.bus.read8(FUNC_BACKPLANE, REG_BACKPLANE_CHIP_CLOCK_CSR).await & BACKPLANE_ALP_AVAIL == 0 {} + debug!("clock ok"); + + let chip_id = self.bus.bp_read16(0x1800_0000).await; + debug!("chip ID: {}", chip_id); + + // Upload firmware. + self.core_disable(Core::WLAN).await; + self.core_reset(Core::SOCSRAM).await; + self.bus.bp_write32(CHIP.socsram_base_address + 0x10, 3).await; + self.bus.bp_write32(CHIP.socsram_base_address + 0x44, 0).await; + + let ram_addr = CHIP.atcm_ram_base_address; + + debug!("loading fw"); + self.bus.bp_write(ram_addr, firmware).await; + + debug!("loading nvram"); + // Round up to 4 bytes. + let nvram_len = (NVRAM.len() + 3) / 4 * 4; + self.bus + .bp_write(ram_addr + CHIP.chip_ram_size - 4 - nvram_len as u32, NVRAM) + .await; + + let nvram_len_words = nvram_len as u32 / 4; + let nvram_len_magic = (!nvram_len_words << 16) | nvram_len_words; + self.bus + .bp_write32(ram_addr + CHIP.chip_ram_size - 4, nvram_len_magic) + .await; + + // Start core! + debug!("starting up core..."); + self.core_reset(Core::WLAN).await; + assert!(self.core_is_up(Core::WLAN).await); + + while self.bus.read8(FUNC_BACKPLANE, REG_BACKPLANE_CHIP_CLOCK_CSR).await & 0x80 == 0 {} + + // "Set up the interrupt mask and enable interrupts" + // self.bus.bp_write32(CHIP.sdiod_core_base_address + 0x24, 0xF0).await; + + self.bus + .write16(FUNC_BUS, REG_BUS_INTERRUPT_ENABLE, IRQ_F2_PACKET_AVAILABLE) + .await; + + // "Lower F2 Watermark to avoid DMA Hang in F2 when SD Clock is stopped." + // Sounds scary... + self.bus + .write8(FUNC_BACKPLANE, REG_BACKPLANE_FUNCTION2_WATERMARK, 32) + .await; + + // wait for wifi startup + debug!("waiting for wifi init..."); + while self.bus.read32(FUNC_BUS, REG_BUS_STATUS).await & STATUS_F2_RX_READY == 0 {} + + // Some random configs related to sleep. + // These aren't needed if we don't want to sleep the bus. + // TODO do we need to sleep the bus to read the irq line, due to + // being on the same pin as MOSI/MISO? + + /* + let mut val = self.bus.read8(FUNC_BACKPLANE, REG_BACKPLANE_WAKEUP_CTRL).await; + val |= 0x02; // WAKE_TILL_HT_AVAIL + self.bus.write8(FUNC_BACKPLANE, REG_BACKPLANE_WAKEUP_CTRL, val).await; + self.bus.write8(FUNC_BUS, 0xF0, 0x08).await; // SDIOD_CCCR_BRCM_CARDCAP.CMD_NODEC = 1 + self.bus.write8(FUNC_BACKPLANE, REG_BACKPLANE_CHIP_CLOCK_CSR, 0x02).await; // SBSDIO_FORCE_HT + + let mut val = self.bus.read8(FUNC_BACKPLANE, REG_BACKPLANE_SLEEP_CSR).await; + val |= 0x01; // SBSDIO_SLPCSR_KEEP_SDIO_ON + self.bus.write8(FUNC_BACKPLANE, REG_BACKPLANE_SLEEP_CSR, val).await; + */ + + // clear pulls + self.bus.write8(FUNC_BACKPLANE, REG_BACKPLANE_PULL_UP, 0).await; + let _ = self.bus.read8(FUNC_BACKPLANE, REG_BACKPLANE_PULL_UP).await; + + // start HT clock + //self.bus.write8(FUNC_BACKPLANE, REG_BACKPLANE_CHIP_CLOCK_CSR, 0x10).await; + //debug!("waiting for HT clock..."); + //while self.bus.read8(FUNC_BACKPLANE, REG_BACKPLANE_CHIP_CLOCK_CSR).await & 0x80 == 0 {} + //debug!("clock ok"); + + #[cfg(feature = "firmware-logs")] + self.log_init().await; + + debug!("wifi init done"); + } + + #[cfg(feature = "firmware-logs")] + async fn log_init(&mut self) { + // Initialize shared memory for logging. + + let addr = CHIP.atcm_ram_base_address + CHIP.chip_ram_size - 4 - CHIP.socram_srmem_size; + let shared_addr = self.bus.bp_read32(addr).await; + debug!("shared_addr {:08x}", shared_addr); + + let mut shared = [0; SharedMemData::SIZE]; + self.bus.bp_read(shared_addr, &mut shared).await; + let shared = SharedMemData::from_bytes(&shared); + + self.log.addr = shared.console_addr + 8; + } + + #[cfg(feature = "firmware-logs")] + async fn log_read(&mut self) { + // Read log struct + let mut log = [0; SharedMemLog::SIZE]; + self.bus.bp_read(self.log.addr, &mut log).await; + let log = SharedMemLog::from_bytes(&log); + + let idx = log.idx as usize; + + // If pointer hasn't moved, no need to do anything. + if idx == self.log.last_idx { + return; + } + + // Read entire buf for now. We could read only what we need, but then we + // run into annoying alignment issues in `bp_read`. + let mut buf = [0; 0x400]; + self.bus.bp_read(log.buf, &mut buf).await; + + while self.log.last_idx != idx as usize { + let b = buf[self.log.last_idx]; + if b == b'\r' || b == b'\n' { + if self.log.buf_count != 0 { + let s = unsafe { core::str::from_utf8_unchecked(&self.log.buf[..self.log.buf_count]) }; + debug!("LOGS: {}", s); + self.log.buf_count = 0; + } + } else if self.log.buf_count < self.log.buf.len() { + self.log.buf[self.log.buf_count] = b; + self.log.buf_count += 1; + } + + self.log.last_idx += 1; + if self.log.last_idx == 0x400 { + self.log.last_idx = 0; + } + } + } + + pub async fn run(mut self) -> ! { + let mut buf = [0; 512]; + loop { + #[cfg(feature = "firmware-logs")] + self.log_read().await; + + if self.has_credit() { + let ioctl = self.ioctl_state.wait_pending(); + let tx = self.ch.tx_buf(); + let ev = self.bus.wait_for_event(); + + match select3(ioctl, tx, ev).await { + Either3::First(PendingIoctl { + buf: iobuf, + kind, + cmd, + iface, + }) => { + self.send_ioctl(kind, cmd, iface, unsafe { &*iobuf }).await; + self.check_status(&mut buf).await; + } + Either3::Second(packet) => { + trace!("tx pkt {:02x}", Bytes(&packet[..packet.len().min(48)])); + + let mut buf = [0; 512]; + let buf8 = slice8_mut(&mut buf); + + // There MUST be 2 bytes of padding between the SDPCM and BDC headers. + // And ONLY for data packets! + // No idea why, but the firmware will append two zero bytes to the tx'd packets + // otherwise. If the packet is exactly 1514 bytes (the max MTU), this makes it + // be oversized and get dropped. + // WHD adds it here https://github.com/Infineon/wifi-host-driver/blob/c04fcbb6b0d049304f376cf483fd7b1b570c8cd5/WiFi_Host_Driver/src/include/whd_sdpcm.h#L90 + // and adds it to the header size her https://github.com/Infineon/wifi-host-driver/blob/c04fcbb6b0d049304f376cf483fd7b1b570c8cd5/WiFi_Host_Driver/src/whd_sdpcm.c#L597 + // ¯\_(ツ)_/¯ + const PADDING_SIZE: usize = 2; + let total_len = SdpcmHeader::SIZE + PADDING_SIZE + BdcHeader::SIZE + packet.len(); + + let seq = self.sdpcm_seq; + self.sdpcm_seq = self.sdpcm_seq.wrapping_add(1); + + let sdpcm_header = SdpcmHeader { + len: total_len as u16, // TODO does this len need to be rounded up to u32? + len_inv: !total_len as u16, + sequence: seq, + channel_and_flags: CHANNEL_TYPE_DATA, + next_length: 0, + header_length: (SdpcmHeader::SIZE + PADDING_SIZE) as _, + wireless_flow_control: 0, + bus_data_credit: 0, + reserved: [0, 0], + }; + + let bdc_header = BdcHeader { + flags: BDC_VERSION << BDC_VERSION_SHIFT, + priority: 0, + flags2: 0, + data_offset: 0, + }; + trace!("tx {:?}", sdpcm_header); + trace!(" {:?}", bdc_header); + + buf8[0..SdpcmHeader::SIZE].copy_from_slice(&sdpcm_header.to_bytes()); + buf8[SdpcmHeader::SIZE + PADDING_SIZE..][..BdcHeader::SIZE] + .copy_from_slice(&bdc_header.to_bytes()); + buf8[SdpcmHeader::SIZE + PADDING_SIZE + BdcHeader::SIZE..][..packet.len()] + .copy_from_slice(packet); + + let total_len = (total_len + 3) & !3; // round up to 4byte + + trace!(" {:02x}", Bytes(&buf8[..total_len.min(48)])); + + self.bus.wlan_write(&buf[..(total_len / 4)]).await; + self.ch.tx_done(); + self.check_status(&mut buf).await; + } + Either3::Third(()) => { + self.handle_irq(&mut buf).await; + } + } + } else { + warn!("TX stalled"); + self.bus.wait_for_event().await; + self.handle_irq(&mut buf).await; + } + } + } + + /// Wait for IRQ on F2 packet available + async fn handle_irq(&mut self, buf: &mut [u32; 512]) { + // Receive stuff + let irq = self.bus.read16(FUNC_BUS, REG_BUS_INTERRUPT).await; + trace!("irq{}", FormatInterrupt(irq)); + + if irq & IRQ_F2_PACKET_AVAILABLE != 0 { + self.check_status(buf).await; + } + + if irq & IRQ_DATA_UNAVAILABLE != 0 { + // TODO what should we do here? + warn!("IRQ DATA_UNAVAILABLE, clearing..."); + self.bus.write16(FUNC_BUS, REG_BUS_INTERRUPT, 1).await; + } + } + + /// Handle F2 events while status register is set + async fn check_status(&mut self, buf: &mut [u32; 512]) { + loop { + let status = self.bus.status(); + trace!("check status{}", FormatStatus(status)); + + if status & STATUS_F2_PKT_AVAILABLE != 0 { + let len = (status & STATUS_F2_PKT_LEN_MASK) >> STATUS_F2_PKT_LEN_SHIFT; + self.bus.wlan_read(buf, len).await; + trace!("rx {:02x}", Bytes(&slice8_mut(buf)[..(len as usize).min(48)])); + self.rx(&mut slice8_mut(buf)[..len as usize]); + } else { + break; + } + } + } + + fn rx(&mut self, packet: &mut [u8]) { + let Some((sdpcm_header, payload)) = SdpcmHeader::parse(packet) else { return }; + + self.update_credit(&sdpcm_header); + + let channel = sdpcm_header.channel_and_flags & 0x0f; + + match channel { + CHANNEL_TYPE_CONTROL => { + let Some((cdc_header, response)) = CdcHeader::parse(payload) else { return; }; + trace!(" {:?}", cdc_header); + + if cdc_header.id == self.ioctl_id { + if cdc_header.status != 0 { + // TODO: propagate error instead + panic!("IOCTL error {}", cdc_header.status as i32); + } + + self.ioctl_state.ioctl_done(response); + } + } + CHANNEL_TYPE_EVENT => { + let Some((_, bdc_packet)) = BdcHeader::parse(payload) else { + warn!("BDC event, incomplete header"); + return; + }; + + let Some((event_packet, evt_data)) = EventPacket::parse(bdc_packet) else { + warn!("BDC event, incomplete data"); + return; + }; + + const ETH_P_LINK_CTL: u16 = 0x886c; // HPNA, wlan link local tunnel, according to linux if_ether.h + if event_packet.eth.ether_type != ETH_P_LINK_CTL { + warn!( + "unexpected ethernet type 0x{:04x}, expected Broadcom ether type 0x{:04x}", + event_packet.eth.ether_type, ETH_P_LINK_CTL + ); + return; + } + const BROADCOM_OUI: &[u8] = &[0x00, 0x10, 0x18]; + if event_packet.hdr.oui != BROADCOM_OUI { + warn!( + "unexpected ethernet OUI {:02x}, expected Broadcom OUI {:02x}", + Bytes(&event_packet.hdr.oui), + Bytes(BROADCOM_OUI) + ); + return; + } + const BCMILCP_SUBTYPE_VENDOR_LONG: u16 = 32769; + if event_packet.hdr.subtype != BCMILCP_SUBTYPE_VENDOR_LONG { + warn!("unexpected subtype {}", event_packet.hdr.subtype); + return; + } + + const BCMILCP_BCM_SUBTYPE_EVENT: u16 = 1; + if event_packet.hdr.user_subtype != BCMILCP_BCM_SUBTYPE_EVENT { + warn!("unexpected user_subtype {}", event_packet.hdr.subtype); + return; + } + + let evt_type = events::Event::from(event_packet.msg.event_type as u8); + debug!( + "=== EVENT {:?}: {:?} {:02x}", + evt_type, + event_packet.msg, + Bytes(evt_data) + ); + + if self.events.mask.is_enabled(evt_type) { + let status = event_packet.msg.status; + let event_payload = match evt_type { + Event::ESCAN_RESULT if status == EStatus::PARTIAL => { + let Some((_, bss_info)) = ScanResults::parse(evt_data) else { return }; + let Some(bss_info) = BssInfo::parse(bss_info) else { return }; + events::Payload::BssInfo(*bss_info) + } + Event::ESCAN_RESULT => events::Payload::None, + _ => events::Payload::None, + }; + + // this intentionally uses the non-blocking publish immediate + // publish() is a deadlock risk in the current design as awaiting here prevents ioctls + // The `Runner` always yields when accessing the device, so consumers always have a chance to receive the event + // (if they are actively awaiting the queue) + self.events.queue.publish_immediate(events::Message::new( + Status { + event_type: evt_type, + status, + }, + event_payload, + )); + } + } + CHANNEL_TYPE_DATA => { + let Some((_, packet)) = BdcHeader::parse(payload) else { return }; + trace!("rx pkt {:02x}", Bytes(&packet[..packet.len().min(48)])); + + match self.ch.try_rx_buf() { + Some(buf) => { + buf[..packet.len()].copy_from_slice(packet); + self.ch.rx_done(packet.len()) + } + None => warn!("failed to push rxd packet to the channel."), + } + } + _ => {} + } + } + + fn update_credit(&mut self, sdpcm_header: &SdpcmHeader) { + if sdpcm_header.channel_and_flags & 0xf < 3 { + let mut sdpcm_seq_max = sdpcm_header.bus_data_credit; + if sdpcm_seq_max.wrapping_sub(self.sdpcm_seq) > 0x40 { + sdpcm_seq_max = self.sdpcm_seq + 2; + } + self.sdpcm_seq_max = sdpcm_seq_max; + } + } + + fn has_credit(&self) -> bool { + self.sdpcm_seq != self.sdpcm_seq_max && self.sdpcm_seq_max.wrapping_sub(self.sdpcm_seq) & 0x80 == 0 + } + + async fn send_ioctl(&mut self, kind: IoctlType, cmd: u32, iface: u32, data: &[u8]) { + let mut buf = [0; 512]; + let buf8 = slice8_mut(&mut buf); + + let total_len = SdpcmHeader::SIZE + CdcHeader::SIZE + data.len(); + + let sdpcm_seq = self.sdpcm_seq; + self.sdpcm_seq = self.sdpcm_seq.wrapping_add(1); + self.ioctl_id = self.ioctl_id.wrapping_add(1); + + let sdpcm_header = SdpcmHeader { + len: total_len as u16, // TODO does this len need to be rounded up to u32? + len_inv: !total_len as u16, + sequence: sdpcm_seq, + channel_and_flags: CHANNEL_TYPE_CONTROL, + next_length: 0, + header_length: SdpcmHeader::SIZE as _, + wireless_flow_control: 0, + bus_data_credit: 0, + reserved: [0, 0], + }; + + let cdc_header = CdcHeader { + cmd: cmd, + len: data.len() as _, + flags: kind as u16 | (iface as u16) << 12, + id: self.ioctl_id, + status: 0, + }; + trace!("tx {:?}", sdpcm_header); + trace!(" {:?}", cdc_header); + + buf8[0..SdpcmHeader::SIZE].copy_from_slice(&sdpcm_header.to_bytes()); + buf8[SdpcmHeader::SIZE..][..CdcHeader::SIZE].copy_from_slice(&cdc_header.to_bytes()); + buf8[SdpcmHeader::SIZE + CdcHeader::SIZE..][..data.len()].copy_from_slice(data); + + let total_len = (total_len + 3) & !3; // round up to 4byte + + trace!(" {:02x}", Bytes(&buf8[..total_len.min(48)])); + + self.bus.wlan_write(&buf[..total_len / 4]).await; + } + + async fn core_disable(&mut self, core: Core) { + let base = core.base_addr(); + + // Dummy read? + let _ = self.bus.bp_read8(base + AI_RESETCTRL_OFFSET).await; + + // Check it isn't already reset + let r = self.bus.bp_read8(base + AI_RESETCTRL_OFFSET).await; + if r & AI_RESETCTRL_BIT_RESET != 0 { + return; + } + + self.bus.bp_write8(base + AI_IOCTRL_OFFSET, 0).await; + let _ = self.bus.bp_read8(base + AI_IOCTRL_OFFSET).await; + + block_for(Duration::from_millis(1)); + + self.bus + .bp_write8(base + AI_RESETCTRL_OFFSET, AI_RESETCTRL_BIT_RESET) + .await; + let _ = self.bus.bp_read8(base + AI_RESETCTRL_OFFSET).await; + } + + async fn core_reset(&mut self, core: Core) { + self.core_disable(core).await; + + let base = core.base_addr(); + self.bus + .bp_write8(base + AI_IOCTRL_OFFSET, AI_IOCTRL_BIT_FGC | AI_IOCTRL_BIT_CLOCK_EN) + .await; + let _ = self.bus.bp_read8(base + AI_IOCTRL_OFFSET).await; + + self.bus.bp_write8(base + AI_RESETCTRL_OFFSET, 0).await; + + Timer::after(Duration::from_millis(1)).await; + + self.bus + .bp_write8(base + AI_IOCTRL_OFFSET, AI_IOCTRL_BIT_CLOCK_EN) + .await; + let _ = self.bus.bp_read8(base + AI_IOCTRL_OFFSET).await; + + Timer::after(Duration::from_millis(1)).await; + } + + async fn core_is_up(&mut self, core: Core) -> bool { + let base = core.base_addr(); + + let io = self.bus.bp_read8(base + AI_IOCTRL_OFFSET).await; + if io & (AI_IOCTRL_BIT_FGC | AI_IOCTRL_BIT_CLOCK_EN) != AI_IOCTRL_BIT_CLOCK_EN { + debug!("core_is_up: returning false due to bad ioctrl {:02x}", io); + return false; + } + + let r = self.bus.bp_read8(base + AI_RESETCTRL_OFFSET).await; + if r & (AI_RESETCTRL_BIT_RESET) != 0 { + debug!("core_is_up: returning false due to bad resetctrl {:02x}", r); + return false; + } + + true + } +} diff --git a/cyw43/src/structs.rs b/cyw43/src/structs.rs new file mode 100644 index 00000000..5ba633c7 --- /dev/null +++ b/cyw43/src/structs.rs @@ -0,0 +1,496 @@ +use crate::events::Event; +use crate::fmt::Bytes; + +macro_rules! impl_bytes { + ($t:ident) => { + impl $t { + pub const SIZE: usize = core::mem::size_of::(); + + #[allow(unused)] + pub fn to_bytes(&self) -> [u8; Self::SIZE] { + unsafe { core::mem::transmute(*self) } + } + + #[allow(unused)] + pub fn from_bytes(bytes: &[u8; Self::SIZE]) -> &Self { + let alignment = core::mem::align_of::(); + assert_eq!( + bytes.as_ptr().align_offset(alignment), + 0, + "{} is not aligned", + core::any::type_name::() + ); + unsafe { core::mem::transmute(bytes) } + } + + #[allow(unused)] + pub fn from_bytes_mut(bytes: &mut [u8; Self::SIZE]) -> &mut Self { + let alignment = core::mem::align_of::(); + assert_eq!( + bytes.as_ptr().align_offset(alignment), + 0, + "{} is not aligned", + core::any::type_name::() + ); + + unsafe { core::mem::transmute(bytes) } + } + } + }; +} + +#[derive(Clone, Copy)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +#[repr(C)] +pub struct SharedMemData { + pub flags: u32, + pub trap_addr: u32, + pub assert_exp_addr: u32, + pub assert_file_addr: u32, + pub assert_line: u32, + pub console_addr: u32, + pub msgtrace_addr: u32, + pub fwid: u32, +} +impl_bytes!(SharedMemData); + +#[derive(Clone, Copy)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +#[repr(C)] +pub struct SharedMemLog { + pub buf: u32, + pub buf_size: u32, + pub idx: u32, + pub out_idx: u32, +} +impl_bytes!(SharedMemLog); + +#[derive(Debug, Clone, Copy)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +#[repr(C)] +pub struct SdpcmHeader { + pub len: u16, + pub len_inv: u16, + /// Rx/Tx sequence number + pub sequence: u8, + /// 4 MSB Channel number, 4 LSB arbitrary flag + pub channel_and_flags: u8, + /// Length of next data frame, reserved for Tx + pub next_length: u8, + /// Data offset + pub header_length: u8, + /// Flow control bits, reserved for Tx + pub wireless_flow_control: u8, + /// Maximum Sequence number allowed by firmware for Tx + pub bus_data_credit: u8, + /// Reserved + pub reserved: [u8; 2], +} +impl_bytes!(SdpcmHeader); + +impl SdpcmHeader { + pub fn parse(packet: &mut [u8]) -> Option<(&mut Self, &mut [u8])> { + let packet_len = packet.len(); + if packet_len < Self::SIZE { + warn!("packet too short, len={}", packet.len()); + return None; + } + let (sdpcm_header, sdpcm_packet) = packet.split_at_mut(Self::SIZE); + let sdpcm_header = Self::from_bytes_mut(sdpcm_header.try_into().unwrap()); + trace!("rx {:?}", sdpcm_header); + + if sdpcm_header.len != !sdpcm_header.len_inv { + warn!("len inv mismatch"); + return None; + } + + if sdpcm_header.len as usize != packet_len { + warn!("len from header doesn't match len from spi"); + return None; + } + + let sdpcm_packet = &mut sdpcm_packet[(sdpcm_header.header_length as usize - Self::SIZE)..]; + Some((sdpcm_header, sdpcm_packet)) + } +} + +#[derive(Debug, Clone, Copy)] +#[repr(C, packed(2))] +pub struct CdcHeader { + pub cmd: u32, + pub len: u32, + pub flags: u16, + pub id: u16, + pub status: u32, +} +impl_bytes!(CdcHeader); + +#[cfg(feature = "defmt")] +impl defmt::Format for CdcHeader { + fn format(&self, fmt: defmt::Formatter) { + fn copy(t: T) -> T { + t + } + + defmt::write!( + fmt, + "CdcHeader{{cmd: {=u32:08x}, len: {=u32:08x}, flags: {=u16:04x}, id: {=u16:04x}, status: {=u32:08x}}}", + copy(self.cmd), + copy(self.len), + copy(self.flags), + copy(self.id), + copy(self.status), + ) + } +} + +impl CdcHeader { + pub fn parse(packet: &mut [u8]) -> Option<(&mut Self, &mut [u8])> { + if packet.len() < Self::SIZE { + warn!("payload too short, len={}", packet.len()); + return None; + } + + let (cdc_header, payload) = packet.split_at_mut(Self::SIZE); + let cdc_header = Self::from_bytes_mut(cdc_header.try_into().unwrap()); + + let payload = &mut payload[..cdc_header.len as usize]; + Some((cdc_header, payload)) + } +} + +pub const BDC_VERSION: u8 = 2; +pub const BDC_VERSION_SHIFT: u8 = 4; + +#[derive(Debug, Clone, Copy)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +#[repr(C)] +pub struct BdcHeader { + pub flags: u8, + /// 802.1d Priority (low 3 bits) + pub priority: u8, + pub flags2: u8, + /// Offset from end of BDC header to packet data, in 4-uint8_t words. Leaves room for optional headers. + pub data_offset: u8, +} +impl_bytes!(BdcHeader); + +impl BdcHeader { + pub fn parse(packet: &mut [u8]) -> Option<(&mut Self, &mut [u8])> { + if packet.len() < Self::SIZE { + return None; + } + + let (bdc_header, bdc_packet) = packet.split_at_mut(Self::SIZE); + let bdc_header = Self::from_bytes_mut(bdc_header.try_into().unwrap()); + trace!(" {:?}", bdc_header); + + let packet_start = 4 * bdc_header.data_offset as usize; + + let bdc_packet = bdc_packet.get_mut(packet_start..)?; + trace!(" {:02x}", Bytes(&bdc_packet[..bdc_packet.len().min(36)])); + + Some((bdc_header, bdc_packet)) + } +} + +#[derive(Clone, Copy)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +#[repr(C)] +pub struct EthernetHeader { + pub destination_mac: [u8; 6], + pub source_mac: [u8; 6], + pub ether_type: u16, +} + +impl EthernetHeader { + pub fn byteswap(&mut self) { + self.ether_type = self.ether_type.to_be(); + } +} + +#[derive(Clone, Copy)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +#[repr(C)] +pub struct EventHeader { + pub subtype: u16, + pub length: u16, + pub version: u8, + pub oui: [u8; 3], + pub user_subtype: u16, +} + +impl EventHeader { + pub fn byteswap(&mut self) { + self.subtype = self.subtype.to_be(); + self.length = self.length.to_be(); + self.user_subtype = self.user_subtype.to_be(); + } +} + +#[derive(Debug, Clone, Copy)] +// #[cfg_attr(feature = "defmt", derive(defmt::Format))] +#[repr(C, packed(2))] +pub struct EventMessage { + /// version + pub version: u16, + /// see flags below + pub flags: u16, + /// Message (see below) + pub event_type: u32, + /// Status code (see below) + pub status: u32, + /// Reason code (if applicable) + pub reason: u32, + /// WLC_E_AUTH + pub auth_type: u32, + /// data buf + pub datalen: u32, + /// Station address (if applicable) + pub addr: [u8; 6], + /// name of the incoming packet interface + pub ifname: [u8; 16], + /// destination OS i/f index + pub ifidx: u8, + /// source bsscfg index + pub bsscfgidx: u8, +} +impl_bytes!(EventMessage); + +#[cfg(feature = "defmt")] +impl defmt::Format for EventMessage { + fn format(&self, fmt: defmt::Formatter) { + let event_type = self.event_type; + let status = self.status; + let reason = self.reason; + let auth_type = self.auth_type; + let datalen = self.datalen; + + defmt::write!( + fmt, + "EventMessage {{ \ + version: {=u16}, \ + flags: {=u16}, \ + event_type: {=u32}, \ + status: {=u32}, \ + reason: {=u32}, \ + auth_type: {=u32}, \ + datalen: {=u32}, \ + addr: {=[u8; 6]:x}, \ + ifname: {=[u8; 16]:x}, \ + ifidx: {=u8}, \ + bsscfgidx: {=u8}, \ + }} ", + self.version, + self.flags, + event_type, + status, + reason, + auth_type, + datalen, + self.addr, + self.ifname, + self.ifidx, + self.bsscfgidx + ); + } +} + +impl EventMessage { + pub fn byteswap(&mut self) { + self.version = self.version.to_be(); + self.flags = self.flags.to_be(); + self.event_type = self.event_type.to_be(); + self.status = self.status.to_be(); + self.reason = self.reason.to_be(); + self.auth_type = self.auth_type.to_be(); + self.datalen = self.datalen.to_be(); + } +} + +#[derive(Clone, Copy)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +#[repr(C, packed(2))] +pub struct EventPacket { + pub eth: EthernetHeader, + pub hdr: EventHeader, + pub msg: EventMessage, +} +impl_bytes!(EventPacket); + +impl EventPacket { + pub fn parse(packet: &mut [u8]) -> Option<(&mut Self, &mut [u8])> { + if packet.len() < Self::SIZE { + return None; + } + + let (event_header, event_packet) = packet.split_at_mut(Self::SIZE); + let event_header = Self::from_bytes_mut(event_header.try_into().unwrap()); + // warn!("event_header {:x}", event_header as *const _); + event_header.byteswap(); + + let event_packet = event_packet.get_mut(..event_header.msg.datalen as usize)?; + + Some((event_header, event_packet)) + } + + pub fn byteswap(&mut self) { + self.eth.byteswap(); + self.hdr.byteswap(); + self.msg.byteswap(); + } +} + +#[derive(Clone, Copy)] +#[repr(C)] +pub struct DownloadHeader { + pub flag: u16, // + pub dload_type: u16, + pub len: u32, + pub crc: u32, +} +impl_bytes!(DownloadHeader); + +#[allow(unused)] +pub const DOWNLOAD_FLAG_NO_CRC: u16 = 0x0001; +pub const DOWNLOAD_FLAG_BEGIN: u16 = 0x0002; +pub const DOWNLOAD_FLAG_END: u16 = 0x0004; +pub const DOWNLOAD_FLAG_HANDLER_VER: u16 = 0x1000; + +// Country Locale Matrix (CLM) +pub const DOWNLOAD_TYPE_CLM: u16 = 2; + +#[derive(Clone, Copy)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +#[repr(C)] +pub struct CountryInfo { + pub country_abbrev: [u8; 4], + pub rev: i32, + pub country_code: [u8; 4], +} +impl_bytes!(CountryInfo); + +#[derive(Clone, Copy)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +#[repr(C)] +pub struct SsidInfo { + pub len: u32, + pub ssid: [u8; 32], +} +impl_bytes!(SsidInfo); + +#[derive(Clone, Copy)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +#[repr(C)] +pub struct PassphraseInfo { + pub len: u16, + pub flags: u16, + pub passphrase: [u8; 64], +} +impl_bytes!(PassphraseInfo); + +#[derive(Clone, Copy)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +#[repr(C)] +pub struct SsidInfoWithIndex { + pub index: u32, + pub ssid_info: SsidInfo, +} +impl_bytes!(SsidInfoWithIndex); + +#[derive(Clone, Copy)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +#[repr(C)] +pub struct EventMask { + pub iface: u32, + pub events: [u8; 24], +} +impl_bytes!(EventMask); + +impl EventMask { + pub fn unset(&mut self, evt: Event) { + let evt = evt as u8 as usize; + self.events[evt / 8] &= !(1 << (evt % 8)); + } +} + +/// Parameters for a wifi scan +#[derive(Clone, Copy)] +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +#[repr(C)] +pub struct ScanParams { + pub version: u32, + pub action: u16, + pub sync_id: u16, + pub ssid_len: u32, + pub ssid: [u8; 32], + pub bssid: [u8; 6], + pub bss_type: u8, + pub scan_type: u8, + pub nprobes: u32, + pub active_time: u32, + pub passive_time: u32, + pub home_time: u32, + pub channel_num: u32, + pub channel_list: [u16; 1], +} +impl_bytes!(ScanParams); + +/// Wifi Scan Results Header, followed by `bss_count` `BssInfo` +#[derive(Clone, Copy)] +// #[cfg_attr(feature = "defmt", derive(defmt::Format))] +#[repr(C, packed(2))] +pub struct ScanResults { + pub buflen: u32, + pub version: u32, + pub sync_id: u16, + pub bss_count: u16, +} +impl_bytes!(ScanResults); + +impl ScanResults { + pub fn parse(packet: &mut [u8]) -> Option<(&mut ScanResults, &mut [u8])> { + if packet.len() < ScanResults::SIZE { + return None; + } + + let (scan_results, bssinfo) = packet.split_at_mut(ScanResults::SIZE); + let scan_results = ScanResults::from_bytes_mut(scan_results.try_into().unwrap()); + + if scan_results.bss_count > 0 && bssinfo.len() < BssInfo::SIZE { + warn!("Scan result, incomplete BssInfo"); + return None; + } + + Some((scan_results, bssinfo)) + } +} + +/// Wifi Scan Result +#[derive(Clone, Copy)] +// #[cfg_attr(feature = "defmt", derive(defmt::Format))] +#[repr(C, packed(2))] +#[non_exhaustive] +pub struct BssInfo { + pub version: u32, + pub length: u32, + pub bssid: [u8; 6], + pub beacon_period: u16, + pub capability: u16, + pub ssid_len: u8, + pub ssid: [u8; 32], + // there will be more stuff here +} +impl_bytes!(BssInfo); + +impl BssInfo { + pub fn parse(packet: &mut [u8]) -> Option<&mut Self> { + if packet.len() < BssInfo::SIZE { + return None; + } + + Some(BssInfo::from_bytes_mut( + packet[..BssInfo::SIZE].as_mut().try_into().unwrap(), + )) + } +} diff --git a/examples/rp/Cargo.toml b/examples/rp/Cargo.toml index ffeb69f1..f77377a6 100644 --- a/examples/rp/Cargo.toml +++ b/examples/rp/Cargo.toml @@ -19,6 +19,8 @@ embassy-lora = { version = "0.1.0", path = "../../embassy-lora", features = ["ti lora-phy = { version = "1" } lorawan-device = { version = "0.10.0", default-features = false, features = ["async", "external-lora-phy"] } lorawan = { version = "0.7.3", default-features = false, features = ["default-crypto"] } +cyw43 = { path = "../../cyw43", features = ["defmt", "firmware-logs"] } +cyw43-pio = { path = "../../cyw43-pio", features = ["defmt", "overclock"] } defmt = "0.3" defmt-rtt = "0.4" @@ -36,6 +38,7 @@ st7789 = "0.6.1" display-interface = "0.4.1" byte-slice-cast = { version = "1.2.0", default-features = false } smart-leds = "0.3.0" +heapless = "0.7.15" embedded-hal-1 = { package = "embedded-hal", version = "=1.0.0-alpha.10" } embedded-hal-async = "0.2.0-alpha.1" diff --git a/examples/rp/src/bin/wifi_ap_tcp_server.rs b/examples/rp/src/bin/wifi_ap_tcp_server.rs new file mode 100644 index 00000000..15264524 --- /dev/null +++ b/examples/rp/src/bin/wifi_ap_tcp_server.rs @@ -0,0 +1,139 @@ +#![no_std] +#![no_main] +#![feature(type_alias_impl_trait)] +#![feature(async_fn_in_trait)] +#![allow(incomplete_features)] + +use core::str::from_utf8; + +use cyw43_pio::PioSpi; +use defmt::*; +use embassy_executor::Spawner; +use embassy_net::tcp::TcpSocket; +use embassy_net::{Config, Stack, StackResources}; +use embassy_rp::gpio::{Level, Output}; +use embassy_rp::peripherals::{DMA_CH0, PIN_23, PIN_25, PIO0}; +use embassy_rp::pio::Pio; +use embassy_time::Duration; +use embedded_io::asynch::Write; +use static_cell::StaticCell; +use {defmt_rtt as _, panic_probe as _}; + +macro_rules! singleton { + ($val:expr) => {{ + type T = impl Sized; + static STATIC_CELL: StaticCell = StaticCell::new(); + STATIC_CELL.init_with(move || $val) + }}; +} + +#[embassy_executor::task] +async fn wifi_task( + runner: cyw43::Runner<'static, Output<'static, PIN_23>, PioSpi<'static, PIN_25, PIO0, 0, DMA_CH0>>, +) -> ! { + runner.run().await +} + +#[embassy_executor::task] +async fn net_task(stack: &'static Stack>) -> ! { + stack.run().await +} + +#[embassy_executor::main] +async fn main(spawner: Spawner) { + info!("Hello World!"); + + let p = embassy_rp::init(Default::default()); + + let fw = include_bytes!("../../../../cyw43-firmware/43439A0.bin"); + let clm = include_bytes!("../../../../cyw43-firmware/43439A0_clm.bin"); + + // To make flashing faster for development, you may want to flash the firmwares independently + // at hardcoded addresses, instead of baking them into the program with `include_bytes!`: + // probe-rs-cli download 43439A0.bin --format bin --chip RP2040 --base-address 0x10100000 + // probe-rs-cli download 43439A0_clm.bin --format bin --chip RP2040 --base-address 0x10140000 + //let fw = unsafe { core::slice::from_raw_parts(0x10100000 as *const u8, 224190) }; + //let clm = unsafe { core::slice::from_raw_parts(0x10140000 as *const u8, 4752) }; + + let pwr = Output::new(p.PIN_23, Level::Low); + let cs = Output::new(p.PIN_25, Level::High); + let mut pio = Pio::new(p.PIO0); + let spi = PioSpi::new(&mut pio.common, pio.sm0, pio.irq0, cs, p.PIN_24, p.PIN_29, p.DMA_CH0); + + let state = singleton!(cyw43::State::new()); + let (net_device, mut control, runner) = cyw43::new(state, pwr, spi, fw).await; + unwrap!(spawner.spawn(wifi_task(runner))); + + control.init(clm).await; + control + .set_power_management(cyw43::PowerManagementMode::PowerSave) + .await; + + // Use a link-local address for communication without DHCP server + let config = Config::Static(embassy_net::StaticConfig { + address: embassy_net::Ipv4Cidr::new(embassy_net::Ipv4Address::new(169, 254, 1, 1), 16), + dns_servers: heapless::Vec::new(), + gateway: None, + }); + + // Generate random seed + let seed = 0x0123_4567_89ab_cdef; // chosen by fair dice roll. guarenteed to be random. + + // Init network stack + let stack = &*singleton!(Stack::new( + net_device, + config, + singleton!(StackResources::<2>::new()), + seed + )); + + unwrap!(spawner.spawn(net_task(stack))); + + //control.start_ap_open("cyw43", 5).await; + control.start_ap_wpa2("cyw43", "password", 5).await; + + // And now we can use it! + + let mut rx_buffer = [0; 4096]; + let mut tx_buffer = [0; 4096]; + let mut buf = [0; 4096]; + + loop { + let mut socket = TcpSocket::new(stack, &mut rx_buffer, &mut tx_buffer); + socket.set_timeout(Some(Duration::from_secs(10))); + + control.gpio_set(0, false).await; + info!("Listening on TCP:1234..."); + if let Err(e) = socket.accept(1234).await { + warn!("accept error: {:?}", e); + continue; + } + + info!("Received connection from {:?}", socket.remote_endpoint()); + control.gpio_set(0, true).await; + + loop { + let n = match socket.read(&mut buf).await { + Ok(0) => { + warn!("read EOF"); + break; + } + Ok(n) => n, + Err(e) => { + warn!("read error: {:?}", e); + break; + } + }; + + info!("rxd {}", from_utf8(&buf[..n]).unwrap()); + + match socket.write_all(&buf[..n]).await { + Ok(()) => {} + Err(e) => { + warn!("write error: {:?}", e); + break; + } + }; + } + } +} diff --git a/examples/rp/src/bin/wifi_scan.rs b/examples/rp/src/bin/wifi_scan.rs new file mode 100644 index 00000000..aa5e5a39 --- /dev/null +++ b/examples/rp/src/bin/wifi_scan.rs @@ -0,0 +1,75 @@ +#![no_std] +#![no_main] +#![feature(type_alias_impl_trait)] +#![feature(async_fn_in_trait)] +#![allow(incomplete_features)] + +use core::str; + +use cyw43_pio::PioSpi; +use defmt::*; +use embassy_executor::Spawner; +use embassy_net::Stack; +use embassy_rp::gpio::{Level, Output}; +use embassy_rp::peripherals::{DMA_CH0, PIN_23, PIN_25, PIO0}; +use embassy_rp::pio::Pio; +use static_cell::StaticCell; +use {defmt_rtt as _, panic_probe as _}; + +macro_rules! singleton { + ($val:expr) => {{ + type T = impl Sized; + static STATIC_CELL: StaticCell = StaticCell::new(); + STATIC_CELL.init_with(move || $val) + }}; +} + +#[embassy_executor::task] +async fn wifi_task( + runner: cyw43::Runner<'static, Output<'static, PIN_23>, PioSpi<'static, PIN_25, PIO0, 0, DMA_CH0>>, +) -> ! { + runner.run().await +} + +#[embassy_executor::task] +async fn net_task(stack: &'static Stack>) -> ! { + stack.run().await +} + +#[embassy_executor::main] +async fn main(spawner: Spawner) { + info!("Hello World!"); + + let p = embassy_rp::init(Default::default()); + + let fw = include_bytes!("../../../../cyw43-firmware/43439A0.bin"); + let clm = include_bytes!("../../../../cyw43-firmware/43439A0_clm.bin"); + + // To make flashing faster for development, you may want to flash the firmwares independently + // at hardcoded addresses, instead of baking them into the program with `include_bytes!`: + // probe-rs-cli download 43439A0.bin --format bin --chip RP2040 --base-address 0x10100000 + // probe-rs-cli download 43439A0_clm.bin --format bin --chip RP2040 --base-address 0x10140000 + //let fw = unsafe { core::slice::from_raw_parts(0x10100000 as *const u8, 224190) }; + //let clm = unsafe { core::slice::from_raw_parts(0x10140000 as *const u8, 4752) }; + + let pwr = Output::new(p.PIN_23, Level::Low); + let cs = Output::new(p.PIN_25, Level::High); + let mut pio = Pio::new(p.PIO0); + let spi = PioSpi::new(&mut pio.common, pio.sm0, pio.irq0, cs, p.PIN_24, p.PIN_29, p.DMA_CH0); + + let state = singleton!(cyw43::State::new()); + let (_net_device, mut control, runner) = cyw43::new(state, pwr, spi, fw).await; + unwrap!(spawner.spawn(wifi_task(runner))); + + control.init(clm).await; + control + .set_power_management(cyw43::PowerManagementMode::PowerSave) + .await; + + let mut scanner = control.scan().await; + while let Some(bss) = scanner.next().await { + if let Ok(ssid_str) = str::from_utf8(&bss.ssid) { + info!("scanned {} == {:x}", ssid_str, bss.bssid); + } + } +} diff --git a/examples/rp/src/bin/wifi_tcp_server.rs b/examples/rp/src/bin/wifi_tcp_server.rs new file mode 100644 index 00000000..eafa25f6 --- /dev/null +++ b/examples/rp/src/bin/wifi_tcp_server.rs @@ -0,0 +1,146 @@ +#![no_std] +#![no_main] +#![feature(type_alias_impl_trait)] +#![feature(async_fn_in_trait)] +#![allow(incomplete_features)] + +use core::str::from_utf8; + +use cyw43_pio::PioSpi; +use defmt::*; +use embassy_executor::Spawner; +use embassy_net::tcp::TcpSocket; +use embassy_net::{Config, Stack, StackResources}; +use embassy_rp::gpio::{Level, Output}; +use embassy_rp::peripherals::{DMA_CH0, PIN_23, PIN_25, PIO0}; +use embassy_rp::pio::Pio; +use embassy_time::Duration; +use embedded_io::asynch::Write; +use static_cell::StaticCell; +use {defmt_rtt as _, panic_probe as _}; + +macro_rules! singleton { + ($val:expr) => {{ + type T = impl Sized; + static STATIC_CELL: StaticCell = StaticCell::new(); + STATIC_CELL.init_with(move || $val) + }}; +} + +#[embassy_executor::task] +async fn wifi_task( + runner: cyw43::Runner<'static, Output<'static, PIN_23>, PioSpi<'static, PIN_25, PIO0, 0, DMA_CH0>>, +) -> ! { + runner.run().await +} + +#[embassy_executor::task] +async fn net_task(stack: &'static Stack>) -> ! { + stack.run().await +} + +#[embassy_executor::main] +async fn main(spawner: Spawner) { + info!("Hello World!"); + + let p = embassy_rp::init(Default::default()); + + let fw = include_bytes!("../../../../cyw43-firmware/43439A0.bin"); + let clm = include_bytes!("../../../../cyw43-firmware/43439A0_clm.bin"); + + // To make flashing faster for development, you may want to flash the firmwares independently + // at hardcoded addresses, instead of baking them into the program with `include_bytes!`: + // probe-rs-cli download 43439A0.bin --format bin --chip RP2040 --base-address 0x10100000 + // probe-rs-cli download 43439A0_clm.bin --format bin --chip RP2040 --base-address 0x10140000 + //let fw = unsafe { core::slice::from_raw_parts(0x10100000 as *const u8, 224190) }; + //let clm = unsafe { core::slice::from_raw_parts(0x10140000 as *const u8, 4752) }; + + let pwr = Output::new(p.PIN_23, Level::Low); + let cs = Output::new(p.PIN_25, Level::High); + let mut pio = Pio::new(p.PIO0); + let spi = PioSpi::new(&mut pio.common, pio.sm0, pio.irq0, cs, p.PIN_24, p.PIN_29, p.DMA_CH0); + + let state = singleton!(cyw43::State::new()); + let (net_device, mut control, runner) = cyw43::new(state, pwr, spi, fw).await; + unwrap!(spawner.spawn(wifi_task(runner))); + + control.init(clm).await; + control + .set_power_management(cyw43::PowerManagementMode::PowerSave) + .await; + + let config = Config::Dhcp(Default::default()); + //let config = embassy_net::Config::Static(embassy_net::Config { + // address: Ipv4Cidr::new(Ipv4Address::new(192, 168, 69, 2), 24), + // dns_servers: Vec::new(), + // gateway: Some(Ipv4Address::new(192, 168, 69, 1)), + //}); + + // Generate random seed + let seed = 0x0123_4567_89ab_cdef; // chosen by fair dice roll. guarenteed to be random. + + // Init network stack + let stack = &*singleton!(Stack::new( + net_device, + config, + singleton!(StackResources::<2>::new()), + seed + )); + + unwrap!(spawner.spawn(net_task(stack))); + + loop { + //control.join_open(env!("WIFI_NETWORK")).await; + match control.join_wpa2(env!("WIFI_NETWORK"), env!("WIFI_PASSWORD")).await { + Ok(_) => break, + Err(err) => { + info!("join failed with status={}", err.status); + } + } + } + + // And now we can use it! + + let mut rx_buffer = [0; 4096]; + let mut tx_buffer = [0; 4096]; + let mut buf = [0; 4096]; + + loop { + let mut socket = TcpSocket::new(stack, &mut rx_buffer, &mut tx_buffer); + socket.set_timeout(Some(Duration::from_secs(10))); + + control.gpio_set(0, false).await; + info!("Listening on TCP:1234..."); + if let Err(e) = socket.accept(1234).await { + warn!("accept error: {:?}", e); + continue; + } + + info!("Received connection from {:?}", socket.remote_endpoint()); + control.gpio_set(0, true).await; + + loop { + let n = match socket.read(&mut buf).await { + Ok(0) => { + warn!("read EOF"); + break; + } + Ok(n) => n, + Err(e) => { + warn!("read error: {:?}", e); + break; + } + }; + + info!("rxd {}", from_utf8(&buf[..n]).unwrap()); + + match socket.write_all(&buf[..n]).await { + Ok(()) => {} + Err(e) => { + warn!("write error: {:?}", e); + break; + } + }; + } + } +}