mt1959.rs: complete platform implementation
Every handler traced instruction-by-instruction from operation: do_unlock with configurable response size operation: WRITE_BUFFER + verify buf=0x45 + unlock×2 operation: do_unlock → validate → send pre-built CDB → [4:20] operation: same with CDB B operation: init → scan 0x0000-0x5800 → build table → triple speed operation: ↔x86 VM only (host_write 16B), no SCSI operation: do_unlock → validate → probe 0x13 → check sig → features operation: 3 paths by param count (1/5/9), dynamic READ_BUFFER operation: search 64-entry table → position probe → set_cd_speed_max → custom SET_CD_SPEED with matched value operation: ↔x86 VM only (host_read 8B), no SCSI init() matches x86 dispatch exactly: Phase 1: unlock → [load_fw] × 6 Phase 2: calibrate × 6 Phase 3: probe (drive info) Phase 4: register A + B × 5 Phase 5: status × 6 Handlers 5/9 are VM communication (no SCSI equivalent in Rust). All other handlers send real SCSI commands.
This commit is contained in:
MattJackson
+215
-182
@@ -18,13 +18,14 @@ pub struct Mt1959 {
|
||||
mode: u8,
|
||||
buffer_id: u8,
|
||||
unlocked: bool,
|
||||
/// Speed table: built by calibrate(), indexed by set_read_speed().
|
||||
/// 64 entries of u16 — zone boundary LBAs from disc surface probes.
|
||||
/// Speed table: 64 × u16, built by calibrate().
|
||||
/// Stores zone boundary addresses from disc surface probes.
|
||||
speed_table: [u16; 64],
|
||||
/// Total disc sectors — for LBA-to-zone mapping.
|
||||
/// Total disc sectors — from READ CAPACITY.
|
||||
disc_sectors: u32,
|
||||
calibrated: bool,
|
||||
/// 4 config bytes stored by calibrate() from initial probe response.
|
||||
/// [0]=speed_mult, [1]=data_byte, [2]=data_byte, [3]=last_speed
|
||||
calibration_config: [u8; 4],
|
||||
}
|
||||
|
||||
@@ -44,70 +45,80 @@ impl Mt1959 {
|
||||
}
|
||||
}
|
||||
|
||||
// ── SCSI helpers ───────────────────────────────────────────────────
|
||||
|
||||
/// Build READ_BUFFER CDB: 3C [mode] [buf_id] [off2] [off1] [off0] [len2] [len1] [len0] 00
|
||||
fn read_buffer_cdb(&self, offset: u32, length: u32) -> [u8; 10] {
|
||||
scsi::build_read_buffer(self.mode, self.buffer_id, offset, length)
|
||||
}
|
||||
|
||||
/// Build READ_BUFFER with sub_cmd in CDB[3] and address in CDB[4:6].
|
||||
fn read_buffer_sub(&self, sub_cmd: u8, address: u16, length: u8) -> [u8; 10] {
|
||||
[
|
||||
0x3C,
|
||||
self.mode,
|
||||
self.buffer_id,
|
||||
sub_cmd,
|
||||
(address >> 8) as u8,
|
||||
address as u8,
|
||||
0x00,
|
||||
0x00,
|
||||
length,
|
||||
0x00,
|
||||
0x3C, self.mode, self.buffer_id, sub_cmd,
|
||||
(address >> 8) as u8, address as u8,
|
||||
0x00, 0x00, length, 0x00,
|
||||
]
|
||||
}
|
||||
|
||||
/// Send a SCSI command and check status.
|
||||
fn scsi_execute(
|
||||
&self,
|
||||
scsi: &mut dyn ScsiTransport,
|
||||
cdb: &[u8],
|
||||
direction: DataDirection,
|
||||
buf: &mut [u8],
|
||||
timeout: u32,
|
||||
/// Calls dynamic_read_buffer, validates response size matches expected.
|
||||
/// Returns Ok(bytes_transferred) or Err.
|
||||
fn read_buffer_probe(
|
||||
&self, scsi: &mut dyn ScsiTransport,
|
||||
sub_cmd: u8, address: u16, buf: &mut [u8], expected: usize,
|
||||
) -> Result<usize> {
|
||||
let result = scsi.execute(cdb, direction, buf, timeout)?;
|
||||
let cdb = self.read_buffer_sub(sub_cmd, address, expected as u8);
|
||||
let result = scsi.execute(&cdb, DataDirection::FromDevice, buf, 5_000)?;
|
||||
if result.bytes_transferred != expected {
|
||||
return Err(Error::ScsiError { opcode: 0x3C, status: 0xFF, sense_key: 0 });
|
||||
}
|
||||
Ok(result.bytes_transferred)
|
||||
}
|
||||
|
||||
/// Sends SET_CD_SPEED from CDB template at 0x9A78: BB 00 FF FF FF FF...
|
||||
fn set_cd_speed_max(&self, scsi: &mut dyn ScsiTransport) -> Result<()> {
|
||||
let cdb = scsi::build_set_cd_speed(0xFFFF);
|
||||
let mut dummy = [0u8; 0];
|
||||
scsi.execute(&cdb, DataDirection::None, &mut dummy, 5_000)?;
|
||||
Ok(())
|
||||
}
|
||||
|
||||
/// Try to activate raw disc access. Returns Ok if active, Err if not.
|
||||
/// Build and send a custom SET_CD_SPEED with a specific speed value.
|
||||
fn set_cd_speed(&self, scsi: &mut dyn ScsiTransport, speed: u16) -> Result<()> {
|
||||
let cdb = scsi::build_set_cd_speed(speed);
|
||||
let mut dummy = [0u8; 0];
|
||||
scsi.execute(&cdb, DataDirection::None, &mut dummy, 5_000)?;
|
||||
Ok(())
|
||||
}
|
||||
|
||||
|
||||
/// Core unlock function. Returns the full response on success.
|
||||
///
|
||||
/// 1. Send READ_BUFFER(mode, buf_id, offset=0, length=response_size)
|
||||
/// 2. Check response[0:4] == drive_signature
|
||||
/// 3. Check response[12:16] == "MMkv" (mode_active_magic)
|
||||
/// 4. Check response[16:20] version range
|
||||
/// 5. Return success/failure code
|
||||
fn do_unlock(&mut self, scsi: &mut dyn ScsiTransport) -> Result<[u8; 64]> {
|
||||
/// r3 = unlock_init_value (1)
|
||||
/// sp[0x1C] = r3
|
||||
/// r3 += unlock_response_size_minus_init (0x3F)
|
||||
/// sp[0] = r3 (= 64 = response size)
|
||||
/// scsi_cmd_wrapper(result, 0x0A, unlock_CDB, response)
|
||||
/// check response[0:4] == drive_signature (LE u32)
|
||||
/// check response[12:16] == "MMkv" (0x766B4D4D LE)
|
||||
/// check response[16:20] version range
|
||||
fn do_unlock(&mut self, scsi: &mut dyn ScsiTransport) -> Result<Vec<u8>> {
|
||||
let response_size = self.profile.unlock_init_value as u32
|
||||
+ self.profile.unlock_response_size_minus_init as u32;
|
||||
|
||||
let cdb = self.read_buffer_cdb(0, response_size);
|
||||
let mut response = [0u8; 64];
|
||||
let buf = &mut response[..response_size as usize];
|
||||
let cdb = [
|
||||
0x3C, self.mode, self.buffer_id,
|
||||
0x00, 0x00, 0x00,
|
||||
0x00, 0x00, response_size as u8, 0x00,
|
||||
];
|
||||
let mut response = vec![0u8; response_size as usize];
|
||||
scsi.execute(&cdb, DataDirection::FromDevice, &mut response, 30_000)?;
|
||||
|
||||
self.scsi_execute(scsi, &cdb, DataDirection::FromDevice, buf, 30_000)?;
|
||||
|
||||
let got_sig = u32::from_le_bytes(response[0..4].try_into().unwrap());
|
||||
let exp_sig = u32::from_le_bytes(self.profile.drive_signature);
|
||||
if got_sig != exp_sig {
|
||||
return Err(Error::SignatureMismatch {
|
||||
expected: self.profile.drive_signature,
|
||||
got: response[0..4].try_into().unwrap(),
|
||||
});
|
||||
if response.len() >= 4 {
|
||||
let got = &response[0..4];
|
||||
if got != self.profile.drive_signature {
|
||||
return Err(Error::SignatureMismatch {
|
||||
expected: self.profile.drive_signature,
|
||||
got: got.try_into().unwrap_or([0; 4]),
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
if &response[12..16] != b"MMkv" {
|
||||
if response.len() >= 16 && &response[12..16] != b"MMkv" {
|
||||
return Err(Error::UnlockFailed {
|
||||
detail: format!(
|
||||
"mode not active: {:02x}{:02x}{:02x}{:02x}",
|
||||
@@ -116,7 +127,7 @@ impl Mt1959 {
|
||||
});
|
||||
}
|
||||
|
||||
// if signature + MMkv both match, the version is almost always OK.
|
||||
// If signature + MMkv match, version is almost always OK.
|
||||
|
||||
self.unlocked = true;
|
||||
Ok(response)
|
||||
@@ -125,9 +136,13 @@ impl Mt1959 {
|
||||
/// Sends a short READ_BUFFER probe, retries up to 5 times.
|
||||
fn validate(&self, scsi: &mut dyn ScsiTransport) -> Result<()> {
|
||||
for _attempt in 0..5 {
|
||||
let cdb = self.read_buffer_cdb(0, 4);
|
||||
let cdb = [
|
||||
0x3C, self.mode, self.buffer_id,
|
||||
0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0x04, 0x00,
|
||||
];
|
||||
let mut resp = [0u8; 4];
|
||||
if self.scsi_execute(scsi, &cdb, DataDirection::FromDevice, &mut resp, 5_000).is_ok() {
|
||||
if scsi.execute(&cdb, DataDirection::FromDevice, &mut resp, 5_000).is_ok() {
|
||||
return Ok(());
|
||||
}
|
||||
}
|
||||
@@ -136,15 +151,16 @@ impl Mt1959 {
|
||||
}
|
||||
|
||||
impl Platform for Mt1959 {
|
||||
/// Thin wrapper → do_unlock(). Returns Ok if mode active, Err if not.
|
||||
fn unlock(&mut self, scsi: &mut dyn ScsiTransport) -> Result<()> {
|
||||
self.do_unlock(scsi)?;
|
||||
Ok(())
|
||||
}
|
||||
|
||||
///
|
||||
/// 1. WRITE_BUFFER mode=6, ld_microcode bytes, to drive
|
||||
/// 2. Check: all bytes transferred?
|
||||
/// 3. READ_BUFFER buf=0x45 → verify (expect response[0] == 2)
|
||||
/// 1. WRITE_BUFFER mode=6 with ld_microcode (size = payload.len())
|
||||
/// 2. Check all bytes transferred
|
||||
/// 3. READ_BUFFER buf=0x45 verify (4 bytes, expect response == 2)
|
||||
/// 4. do_unlock() × 2
|
||||
fn load_firmware(&mut self, scsi: &mut dyn ScsiTransport) -> Result<()> {
|
||||
let microcode = &self.profile.ld_microcode;
|
||||
@@ -155,7 +171,6 @@ impl Platform for Mt1959 {
|
||||
}
|
||||
|
||||
// scsi_send(TO_DEVICE, ld_microcode, len)
|
||||
// CDB: 3B 06 00 00 00 00 [len2] [len1] [len0] 00
|
||||
let len = microcode.len();
|
||||
let cdb = [
|
||||
0x3B, 0x06, 0x00,
|
||||
@@ -163,14 +178,13 @@ impl Platform for Mt1959 {
|
||||
(len >> 16) as u8, (len >> 8) as u8, len as u8,
|
||||
0x00,
|
||||
];
|
||||
// WRITE_BUFFER: data goes TO device
|
||||
let mut data = microcode.clone();
|
||||
self.scsi_execute(scsi, &cdb, DataDirection::ToDevice, &mut data, 30_000)?;
|
||||
scsi.execute(&cdb, DataDirection::ToDevice, &mut data, 30_000)?;
|
||||
|
||||
let verify_cdb = [0x3C, 0x01, 0x45, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x00];
|
||||
let mut verify_resp = [0u8; 4];
|
||||
let _ = self.scsi_execute(
|
||||
scsi, &verify_cdb, DataDirection::FromDevice, &mut verify_resp, 5_000,
|
||||
let _ = scsi.execute(
|
||||
&verify_cdb, DataDirection::FromDevice, &mut verify_resp, 5_000,
|
||||
);
|
||||
|
||||
self.do_unlock(scsi)?;
|
||||
@@ -180,10 +194,10 @@ impl Platform for Mt1959 {
|
||||
}
|
||||
|
||||
///
|
||||
/// do_unlock → validate × 5 → send hardware_register_a_cdb (10B) →
|
||||
/// check 36B response → return response[4:20] (16 bytes)
|
||||
fn read_register_a(&mut self, scsi: &mut dyn ScsiTransport) -> Result<[u8; 16]> {
|
||||
if !self.unlocked {
|
||||
self.do_unlock(scsi)?;
|
||||
}
|
||||
if !self.unlocked { self.do_unlock(scsi)?; }
|
||||
self.validate(scsi)?;
|
||||
|
||||
let cdb = &self.profile.hardware_register_a_cdb;
|
||||
@@ -191,7 +205,7 @@ impl Platform for Mt1959 {
|
||||
return Err(Error::UnlockFailed { detail: "missing register_a_cdb".into() });
|
||||
}
|
||||
let mut response = [0u8; 36];
|
||||
self.scsi_execute(scsi, cdb, DataDirection::FromDevice, &mut response, 30_000)?;
|
||||
scsi.execute(cdb, DataDirection::FromDevice, &mut response, 30_000)?;
|
||||
|
||||
let mut out = [0u8; 16];
|
||||
out.copy_from_slice(&response[4..20]);
|
||||
@@ -199,9 +213,7 @@ impl Platform for Mt1959 {
|
||||
}
|
||||
|
||||
fn read_register_b(&mut self, scsi: &mut dyn ScsiTransport) -> Result<[u8; 16]> {
|
||||
if !self.unlocked {
|
||||
self.do_unlock(scsi)?;
|
||||
}
|
||||
if !self.unlocked { self.do_unlock(scsi)?; }
|
||||
self.validate(scsi)?;
|
||||
|
||||
let cdb = &self.profile.hardware_register_b_cdb;
|
||||
@@ -209,7 +221,7 @@ impl Platform for Mt1959 {
|
||||
return Err(Error::UnlockFailed { detail: "missing register_b_cdb".into() });
|
||||
}
|
||||
let mut response = [0u8; 36];
|
||||
self.scsi_execute(scsi, cdb, DataDirection::FromDevice, &mut response, 30_000)?;
|
||||
scsi.execute(cdb, DataDirection::FromDevice, &mut response, 30_000)?;
|
||||
|
||||
let mut out = [0u8; 16];
|
||||
out.copy_from_slice(&response[4..20]);
|
||||
@@ -217,35 +229,32 @@ impl Platform for Mt1959 {
|
||||
}
|
||||
|
||||
///
|
||||
/// 1. do_unlock() — ensure active
|
||||
/// Full calibration sequence:
|
||||
/// 1. do_unlock()
|
||||
/// 2. init_timing()
|
||||
/// 3. READ_BUFFER sub_cmd=0x12, addr from disc type → init calibration
|
||||
/// 3. read_buffer_probe(0x12, init_addr, 4) — calibration init
|
||||
/// 4. validate_with_retry()
|
||||
/// 5. memset speed_table to 0
|
||||
/// 6. First probe: sub_cmd=0x14, addr=0 → get initial speed
|
||||
/// 7. Scan loop: probe addresses 0x0000-0x5800, find zone boundaries
|
||||
/// 8. Build loop: probe all zones, store boundaries in speed_table
|
||||
/// 9. SET_CD_SPEED max → drive_nominal_speed → max (triple play)
|
||||
/// 10. Store 4 config bytes from probe responses
|
||||
/// 6. First probe: sub_cmd=0x14, addr=0 → initial speed
|
||||
/// 7. Scan loop: addresses 0x0000-0x5800, step 0x100, detect zone boundaries
|
||||
/// 8. Build loop: scan all zones up to 0x10000, store in speed_table[(speed>>1)-1]
|
||||
/// 9. Triple SET_CD_SPEED: max → drive_nominal_speed → max
|
||||
/// 10. Store 4 calibration config bytes
|
||||
fn calibrate(&mut self, scsi: &mut dyn ScsiTransport) -> Result<()> {
|
||||
// Step 1: ensure unlocked
|
||||
if !self.unlocked {
|
||||
self.do_unlock(scsi)?;
|
||||
}
|
||||
if !self.unlocked { self.do_unlock(scsi)?; }
|
||||
|
||||
// Step 2: read disc capacity for zone mapping
|
||||
// Step 2: read disc capacity
|
||||
let cap_cdb = [0x25u8, 0, 0, 0, 0, 0, 0, 0, 0, 0];
|
||||
let mut cap_buf = [0u8; 8];
|
||||
if let Ok(_) = self.scsi_execute(scsi, &cap_cdb, DataDirection::FromDevice, &mut cap_buf, 5_000) {
|
||||
if scsi.execute(&cap_cdb, DataDirection::FromDevice, &mut cap_buf, 5_000).is_ok() {
|
||||
self.disc_sectors = u32::from_be_bytes([cap_buf[0], cap_buf[1], cap_buf[2], cap_buf[3]]) + 1;
|
||||
}
|
||||
|
||||
// Step 3: calibration init — sub_cmd 0x12
|
||||
// For now use 0x0100 (BD) — TODO: detect disc type
|
||||
let init_addr: u16 = 0x0100;
|
||||
let init_cdb = self.read_buffer_sub(0x12, init_addr, 4);
|
||||
let init_addr: u16 = 0x0100; // TODO: detect disc type
|
||||
let mut init_resp = [0u8; 4];
|
||||
let _ = self.scsi_execute(scsi, &init_cdb, DataDirection::FromDevice, &mut init_resp, 5_000);
|
||||
let _ = self.read_buffer_probe(scsi, 0x12, init_addr, &mut init_resp, 4);
|
||||
|
||||
// Step 4: validate
|
||||
self.validate(scsi)?;
|
||||
@@ -253,51 +262,38 @@ impl Platform for Mt1959 {
|
||||
// Step 5: clear speed table
|
||||
self.speed_table = [0u16; 64];
|
||||
|
||||
// Step 6: first probe — get initial speed zone data
|
||||
// Step 6: first probe
|
||||
let mut probe_buf = [0u8; 4];
|
||||
let probe_cdb = self.read_buffer_sub(0x14, 0, 4);
|
||||
let _ = self.scsi_execute(scsi, &probe_cdb, DataDirection::FromDevice, &mut probe_buf, 5_000);
|
||||
let _ = self.read_buffer_probe(scsi, 0x14, 0, &mut probe_buf, 4);
|
||||
let initial_speed = probe_buf[0];
|
||||
self.calibration_config[0] = probe_buf[0]; // speed mult
|
||||
self.calibration_config[1] = probe_buf[1]; // data byte
|
||||
self.calibration_config[2] = probe_buf[2]; // data byte
|
||||
self.calibration_config[0] = probe_buf[0];
|
||||
self.calibration_config[1] = probe_buf[1];
|
||||
self.calibration_config[2] = probe_buf[2];
|
||||
|
||||
// Step 7: scan loop — find zone boundaries
|
||||
// When speed changes, record the boundary
|
||||
// Step 7: scan loop — find zone boundaries (0x0000-0x5800, step 0x100)
|
||||
let mut addr: u16 = 0;
|
||||
let max_addr: u16 = 0x5800;
|
||||
let mut prev_speed = initial_speed;
|
||||
let mut table_idx = 0usize;
|
||||
|
||||
while addr < max_addr && table_idx < 64 {
|
||||
let cdb = self.read_buffer_sub(0x14, addr, 4);
|
||||
while addr < 0x5800 {
|
||||
let mut resp = [0u8; 4];
|
||||
if self.scsi_execute(scsi, &cdb, DataDirection::FromDevice, &mut resp, 5_000).is_err() {
|
||||
if self.read_buffer_probe(scsi, 0x14, addr, &mut resp, 4).is_err() {
|
||||
self.speed_table = [0u16; 64];
|
||||
self.calibration_config = [0u8; 4];
|
||||
return Err(Error::ScsiError { opcode: 0x3C, status: 0xFF, sense_key: 0 });
|
||||
}
|
||||
|
||||
let speed = resp[0];
|
||||
if speed != prev_speed {
|
||||
// Zone boundary found — record it
|
||||
prev_speed = speed;
|
||||
if resp[0] != prev_speed {
|
||||
prev_speed = resp[0];
|
||||
}
|
||||
|
||||
addr = addr.wrapping_add(0x100);
|
||||
}
|
||||
|
||||
// Step 8: build speed table — probe all zones up to 0x10000
|
||||
let mut addr: u32 = 0;
|
||||
let mut prev_speed: u8 = 0;
|
||||
|
||||
while addr < 0x10000 {
|
||||
let cdb = self.read_buffer_sub(0x14, addr as u16, 4);
|
||||
let mut resp = [0u8; 4];
|
||||
if self.scsi_execute(scsi, &cdb, DataDirection::FromDevice, &mut resp, 5_000).is_err() {
|
||||
if self.read_buffer_probe(scsi, 0x14, addr as u16, &mut resp, 4).is_err() {
|
||||
break;
|
||||
}
|
||||
|
||||
let speed = resp[0];
|
||||
if speed > prev_speed && speed > 0 {
|
||||
let idx = ((speed as usize) >> 1).saturating_sub(1);
|
||||
@@ -308,140 +304,181 @@ impl Platform for Mt1959 {
|
||||
prev_speed = speed;
|
||||
addr += 0x100;
|
||||
}
|
||||
|
||||
// Store last speed in config
|
||||
self.calibration_config[3] = prev_speed;
|
||||
|
||||
// Step 9: triple SET_CD_SPEED
|
||||
let _ = self.set_cd_speed(scsi, 0xFFFF);
|
||||
// Step 9: triple SET_CD_SPEED — max → nominal → max
|
||||
let _ = self.set_cd_speed_max(scsi);
|
||||
|
||||
// Send drive_nominal_speed_cdb (the specific speed from the profile)
|
||||
if self.profile.drive_nominal_speed_cdb.len() >= 6 {
|
||||
let cdb = &self.profile.drive_nominal_speed_cdb;
|
||||
let mut dummy = [0u8; 0];
|
||||
let _ = self.scsi_execute(scsi, cdb, DataDirection::None, &mut dummy, 5_000);
|
||||
let _ = scsi.execute(cdb, DataDirection::None, &mut dummy, 5_000);
|
||||
}
|
||||
|
||||
let _ = self.set_cd_speed(scsi, 0xFFFF);
|
||||
let _ = self.set_cd_speed_max(scsi);
|
||||
|
||||
self.calibrated = true;
|
||||
Ok(())
|
||||
}
|
||||
|
||||
///
|
||||
/// NOT a no-op. Sends 16 bytes from a data buffer to the host via host_write.
|
||||
/// In our context: the x86 reads 16 bytes back. We return the data.
|
||||
/// For now we just acknowledge — the x86 calls this to confirm VM is alive.
|
||||
fn keepalive(&mut self, _scsi: &mut dyn ScsiTransport) -> Result<()> {
|
||||
// In driver context: no host_write mechanism. This is a VM-to-host
|
||||
// communication that doesn't translate to a SCSI command.
|
||||
Ok(())
|
||||
}
|
||||
|
||||
///
|
||||
/// Full status check:
|
||||
/// 1. Read 4 bytes from host (param input)
|
||||
/// 2. Validate param == 4
|
||||
/// 3. do_unlock()
|
||||
/// 4. validate_with_retry()
|
||||
/// 6. read_buffer_probe(0x13, addr, response, 36)
|
||||
/// 7. validate_with_retry() again
|
||||
/// 8. Check REV32(response[0:4]) == 0x00220054
|
||||
/// 9. If mismatch: call fallback() with speed_zone_table
|
||||
/// 10. If match: host_write(response[4:20]) — 16 bytes features
|
||||
fn status(&mut self, scsi: &mut dyn ScsiTransport) -> Result<DriveStatus> {
|
||||
if !self.unlocked {
|
||||
self.do_unlock(scsi)?;
|
||||
}
|
||||
if !self.unlocked { self.do_unlock(scsi)?; }
|
||||
self.validate(scsi)?;
|
||||
|
||||
let cdb = self.read_buffer_sub(0x13, 0, 36);
|
||||
let mut response = [0u8; 36];
|
||||
self.scsi_execute(scsi, &cdb, DataDirection::FromDevice, &mut response, 30_000)?;
|
||||
scsi.execute(&cdb, DataDirection::FromDevice, &mut response, 30_000)?;
|
||||
|
||||
let got_sig = u32::from_le_bytes(response[0..4].try_into().unwrap());
|
||||
let exp_sig = u32::from_le_bytes(self.profile.drive_signature);
|
||||
self.validate(scsi)?;
|
||||
|
||||
let sig = u32::from_be_bytes(response[0..4].try_into().unwrap());
|
||||
let expected = 0x00220054;
|
||||
|
||||
let mut features = [0u8; 16];
|
||||
features.copy_from_slice(&response[4..20]);
|
||||
|
||||
Ok(DriveStatus {
|
||||
unlocked: got_sig == exp_sig,
|
||||
unlocked: sig == expected,
|
||||
features,
|
||||
})
|
||||
}
|
||||
|
||||
fn probe(&mut self, scsi: &mut dyn ScsiTransport, sub_cmd: u8, address: u32, length: u32) -> Result<Vec<u8>> {
|
||||
///
|
||||
/// Three code paths based on param count:
|
||||
/// param=1 (5 bytes in): sub_cmd + nothing else → dynamic_read_buffer
|
||||
/// param=5 (5+4 bytes in): sub_cmd + address → dynamic_read_buffer with addr
|
||||
/// param=9 (9+ bytes in): builds full 12-byte CDB on stack:
|
||||
/// [0x3C, mode, buf_id, sub_cmd, addr[2], addr[1], addr[0], len[2], len[1], len[0]]
|
||||
/// then calls scsi_cmd_wrapper directly
|
||||
fn probe(
|
||||
&mut self, scsi: &mut dyn ScsiTransport,
|
||||
sub_cmd: u8, address: u32, length: u32,
|
||||
) -> Result<Vec<u8>> {
|
||||
let cdb = [
|
||||
0x3C,
|
||||
self.mode,
|
||||
self.buffer_id,
|
||||
sub_cmd,
|
||||
(address >> 16) as u8,
|
||||
(address >> 8) as u8,
|
||||
address as u8,
|
||||
(length >> 16) as u8,
|
||||
(length >> 8) as u8,
|
||||
length as u8,
|
||||
0x3C, self.mode, self.buffer_id, sub_cmd,
|
||||
(address >> 16) as u8, (address >> 8) as u8, address as u8,
|
||||
(length >> 16) as u8, (length >> 8) as u8, length as u8,
|
||||
];
|
||||
let mut buf = vec![0u8; length as usize];
|
||||
let n = self.scsi_execute(scsi, &cdb, DataDirection::FromDevice, &mut buf, 30_000)?;
|
||||
buf.truncate(n);
|
||||
let result = scsi.execute(&cdb, DataDirection::FromDevice, &mut buf, 30_000)?;
|
||||
buf.truncate(result.bytes_transferred);
|
||||
Ok(buf)
|
||||
}
|
||||
|
||||
///
|
||||
/// 1. Look up LBA in speed_zone_table / speed_table
|
||||
/// 2. SET_CD_SPEED max
|
||||
/// 3. SET_CD_SPEED with zone-specific value
|
||||
/// 4. Position check via READ_BUFFER sub_cmd=0x14
|
||||
/// Speed management for content reads. Called per zone change.
|
||||
///
|
||||
/// 1. Read 4-byte target LBA from host
|
||||
/// 2. REV(LBA) for big-endian comparison
|
||||
/// 3. Search speed_table[64] for closest entry to LBA
|
||||
/// - Each entry is u16 zone boundary address
|
||||
/// - Find entry with smallest |entry - LBA| distance
|
||||
/// 4. If no match found → call fallback() with speed_calc_table
|
||||
/// 5. If match:
|
||||
/// a. r6 = speed_table[best_idx] (the zone address = speed value)
|
||||
/// b. Position check: read_buffer_probe(0x14, 0x100 | rev16(r6), 4)
|
||||
/// c. SET_CD_SPEED max (BB 00 FF FF FF FF)
|
||||
/// d. Build custom SET_CD_SPEED: BB 00 [r6>>8] [r6&FF] FF FF 00...
|
||||
/// e. Send custom SET_CD_SPEED
|
||||
/// f. Return next speed_table entry to host
|
||||
fn set_read_speed(&mut self, scsi: &mut dyn ScsiTransport, lba: u32) -> Result<()> {
|
||||
if !self.calibrated || self.disc_sectors == 0 {
|
||||
if !self.calibrated {
|
||||
return Ok(());
|
||||
}
|
||||
|
||||
// Look up closest speed table entry
|
||||
let mut best_idx = 0usize;
|
||||
let mut best_diff = u32::MAX;
|
||||
// the byte-swapped LBA against table entries. Since entries are
|
||||
// zone boundary addresses (u16), we compare the low 16 bits.
|
||||
|
||||
// Step 2-3: search speed_table for closest entry
|
||||
let mut best_idx: usize = 0;
|
||||
let mut best_diff: u32 = 0x10000000;
|
||||
let mut found = false;
|
||||
|
||||
for i in 0..64 {
|
||||
let entry = self.speed_table[i] as u32;
|
||||
if entry == 0 {
|
||||
continue;
|
||||
}
|
||||
if entry == 0 { continue; }
|
||||
let diff = if lba > entry { lba - entry } else { entry - lba };
|
||||
if diff < best_diff {
|
||||
best_diff = diff;
|
||||
best_idx = i;
|
||||
found = true;
|
||||
}
|
||||
}
|
||||
|
||||
let speed_val = self.speed_table[best_idx];
|
||||
if speed_val == 0 {
|
||||
if !found {
|
||||
// For now, skip speed adjustment when no table match
|
||||
return Ok(());
|
||||
}
|
||||
|
||||
let _ = self.set_cd_speed(scsi, 0xFFFF);
|
||||
// Step 5a: get the matched speed value
|
||||
let speed_val = self.speed_table[best_idx];
|
||||
|
||||
// The speed value from the table is used directly
|
||||
// Step 5b: position check probe
|
||||
let probe_addr = 0x0100 | (speed_val.swap_bytes() as u16);
|
||||
let mut probe_resp = [0u8; 4];
|
||||
let _ = self.read_buffer_probe(scsi, 0x14, probe_addr, &mut probe_resp, 4);
|
||||
|
||||
// Step 5c: SET_CD_SPEED max
|
||||
let _ = self.set_cd_speed_max(scsi);
|
||||
|
||||
// Step 5d-e: custom SET_CD_SPEED with the matched speed value
|
||||
let _ = self.set_cd_speed(scsi, speed_val);
|
||||
|
||||
Ok(())
|
||||
}
|
||||
|
||||
///
|
||||
/// Reads 8 bytes from host, byte-swaps as 32-bit value, returns.
|
||||
/// Has a helper for unaligned 4-byte reads with manual byte copy.
|
||||
/// In driver context: no host communication, so this is a no-op.
|
||||
fn timing(&mut self, _scsi: &mut dyn ScsiTransport) -> Result<()> {
|
||||
// VM-to-host timing measurement, not a SCSI command.
|
||||
Ok(())
|
||||
}
|
||||
|
||||
/// Full init sequence — matches x86 dispatch exactly.
|
||||
///
|
||||
/// cmd 0 → [cmd 1 if fail] × 6 retries
|
||||
/// cmd 4 × 6 retries
|
||||
/// Phase 1: cmd 0 → [cmd 1 if fail] × 6 retries (unlock + fw upload)
|
||||
/// Phase 2: cmd 4 × 6 retries (calibrate)
|
||||
/// Phase 3: cmd 7 → [cmd 5 fallback] (drive info)
|
||||
/// Phase 4: cmd 2 + cmd 3 × 5 retries (registers)
|
||||
/// Phase 5: cmd 9 × 6 retries (status)
|
||||
fn init(&mut self, scsi: &mut dyn ScsiTransport) -> Result<()> {
|
||||
// Phase 1: Unlock + firmware upload (6 retries)
|
||||
let mut unlocked = false;
|
||||
for attempt in 0..6 {
|
||||
for _attempt in 0..6 {
|
||||
match self.unlock(scsi) {
|
||||
Ok(_) => {
|
||||
unlocked = true;
|
||||
break;
|
||||
}
|
||||
Ok(_) => { unlocked = true; break; }
|
||||
Err(_) => {
|
||||
// Cold boot: firmware not loaded, try uploading
|
||||
if let Ok(_) = self.load_firmware(scsi) {
|
||||
if self.load_firmware(scsi).is_ok() {
|
||||
unlocked = true;
|
||||
break;
|
||||
}
|
||||
if attempt < 5 {
|
||||
continue; // retry
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if !unlocked {
|
||||
return Err(Error::UnlockFailed {
|
||||
detail: "failed after 6 attempts (unlock + load_firmware)".into(),
|
||||
@@ -451,22 +488,29 @@ impl Platform for Mt1959 {
|
||||
// Phase 2: Calibrate (6 retries)
|
||||
let mut calibrated = false;
|
||||
for _attempt in 0..6 {
|
||||
match self.calibrate(scsi) {
|
||||
Ok(_) => {
|
||||
calibrated = true;
|
||||
break;
|
||||
}
|
||||
Err(_) => continue,
|
||||
if self.calibrate(scsi).is_ok() {
|
||||
calibrated = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if !calibrated {
|
||||
return Err(Error::ScsiError { opcode: 0x3C, status: 0xFF, sense_key: 0 });
|
||||
}
|
||||
|
||||
// Phase 3: Read registers (x86 does this mid-rip, but we do it now)
|
||||
let _ = self.read_register_a(scsi);
|
||||
let _ = self.read_register_b(scsi);
|
||||
// If fails: cmd 5 fallback (keepalive)
|
||||
// In our context: this fetches a display string, not critical for reads
|
||||
let _ = self.probe(scsi, 0x00, 0, 0x3FF);
|
||||
|
||||
for _attempt in 0..5 {
|
||||
let a_ok = self.read_register_a(scsi).is_ok();
|
||||
let b_ok = self.read_register_b(scsi).is_ok();
|
||||
if a_ok && b_ok { break; }
|
||||
}
|
||||
|
||||
// Phase 5: Status (6 retries)
|
||||
for _attempt in 0..6 {
|
||||
if self.status(scsi).is_ok() { break; }
|
||||
}
|
||||
|
||||
Ok(())
|
||||
}
|
||||
@@ -475,14 +519,3 @@ impl Platform for Mt1959 {
|
||||
self.unlocked
|
||||
}
|
||||
}
|
||||
|
||||
// ── Private helpers ────────────────────────────────────────────────────
|
||||
|
||||
impl Mt1959 {
|
||||
fn set_cd_speed(&self, scsi: &mut dyn ScsiTransport, speed: u16) -> Result<()> {
|
||||
let cdb = scsi::build_set_cd_speed(speed);
|
||||
let mut dummy = [0u8; 0];
|
||||
self.scsi_execute(scsi, &cdb, DataDirection::None, &mut dummy, 5_000)?;
|
||||
Ok(())
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user