disk.c 9.7 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452
  1. /* Disk layer: read 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_SEEK 0x0F
  31. #define MOTOR_DELAY_MS 300
  32. #define IRQ_TIMEOUT_MS 2000
  33. #define MAX_RETRIES 10
  34. #define SEEK_SETTLE_MS 25 /* head settle after seek */
  35. /* PIC master mask port */
  36. #define PIC1_DATA 0x21
  37. static inline unsigned char inb(unsigned short port)
  38. {
  39. unsigned char v;
  40. __asm__ volatile ("inb %1, %0" : "=a"(v) : "Nd"(port));
  41. return v;
  42. }
  43. static inline void outb(unsigned short port, unsigned char v)
  44. {
  45. __asm__ volatile ("outb %0, %1" : : "a"(v), "Nd"(port));
  46. }
  47. volatile int fdc_irq_fired;
  48. void fdc_irq(void)
  49. {
  50. fdc_irq_fired = 1;
  51. outb(0x20, 0x20);
  52. }
  53. static int fdc_initialized;
  54. static int motor_on;
  55. static unsigned int cur_cylinder;
  56. static unsigned int current_drive;
  57. /* ---- low-level FDC helpers ---- */
  58. static int fdc_wait_rqm(void)
  59. {
  60. unsigned int t0 = get_ticks_ms();
  61. while ((get_ticks_ms() - t0) < 500) {
  62. if (inb(FDC_MSR) & MSR_RQM)
  63. return 0;
  64. }
  65. return -1;
  66. }
  67. static int fdc_send(unsigned char b)
  68. {
  69. if (fdc_wait_rqm() != 0)
  70. return -1;
  71. if (inb(FDC_MSR) & MSR_DIO)
  72. return -1;
  73. outb(FDC_FIFO, b);
  74. return 0;
  75. }
  76. static int fdc_recv(unsigned char *b)
  77. {
  78. if (fdc_wait_rqm() != 0)
  79. return -1;
  80. if (!(inb(FDC_MSR) & MSR_DIO))
  81. return -1;
  82. *b = inb(FDC_FIFO);
  83. return 0;
  84. }
  85. static int fdc_wait_irq(void)
  86. {
  87. unsigned int t0 = get_ticks_ms();
  88. while (!fdc_irq_fired) {
  89. if ((get_ticks_ms() - t0) >= IRQ_TIMEOUT_MS)
  90. return -1;
  91. __asm__ volatile ("hlt");
  92. }
  93. return 0;
  94. }
  95. static int fdc_sense_interrupt(unsigned char *st0, unsigned char *cyl)
  96. {
  97. if (fdc_send(CMD_SENSE_INT) != 0)
  98. return -1;
  99. if (fdc_recv(st0) != 0)
  100. return -1;
  101. if (fdc_recv(cyl) != 0)
  102. return -1;
  103. return 0;
  104. }
  105. /* ---- motor control ---- */
  106. static void fdc_motor_on(void)
  107. {
  108. if (motor_on)
  109. return;
  110. outb(FDC_DOR, (unsigned char)((1 << (4 + current_drive)) | 0x0C | current_drive));
  111. {
  112. unsigned int t0 = get_ticks_ms();
  113. while ((get_ticks_ms() - t0) < MOTOR_DELAY_MS)
  114. __asm__ volatile ("hlt");
  115. }
  116. motor_on = 1;
  117. }
  118. /* ---- reset ---- */
  119. static int fdc_reset(void)
  120. {
  121. unsigned char st0, cyl;
  122. int i;
  123. /* Pull reset line: DOR = 0 turns off controller + motor */
  124. outb(FDC_DOR, 0x00);
  125. motor_on = 0;
  126. {
  127. unsigned int t0 = get_ticks_ms();
  128. while ((get_ticks_ms() - t0) < 10)
  129. ;
  130. }
  131. /* Release reset: controller on, DMA/IRQ enabled, no motor.
  132. * The FDC fires IRQ 6 once the reset completes. */
  133. fdc_irq_fired = 0;
  134. outb(FDC_DOR, (unsigned char)(0x0C | current_drive));
  135. if (fdc_wait_irq() != 0)
  136. return -1;
  137. for (i = 0; i < 4; i++)
  138. fdc_sense_interrupt(&st0, &cyl);
  139. outb(FDC_CCR, 0x00); /* 500 kbps for 1.44 MB */
  140. /* SPECIFY: SRT=12ms, HUT=240ms, HLT=30ms, DMA mode
  141. * Conservative timings for real hardware compatibility.
  142. * Byte 1: SRT_nibble=4 (16-4=12ms), HUT_nibble=F (15*16=240ms) → 0x4F
  143. * Byte 2: HLT=15 (15*2=30ms)<<1 | NDMA=0 → 0x1E */
  144. if (fdc_send(CMD_SPECIFY) != 0)
  145. return -1;
  146. if (fdc_send(0x4F) != 0)
  147. return -1;
  148. if (fdc_send(0x1E) != 0)
  149. return -1;
  150. return 0;
  151. }
  152. static int fdc_recalibrate(void)
  153. {
  154. unsigned char st0, cyl;
  155. fdc_irq_fired = 0;
  156. if (fdc_send(CMD_RECALIBRATE) != 0)
  157. return -1;
  158. if (fdc_send((unsigned char)current_drive) != 0)
  159. return -1;
  160. if (fdc_wait_irq() != 0)
  161. return -1;
  162. if (fdc_sense_interrupt(&st0, &cyl) != 0)
  163. return -1;
  164. if (st0 & 0xC0)
  165. return -1;
  166. cur_cylinder = 0;
  167. return 0;
  168. }
  169. static int fdc_seek(unsigned int cylinder, unsigned int head)
  170. {
  171. unsigned char st0, cyl;
  172. fdc_irq_fired = 0;
  173. if (fdc_send(CMD_SEEK) != 0)
  174. return -1;
  175. if (fdc_send((unsigned char)((head << 2) | current_drive)) != 0)
  176. return -1;
  177. if (fdc_send((unsigned char)cylinder) != 0)
  178. return -1;
  179. if (fdc_wait_irq() != 0)
  180. return -1;
  181. if (fdc_sense_interrupt(&st0, &cyl) != 0)
  182. return -1;
  183. if ((st0 & 0xC0) || cyl != (unsigned char)cylinder)
  184. return -1;
  185. cur_cylinder = cylinder;
  186. /* Let head settle on real hardware before issuing read (avoids LBA 33+ read failures). */
  187. {
  188. unsigned int t0 = get_ticks_ms();
  189. while ((get_ticks_ms() - t0) < SEEK_SETTLE_MS)
  190. ;
  191. }
  192. return 0;
  193. }
  194. /* ---- DMA channel 2 setup ---- */
  195. static void dma_setup_read(unsigned int phys, unsigned int len)
  196. {
  197. unsigned int count = len - 1;
  198. outb(DMA_SINGLEMASK, 0x06); /* mask channel 2 */
  199. outb(DMA_FLIPFLOP, 0xFF); /* reset flip-flop */
  200. outb(DMA_CH2_ADDR, (unsigned char)(phys & 0xFF));
  201. outb(DMA_CH2_ADDR, (unsigned char)((phys >> 8) & 0xFF));
  202. outb(DMA_CH2_PAGE, (unsigned char)((phys >> 16) & 0xFF));
  203. outb(DMA_FLIPFLOP, 0xFF); /* reset flip-flop */
  204. outb(DMA_CH2_COUNT, (unsigned char)(count & 0xFF));
  205. outb(DMA_CH2_COUNT, (unsigned char)((count >> 8) & 0xFF));
  206. outb(DMA_MODE, 0x46); /* single, increment, write, ch2 */
  207. outb(DMA_SINGLEMASK, 0x02); /* unmask channel 2 */
  208. }
  209. /* ---- read one sector ---- */
  210. static void lba_to_chs(unsigned int lba,
  211. unsigned int *cyl, unsigned int *head, unsigned int *sec)
  212. {
  213. unsigned int tmp = lba / SECTORS_PER_TRACK;
  214. *sec = (lba % SECTORS_PER_TRACK) + 1;
  215. *head = tmp % HEADS;
  216. *cyl = tmp / HEADS;
  217. }
  218. static int fdc_do_read(unsigned int cyl, unsigned int head, unsigned int sec)
  219. {
  220. unsigned char results[7];
  221. int i;
  222. dma_setup_read(DMA_BUF_ADDR, SECTOR_SIZE);
  223. fdc_irq_fired = 0;
  224. if (fdc_send(CMD_READ_DATA) != 0) return -1;
  225. if (fdc_send((unsigned char)((head << 2) | current_drive)) != 0) return -1;
  226. if (fdc_send((unsigned char)cyl) != 0) return -1;
  227. if (fdc_send((unsigned char)head) != 0) return -1;
  228. if (fdc_send((unsigned char)sec) != 0) return -1;
  229. if (fdc_send(0x02) != 0) return -1; /* 512 bytes/sector */
  230. if (fdc_send((unsigned char)SECTORS_PER_TRACK) != 0) return -1;
  231. if (fdc_send(0x1B) != 0) return -1; /* gap3 */
  232. if (fdc_send(0xFF) != 0) return -1; /* data length */
  233. if (fdc_wait_irq() != 0)
  234. return -2;
  235. for (i = 0; i < 7; i++) {
  236. if (fdc_recv(&results[i]) != 0)
  237. return -3;
  238. }
  239. if (results[0] & 0xC0) {
  240. io_putstr("fdc: CHS=");
  241. io_puthex(cyl);
  242. io_putchar('/');
  243. io_puthex(head);
  244. io_putchar('/');
  245. io_puthex(sec);
  246. io_putstr(" ST0=");
  247. io_puthex(results[0]);
  248. io_putstr(" ST1=");
  249. io_puthex(results[1]);
  250. io_putstr(" ST2=");
  251. io_puthex(results[2]);
  252. io_putchar('\n');
  253. return -1;
  254. }
  255. return 0;
  256. }
  257. static int fdc_read_one(unsigned int lba)
  258. {
  259. unsigned int cyl, head, sec;
  260. lba_to_chs(lba, &cyl, &head, &sec);
  261. if (cyl != cur_cylinder) {
  262. if (fdc_seek(cyl, head) != 0)
  263. return -1;
  264. }
  265. return fdc_do_read(cyl, head, sec);
  266. }
  267. /* Aggressive retry: seek far away then back to improve head alignment. */
  268. static int fdc_read_one_retry(unsigned int lba)
  269. {
  270. unsigned int cyl, head, sec;
  271. lba_to_chs(lba, &cyl, &head, &sec);
  272. /* Seek to cylinder 40 (mid-disk) first, then to target.
  273. * The long travel forces the stepper to settle accurately. */
  274. fdc_seek(cyl > 40 ? 0 : 40, 0);
  275. if (fdc_seek(cyl, head) != 0)
  276. return -1;
  277. return fdc_do_read(cyl, head, sec);
  278. }
  279. /* ---- public API ---- */
  280. static int fdc_init_once(void)
  281. {
  282. /* Mask IRQ 6 before reset so the completion interrupt doesn't
  283. * fire before we're ready (IRQ 6 may already be unmasked from
  284. * a previous init). Then clear the flag and release the reset. */
  285. outb(PIC1_DATA, inb(PIC1_DATA) | 0x40);
  286. outb(FDC_DOR, 0x00);
  287. motor_on = 0;
  288. {
  289. unsigned int t0 = get_ticks_ms();
  290. while ((get_ticks_ms() - t0) < 10)
  291. ;
  292. }
  293. fdc_irq_fired = 0;
  294. outb(FDC_DOR, (unsigned char)(0x0C | current_drive));
  295. /* Unmask IRQ 6 — the latched reset-completion interrupt fires now. */
  296. outb(PIC1_DATA, inb(PIC1_DATA) & ~0x40);
  297. if (fdc_wait_irq() != 0)
  298. return -1;
  299. {
  300. unsigned char st0, cyl;
  301. int i;
  302. for (i = 0; i < 4; i++)
  303. fdc_sense_interrupt(&st0, &cyl);
  304. outb(FDC_CCR, 0x00);
  305. if (fdc_send(CMD_SPECIFY) != 0)
  306. return -1;
  307. if (fdc_send(0x4F) != 0)
  308. return -1;
  309. if (fdc_send(0x1E) != 0)
  310. return -1;
  311. }
  312. fdc_motor_on();
  313. if (fdc_recalibrate() != 0)
  314. return -1;
  315. fdc_initialized = 1;
  316. return 0;
  317. }
  318. void disk_set_drive(unsigned int drive)
  319. {
  320. if (drive > 1)
  321. return;
  322. if (drive == current_drive && fdc_initialized)
  323. return;
  324. current_drive = drive;
  325. motor_on = 0;
  326. fdc_initialized = 0;
  327. }
  328. unsigned int disk_get_drive(void)
  329. {
  330. return current_drive;
  331. }
  332. int disk_read_sectors(unsigned int lba, void *buf, unsigned int count)
  333. {
  334. unsigned int i;
  335. if (!fdc_initialized) {
  336. if (fdc_init_once() != 0)
  337. return -1;
  338. }
  339. for (i = 0; i < count; i++) {
  340. int attempt, ok = 0;
  341. for (attempt = 0; attempt < MAX_RETRIES; attempt++) {
  342. int rc;
  343. if (attempt < 3) {
  344. rc = fdc_read_one(lba + i);
  345. } else {
  346. /* Long-seek retry: seek far away then back to
  347. * force accurate head positioning. */
  348. rc = fdc_read_one_retry(lba + i);
  349. }
  350. if (rc == 0) {
  351. ok = 1;
  352. break;
  353. }
  354. fdc_reset();
  355. fdc_motor_on();
  356. fdc_recalibrate();
  357. }
  358. if (!ok) {
  359. io_putstr("disk: read failed LBA=");
  360. io_puthex(lba + i);
  361. io_putstr(" (sector ");
  362. io_puthex(i);
  363. io_putstr(" of ");
  364. io_puthex(count);
  365. io_putstr(")\n");
  366. return -1;
  367. }
  368. {
  369. unsigned int j;
  370. unsigned char *dst = (unsigned char *)buf + (i * SECTOR_SIZE);
  371. const unsigned char *src = (const unsigned char *)DMA_BUF_ADDR;
  372. for (j = 0; j < SECTOR_SIZE; j++)
  373. dst[j] = src[j];
  374. }
  375. }
  376. return 0;
  377. }