amd64/ata: write support

3 files changed, 62 insertions, 1 deletions

diff --git a/src/kernel/arch/amd64/ata.c b/src/kernel/arch/amd64/ata.c
index fa55335..4c1f2e2 100644
--- a/src/kernel/arch/amd64/ata.c
+++ b/src/kernel/arch/amd64/ata.c
@@ -1,7 +1,9 @@
+/* http://hddguru.com/documentation/2006.01.27-ATA-ATAPI-7/ */
 #include <kernel/arch/amd64/ata.h>
 #include <kernel/arch/amd64/port_io.h>
 #include <kernel/panic.h>
 #include <kernel/util.h>
+#include <shared/mem.h>
 #include <stdbool.h>
 
 static struct {
@@ -171,3 +173,50 @@ int ata_read(int drive, void *buf, size_t len, size_t off) {
 	}
 	return len;
 }
+
+static void ata_rawwrite(int drive, const void *buf, uint32_t lba, uint32_t cnt) {
+	int iobase = ata_iobase(drive);
+	ata_driveselect(drive, lba);
+	port_out8(iobase + FEAT, 0);
+	port_out8(iobase + SCNT, cnt);
+	port_out8(iobase + LBAlo, lba);
+	port_out8(iobase + LBAmid, lba >> 8);
+	port_out8(iobase + LBAhi, lba >> 16);
+	port_out8(iobase + CMD, 0x30); /* WRITE SECTORS */
+
+	for (uint32_t i = 0; i < cnt; i++) {
+		ata_poll(drive, -1);
+		for (int j = 0; j < 256; j++) {
+			port_out16(iobase, ((uint16_t*)buf)[i * 256 + j]);
+		}
+	}
+}
+
+int ata_write(int drive, const void *buf, size_t len, size_t off) {
+	char sec[512];
+	size_t clen;
+	if (ata_drives[drive].type != DEV_PATA) {
+		panic_unimplemented();
+	}
+	if (off & 511) {
+		clen = min(len, 512 - off % 512);
+		ata_read(drive, sec, 512, off / 512);
+		memcpy(sec + off % 512, buf, clen);
+		ata_rawwrite(drive, sec, off / 512, 1);
+		off += clen,
+		buf += clen;
+		len -= clen;
+	}
+	if (512 <= len) {
+		ata_rawwrite(drive, buf, off / 512, len / 512);
+	}
+	if (len & 511) {
+		buf += len & ~511;
+		off += len & ~511;
+		len = len & 511;
+		ata_read(drive, sec, 512, off / 512);
+		memcpy(sec, buf, len);
+		ata_rawwrite(drive, sec, off / 512, 1);
+	}
+	return 0;
+}
diff --git a/src/kernel/arch/amd64/ata.h b/src/kernel/arch/amd64/ata.h
index c43ac23..924cabd 100644
--- a/src/kernel/arch/amd64/ata.h
+++ b/src/kernel/arch/amd64/ata.h
@@ -7,3 +7,4 @@ void ata_init(void);
 bool ata_available(int drive);
 size_t ata_size(int drive);
 int ata_read(int drive, void *buf, size_t len, size_t off);
+int ata_write(int drive, const void *buf, size_t len, size_t off);
diff --git a/src/kernel/arch/amd64/driver/pata.c b/src/kernel/arch/amd64/driver/pata.c
index 77b654c..3d5e3c6 100644
--- a/src/kernel/arch/amd64/driver/pata.c
+++ b/src/kernel/arch/amd64/driver/pata.c
@@ -56,7 +56,18 @@ static void accept(struct vfs_request *req) {
 			break;
 
 		case VFSOP_WRITE:
-			panic_unimplemented();
+			if (id == root_id) {
+				vfsreq_finish_short(req, -EACCES);
+				break;
+			}
+			fs_normslice(&req->offset, &req->input.len, ata_size(id), false);
+			len = min(req->input.len, sizeof wbuf);
+			if (len != 0) {
+				virt_cpy_from(req->caller->pages, wbuf, req->input.buf, len);
+				ata_write(id, wbuf, len, req->offset);
+			}
+			vfsreq_finish_short(req, len);
+			break;
 
 		case VFSOP_GETSIZE:
 			if (id == root_id) {