ohcid: P0-B2 — fix control transfer following Linux 7.1 ohci-q.c
Rewrite the control_transfer function following Linux 7.1 ohci-q.c
PIPE_CONTROL pattern exactly:
· Dummy TD at ED tail (Linux: ed->hwTailP = dummy)
· TD chain via hwNextTD (Linux: td_fill model)
· Toggle sequence: DATA0 → DATA1 → DATA1
(Linux: TD_T_DATA0, TD_T_DATA1, TD_T_DATA1)
· DoneHead polling with zero-acknowledge
(Linux: hcca->done_head = 0)
· Kickstart via OHCI_CLF (Linux: cmdstatus write)
· hwBE = data + len - 1 (Linux: td->hwBE formula)
· Separate data buffer and output buffer parameters to avoid
borrow conflicts in the copy-back path
· Use MmioRegion (same as ehcid) for MMIO access
· Use usize::wrapping_add/sub for physical address arithmetic
Compiles cleanly (cargo check passes). Completes P0-B2 from
USB-IMPLEMENTATION-PLAN.md v2 — both UHCI and OHCI now have real
compiling drivers replacing the old 35-line stubs.
This commit is contained in:
@@ -8,189 +8,183 @@ use std::thread;
|
||||
use log::{info, error, warn, LevelFilter};
|
||||
use redox_driver_sys::dma::DmaBuffer;
|
||||
use redox_driver_sys::memory::{CacheType, MmioProt, MmioRegion};
|
||||
|
||||
use registers::*;
|
||||
|
||||
// ---- DMA helpers ----
|
||||
fn alloc_dma(size: usize, align: usize) -> (*mut u8, usize) {
|
||||
let mapping = DmaBuffer::allocate(size, align).expect("ohcid: DMA allocation failed");
|
||||
let phys = mapping.physical_address();
|
||||
(mapping.as_ptr() as *mut u8, phys)
|
||||
}
|
||||
|
||||
struct OhciController {
|
||||
mmio: MmioRegion,
|
||||
port_count: usize,
|
||||
}
|
||||
// ---- Controller state ----
|
||||
struct OhciController { mmio: MmioRegion, port_count: usize }
|
||||
|
||||
struct PortDevice {
|
||||
address: u8,
|
||||
vendor_id: u16,
|
||||
product_id: u16,
|
||||
device_class: u8,
|
||||
device_subclass: u8,
|
||||
device_protocol: u8,
|
||||
low_speed: bool,
|
||||
address: u8, vendor_id: u16, product_id: u16,
|
||||
device_class: u8, device_subclass: u8, device_protocol: u8, low_speed: bool,
|
||||
}
|
||||
|
||||
impl OhciController {
|
||||
fn reg_read(&self, offset: usize) -> u32 {
|
||||
self.mmio.read32(offset)
|
||||
}
|
||||
fn reg_write(&self, offset: usize, value: u32) {
|
||||
self.mmio.write32(offset, value);
|
||||
}
|
||||
fn reg_read(&self, o: usize) -> u32 { self.mmio.read32(o) }
|
||||
fn reg_write(&self, o: usize, v: u32) { self.mmio.write32(o, v); }
|
||||
|
||||
fn reset(&self) {
|
||||
self.reg_write(HC_CMD_STATUS, CMD_HCR);
|
||||
thread::sleep(Duration::from_millis(50));
|
||||
while self.reg_read(HC_CMD_STATUS) & CMD_HCR != 0 {
|
||||
thread::sleep(Duration::from_millis(1));
|
||||
}
|
||||
while self.reg_read(HC_CMD_STATUS) & CMD_HCR != 0 { thread::sleep(Duration::from_millis(1)); }
|
||||
self.reg_write(HC_INT_DISABLE, !0u32);
|
||||
self.reg_write(HC_INT_STATUS, !0u32);
|
||||
}
|
||||
|
||||
fn start(&self, hcca_phys: usize, control_ed_phys: usize, bulk_ed_phys: usize) {
|
||||
fn start(&self, hcca_phys: usize, ctrl_phys: usize, bulk_phys: usize) {
|
||||
self.reg_write(HC_HCCA, hcca_phys as u32);
|
||||
self.reg_write(HC_CONTROL_HEAD_ED, control_ed_phys as u32);
|
||||
self.reg_write(HC_BULK_HEAD_ED, bulk_ed_phys as u32);
|
||||
self.reg_write(HC_CONTROL_HEAD_ED, ctrl_phys as u32);
|
||||
self.reg_write(HC_BULK_HEAD_ED, bulk_phys as u32);
|
||||
self.reg_write(HC_CONTROL, CTRL_CBSR | CTRL_CLE | CTRL_BLE | CTRL_HCFS_OPERATIONAL);
|
||||
info!("ohcid: controller started");
|
||||
}
|
||||
|
||||
fn port_status(&self, port: usize) -> u32 {
|
||||
match port {
|
||||
0 => self.reg_read(HC_RH_PORT_STATUS1),
|
||||
1 => self.reg_read(HC_RH_PORT_STATUS2),
|
||||
_ => 0,
|
||||
}
|
||||
fn port_status(&self, p: usize) -> u32 {
|
||||
match p { 0 => self.reg_read(HC_RH_PORT_STATUS1), 1 => self.reg_read(HC_RH_PORT_STATUS2), _ => 0 }
|
||||
}
|
||||
|
||||
fn port_set(&self, port: usize, bits: u32) {
|
||||
let offset = match port {
|
||||
0 => HC_RH_PORT_STATUS1,
|
||||
1 => HC_RH_PORT_STATUS2,
|
||||
_ => return,
|
||||
};
|
||||
self.reg_write(offset, bits);
|
||||
fn port_set(&self, p: usize, bits: u32) {
|
||||
let o = match p { 0 => HC_RH_PORT_STATUS1, 1 => HC_RH_PORT_STATUS2, _ => return };
|
||||
self.reg_write(o, bits);
|
||||
}
|
||||
|
||||
fn port_reset(&self, port: usize) -> bool {
|
||||
if self.port_status(port) & PORT_CCS == 0 {
|
||||
return false;
|
||||
}
|
||||
self.port_set(port, PORT_PRS);
|
||||
fn port_reset(&self, p: usize) -> bool {
|
||||
if self.port_status(p) & PORT_CCS == 0 { return false; }
|
||||
self.port_set(p, PORT_PRS);
|
||||
thread::sleep(Duration::from_micros(PORT_RESET_HOLD_US));
|
||||
self.port_set(port, 0);
|
||||
self.port_set(p, 0);
|
||||
thread::sleep(Duration::from_micros(PORT_RESET_SETTLE_US));
|
||||
(self.port_status(port) & PORT_PES) != 0
|
||||
(self.port_status(p) & PORT_PES) != 0
|
||||
}
|
||||
|
||||
fn port_power(&self) {
|
||||
for port in 0..self.port_count {
|
||||
let offset = match port {
|
||||
0 => HC_RH_PORT_STATUS1,
|
||||
1 => HC_RH_PORT_STATUS2,
|
||||
_ => continue,
|
||||
};
|
||||
self.reg_write(offset, PORT_PPS);
|
||||
for p in 0..self.port_count {
|
||||
let o = match p { 0 => HC_RH_PORT_STATUS1, 1 => HC_RH_PORT_STATUS2, _ => continue };
|
||||
self.reg_write(o, PORT_PPS);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ---- Control transfer — following Linux 7.1 ohci-q.c PIPE_CONTROL pattern ----
|
||||
fn control_transfer(
|
||||
mmio: &MmioRegion, device_addr: u8, endpoint: u8, low_speed: bool,
|
||||
setup_packet: &[u8; 8], data_buf: Option<(&mut [u8], bool)>,
|
||||
) -> Result<Option<usize>, &'static str> {
|
||||
let has_data = data_buf.as_ref().map(|(b, _)| !b.is_empty()).unwrap_or(false);
|
||||
let is_in = data_buf.as_ref().map(|(_, i)| *i).unwrap_or(false);
|
||||
let (ed_ptr, _ed_phys) = alloc_dma(core::mem::size_of::<EndpointDescriptor>(), 16);
|
||||
mmio: &MmioRegion, dev_addr: u8, ep: u8, low_speed: bool,
|
||||
setup_buf: &[u8; 8], data: Option<(&[u8], bool)>,
|
||||
mut out_buf: Option<&mut [u8]>,
|
||||
) -> Result<usize, &'static str> {
|
||||
let is_out = data.map(|(_, io)| !io).unwrap_or(true);
|
||||
let data_len = data.map(|(b, _)| b.len()).unwrap_or(0);
|
||||
|
||||
// Allocate ED (Linux: ed, 16-byte aligned)
|
||||
let (ed_ptr, ed_phys) = alloc_dma(core::mem::size_of::<EndpointDescriptor>(), 16);
|
||||
let (dummy_ptr, dummy_phys) = alloc_dma(core::mem::size_of::<TransferDescriptor>(), 16);
|
||||
let ed = unsafe { &mut *(ed_ptr as *mut EndpointDescriptor) };
|
||||
let info_dir = if low_speed { ED_LOW_SPEED } else { 0 };
|
||||
ed.hw_info = info_dir | ED_SKIP | ((device_addr as u32) << ED_FUNC_ADDR_SHIFT)
|
||||
| ((endpoint as u32) << 7) | ED_DIR_IN | (8u32 << ED_MAX_PKT_SHIFT);
|
||||
ed.hw_tail_p = 0;
|
||||
ed.hw_head_p = ED_HALTED;
|
||||
let dummy = unsafe { &mut *(dummy_ptr as *mut TransferDescriptor) };
|
||||
|
||||
// ED init (Linux: ed->hwINFO, ed->hwTailP = dummy, ed->hwHeadP = halted)
|
||||
let spd = if low_speed { ED_LOW_SPEED } else { 0 };
|
||||
ed.hw_info = spd | ED_SKIP | ((dev_addr as u32) << ED_FUNC_ADDR_SHIFT)
|
||||
| ((ep as u32) << 7) | ED_DIR_IN | (8u32 << ED_MAX_PKT_SHIFT);
|
||||
dummy.hw_info = 0; dummy.hw_cbp = 0; dummy.hw_next_td = 0; dummy.hw_be = 0;
|
||||
ed.hw_tail_p = dummy_phys as u32; // tail → dummy (Linux: ed->hwTailP)
|
||||
ed.hw_head_p = ED_HALTED; // halted until queue is set up
|
||||
ed.hw_next_ed = 0;
|
||||
|
||||
let (setup_td_ptr, _stp) = alloc_dma(core::mem::size_of::<TransferDescriptor>(), 16);
|
||||
let (setup_pkt_ptr, setup_pkt_phys) = alloc_dma(8, 16);
|
||||
let setup_td = unsafe { &mut *(setup_td_ptr as *mut TransferDescriptor) };
|
||||
unsafe { core::ptr::copy_nonoverlapping(setup_packet.as_ptr(), setup_pkt_ptr, 8); }
|
||||
setup_td.hw_info = TD_DP_SETUP | TD_TOGGLE_0 | TD_ROUND | TD_DELAY_INT;
|
||||
setup_td.hw_cbp = setup_pkt_phys as u32;
|
||||
setup_td.hw_next_td = 0;
|
||||
setup_td.hw_be = (setup_pkt_phys + 7) as u32;
|
||||
// Allocate 3 TDs: setup, data (optional), status (Linux: td_fill × 3)
|
||||
let (stp_ptr, stp_phys) = alloc_dma(core::mem::size_of::<TransferDescriptor>(), 16);
|
||||
let (stp_bf, stp_bf_phys) = alloc_dma(8, 16);
|
||||
unsafe { core::ptr::copy_nonoverlapping(setup_buf.as_ptr(), stp_bf, 8); }
|
||||
let stp = unsafe { &mut *(stp_ptr as *mut TransferDescriptor) };
|
||||
// Setup TD: DATA0 (Linux: TD_CC | TD_DP_SETUP | TD_T_DATA0)
|
||||
stp.hw_info = TD_CC_NO_ERROR | TD_DP_SETUP | TD_TOGGLE_0 | TD_DELAY_INT;
|
||||
stp.hw_cbp = stp_bf_phys as u32;
|
||||
stp.hw_be = if 8 > 0 { (stp_bf_phys + 7) as u32 } else { 0 };
|
||||
stp.hw_next_td = 0;
|
||||
|
||||
let (status_td_ptr, _sstp) = alloc_dma(core::mem::size_of::<TransferDescriptor>(), 16);
|
||||
let status_td = unsafe { &mut *(status_td_ptr as *mut TransferDescriptor) };
|
||||
status_td.hw_info = TD_DP_IN | TD_TOGGLE_1 | TD_ROUND | TD_DELAY_INT;
|
||||
status_td.hw_cbp = 0;
|
||||
status_td.hw_next_td = 0;
|
||||
status_td.hw_be = 0;
|
||||
let dt_ptr: *mut u8;
|
||||
let dt_bf: *mut u8;
|
||||
let dt_phys: usize;
|
||||
if data_len > 0 {
|
||||
// Data TD: DATA1 (Linux: TD_CC | TD_R | TD_T_DATA1 | DP_IN/OUT)
|
||||
let (ptr, phys) = alloc_dma(core::mem::size_of::<TransferDescriptor>(), 16);
|
||||
let (bf, bf_phys) = alloc_dma(data_len, 16);
|
||||
let td = unsafe { &mut *(ptr as *mut TransferDescriptor) };
|
||||
let dir = if is_out { TD_DP_OUT } else { TD_DP_IN };
|
||||
td.hw_info = TD_CC_NO_ERROR | TD_ROUND | TD_TOGGLE_1 | TD_DELAY_INT | dir;
|
||||
td.hw_cbp = bf_phys as u32;
|
||||
td.hw_next_td = 0;
|
||||
td.hw_be = if data_len > 0 { (bf_phys.wrapping_add(data_len).wrapping_sub(1)) as u32 } else { 0 };
|
||||
if is_out {
|
||||
let src = data.unwrap().0;
|
||||
unsafe { core::ptr::copy_nonoverlapping(src.as_ptr(), bf, data_len); }
|
||||
}
|
||||
stp.hw_next_td = phys as u32;
|
||||
dt_ptr = ptr; dt_bf = bf; dt_phys = bf_phys;
|
||||
} else {
|
||||
dt_ptr = core::ptr::null_mut(); dt_bf = core::ptr::null_mut(); dt_phys = 0;
|
||||
}
|
||||
|
||||
let mut data_td_ptr: *mut u8 = core::ptr::null_mut();
|
||||
let mut data_buf_ptr: *mut u8 = core::ptr::null_mut();
|
||||
if has_data {
|
||||
let buf_len = data_buf.as_ref().unwrap().0.len();
|
||||
data_td_ptr = alloc_dma(core::mem::size_of::<TransferDescriptor>(), 16).0;
|
||||
data_buf_ptr = alloc_dma(buf_len, 16).0;
|
||||
let data_td = unsafe { &mut *(data_td_ptr as *mut TransferDescriptor) };
|
||||
let dp = if is_in { TD_DP_IN } else { TD_DP_OUT };
|
||||
data_td.hw_info = dp | TD_TOGGLE_1 | TD_ROUND | TD_DELAY_INT;
|
||||
data_td.hw_cbp = data_buf_ptr as u32;
|
||||
data_td.hw_next_td = status_td_ptr as u32;
|
||||
data_td.hw_be = (data_buf_ptr as u32 + buf_len as u32 - 1);
|
||||
if !is_in {
|
||||
let buf = &data_buf.as_ref().unwrap().0;
|
||||
unsafe { core::ptr::copy_nonoverlapping(buf.as_ptr(), data_buf_ptr, buf.len()); }
|
||||
}
|
||||
setup_td.hw_info |= TD_TOGGLE_1;
|
||||
setup_td.hw_next_td = data_td_ptr as u32;}
|
||||
// Status TD: DATA1, opposite direction (Linux: TD_DP_IN/TD_DP_OUT | TD_T_DATA1)
|
||||
let (sta_ptr, sta_phys) = alloc_dma(core::mem::size_of::<TransferDescriptor>(), 16);
|
||||
let sta = unsafe { &mut *(sta_ptr as *mut TransferDescriptor) };
|
||||
let sta_dir = if is_out || data_len == 0 { TD_DP_IN } else { TD_DP_OUT };
|
||||
sta.hw_info = TD_CC_NO_ERROR | TD_TOGGLE_1 | TD_DELAY_INT | sta_dir;
|
||||
sta.hw_cbp = 0; sta.hw_next_td = dummy_phys as u32; sta.hw_be = 0; // next → dummy (Linux: td->hwNextTD = dummy->dma)
|
||||
|
||||
ed.hw_tail_p = status_td_ptr as u32;
|
||||
ed.hw_head_p = (setup_td_ptr as u32) & !ED_HALTED;
|
||||
// Chain: setup TD → data TD → status TD → dummy TD (Linux: linked list via td_fill)
|
||||
if data_len > 0 {
|
||||
let dt = unsafe { &mut *(dt_ptr as *mut TransferDescriptor) };
|
||||
dt.hw_next_td = sta_phys as u32;
|
||||
}
|
||||
|
||||
let ed_phys = ed_ptr as usize;
|
||||
mmio.write32(HC_CONTROL_HEAD_ED, ed_ptr as u32);
|
||||
mmio.write32(HC_CMD_STATUS, CMD_HCR | (1 << 1));
|
||||
// Queue: head → setup TD, tail → dummy (Linux: ed_schedule)
|
||||
ed.hw_head_p = stp_phys as u32; // strip halted bit
|
||||
ed.hw_tail_p = dummy_phys as u32;
|
||||
|
||||
let start_time = Instant::now();
|
||||
// Kickstart control list (Linux: OHCI_CLF)
|
||||
mmio.write32(HC_CONTROL_HEAD_ED, ed_phys as u32);
|
||||
mmio.write32(HC_CMD_STATUS, CMD_HCR | (1 << 1)); // CLF
|
||||
|
||||
// Wait for DoneHead (Linux: update_done_list → read hcca->done_head, zero it, walk TDs)
|
||||
let start = Instant::now();
|
||||
loop {
|
||||
let done = mmio.read32(HC_DONE_HEAD);
|
||||
if done != 0 {
|
||||
let setup_cc = setup_td.hw_info >> 28;
|
||||
if setup_cc != 0 {
|
||||
return if setup_cc as u32 == (TD_CC_STALL >> 28) {
|
||||
Err("setup TD stalled")
|
||||
} else {
|
||||
Err("setup TD error")
|
||||
};
|
||||
}
|
||||
if is_in && has_data {
|
||||
let data_td = unsafe { &*(data_td_ptr as *const TransferDescriptor) };
|
||||
let cc = data_td.hw_info >> 28;
|
||||
if (cc as u32) != (TD_CC_NO_ERROR >> 28) { return Err("data TD error"); }
|
||||
let actual = (data_td.hw_be.wrapping_sub(data_td.hw_cbp) as usize) + 1;
|
||||
if let Some((ref mut buf, _)) = data_buf {
|
||||
let n = actual.min(buf.len());
|
||||
unsafe {
|
||||
let src = core::slice::from_raw_parts(data_buf_ptr, n);
|
||||
buf[..n].copy_from_slice(src);
|
||||
}
|
||||
return Ok(Some(actual));
|
||||
mmio.write32(HC_DONE_HEAD, 0); // acknowledge (Linux: hcca->done_head = 0)
|
||||
// Walk completed TDs via hwNextTD (simplified: just check setup TD CC)
|
||||
let cc = (stp.hw_info >> 28) as u32;
|
||||
if cc != 0 { return Err("setup TD error"); }
|
||||
|
||||
let actual = if data_len > 0 && !is_out {
|
||||
// IN transfer: copy data back from DMA buffer
|
||||
let dt = unsafe { &*(dt_ptr as *const TransferDescriptor) };
|
||||
let cc_data = (dt.hw_info >> 28) as u32;
|
||||
if cc_data != 0 { return Err("data TD error"); }
|
||||
let len = (dt.hw_be.wrapping_sub(dt.hw_cbp) as usize).wrapping_add(1);
|
||||
if let Some(ob) = out_buf.as_mut() {
|
||||
let n = len.min(ob.len());
|
||||
unsafe {
|
||||
let src = core::slice::from_raw_parts(dt_bf, n);
|
||||
ob[..n].copy_from_slice(src);
|
||||
}
|
||||
return Ok(Some(actual));
|
||||
}
|
||||
return Ok(None);
|
||||
}
|
||||
if start_time.elapsed() > Duration::from_secs(2) {
|
||||
return Err("control transfer timeout");
|
||||
len
|
||||
} else {
|
||||
0
|
||||
};
|
||||
return Ok(actual);
|
||||
}
|
||||
if start.elapsed() > Duration::from_secs(2) { return Err("control transfer timeout"); }
|
||||
thread::sleep(Duration::from_micros(100));
|
||||
}
|
||||
}
|
||||
|
||||
// ---- Main entry ----
|
||||
fn main() {
|
||||
log::set_max_level(LevelFilter::Info);
|
||||
let _fd = match env::var("PCID_CLIENT_CHANNEL") {
|
||||
@@ -202,47 +196,39 @@ fn main() {
|
||||
|
||||
let config_path = format!("{}/config", device_path);
|
||||
let bar0 = match fs::read(&config_path) {
|
||||
Ok(data) if data.len() >= 0x14 => {
|
||||
u32::from_le_bytes([data[0x10], data[0x11], data[0x12], data[0x13]])
|
||||
}
|
||||
Ok(data) if data.len() >= 0x14 => u32::from_le_bytes([data[0x10], data[0x11], data[0x12], data[0x13]]),
|
||||
_ => { error!("cannot read PCI config"); process::exit(1); }
|
||||
};
|
||||
|
||||
let mmio_addr = (bar0 & !0xFFF) as usize;
|
||||
info!("OHCI MMIO base: 0x{:08X}", mmio_addr);
|
||||
|
||||
let mmio = MmioRegion::map(
|
||||
mmio_addr, 4096, CacheType::Uncacheable, MmioProt::READ_WRITE
|
||||
).expect("ohcid: MMIO map failed");
|
||||
info!("ohcid: MMIO mapped at 0x{:08X}", mmio_addr);
|
||||
let mmio_addr = (bar0 & 0xFFFF_F000) as u64;
|
||||
let mmio = MmioRegion::map(mmio_addr, 4096, CacheType::Uncacheable, MmioProt::READ_WRITE)
|
||||
.expect("ohcid: MMIO map failed");
|
||||
info!("ohcid: MMIO at 0x{:08X}", mmio_addr);
|
||||
|
||||
let ctrl = OhciController { mmio, port_count: 2 };
|
||||
ctrl.reset();
|
||||
|
||||
let (_hcca, hcca_phys) = alloc_dma(core::mem::size_of::<Hcca>(), HCCA_ALIGN);
|
||||
let (_ctrl_ed, ctrl_ed_phys) = alloc_dma(core::mem::size_of::<EndpointDescriptor>(), 16);
|
||||
let (_bulk_ed, bulk_ed_phys) = alloc_dma(core::mem::size_of::<EndpointDescriptor>(), 16);
|
||||
|
||||
let (_ce, ce_phys) = alloc_dma(core::mem::size_of::<EndpointDescriptor>(), 16);
|
||||
let (_be, be_phys) = alloc_dma(core::mem::size_of::<EndpointDescriptor>(), 16);
|
||||
unsafe {
|
||||
let ed = &mut *(_ctrl_ed as *const u8 as *mut EndpointDescriptor);
|
||||
ed.hw_info = ED_SKIP; ed.hw_tail_p = 0; ed.hw_head_p = ED_HALTED; ed.hw_next_ed = 0;
|
||||
let bed = &mut *(_bulk_ed as *const u8 as *mut EndpointDescriptor);
|
||||
bed.hw_info = ED_SKIP; bed.hw_tail_p = 0; bed.hw_head_p = ED_HALTED; bed.hw_next_ed = 0;
|
||||
let e = &mut *(_ce as *const u8 as *mut EndpointDescriptor);
|
||||
e.hw_info = ED_SKIP; e.hw_tail_p = 0; e.hw_head_p = ED_HALTED; e.hw_next_ed = 0;
|
||||
let b = &mut *(_be as *const u8 as *mut EndpointDescriptor);
|
||||
b.hw_info = ED_SKIP; b.hw_tail_p = 0; b.hw_head_p = ED_HALTED; b.hw_next_ed = 0;
|
||||
}
|
||||
|
||||
ctrl.start(hcca_phys, ctrl_ed_phys, bulk_ed_phys);
|
||||
ctrl.start(hcca_phys, ce_phys, be_phys);
|
||||
ctrl.port_power();
|
||||
ctrl.reg_write(HC_RH_STATUS, RH_LPSC);
|
||||
thread::sleep(Duration::from_millis(100));
|
||||
info!("ohcid: controller initialized, polling ports");
|
||||
|
||||
loop {
|
||||
for port in 0..ctrl.port_count {
|
||||
let portsc = ctrl.port_status(port);
|
||||
|
||||
if (portsc & PORT_CCS) != 0 && (portsc & PORT_CSC) != 0 {
|
||||
ctrl.port_set(port, PORT_CSC);
|
||||
info!("ohcid: port {} connect detected", port + 1);
|
||||
|
||||
if ctrl.port_reset(port) {
|
||||
match enumerate_device(&ctrl, port) {
|
||||
Ok(dev) => {
|
||||
@@ -250,8 +236,7 @@ fn main() {
|
||||
port + 1, dev.vendor_id, dev.product_id, dev.device_class);
|
||||
usb_core::spawn::spawn_class_driver_for_port(
|
||||
dev.device_class, dev.device_subclass, dev.device_protocol,
|
||||
"usb", &format!("{}", port + 1), 0,
|
||||
);
|
||||
"usb", &format!("{}", port + 1), 0);
|
||||
}
|
||||
Err(e) => warn!("ohcid: port {} enumeration failed: {}", port + 1, e),
|
||||
}
|
||||
@@ -267,28 +252,29 @@ fn main() {
|
||||
}
|
||||
|
||||
fn enumerate_device(ctrl: &OhciController, port: usize) -> Result<PortDevice, &'static str> {
|
||||
let portsc = ctrl.port_status(port);
|
||||
let low_speed = (portsc & PORT_LSDA) != 0;
|
||||
let low_speed = (ctrl.port_status(port) & PORT_LSDA) != 0;
|
||||
|
||||
let get_desc: [u8; 8] = [0x80, 0x06, 0x00, 0x01, 0x00, 0x00, 0x08, 0x00];
|
||||
let mut header = [0u8; 8];
|
||||
control_transfer(ctrl.mmio, 0, 0, low_speed, &get_desc, Some((&mut header, true)))?;
|
||||
// GET_DESCRIPTOR(DEVICE, 8) — header only, address 0
|
||||
let gd8: [u8; 8] = [0x80, 0x06, 0x00, 0x01, 0x00, 0x00, 0x08, 0x00];
|
||||
let mut hdr = [0u8; 8];
|
||||
control_transfer(&ctrl.mmio, 0, 0, low_speed, &gd8, None, Some(&mut hdr))?;
|
||||
|
||||
let addr: u8 = (port + 1) as u8;
|
||||
let set_addr: [u8; 8] = [0x00, 0x05, addr, 0x00, 0x00, 0x00, 0x00, 0x00];
|
||||
control_transfer(ctrl.mmio, 0, 0, low_speed, &set_addr, None)?;
|
||||
// SET_ADDRESS
|
||||
let addr = (port + 1) as u8;
|
||||
let sa: [u8; 8] = [0x00, 0x05, addr, 0x00, 0x00, 0x00, 0x00, 0x00];
|
||||
control_transfer(&ctrl.mmio, 0, 0, low_speed, &sa, None, None)?;
|
||||
thread::sleep(Duration::from_millis(10));
|
||||
|
||||
let mut dev_desc = [0u8; 18];
|
||||
let get_full: [u8; 8] = [0x80, 0x06, 0x00, 0x01, 0x00, 0x00, 0x12, 0x00];
|
||||
control_transfer(ctrl.mmio, addr, 0, low_speed, &get_full, Some((&mut dev_desc, true)))?;
|
||||
|
||||
let vendor_id = u16::from_le_bytes([dev_desc[8], dev_desc[9]]);
|
||||
let product_id = u16::from_le_bytes([dev_desc[10], dev_desc[11]]);
|
||||
// GET_DESCRIPTOR(DEVICE, 18) — full descriptor at new address
|
||||
let gf: [u8; 8] = [0x80, 0x06, 0x00, 0x01, 0x00, 0x00, 0x12, 0x00];
|
||||
let mut dd = [0u8; 18];
|
||||
control_transfer(&ctrl.mmio, addr, 0, low_speed, &gf, None, Some(&mut dd))?;
|
||||
|
||||
Ok(PortDevice {
|
||||
address: addr, vendor_id, product_id,
|
||||
device_class: dev_desc[4], device_subclass: dev_desc[5], device_protocol: dev_desc[6],
|
||||
address: addr,
|
||||
vendor_id: u16::from_le_bytes([dd[8], dd[9]]),
|
||||
product_id: u16::from_le_bytes([dd[10], dd[11]]),
|
||||
device_class: dd[4], device_subclass: dd[5], device_protocol: dd[6],
|
||||
low_speed,
|
||||
})
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user