Rewrite mt1959.rs: complete platform driver with full profile support
Complete rewrite of MT1959 platform driver: - All 10 handlers implemented matching firmware logic 1:1 - load_firmware(): WRITE_BUFFER ld_microcode on cold boot - calibrate(): full zone probe + speed table + triple SET_CD_SPEED - init(): x86 dispatch sequence (unlock → fw × 6, calibrate × 6) - read_register_a/b(): use pre-built CDBs from profile - set_read_speed(): speed table lookup per zone - status(), probe(), keepalive(), timing() Platform trait updated: - Renamed read_config → load_firmware (matches actual function) - Added init() for full x86 dispatch sequence - Renamed read_sectors → set_read_speed (operation sets speed, not reads) - Split read_register into read_register_a/b (separate CDBs) Profile fields used: - drive_signature, unlock_init_value, unlock_response_size_minus_init - ld_microcode (1888B firmware payload) - hardware_register_a_cdb, hardware_register_b_cdb (pre-built CDBs) - drive_nominal_speed_cdb (calibration triple-play) - speed_zone_table, speed_calc_table (operation lookups)
This commit is contained in:
MattJackson
+316
-215
@@ -1,33 +1,31 @@
|
||||
//! MT1959 platform implementation — covers all LG/ASUS MediaTek drives.
|
||||
//!
|
||||
//! Two variants share this code:
|
||||
//! MT1959-A: mode=0x01, buffer_id=0x44 (handlers 0-9)
|
||||
//! MT1959-A: mode=0x01, buffer_id=0x44 (all 10 handlers)
|
||||
//! MT1959-B: mode=0x02, buffer_id=0x77 (handlers 4-9, 0-3 are no-ops)
|
||||
//!
|
||||
//! The logic is identical between A and B — only the SCSI READ BUFFER
|
||||
//! mode and buffer ID differ. Per-drive data (signature, register offsets)
|
||||
//! comes from the profile.
|
||||
//! The handler logic is identical across all 206 drives — only the
|
||||
//! profile data differs (signature, register CDBs, microcode, etc).
|
||||
//!
|
||||
|
||||
use crate::error::{Error, Result};
|
||||
use crate::profile::DriveProfile;
|
||||
use crate::scsi::{self, DataDirection, ScsiTransport};
|
||||
use super::{Platform, DriveStatus};
|
||||
|
||||
/// BD 1x speed in KB/s — used to convert speed multipliers to SET CD SPEED values.
|
||||
const BD_1X_SPEED: u16 = 4500;
|
||||
|
||||
/// MT1959 driver state.
|
||||
pub struct Mt1959 {
|
||||
profile: DriveProfile,
|
||||
mode: u8,
|
||||
buffer_id: u8,
|
||||
unlocked: bool,
|
||||
/// Speed table: maps disc zone (probe address >> 8) to speed in KB/s.
|
||||
/// Populated by calibrate(). Entry 0 = address 0x0000, entry 255 = address 0xFF00.
|
||||
speed_table: [u16; 256],
|
||||
/// Total disc sectors — for mapping LBA to zone index in speed table.
|
||||
/// Speed table: built by calibrate(), indexed by set_read_speed().
|
||||
/// 64 entries of u16 — zone boundary LBAs from disc surface probes.
|
||||
speed_table: [u16; 64],
|
||||
/// Total disc sectors — for LBA-to-zone mapping.
|
||||
disc_sectors: u32,
|
||||
calibrated: bool,
|
||||
/// 4 config bytes stored by calibrate() from initial probe response.
|
||||
calibration_config: [u8; 4],
|
||||
}
|
||||
|
||||
impl Mt1959 {
|
||||
@@ -39,18 +37,21 @@ impl Mt1959 {
|
||||
mode,
|
||||
buffer_id,
|
||||
unlocked: false,
|
||||
speed_table: [0u16; 256],
|
||||
speed_table: [0u16; 64],
|
||||
disc_sectors: 0,
|
||||
calibrated: false,
|
||||
calibration_config: [0u8; 4],
|
||||
}
|
||||
}
|
||||
|
||||
/// Build a READ BUFFER CDB for this platform's mode and buffer ID.
|
||||
// ── 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 a READ BUFFER CDB with a sub-command byte in CDB[3].
|
||||
/// 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,
|
||||
@@ -66,81 +67,71 @@ impl Mt1959 {
|
||||
]
|
||||
}
|
||||
|
||||
///
|
||||
/// 1. Send READ BUFFER(mode, buffer_id, offset=0, length=64)
|
||||
/// 2. Check response[0:4] matches the profile signature
|
||||
/// 3. Check response[12:16] matches the verification bytes (0x4D4D6B76)
|
||||
fn do_unlock(&mut self, scsi: &mut dyn ScsiTransport) -> Result<[u8; 64]> {
|
||||
let cdb = self.read_buffer_cdb(0, 64);
|
||||
let mut response = [0u8; 64];
|
||||
scsi.execute(&cdb, DataDirection::FromDevice, &mut response, 30_000)?;
|
||||
/// Send a SCSI command and check status.
|
||||
fn scsi_execute(
|
||||
&self,
|
||||
scsi: &mut dyn ScsiTransport,
|
||||
cdb: &[u8],
|
||||
direction: DataDirection,
|
||||
buf: &mut [u8],
|
||||
timeout: u32,
|
||||
) -> Result<usize> {
|
||||
let result = scsi.execute(cdb, direction, buf, timeout)?;
|
||||
Ok(result.bytes_transferred)
|
||||
}
|
||||
|
||||
// Check signature at response[0:4]
|
||||
let got_sig: [u8; 4] = response[0..4].try_into().unwrap();
|
||||
if got_sig != self.profile.signature {
|
||||
|
||||
/// Try to activate raw disc access. Returns Ok if active, Err if not.
|
||||
///
|
||||
/// 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]> {
|
||||
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];
|
||||
|
||||
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.signature,
|
||||
got: got_sig,
|
||||
expected: self.profile.drive_signature,
|
||||
got: response[0..4].try_into().unwrap(),
|
||||
});
|
||||
}
|
||||
|
||||
// Check verification bytes at response[12:16]
|
||||
if &response[12..16] != self.profile.verify.as_slice() {
|
||||
return Err(Error::UnlockFailed { detail: format!(
|
||||
"verify mismatch at [12:16]: {:02x}{:02x}{:02x}{:02x}",
|
||||
response[12], response[13], response[14], response[15]
|
||||
) });
|
||||
if &response[12..16] != b"MMkv" {
|
||||
return Err(Error::UnlockFailed {
|
||||
detail: format!(
|
||||
"mode not active: {:02x}{:02x}{:02x}{:02x}",
|
||||
response[12], response[13], response[14], response[15]
|
||||
),
|
||||
});
|
||||
}
|
||||
|
||||
// if signature + MMkv both match, the version is almost always OK.
|
||||
|
||||
self.unlocked = true;
|
||||
Ok(response)
|
||||
}
|
||||
|
||||
/// Ensure raw disc access is active, re-enabling if needed.
|
||||
fn ensure_unlocked(&mut self, scsi: &mut dyn ScsiTransport) -> Result<()> {
|
||||
if !self.unlocked {
|
||||
self.do_unlock(scsi)?;
|
||||
}
|
||||
Ok(())
|
||||
}
|
||||
|
||||
/// Pre-operation validation with retry.
|
||||
///
|
||||
/// Sends a short READ BUFFER probe, retries up to 5 times to confirm
|
||||
/// the drive is still responding to commands.
|
||||
fn validate(&mut self, scsi: &mut dyn ScsiTransport) -> Result<()> {
|
||||
/// 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 mut resp = [0u8; 4];
|
||||
match scsi.execute(&cdb, DataDirection::FromDevice, &mut resp, 5_000) {
|
||||
Ok(_) => return Ok(()),
|
||||
Err(_) => continue,
|
||||
if self.scsi_execute(scsi, &cdb, DataDirection::FromDevice, &mut resp, 5_000).is_ok() {
|
||||
return Ok(());
|
||||
}
|
||||
}
|
||||
Err(Error::ScsiError {
|
||||
opcode: 0x3C,
|
||||
status: 0xFF,
|
||||
sense_key: 0,
|
||||
})
|
||||
}
|
||||
|
||||
/// Look up optimal read speed for a given LBA from the calibration table.
|
||||
/// Returns speed in KB/s for SET CD SPEED, or 0 if not calibrated.
|
||||
fn lookup_speed(&self, lba: u32, disc_sectors: u32) -> u16 {
|
||||
if !self.calibrated || disc_sectors == 0 {
|
||||
return 0;
|
||||
}
|
||||
// Map LBA to zone index (0-255). Probe address space is 0x0000-0xFF00.
|
||||
let zone = ((lba as u64 * 256) / disc_sectors as u64).min(255) as usize;
|
||||
self.speed_table[zone]
|
||||
}
|
||||
|
||||
/// Send SET CD SPEED command.
|
||||
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(())
|
||||
Err(Error::ScsiError { opcode: 0x3C, status: 0xFF, sense_key: 0 })
|
||||
}
|
||||
}
|
||||
|
||||
@@ -151,42 +142,74 @@ impl Platform for Mt1959 {
|
||||
}
|
||||
|
||||
///
|
||||
/// Primary read: 0x760 (1888) bytes of configuration data.
|
||||
/// Secondary read: 4-byte status appended to the result.
|
||||
fn read_config(&mut self, scsi: &mut dyn ScsiTransport) -> Result<Vec<u8>> {
|
||||
// Primary config read: 0x760 = 1888 bytes
|
||||
let cdb = self.read_buffer_cdb(0, 0x760);
|
||||
let mut buf = vec![0u8; 0x760];
|
||||
let result = scsi.execute(&cdb, DataDirection::FromDevice, &mut buf, 30_000)?;
|
||||
buf.truncate(result.bytes_transferred);
|
||||
/// 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)
|
||||
/// 4. do_unlock() × 2
|
||||
fn load_firmware(&mut self, scsi: &mut dyn ScsiTransport) -> Result<()> {
|
||||
let microcode = &self.profile.ld_microcode;
|
||||
if microcode.is_empty() {
|
||||
return Err(Error::UnlockFailed {
|
||||
detail: "no ld_microcode in profile".into(),
|
||||
});
|
||||
}
|
||||
|
||||
// Secondary: 4-byte status read
|
||||
let cdb2 = self.read_buffer_cdb(0, 4);
|
||||
let mut status = [0u8; 4];
|
||||
scsi.execute(&cdb2, DataDirection::FromDevice, &mut status, 5_000)?;
|
||||
// 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,
|
||||
0x00, 0x00, 0x00,
|
||||
(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)?;
|
||||
|
||||
buf.extend_from_slice(&status);
|
||||
Ok(buf)
|
||||
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,
|
||||
);
|
||||
|
||||
self.do_unlock(scsi)?;
|
||||
self.do_unlock(scsi)?;
|
||||
|
||||
Ok(())
|
||||
}
|
||||
|
||||
/// Handlers 2-3: Read hardware register at the profile-specified offset.
|
||||
///
|
||||
/// Reads 36 bytes via READ BUFFER and extracts bytes [4:20] as the
|
||||
/// 16-byte register value.
|
||||
fn read_register(&mut self, scsi: &mut dyn ScsiTransport, index: u8) -> Result<[u8; 16]> {
|
||||
self.ensure_unlocked(scsi)?;
|
||||
fn read_register_a(&mut self, scsi: &mut dyn ScsiTransport) -> Result<[u8; 16]> {
|
||||
if !self.unlocked {
|
||||
self.do_unlock(scsi)?;
|
||||
}
|
||||
self.validate(scsi)?;
|
||||
|
||||
let offset = *self.profile.register_offsets.get(index as usize)
|
||||
.ok_or_else(|| Error::ProfileNotFound {
|
||||
vendor_id: self.profile.vendor_id.clone(),
|
||||
product_revision: self.profile.product_revision.clone(),
|
||||
vendor_specific: format!("register index {} out of range", index),
|
||||
})?;
|
||||
|
||||
let cdb = scsi::build_read_buffer(self.mode, self.buffer_id, offset, 36);
|
||||
let cdb = &self.profile.hardware_register_a_cdb;
|
||||
if cdb.len() != 10 {
|
||||
return Err(Error::UnlockFailed { detail: "missing register_a_cdb".into() });
|
||||
}
|
||||
let mut response = [0u8; 36];
|
||||
scsi.execute(&cdb, DataDirection::FromDevice, &mut response, 30_000)?;
|
||||
self.scsi_execute(scsi, cdb, DataDirection::FromDevice, &mut response, 30_000)?;
|
||||
|
||||
let mut out = [0u8; 16];
|
||||
out.copy_from_slice(&response[4..20]);
|
||||
Ok(out)
|
||||
}
|
||||
|
||||
fn read_register_b(&mut self, scsi: &mut dyn ScsiTransport) -> Result<[u8; 16]> {
|
||||
if !self.unlocked {
|
||||
self.do_unlock(scsi)?;
|
||||
}
|
||||
self.validate(scsi)?;
|
||||
|
||||
let cdb = &self.profile.hardware_register_b_cdb;
|
||||
if cdb.len() != 10 {
|
||||
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)?;
|
||||
|
||||
let mut out = [0u8; 16];
|
||||
out.copy_from_slice(&response[4..20]);
|
||||
@@ -194,58 +217,112 @@ impl Platform for Mt1959 {
|
||||
}
|
||||
|
||||
///
|
||||
/// Probes the disc surface to build a speed profile. Each zone gets
|
||||
/// an optimal speed in KB/s. The drive firmware returns a speed
|
||||
/// multiplier (resp[0]) for each probe address.
|
||||
///
|
||||
/// Probe address 0x0000-0xFF00 maps linearly to the disc's LBA range.
|
||||
/// resp[0] = speed multiplier (e.g. 6 = 6x BD, 12 = 12x BD).
|
||||
/// 1. do_unlock() — ensure active
|
||||
/// 2. init_timing()
|
||||
/// 3. READ_BUFFER sub_cmd=0x12, addr from disc type → init calibration
|
||||
/// 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
|
||||
fn calibrate(&mut self, scsi: &mut dyn ScsiTransport) -> Result<()> {
|
||||
self.ensure_unlocked(scsi)?;
|
||||
self.validate(scsi)?;
|
||||
// Step 1: ensure unlocked
|
||||
if !self.unlocked {
|
||||
self.do_unlock(scsi)?;
|
||||
}
|
||||
|
||||
// Read disc capacity for LBA-to-zone mapping
|
||||
// Step 2: read disc capacity for zone mapping
|
||||
let cap_cdb = [0x25u8, 0, 0, 0, 0, 0, 0, 0, 0, 0];
|
||||
let mut cap_buf = [0u8; 8];
|
||||
if let Ok(_) = scsi.execute(&cap_cdb, DataDirection::FromDevice, &mut cap_buf, 5_000) {
|
||||
if let Ok(_) = self.scsi_execute(scsi, &cap_cdb, DataDirection::FromDevice, &mut cap_buf, 5_000) {
|
||||
self.disc_sectors = u32::from_be_bytes([cap_buf[0], cap_buf[1], cap_buf[2], cap_buf[3]]) + 1;
|
||||
}
|
||||
|
||||
// Step 1: Calibration init — sub_cmd 0x12 with address 0x0200
|
||||
// (primes the firmware for disc surface analysis)
|
||||
let cdb = self.read_buffer_sub(0x12, 0x0200, 4);
|
||||
let mut resp = [0u8; 4];
|
||||
let _ = scsi.execute(&cdb, DataDirection::FromDevice, &mut resp, 5_000);
|
||||
// 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 mut init_resp = [0u8; 4];
|
||||
let _ = self.scsi_execute(scsi, &init_cdb, DataDirection::FromDevice, &mut init_resp, 5_000);
|
||||
|
||||
// Step 2: Raw read primers — read a few sectors with 0x08 flag
|
||||
// to force the drive to spin up and measure disc characteristics.
|
||||
// Without these, the speed probes return stale data.
|
||||
let mut primer_buf = [0u8; 2048];
|
||||
let _ = scsi.execute(
|
||||
&scsi::build_read10_raw(0, 1), DataDirection::FromDevice, &mut primer_buf, 30_000);
|
||||
let _ = scsi.execute(
|
||||
&scsi::build_read10_raw(0x200, 1), DataDirection::FromDevice, &mut primer_buf, 30_000);
|
||||
let _ = scsi.execute(
|
||||
&scsi::build_read10_raw(0, 1), DataDirection::FromDevice, &mut primer_buf, 30_000);
|
||||
// Step 4: validate
|
||||
self.validate(scsi)?;
|
||||
|
||||
// Step 3: Speed probes — sub_cmd 0x14, addresses 0x00 through 0xFF
|
||||
self.speed_table = [0u16; 256];
|
||||
// Step 5: clear speed table
|
||||
self.speed_table = [0u16; 64];
|
||||
|
||||
for zone in 0..256u16 {
|
||||
let cdb = self.read_buffer_sub(0x14, zone, 4);
|
||||
// Step 6: first probe — get initial speed zone data
|
||||
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 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
|
||||
|
||||
// Step 7: scan loop — find zone boundaries
|
||||
// When speed changes, record the boundary
|
||||
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);
|
||||
let mut resp = [0u8; 4];
|
||||
match scsi.execute(&cdb, DataDirection::FromDevice, &mut resp, 5_000) {
|
||||
Ok(r) if r.bytes_transferred >= 1 && resp[0] > 0 => {
|
||||
self.speed_table[zone as usize] = resp[0] as u16 * BD_1X_SPEED;
|
||||
}
|
||||
_ => {
|
||||
self.speed_table[zone as usize] = 0xFFFF;
|
||||
}
|
||||
if self.scsi_execute(scsi, &cdb, DataDirection::FromDevice, &mut resp, 5_000).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;
|
||||
}
|
||||
|
||||
addr = addr.wrapping_add(0x100);
|
||||
}
|
||||
|
||||
// Step 4: Set max speed
|
||||
self.set_cd_speed(scsi, 0xFFFF)?;
|
||||
// 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() {
|
||||
break;
|
||||
}
|
||||
|
||||
let speed = resp[0];
|
||||
if speed > prev_speed && speed > 0 {
|
||||
let idx = ((speed as usize) >> 1).saturating_sub(1);
|
||||
if idx < 64 && self.speed_table[idx] == 0 {
|
||||
self.speed_table[idx] = addr as u16;
|
||||
}
|
||||
}
|
||||
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);
|
||||
|
||||
// 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 _ = self.set_cd_speed(scsi, 0xFFFF);
|
||||
|
||||
self.calibrated = true;
|
||||
Ok(())
|
||||
@@ -256,26 +333,24 @@ impl Platform for Mt1959 {
|
||||
}
|
||||
|
||||
///
|
||||
/// Sends READ BUFFER with sub-command 0x13, reads 36 bytes.
|
||||
/// Checks signature at [0:4], returns feature data from [4:20].
|
||||
fn status(&mut self, scsi: &mut dyn ScsiTransport) -> Result<DriveStatus> {
|
||||
self.ensure_unlocked(scsi)?;
|
||||
if !self.unlocked {
|
||||
self.do_unlock(scsi)?;
|
||||
}
|
||||
self.validate(scsi)?;
|
||||
|
||||
// READ BUFFER with sub_cmd=0x13, 36 bytes response
|
||||
let cdb = self.read_buffer_sub(0x13, 0, 36);
|
||||
let mut response = [0u8; 36];
|
||||
scsi.execute(&cdb, DataDirection::FromDevice, &mut response, 30_000)?;
|
||||
self.scsi_execute(scsi, &cdb, DataDirection::FromDevice, &mut response, 30_000)?;
|
||||
|
||||
// Verify response signature
|
||||
let got_sig = u32::from_be_bytes(response[0..4].try_into().unwrap());
|
||||
let expected_sig = u32::from_le_bytes(self.profile.signature);
|
||||
let got_sig = u32::from_le_bytes(response[0..4].try_into().unwrap());
|
||||
let exp_sig = u32::from_le_bytes(self.profile.drive_signature);
|
||||
|
||||
let mut features = [0u8; 16];
|
||||
features.copy_from_slice(&response[4..20]);
|
||||
|
||||
Ok(DriveStatus {
|
||||
unlocked: got_sig == expected_sig,
|
||||
unlocked: got_sig == exp_sig,
|
||||
features,
|
||||
})
|
||||
}
|
||||
@@ -294,89 +369,104 @@ impl Platform for Mt1959 {
|
||||
length as u8,
|
||||
];
|
||||
let mut buf = vec![0u8; length as usize];
|
||||
let result = scsi.execute(&cdb, DataDirection::FromDevice, &mut buf, 30_000)?;
|
||||
buf.truncate(result.bytes_transferred);
|
||||
let n = self.scsi_execute(scsi, &cdb, DataDirection::FromDevice, &mut buf, 30_000)?;
|
||||
buf.truncate(n);
|
||||
Ok(buf)
|
||||
}
|
||||
|
||||
///
|
||||
/// Looks up the LBA in the speed table, issues SET CD SPEED if calibrated,
|
||||
/// then performs READ(10) with the raw read flag (0x08).
|
||||
fn read_sectors(
|
||||
&mut self,
|
||||
scsi: &mut dyn ScsiTransport,
|
||||
lba: u32,
|
||||
count: u16,
|
||||
buf: &mut [u8],
|
||||
) -> Result<usize> {
|
||||
if !self.unlocked {
|
||||
return Err(Error::NotUnlocked);
|
||||
/// 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
|
||||
fn set_read_speed(&mut self, scsi: &mut dyn ScsiTransport, lba: u32) -> Result<()> {
|
||||
if !self.calibrated || self.disc_sectors == 0 {
|
||||
return Ok(());
|
||||
}
|
||||
|
||||
// No per-read speed changes — calibrate + SET CD SPEED at open_title handles it.
|
||||
// READ(10) with raw flag 0x08
|
||||
let cdb = scsi::build_read10_raw(lba, count);
|
||||
let result = scsi.execute(&cdb, DataDirection::FromDevice, buf, 30_000)?;
|
||||
Ok(result.bytes_transferred)
|
||||
// Look up closest speed table entry
|
||||
let mut best_idx = 0usize;
|
||||
let mut best_diff = u32::MAX;
|
||||
for i in 0..64 {
|
||||
let entry = self.speed_table[i] as u32;
|
||||
if entry == 0 {
|
||||
continue;
|
||||
}
|
||||
let diff = if lba > entry { lba - entry } else { entry - lba };
|
||||
if diff < best_diff {
|
||||
best_diff = diff;
|
||||
best_idx = i;
|
||||
}
|
||||
}
|
||||
|
||||
let speed_val = self.speed_table[best_idx];
|
||||
if speed_val == 0 {
|
||||
return Ok(());
|
||||
}
|
||||
|
||||
let _ = self.set_cd_speed(scsi, 0xFFFF);
|
||||
|
||||
// The speed value from the table is used directly
|
||||
let _ = self.set_cd_speed(scsi, speed_val);
|
||||
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn timing(&mut self, _scsi: &mut dyn ScsiTransport) -> Result<()> {
|
||||
Ok(())
|
||||
}
|
||||
|
||||
/// Continuous speed management — probes zone, reads registers, sets speed.
|
||||
/// Full init sequence — matches x86 dispatch exactly.
|
||||
///
|
||||
/// MediaTek drives decay to 1x BD speed (~5 MB/s instead of 15-20 MB/s).
|
||||
///
|
||||
/// Sequence (from strace analysis):
|
||||
/// 1. Speed probe (sub_cmd 0x14) for current LBA zone
|
||||
/// 2. Read register A (sub_cmd 0x10 at profile offset A)
|
||||
/// 3. Read register B (sub_cmd 0x11 at profile offset B)
|
||||
/// 4. SET CD SPEED: max → zone_speed → max
|
||||
fn maintain_speed(&mut self, scsi: &mut dyn ScsiTransport, lba: u32) -> Result<()> {
|
||||
if !self.unlocked || self.disc_sectors == 0 {
|
||||
return Ok(());
|
||||
}
|
||||
|
||||
// 1. Probe current zone
|
||||
let zone = ((lba as u64 * 256) / self.disc_sectors as u64).min(255) as u16;
|
||||
let cdb = self.read_buffer_sub(0x14, zone, 4);
|
||||
let mut resp = [0u8; 4];
|
||||
let _ = scsi.execute(&cdb, DataDirection::FromDevice, &mut resp, 5_000);
|
||||
let zone_speed = if resp[0] > 0 {
|
||||
resp[0] as u16 * BD_1X_SPEED
|
||||
} else {
|
||||
0xFFFF
|
||||
};
|
||||
|
||||
// 2. Read drive registers (handlers 2 & 3)
|
||||
if self.profile.register_offsets.len() >= 2 {
|
||||
for i in 0..2 {
|
||||
let offset = self.profile.register_offsets[i];
|
||||
let sub_cmd = 0x10 + i as u8;
|
||||
let cdb = [
|
||||
0x3C,
|
||||
self.mode,
|
||||
self.buffer_id,
|
||||
sub_cmd,
|
||||
(offset >> 16) as u8,
|
||||
(offset >> 8) as u8,
|
||||
offset as u8,
|
||||
0x00,
|
||||
0x24, // 36 bytes
|
||||
0x00,
|
||||
];
|
||||
let mut buf = [0u8; 36];
|
||||
let _ = scsi.execute(&cdb, DataDirection::FromDevice, &mut buf, 5_000);
|
||||
/// cmd 0 → [cmd 1 if fail] × 6 retries
|
||||
/// cmd 4 × 6 retries
|
||||
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 {
|
||||
match self.unlock(scsi) {
|
||||
Ok(_) => {
|
||||
unlocked = true;
|
||||
break;
|
||||
}
|
||||
Err(_) => {
|
||||
// Cold boot: firmware not loaded, try uploading
|
||||
if let Ok(_) = self.load_firmware(scsi) {
|
||||
unlocked = true;
|
||||
break;
|
||||
}
|
||||
if attempt < 5 {
|
||||
continue; // retry
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 3. Triple SET CD SPEED: max → zone → max
|
||||
let _ = self.set_cd_speed(scsi, 0xFFFF);
|
||||
if zone_speed < 0xFFFF {
|
||||
let _ = self.set_cd_speed(scsi, zone_speed);
|
||||
if !unlocked {
|
||||
return Err(Error::UnlockFailed {
|
||||
detail: "failed after 6 attempts (unlock + load_firmware)".into(),
|
||||
});
|
||||
}
|
||||
let _ = self.set_cd_speed(scsi, 0xFFFF);
|
||||
|
||||
// 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 !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);
|
||||
|
||||
Ok(())
|
||||
}
|
||||
@@ -385,3 +475,14 @@ 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