8 files changed, 257 insertions, 40 deletions

diff --git a/Source/Kernel/Devices/ATA/ATAController.class.cpp b/Source/Kernel/Devices/ATA/ATAController.class.cpp
new file mode 100644
index 0000000..d8073d0
--- /dev/null
+++ b/Source/Kernel/Devices/ATA/ATAController.class.cpp
@@ -0,0 +1,55 @@
+#include "ATADrive.class.h"
+
+#include <DeviceManager/Dev.ns.h>
+#include <VFS/Part.ns.h>
+
+String ATAController::getClass() {
+	return "controller.ata";
+}
+
+String ATAController::getName() {
+	return String("ATA controller #") + String::number(m_number);
+}
+
+void ATAController::detect() {
+	ATAController *c[2];
+	c[0] = new ATAController(ATA_BUS1_BASE, 0);
+	c[1] = new ATAController(ATA_BUS2_BASE, 1);
+	Dev::registerDevice(c[0]); Dev::registerDevice(c[1]);
+	for (u32int d = 0; d < 2; d++) {
+		for (u32int r = 0; r < 2; r++) {
+			ATADrive* drv = c[d]->m_drives[r];
+			if (drv != 0) {
+				Dev::registerDevice(drv);
+				Part::registerDevice(drv);
+			}
+		}
+	}
+}
+
+ATAController::ATAController(u32int base, u8int number) : Mutex(MUTEX_TRUE) {
+	m_base = base; m_number = number;
+	m_drives[0] = NULL; m_drives[1] = NULL;
+	identify(false);	//Identify master device
+	identify(true);		//Identify slave device
+	unlock();
+}
+
+void ATAController::identify(bool slave) {
+	if (!locked()) return;
+	if (m_drives[(slave ? 1 : 0)] != NULL) return;
+	writeByte(ATA_PORT_DRIVE_SELECT, (slave ? 0xB0 : 0xA0)); 
+	writeByte(ATA_PORT_COMMAND, ATA_CMD_IDENTIFY);
+	u8int ret = readByte(ATA_PORT_COMMAND);
+	if (ret == 0) return;	//Drive does not exist
+	while ((ret & 0x88) != 0x08 and (ret & 1) != 1) {
+		ret = readByte(ATA_PORT_COMMAND);
+	}
+	if ((ret & 1) == 1) return;	//Error while IDENTIFY
+	u16int data[256];
+	for (u32int i = 0; i < 256; i++) data[i] = readWord(ATA_PORT_DATA);
+	u32int blocks = (data[61] << 16) | data[60];
+	if (blocks != 0) {	//Drive supports LBA28
+		m_drives[(slave ? 1 : 0)] = new ATADrive(this, slave, blocks, data);
+	}
+}
diff --git a/Source/Kernel/Devices/ATA/ATAController.class.h b/Source/Kernel/Devices/ATA/ATAController.class.h
new file mode 100644
index 0000000..89176c9
--- /dev/null
+++ b/Source/Kernel/Devices/ATA/ATAController.class.h
@@ -0,0 +1,48 @@
+#ifndef DEF_ATACONTROLLER_CLASS_H
+#define DEF_ATACONTROLLER_CLASS_H
+
+#include <Devices/Device.proto.h>
+#include <Mutex.class.h>
+
+#define ATA_BUS1_BASE 0x1F0
+#define ATA_BUS2_BASE 0x170
+
+#define ATA_PORT_DATA 0
+#define ATA_PORT_FEATURES_ERROR 1
+#define ATA_PORT_SECT_COUNT 2
+#define ATA_PORT_PARTIAL1 3
+#define ATA_PORT_PARTIAL2 4
+#define ATA_PORT_PARTIAL3 5
+#define ATA_PORT_DRIVE_SELECT 6
+#define ATA_PORT_COMMAND 7
+
+#define ATA_CMD_IDENTIFY 0xEC
+#define ATA_CMD_READ 0x20
+#define ATA_CMD_WRITE 0x30
+
+class ATADrive;
+
+class ATAController : public Device, public Mutex {
+	friend class ATADrive;
+	private:
+	ATAController(u32int base, u8int number);
+
+	u32int m_base;
+	u8int m_number;
+
+	ATADrive* m_drives[2];
+
+	void writeWord(u16int port, u16int word) { Sys::outw(m_base + port, word); }
+	void writeByte(u16int port, u8int byte) { Sys::outb(m_base + port, byte); }
+	u8int readByte(u16int port) { return Sys::inb(m_base + port); }
+	u16int readWord(u16int port) { return Sys::inw(m_base + port); }
+
+	void identify(bool slave);	//Identifies a drive and adds it to m_drives
+
+	public:
+	static void detect();
+	String getClass();
+	String getName();
+};
+
+#endif
diff --git a/Source/Kernel/Devices/ATA/ATADrive.class.cpp b/Source/Kernel/Devices/ATA/ATADrive.class.cpp
new file mode 100644
index 0000000..247e798
--- /dev/null
+++ b/Source/Kernel/Devices/ATA/ATADrive.class.cpp
@@ -0,0 +1,75 @@
+#include "ATADrive.class.h"
+
+String ATADrive::getClass() {
+	return "block.ata";
+}
+
+String ATADrive::getName() {
+	return String("ATA drive ") += String::number(m_ctrlr->m_number) += String(m_isSlave ? ".slave " : ".master ")
+		+= String::number(m_blockCount / (1024 * 1024 / blockSize())) += "MB";
+}
+
+ATADrive::ATADrive(ATAController* ctrlr, bool isSlave, u32int blockCount, u16int* identifyData) {
+	m_ctrlr = ctrlr;
+	m_isSlave = isSlave;
+	m_blockCount = blockCount;
+	for (u32int i = 0; i < 256; i++) m_identifyData[i] = identifyData[i];
+}
+
+bool ATADrive::readOnly() {
+	return true;
+}
+
+u64int ATADrive::blocks() {
+	return m_blockCount;
+}
+
+void ATADrive::cmdCommon(u32int numblock, u32int count) {
+	m_ctrlr->writeByte(ATA_PORT_FEATURES_ERROR, 0);
+	m_ctrlr->writeByte(ATA_PORT_SECT_COUNT, count);
+	m_ctrlr->writeByte(ATA_PORT_PARTIAL1, numblock);
+	m_ctrlr->writeByte(ATA_PORT_PARTIAL2, numblock >> 8);
+	m_ctrlr->writeByte(ATA_PORT_PARTIAL3, numblock >> 16);
+
+	m_ctrlr->writeByte(ATA_PORT_DRIVE_SELECT, 0xE0 | (m_isSlave ? 0x10 : 0) | ((numblock >> 24) & 0x0F));
+}
+
+bool ATADrive::readBlocks(u64int start, u32int count, u8int* data) {
+	if (start + count >= m_blockCount) return false;
+
+	m_ctrlr->waitLock();
+	cmdCommon(start, count);
+	m_ctrlr->writeByte(ATA_PORT_COMMAND, ATA_CMD_READ);
+
+	while (!(m_ctrlr->readByte(ATA_PORT_COMMAND) & 0x08));	//Wait for the drive to be ready
+
+	for (u32int idx = 0; idx < count * 256; idx++) {
+		u16int tmpword = m_ctrlr->readWord(ATA_PORT_DATA);
+		data[idx * 2] = (u8int)tmpword;
+		data[idx * 2 + 1] = (u8int)(tmpword >> 8);
+	}
+	m_ctrlr->unlock();
+	return true;	
+}
+
+bool ATADrive::writeBlocks(u64int start, u32int count, u8int* data) {
+	if (start + count >= m_blockCount) return false;
+	if (readOnly()) return false;
+
+	m_ctrlr->waitLock();
+	cmdCommon(start, count);
+	m_ctrlr->writeByte(ATA_PORT_COMMAND, ATA_CMD_WRITE);
+
+	while (!(m_ctrlr->readByte(ATA_PORT_COMMAND) & 0x08));	//Wait for the drive to be ready
+
+	for (u32int idx = 0; idx < count * 256; idx++) {
+		u16int tmpword = (data[idx * 2]) | (data[idx * 2 + 1] << 8); 
+		m_ctrlr->writeByte(ATA_PORT_DATA, tmpword);
+	}
+	m_ctrlr->unlock();
+	return true;	
+}
+
+u32int ATADrive::blockSize() {
+	return 512;
+}
diff --git a/Source/Kernel/Devices/ATA/ATADrive.class.h b/Source/Kernel/Devices/ATA/ATADrive.class.h
new file mode 100644
index 0000000..b6087bf
--- /dev/null
+++ b/Source/Kernel/Devices/ATA/ATADrive.class.h
@@ -0,0 +1,30 @@
+#ifndef DEF_ATADRIVE_CLASS_H
+#define DEF_ATADRIVE_CLASS_H
+
+#include "ATAController.class.h"
+#include <Devices/BlockDevice.proto.h>
+
+class ATADrive : public BlockDevice {
+	friend class ATAController;
+	private:
+	ATADrive(ATAController* ctrlr, bool isSlave, u32int blockCount, u16int* identifyData);
+
+	ATAController* m_ctrlr;
+	bool m_isSlave;
+	u32int m_blockCount;
+	u16int m_identifyData[256];
+
+	void cmdCommon(u32int numblock, u32int count);
+
+	public:
+	String getClass();
+	String getName();
+
+	bool readOnly();
+	u64int blocks();
+	bool readBlocks(u64int start, u32int count, u8int* data);
+	bool writeBlocks(u64int start, u32int count, u8int* data);
+	u32int blockSize();
+};
+
+#endif
diff --git a/Source/Kernel/Devices/Floppy/FloppyController.class.cpp b/Source/Kernel/Devices/Floppy/FloppyController.class.cpp
index 146ce28..7201c5c 100644
--- a/Source/Kernel/Devices/Floppy/FloppyController.class.cpp
+++ b/Source/Kernel/Devices/Floppy/FloppyController.class.cpp
@@ -96,6 +96,7 @@ FloppyController::FloppyController(u32int base, u8int irq) : m_driveMutex(false)
 	m_drives[0] = NULL;
 	m_drives[1] = NULL;
 	m_first = false;
+	Dev::requestIRQ(this, m_irq);
 }
 
 void FloppyController::detect() {	//TODO : do this better
@@ -144,16 +145,14 @@ void FloppyController::setDOR() {
 		dor |= 0x10;
 	if (m_drives[1] != NULL and m_drives[1]->m_motorState != 0)
 		dor |= 0x20;
-	asm volatile ("cli");
+	resetIrq();
 	outb(m_base + FR_DOR, dor);
 	if (m_first) {	//First time we set the DOR, controller initialized
-		Task::currThread()->waitIRQ(m_irq);
+		waitIrq();
 		int st0, cyl;
 		checkInterrupt(&st0, &cyl);
 		m_first = false;
 	}
-	asm volatile ("sti");
-			//PANIC("test");
 }
 
 void FloppyController::setActiveDrive(u8int drive) {
@@ -190,10 +189,10 @@ u8int FloppyController::readData() {
 bool FloppyController::reset() {
 	outb(m_base + FR_DOR, 0x00);	//Disable controller
 	m_first = true;
-	setNoActiveDrive();
 
 	if (m_drives[0] != NULL) m_drives[0]->m_motorState = 0;
 	if (m_drives[1] != NULL) m_drives[1]->m_motorState = 0;
+	setNoActiveDrive();
 
 	for (int i = 0; i < 2; i++) {
 		if (m_drives[i] != NULL) {
@@ -203,3 +202,11 @@ bool FloppyController::reset() {
 	}
 	return true;
 }
+
+void FloppyController::handleIRQ(registers_t regs, int irq) {
+	m_irqHappened = true;
+}
+
+void FloppyController::waitIrq() {
+	while (!m_irqHappened) Task::currThread()->sleep(10);
+}
diff --git a/Source/Kernel/Devices/Floppy/FloppyController.class.h b/Source/Kernel/Devices/Floppy/FloppyController.class.h
index a27d853..f0a7c10 100644
--- a/Source/Kernel/Devices/Floppy/FloppyController.class.h
+++ b/Source/Kernel/Devices/Floppy/FloppyController.class.h
@@ -60,6 +60,8 @@ class FloppyController : public Device {
 
 	bool m_first;
 
+	bool m_irqHappened;
+
 	FloppyDrive* m_drives[2];
 
 	void checkInterrupt(int *st0, int *cyl);
@@ -69,6 +71,10 @@ class FloppyController : public Device {
 	bool writeCmd(u8int cmd);			//Sends command to controller
 	u8int readData();					//Reads a byte from controller
 	bool reset();
+
+	void handleIRQ(registers_t regs, int irq);
+	void resetIrq() { m_irqHappened = false; }
+	void waitIrq();
 	
 	public:
 	static void detect();
diff --git a/Source/Kernel/Devices/Floppy/FloppyDrive.class.cpp b/Source/Kernel/Devices/Floppy/FloppyDrive.class.cpp
index 7edf24a..b7fd69b 100644
--- a/Source/Kernel/Devices/Floppy/FloppyDrive.class.cpp
+++ b/Source/Kernel/Devices/Floppy/FloppyDrive.class.cpp
@@ -2,6 +2,7 @@
 #include "FloppyController.class.h"
 #include <TaskManager/Task.ns.h>
 #include <DeviceManager/Time.ns.h>
+#include <VTManager/SimpleVT.class.h>
 #include <Core/Log.ns.h>
 
 using namespace Sys;
@@ -91,16 +92,18 @@ bool FloppyDrive::calibrate() {
 
 	int st0, cyl = -1;
 
-	if (!setMotorState(true)) return false;
+	if (!setMotorState(true)) {
+		*kvt << getName() << ": calibrate fail\n";
+		return false;
+	}
 
 	for (int i = 0; i < 10; i++) {
-		asm volatile("cli");
+		m_fdc->resetIrq();
 		m_fdc->writeCmd(FC_RECALIBRATE);
 		m_fdc->writeCmd(m_driveNumber);
 
-		Task::currThread()->waitIRQ(m_fdc->m_irq);
+		m_fdc->waitIrq();
 		m_fdc->checkInterrupt(&st0, &cyl);
-		asm volatile("sti");
 
 		if (st0 & 0xC0) {
 			continue;
@@ -111,6 +114,7 @@ bool FloppyDrive::calibrate() {
 		}
 	}
 	setMotorState(false);
+	*kvt << getName() << ": calibrate fail\n";
 	return false;
 }
 
@@ -137,47 +141,44 @@ bool FloppyDrive::killMotor() {
 	return true;
 }
 
-bool FloppyDrive::seek(u32int cyli, s32int head) {
+bool FloppyDrive::seek(u32int cyli, s32int head, bool recursive) {
 	if (cyli >= m_cylinders) return false;
-	m_fdc->setActiveDrive(m_driveNumber);
 
 	setMotorState(true);	//Turn on motor
 
 	int st0, cyl = -1;
 
-	for (u32int i = 0; i < 10; i++) {
-		asm volatile ("cli");
+	for (u32int i = 0; i < (recursive ? 10 : 5); i++) {
+		m_fdc->resetIrq();
 		m_fdc->writeCmd(FC_SEEK);
-		m_fdc->writeCmd(head << 2);
+		m_fdc->writeCmd(head << 2 | m_driveNumber);
 		m_fdc->writeCmd(cyl);
 
-		Task::currThread()->waitIRQ(m_fdc->m_irq);
+		m_fdc->waitIrq();
 		m_fdc->checkInterrupt(&st0, &cyl);
-		asm volatile("sti");
 
 		if (st0 & 0xC0) { //Error
 			continue;
 		}
-		if (cyl == 0xFF or cyl == 0x00) {	//0xFF for bochs, 0x00 for qemu :D
-			setMotorState(false);
-			m_fdc->setNoActiveDrive();
-			return true;
-		}
-		if (cyl == (int)cyli) {
+		if (cyl == 0xFF or cyl == 0x00 or cyl == (int)cyli) {	//0xFF for bochs, 0x00 for qemu :D
 			setMotorState(false);
-			m_fdc->setNoActiveDrive();
 			return true;
 		}
 	}
-	setMotorState(false);
-	m_fdc->setNoActiveDrive();
-	return false;
+	if (recursive) {
+		setMotorState(false);
+		*kvt << getName() << ": seek fail\n";
+		return false;
+	} else {
+		calibrate();
+		return seek(cyli, head, true);
+	}
 }
 
 bool FloppyDrive::doTrack(u32int cyl, u8int dir) {
+	m_fdc->setActiveDrive(m_driveNumber);
 	if (!seek(cyl, 0)) return false;
 	if (!seek(cyl, 1)) return false;
-	m_fdc->setActiveDrive(m_driveNumber);
 
 	u8int cmd, flags = 0xC0;
 	switch (dir) {
@@ -203,7 +204,7 @@ bool FloppyDrive::doTrack(u32int cyl, u8int dir) {
 
 		Task::currThread()->sleep(100);
 
-		asm volatile("cli");
+		m_fdc->resetIrq();
 		m_fdc->writeCmd(cmd);
 		m_fdc->writeCmd(m_driveNumber);	//Drive number & first head << 2
 		m_fdc->writeCmd(cyl);	//Cylinder
@@ -214,7 +215,7 @@ bool FloppyDrive::doTrack(u32int cyl, u8int dir) {
 		m_fdc->writeCmd(0x1B);
 		m_fdc->writeCmd(0xFF);
 
-		Task::currThread()->waitIRQ(m_fdc->m_irq);
+		m_fdc->waitIrq();
 
 		u8int st0, st1, st2, rcy, rhe, rse, bps;
 		st0 = m_fdc->readData();
@@ -224,7 +225,6 @@ bool FloppyDrive::doTrack(u32int cyl, u8int dir) {
 		rhe = m_fdc->readData();
 		rse = m_fdc->readData();
 		bps = m_fdc->readData();
-		asm volatile("sti");
 
 		int error = 0;
 	
@@ -274,12 +274,11 @@ bool FloppyDrive::doTrack(u32int cyl, u8int dir) {
 
 bool FloppyDrive::readOnly() {
 	m_fdc->setActiveDrive(m_driveNumber);
-	asm volatile("cli");
+	m_fdc->resetIrq();
 	m_fdc->writeCmd(FC_SENSE_DRIVE_STATUS);
 	m_fdc->writeCmd(m_driveNumber);
-	Task::currThread()->waitIRQ(m_fdc->m_irq);
+	m_fdc->waitIrq();
 	u8int st3 = m_fdc->readData();
-	asm volatile("sti");
 
 	bool ret = (st3 & 0x80 ? true : false);
 
@@ -296,16 +295,13 @@ bool FloppyDrive::readBlocks(u64int start, u32int count, u8int *data) {
 	u32int startblock = start;
 	if (count == 1) {
 		u32int cylinder = (startblock / (m_sectors * 2)), offset = (startblock % (m_sectors * 2)) * 512;
-		if (m_buffCyl == cylinder && m_buffTime >= Time::uptime() - 4) {
-			memcpy(data, (const u8int*)(&m_buffer[offset]), 512);
-			return true;
-		} else {
+		if (m_buffCyl != cylinder or m_buffTime < Time::uptime() - 4) {
 			if (!doTrack(cylinder, FD_READ)) return false;
 			m_buffCyl = cylinder;
 			m_buffTime = Time::uptime();
-			memcpy(data, (const u8int*)(&m_buffer[offset]), 512);
-			return true;
 		}
+		memcpy(data, (const u8int*)(&m_buffer[offset]), 512);
+		return true;
 	} else {
 		m_buffCyl = 0xFFFF;	//Invalid cylinder
 		m_buffTime = 0;
diff --git a/Source/Kernel/Devices/Floppy/FloppyDrive.class.h b/Source/Kernel/Devices/Floppy/FloppyDrive.class.h
index 75926ea..73229b1 100644
--- a/Source/Kernel/Devices/Floppy/FloppyDrive.class.h
+++ b/Source/Kernel/Devices/Floppy/FloppyDrive.class.h
@@ -22,7 +22,7 @@ class FloppyDrive : public BlockDevice {
 	bool calibrate();
 	bool setMotorState(bool on);
 	bool killMotor();
-	bool seek(u32int cyli, s32int head);
+	bool seek(u32int cyli, s32int head, bool recursive = false);
 	bool doTrack(u32int cyl, u8int dir);
 
 	public: