disk.c 17 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710
  1. /* Disk layer: read/write sectors via direct FDC + ISA DMA in protected mode. */
  2. #include "disk.h"
  3. #include "vfs.h"
  4. #include "io.h"
  5. #include "timer.h"
  6. #define SECTOR_SIZE 512
  7. #define SECTORS_PER_TRACK 18
  8. #define HEADS 2
  9. #define DMA_BUF_ADDR 0x4000 /* low RAM below the kernel; single sector (64 KB-safe) */
  10. /* FDC I/O ports */
  11. #define FDC_DOR 0x3F2
  12. #define FDC_MSR 0x3F4
  13. #define FDC_FIFO 0x3F5
  14. #define FDC_CCR 0x3F7
  15. /* ISA DMA channel 2 ports */
  16. #define DMA_CH2_ADDR 0x04
  17. #define DMA_CH2_COUNT 0x05
  18. #define DMA_CH2_PAGE 0x81
  19. #define DMA_SINGLEMASK 0x0A
  20. #define DMA_MODE 0x0B
  21. #define DMA_FLIPFLOP 0x0C
  22. /* MSR bits */
  23. #define MSR_RQM 0x80
  24. #define MSR_DIO 0x40
  25. #define MSR_CB 0x10
  26. /* FDC commands */
  27. #define CMD_SPECIFY 0x03
  28. #define CMD_RECALIBRATE 0x07
  29. #define CMD_SENSE_INT 0x08
  30. #define CMD_READ_DATA 0x66 /* READ | MFM | SK (no Multi-Track) */
  31. #define CMD_WRITE_DATA 0xC5 /* WRITE | MFM */
  32. #define CMD_SEEK 0x0F
  33. #define MOTOR_DELAY_MS 300
  34. #define IRQ_TIMEOUT_MS 2000
  35. #define PROBE_TIMEOUT_MS 500 /* boot-scan probe: shorter than normal I/O */
  36. #define MAX_RETRIES 10
  37. #define SEEK_SETTLE_MS 25 /* head settle after seek */
  38. /* PIC master mask port */
  39. #define PIC1_DATA 0x21
  40. static inline unsigned char inb(unsigned short port)
  41. {
  42. unsigned char v;
  43. __asm__ volatile ("inb %1, %0" : "=a"(v) : "Nd"(port));
  44. return v;
  45. }
  46. static inline void outb(unsigned short port, unsigned char v)
  47. {
  48. __asm__ volatile ("outb %0, %1" : : "a"(v), "Nd"(port));
  49. }
  50. volatile int fdc_irq_fired;
  51. void fdc_irq(void)
  52. {
  53. fdc_irq_fired = 1;
  54. outb(0x20, 0x20);
  55. }
  56. static int fdc_initialized;
  57. static int fdc_absent; /* controller did not answer at all */
  58. static unsigned int motor_mask; /* bits 0/1: motor spinning for drive N */
  59. static unsigned int cur_cylinder;
  60. static unsigned int current_drive;
  61. static unsigned int irq_timeout_ms = IRQ_TIMEOUT_MS;
  62. static int quiet_errors; /* presence probe: don't print read errors */
  63. /* ---- low-level FDC helpers ---- */
  64. static int fdc_wait_rqm(void)
  65. {
  66. unsigned int t0 = get_ticks_ms();
  67. while ((get_ticks_ms() - t0) < 500) {
  68. if (inb(FDC_MSR) & MSR_RQM)
  69. return 0;
  70. vfs_scan_poll(t0);
  71. }
  72. return -1;
  73. }
  74. static int fdc_send(unsigned char b)
  75. {
  76. if (fdc_wait_rqm() != 0)
  77. return -1;
  78. if (inb(FDC_MSR) & MSR_DIO)
  79. return -1;
  80. outb(FDC_FIFO, b);
  81. return 0;
  82. }
  83. static int fdc_recv(unsigned char *b)
  84. {
  85. if (fdc_wait_rqm() != 0)
  86. return -1;
  87. if (!(inb(FDC_MSR) & MSR_DIO))
  88. return -1;
  89. *b = inb(FDC_FIFO);
  90. return 0;
  91. }
  92. static int fdc_wait_irq(void)
  93. {
  94. unsigned int t0 = get_ticks_ms();
  95. while (!fdc_irq_fired) {
  96. if ((get_ticks_ms() - t0) >= irq_timeout_ms)
  97. return -1;
  98. if (!vfs_scan_relax(1))
  99. __asm__ volatile ("hlt");
  100. }
  101. return 0;
  102. }
  103. /* A controller that reads back 0x00/0xFF on the MSR is not there (or the
  104. * floppy function is disabled). Checking this before waiting for IRQ 6 keeps
  105. * a machine with no floppy controller from sitting in a 2 s timeout. */
  106. static int fdc_controller_alive(void)
  107. {
  108. unsigned int t0 = get_ticks_ms();
  109. for (;;) {
  110. unsigned char msr = inb(FDC_MSR);
  111. if (msr != 0x00 && msr != 0xFF && (msr & MSR_RQM))
  112. return 1;
  113. if ((get_ticks_ms() - t0) >= 200)
  114. return 0;
  115. vfs_scan_poll(t0);
  116. }
  117. }
  118. static int fdc_sense_interrupt(unsigned char *st0, unsigned char *cyl)
  119. {
  120. if (fdc_send(CMD_SENSE_INT) != 0)
  121. return -1;
  122. if (fdc_recv(st0) != 0)
  123. return -1;
  124. if (fdc_recv(cyl) != 0)
  125. return -1;
  126. return 0;
  127. }
  128. /* ---- motor control ---- */
  129. /* Program DOR: controller on, DMA+IRQ enabled, current drive selected, and
  130. * every motor that has been spun up left running. Keeping the other drive's
  131. * motor bit set avoids a second 300 ms spin-up when the scan switches from
  132. * floppy0 to floppy1. */
  133. static void fdc_select_drive(void)
  134. {
  135. unsigned int dor = 0x0Cu | current_drive;
  136. if (motor_mask & 1u)
  137. dor |= 1u << 4;
  138. if (motor_mask & 2u)
  139. dor |= 1u << 5;
  140. outb(FDC_DOR, (unsigned char)dor);
  141. }
  142. static void fdc_motor_on(void)
  143. {
  144. unsigned int bit = 1u << current_drive;
  145. if (motor_mask & bit) {
  146. fdc_select_drive(); /* switch select bits, no delay */
  147. return;
  148. }
  149. motor_mask |= bit;
  150. fdc_select_drive();
  151. {
  152. unsigned int t0 = get_ticks_ms();
  153. while ((get_ticks_ms() - t0) < MOTOR_DELAY_MS)
  154. if (!vfs_scan_relax(MOTOR_DELAY_MS))
  155. __asm__ volatile ("hlt");
  156. }
  157. }
  158. /* ---- reset ---- */
  159. static int fdc_reset(void)
  160. {
  161. unsigned char st0, cyl;
  162. int i;
  163. if (fdc_absent)
  164. return -1;
  165. /* Pull reset line: DOR = 0 turns off controller + motor */
  166. outb(FDC_DOR, 0x00);
  167. motor_mask = 0;
  168. {
  169. unsigned int t0 = get_ticks_ms();
  170. while ((get_ticks_ms() - t0) < 10)
  171. vfs_scan_poll(t0);
  172. }
  173. /* Release reset: controller on, DMA/IRQ enabled, no motor.
  174. * The FDC fires IRQ 6 once the reset completes. */
  175. fdc_irq_fired = 0;
  176. fdc_select_drive();
  177. if (fdc_wait_irq() != 0)
  178. return -1;
  179. for (i = 0; i < 4; i++)
  180. fdc_sense_interrupt(&st0, &cyl);
  181. outb(FDC_CCR, 0x00); /* 500 kbps for 1.44 MB */
  182. /* SPECIFY: SRT=12ms, HUT=240ms, HLT=30ms, DMA mode
  183. * Conservative timings for real hardware compatibility.
  184. * Byte 1: SRT_nibble=4 (16-4=12ms), HUT_nibble=F (15*16=240ms) → 0x4F
  185. * Byte 2: HLT=15 (15*2=30ms)<<1 | NDMA=0 → 0x1E */
  186. if (fdc_send(CMD_SPECIFY) != 0)
  187. return -1;
  188. if (fdc_send(0x4F) != 0)
  189. return -1;
  190. if (fdc_send(0x1E) != 0)
  191. return -1;
  192. return 0;
  193. }
  194. static int fdc_recalibrate(void)
  195. {
  196. unsigned char st0, cyl;
  197. fdc_irq_fired = 0;
  198. if (fdc_send(CMD_RECALIBRATE) != 0)
  199. return -1;
  200. if (fdc_send((unsigned char)current_drive) != 0)
  201. return -1;
  202. if (fdc_wait_irq() != 0)
  203. return -1;
  204. if (fdc_sense_interrupt(&st0, &cyl) != 0)
  205. return -1;
  206. if (st0 & 0xC0)
  207. return -1;
  208. cur_cylinder = 0;
  209. return 0;
  210. }
  211. static int fdc_seek(unsigned int cylinder, unsigned int head)
  212. {
  213. unsigned char st0, cyl;
  214. fdc_irq_fired = 0;
  215. if (fdc_send(CMD_SEEK) != 0)
  216. return -1;
  217. if (fdc_send((unsigned char)((head << 2) | current_drive)) != 0)
  218. return -1;
  219. if (fdc_send((unsigned char)cylinder) != 0)
  220. return -1;
  221. if (fdc_wait_irq() != 0)
  222. return -1;
  223. if (fdc_sense_interrupt(&st0, &cyl) != 0)
  224. return -1;
  225. if ((st0 & 0xC0) || cyl != (unsigned char)cylinder)
  226. return -1;
  227. cur_cylinder = cylinder;
  228. /* Let head settle on real hardware before issuing read (avoids LBA 33+ read failures). */
  229. {
  230. unsigned int t0 = get_ticks_ms();
  231. while ((get_ticks_ms() - t0) < SEEK_SETTLE_MS)
  232. vfs_scan_poll(t0);
  233. }
  234. return 0;
  235. }
  236. /* ---- DMA channel 2 setup ---- */
  237. static void dma_setup_read(unsigned int phys, unsigned int len)
  238. {
  239. unsigned int count = len - 1;
  240. outb(DMA_SINGLEMASK, 0x06); /* mask channel 2 */
  241. outb(DMA_FLIPFLOP, 0xFF); /* reset flip-flop */
  242. outb(DMA_CH2_ADDR, (unsigned char)(phys & 0xFF));
  243. outb(DMA_CH2_ADDR, (unsigned char)((phys >> 8) & 0xFF));
  244. outb(DMA_CH2_PAGE, (unsigned char)((phys >> 16) & 0xFF));
  245. outb(DMA_FLIPFLOP, 0xFF); /* reset flip-flop */
  246. outb(DMA_CH2_COUNT, (unsigned char)(count & 0xFF));
  247. outb(DMA_CH2_COUNT, (unsigned char)((count >> 8) & 0xFF));
  248. outb(DMA_MODE, 0x46); /* single, increment, write, ch2 */
  249. outb(DMA_SINGLEMASK, 0x02); /* unmask channel 2 */
  250. }
  251. static void dma_setup_write(unsigned int phys, unsigned int len)
  252. {
  253. unsigned int count = len - 1;
  254. outb(DMA_SINGLEMASK, 0x06); /* mask channel 2 */
  255. outb(DMA_FLIPFLOP, 0xFF); /* reset flip-flop */
  256. outb(DMA_CH2_ADDR, (unsigned char)(phys & 0xFF));
  257. outb(DMA_CH2_ADDR, (unsigned char)((phys >> 8) & 0xFF));
  258. outb(DMA_CH2_PAGE, (unsigned char)((phys >> 16) & 0xFF));
  259. outb(DMA_FLIPFLOP, 0xFF); /* reset flip-flop */
  260. outb(DMA_CH2_COUNT, (unsigned char)(count & 0xFF));
  261. outb(DMA_CH2_COUNT, (unsigned char)((count >> 8) & 0xFF));
  262. outb(DMA_MODE, 0x4A); /* single, increment, read (mem->dev), ch2 */
  263. outb(DMA_SINGLEMASK, 0x02); /* unmask channel 2 */
  264. }
  265. /* ---- read one sector ---- */
  266. static void lba_to_chs(unsigned int lba,
  267. unsigned int *cyl, unsigned int *head, unsigned int *sec)
  268. {
  269. unsigned int tmp = lba / SECTORS_PER_TRACK;
  270. *sec = (lba % SECTORS_PER_TRACK) + 1;
  271. *head = tmp % HEADS;
  272. *cyl = tmp / HEADS;
  273. }
  274. static int fdc_do_read(unsigned int cyl, unsigned int head, unsigned int sec)
  275. {
  276. unsigned char results[7];
  277. int i;
  278. dma_setup_read(DMA_BUF_ADDR, SECTOR_SIZE);
  279. fdc_irq_fired = 0;
  280. if (fdc_send(CMD_READ_DATA) != 0) return -1;
  281. if (fdc_send((unsigned char)((head << 2) | current_drive)) != 0) return -1;
  282. if (fdc_send((unsigned char)cyl) != 0) return -1;
  283. if (fdc_send((unsigned char)head) != 0) return -1;
  284. if (fdc_send((unsigned char)sec) != 0) return -1;
  285. if (fdc_send(0x02) != 0) return -1; /* 512 bytes/sector */
  286. if (fdc_send((unsigned char)SECTORS_PER_TRACK) != 0) return -1;
  287. if (fdc_send(0x1B) != 0) return -1; /* gap3 */
  288. if (fdc_send(0xFF) != 0) return -1; /* data length */
  289. if (fdc_wait_irq() != 0)
  290. return -2;
  291. for (i = 0; i < 7; i++) {
  292. if (fdc_recv(&results[i]) != 0)
  293. return -3;
  294. }
  295. if (results[0] & 0xC0) {
  296. if (!quiet_errors) {
  297. io_putstr("fdc: CHS=");
  298. io_puthex(cyl);
  299. io_putchar('/');
  300. io_puthex(head);
  301. io_putchar('/');
  302. io_puthex(sec);
  303. io_putstr(" ST0=");
  304. io_puthex(results[0]);
  305. io_putstr(" ST1=");
  306. io_puthex(results[1]);
  307. io_putstr(" ST2=");
  308. io_puthex(results[2]);
  309. io_putchar('\n');
  310. }
  311. return -1;
  312. }
  313. return 0;
  314. }
  315. static int fdc_do_write(unsigned int cyl, unsigned int head, unsigned int sec)
  316. {
  317. unsigned char results[7];
  318. int i;
  319. dma_setup_write(DMA_BUF_ADDR, SECTOR_SIZE);
  320. fdc_irq_fired = 0;
  321. if (fdc_send(CMD_WRITE_DATA) != 0) return -1;
  322. if (fdc_send((unsigned char)((head << 2) | current_drive)) != 0) return -1;
  323. if (fdc_send((unsigned char)cyl) != 0) return -1;
  324. if (fdc_send((unsigned char)head) != 0) return -1;
  325. if (fdc_send((unsigned char)sec) != 0) return -1;
  326. if (fdc_send(0x02) != 0) return -1; /* 512 bytes/sector */
  327. if (fdc_send((unsigned char)SECTORS_PER_TRACK) != 0) return -1;
  328. if (fdc_send(0x1B) != 0) return -1; /* gap3 */
  329. if (fdc_send(0xFF) != 0) return -1; /* data length */
  330. if (fdc_wait_irq() != 0)
  331. return -2;
  332. for (i = 0; i < 7; i++) {
  333. if (fdc_recv(&results[i]) != 0)
  334. return -3;
  335. }
  336. if (results[0] & 0xC0) {
  337. if (!quiet_errors) {
  338. io_putstr("fdc write: CHS=");
  339. io_puthex(cyl);
  340. io_putchar('/');
  341. io_puthex(head);
  342. io_putchar('/');
  343. io_puthex(sec);
  344. io_putstr(" ST0=");
  345. io_puthex(results[0]);
  346. io_putstr(" ST1=");
  347. io_puthex(results[1]);
  348. io_putstr(" ST2=");
  349. io_puthex(results[2]);
  350. io_putchar('\n');
  351. }
  352. return -1;
  353. }
  354. return 0;
  355. }
  356. static int fdc_read_one(unsigned int lba)
  357. {
  358. unsigned int cyl, head, sec;
  359. lba_to_chs(lba, &cyl, &head, &sec);
  360. if (cyl != cur_cylinder) {
  361. if (fdc_seek(cyl, head) != 0)
  362. return -1;
  363. }
  364. return fdc_do_read(cyl, head, sec);
  365. }
  366. static int fdc_write_one(unsigned int lba)
  367. {
  368. unsigned int cyl, head, sec;
  369. lba_to_chs(lba, &cyl, &head, &sec);
  370. if (cyl != cur_cylinder) {
  371. if (fdc_seek(cyl, head) != 0)
  372. return -1;
  373. }
  374. return fdc_do_write(cyl, head, sec);
  375. }
  376. /* Aggressive retry: seek far away then back to improve head alignment. */
  377. static int fdc_read_one_retry(unsigned int lba)
  378. {
  379. unsigned int cyl, head, sec;
  380. lba_to_chs(lba, &cyl, &head, &sec);
  381. /* Seek to cylinder 40 (mid-disk) first, then to target.
  382. * The long travel forces the stepper to settle accurately. */
  383. fdc_seek(cyl > 40 ? 0 : 40, 0);
  384. if (fdc_seek(cyl, head) != 0)
  385. return -1;
  386. return fdc_do_read(cyl, head, sec);
  387. }
  388. static int fdc_write_one_retry(unsigned int lba)
  389. {
  390. unsigned int cyl, head, sec;
  391. lba_to_chs(lba, &cyl, &head, &sec);
  392. fdc_seek(cyl > 40 ? 0 : 40, 0);
  393. if (fdc_seek(cyl, head) != 0)
  394. return -1;
  395. return fdc_do_write(cyl, head, sec);
  396. }
  397. /* ---- public API ---- */
  398. static int fdc_init_once(void)
  399. {
  400. if (fdc_absent)
  401. return -1;
  402. /* Mask IRQ 6 before reset so the completion interrupt doesn't
  403. * fire before we're ready (IRQ 6 may already be unmasked from
  404. * a previous init). Then clear the flag and release the reset. */
  405. outb(PIC1_DATA, inb(PIC1_DATA) | 0x40);
  406. outb(FDC_DOR, 0x00);
  407. motor_mask = 0;
  408. {
  409. unsigned int t0 = get_ticks_ms();
  410. while ((get_ticks_ms() - t0) < 10)
  411. vfs_scan_poll(t0);
  412. }
  413. fdc_irq_fired = 0;
  414. fdc_select_drive();
  415. /* Unmask IRQ 6 — the latched reset-completion interrupt fires now. */
  416. outb(PIC1_DATA, inb(PIC1_DATA) & ~0x40);
  417. /* No FDC at all: fail now instead of waiting out the IRQ timeout. */
  418. if (!fdc_controller_alive()) {
  419. outb(PIC1_DATA, inb(PIC1_DATA) | 0x40);
  420. fdc_absent = 1;
  421. return -1;
  422. }
  423. if (fdc_wait_irq() != 0)
  424. return -1;
  425. {
  426. unsigned char st0, cyl;
  427. int i;
  428. for (i = 0; i < 4; i++)
  429. fdc_sense_interrupt(&st0, &cyl);
  430. outb(FDC_CCR, 0x00);
  431. if (fdc_send(CMD_SPECIFY) != 0)
  432. return -1;
  433. if (fdc_send(0x4F) != 0)
  434. return -1;
  435. if (fdc_send(0x1E) != 0)
  436. return -1;
  437. }
  438. fdc_motor_on();
  439. if (fdc_recalibrate() != 0)
  440. return -1;
  441. fdc_initialized = 1;
  442. return 0;
  443. }
  444. void disk_set_drive(unsigned int drive)
  445. {
  446. if (drive > 1)
  447. return;
  448. if (drive == current_drive)
  449. return;
  450. current_drive = drive;
  451. if (fdc_initialized) {
  452. /* Same controller, different drive: its motor may still be off
  453. * and its head position is unknown. Keep the controller
  454. * initialised instead of paying a full reset. */
  455. cur_cylinder = 0xFFFFFFFFu;
  456. fdc_motor_on();
  457. fdc_recalibrate();
  458. }
  459. }
  460. /* Quick presence/media probe used by the boot scan. One command with a short
  461. * timeout, never the ten-retry recovery loop: an empty drive must not hold the
  462. * UI back. Normal I/O after a successful mount still gets full retries. */
  463. /* CMOS register 0x10 holds the BIOS's floppy drive types (high nibble
  464. * drive 0, low nibble drive 1; 0 = not installed). Every AT-compatible
  465. * BIOS fills it in. Without this gate a machine whose FDC exists but has
  466. * no drive (QEMU without -fda, most laptops/boards with a Super I/O FDC)
  467. * paid the 300 ms motor spin-up plus a probe timeout per drive at boot. */
  468. static int cmos_floppy_type(unsigned int drive)
  469. {
  470. unsigned char v;
  471. outb(0x70, 0x80 | 0x10); /* bit 7: NMI disabled while reading */
  472. v = inb(0x71);
  473. outb(0x70, 0x00);
  474. return drive == 0 ? (v >> 4) & 0x0F : v & 0x0F;
  475. }
  476. int disk_probe(unsigned int drive)
  477. {
  478. int rc;
  479. if (drive > 1 || fdc_absent)
  480. return 0;
  481. if (cmos_floppy_type(drive) == 0)
  482. return 0;
  483. irq_timeout_ms = PROBE_TIMEOUT_MS;
  484. disk_set_drive(drive);
  485. if (!fdc_initialized && fdc_init_once() != 0) {
  486. irq_timeout_ms = IRQ_TIMEOUT_MS;
  487. return 0;
  488. }
  489. quiet_errors = 1;
  490. rc = fdc_read_one(0);
  491. quiet_errors = 0;
  492. irq_timeout_ms = IRQ_TIMEOUT_MS;
  493. if (rc != 0) {
  494. /* A timed-out command may still be sitting in the FDC: force a
  495. * clean re-init next time. A normal error response (empty
  496. * drive / no media) leaves the controller usable, so keep it
  497. * initialized for the mount that follows. */
  498. if (rc == -2)
  499. fdc_initialized = 0;
  500. return 0;
  501. }
  502. return 1;
  503. }
  504. unsigned int disk_get_drive(void)
  505. {
  506. return current_drive;
  507. }
  508. int disk_read_sectors(unsigned int lba, void *buf, unsigned int count)
  509. {
  510. unsigned int i;
  511. if (!fdc_initialized) {
  512. if (fdc_init_once() != 0)
  513. return -1;
  514. }
  515. for (i = 0; i < count; i++) {
  516. int attempt, ok = 0;
  517. for (attempt = 0; attempt < MAX_RETRIES; attempt++) {
  518. int rc;
  519. if (attempt < 3) {
  520. rc = fdc_read_one(lba + i);
  521. } else {
  522. /* Long-seek retry: seek far away then back to
  523. * force accurate head positioning. */
  524. rc = fdc_read_one_retry(lba + i);
  525. }
  526. if (rc == 0) {
  527. ok = 1;
  528. break;
  529. }
  530. fdc_reset();
  531. fdc_motor_on();
  532. fdc_recalibrate();
  533. }
  534. if (!ok) {
  535. io_putstr("disk: read failed LBA=");
  536. io_puthex(lba + i);
  537. io_putstr(" (sector ");
  538. io_puthex(i);
  539. io_putstr(" of ");
  540. io_puthex(count);
  541. io_putstr(")\n");
  542. return -1;
  543. }
  544. {
  545. unsigned int j;
  546. unsigned char *dst = (unsigned char *)buf + (i * SECTOR_SIZE);
  547. const unsigned char *src = (const unsigned char *)DMA_BUF_ADDR;
  548. for (j = 0; j < SECTOR_SIZE; j++)
  549. dst[j] = src[j];
  550. }
  551. }
  552. return 0;
  553. }
  554. int disk_write_sectors(unsigned int lba, const void *buf, unsigned int count)
  555. {
  556. unsigned int i;
  557. if (!fdc_initialized) {
  558. if (fdc_init_once() != 0)
  559. return -1;
  560. }
  561. for (i = 0; i < count; i++) {
  562. int attempt, ok = 0;
  563. /* Copy the sector to write into the ISA DMA buffer first. */
  564. {
  565. unsigned int j;
  566. const unsigned char *src = (const unsigned char *)buf + (i * SECTOR_SIZE);
  567. volatile unsigned char *dst = (volatile unsigned char *)DMA_BUF_ADDR;
  568. for (j = 0; j < SECTOR_SIZE; j++)
  569. dst[j] = src[j];
  570. }
  571. for (attempt = 0; attempt < MAX_RETRIES; attempt++) {
  572. int rc;
  573. if (attempt < 3) {
  574. rc = fdc_write_one(lba + i);
  575. } else {
  576. rc = fdc_write_one_retry(lba + i);
  577. }
  578. if (rc == 0) {
  579. ok = 1;
  580. break;
  581. }
  582. fdc_reset();
  583. fdc_motor_on();
  584. fdc_recalibrate();
  585. }
  586. if (!ok) {
  587. io_putstr("disk: write failed LBA=");
  588. io_puthex(lba + i);
  589. io_putstr(" (sector ");
  590. io_puthex(i);
  591. io_putstr(" of ");
  592. io_puthex(count);
  593. io_putstr(")\n");
  594. return -1;
  595. }
  596. }
  597. return 0;
  598. }