disk.c 13 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577
  1. /* Disk layer: read/write sectors via direct FDC + ISA DMA in protected mode. */
  2. #include "disk.h"
  3. #include "io.h"
  4. #include "timer.h"
  5. #define SECTOR_SIZE 512
  6. #define SECTORS_PER_TRACK 18
  7. #define HEADS 2
  8. #define DMA_BUF_ADDR 0x20000
  9. /* FDC I/O ports */
  10. #define FDC_DOR 0x3F2
  11. #define FDC_MSR 0x3F4
  12. #define FDC_FIFO 0x3F5
  13. #define FDC_CCR 0x3F7
  14. /* ISA DMA channel 2 ports */
  15. #define DMA_CH2_ADDR 0x04
  16. #define DMA_CH2_COUNT 0x05
  17. #define DMA_CH2_PAGE 0x81
  18. #define DMA_SINGLEMASK 0x0A
  19. #define DMA_MODE 0x0B
  20. #define DMA_FLIPFLOP 0x0C
  21. /* MSR bits */
  22. #define MSR_RQM 0x80
  23. #define MSR_DIO 0x40
  24. #define MSR_CB 0x10
  25. /* FDC commands */
  26. #define CMD_SPECIFY 0x03
  27. #define CMD_RECALIBRATE 0x07
  28. #define CMD_SENSE_INT 0x08
  29. #define CMD_READ_DATA 0x66 /* READ | MFM | SK (no Multi-Track) */
  30. #define CMD_WRITE_DATA 0xC5 /* WRITE | MFM */
  31. #define CMD_SEEK 0x0F
  32. #define MOTOR_DELAY_MS 300
  33. #define IRQ_TIMEOUT_MS 2000
  34. #define MAX_RETRIES 10
  35. #define SEEK_SETTLE_MS 25 /* head settle after seek */
  36. /* PIC master mask port */
  37. #define PIC1_DATA 0x21
  38. static inline unsigned char inb(unsigned short port)
  39. {
  40. unsigned char v;
  41. __asm__ volatile ("inb %1, %0" : "=a"(v) : "Nd"(port));
  42. return v;
  43. }
  44. static inline void outb(unsigned short port, unsigned char v)
  45. {
  46. __asm__ volatile ("outb %0, %1" : : "a"(v), "Nd"(port));
  47. }
  48. volatile int fdc_irq_fired;
  49. void fdc_irq(void)
  50. {
  51. fdc_irq_fired = 1;
  52. outb(0x20, 0x20);
  53. }
  54. static int fdc_initialized;
  55. static int motor_on;
  56. static unsigned int cur_cylinder;
  57. static unsigned int current_drive;
  58. /* ---- low-level FDC helpers ---- */
  59. static int fdc_wait_rqm(void)
  60. {
  61. unsigned int t0 = get_ticks_ms();
  62. while ((get_ticks_ms() - t0) < 500) {
  63. if (inb(FDC_MSR) & MSR_RQM)
  64. return 0;
  65. }
  66. return -1;
  67. }
  68. static int fdc_send(unsigned char b)
  69. {
  70. if (fdc_wait_rqm() != 0)
  71. return -1;
  72. if (inb(FDC_MSR) & MSR_DIO)
  73. return -1;
  74. outb(FDC_FIFO, b);
  75. return 0;
  76. }
  77. static int fdc_recv(unsigned char *b)
  78. {
  79. if (fdc_wait_rqm() != 0)
  80. return -1;
  81. if (!(inb(FDC_MSR) & MSR_DIO))
  82. return -1;
  83. *b = inb(FDC_FIFO);
  84. return 0;
  85. }
  86. static int fdc_wait_irq(void)
  87. {
  88. unsigned int t0 = get_ticks_ms();
  89. while (!fdc_irq_fired) {
  90. if ((get_ticks_ms() - t0) >= IRQ_TIMEOUT_MS)
  91. return -1;
  92. __asm__ volatile ("hlt");
  93. }
  94. return 0;
  95. }
  96. static int fdc_sense_interrupt(unsigned char *st0, unsigned char *cyl)
  97. {
  98. if (fdc_send(CMD_SENSE_INT) != 0)
  99. return -1;
  100. if (fdc_recv(st0) != 0)
  101. return -1;
  102. if (fdc_recv(cyl) != 0)
  103. return -1;
  104. return 0;
  105. }
  106. /* ---- motor control ---- */
  107. static void fdc_motor_on(void)
  108. {
  109. if (motor_on)
  110. return;
  111. outb(FDC_DOR, (unsigned char)((1 << (4 + current_drive)) | 0x0C | current_drive));
  112. {
  113. unsigned int t0 = get_ticks_ms();
  114. while ((get_ticks_ms() - t0) < MOTOR_DELAY_MS)
  115. __asm__ volatile ("hlt");
  116. }
  117. motor_on = 1;
  118. }
  119. /* ---- reset ---- */
  120. static int fdc_reset(void)
  121. {
  122. unsigned char st0, cyl;
  123. int i;
  124. /* Pull reset line: DOR = 0 turns off controller + motor */
  125. outb(FDC_DOR, 0x00);
  126. motor_on = 0;
  127. {
  128. unsigned int t0 = get_ticks_ms();
  129. while ((get_ticks_ms() - t0) < 10)
  130. ;
  131. }
  132. /* Release reset: controller on, DMA/IRQ enabled, no motor.
  133. * The FDC fires IRQ 6 once the reset completes. */
  134. fdc_irq_fired = 0;
  135. outb(FDC_DOR, (unsigned char)(0x0C | current_drive));
  136. if (fdc_wait_irq() != 0)
  137. return -1;
  138. for (i = 0; i < 4; i++)
  139. fdc_sense_interrupt(&st0, &cyl);
  140. outb(FDC_CCR, 0x00); /* 500 kbps for 1.44 MB */
  141. /* SPECIFY: SRT=12ms, HUT=240ms, HLT=30ms, DMA mode
  142. * Conservative timings for real hardware compatibility.
  143. * Byte 1: SRT_nibble=4 (16-4=12ms), HUT_nibble=F (15*16=240ms) → 0x4F
  144. * Byte 2: HLT=15 (15*2=30ms)<<1 | NDMA=0 → 0x1E */
  145. if (fdc_send(CMD_SPECIFY) != 0)
  146. return -1;
  147. if (fdc_send(0x4F) != 0)
  148. return -1;
  149. if (fdc_send(0x1E) != 0)
  150. return -1;
  151. return 0;
  152. }
  153. static int fdc_recalibrate(void)
  154. {
  155. unsigned char st0, cyl;
  156. fdc_irq_fired = 0;
  157. if (fdc_send(CMD_RECALIBRATE) != 0)
  158. return -1;
  159. if (fdc_send((unsigned char)current_drive) != 0)
  160. return -1;
  161. if (fdc_wait_irq() != 0)
  162. return -1;
  163. if (fdc_sense_interrupt(&st0, &cyl) != 0)
  164. return -1;
  165. if (st0 & 0xC0)
  166. return -1;
  167. cur_cylinder = 0;
  168. return 0;
  169. }
  170. static int fdc_seek(unsigned int cylinder, unsigned int head)
  171. {
  172. unsigned char st0, cyl;
  173. fdc_irq_fired = 0;
  174. if (fdc_send(CMD_SEEK) != 0)
  175. return -1;
  176. if (fdc_send((unsigned char)((head << 2) | current_drive)) != 0)
  177. return -1;
  178. if (fdc_send((unsigned char)cylinder) != 0)
  179. return -1;
  180. if (fdc_wait_irq() != 0)
  181. return -1;
  182. if (fdc_sense_interrupt(&st0, &cyl) != 0)
  183. return -1;
  184. if ((st0 & 0xC0) || cyl != (unsigned char)cylinder)
  185. return -1;
  186. cur_cylinder = cylinder;
  187. /* Let head settle on real hardware before issuing read (avoids LBA 33+ read failures). */
  188. {
  189. unsigned int t0 = get_ticks_ms();
  190. while ((get_ticks_ms() - t0) < SEEK_SETTLE_MS)
  191. ;
  192. }
  193. return 0;
  194. }
  195. /* ---- DMA channel 2 setup ---- */
  196. static void dma_setup_read(unsigned int phys, unsigned int len)
  197. {
  198. unsigned int count = len - 1;
  199. outb(DMA_SINGLEMASK, 0x06); /* mask channel 2 */
  200. outb(DMA_FLIPFLOP, 0xFF); /* reset flip-flop */
  201. outb(DMA_CH2_ADDR, (unsigned char)(phys & 0xFF));
  202. outb(DMA_CH2_ADDR, (unsigned char)((phys >> 8) & 0xFF));
  203. outb(DMA_CH2_PAGE, (unsigned char)((phys >> 16) & 0xFF));
  204. outb(DMA_FLIPFLOP, 0xFF); /* reset flip-flop */
  205. outb(DMA_CH2_COUNT, (unsigned char)(count & 0xFF));
  206. outb(DMA_CH2_COUNT, (unsigned char)((count >> 8) & 0xFF));
  207. outb(DMA_MODE, 0x46); /* single, increment, write, ch2 */
  208. outb(DMA_SINGLEMASK, 0x02); /* unmask channel 2 */
  209. }
  210. static void dma_setup_write(unsigned int phys, unsigned int len)
  211. {
  212. unsigned int count = len - 1;
  213. outb(DMA_SINGLEMASK, 0x06); /* mask channel 2 */
  214. outb(DMA_FLIPFLOP, 0xFF); /* reset flip-flop */
  215. outb(DMA_CH2_ADDR, (unsigned char)(phys & 0xFF));
  216. outb(DMA_CH2_ADDR, (unsigned char)((phys >> 8) & 0xFF));
  217. outb(DMA_CH2_PAGE, (unsigned char)((phys >> 16) & 0xFF));
  218. outb(DMA_FLIPFLOP, 0xFF); /* reset flip-flop */
  219. outb(DMA_CH2_COUNT, (unsigned char)(count & 0xFF));
  220. outb(DMA_CH2_COUNT, (unsigned char)((count >> 8) & 0xFF));
  221. outb(DMA_MODE, 0x4A); /* single, increment, read (mem->dev), ch2 */
  222. outb(DMA_SINGLEMASK, 0x02); /* unmask channel 2 */
  223. }
  224. /* ---- read one sector ---- */
  225. static void lba_to_chs(unsigned int lba,
  226. unsigned int *cyl, unsigned int *head, unsigned int *sec)
  227. {
  228. unsigned int tmp = lba / SECTORS_PER_TRACK;
  229. *sec = (lba % SECTORS_PER_TRACK) + 1;
  230. *head = tmp % HEADS;
  231. *cyl = tmp / HEADS;
  232. }
  233. static int fdc_do_read(unsigned int cyl, unsigned int head, unsigned int sec)
  234. {
  235. unsigned char results[7];
  236. int i;
  237. dma_setup_read(DMA_BUF_ADDR, SECTOR_SIZE);
  238. fdc_irq_fired = 0;
  239. if (fdc_send(CMD_READ_DATA) != 0) return -1;
  240. if (fdc_send((unsigned char)((head << 2) | current_drive)) != 0) return -1;
  241. if (fdc_send((unsigned char)cyl) != 0) return -1;
  242. if (fdc_send((unsigned char)head) != 0) return -1;
  243. if (fdc_send((unsigned char)sec) != 0) return -1;
  244. if (fdc_send(0x02) != 0) return -1; /* 512 bytes/sector */
  245. if (fdc_send((unsigned char)SECTORS_PER_TRACK) != 0) return -1;
  246. if (fdc_send(0x1B) != 0) return -1; /* gap3 */
  247. if (fdc_send(0xFF) != 0) return -1; /* data length */
  248. if (fdc_wait_irq() != 0)
  249. return -2;
  250. for (i = 0; i < 7; i++) {
  251. if (fdc_recv(&results[i]) != 0)
  252. return -3;
  253. }
  254. if (results[0] & 0xC0) {
  255. io_putstr("fdc: CHS=");
  256. io_puthex(cyl);
  257. io_putchar('/');
  258. io_puthex(head);
  259. io_putchar('/');
  260. io_puthex(sec);
  261. io_putstr(" ST0=");
  262. io_puthex(results[0]);
  263. io_putstr(" ST1=");
  264. io_puthex(results[1]);
  265. io_putstr(" ST2=");
  266. io_puthex(results[2]);
  267. io_putchar('\n');
  268. return -1;
  269. }
  270. return 0;
  271. }
  272. static int fdc_do_write(unsigned int cyl, unsigned int head, unsigned int sec)
  273. {
  274. unsigned char results[7];
  275. int i;
  276. dma_setup_write(DMA_BUF_ADDR, SECTOR_SIZE);
  277. fdc_irq_fired = 0;
  278. if (fdc_send(CMD_WRITE_DATA) != 0) return -1;
  279. if (fdc_send((unsigned char)((head << 2) | current_drive)) != 0) return -1;
  280. if (fdc_send((unsigned char)cyl) != 0) return -1;
  281. if (fdc_send((unsigned char)head) != 0) return -1;
  282. if (fdc_send((unsigned char)sec) != 0) return -1;
  283. if (fdc_send(0x02) != 0) return -1; /* 512 bytes/sector */
  284. if (fdc_send((unsigned char)SECTORS_PER_TRACK) != 0) return -1;
  285. if (fdc_send(0x1B) != 0) return -1; /* gap3 */
  286. if (fdc_send(0xFF) != 0) return -1; /* data length */
  287. if (fdc_wait_irq() != 0)
  288. return -2;
  289. for (i = 0; i < 7; i++) {
  290. if (fdc_recv(&results[i]) != 0)
  291. return -3;
  292. }
  293. if (results[0] & 0xC0) {
  294. io_putstr("fdc write: CHS=");
  295. io_puthex(cyl);
  296. io_putchar('/');
  297. io_puthex(head);
  298. io_putchar('/');
  299. io_puthex(sec);
  300. io_putstr(" ST0=");
  301. io_puthex(results[0]);
  302. io_putstr(" ST1=");
  303. io_puthex(results[1]);
  304. io_putstr(" ST2=");
  305. io_puthex(results[2]);
  306. io_putchar('\n');
  307. return -1;
  308. }
  309. return 0;
  310. }
  311. static int fdc_read_one(unsigned int lba)
  312. {
  313. unsigned int cyl, head, sec;
  314. lba_to_chs(lba, &cyl, &head, &sec);
  315. if (cyl != cur_cylinder) {
  316. if (fdc_seek(cyl, head) != 0)
  317. return -1;
  318. }
  319. return fdc_do_read(cyl, head, sec);
  320. }
  321. static int fdc_write_one(unsigned int lba)
  322. {
  323. unsigned int cyl, head, sec;
  324. lba_to_chs(lba, &cyl, &head, &sec);
  325. if (cyl != cur_cylinder) {
  326. if (fdc_seek(cyl, head) != 0)
  327. return -1;
  328. }
  329. return fdc_do_write(cyl, head, sec);
  330. }
  331. /* Aggressive retry: seek far away then back to improve head alignment. */
  332. static int fdc_read_one_retry(unsigned int lba)
  333. {
  334. unsigned int cyl, head, sec;
  335. lba_to_chs(lba, &cyl, &head, &sec);
  336. /* Seek to cylinder 40 (mid-disk) first, then to target.
  337. * The long travel forces the stepper to settle accurately. */
  338. fdc_seek(cyl > 40 ? 0 : 40, 0);
  339. if (fdc_seek(cyl, head) != 0)
  340. return -1;
  341. return fdc_do_read(cyl, head, sec);
  342. }
  343. static int fdc_write_one_retry(unsigned int lba)
  344. {
  345. unsigned int cyl, head, sec;
  346. lba_to_chs(lba, &cyl, &head, &sec);
  347. fdc_seek(cyl > 40 ? 0 : 40, 0);
  348. if (fdc_seek(cyl, head) != 0)
  349. return -1;
  350. return fdc_do_write(cyl, head, sec);
  351. }
  352. /* ---- public API ---- */
  353. static int fdc_init_once(void)
  354. {
  355. /* Mask IRQ 6 before reset so the completion interrupt doesn't
  356. * fire before we're ready (IRQ 6 may already be unmasked from
  357. * a previous init). Then clear the flag and release the reset. */
  358. outb(PIC1_DATA, inb(PIC1_DATA) | 0x40);
  359. outb(FDC_DOR, 0x00);
  360. motor_on = 0;
  361. {
  362. unsigned int t0 = get_ticks_ms();
  363. while ((get_ticks_ms() - t0) < 10)
  364. ;
  365. }
  366. fdc_irq_fired = 0;
  367. outb(FDC_DOR, (unsigned char)(0x0C | current_drive));
  368. /* Unmask IRQ 6 — the latched reset-completion interrupt fires now. */
  369. outb(PIC1_DATA, inb(PIC1_DATA) & ~0x40);
  370. if (fdc_wait_irq() != 0)
  371. return -1;
  372. {
  373. unsigned char st0, cyl;
  374. int i;
  375. for (i = 0; i < 4; i++)
  376. fdc_sense_interrupt(&st0, &cyl);
  377. outb(FDC_CCR, 0x00);
  378. if (fdc_send(CMD_SPECIFY) != 0)
  379. return -1;
  380. if (fdc_send(0x4F) != 0)
  381. return -1;
  382. if (fdc_send(0x1E) != 0)
  383. return -1;
  384. }
  385. fdc_motor_on();
  386. if (fdc_recalibrate() != 0)
  387. return -1;
  388. fdc_initialized = 1;
  389. return 0;
  390. }
  391. int disk_read_sectors(unsigned int lba, void *buf, unsigned int count)
  392. {
  393. unsigned int i;
  394. if (!fdc_initialized) {
  395. if (fdc_init_once() != 0)
  396. return -1;
  397. }
  398. for (i = 0; i < count; i++) {
  399. int attempt, ok = 0;
  400. for (attempt = 0; attempt < MAX_RETRIES; attempt++) {
  401. int rc;
  402. if (attempt < 3) {
  403. rc = fdc_read_one(lba + i);
  404. } else {
  405. /* Long-seek retry: seek far away then back to
  406. * force accurate head positioning. */
  407. rc = fdc_read_one_retry(lba + i);
  408. }
  409. if (rc == 0) {
  410. ok = 1;
  411. break;
  412. }
  413. fdc_reset();
  414. fdc_motor_on();
  415. fdc_recalibrate();
  416. }
  417. if (!ok) {
  418. io_putstr("disk: read failed LBA=");
  419. io_puthex(lba + i);
  420. io_putstr(" (sector ");
  421. io_puthex(i);
  422. io_putstr(" of ");
  423. io_puthex(count);
  424. io_putstr(")\n");
  425. return -1;
  426. }
  427. {
  428. unsigned int j;
  429. unsigned char *dst = (unsigned char *)buf + (i * SECTOR_SIZE);
  430. const unsigned char *src = (const unsigned char *)DMA_BUF_ADDR;
  431. for (j = 0; j < SECTOR_SIZE; j++)
  432. dst[j] = src[j];
  433. }
  434. }
  435. return 0;
  436. }
  437. int disk_write_sectors(unsigned int lba, const void *buf, unsigned int count)
  438. {
  439. unsigned int i;
  440. if (!fdc_initialized) {
  441. if (fdc_init_once() != 0)
  442. return -1;
  443. }
  444. for (i = 0; i < count; i++) {
  445. int attempt, ok = 0;
  446. /* Copy the sector to write into the ISA DMA buffer first. */
  447. {
  448. unsigned int j;
  449. const unsigned char *src = (const unsigned char *)buf + (i * SECTOR_SIZE);
  450. volatile unsigned char *dst = (volatile unsigned char *)DMA_BUF_ADDR;
  451. for (j = 0; j < SECTOR_SIZE; j++)
  452. dst[j] = src[j];
  453. }
  454. for (attempt = 0; attempt < MAX_RETRIES; attempt++) {
  455. int rc;
  456. if (attempt < 3) {
  457. rc = fdc_write_one(lba + i);
  458. } else {
  459. rc = fdc_write_one_retry(lba + i);
  460. }
  461. if (rc == 0) {
  462. ok = 1;
  463. break;
  464. }
  465. fdc_reset();
  466. fdc_motor_on();
  467. fdc_recalibrate();
  468. }
  469. if (!ok) {
  470. io_putstr("disk: write failed LBA=");
  471. io_puthex(lba + i);
  472. io_putstr(" (sector ");
  473. io_puthex(i);
  474. io_putstr(" of ");
  475. io_puthex(count);
  476. io_putstr(")\n");
  477. return -1;
  478. }
  479. }
  480. return 0;
  481. }