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.

src/platform/mt1959.rs

@@ -18,13 +18,14 @@ pub struct Mt1959 {
     mode: u8,
     buffer_id: u8,
     unlocked: bool,
-    /// Speed table: built by calibrate(), indexed by set_read_speed().
+    /// Speed table: 64 × u16, built by calibrate().
-    /// 64 entries of u16 — zone boundary LBAs from disc surface probes.
+    /// 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,
+            0x3C, self.mode, self.buffer_id, sub_cmd,
-            self.mode,
+            (address >> 8) as u8, address as u8,
-            self.buffer_id,
+            0x00, 0x00, length, 0x00,
-            sub_cmd,
-            (address >> 8) as u8,
-            address as u8,
-            0x00,
-            0x00,
-            length,
-            0x00,
         ]
     }
 
-    /// Send a SCSI command and check status.
+    /// Calls dynamic_read_buffer, validates response size matches expected.
-    fn scsi_execute(
+    /// Returns Ok(bytes_transferred) or Err.
-        &self,
+    fn read_buffer_probe(
-        scsi: &mut dyn ScsiTransport,
+        &self, scsi: &mut dyn ScsiTransport,
-        cdb: &[u8],
+        sub_cmd: u8, address: u16, buf: &mut [u8], expected: usize,
-        direction: DataDirection,
-        buf: &mut [u8],
-        timeout: u32,
     ) -> 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)
+    ///   r3 = unlock_init_value (1)
-    ///   2. Check response[0:4] == drive_signature
+    ///   sp[0x1C] = r3
-    ///   3. Check response[12:16] == "MMkv" (mode_active_magic)
+    ///   r3 += unlock_response_size_minus_init (0x3F)
-    ///   4. Check response[16:20] version range
+    ///   sp[0] = r3 (= 64 = response size)
-    ///   5. Return success/failure code
+    ///   scsi_cmd_wrapper(result, 0x0A, unlock_CDB, response)
-    fn do_unlock(&mut self, scsi: &mut dyn ScsiTransport) -> Result<[u8; 64]> {
+    ///   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 cdb = [
-        let mut response = [0u8; 64];
+            0x3C, self.mode, self.buffer_id,
-        let buf = &mut response[..response_size as usize];
+            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)?;
+        if response.len() >= 4 {
-
+            let got = &response[0..4];
-        let got_sig = u32::from_le_bytes(response[0..4].try_into().unwrap());
+            if got != self.profile.drive_signature {
-        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(),
+                    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
+    ///   1. WRITE_BUFFER mode=6 with ld_microcode (size = payload.len())
-    ///   2. Check: all bytes transferred?
+    ///   2. Check all bytes transferred
-    ///   3. READ_BUFFER buf=0x45 → verify (expect response[0] == 2)
+    ///   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(
+        let _ = scsi.execute(
-            scsi, &verify_cdb, DataDirection::FromDevice, &mut verify_resp, 5_000,
+            &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 {
+        if !self.unlocked { self.do_unlock(scsi)?; }
-            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 {
+        if !self.unlocked { self.do_unlock(scsi)?; }
-            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
+    ///   6. First probe: sub_cmd=0x14, addr=0 → initial speed
-    ///   7. Scan loop: probe addresses 0x0000-0x5800, find zone boundaries
+    ///   7. Scan loop: addresses 0x0000-0x5800, step 0x100, detect zone boundaries
-    ///   8. Build loop: probe all zones, store boundaries in speed_table
+    ///   8. Build loop: scan all zones up to 0x10000, store in speed_table[(speed>>1)-1]
-    ///   9. SET_CD_SPEED max → drive_nominal_speed → max (triple play)
+    ///   9. Triple SET_CD_SPEED: max → drive_nominal_speed → max
-    ///  10. Store 4 config bytes from probe responses
+    ///  10. Store 4 calibration config bytes
     fn calibrate(&mut self, scsi: &mut dyn ScsiTransport) -> Result<()> {
         // Step 1: ensure unlocked
-        if !self.unlocked {
+        if !self.unlocked { self.do_unlock(scsi)?; }
-            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; // 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);
+        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.read_buffer_probe(scsi, 0x14, 0, &mut probe_buf, 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[0] = probe_buf[0];
-        self.calibration_config[1] = probe_buf[1]; // data byte
+        self.calibration_config[1] = probe_buf[1];
-        self.calibration_config[2] = probe_buf[2]; // data byte
+        self.calibration_config[2] = probe_buf[2];
 
-        // Step 7: scan loop — find zone boundaries
+        // Step 7: scan loop — find zone boundaries (0x0000-0x5800, step 0x100)
-        // 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 < 0x5800 {
-
-        while addr < max_addr && table_idx < 64 {
-            let cdb = self.read_buffer_sub(0x14, addr, 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, &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 });
             }
-
+            if resp[0] != prev_speed {
-            let speed = resp[0];
+                prev_speed = resp[0];
-            if speed != prev_speed {
-                // Zone boundary found — record it
-                prev_speed = speed;
             }
-
             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
+        // Step 9: triple SET_CD_SPEED — max → nominal → max
-        let _ = self.set_cd_speed(scsi, 0xFFFF);
+        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 {
+        if !self.unlocked { self.do_unlock(scsi)?; }
-            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());
+        self.validate(scsi)?;
-        let exp_sig = u32::from_le_bytes(self.profile.drive_signature);
+
+        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,
+            0x3C, self.mode, self.buffer_id, sub_cmd,
-            self.mode,
+            (address >> 16) as u8, (address >> 8) as u8, address as u8,
-            self.buffer_id,
+            (length >> 16) as u8, (length >> 8) as u8, length as u8,
-            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)?;
+        let result = scsi.execute(&cdb, DataDirection::FromDevice, &mut buf, 30_000)?;
-        buf.truncate(n);
+        buf.truncate(result.bytes_transferred);
         Ok(buf)
     }
 
     ///
-    ///   1. Look up LBA in speed_zone_table / speed_table
+    /// Speed management for content reads. Called per zone change.
-    ///   2. SET_CD_SPEED max
+    ///
-    ///   3. SET_CD_SPEED with zone-specific value
+    ///   1. Read 4-byte target LBA from host
-    ///   4. Position check via READ_BUFFER sub_cmd=0x14
+    ///   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
+        // the byte-swapped LBA against table entries. Since entries are
-        let mut best_idx = 0usize;
+        // zone boundary addresses (u16), we compare the low 16 bits.
-        let mut best_diff = u32::MAX;
+
+        // 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 {
+            if entry == 0 { continue; }
-                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 !found {
-        if speed_val == 0 {
+            // 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
+    ///   Phase 1: cmd 0 → [cmd 1 if fail] × 6 retries (unlock + fw upload)
-    ///   cmd 4 × 6 retries
+    ///   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(_) => {
+                Ok(_) => { unlocked = true; break; }
-                    unlocked = true;
-                    break;
-                }
                 Err(_) => {
-                    // Cold boot: firmware not loaded, try uploading
+                    if self.load_firmware(scsi).is_ok() {
-                    if let Ok(_) = self.load_firmware(scsi) {
                         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) {
+            if self.calibrate(scsi).is_ok() {
-                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)
+        // If fails: cmd 5 fallback (keepalive)
-        let _ = self.read_register_a(scsi);
+        // In our context: this fetches a display string, not critical for reads
-        let _ = self.read_register_b(scsi);
+        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(())
-    }
-}