From 1a337881f3b6744510018e8b28d87d8f0ea1b74c Mon Sep 17 00:00:00 2001
From: Mathieu Maret <mathieu.maret@gmail.com>
Date: Tue, 5 Oct 2021 23:10:10 +0200
Subject: [PATCH] ATA Add Read function

---
 drivers/ata.c | 41 +++++++++++++++++++++++++++++++++++++++--
 drivers/ata.h | 18 +++++++++++++++++-
 2 files changed, 56 insertions(+), 3 deletions(-)

diff --git a/drivers/ata.c b/drivers/ata.c
index 9d2264e..fd48c64 100644
--- a/drivers/ata.c
+++ b/drivers/ata.c
@@ -383,17 +383,19 @@ static struct ata_controller controllers[MAX_ATA_CONTROLLER] = {{
 								    .base    = 0x1F0,
 								    .dev_ctl = 0x3F6,
 								    .present = 0,
+								    .last_device_used = -1,
 								},
 								{
 								    .id      = 1,
 								    .base    = 0x170,
 								    .dev_ctl = 0x376,
 								    .present = 0,
+								    .last_device_used = -1,
 								}};
 int ATADetectDevice(struct ata_device *dev)
 {
 	struct ata_controller *ctl = dev->ctl;
-	outb(ctl->base + ATA_PIO_DRIVE, 0xA0 | dev->isSlave << 4);
+	outb(ctl->base + ATA_PIO_DRIVE, ATA_PIO_DRIVE_IBM | dev->isSlave? ATA_PIO_DRIVE_SLAVE: ATA_PIO_DRIVE_MASTER);
 	unsigned st = inb(ctl->base + ATA_PIO_STATUS);
 	if (st & ATA_PIO_STATUS_DRIVE_BUSY)
 		goto no_disk;
@@ -425,7 +427,7 @@ int ATAGetDeviceInfo(struct ata_device *dev)
 	struct ata_controller *ctl = dev->ctl;
 	struct _IDENTIFY_DEVICE_DATA *deviceInfo;
 
-	outb(ctl->base + ATA_PIO_DRIVE, 0xA0 | dev->isSlave << 4);
+	outb(ctl->base + ATA_PIO_DRIVE, ATA_PIO_DRIVE_IBM | dev->isSlave? ATA_PIO_DRIVE_SLAVE: ATA_PIO_DRIVE_MASTER);
 	outb(ctl->base + ATA_PIO_CMD, ATA_PIO_CMD_IDENTIFY);
 
 	/* Wait for command completion (wait while busy bit is set) */
@@ -456,11 +458,13 @@ int ATAGetDeviceInfo(struct ata_device *dev)
 
 int ATADetectController(struct ata_controller *controller)
 {
+
 	controller->devices[0].id      = 0;
 	controller->devices[0].isSlave = 0;
 	controller->devices[0].ctl     = controller;
 	controller->devices[0].type    = ATADetectDevice(&controllers->devices[0]);
 
+
 	if (controller->devices[0].type == ATA_DEV_PATA ||
 	    controller->devices[0].type == ATA_DEV_SATA) {
 		ATAGetDeviceInfo(&controller->devices[0]);
@@ -486,3 +490,36 @@ int ATAInit()
 	}
 	return 0;
 }
+
+int ATARead(struct ata_device *dev, int lba, int size, void *buf)
+{
+	struct ata_controller *ctl = dev->ctl;
+	uint nb_block = DIV_ROUND_UP(size, DISK_SECTOR_SIZE);
+
+	mutexLock(&ctl->mutex);
+
+	if (ctl->last_device_used != dev->id) {
+		outb(ctl->base + ATA_PIO_DRIVE, ATA_PIO_DRIVE_IBM | dev->isSlave << 4);
+		while(inb(ctl->base +ATA_PIO_STATUS)&ATA_PIO_STATUS_DRIVE_BUSY);
+		ctl->last_device_used = dev->id;
+	}
+	outb(ctl->base + ATA_PIO_DRIVE, lba >> 24 | ATA_PIO_DRIVE_IBM | ATA_PIO_DRIVE_LBA);
+	outb(ctl->base + ATA_PIO_SEC_COUNT, nb_block);
+	outb(ctl->base + ATA_PIO_LBALO, lba & 0xff);
+	outb(ctl->base + ATA_PIO_LBAMID, (lba >> 8) & 0xff);
+	outb(ctl->base + ATA_PIO_LBAHI, (lba >> 16) & 0xff);
+
+	uint16_t *ptr = (uint16_t *)buf;
+	for(uint block = 0; block < nb_block ; block ++){
+		while(inb(ctl->base +ATA_PIO_STATUS)&ATA_PIO_STATUS_DRIVE_BUSY);
+		for(int i = 0; i< 256 && size >0; i++, size --){
+			*ptr = inw(ctl->base +ATA_PIO_DATA);
+			ptr++;
+		}
+
+	}
+
+	mutexUnlock(&ctl->mutex);
+
+	return 0;
+}
diff --git a/drivers/ata.h b/drivers/ata.h
index 2988ad2..2afbc80 100644
--- a/drivers/ata.h
+++ b/drivers/ata.h
@@ -1,5 +1,6 @@
 #pragma once
 #include "stdarg.h"
+#include "synchro.h"
 
 #define ATA_PIO_DATA 0
 #define ATA_PIO_ERR 1
@@ -16,7 +17,20 @@
 
 // cmd details
 // https://people.freebsd.org/~imp/asiabsdcon2015/works/d2161r5-ATAATAPI_Command_Set_-_3.pdf
-#define ATA_PIO_CMD_IDENTIFY 0xEC
+#define ATA_PIO_CMD_IDENTIFY 0xEC	/* get ATA params */
+#define ATA_PIO_CMD_ATAPI_IDENTIFY 0xA1 /* get ATAPI params*/
+#define ATA_PIO_CMD_READ 0x20           /* read command */
+#define ATA_PIO_CMD_WRITE 0x30          /* write command */
+#define ATA_PIO_CMD_READ_MULTI 0xC4     /* read multi command */
+#define ATA_PIO_CMD_WRITE_MULTI 0xC5    /* write multi command */
+#define ATA_PIO_CMD_SET_MULTI 0xC6      /* set multi size command */
+#define ATA_PIO_CMD_PACKET_CMD 0xA0     /* set multi size command */
+
+#define         ATA_PIO_DRIVE_IBM               0xA0    /* bits that must be set */
+#define         ATA_PIO_DRIVE_LBA               0x40    /* use LBA ? */
+#define         ATA_PIO_DRIVE_MASTER            0x00    /* select master */
+#define         ATA_PIO_DRIVE_SLAVE             0x10    /* select slave */
+
 
 #define ATA_PIO_ERROR_ADDR_MARK_NOT_FOUND 0
 #define ATA_PIO_ERROR_TRACK_ZERO_NOT_FOUND 1
@@ -65,6 +79,8 @@ struct ata_controller {
 	int16_t dev_ctl;
 	int present;
 	struct ata_device devices[MAX_ATA_DEVICES];
+	struct mutex mutex;
+	int last_device_used;
 };
 
 int ATAInit();