1#![forbid(unsafe_code)]
30
31use self::floppy_sizes::FloppyImageType;
32use self::protocol::FLOPPY_TOTAL_CYLINDERS;
33use self::protocol::FloppyCommand;
34use self::protocol::INVALID_COMMAND_STATUS;
35use self::protocol::RegisterOffset;
36use self::protocol::STANDARD_FLOPPY_SECTOR_SIZE;
37use arrayvec::ArrayVec;
38use chipset_device::ChipsetDevice;
39use chipset_device::io::IoError;
40use chipset_device::io::IoResult;
41use chipset_device::pio::ControlPortIoIntercept;
42use chipset_device::pio::PortIoIntercept;
43use chipset_device::pio::RegisterPortIoIntercept;
44use chipset_device::poll_device::PollDevice;
45use core::sync::atomic::Ordering;
46use disk_backend::Disk;
47use guestmem::AlignedHeapMemory;
48use guestmem::GuestMemory;
49use guestmem::ranges::PagedRange;
50use inspect::Inspect;
51use inspect::InspectMut;
52use scsi_buffers::RequestBuffers;
53use std::future::Future;
54use std::pin::Pin;
55use std::sync::Arc;
56use std::task::Context;
57use std::task::Poll;
58use std::task::Waker;
59use thiserror::Error;
60use vmcore::device_state::ChangeDeviceState;
61use vmcore::isa_dma_channel::IsaDmaChannel;
62use vmcore::isa_dma_channel::IsaDmaDirection;
63use vmcore::line_interrupt::LineInterrupt;
64
65mod floppy_sizes {
66 use super::protocol::FLOPPY_TOTAL_CYLINDERS;
67 use super::protocol::STANDARD_FLOPPY_SECTOR_SIZE;
68
69 const HDMSS_SECTORS_PER_TRACK: u8 = 23;
70 const DMF_SECTORS_PER_TRACK: u8 = 21;
71 const HD_SECTORS_PER_TRACK: u8 = 18;
72 const MD_SECTORS_PER_TRACK: u8 = 15;
73 const LD_SECTORS_PER_TRACK: u8 = 9;
74
75 const fn calculate_image_size(sectors_per_track: u8) -> u64 {
76 sectors_per_track as u64
77 * STANDARD_FLOPPY_SECTOR_SIZE as u64
78 * FLOPPY_TOTAL_CYLINDERS as u64
79 * 2
80 }
81
82 const HDMSS_FLOPY_IMAGE_SIZE: u64 = calculate_image_size(HDMSS_SECTORS_PER_TRACK);
83 const DMF_FLOPPY_IMAGE_SIZE: u64 = calculate_image_size(DMF_SECTORS_PER_TRACK);
84 const HD_FLOPPY_IMAGE_SIZE: u64 = calculate_image_size(HD_SECTORS_PER_TRACK);
85 const MD_FLOPPY_IMAGE_SIZE: u64 = calculate_image_size(MD_SECTORS_PER_TRACK);
86 const LD_FLOPPY_IMAGE_SIZE: u64 = calculate_image_size(LD_SECTORS_PER_TRACK);
87 const LDSS_FLOPPY_IMAGE_SIZE: u64 = calculate_image_size(LD_SECTORS_PER_TRACK) / 2;
88
89 pub enum FloppyImageType {
90 LowDensitySingleSided,
92 LowDensity,
94 MediumDensity,
96 HighDensity,
98 Dmf,
100 HighDensityMss,
103 }
104
105 impl FloppyImageType {
106 pub fn sectors(&self) -> u8 {
107 match self {
108 FloppyImageType::LowDensity => LD_SECTORS_PER_TRACK,
109 FloppyImageType::HighDensity => HD_SECTORS_PER_TRACK,
110 FloppyImageType::Dmf => DMF_SECTORS_PER_TRACK,
111 FloppyImageType::LowDensitySingleSided => LD_SECTORS_PER_TRACK,
112 FloppyImageType::MediumDensity => MD_SECTORS_PER_TRACK,
113 FloppyImageType::HighDensityMss => HDMSS_SECTORS_PER_TRACK,
114 }
115 }
116
117 pub fn from_file_size(file_size: u64) -> Option<Self> {
118 let res = match file_size {
119 HD_FLOPPY_IMAGE_SIZE => FloppyImageType::HighDensity,
120 DMF_FLOPPY_IMAGE_SIZE => FloppyImageType::Dmf,
121 LD_FLOPPY_IMAGE_SIZE => FloppyImageType::LowDensity,
122 MD_FLOPPY_IMAGE_SIZE => FloppyImageType::MediumDensity,
123 LDSS_FLOPPY_IMAGE_SIZE => FloppyImageType::LowDensitySingleSided,
124 HDMSS_FLOPY_IMAGE_SIZE => FloppyImageType::HighDensityMss,
125 _ => return None,
126 };
127 Some(res)
128 }
129 }
130}
131
132mod protocol {
133 use bitfield_struct::bitfield;
134 use inspect::Inspect;
135 use open_enum::open_enum;
136
137 pub const FIFO_SIZE: usize = 16;
138
139 pub const INVALID_COMMAND_STATUS: u8 = 0x80; pub const STANDARD_FLOPPY_SECTOR_SIZE: usize = 512;
142 pub const FLOPPY_TOTAL_CYLINDERS: u8 = 80;
143
144 #[derive(Inspect)]
145 #[bitfield(u8)]
146 pub struct InputRegister {
147 #[bits(2)]
148 pub drive_select: u8,
149 #[bits(1)]
150 pub head: u8,
151 #[bits(5)]
152 unused2: u8,
153 }
154
155 #[derive(Inspect)]
156 #[bitfield(u8)]
157 pub struct StatusRegister0 {
158 #[bits(2)]
159 pub drive_select: u8,
160 #[bits(1)]
161 pub head: u8,
162 #[bits(2)]
163 unused: u8,
164 pub seek_end: bool,
165 pub abnormal_termination: bool,
166 pub invalid_command: bool,
167 }
168
169 #[derive(Inspect)]
170 #[bitfield(u8)]
171 pub struct StatusRegister1 {
172 pub missing_address: bool,
173 pub write_protected: bool,
174 pub no_data: bool,
175 #[bits(5)]
176 unused: u8,
177 }
178
179 #[derive(Inspect)]
180 #[bitfield(u8)]
181 pub struct StatusRegister2 {
182 pub missing_address: bool,
183 pub bad_cylinder: bool,
184 #[bits(6)]
185 unused: u8,
186 }
187
188 #[derive(Inspect)]
189 #[bitfield(u8)]
190 pub struct StatusRegister3 {
191 #[bits(2)]
192 pub drive_select: u8,
193 #[bits(1)]
194 pub head: u8,
195 pub unused1: bool, pub track0: bool,
197 pub unused2: bool, pub write_protected: bool,
199 pub unused3: bool, }
201
202 open_enum! {
203 #[derive(Default)]
204 pub enum RegisterOffset: u16 {
205 STATUS_A = 0, STATUS_B = 1, DIGITAL_OUTPUT = 2,
208 TAPE_DRIVE = 3, MAIN_STATUS = 4, DATA_RATE = 4, DATA = 5,
212 DIGITAL_INPUT = 7,CONFIG_CONTROL = 7, }
215 }
216
217 #[derive(Inspect)]
223 #[bitfield(u8)]
224 pub struct DigitalOutputRegister {
225 #[bits(2)]
231 pub _drive_select: u8,
232
233 pub controller_enabled: bool,
235
236 pub dma_enabled: bool,
238
239 #[bits(4)]
242 pub motors_active: u8,
243 }
244
245 #[derive(Debug, Copy, Clone, PartialEq, Eq)]
246 pub enum DataDirection {
247 Write = 0,
252 Read = 1,
257 }
258
259 impl DataDirection {
260 pub fn as_bool(self) -> bool {
261 match self {
262 Self::Write => false,
263 Self::Read => true,
264 }
265 }
266 }
267
268 #[derive(Inspect)]
270 #[bitfield(u8)]
271 pub struct MainStatusRegister {
272 #[bits(4)]
275 pub active_drives: u8,
276 pub busy: bool,
278 pub non_dma_mode: bool,
280 pub data_direction: bool, pub main_request: bool,
286 }
287
288 #[derive(Inspect)]
291 #[bitfield(u8)]
292 pub struct DigitalInputRegister {
293 #[bits(7)]
295 pub tristated: u8,
296
297 #[bits(1)]
298 pub disk_change: bool,
299 }
300
301 open_enum! {
302 #[derive(Default)]
303 pub enum FloppyCommand: u8 {
307 VERIFY = 0x16,
315 VERIFY2 = 0xF6,
316 VERSION = 0x10,
319 FORMAT_TRACK = 0x4D,
320 FORMAT_DOUBLE_DENSITY_MODE = 0xCD,
321 SCAN_EQUAL_ALL = 0xD1,
323 SCAN_EQUAL = 0xF1,
324 SCAN_LOW_OR_EQUAL = 0x19,
325 SCAN_HIGH_OR_EQUAL = 0x1D,
326 RECALIBRATE = 0x07,
337 SENSE_INTERRUPT_STATUS = 0x08,
343 SPECIFY = 0x03,
350 SENSE_DRIVE_STATUS = 0x04,
353 DRIVE_SPECIFICATION_COMMAND = 0x8E,
354 SEEK = 0x0F,
369 CONFIGURE = 0x13,
372 RELATIVE_SEEK_IN = 0xCF,
376 RELATIVE_SEEK_OUT = 0x8F,
377 DUMP_REGISTERS = 0x0E,
379 READ_ID = 0x4A, PERP288_MODE = 0x12,
387 UNLOCK_FIFO_FUNCTIONS = 0x14,
393 LOCK_FIFO_FUNCTIONS = 0x94,
395 PART_ID = 0x18,
397 POWERDOWN_MODE = 0x17,
398 OPTION = 0x33,
399 SAVE = 0x2E,
400 RESTORE = 0x4E,
401 FORMAT_AND_WRITE = 0xAD,
402
403 EXIT_STANDBY_MODE = 0x34,
404 GOTO_STANDBY_MODE = 0x35,
405 HARD_RESET = 0x36,
406 READ_TRACK = 0x42,
407 SEEK_AND_WRITE = 0x45,
408 SEEK_AND_READ = 0x46,
409 ALT_SEEK_AND_READ = 0x66,
410 WRITE_DATA = 0xC5,
411 READ_NORMAL_DEL_DATA = 0xC6,
412 WRITE_DEL_DATA = 0xC9,
413 READ_DEL_DATA = 0xCC,
414 WRITE_NORMAL_DATA = 0xE5, READ_NORMAL_DATA = 0xE6,
416
417 INVALID = 0x00,
418 }
419 }
420
421 impl FloppyCommand {
422 pub fn input_bytes_needed(&self) -> usize {
426 1 + match *self {
428 Self::READ_DEL_DATA => 8,
429 Self::WRITE_DATA => 8,
430 Self::WRITE_DEL_DATA => 8,
431 Self::READ_TRACK => 8,
432 Self::VERIFY => 8,
433 Self::VERSION => 0,
434 Self::FORMAT_TRACK => 5,
435 Self::FORMAT_DOUBLE_DENSITY_MODE => 5,
436 Self::SCAN_EQUAL_ALL => 8,
437 Self::SCAN_EQUAL => 8,
438 Self::SCAN_LOW_OR_EQUAL => 8,
439 Self::SCAN_HIGH_OR_EQUAL => 8,
440 Self::RECALIBRATE => 1,
441 Self::SENSE_INTERRUPT_STATUS => 0,
442 Self::SPECIFY => 2,
443 Self::SENSE_DRIVE_STATUS => 1,
444 Self::DRIVE_SPECIFICATION_COMMAND => 6,
445 Self::SEEK => 2,
446 Self::CONFIGURE => 3,
447 Self::RELATIVE_SEEK_IN => 2,
448 Self::RELATIVE_SEEK_OUT => 2,
449 Self::DUMP_REGISTERS => 0,
450 Self::READ_ID => 1,
451 Self::PERP288_MODE => 1,
452 Self::UNLOCK_FIFO_FUNCTIONS => 0,
453 Self::LOCK_FIFO_FUNCTIONS => 0,
454 Self::PART_ID => 0,
455 Self::POWERDOWN_MODE => 1,
456 Self::OPTION => 1,
457 Self::SAVE => 0,
458 Self::RESTORE => 16,
459 Self::FORMAT_AND_WRITE => 5,
460
461 Self::SEEK_AND_WRITE => 8,
466 Self::SEEK_AND_READ => 8,
467 Self::ALT_SEEK_AND_READ => 8,
468 Self::READ_NORMAL_DEL_DATA => 8,
469 Self::WRITE_NORMAL_DATA => 8,
470 Self::READ_NORMAL_DATA => 8,
471
472 _ => 0,
474 }
475 }
476
477 pub fn result_bytes_expected(&self) -> usize {
478 match *self {
479 Self::READ_DEL_DATA => 7,
480 Self::WRITE_DATA => 7,
481 Self::WRITE_DEL_DATA => 7,
482 Self::READ_TRACK => 7,
483 Self::VERIFY => 7,
484 Self::VERSION => 1,
485 Self::FORMAT_TRACK => 7,
486 Self::FORMAT_DOUBLE_DENSITY_MODE => 7,
487 Self::SCAN_EQUAL_ALL => 7,
488 Self::SCAN_EQUAL => 7,
489 Self::SCAN_LOW_OR_EQUAL => 7,
490 Self::SCAN_HIGH_OR_EQUAL => 7,
491 Self::RECALIBRATE => 2,
492 Self::SENSE_INTERRUPT_STATUS => 2,
493 Self::SPECIFY => 0,
494 Self::SENSE_DRIVE_STATUS => 1,
495 Self::DRIVE_SPECIFICATION_COMMAND => 0,
496 Self::SEEK => 2, Self::CONFIGURE => 0,
498 Self::RELATIVE_SEEK_IN => 2, Self::RELATIVE_SEEK_OUT => 2, Self::DUMP_REGISTERS => 10,
501 Self::READ_ID => 7,
502 Self::PERP288_MODE => 0,
503 Self::UNLOCK_FIFO_FUNCTIONS => 1,
504 Self::LOCK_FIFO_FUNCTIONS => 1,
505 Self::PART_ID => 1,
506 Self::POWERDOWN_MODE => 1,
507 Self::OPTION => 1,
508 Self::SAVE => 16,
509 Self::RESTORE => 0,
510 Self::FORMAT_AND_WRITE => 7,
511
512 Self::SEEK_AND_WRITE => 7,
516 Self::SEEK_AND_READ => 7,
517 Self::ALT_SEEK_AND_READ => 7,
518 Self::READ_NORMAL_DEL_DATA => 7,
519 Self::WRITE_NORMAL_DATA => 7,
520 Self::READ_NORMAL_DATA => 7,
521
522 Self::INVALID => 1,
523 _ => 0,
524 }
525 }
526 }
527
528 #[derive(Inspect)]
529 #[bitfield(u8)]
530 pub struct SpecifyParam1 {
531 #[bits(4)]
532 pub head_unload_timer: u8,
533 #[bits(4)]
534 pub step_rate_time: u8,
535 }
536
537 #[derive(Inspect)]
538 #[bitfield(u8)]
539 pub struct SpecifyParam2 {
540 #[bits(7)]
541 pub head_load_timer: u8,
542 pub dma_disabled: bool,
543 }
544}
545
546const MAX_CMD_BUFFER_BYTES: usize = 64 * 1024;
547
548#[derive(Debug)]
549struct CommandBuffer {
550 buffer: Arc<AlignedHeapMemory>,
551}
552
553#[derive(Debug)]
554struct CommandBufferAccess {
555 memory: GuestMemory,
556}
557
558impl CommandBuffer {
559 fn new() -> Self {
560 Self {
561 buffer: Arc::new(AlignedHeapMemory::new(MAX_CMD_BUFFER_BYTES)),
562 }
563 }
564
565 fn access(&self) -> CommandBufferAccess {
566 CommandBufferAccess {
567 memory: GuestMemory::new("floppy_buffer", self.buffer.clone()),
568 }
569 }
570}
571
572impl CommandBufferAccess {
573 fn buffers(&self, offset: usize, len: usize, is_write: bool) -> RequestBuffers<'_> {
574 static BUFFER_RANGE: Option<PagedRange<'_>> = PagedRange::new(
576 0,
577 MAX_CMD_BUFFER_BYTES,
578 &[0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15],
579 );
580
581 RequestBuffers::new(
582 &self.memory,
583 BUFFER_RANGE.unwrap().subrange(offset, len),
584 is_write,
585 )
586 }
587}
588
589struct Io(Pin<Box<dyn Send + Future<Output = Result<(), disk_backend::DiskError>>>>);
590
591impl ChangeDeviceState for FloppyDiskController {
592 fn start(&mut self) {}
593
594 async fn stop(&mut self) {}
595
596 async fn reset(&mut self) {
597 self.reset(false);
598 }
599}
600
601impl ChipsetDevice for FloppyDiskController {
602 fn supports_pio(&mut self) -> Option<&mut dyn PortIoIntercept> {
603 Some(self)
604 }
605
606 fn supports_poll_device(&mut self) -> Option<&mut dyn PollDevice> {
607 Some(self)
608 }
609}
610
611impl PollDevice for FloppyDiskController {
612 fn poll_device(&mut self, cx: &mut Context<'_>) {
613 if let Some(io) = self.io.as_mut() {
614 if let Poll::Ready(result) = io.0.as_mut().poll(cx) {
615 self.io = None;
616 self.handle_io_completion(result);
617 }
618 }
619 self.waker = Some(cx.waker().clone());
620 }
621}
622
623impl PortIoIntercept for FloppyDiskController {
624 fn io_read(&mut self, io_port: u16, data: &mut [u8]) -> IoResult {
625 if data.len() != 1 {
626 return IoResult::Err(IoError::InvalidAccessSize);
627 }
628
629 let offset = RegisterOffset(io_port % 0x10);
630 data[0] = match offset {
631 RegisterOffset::STATUS_A => 0xFF,
633 RegisterOffset::STATUS_B => 0xFC,
635 RegisterOffset::TAPE_DRIVE => 0xFF,
637 RegisterOffset::DIGITAL_OUTPUT => self.state.digital_output.into(),
638 RegisterOffset::MAIN_STATUS => {
639 if self.state.digital_output.controller_enabled() {
642 self.state.main_status.into()
643 } else {
644 0
645 }
646 }
647 RegisterOffset::DATA => {
648 let active_drive = self.state.main_status.active_drives();
650 let io_direction = self.state.main_status.data_direction();
651 tracing::trace!(?active_drive, ?io_direction, "DATA io read state");
652
653 if let Some(result) = self.state.output_bytes.pop() {
654 self.state.main_status.set_active_drives(0);
655 if self.state.output_bytes.is_empty() {
656 self.state.main_status = (self.state.main_status)
658 .with_non_dma_mode(false)
659 .with_busy(false)
660 .with_main_request(true)
661 .with_data_direction(protocol::DataDirection::Write.as_bool());
662 }
663 result
664 } else {
665 INVALID_COMMAND_STATUS
666 }
667 }
668
669 RegisterOffset::DIGITAL_INPUT => {
672 let val = protocol::DigitalInputRegister::new()
675 .with_tristated(0x7f)
676 .with_disk_change(
677 self.state.digital_output.motors_active() != 0
678 && (!self.state.internals.floppy_present
679 || self.state.internals.floppy_changed),
680 );
681
682 val.into()
683 }
684 _ => return IoResult::Err(IoError::InvalidRegister),
685 };
686
687 tracing::trace!(?offset, ?data, "floppy pio read");
688
689 IoResult::Ok
690 }
691
692 fn io_write(&mut self, io_port: u16, data: &[u8]) -> IoResult {
693 if data.len() != 1 {
694 return IoResult::Err(IoError::InvalidAccessSize);
695 }
696
697 let data = data[0];
698 let offset = RegisterOffset(io_port % 0x10);
699 tracing::trace!(?offset, ?data, "floppy pio write");
700 match offset {
701 RegisterOffset::STATUS_A | RegisterOffset::STATUS_B => {
702 tracelimit::warn_ratelimited!(
703 ?data,
704 ?offset,
705 "write to read-only floppy status register"
706 );
707 }
708 RegisterOffset::TAPE_DRIVE => {
709 tracing::debug!(?data, "write to obsolete tape drive register");
710 } RegisterOffset::CONFIG_CONTROL => {
712 tracing::debug!(?data, "write to control register");
715 }
716 RegisterOffset::DATA_RATE => {
717 const FLOPPY_DSR_DISK_RESET_MASK: u8 = 0x80; if self.state.digital_output.controller_enabled()
720 && (data & FLOPPY_DSR_DISK_RESET_MASK) != 0
721 {
722 self.reset(true);
723 self.state.sense_output = Some(SenseOutput::ResetCounter { count: 4 });
724 self.raise_interrupt(true);
726 tracing::trace!("DSR wr: Un-resetting - asserting floppy interrupt");
727 }
728 }
729 RegisterOffset::DIGITAL_OUTPUT => {
730 let new_digital_output = protocol::DigitalOutputRegister::from(data);
731 let was_reset = !self.state.digital_output.controller_enabled();
738 let is_reset = !new_digital_output.controller_enabled();
739 let interrupts_were_enabled = self.state.digital_output.dma_enabled();
740 let interrupts_enabled = new_digital_output.dma_enabled();
741 self.state.digital_output = new_digital_output;
742
743 if was_reset && !is_reset {
744 tracing::trace!("DOR wr: Un-resetting - asserting floppy interrupt");
745 self.state.sense_output = Some(SenseOutput::ResetCounter { count: 4 });
746 self.raise_interrupt(true);
748 } else if is_reset {
749 tracing::debug!("DOR wr: Software reset on fdc");
750 self.reset(true);
751 } else {
752 if !interrupts_were_enabled && interrupts_enabled {
753 tracing::trace!("Re-enabling floppy interrupts");
754 self.raise_interrupt(false);
755 } else if interrupts_were_enabled && !interrupts_enabled {
756 tracing::trace!("Disabling floppy interrupts");
757 self.lower_interrupt();
758 }
759 }
760 }
761 RegisterOffset::DATA => self.handle_data_write(data),
762 _ => return IoResult::Err(IoError::InvalidRegister),
763 }
764
765 IoResult::Ok
766 }
767}
768
769#[derive(Clone, Inspect)]
770struct FloppyState {
771 digital_output: protocol::DigitalOutputRegister,
772 main_status: protocol::MainStatusRegister,
773
774 #[inspect(bytes)]
776 input_bytes: ArrayVec<u8, { protocol::FIFO_SIZE }>,
777
778 #[inspect(bytes)]
780 output_bytes: ArrayVec<u8, { protocol::FIFO_SIZE }>,
781
782 #[inspect(skip)]
784 pending_command: FloppyCommand,
785
786 head_unload_timer: u8,
788 step_rate_time: u8,
789 head_load_timer: u8,
790 dma_disabled: bool,
791
792 sense_output: Option<SenseOutput>,
793
794 internals: FloppyStateInternals,
795
796 position: ReadWriteHeadLocation,
800 end_of_track: u8,
801
802 interrupt_level: bool,
804}
805
806#[derive(Clone, Inspect, Debug, Default, Copy)]
807struct FloppyStateInternals {
808 floppy_changed: bool,
809 floppy_present: bool,
810 media_write_protected: bool,
811 io_pending: bool,
812
813 num_bytes_rd: u32,
814 num_bytes_wr: u32,
815 sectors_per_track: u8,
816 start_sector_pos: u32,
817 sector_cache_start_logical: u32,
818 sector_cache_end_logical: u32,
819}
820
821#[derive(Inspect, Debug, Clone, Copy)]
822struct ReadWriteHeadLocation {
823 cylinder: u8,
824 head: u8,
825 sector: u8,
826}
827
828impl ReadWriteHeadLocation {
829 fn new() -> Self {
830 Self {
831 cylinder: 0,
832 head: 0,
833 sector: 0,
834 }
835 }
836
837 fn chs_to_lba(&self, sectors_per_track: u8) -> u32 {
841 (self.cylinder as u32 * 2 + self.head as u32) * sectors_per_track as u32
842 + (self.sector as u32 - 1)
843 }
844}
845
846#[derive(Clone, Inspect, Debug)]
847#[inspect(external_tag)]
848enum SenseOutput {
849 ResetCounter { count: u8 },
853 Value { value: protocol::StatusRegister0 },
855}
856
857impl FloppyState {
858 fn new(sectors_per_track: u8, read_only: bool) -> Self {
859 Self {
860 digital_output: protocol::DigitalOutputRegister::new(),
861 main_status: protocol::MainStatusRegister::new(),
862 position: ReadWriteHeadLocation::new(),
863 end_of_track: 0,
864
865 input_bytes: ArrayVec::new(),
866 output_bytes: ArrayVec::new(),
867
868 head_unload_timer: 0,
869 step_rate_time: 0,
870 head_load_timer: 0,
871 dma_disabled: false,
872 sense_output: None,
873 interrupt_level: false,
874
875 internals: FloppyStateInternals::new(sectors_per_track, read_only),
876
877 pending_command: FloppyCommand::INVALID,
878 }
879 }
880}
881
882impl FloppyStateInternals {
883 fn new(sectors_per_track: u8, read_only: bool) -> Self {
884 let floppy_present = sectors_per_track != 0;
887
888 Self {
889 floppy_changed: false,
890 floppy_present,
891 media_write_protected: read_only,
892 io_pending: false,
893
894 num_bytes_rd: 0,
895 num_bytes_wr: 0,
896 sectors_per_track,
897 start_sector_pos: 0,
898 sector_cache_start_logical: 0,
899 sector_cache_end_logical: 0,
900 }
901 }
902}
903
904#[derive(Inspect)]
905struct FloppyRt {
906 interrupt: LineInterrupt,
907 pio_base: Box<dyn ControlPortIoIntercept>,
908 pio_control: Box<dyn ControlPortIoIntercept>,
909}
910
911#[derive(InspectMut)]
913pub struct FloppyDiskController {
914 guest_memory: GuestMemory,
915
916 rt: FloppyRt,
918
919 state: FloppyState,
921
922 disk_drive: DriveRibbon,
924
925 #[inspect(skip)]
926 dma: Box<dyn IsaDmaChannel>,
927
928 #[inspect(skip)]
929 command_buffer: CommandBuffer,
930
931 #[inspect(with = "Option::is_some")]
932 io: Option<Io>,
933 #[inspect(skip)]
934 waker: Option<Waker>,
935}
936
937#[derive(Inspect)]
939#[inspect(external_tag)]
940pub enum DriveRibbon {
941 None,
943 Single(#[inspect(rename = "media")] Disk),
945 }
948
949#[derive(Debug, Error)]
951#[error("too many drives")]
952pub struct TooManyDrives;
953
954impl DriveRibbon {
955 pub fn from_vec(drives: Vec<Disk>) -> Result<Self, TooManyDrives> {
957 match drives.len() {
958 0 => Ok(Self::None),
959 1 => Ok(Self::Single(drives.into_iter().next().unwrap())),
960 _ => Err(TooManyDrives),
961 }
962 }
963}
964
965#[derive(Debug, Error)]
967pub enum NewFloppyDiskControllerError {
968 #[error("disk is non-standard size: {0} bytes")]
970 NonStandardDisk(u64),
971}
972
973impl FloppyDiskController {
974 pub fn new(
976 guest_memory: GuestMemory,
977 interrupt: LineInterrupt,
978 register_pio: &mut dyn RegisterPortIoIntercept,
979 pio_base_addr: u16,
980 disk_drive: DriveRibbon,
981 dma: Box<dyn IsaDmaChannel>,
982 ) -> Result<Self, NewFloppyDiskControllerError> {
983 let mut pio_base = register_pio.new_io_region("base", 6);
984 let mut pio_control = register_pio.new_io_region("control", 1);
985
986 pio_base.map(pio_base_addr);
987 pio_control.map(pio_base_addr + RegisterOffset::DIGITAL_INPUT.0);
990
991 Ok(Self {
992 guest_memory,
993 rt: FloppyRt {
994 interrupt,
995 pio_base,
996 pio_control,
997 },
998 state: FloppyState::new(
999 {
1000 match &disk_drive {
1001 DriveRibbon::None => {
1002 0
1005 }
1006 DriveRibbon::Single(disk) => {
1007 let file_size = disk.sector_count() * disk.sector_size() as u64;
1008
1009 let image_type = FloppyImageType::from_file_size(file_size)
1010 .ok_or(NewFloppyDiskControllerError::NonStandardDisk(file_size))?;
1011 image_type.sectors()
1012 }
1013 }
1014 },
1015 match &disk_drive {
1016 DriveRibbon::Single(disk) => disk.is_read_only(),
1017 DriveRibbon::None => false,
1018 },
1019 ),
1020 disk_drive,
1021 dma,
1022 command_buffer: CommandBuffer::new(),
1023 io: None,
1024 waker: None,
1025 })
1026 }
1027
1028 fn set_io<F, Fut>(&mut self, f: F)
1030 where
1031 F: FnOnce(Disk) -> Fut,
1032 Fut: 'static + Future<Output = Result<(), disk_backend::DiskError>> + Send,
1033 {
1034 let DriveRibbon::Single(disk) = &self.disk_drive else {
1035 panic!();
1036 };
1037
1038 let fut = (f)(disk.clone());
1039 assert!(self.io.is_none());
1040 self.io = Some(Io(Box::pin(fut)));
1041 if let Some(waker) = self.waker.take() {
1043 waker.wake();
1044 }
1045 }
1046
1047 fn handle_io_completion(&mut self, result: Result<(), disk_backend::DiskError>) {
1048 let command = self.state.pending_command;
1049 tracing::trace!(?command, ?result, "io completion");
1050
1051 let result = match command {
1052 FloppyCommand::READ_NORMAL_DATA
1053 | FloppyCommand::READ_NORMAL_DEL_DATA
1054 | FloppyCommand::READ_DEL_DATA
1055 | FloppyCommand::SEEK_AND_READ
1056 | FloppyCommand::ALT_SEEK_AND_READ
1057 | FloppyCommand::READ_TRACK => match result {
1058 Ok(()) => self.read_complete(),
1059 Err(err) => Err(err),
1060 },
1061 FloppyCommand::WRITE_NORMAL_DATA
1062 | FloppyCommand::WRITE_DATA
1063 | FloppyCommand::WRITE_DEL_DATA
1064 | FloppyCommand::SEEK_AND_WRITE => match result {
1065 Ok(()) => self.write_complete(),
1066 Err(err) => Err(err),
1067 },
1068 FloppyCommand::FORMAT_TRACK | FloppyCommand::FORMAT_DOUBLE_DENSITY_MODE => match result
1069 {
1070 Ok(()) => self.write_zeros_complete(),
1071 Err(err) => Err(err),
1072 },
1073 _ => {
1074 tracelimit::error_ratelimited!(?command, "unexpected command!");
1075 return;
1076 }
1077 };
1078
1079 if let Err(err) = result {
1080 let wo_error = matches!(err, disk_backend::DiskError::ReadOnly);
1081 self.set_output_status(true, wo_error, true);
1082 }
1083
1084 self.state.pending_command = FloppyCommand::INVALID;
1085 self.complete_command(true);
1086 }
1087
1088 fn read_complete(&mut self) -> Result<(), disk_backend::DiskError> {
1091 let buffer = match self.dma.request(IsaDmaDirection::Write) {
1094 Some(r) => r,
1095 None => {
1096 tracelimit::error_ratelimited!("request_dma for read failed");
1097 return Err(disk_backend::DiskError::Io(std::io::Error::new(
1098 std::io::ErrorKind::Other,
1099 "request_dma for read failed",
1100 )));
1101 }
1102 };
1103
1104 let size = (buffer.size.div_ceil(STANDARD_FLOPPY_SECTOR_SIZE) * STANDARD_FLOPPY_SECTOR_SIZE)
1105 as u32;
1106
1107 let buffer_ptr = &self.command_buffer.buffer[0..size as usize][..size as usize];
1108
1109 let res = self
1110 .guest_memory
1111 .write_from_atomic(buffer.address, buffer_ptr);
1112
1113 self.dma.complete();
1114
1115 if let Err(err) = res {
1116 tracelimit::error_ratelimited!(
1117 error = &err as &dyn std::error::Error,
1118 "dma transfer failed"
1119 );
1120 return Err(disk_backend::DiskError::MemoryAccess(err.into()));
1121 }
1122
1123 self.set_output_status(false, false, false);
1124
1125 Ok(())
1126 }
1127
1128 fn write_complete(&mut self) -> Result<(), disk_backend::DiskError> {
1129 self.set_output_status(false, false, false);
1130 Ok(())
1131 }
1132
1133 fn write_zeros_complete(&mut self) -> Result<(), disk_backend::DiskError> {
1134 self.set_output_status(false, false, true);
1135 Ok(())
1136 }
1137
1138 pub fn offset_of(&self, addr: u16) -> Option<u16> {
1143 self.rt.pio_base.offset_of(addr).or_else(|| {
1144 self.rt
1145 .pio_control
1146 .offset_of(addr)
1147 .map(|_| RegisterOffset::DIGITAL_INPUT.0)
1148 })
1149 }
1150
1151 fn raise_interrupt(&mut self, is_reset: bool) {
1152 if self.state.digital_output.dma_enabled() || is_reset {
1153 self.rt.interrupt.set_level(true);
1154 self.state.interrupt_level = true;
1155 }
1156 }
1157
1158 fn lower_interrupt(&mut self) {
1159 self.rt.interrupt.set_level(false);
1160 self.state.interrupt_level = false;
1161 }
1162
1163 fn reset(&mut self, preserve_digital_output: bool) {
1167 self.lower_interrupt();
1168 self.state = FloppyState {
1169 digital_output: if preserve_digital_output {
1170 self.state.digital_output
1171 } else {
1172 protocol::DigitalOutputRegister::new()
1173 },
1174 ..FloppyState::new(
1175 self.state.internals.sectors_per_track,
1176 self.state.internals.media_write_protected,
1177 )
1178 };
1179
1180 self.state.main_status = protocol::MainStatusRegister::new().with_main_request(true);
1183
1184 tracing::trace!(
1185 preserve_digital_output,
1186 "controller reset - deasserting floppy interrupt"
1187 );
1188 }
1189
1190 fn parse_input_for_readwrite(&mut self) {
1191 let input = protocol::InputRegister::from(self.state.input_bytes[1]);
1192 if input.drive_select() != 0 {
1193 tracelimit::warn_ratelimited!(
1194 "Drive selected as outside of what is supported in data read"
1195 );
1196 }
1197
1198 let head = input.head();
1199
1200 self.state.position.head = head;
1201 self.state.position.cylinder = self.state.input_bytes[2];
1202 if self.state.position.cylinder > FLOPPY_TOTAL_CYLINDERS {
1203 tracelimit::warn_ratelimited!(?self.state.position.cylinder, "Floppy seek to cylinder > 80");
1204 }
1205 self.state.position.sector = self.state.input_bytes[4];
1206 self.state.end_of_track = self.state.input_bytes[6];
1207 if self.state.input_bytes[5] != 2 || self.state.input_bytes[8] != 0xFF {
1208 tracelimit::warn_ratelimited!(?self.state.input_bytes, "non-standard floppy read command parameters for PC floppy format");
1209 }
1210 }
1211
1212 fn get_sense_output(&mut self) -> &mut protocol::StatusRegister0 {
1213 match self.state.sense_output {
1214 Some(SenseOutput::Value { ref mut value }) => value,
1215 _ => {
1216 self.state.sense_output = Some(SenseOutput::Value {
1217 value: protocol::StatusRegister0::new(),
1218 });
1219
1220 match self.state.sense_output {
1221 Some(SenseOutput::Value { ref mut value }) => value,
1222 _ => panic!(),
1223 }
1224 }
1225 }
1226 }
1227
1228 fn handle_sense_interrupt_status(&mut self) {
1229 match self.state.sense_output {
1230 Some(SenseOutput::ResetCounter { ref mut count }) => {
1231 if *count > 0 {
1236 let out = protocol::StatusRegister0::from(4 - *count)
1237 .with_invalid_command(true)
1238 .with_abnormal_termination(true);
1239 self.state.sense_output = if (*count - 1) == 0 {
1240 None
1241 } else {
1242 Some(SenseOutput::ResetCounter { count: *count - 1 })
1243 };
1244 self.state.output_bytes.push(self.state.position.cylinder);
1245 self.state.output_bytes.push(out.into());
1246 } else {
1247 tracelimit::error_ratelimited!(
1248 "SENSE_INTERRUPT_STATUS called with ResetCount stage 0. p lease fix me"
1249 );
1250 self.state.output_bytes.push(INVALID_COMMAND_STATUS);
1251 self.state.output_bytes.push(INVALID_COMMAND_STATUS);
1252 }
1253 }
1254 Some(SenseOutput::Value { value }) => {
1255 self.state.output_bytes.push(self.state.position.cylinder);
1256 self.state.output_bytes.push(value.into());
1257
1258 self.state.sense_output = None;
1259 }
1260 _ => {
1261 self.state.output_bytes.push(INVALID_COMMAND_STATUS);
1262 self.state.output_bytes.push(INVALID_COMMAND_STATUS);
1263 }
1264 }
1265
1266 tracing::trace!("sense interrupt status cmd - deasserting floppy interrupt");
1267
1268 self.lower_interrupt();
1269
1270 self.state.main_status = (self.state.main_status)
1271 .with_data_direction(protocol::DataDirection::Write.as_bool())
1272 .with_non_dma_mode(false)
1273 .with_busy(false)
1274 .with_main_request(true);
1275 }
1276
1277 fn handle_sense_drive_status(&mut self) {
1278 let input = protocol::InputRegister::from(self.state.input_bytes[1]);
1279 let drive: u8 = input.drive_select();
1280 if drive != 0 {
1281 tracelimit::warn_ratelimited!(
1282 ?drive,
1283 "Floppy drive number out of range from DSDT enforcement"
1284 );
1285 }
1286
1287 let head: u8 = input.head();
1288 self.state.position.head = head;
1289
1290 let output = protocol::StatusRegister3::new()
1291 .with_drive_select(drive)
1292 .with_head(head)
1293 .with_unused1(true)
1294 .with_track0(self.state.position.cylinder == 0)
1295 .with_unused2(true)
1296 .with_write_protected(self.state.internals.media_write_protected);
1297
1298 self.state.output_bytes.push(output.into());
1299 self.get_sense_output().set_seek_end(true);
1300 }
1301
1302 fn set_output_status(&mut self, rw_error: bool, wo_error: bool, end_seek: bool) {
1303 if !self.state.output_bytes.is_empty() {
1304 tracelimit::warn_ratelimited!("output_setup_long called with non-empty output_bytes");
1305 }
1306 self.state.output_bytes.push(0x2); self.state.output_bytes.push(1);
1308
1309 self.state.output_bytes.push(self.state.position.head);
1310 self.state.output_bytes.push(self.state.position.cylinder);
1311 self.state.output_bytes.push(
1312 protocol::StatusRegister2::new()
1313 .with_missing_address(rw_error)
1314 .with_bad_cylinder(rw_error)
1315 .into(),
1316 );
1317
1318 self.state.output_bytes.push(
1319 protocol::StatusRegister1::new()
1320 .with_no_data(rw_error)
1321 .with_missing_address(rw_error)
1322 .with_write_protected(wo_error)
1323 .into(),
1324 );
1325
1326 self.state.output_bytes.push({
1327 let drive = 0; let out = protocol::StatusRegister0::new()
1329 .with_drive_select(drive)
1330 .with_head(self.state.position.head)
1331 .with_abnormal_termination(rw_error || wo_error)
1332 .with_seek_end(end_seek);
1333
1334 out.into()
1335 });
1336 }
1337
1338 fn complete_command(&mut self, request_interrupt: bool) {
1339 let has_output = !self.state.output_bytes.is_empty();
1340 self.state.main_status.set_busy(has_output);
1341 self.state.main_status.set_non_dma_mode(false);
1342
1343 let dma_type = if has_output {
1344 protocol::DataDirection::Read
1345 } else {
1346 protocol::DataDirection::Write
1347 };
1348 self.state
1349 .main_status
1350 .set_data_direction(dma_type.as_bool());
1351 self.state.main_status.set_main_request(true);
1352
1353 if request_interrupt {
1354 self.raise_interrupt(false);
1355 }
1356 }
1357
1358 fn handle_data_write(&mut self, data: u8) {
1363 if !self.state.digital_output.controller_enabled() {
1365 return;
1367 }
1368
1369 self.state.input_bytes.push(data);
1370 let command = FloppyCommand(self.state.input_bytes[0]);
1371
1372 tracing::trace!(
1376 ?data,
1377 ?self.state.input_bytes,
1378 "floppy byte (cmd or param)"
1379 );
1380
1381 self.handle_command(command);
1382 }
1383
1384 fn handle_command(&mut self, command: FloppyCommand) {
1385 if !self.state.output_bytes.is_empty() {
1386 tracelimit::warn_ratelimited!(output_bytes = ?self.state.output_bytes, "Floppy data register write with bytes still pending");
1387 }
1388 self.state.output_bytes.clear();
1389
1390 if self.state.input_bytes.len() < command.input_bytes_needed() {
1391 tracing::debug!(
1392 ?command,
1393 bytes_needed = ?command.input_bytes_needed(),
1394 bytes_received = ?self.state.input_bytes.len(),
1395 "floppy command missing (or waiting for) parameters"
1396 );
1397
1398 self.state.main_status.set_busy(true);
1400 return;
1401 }
1402
1403 tracing::trace!(
1404 ?command,
1405 input_bytes = ?self.state.input_bytes,
1406 "executing floppy command"
1407 );
1408
1409 if self.state.interrupt_level && command != FloppyCommand::SENSE_INTERRUPT_STATUS {
1415 tracing::trace!(
1416 ?command,
1417 "Floppy interrupt level was high before command execution. Now de-asserting interrupt"
1418 );
1419 self.lower_interrupt();
1420 self.state.main_status.set_active_drives(0);
1421 }
1422
1423 let mut complete_command = true;
1424 let mut request_interrupt = false;
1425
1426 match command {
1427 FloppyCommand::READ_NORMAL_DATA
1428 | FloppyCommand::READ_NORMAL_DEL_DATA
1429 | FloppyCommand::READ_DEL_DATA
1430 | FloppyCommand::SEEK_AND_READ
1431 | FloppyCommand::ALT_SEEK_AND_READ => {
1432 let success = self.handle_read();
1433 request_interrupt = !success;
1434 complete_command = !success;
1435 }
1436 FloppyCommand::WRITE_NORMAL_DATA
1437 | FloppyCommand::WRITE_DATA
1438 | FloppyCommand::WRITE_DEL_DATA
1439 | FloppyCommand::SEEK_AND_WRITE => {
1440 let success = self.handle_write();
1441 request_interrupt = !success;
1442 complete_command = !success;
1443 }
1444 FloppyCommand::READ_TRACK => {
1445 self.state.input_bytes[2] = 0;
1447 let success = self.handle_read();
1448 request_interrupt = !success;
1449 complete_command = !success;
1450 }
1451 FloppyCommand::VERSION => {
1452 self.state.output_bytes.push(0x90);
1454 }
1455 FloppyCommand::FORMAT_TRACK | FloppyCommand::FORMAT_DOUBLE_DENSITY_MODE => {
1456 let success = self.format();
1457 request_interrupt = !success;
1458 complete_command = !success;
1459 }
1460 FloppyCommand::SEEK => {
1461 self.handle_seek();
1462 request_interrupt = true;
1463 }
1464 FloppyCommand::RECALIBRATE => {
1465 self.handle_recalibrate();
1466 request_interrupt = true;
1467 }
1468 FloppyCommand::SENSE_INTERRUPT_STATUS => {
1469 self.handle_sense_interrupt_status();
1470 }
1471 FloppyCommand::SPECIFY => self.handle_specify(),
1472 FloppyCommand::SENSE_DRIVE_STATUS => self.handle_sense_drive_status(),
1473 FloppyCommand::DUMP_REGISTERS => self.handle_dump_registers(),
1474 FloppyCommand::READ_ID => {
1475 self.read_id();
1476 request_interrupt = true;
1477 }
1478
1479 FloppyCommand::UNLOCK_FIFO_FUNCTIONS => {
1482 self.state.output_bytes.push(0);
1483 }
1484 FloppyCommand::LOCK_FIFO_FUNCTIONS => {
1485 self.state.output_bytes.push(0x10);
1486 }
1487 FloppyCommand::PART_ID => {
1488 self.state.output_bytes.push(0x01);
1489 }
1490 FloppyCommand::CONFIGURE | FloppyCommand::PERP288_MODE => {
1491 tracing::debug!(?command, "command ignored");
1493 }
1494 _ => {
1495 tracelimit::error_ratelimited!(?command, "unimplemented/unsupported command");
1496 self.state.output_bytes.push(INVALID_COMMAND_STATUS);
1497 }
1498 }
1499
1500 self.state.input_bytes.clear();
1502
1503 if !self.state.output_bytes.is_empty() {
1504 if self.state.output_bytes.len() != command.result_bytes_expected() {
1505 tracelimit::warn_ratelimited!(?command, output_bytes = ?self.state.output_bytes, "command output size doesn't match expected");
1506 } else {
1507 tracing::trace!(
1508 ?command,
1509 output_bytes = ?self.state.output_bytes,
1510 "floppy command output"
1511 );
1512 }
1513 }
1514
1515 self.state.pending_command = if complete_command {
1516 self.complete_command(request_interrupt);
1517 FloppyCommand::INVALID
1518 } else {
1519 command
1520 };
1521
1522 tracing::trace!(
1523 main_status = ?self.state.main_status,
1524 digital_output = ?self.state.digital_output,
1525 sense_output = ?self.state.sense_output,
1526 dma_disabled = ?self.state.dma_disabled,
1527 cylinder = ?self.state.position.cylinder,
1528 head = ?self.state.position.head,
1529 sector = ?self.state.position.sector,
1530 interrupt_level = ?self.state.interrupt_level,
1531 "floppy state"
1532 );
1533
1534 tracing::trace!("floppy command completed");
1535 }
1536
1537 fn handle_read(&mut self) -> bool {
1538 self.state.internals.floppy_changed = false;
1540
1541 self.get_sense_output().set_seek_end(true);
1543
1544 self.state.main_status.set_main_request(false);
1546
1547 if self.state.dma_disabled {
1552 tracelimit::warn_ratelimited!("non-dma mode is not supported");
1553 self.state.main_status.set_non_dma_mode(true);
1554 }
1555
1556 let input = protocol::InputRegister::from(self.state.input_bytes[1]);
1558 let busy_drive = input.drive_select();
1559 self.state
1560 .main_status
1561 .set_active_drives(self.state.main_status.active_drives() | (1 << busy_drive));
1562
1563 self.state.main_status.set_busy(true);
1564
1565 self.parse_input_for_readwrite();
1566
1567 let error = !self.read_data();
1568 if error {
1569 self.state.internals.io_pending = false;
1570 self.set_output_status(error, false, error);
1571 }
1572 !error
1573 }
1574
1575 fn read_data(&mut self) -> bool {
1576 if !self.state.internals.floppy_present {
1577 tracelimit::error_ratelimited!("read attempted, but floppy not present");
1578 return false;
1579 }
1580
1581 if self.state.position.sector == 0
1582 || self.state.position.sector > self.state.end_of_track
1583 || self.state.position.sector > self.state.internals.sectors_per_track
1584 {
1585 tracelimit::error_ratelimited!(
1586 position = ?self.state.position,
1587 end_of_track = self.state.end_of_track,
1588 sectors_per_track = self.state.internals.sectors_per_track,
1589 "invalid read position"
1590 );
1591 return false;
1592 }
1593
1594 if self.state.position.cylinder > FLOPPY_TOTAL_CYLINDERS {
1595 tracelimit::error_ratelimited!(sector = ?self.state.position.sector, "bad sector in floppy read");
1596 return false;
1597 }
1598
1599 self.state.internals.io_pending = true;
1600 let size_hint = self.dma.check_transfer_size() as usize;
1601
1602 let lba = (self.state.position).chs_to_lba(self.state.internals.sectors_per_track) as u64;
1604
1605 let size = {
1606 let num = (size_hint.div_ceil(STANDARD_FLOPPY_SECTOR_SIZE)
1607 * STANDARD_FLOPPY_SECTOR_SIZE) as u32;
1608 if num < STANDARD_FLOPPY_SECTOR_SIZE as u32 {
1609 STANDARD_FLOPPY_SECTOR_SIZE as u32
1610 } else {
1611 num
1612 }
1613 };
1614
1615 let command_buffer = self.command_buffer.access();
1616
1617 tracing::trace!(lba, size, "starting disk read");
1618 self.set_io(async move |disk| {
1619 let buffers = command_buffer.buffers(0, size as usize, true);
1620 disk.read_vectored(&buffers, lba).await
1621 });
1622
1623 true
1624 }
1625
1626 fn handle_write(&mut self) -> bool {
1627 self.state.internals.floppy_changed = false;
1628
1629 self.get_sense_output().set_seek_end(true);
1631
1632 self.state.main_status.set_main_request(false);
1634
1635 if self.state.dma_disabled {
1640 tracelimit::warn_ratelimited!("non-dma mode is not supported");
1641 self.state.main_status.set_non_dma_mode(true);
1642 }
1643
1644 let input = protocol::InputRegister::from(self.state.input_bytes[1]);
1646 let busy_drive = input.drive_select();
1647 self.state
1648 .main_status
1649 .set_active_drives(self.state.main_status.active_drives() | (1 << busy_drive));
1650
1651 self.state.main_status.set_busy(true);
1652
1653 self.parse_input_for_readwrite();
1654
1655 let wo_error = self.state.internals.media_write_protected;
1656 let error = if wo_error { true } else { !self.write_data() };
1657
1658 if error {
1659 self.set_output_status(error, wo_error, error);
1660 }
1661 !error
1662 }
1663
1664 fn write_data(&mut self) -> bool {
1665 if !self.state.internals.floppy_present {
1666 tracelimit::error_ratelimited!("write attempted, but floppy not present");
1667 return false;
1668 }
1669
1670 if self.state.position.sector == 0
1671 || self.state.position.sector > self.state.end_of_track
1672 || self.state.position.sector > self.state.internals.sectors_per_track
1673 {
1674 tracelimit::error_ratelimited!(
1675 position = ?self.state.position,
1676 end_of_track = self.state.end_of_track,
1677 sectors_per_track = self.state.internals.sectors_per_track,
1678 "invalid write position"
1679 );
1680 return false;
1681 }
1682
1683 let lba = (self.state.position).chs_to_lba(self.state.internals.sectors_per_track) as u64;
1684 let buffer = match self.dma.request(IsaDmaDirection::Read) {
1685 Some(r) => r,
1686 None => {
1687 tracelimit::error_ratelimited!("request_dma for write failed");
1688 return false;
1689 }
1690 };
1691
1692 let size = buffer.size.div_ceil(STANDARD_FLOPPY_SECTOR_SIZE) * STANDARD_FLOPPY_SECTOR_SIZE;
1693
1694 let command_buffer = self.command_buffer.access();
1695
1696 let buffer_ptr = &self.command_buffer.buffer[0..size as usize][..size as usize];
1697 let r = self.guest_memory.read_to_atomic(buffer.address, buffer_ptr);
1698
1699 self.dma.complete();
1700
1701 if let Err(err) = r {
1702 tracelimit::error_ratelimited!(
1703 error = &err as &dyn std::error::Error,
1704 "dma transfer failed"
1705 );
1706
1707 return false;
1708 }
1709
1710 let DriveRibbon::Single(disk) = &self.disk_drive else {
1711 tracelimit::error_ratelimited!("No disk");
1712 return false;
1713 };
1714
1715 if disk.is_read_only() {
1716 tracelimit::error_ratelimited!("Read only");
1717 return false;
1718 }
1719
1720 self.set_io(async move |disk| {
1721 let buffers = command_buffer.buffers(0, size as usize, false);
1722 let result = disk.write_vectored(&buffers, lba, false).await;
1723 if let Err(err) = result {
1724 tracelimit::error_ratelimited!(
1725 error = &err as &dyn std::error::Error,
1726 "write failed"
1727 );
1728 return Err(err);
1729 }
1730 let result = disk.sync_cache().await;
1731 if let Err(err) = result {
1732 tracelimit::error_ratelimited!(
1733 error = &err as &dyn std::error::Error,
1734 "flush failed"
1735 );
1736 return Err(err);
1737 }
1738
1739 result
1740 });
1741
1742 true
1743 }
1744
1745 fn write_zeros(&mut self) -> bool {
1746 let DriveRibbon::Single(disk) = &self.disk_drive else {
1747 tracelimit::error_ratelimited!("No disk");
1748 return false;
1749 };
1750
1751 if disk.is_read_only() {
1752 tracelimit::error_ratelimited!("Read only");
1753 return false;
1754 }
1755
1756 let buffer = match self.dma.request(IsaDmaDirection::Read) {
1757 Some(r) => r,
1758 None => {
1759 tracelimit::error_ratelimited!("request_dma for format failed");
1760 return false;
1761 }
1762 };
1763
1764 let size = (buffer.size.div_ceil(STANDARD_FLOPPY_SECTOR_SIZE) * STANDARD_FLOPPY_SECTOR_SIZE)
1765 as u32;
1766
1767 let command_buffer = self.command_buffer.access();
1768
1769 let buffer_ptr = &self.command_buffer.buffer[0..size as usize][..size as usize];
1770 let r = self.guest_memory.read_to_atomic(buffer.address, buffer_ptr);
1771
1772 self.dma.complete();
1773
1774 if let Err(err) = r {
1775 tracelimit::error_ratelimited!(
1776 error = &err as &dyn std::error::Error,
1777 "dma transfer failed"
1778 );
1779
1780 return false;
1781 }
1782
1783 let Some(cylinder) = buffer_ptr.first() else {
1784 tracelimit::error_ratelimited!("failed to get(0)");
1785 return false;
1786 };
1787
1788 let cylinder = cylinder.load(Ordering::Relaxed) as u64;
1789
1790 let Some(head) = buffer_ptr.get(1) else {
1791 tracelimit::error_ratelimited!("failed to get(1)");
1792 return false;
1793 };
1794
1795 let head = head.load(Ordering::Relaxed) as u64;
1796
1797 let size = STANDARD_FLOPPY_SECTOR_SIZE * self.state.internals.sectors_per_track as usize;
1798 let buffers = command_buffer.buffers(0, size, false);
1799
1800 let res = buffers.guest_memory().zero_range(&buffers.range());
1801 if let Err(err) = res {
1802 tracelimit::error_ratelimited!(
1803 error = &err as &dyn std::error::Error,
1804 "zero_range failed"
1805 );
1806 return false;
1807 }
1808
1809 let lba = (cylinder * 2 + head) * self.state.internals.sectors_per_track as u64;
1810
1811 tracing::trace!(?cylinder, ?head, ?lba, ?buffer_ptr, "Format: ");
1812
1813 self.set_io(async move |disk| {
1814 let buffers = command_buffer.buffers(0, size, false);
1815 let result = disk.write_vectored(&buffers, lba, false).await;
1816 if let Err(err) = result {
1817 tracelimit::error_ratelimited!(
1818 error = &err as &dyn std::error::Error,
1819 "write failed"
1820 );
1821 return Err(err);
1822 }
1823 let result = disk.sync_cache().await;
1824 if let Err(err) = result {
1825 tracelimit::error_ratelimited!(
1826 error = &err as &dyn std::error::Error,
1827 "flush failed"
1828 );
1829 return Err(err);
1830 }
1831 result
1832 });
1833
1834 true
1835 }
1836
1837 fn format(&mut self) -> bool {
1838 let wo_err_occurred = self.state.internals.media_write_protected;
1839 let error = if wo_err_occurred {
1840 true
1841 } else {
1842 self.state.main_status.set_busy(true);
1843 !self.write_zeros()
1844 };
1845
1846 if error {
1847 self.set_output_status(error, wo_err_occurred, true);
1848 }
1849 !error
1850 }
1851
1852 fn handle_seek(&mut self) {
1853 self.state.internals.floppy_changed = false;
1854
1855 self.state.position.sector = 0;
1856
1857 let input = protocol::InputRegister::from(self.state.input_bytes[1]);
1858 self.state.position.head = input.head();
1859
1860 self.state.position.cylinder = if self.state.input_bytes[2] >= FLOPPY_TOTAL_CYLINDERS {
1861 tracelimit::warn_ratelimited!(?self.state.position.cylinder, "Floppy seek to cylinder > 80");
1862 0
1863 } else {
1864 self.state.input_bytes[2] };
1866
1867 self.recalibrate();
1868 }
1869
1870 fn handle_recalibrate(&mut self) {
1871 self.state.position.cylinder = 0;
1872 self.recalibrate();
1873 }
1874
1875 fn recalibrate(&mut self) {
1876 if let Some(SenseOutput::ResetCounter { .. }) = self.state.sense_output {
1877 self.state.sense_output = None;
1878 }
1879
1880 let head = self.state.position.head;
1885 self.get_sense_output().set_seek_end(true);
1886 self.get_sense_output().set_head(head);
1887
1888 let input = protocol::InputRegister::from(self.state.input_bytes[1]);
1890 let busy_drive = input.drive_select();
1891 self.state
1892 .main_status
1893 .set_active_drives(self.state.main_status.active_drives() | (1 << busy_drive));
1894 if busy_drive > 0 {
1895 tracelimit::warn_ratelimited!(
1896 ?busy_drive,
1897 "Floppy seek to drive outside of what is supported"
1898 );
1899 }
1900 }
1901
1902 fn handle_specify(&mut self) {
1903 let param1 = protocol::SpecifyParam1::from(self.state.input_bytes[1]);
1904 let param2 = protocol::SpecifyParam2::from(self.state.input_bytes[2]);
1905
1906 self.state.head_unload_timer = param1.head_unload_timer();
1907 self.state.step_rate_time = param1.step_rate_time();
1908 self.state.head_load_timer = param2.head_load_timer();
1909 self.state.dma_disabled = param2.dma_disabled();
1910 }
1911
1912 fn handle_dump_registers(&mut self) {
1913 self.state.output_bytes.push(self.state.position.cylinder);
1914 self.state.output_bytes.push(0); self.state.output_bytes.push(0); self.state.output_bytes.push(0); self.state.output_bytes.push(
1919 protocol::SpecifyParam1::new()
1920 .with_head_unload_timer(self.state.head_unload_timer)
1921 .with_step_rate_time(self.state.step_rate_time)
1922 .into(),
1923 );
1924 self.state.output_bytes.push(
1925 protocol::SpecifyParam2::new()
1926 .with_head_load_timer(self.state.head_load_timer)
1927 .with_dma_disabled(self.state.dma_disabled)
1928 .into(),
1929 );
1930
1931 self.state.output_bytes.push(0); self.state.output_bytes.push(0); self.state.output_bytes.push(0); self.state.output_bytes.push(0); }
1939
1940 fn read_id(&mut self) {
1941 let input = protocol::InputRegister::from(self.state.input_bytes[1]);
1943 self.state.position.head = input.head();
1944
1945 self.set_output_status(false, false, false);
1947 let head = self.state.position.head;
1948 self.get_sense_output().set_head(head);
1949 }
1950}
1951
1952mod save_restore {
1953 use super::*;
1954 use vmcore::save_restore::RestoreError;
1955 use vmcore::save_restore::SaveError;
1956 use vmcore::save_restore::SaveRestore;
1957
1958 mod state {
1959 use mesh::payload::Protobuf;
1960 use vmcore::save_restore::SavedStateRoot;
1961
1962 #[derive(Protobuf, SavedStateRoot)]
1963 #[mesh(package = "chipset.floppy")]
1964 pub struct SavedState {
1965 #[mesh(1)]
1966 pub digital_output: u8,
1967 #[mesh(2)]
1968 pub main_status: u8,
1969 #[mesh(3)]
1970 pub input_bytes: Vec<u8>,
1971 #[mesh(4)]
1972 pub output_bytes: Vec<u8>,
1973 #[mesh(5)]
1974 pub head_unload_timer: u8,
1975 #[mesh(6)]
1976 pub step_rate_time: u8,
1977 #[mesh(7)]
1978 pub head_load_timer: u8,
1979 #[mesh(8)]
1980 pub dma_disabled: bool,
1981 #[mesh(9)]
1982 pub interrupt_output: Option<SavedInterruptOutput>,
1983 #[mesh(10)]
1984 pub interrupt_level: bool,
1985
1986 #[mesh(11)]
1987 pub end_of_track: u8,
1988 #[mesh(12)]
1991 pub drive: u8,
1992 #[mesh(13)]
1994 pub floppies: [SavedFloppyState; 1],
1995 #[mesh(14)]
1996 pub pending_command: u8,
1997 }
1998
1999 #[derive(Protobuf)]
2000 #[mesh(package = "chipset.floppy")]
2001 pub struct SavedFloppyState {
2002 #[mesh(1)]
2003 pub cylinder: u8,
2004 #[mesh(2)]
2005 pub head: u8,
2006 #[mesh(3)]
2007 pub sector: u8,
2008 #[mesh(4)]
2009 pub internals: SavedFloppyStateInternals,
2010 }
2011
2012 #[derive(Protobuf)]
2013 #[mesh(package = "chipset.floppy")]
2014 pub enum SavedInterruptOutput {
2015 #[mesh(1)]
2016 ResetCounter {
2017 #[mesh(1)]
2018 count: u8,
2019 },
2020 #[mesh(2)]
2021 Value {
2022 #[mesh(1)]
2023 value: u8,
2024 },
2025 }
2026
2027 impl From<SavedInterruptOutput> for super::SenseOutput {
2028 fn from(value: SavedInterruptOutput) -> Self {
2029 match value {
2030 SavedInterruptOutput::ResetCounter { count } => {
2031 super::SenseOutput::ResetCounter { count }
2032 }
2033 SavedInterruptOutput::Value { value } => super::SenseOutput::Value {
2034 value: super::protocol::StatusRegister0::from(value),
2035 },
2036 }
2037 }
2038 }
2039
2040 impl From<super::SenseOutput> for SavedInterruptOutput {
2041 fn from(value: super::SenseOutput) -> Self {
2042 match value {
2043 super::SenseOutput::ResetCounter { count } => {
2044 SavedInterruptOutput::ResetCounter { count }
2045 }
2046 super::SenseOutput::Value { value } => SavedInterruptOutput::Value {
2047 value: u8::from(value),
2048 },
2049 }
2050 }
2051 }
2052
2053 #[derive(Protobuf, Clone, Copy)]
2054 #[mesh(package = "chipset.floppy")]
2055 pub struct SavedFloppyStateInternals {
2056 #[mesh(1)]
2057 floppy_changed: bool,
2058 #[mesh(2)]
2059 floppy_present: bool,
2060 #[mesh(3)]
2061 media_write_protected: bool,
2062 #[mesh(4)]
2063 io_pending: bool,
2064
2065 #[mesh(5)]
2066 num_bytes_rd: u32,
2067 #[mesh(6)]
2068 num_bytes_wr: u32,
2069 #[mesh(7)]
2070 sectors_per_track: u8,
2071 #[mesh(8)]
2072 start_sector_pos: u32,
2073 #[mesh(9)]
2074 sector_cache_start_logical: u32,
2075 #[mesh(10)]
2076 sector_cache_end_logical: u32,
2077 }
2078
2079 impl From<super::FloppyStateInternals> for SavedFloppyStateInternals {
2080 fn from(value: super::FloppyStateInternals) -> Self {
2081 let super::FloppyStateInternals {
2082 floppy_changed,
2083 floppy_present,
2084 media_write_protected,
2085 io_pending,
2086 num_bytes_rd,
2087 num_bytes_wr,
2088 sectors_per_track,
2089 start_sector_pos,
2090 sector_cache_start_logical,
2091 sector_cache_end_logical,
2092 } = value;
2093
2094 Self {
2095 floppy_changed,
2096 floppy_present,
2097 media_write_protected,
2098 io_pending,
2099 num_bytes_rd,
2100 num_bytes_wr,
2101 sectors_per_track,
2102 start_sector_pos,
2103 sector_cache_start_logical,
2104 sector_cache_end_logical,
2105 }
2106 }
2107 }
2108
2109 impl From<SavedFloppyStateInternals> for super::FloppyStateInternals {
2110 fn from(value: SavedFloppyStateInternals) -> Self {
2111 let SavedFloppyStateInternals {
2112 floppy_changed,
2113 floppy_present,
2114 media_write_protected,
2115 io_pending,
2116 num_bytes_rd,
2117 num_bytes_wr,
2118 sectors_per_track,
2119 start_sector_pos,
2120 sector_cache_start_logical,
2121 sector_cache_end_logical,
2122 } = value;
2123
2124 Self {
2125 floppy_changed,
2126 floppy_present,
2127 media_write_protected,
2128 io_pending,
2129 num_bytes_rd,
2130 num_bytes_wr,
2131 sectors_per_track,
2132 start_sector_pos,
2133 sector_cache_start_logical,
2134 sector_cache_end_logical,
2135 }
2136 }
2137 }
2138 }
2139
2140 impl SaveRestore for FloppyDiskController {
2141 type SavedState = state::SavedState;
2142
2143 fn save(&mut self) -> Result<Self::SavedState, SaveError> {
2144 let FloppyState {
2145 digital_output,
2146 main_status,
2147 ref input_bytes,
2148 ref output_bytes,
2149 head_unload_timer,
2150 step_rate_time,
2151 head_load_timer,
2152 dma_disabled,
2153 sense_output: ref interrupt_output,
2154 interrupt_level,
2155 position,
2156 internals,
2157 end_of_track,
2158 pending_command,
2159 } = self.state;
2160
2161 let saved_state = state::SavedState {
2162 digital_output: digital_output.into(),
2163 main_status: main_status.into(),
2164 input_bytes: input_bytes.to_vec(),
2165 output_bytes: output_bytes.to_vec(),
2166 head_unload_timer,
2167 step_rate_time,
2168 head_load_timer,
2169 dma_disabled,
2170 interrupt_output: interrupt_output.clone().map(|x| x.into()),
2171 interrupt_level,
2172 end_of_track,
2173 drive: 0,
2174 floppies: [state::SavedFloppyState {
2175 cylinder: position.cylinder,
2176 head: position.head,
2177 sector: position.sector,
2178 internals: internals.into(),
2179 }],
2180 pending_command: pending_command.0,
2181 };
2182
2183 Ok(saved_state)
2184 }
2185
2186 fn restore(&mut self, state: Self::SavedState) -> Result<(), RestoreError> {
2187 let state::SavedState {
2188 digital_output,
2189 main_status,
2190 input_bytes,
2191 output_bytes,
2192 head_unload_timer,
2193 step_rate_time,
2194 head_load_timer,
2195 dma_disabled,
2196 interrupt_output,
2197 interrupt_level,
2198 end_of_track,
2199 drive: _,
2200 floppies,
2201 pending_command,
2202 } = state;
2203
2204 self.state = FloppyState {
2205 digital_output: digital_output.into(),
2206 main_status: main_status.into(),
2207 input_bytes: input_bytes.as_slice().try_into().map_err(
2208 |e: arrayvec::CapacityError| RestoreError::InvalidSavedState(e.into()),
2209 )?,
2210 output_bytes: output_bytes.as_slice().try_into().map_err(
2211 |e: arrayvec::CapacityError| RestoreError::InvalidSavedState(e.into()),
2212 )?,
2213 head_unload_timer,
2214 step_rate_time,
2215 head_load_timer,
2216 dma_disabled,
2217 sense_output: interrupt_output.map(|x| x.into()),
2218 interrupt_level,
2219 end_of_track,
2220 position: ReadWriteHeadLocation {
2221 cylinder: floppies[0].cylinder,
2222 head: floppies[0].head,
2223 sector: floppies[0].sector,
2224 },
2225 internals: floppies[0].internals.into(),
2226 pending_command: FloppyCommand(pending_command),
2227 };
2228
2229 self.rt.interrupt.set_level(interrupt_level);
2230
2231 Ok(())
2232 }
2233 }
2234}