Răsfoiți Sursa

Add disk_write_sectors (FDC WRITE + ISA DMA write mode)

Johannes Zellner 1 lună în urmă
părinte
comite
58e4853244
2 a modificat fișierele cu 146 adăugiri și 1 ștergeri
  1. 142 1
      disk.c
  2. 4 0
      disk.h

+ 142 - 1
disk.c

@@ -1,4 +1,4 @@
-/* Disk layer: read sectors via direct FDC + ISA DMA in protected mode. */
+/* Disk layer: read/write sectors via direct FDC + ISA DMA in protected mode. */
 
 #include "disk.h"
 #include "io.h"
@@ -33,6 +33,7 @@
 #define CMD_RECALIBRATE  0x07
 #define CMD_SENSE_INT    0x08
 #define CMD_READ_DATA    0x66   /* READ | MFM | SK  (no Multi-Track) */
+#define CMD_WRITE_DATA   0xC5   /* WRITE | MFM */
 #define CMD_SEEK         0x0F
 
 #define MOTOR_DELAY_MS   300
@@ -248,6 +249,22 @@ static void dma_setup_read(unsigned int phys, unsigned int len)
 	outb(DMA_SINGLEMASK, 0x02);        /* unmask channel 2 */
 }
 
+static void dma_setup_write(unsigned int phys, unsigned int len)
+{
+	unsigned int count = len - 1;
+
+	outb(DMA_SINGLEMASK, 0x06);        /* mask channel 2 */
+	outb(DMA_FLIPFLOP, 0xFF);         /* reset flip-flop */
+	outb(DMA_CH2_ADDR, (unsigned char)(phys & 0xFF));
+	outb(DMA_CH2_ADDR, (unsigned char)((phys >> 8) & 0xFF));
+	outb(DMA_CH2_PAGE, (unsigned char)((phys >> 16) & 0xFF));
+	outb(DMA_FLIPFLOP, 0xFF);         /* reset flip-flop */
+	outb(DMA_CH2_COUNT, (unsigned char)(count & 0xFF));
+	outb(DMA_CH2_COUNT, (unsigned char)((count >> 8) & 0xFF));
+	outb(DMA_MODE, 0x4A);             /* single, increment, read (mem->dev), ch2 */
+	outb(DMA_SINGLEMASK, 0x02);        /* unmask channel 2 */
+}
+
 /* ---- read one sector ---- */
 
 static void lba_to_chs(unsigned int lba,
@@ -306,6 +323,53 @@ static int fdc_do_read(unsigned int cyl, unsigned int head, unsigned int sec)
 	return 0;
 }
 
+static int fdc_do_write(unsigned int cyl, unsigned int head, unsigned int sec)
+{
+	unsigned char results[7];
+	int i;
+
+	dma_setup_write(DMA_BUF_ADDR, SECTOR_SIZE);
+
+	fdc_irq_fired = 0;
+
+	if (fdc_send(CMD_WRITE_DATA) != 0)   return -1;
+	if (fdc_send((unsigned char)((head << 2) | current_drive)) != 0) return -1;
+	if (fdc_send((unsigned char)cyl) != 0)  return -1;
+	if (fdc_send((unsigned char)head) != 0) return -1;
+	if (fdc_send((unsigned char)sec) != 0)  return -1;
+	if (fdc_send(0x02) != 0)            return -1; /* 512 bytes/sector */
+	if (fdc_send((unsigned char)SECTORS_PER_TRACK) != 0) return -1;
+	if (fdc_send(0x1B) != 0)            return -1; /* gap3 */
+	if (fdc_send(0xFF) != 0)            return -1; /* data length */
+
+	if (fdc_wait_irq() != 0)
+		return -2;
+
+	for (i = 0; i < 7; i++) {
+		if (fdc_recv(&results[i]) != 0)
+			return -3;
+	}
+
+	if (results[0] & 0xC0) {
+		io_putstr("fdc write: CHS=");
+		io_puthex(cyl);
+		io_putchar('/');
+		io_puthex(head);
+		io_putchar('/');
+		io_puthex(sec);
+		io_putstr(" ST0=");
+		io_puthex(results[0]);
+		io_putstr(" ST1=");
+		io_puthex(results[1]);
+		io_putstr(" ST2=");
+		io_puthex(results[2]);
+		io_putchar('\n');
+		return -1;
+	}
+
+	return 0;
+}
+
 static int fdc_read_one(unsigned int lba)
 {
 	unsigned int cyl, head, sec;
@@ -320,6 +384,20 @@ static int fdc_read_one(unsigned int lba)
 	return fdc_do_read(cyl, head, sec);
 }
 
+static int fdc_write_one(unsigned int lba)
+{
+	unsigned int cyl, head, sec;
+
+	lba_to_chs(lba, &cyl, &head, &sec);
+
+	if (cyl != cur_cylinder) {
+		if (fdc_seek(cyl, head) != 0)
+			return -1;
+	}
+
+	return fdc_do_write(cyl, head, sec);
+}
+
 /* Aggressive retry: seek far away then back to improve head alignment. */
 static int fdc_read_one_retry(unsigned int lba)
 {
@@ -336,6 +414,19 @@ static int fdc_read_one_retry(unsigned int lba)
 	return fdc_do_read(cyl, head, sec);
 }
 
+static int fdc_write_one_retry(unsigned int lba)
+{
+	unsigned int cyl, head, sec;
+
+	lba_to_chs(lba, &cyl, &head, &sec);
+
+	fdc_seek(cyl > 40 ? 0 : 40, 0);
+	if (fdc_seek(cyl, head) != 0)
+		return -1;
+
+	return fdc_do_write(cyl, head, sec);
+}
+
 /* ---- public API ---- */
 
 static int fdc_init_once(void)
@@ -450,3 +541,53 @@ int disk_read_sectors(unsigned int lba, void *buf, unsigned int count)
 	}
 	return 0;
 }
+
+int disk_write_sectors(unsigned int lba, const void *buf, unsigned int count)
+{
+	unsigned int i;
+
+	if (!fdc_initialized) {
+		if (fdc_init_once() != 0)
+			return -1;
+	}
+
+	for (i = 0; i < count; i++) {
+		int attempt, ok = 0;
+
+		/* Copy the sector to write into the ISA DMA buffer first. */
+		{
+			unsigned int j;
+			const unsigned char *src = (const unsigned char *)buf + (i * SECTOR_SIZE);
+			volatile unsigned char *dst = (volatile unsigned char *)DMA_BUF_ADDR;
+			for (j = 0; j < SECTOR_SIZE; j++)
+				dst[j] = src[j];
+		}
+
+		for (attempt = 0; attempt < MAX_RETRIES; attempt++) {
+			int rc;
+			if (attempt < 3) {
+				rc = fdc_write_one(lba + i);
+			} else {
+				rc = fdc_write_one_retry(lba + i);
+			}
+			if (rc == 0) {
+				ok = 1;
+				break;
+			}
+			fdc_reset();
+			fdc_motor_on();
+			fdc_recalibrate();
+		}
+		if (!ok) {
+			io_putstr("disk: write failed LBA=");
+			io_puthex(lba + i);
+			io_putstr(" (sector ");
+			io_puthex(i);
+			io_putstr(" of ");
+			io_puthex(count);
+			io_putstr(")\n");
+			return -1;
+		}
+	}
+	return 0;
+}

+ 4 - 0
disk.h

@@ -14,4 +14,8 @@ unsigned int disk_get_drive(void);
  * Returns 0 on success, -1 on error. */
 int disk_read_sectors(unsigned int lba, void *buf, unsigned int count);
 
+/* Write count sectors from buf starting at lba.
+ * Returns 0 on success, -1 on error. */
+int disk_write_sectors(unsigned int lba, const void *buf, unsigned int count);
+
 #endif