Joey Yakimowich-Payne
80b788099b
Preserve native frequency/amplitude timelines for DualSense PCM output, remove the unsupported 64-frame path, and retain compatibility rumble for other controllers. Use the validated 300 MHz sampling phase and restrict non-bondable Classic discovery autoconnect to previously paired peers. Buffer native-hub UART stdout and release USB IRQs around port-reset callbacks. Add observer liveness and pre-SETUP root-response observations without changing recovery behavior. Cover transport and haptics boundaries. Record the user-accepted 0.108 trial: controls remained responsive and rumble felt fine. Instrumentation changes timing; the disconnect root cause and long-term reliability remain unqualified. Validation: 624 tests, 11 affected firmware/probe builds, and on-device concurrent USB and HD-auto-start checks. Private captures, generated images, and unrelated working-tree files are intentionally excluded.
889 lines
36 KiB
C
889 lines
36 KiB
C
#include "router.h"
|
|
|
|
#include <string.h>
|
|
|
|
#include "pico.h"
|
|
#include "hardware/structs/sio.h"
|
|
#include "hardware/structs/usb.h"
|
|
#include "hardware/sync.h"
|
|
|
|
#if defined(SWITCH2_PROBE_HUB) && SWITCH2_PROBE_HUB
|
|
extern bool native_hub_select_device(uint8_t address, uint8_t owner, uint32_t cutoff);
|
|
#if defined(SWITCH2_PROBE_TRACE_NATIVE_INPUT)
|
|
#include "usb/native_hub/native_hub_trace.h"
|
|
#endif
|
|
#endif
|
|
|
|
#if !PICO_RP2350 || defined(__riscv)
|
|
#error "The native PHY observer requires an RP2350 Arm core"
|
|
#endif
|
|
|
|
// This sampler does not drive USB data. Main enables the native-pad input
|
|
// mux and attachment pull-up; the native SIE remains the USB transmitter.
|
|
// This isolated probe owns SIO MTIME, usable by a Secure Arm core. FULLSPEED
|
|
// makes it a zero-wait-state cycle counter next to the GPIO inputs, avoiding
|
|
// SysTick's PPB accesses and 24-bit down-counter arithmetic in every sample.
|
|
// Deadlines use modular 32-bit arithmetic for intervals below 2^31 cycles.
|
|
#define LINE_SE0 0u
|
|
#define LINE_J 1u
|
|
#define LINE_K 2u
|
|
#define LINE_SE1 3u
|
|
#define PID_OUT 0xe1u
|
|
#define PID_IN 0x69u
|
|
#define PID_SETUP 0x2du
|
|
// NRZI SYNC+PID words, with K=2 and J=1 packed into two-bit samples.
|
|
#define TOKEN_OUT_SIGNATURE 0xaa66a666u
|
|
#define TOKEN_IN_SIGNATURE 0x95a6a666u
|
|
#define TOKEN_SETUP_SIGNATURE 0x9a56a666u
|
|
#define NO_READER 2u
|
|
#if PROBE_ROUTER_SLOTS > 3u
|
|
#define SETUP_SLOT_BITS 3u
|
|
#else
|
|
#define SETUP_SLOT_BITS 2u
|
|
#endif
|
|
#define SETUP_SLOT_SHIFT (32u - SETUP_SLOT_BITS)
|
|
#define SETUP_SEQUENCE_MASK ((1u << SETUP_SLOT_SHIFT) - 1u)
|
|
#define SETUP_INVALID_OWNER ((1u << SETUP_SLOT_BITS) - 1u)
|
|
#define SETUP_INVALID (SETUP_INVALID_OWNER << SETUP_SLOT_SHIFT)
|
|
#define RAW_BITS 40u
|
|
|
|
_Static_assert(SIO_GPIO_HI_IN_USB_DP_BITS == (1u << 24), "SIO USB DP layout");
|
|
_Static_assert(SIO_GPIO_HI_IN_USB_DM_BITS == (1u << 25), "SIO USB DM layout");
|
|
_Static_assert(PROBE_ROUTER_SLOTS <= SETUP_INVALID_OWNER, "Packed setup owner must reserve an invalid value");
|
|
|
|
typedef struct {
|
|
uint8_t owner[128];
|
|
#if defined(SWITCH2_PROBE_HUB) && SWITCH2_PROBE_HUB
|
|
uint8_t early_address[2][256];
|
|
#endif
|
|
} routing_table;
|
|
|
|
// Complete physical NRZI SYNC+PID signatures. The PID's two distinguishing
|
|
// symbols index this table, but the entire signature must match.
|
|
static uint32_t token_words[16];
|
|
|
|
typedef struct {
|
|
uint32_t words[3];
|
|
uint32_t count;
|
|
uint32_t retargets;
|
|
bool eop;
|
|
bool late;
|
|
bool sop;
|
|
bool resync;
|
|
} raw_packet;
|
|
|
|
static routing_table tables[2];
|
|
static probe_router_stats counters;
|
|
static uint32_t published_generation;
|
|
static uint32_t reader_index;
|
|
static uint32_t enabled;
|
|
static uint32_t phase_cycles;
|
|
static uint32_t setup_publication;
|
|
static uint32_t fatal_fault;
|
|
static bool valid_clock;
|
|
static uint8_t address_decoder[2][256];
|
|
static bool address_decoder_ready;
|
|
#if defined(SWITCH2_PROBE_HUB) && SWITCH2_PROBE_HUB && defined(SWITCH2_PROBE_TRACE_NATIVE_INPUT)
|
|
enum { ROOT_OBSERVE_IDLE, ROOT_OBSERVE_HEADER, ROOT_OBSERVE_EOP };
|
|
static uint8_t root_observation_stage; // Core1 only after initialization.
|
|
#endif
|
|
|
|
static __force_inline uint32_t atomic_read(const uint32_t* value) {
|
|
return __atomic_load_n(value, __ATOMIC_RELAXED);
|
|
}
|
|
|
|
static __force_inline void atomic_write(uint32_t* value, uint32_t next) {
|
|
__atomic_store_n(value, next, __ATOMIC_RELAXED);
|
|
}
|
|
|
|
// These counters have one writer (Core 1); only loads/stores, not exclusive
|
|
// read-modify-write loops, are needed. They are updated outside sample windows.
|
|
static __force_inline void count_one(uint32_t* counter) {
|
|
atomic_write(counter, atomic_read(counter) + 1u);
|
|
}
|
|
|
|
static __force_inline void invalidate_setup(void) {
|
|
const uint32_t previous = atomic_read(&setup_publication);
|
|
__atomic_store_n(&setup_publication,
|
|
(previous & SETUP_SEQUENCE_MASK) | SETUP_INVALID,
|
|
__ATOMIC_RELEASE);
|
|
}
|
|
|
|
static __force_inline void publish_setup(uint8_t slot) {
|
|
const uint32_t sequence = (atomic_read(&setup_publication) + 1u) & SETUP_SEQUENCE_MASK;
|
|
const uint32_t owner = slot < PROBE_ROUTER_SLOTS ? slot : SETUP_INVALID_OWNER;
|
|
__atomic_store_n(&setup_publication, sequence | (owner << SETUP_SLOT_SHIFT),
|
|
__ATOMIC_RELEASE);
|
|
}
|
|
|
|
|
|
static void build_address_decoder(void) {
|
|
if (address_decoder_ready) return;
|
|
// C0 initializes once. Eight observed D+ symbols cover all seven address
|
|
// bits plus at most one stuffed bit. All three token PIDs end in K.
|
|
for (unsigned kind = 0; kind < 2; ++kind) {
|
|
for (unsigned wire = 0; wire < 256; ++wire) {
|
|
unsigned previous = 0, ones = kind ? 3u : 0u, bits = 0, address = 0;
|
|
bool valid = true;
|
|
for (unsigned n = 0; n < 8 && bits < 7; ++n) {
|
|
const unsigned line = (wire >> n) & 1u;
|
|
const unsigned bit = line == previous;
|
|
previous = line;
|
|
if (ones == 6u) {
|
|
if (bit != 0u) valid = false;
|
|
ones = 0;
|
|
continue;
|
|
}
|
|
address |= bit << bits++;
|
|
ones = bit ? ones + 1u : 0u;
|
|
}
|
|
address_decoder[kind][wire] = valid && bits == 7 ?
|
|
(uint8_t)address : PROBE_ROUTER_UNASSIGNED;
|
|
}
|
|
}
|
|
address_decoder_ready = true;
|
|
}
|
|
|
|
static void build_table(routing_table* table,
|
|
const uint8_t addresses[PROBE_ROUTER_SLOTS], uint8_t default_slot) {
|
|
build_address_decoder();
|
|
memset(table->owner, PROBE_ROUTER_UNASSIGNED, sizeof(table->owner));
|
|
if (default_slot < PROBE_ROUTER_SLOTS)
|
|
table->owner[0] = default_slot;
|
|
for (uint8_t slot = 0; slot < PROBE_ROUTER_SLOTS; ++slot) {
|
|
const uint8_t address = addresses[slot];
|
|
if (address == 0 || address >= 128) continue;
|
|
bool unique = true;
|
|
for (uint8_t other = 0; other < PROBE_ROUTER_SLOTS; ++other) {
|
|
if (other != slot && addresses[other] == address)
|
|
unique = false;
|
|
}
|
|
if (unique)
|
|
table->owner[address] = slot;
|
|
}
|
|
#if defined(SWITCH2_PROBE_HUB) && SWITCH2_PROBE_HUB
|
|
// A unique observed prefix can preselect the SIE sooner. It still compares
|
|
// the complete hardware address and CRC before accepting the transaction.
|
|
// Index the four captured line pairs directly. Packing their D+ bits in
|
|
// the sampling window delays bit 20 even when the prefix is ambiguous.
|
|
memset(table->early_address, PROBE_ROUTER_UNASSIGNED, sizeof(table->early_address));
|
|
for (unsigned kind = 0; kind < 2; ++kind) {
|
|
for (unsigned prefix = 0; prefix < 16; ++prefix) {
|
|
uint8_t candidate = PROBE_ROUTER_UNASSIGNED;
|
|
for (unsigned suffix = 0; suffix < 16; ++suffix) {
|
|
uint8_t address = address_decoder[kind][prefix | (suffix << 4)];
|
|
if (address >= 128 || table->owner[address] >= PROBE_ROUTER_SLOTS) continue;
|
|
if (candidate != PROBE_ROUTER_UNASSIGNED && candidate != address) {
|
|
candidate = PROBE_ROUTER_UNASSIGNED;
|
|
break;
|
|
}
|
|
candidate = address;
|
|
}
|
|
unsigned raw_prefix = 0u;
|
|
for (unsigned bit = 0; bit < 4; ++bit)
|
|
raw_prefix |= ((prefix >> bit) & 1u ? LINE_J : LINE_K) << (2u * bit);
|
|
table->early_address[kind][raw_prefix] = candidate;
|
|
}
|
|
}
|
|
#endif
|
|
}
|
|
|
|
void probe_router_init(uint32_t system_clock_hz) {
|
|
// Explicit SRAM data: Core1 must never fetch flash during durable saves.
|
|
token_words[6] = TOKEN_OUT_SIGNATURE;
|
|
token_words[10] = TOKEN_IN_SIGNATURE;
|
|
token_words[5] = TOKEN_SETUP_SIGNATURE;
|
|
uint8_t addresses[PROBE_ROUTER_SLOTS];
|
|
memset(addresses, PROBE_ROUTER_UNASSIGNED, sizeof(addresses));
|
|
addresses[0] = 0u;
|
|
memset(&counters, 0, sizeof(counters));
|
|
#if defined(SWITCH2_PROBE_HUB) && SWITCH2_PROBE_HUB && defined(SWITCH2_PROBE_TRACE_NATIVE_INPUT)
|
|
root_observation_stage = ROOT_OBSERVE_IDLE;
|
|
#endif
|
|
published_generation = 0u;
|
|
reader_index = NO_READER;
|
|
enabled = 0u;
|
|
phase_cycles = 0u;
|
|
setup_publication = SETUP_INVALID;
|
|
fatal_fault = 0u;
|
|
valid_clock = system_clock_hz == FS_CLOCK_HZ;
|
|
counters.cycles_per_bit = valid_clock ? FS_BIT_CYCLES : 0u;
|
|
build_table(&tables[0], addresses, 0u);
|
|
}
|
|
|
|
void probe_router_publish(const uint8_t addresses[PROBE_ROUTER_SLOTS], uint8_t default_slot) {
|
|
const uint32_t generation = atomic_read(&published_generation);
|
|
const uint32_t next_index = (generation + 1u) & 1u;
|
|
// A pointer swap alone is NOT safe double buffering: a second publication
|
|
// could overwrite the table still in use by a packet. The reader's hazard
|
|
// index protects that table until decoding finishes. Core 1 never waits.
|
|
// Bound the writer's wait as well: an unexpectedly stopped observer must
|
|
// not trap Core 0 or prevent the watchdog/reboot control path from running.
|
|
uint32_t remaining = 1000000u;
|
|
while (__atomic_load_n(&reader_index, __ATOMIC_SEQ_CST) == next_index) {
|
|
if (--remaining == 0u) {
|
|
atomic_write(&enabled, 0u);
|
|
atomic_write(&fatal_fault, 1u);
|
|
atomic_write(&counters.ready, 0u);
|
|
return;
|
|
}
|
|
}
|
|
build_table(&tables[next_index], addresses, default_slot);
|
|
__atomic_store_n(&published_generation, generation + 1u, __ATOMIC_SEQ_CST);
|
|
}
|
|
|
|
void probe_router_enable(bool enable) {
|
|
// ARM qualification (observed hub tokens/SETUPs) belongs to the control
|
|
// request handler. This additionally prevents enabling a failed observer.
|
|
__atomic_store_n(&enabled, enable && valid_clock && atomic_read(&counters.ready) != 0u &&
|
|
atomic_read(&fatal_fault) == 0u, __ATOMIC_RELEASE);
|
|
}
|
|
|
|
bool probe_router_set_phase(uint32_t cycles) {
|
|
if (!valid_clock || cycles >= FS_BIT_CYCLES || atomic_read(&enabled) != 0u)
|
|
return false;
|
|
__atomic_store_n(&phase_cycles, cycles, __ATOMIC_RELEASE);
|
|
return true;
|
|
}
|
|
|
|
void probe_router_snapshot(probe_router_stats* out) {
|
|
#define SNAPSHOT(member) out->member = atomic_read(&counters.member)
|
|
SNAPSHOT(ready);
|
|
SNAPSHOT(sops);
|
|
SNAPSHOT(sync_ok);
|
|
SNAPSHOT(valid_tokens);
|
|
SNAPSHOT(valid_setups);
|
|
SNAPSHOT(crc_errors);
|
|
SNAPSHOT(late_samples);
|
|
SNAPSHOT(retargets);
|
|
for (uint32_t slot = 0u; slot < PROBE_ROUTER_SLOTS; ++slot)
|
|
out->address_hits[slot] = atomic_read(&counters.address_hits[slot]);
|
|
SNAPSHOT(cycles_per_bit);
|
|
SNAPSHOT(last_pid);
|
|
SNAPSHOT(last_address);
|
|
for (uint32_t i = 0; i < 3; ++i)
|
|
out->last_raw[i] = atomic_read(&counters.last_raw[i]);
|
|
SNAPSHOT(last_raw_count);
|
|
SNAPSHOT(last_raw_eop);
|
|
SNAPSHOT(last_raw_late);
|
|
SNAPSHOT(capture_returns);
|
|
SNAPSHOT(discarded_headers);
|
|
SNAPSHOT(last_discarded_header);
|
|
SNAPSHOT(root_in_count);
|
|
SNAPSHOT(root_in_cutoff);
|
|
SNAPSHOT(root_header);
|
|
SNAPSHOT(root_header_cycle);
|
|
SNAPSHOT(root_eop_cycle);
|
|
SNAPSHOT(before_setup_in_count);
|
|
SNAPSHOT(before_setup_in_cutoff);
|
|
SNAPSHOT(before_setup_header);
|
|
SNAPSHOT(before_setup_header_cycle);
|
|
SNAPSHOT(before_setup_eop_cycle);
|
|
#undef SNAPSHOT
|
|
out->enabled = atomic_read(&enabled);
|
|
out->fatal_fault = atomic_read(&fatal_fault);
|
|
out->published_generation = atomic_read(&published_generation);
|
|
out->reader_index = atomic_read(&reader_index);
|
|
const uint32_t setup = __atomic_load_n(&setup_publication, __ATOMIC_ACQUIRE);
|
|
out->last_setup_sequence = setup & SETUP_SEQUENCE_MASK;
|
|
const uint32_t slot = setup >> SETUP_SLOT_SHIFT;
|
|
out->last_setup_slot = slot < PROBE_ROUTER_SLOTS ? slot : PROBE_ROUTER_UNASSIGNED;
|
|
}
|
|
|
|
uint8_t probe_router_setup_slot(uint32_t* sequence) {
|
|
const uint32_t setup = __atomic_load_n(&setup_publication, __ATOMIC_ACQUIRE);
|
|
*sequence = setup & SETUP_SEQUENCE_MASK;
|
|
const uint32_t slot = setup >> SETUP_SLOT_SHIFT;
|
|
return slot < PROBE_ROUTER_SLOTS ? (uint8_t)slot : PROBE_ROUTER_UNASSIGNED;
|
|
}
|
|
|
|
static __force_inline uint32_t cycles_now(void) {
|
|
return sio_hw->mtime;
|
|
}
|
|
|
|
static __force_inline int32_t cycles_after(uint32_t now, uint32_t deadline) {
|
|
return (int32_t)(now - deadline);
|
|
}
|
|
|
|
static __force_inline uint32_t receive_line(void) {
|
|
// Native-mode measurements returned zero here while PHY_DIRECT saw traffic.
|
|
// Main can select USBPHY_AS_GPIO to test the separate native-pad SIO path.
|
|
return (sio_hw->gpio_hi_in >> 24) & 3u;
|
|
}
|
|
|
|
static __force_inline bool sample_line(uint32_t* deadline, uint32_t* line) {
|
|
uint32_t now;
|
|
do {
|
|
now = cycles_now();
|
|
} while (cycles_after(now, *deadline) < 0);
|
|
// Reuse the wait-loop timestamp instead of a second timer access per bit.
|
|
// A full-bit overrun is definitely a missed sample. Edge-poll timing still
|
|
// needs calibration: the host correlates sampled headers with actual
|
|
// hardware-accepted SETUP requests before enabling address writes.
|
|
if (cycles_after(now, *deadline) >= (int32_t)FS_BIT_CYCLES)
|
|
return false;
|
|
*line = receive_line();
|
|
*deadline += FS_BIT_CYCLES;
|
|
// Keep one rolling deadline. GCC's unrolled affine expansion otherwise
|
|
// retains SOP/phase and spills/rebuilds per-bit deadlines in the hot path.
|
|
__asm volatile ("" : "+r"(*deadline));
|
|
return true;
|
|
}
|
|
|
|
#if defined(SWITCH2_PROBE_HUB) && SWITCH2_PROBE_HUB && defined(SWITCH2_PROBE_TRACE_NATIVE_INPUT)
|
|
static __force_inline void root_observe_token(uint8_t owner, uint32_t signature,
|
|
uint32_t cutoff, bool selected) {
|
|
if (selected && owner == 0 && signature == TOKEN_IN_SIGNATURE) {
|
|
// Early-prefix and full-address decisions share a cutoff. Do not reset
|
|
// an observation twice for the same token.
|
|
if (root_observation_stage == ROOT_OBSERVE_HEADER &&
|
|
atomic_read(&counters.root_in_cutoff) == cutoff) return;
|
|
count_one(&counters.root_in_count);
|
|
atomic_write(&counters.root_in_cutoff,cutoff);
|
|
atomic_write(&counters.root_header,0);
|
|
atomic_write(&counters.root_header_cycle,0);
|
|
atomic_write(&counters.root_eop_cycle,0);
|
|
root_observation_stage = ROOT_OBSERVE_HEADER;
|
|
return;
|
|
}
|
|
// A new token ends attribution to the preceding root IN, including when
|
|
// its selection fails. A later child's response must not become the root's.
|
|
root_observation_stage = ROOT_OBSERVE_IDLE;
|
|
if (selected && owner == 0 && signature == TOKEN_SETUP_SIGNATURE) {
|
|
atomic_write(&counters.before_setup_in_count,atomic_read(&counters.root_in_count));
|
|
atomic_write(&counters.before_setup_in_cutoff,atomic_read(&counters.root_in_cutoff));
|
|
atomic_write(&counters.before_setup_header,atomic_read(&counters.root_header));
|
|
atomic_write(&counters.before_setup_header_cycle,atomic_read(&counters.root_header_cycle));
|
|
atomic_write(&counters.before_setup_eop_cycle,atomic_read(&counters.root_eop_cycle));
|
|
}
|
|
}
|
|
|
|
static __force_inline void observe_discarded_header(uint32_t header) {
|
|
atomic_write(&counters.last_discarded_header,header);
|
|
count_one(&counters.discarded_headers);
|
|
if (root_observation_stage == ROOT_OBSERVE_HEADER) {
|
|
atomic_write(&counters.root_header,header);
|
|
atomic_write(&counters.root_header_cycle,cycles_now());
|
|
root_observation_stage = ROOT_OBSERVE_EOP;
|
|
}
|
|
}
|
|
#endif
|
|
|
|
static __force_inline void root_observe_eop(uint32_t cycle, bool qualified) {
|
|
#if defined(SWITCH2_PROBE_HUB) && SWITCH2_PROBE_HUB && defined(SWITCH2_PROBE_TRACE_NATIVE_INPUT)
|
|
if (root_observation_stage == ROOT_OBSERVE_EOP) {
|
|
// This is the drain's observed SE0-to-J qualification, not a measured
|
|
// two-bit EOP width. Idle/timeout must not attribute a later EOP here.
|
|
if (qualified) atomic_write(&counters.root_eop_cycle,cycle);
|
|
root_observation_stage = ROOT_OBSERVE_IDLE;
|
|
}
|
|
#else
|
|
(void)cycle;
|
|
(void)qualified;
|
|
#endif
|
|
}
|
|
|
|
static __force_inline void route_header(const routing_table* table, uint32_t address,
|
|
uint32_t signature, uint32_t initial_address,
|
|
uint32_t cutoff, raw_packet* packet) {
|
|
// TinyUSB clears SETUP_REC only AFTER copying the hardware-validated SETUP
|
|
// into its event callback. Until then, preserve both address and owner.
|
|
if (usb_hw->sie_status & USB_SIE_STATUS_SETUP_REC_BITS) {
|
|
#if defined(SWITCH2_PROBE_HUB) && SWITCH2_PROBE_HUB && defined(SWITCH2_PROBE_TRACE_NATIVE_INPUT)
|
|
root_observe_token(PROBE_ROUTER_UNASSIGNED,signature,cutoff,false);
|
|
#endif
|
|
return;
|
|
}
|
|
invalidate_setup();
|
|
if (address >= 128u || table->owner[address] >= PROBE_ROUTER_SLOTS) {
|
|
#if defined(SWITCH2_PROBE_HUB) && SWITCH2_PROBE_HUB && defined(SWITCH2_PROBE_TRACE_NATIVE_INPUT)
|
|
root_observe_token(PROBE_ROUTER_UNASSIGNED,signature,cutoff,false);
|
|
#endif
|
|
return;
|
|
}
|
|
#if defined(SWITCH2_PROBE_HUB) && SWITCH2_PROBE_HUB
|
|
if (atomic_read(&enabled) != 0u) {
|
|
const bool selected = native_hub_select_device((uint8_t)address, table->owner[address], cutoff);
|
|
#if defined(SWITCH2_PROBE_TRACE_NATIVE_INPUT)
|
|
root_observe_token(table->owner[address],signature,cutoff,selected);
|
|
// Keep diagnostic PID classification behind the address-critical call.
|
|
__asm volatile ("" : "+r"(signature) : : "memory");
|
|
const uint8_t pid = signature == TOKEN_OUT_SIGNATURE ? PID_OUT :
|
|
signature == TOKEN_IN_SIGNATURE ? PID_IN : PID_SETUP;
|
|
#endif
|
|
if (!selected) {
|
|
#if defined(SWITCH2_PROBE_TRACE_NATIVE_INPUT)
|
|
native_hub_note_failed_select((uint8_t)address, table->owner[address], cutoff, pid);
|
|
#endif
|
|
return;
|
|
}
|
|
#if defined(SWITCH2_PROBE_TRACE_NATIVE_INPUT)
|
|
native_hub_note_selected_token((uint8_t)address, table->owner[address], cutoff, pid);
|
|
#endif
|
|
if (initial_address != address) ++packet->retargets;
|
|
}
|
|
#if defined(SWITCH2_PROBE_TRACE_NATIVE_INPUT)
|
|
else {
|
|
root_observe_token(PROBE_ROUTER_UNASSIGNED,signature,cutoff,false);
|
|
}
|
|
#endif
|
|
#else
|
|
if (initial_address != address && atomic_read(&enabled) != 0u) {
|
|
if (cycles_after(cycles_now(), cutoff) >= 0) {
|
|
packet->late = true;
|
|
return;
|
|
}
|
|
__dmb();
|
|
usb_hw->dev_addr_ctrl = address;
|
|
++packet->retargets;
|
|
}
|
|
#endif
|
|
// Candidate observations qualify calibration only. Runtime ownership
|
|
// comes from the hardware address frozen by SETUP_REC. A missed software
|
|
// candidate must not reject a correctly addressed, hardware-accepted SETUP.
|
|
if (signature == TOKEN_SETUP_SIGNATURE)
|
|
publish_setup(table->owner[address]);
|
|
}
|
|
|
|
// The timing-critical path samples the complete address before selecting the
|
|
// native SIE. Hardware SETUP acceptance qualifies the candidate; opportunistic
|
|
// full-token CRC decoding below is diagnostic, not an ownership authority.
|
|
static bool observe_idle_j(void);
|
|
|
|
// Prepare before waiting for EOP: an ACK can be followed immediately by a poll.
|
|
#if defined(SWITCH2_PROBE_HUB) && SWITCH2_PROBE_HUB
|
|
static raw_packet __no_inline_not_in_flash_func(capture_packet)(
|
|
uint32_t phase, const routing_table* table, bool draining) {
|
|
#else
|
|
static raw_packet __no_inline_not_in_flash_func(capture_packet)(
|
|
uint32_t phase, const routing_table* table) {
|
|
#endif
|
|
raw_packet result = {0};
|
|
uint32_t word0 = LINE_K, word1 = 0u, word2 = 0u;
|
|
const uint8_t* decoder = NULL;
|
|
uint32_t expected_word = 0u;
|
|
const uint32_t initial_address = usb_hw->dev_addr_ctrl;
|
|
#if defined(SWITCH2_PROBE_HUB) && SWITCH2_PROBE_HUB
|
|
const uint8_t* early_decoder = NULL;
|
|
#endif
|
|
// Complete capture preparation before looking for the edge. The first
|
|
// hardware traces showed that preparing this state after SOP lost bit 1.
|
|
// Later zero-valued accumulators must remain constants until first use;
|
|
// forcing them into live registers adds spills and unnecessary ORs.
|
|
__asm volatile ("" : "+r"(word0), "+m"(result) : : "memory");
|
|
#if defined(SWITCH2_PROBE_HUB) && SWITCH2_PROBE_HUB
|
|
drain_prepared_capture:;
|
|
if (draining) {
|
|
const uint32_t stop = cycles_now() + FS_CLOCK_HZ / 10000u;
|
|
bool saw_se0 = false;
|
|
uint32_t se0_since = 0;
|
|
for (;;) {
|
|
uint32_t line = receive_line(), now = cycles_now();
|
|
if (cycles_after(now,stop) >= 0) {
|
|
root_observe_eop(now,false);
|
|
result.resync = true;
|
|
return result;
|
|
}
|
|
if (line == LINE_SE0) {
|
|
if (!saw_se0) se0_since = now;
|
|
saw_se0 = true;
|
|
} else {
|
|
// Half a bit rejects pad skew while allowing late ACK EOP entry.
|
|
if (line == LINE_J && saw_se0 &&
|
|
cycles_after(now,se0_since) >= (int32_t)FS_HALF_BIT_CYCLES) {
|
|
root_observe_eop(now,true);
|
|
break;
|
|
}
|
|
if (line == LINE_J && observe_idle_j()) {
|
|
root_observe_eop(now,false);
|
|
break;
|
|
}
|
|
saw_se0 = false;
|
|
}
|
|
}
|
|
}
|
|
#endif
|
|
uint32_t line = receive_line();
|
|
if (line != LINE_J) {
|
|
result.resync = true;
|
|
return result;
|
|
}
|
|
// A falling D+ leaves full-speed idle. Inspect the complete captured pair
|
|
// before accepting K; defer normalization until after the polling loop.
|
|
// Eight straight polls amortize loop bookkeeping and reduce edge jitter.
|
|
uint32_t pins;
|
|
#define POLL_IDLE() do { \
|
|
pins = sio_hw->gpio_hi_in; \
|
|
if ((pins & SIO_GPIO_HI_IN_USB_DP_BITS) == 0u) goto edge; \
|
|
} while (0)
|
|
#if defined(SWITCH2_PROBE_HUB) && SWITCH2_PROBE_HUB
|
|
for (;;) {
|
|
POLL_IDLE(); POLL_IDLE(); POLL_IDLE(); POLL_IDLE(); POLL_IDLE(); POLL_IDLE(); POLL_IDLE(); POLL_IDLE();
|
|
POLL_IDLE(); POLL_IDLE(); POLL_IDLE(); POLL_IDLE(); POLL_IDLE(); POLL_IDLE(); POLL_IDLE(); POLL_IDLE();
|
|
POLL_IDLE(); POLL_IDLE(); POLL_IDLE(); POLL_IDLE(); POLL_IDLE(); POLL_IDLE(); POLL_IDLE(); POLL_IDLE();
|
|
POLL_IDLE(); POLL_IDLE(); POLL_IDLE(); POLL_IDLE(); POLL_IDLE(); POLL_IDLE(); POLL_IDLE(); POLL_IDLE();
|
|
POLL_IDLE(); POLL_IDLE(); POLL_IDLE(); POLL_IDLE(); POLL_IDLE(); POLL_IDLE(); POLL_IDLE(); POLL_IDLE();
|
|
POLL_IDLE(); POLL_IDLE(); POLL_IDLE(); POLL_IDLE(); POLL_IDLE(); POLL_IDLE(); POLL_IDLE(); POLL_IDLE();
|
|
POLL_IDLE(); POLL_IDLE(); POLL_IDLE(); POLL_IDLE(); POLL_IDLE(); POLL_IDLE(); POLL_IDLE(); POLL_IDLE();
|
|
POLL_IDLE(); POLL_IDLE(); POLL_IDLE(); POLL_IDLE(); POLL_IDLE(); POLL_IDLE(); POLL_IDLE(); POLL_IDLE();
|
|
// Keep the prepared frame while idle; leave only for a table update/fault.
|
|
if ((atomic_read(&published_generation) & 1u) != atomic_read(&reader_index) ||
|
|
atomic_read(&fatal_fault) != 0u) return result;
|
|
}
|
|
#else
|
|
for (unsigned poll = 0; poll < 512u; ++poll) {
|
|
POLL_IDLE(); POLL_IDLE(); POLL_IDLE(); POLL_IDLE();
|
|
POLL_IDLE(); POLL_IDLE(); POLL_IDLE(); POLL_IDLE();
|
|
}
|
|
#endif
|
|
#undef POLL_IDLE
|
|
return result;
|
|
edge:
|
|
line = (pins >> 24) & 3u;
|
|
if (line != LINE_K) {
|
|
result.resync = true;
|
|
return result;
|
|
}
|
|
const uint32_t sop_time = cycles_now();
|
|
// This timestamp follows the PHY read and edge-detection instructions.
|
|
// Captures showed an extra full-bit delay skipped SYNC's second symbol.
|
|
// Sweep the next sample relative to read completion, then keep one-bit
|
|
// spacing; every stored line symbol is still physically observed.
|
|
uint32_t deadline = sop_time + phase;
|
|
result.sop = true;
|
|
// The first stored K is the observed SOP above, not an invented SYNC bit.
|
|
#if defined(SWITCH2_PROBE_HUB) && SWITCH2_PROBE_HUB
|
|
#define SET_EARLY_DECODER(kind) (early_decoder = table->early_address[kind])
|
|
// PID rejection happens before any address/body samples. Reuse the prepared
|
|
// frame: rebuilding its stack image here can miss the end of a short ACK/NAK
|
|
// and the following token. All other result/address accumulators are still zero.
|
|
#if defined(SWITCH2_PROBE_TRACE_NATIVE_INPUT)
|
|
#define NOTE_DISCARDED_HEADER() observe_discarded_header(word0)
|
|
#else
|
|
#define NOTE_DISCARDED_HEADER() ((void)0)
|
|
#endif
|
|
#define DISCARD_NON_TOKEN() do { \
|
|
NOTE_DISCARDED_HEADER(); \
|
|
result.sop = false; word0 = LINE_K; draining = true; \
|
|
goto drain_prepared_capture; \
|
|
} while (0)
|
|
#define ROUTE_EARLY(bit, base) do { \
|
|
if ((base) + (bit) == 19u && decoder != NULL) { \
|
|
uint8_t candidate = early_decoder[word1 & 0xffu]; \
|
|
if (candidate < 128u) \
|
|
route_header(table, candidate, word0, initial_address, \
|
|
deadline + 11u * FS_BIT_CYCLES, &result); \
|
|
} \
|
|
} while (0)
|
|
#else
|
|
#define SET_EARLY_DECODER(kind) ((void)0)
|
|
#define DISCARD_NON_TOKEN() ((void)0)
|
|
#define ROUTE_EARLY(bit, base) ((void)0)
|
|
#endif
|
|
#define ROUTE_BITS(word, bit, base) do { \
|
|
if ((base) + (bit) == 11u) { \
|
|
const uint32_t index = (word0 >> 20) & 15u; \
|
|
expected_word = token_words[index]; \
|
|
decoder = address_decoder[index == 6u]; \
|
|
SET_EARLY_DECODER(index == 6u); \
|
|
} \
|
|
if ((base) + (bit) == 15u && word0 != expected_word) { \
|
|
decoder = NULL; \
|
|
DISCARD_NON_TOKEN(); \
|
|
} \
|
|
ROUTE_EARLY(bit, base); \
|
|
if ((base) + (bit) == 23u && decoder != NULL) { \
|
|
/* Compact observed D+ bits only after all eight symbols exist. */ \
|
|
uint32_t address_wire = word1 & 0x5555u; \
|
|
address_wire = (address_wire | (address_wire >> 1u)) & 0x3333u; \
|
|
address_wire = (address_wire | (address_wire >> 2u)) & 0x0f0fu; \
|
|
address_wire = (address_wire | (address_wire >> 4u)) & 0xffu; \
|
|
route_header(table, decoder[address_wire], word0, \
|
|
initial_address, deadline + 7u * FS_BIT_CYCLES, &result); \
|
|
if (result.late) { result.count = (base) + (bit) + 1u; goto done; } \
|
|
} \
|
|
} while (0)
|
|
#define CAPTURE(word, bit, base) do { \
|
|
if (!sample_line(&deadline, &line)) { \
|
|
result.count = (base) + (bit); goto late; \
|
|
} \
|
|
if (line == LINE_SE0) { result.count = (base) + (bit); goto eop; } \
|
|
(word) |= line << (2u * (bit)); \
|
|
ROUTE_BITS(word, bit, base); \
|
|
} while (0)
|
|
#define CAPTURE_16(word, base) \
|
|
CAPTURE(word, 0u, base); CAPTURE(word, 1u, base); \
|
|
CAPTURE(word, 2u, base); CAPTURE(word, 3u, base); \
|
|
CAPTURE(word, 4u, base); CAPTURE(word, 5u, base); \
|
|
CAPTURE(word, 6u, base); CAPTURE(word, 7u, base); \
|
|
CAPTURE(word, 8u, base); CAPTURE(word, 9u, base); \
|
|
CAPTURE(word, 10u, base); CAPTURE(word, 11u, base); \
|
|
CAPTURE(word, 12u, base); CAPTURE(word, 13u, base); \
|
|
CAPTURE(word, 14u, base); CAPTURE(word, 15u, base)
|
|
CAPTURE(word0, 1u, 0u); CAPTURE(word0, 2u, 0u);
|
|
CAPTURE(word0, 3u, 0u); CAPTURE(word0, 4u, 0u);
|
|
CAPTURE(word0, 5u, 0u); CAPTURE(word0, 6u, 0u);
|
|
CAPTURE(word0, 7u, 0u); CAPTURE(word0, 8u, 0u);
|
|
CAPTURE(word0, 9u, 0u); CAPTURE(word0, 10u, 0u);
|
|
CAPTURE(word0, 11u, 0u); CAPTURE(word0, 12u, 0u);
|
|
CAPTURE(word0, 13u, 0u); CAPTURE(word0, 14u, 0u);
|
|
CAPTURE(word0, 15u, 0u);
|
|
CAPTURE_16(word1, 16u);
|
|
CAPTURE(word2, 0u, 32u); CAPTURE(word2, 1u, 32u);
|
|
CAPTURE(word2, 2u, 32u); CAPTURE(word2, 3u, 32u);
|
|
CAPTURE(word2, 4u, 32u); CAPTURE(word2, 5u, 32u);
|
|
CAPTURE(word2, 6u, 32u); CAPTURE(word2, 7u, 32u);
|
|
#undef CAPTURE_16
|
|
#undef CAPTURE
|
|
#undef ROUTE_EARLY
|
|
#undef SET_EARLY_DECODER
|
|
#undef DISCARD_NON_TOKEN
|
|
#undef NOTE_DISCARDED_HEADER
|
|
result.count = RAW_BITS;
|
|
goto done;
|
|
eop:
|
|
// Full-speed EOP is two bit times of SE0 followed by one J bit. A reset,
|
|
// truncated packet, or SE1 is not a token. Check all three samples.
|
|
if (!sample_line(&deadline, &line))
|
|
goto late;
|
|
if (line != LINE_SE0)
|
|
goto done;
|
|
if (!sample_line(&deadline, &line))
|
|
goto late;
|
|
result.eop = line == LINE_J;
|
|
goto done;
|
|
late:
|
|
result.late = true;
|
|
done:
|
|
result.words[0] = word0;
|
|
result.words[1] = word1;
|
|
result.words[2] = word2;
|
|
return result;
|
|
}
|
|
|
|
|
|
static bool __not_in_flash_func(observe_idle_j)(void) {
|
|
// Stuffing prohibits eight consecutive J bit times inside a packet.
|
|
// Use tight PHY polling, not sparse timer-paced reads that could miss K.
|
|
const uint32_t start = cycles_now();
|
|
for (uint32_t i = 0; i < FS_IDLE_POLLS; ++i) {
|
|
if (receive_line() != LINE_J)
|
|
return false;
|
|
}
|
|
return cycles_after(cycles_now(), start) >= (int32_t)FS_IDLE_CYCLES;
|
|
}
|
|
|
|
#if !defined(SWITCH2_PROBE_HUB) || !SWITCH2_PROBE_HUB
|
|
static __force_inline uint32_t raw_line(const raw_packet* packet, uint32_t bit) {
|
|
return (packet->words[bit >> 4] >> ((bit & 15u) * 2u)) & 3u;
|
|
}
|
|
|
|
static void __not_in_flash_func(decode_packet)(const raw_packet* packet, const routing_table* table) {
|
|
// With LSB-first two-bit line samples, K J K J K J K K is 0xa666.
|
|
if (packet->count < 8u || (packet->words[0] & 0xffffu) != 0xa666u) {
|
|
return;
|
|
}
|
|
count_one(&counters.sync_ok);
|
|
uint32_t previous = LINE_K;
|
|
uint32_t ones = 1u; // Final decoded SYNC bit is one.
|
|
uint32_t decoded = 0u;
|
|
uint32_t value = 0u;
|
|
uint32_t pid = 0u;
|
|
bool token = false;
|
|
for (uint32_t wire_bit = 8u; wire_bit < packet->count; ++wire_bit) {
|
|
const uint32_t line = raw_line(packet, wire_bit);
|
|
if (line != LINE_J && line != LINE_K) {
|
|
return;
|
|
}
|
|
const uint32_t bit = line == previous;
|
|
previous = line;
|
|
if (ones == 6u) {
|
|
if (bit != 0u) {
|
|
return;
|
|
}
|
|
ones = 0u;
|
|
continue;
|
|
}
|
|
ones = bit != 0u ? ones + 1u : 0u;
|
|
if (decoded < 8u) {
|
|
pid |= bit << decoded;
|
|
++decoded;
|
|
if (decoded == 8u) {
|
|
if ((((pid >> 4) ^ pid) & 15u) != 15u) {
|
|
return;
|
|
}
|
|
atomic_write(&counters.last_pid, pid);
|
|
token = pid == PID_IN || pid == PID_OUT || pid == PID_SETUP;
|
|
if (!token)
|
|
return; // Do not parse device data, SOFs, or handshakes.
|
|
}
|
|
} else {
|
|
if (decoded == 24u) {
|
|
return;
|
|
}
|
|
value |= bit << (decoded - 8u);
|
|
++decoded;
|
|
}
|
|
}
|
|
if (!token || decoded != 24u || ones == 6u || !packet->eop || packet->late) {
|
|
return;
|
|
}
|
|
uint32_t crc = 0x1fu;
|
|
for (uint32_t bit = 0u; bit < 11u; ++bit) {
|
|
const uint32_t feedback = (crc ^ (value >> bit)) & 1u;
|
|
crc >>= 1;
|
|
if (feedback != 0u)
|
|
crc ^= 0x14u; // Reflected x^5 + x^2 + 1.
|
|
}
|
|
if (((crc ^ 0x1fu) & 0x1fu) != (value >> 11)) {
|
|
count_one(&counters.crc_errors);
|
|
return;
|
|
}
|
|
const uint32_t address = value & 0x7fu;
|
|
const uint8_t owner = table->owner[address];
|
|
atomic_write(&counters.last_address, address);
|
|
count_one(&counters.valid_tokens);
|
|
if (owner < PROBE_ROUTER_SLOTS)
|
|
count_one(&counters.address_hits[owner]);
|
|
if (pid == PID_SETUP) {
|
|
count_one(&counters.valid_setups);
|
|
}
|
|
}
|
|
|
|
#endif
|
|
|
|
static const routing_table* __not_in_flash_func(acquire_table)(uint32_t* generation) {
|
|
for (;;) {
|
|
const uint32_t selected = __atomic_load_n(&published_generation, __ATOMIC_SEQ_CST);
|
|
__atomic_store_n(&reader_index, selected & 1u, __ATOMIC_SEQ_CST);
|
|
if (__atomic_load_n(&published_generation, __ATOMIC_SEQ_CST) == selected) {
|
|
*generation = selected;
|
|
return &tables[selected & 1u];
|
|
}
|
|
}
|
|
}
|
|
|
|
static void __not_in_flash_func(observer_failed)(void) {
|
|
atomic_write(&enabled, 0u);
|
|
atomic_write(&counters.ready, 0u);
|
|
invalidate_setup();
|
|
__atomic_store_n(&reader_index, NO_READER, __ATOMIC_SEQ_CST);
|
|
for (;;)
|
|
__wfe();
|
|
}
|
|
|
|
void __not_in_flash_func(probe_router_core1)(void) {
|
|
(void)save_and_disable_interrupts();
|
|
if (!valid_clock || atomic_read(&fatal_fault) != 0u)
|
|
observer_failed();
|
|
|
|
sio_hw->mtime_ctrl = 0u;
|
|
sio_hw->mtimecmp = UINT32_MAX;
|
|
sio_hw->mtimecmph = UINT32_MAX;
|
|
sio_hw->mtime = 0u;
|
|
sio_hw->mtimeh = 0u;
|
|
sio_hw->mtime_ctrl = SIO_MTIME_CTRL_EN_BITS | SIO_MTIME_CTRL_FULLSPEED_BITS;
|
|
__dsb();
|
|
__isb();
|
|
bool timer_running = false;
|
|
uint32_t previous_timer = cycles_now();
|
|
for (uint32_t attempt = 0u; attempt < 256u; ++attempt) {
|
|
const uint32_t now = cycles_now();
|
|
const int32_t elapsed = cycles_after(now, previous_timer);
|
|
if (elapsed > 0 && elapsed < 1024) {
|
|
timer_running = true;
|
|
break;
|
|
}
|
|
previous_timer = now;
|
|
}
|
|
if (!timer_running)
|
|
observer_failed();
|
|
atomic_write(&counters.ready, 1u);
|
|
|
|
uint32_t generation;
|
|
const routing_table* table = acquire_table(&generation);
|
|
// Resynchronize at qualified EOP or a long idle J, never an arbitrary
|
|
// data transition. An idle gap must not cost the next control's SETUP.
|
|
bool draining = true;
|
|
#if !defined(SWITCH2_PROBE_HUB) || !SWITCH2_PROBE_HUB
|
|
bool saw_se0 = false;
|
|
uint32_t se0_since = 0u;
|
|
#endif
|
|
for (;;) {
|
|
if (atomic_read(&fatal_fault) != 0u)
|
|
observer_failed();
|
|
const uint32_t phase = atomic_read(&phase_cycles);
|
|
for (;;) {
|
|
if (atomic_read(&published_generation) != generation)
|
|
table = acquire_table(&generation);
|
|
#if !defined(SWITCH2_PROBE_HUB) || !SWITCH2_PROBE_HUB
|
|
if (draining) {
|
|
const uint32_t line = receive_line();
|
|
const uint32_t now = cycles_now();
|
|
if (line == LINE_SE0) {
|
|
if (!saw_se0) se0_since = now;
|
|
saw_se0 = true;
|
|
} else {
|
|
// Reject momentary pad skew as EOP. A real SE0 persists
|
|
// across at least one complete bit before returning to J.
|
|
if (line == LINE_J && saw_se0 &&
|
|
cycles_after(now, se0_since) >= (int32_t)FS_BIT_CYCLES)
|
|
draining = false;
|
|
else if (line == LINE_J && observe_idle_j())
|
|
draining = false;
|
|
saw_se0 = false;
|
|
}
|
|
if (draining) continue;
|
|
break;
|
|
}
|
|
#endif
|
|
break;
|
|
}
|
|
// Phase is relative to the observed J->K edge, not a promised physical
|
|
// edge timestamp. The host sweeps the compiled bit period and correlates
|
|
// sampled headers with the native DCD's CRC-accepted SETUP interrupts. A successful
|
|
// passive phase still does NOT prove when the SIE latches its address.
|
|
// Calibrate the real routing instruction path, not a lighter sampler
|
|
// whose phase/register allocation changes when routing is enabled.
|
|
// The independent enabled flag still forbids every dry-run USB write.
|
|
#if defined(SWITCH2_PROBE_HUB) && SWITCH2_PROBE_HUB
|
|
const raw_packet packet = capture_packet(phase, table, draining);
|
|
#else
|
|
const raw_packet packet = capture_packet(phase, table);
|
|
#endif
|
|
#if defined(SWITCH2_PROBE_HUB) && SWITCH2_PROBE_HUB && defined(SWITCH2_PROBE_TRACE_NATIVE_INPUT)
|
|
count_one(&counters.capture_returns);
|
|
#endif
|
|
if (!packet.sop) {
|
|
if (packet.resync) {
|
|
draining = true;
|
|
#if !defined(SWITCH2_PROBE_HUB) || !SWITCH2_PROBE_HUB
|
|
saw_se0 = false;
|
|
#endif
|
|
}
|
|
continue;
|
|
}
|
|
#if !defined(SWITCH2_PROBE_HUB) || !SWITCH2_PROBE_HUB
|
|
for (uint32_t i = 0; i < 3; ++i)
|
|
atomic_write(&counters.last_raw[i], packet.words[i]);
|
|
atomic_write(&counters.last_raw_count, packet.count);
|
|
atomic_write(&counters.last_raw_eop, packet.eop);
|
|
atomic_write(&counters.last_raw_late, packet.late);
|
|
count_one(&counters.sops);
|
|
atomic_write(&counters.retargets, atomic_read(&counters.retargets) + packet.retargets);
|
|
if (packet.late)
|
|
count_one(&counters.late_samples);
|
|
decode_packet(&packet, table);
|
|
// Decoding can outlast the minimum interpacket gap. Qualify another
|
|
// EOP or a long idle J before accepting a new SOP; an arbitrary J->K
|
|
// inside a packet is not a start. Missing traffic is preferable to
|
|
// manufacturing a SETUP owner from a payload transition.
|
|
draining = true;
|
|
saw_se0 = false;
|
|
#else
|
|
// A response may start during the return/preparation path even if the
|
|
// preceding EOP was sampled. Requalify from the prepared capture frame.
|
|
draining = true;
|
|
#endif
|
|
}
|
|
}
|