4 files changed, 657 insertions, 0 deletions
diff --git a/Source/Kernel/Devices/Floppy/FloppyController.class.cpp b/Source/Kernel/Devices/Floppy/FloppyController.class.cpp
new file mode 100644
index 0000000..bafc0a6
--- /dev/null
+++ b/Source/Kernel/Devices/Floppy/FloppyController.class.cpp
@@ -0,0 +1,205 @@
+#include "FloppyController.class.h"
+#include "FloppyDrive.class.h"
+#include <TaskManager/Task.ns.h>
+#include <DeviceManager/Dev.ns.h>
+#include <VFS/Part.ns.h>
+
+using namespace Sys;	//For outb/inb
+
+//***********************************************************
+//				STATIC, FOR DMA
+//***********************************************************
+Mutex FloppyController::dmaMutex(false);
+u8int FloppyController::dmabuff[FLOPPY_DMALEN]
+	__attribute__((aligned(0x8000)));
+
+bool FloppyController::dmaInit(u8int direction, u32int length) {
+	dmaMutex.waitLock();
+
+	union {
+		u8int b[4];
+		u32int l;
+	} a, c;			//Address, count
+
+	//We want the physical address of dmabuff. We simply remove 0xC0000000, because we've mapped memory linearly.
+	a.l = (u32int) &dmabuff - 0xC0000000;
+	c.l = (u32int) length - 1;
+
+	if (
+			(a.l >> 24) ||		//Address must be under 16mb
+			(c.l >> 16) ||		//Count must be < 64k
+			(((a.l & 0xFFFF) + c.l) >> 16)	//We must not cross a 64k boundary
+	   ) {			//Something is wrong
+		dmaMutex.unlock();
+		return false;
+	}
+
+	u8int mode;
+	switch (direction) {
+		case FD_READ:
+			mode = 0x46;
+			break;
+		case FD_WRITE:
+			mode = 0x4A;
+			break;
+		default:		//Invalid direction
+			dmaMutex.unlock();
+			return false;
+	}
+
+	outb(0x0a, 0x06);	//Mask chan 2
+
+	outb(0x0c, 0xff);	//Reset flip-flop
+	outb(0x04, a.b[0]);	//Address low
+	outb(0x04, a.b[1]);	//Address high
+
+	outb(0x81, a.b[2]);	//External page register
+
+	outb(0x0c, 0xff);	//Reset flip-flop
+	outb(0x05, c.b[0]);	//Count low
+	outb(0x05, c.b[1]);	//Count high
+
+	outb(0x0b, mode);	//Mode
+
+	outb(0x0a, 0x02);	//Unmask chan 2
+
+	return true;
+}
+
+void FloppyController::dmaRelease() {
+	dmaMutex.unlock();
+}
+
+//*********************************************************
+//				FOR THE CONTROLLER
+//*********************************************************
+u32int floppyMotorTimer() {	//This will be an independant thread
+	while(1) {
+		Task::currentThread->sleep(1000);	//Check only every second
+		Vector<Device*> floppys = Dev::findDevices("block.floppy");
+		for (u32int i = 0; i < floppys.size(); i++) {
+			FloppyDrive* f = (FloppyDrive*)floppys[i];
+			if (f->m_motorTimeout > 0 && f->m_motorState == FS_MOTORWAIT) {
+				f->m_motorTimeout--;
+				if (f->m_motorTimeout == 0)
+					f->killMotor();
+			}
+		}
+	}
+	return 0;
+}
+
+FloppyController::FloppyController(u32int base, u8int irq) : m_driveMutex(false) {
+	m_activeDrive = 0;
+	m_base = base;
+	m_irq = irq;
+	m_drives[0] = NULL;
+	m_drives[1] = NULL;
+	m_first = false;
+}
+
+void FloppyController::detect() {	//TODO : do this better
+	FloppyController *fdc = new FloppyController(0x03F0, 6);	//Standard controller, IRQ6 and base port 0x03F0
+	Dev::registerDevice(fdc);
+
+	outb(0x70, 0x10);	//CMOS detect
+	u8int drives = inb(0x71);
+
+	u8int fdd0 = (drives >> 4), fdd1 = (drives & 0x0F);
+	
+	if (fdd0 != FT_NONE)
+		Dev::registerDevice(new FloppyDrive(fdc, 0, fdd0));
+	if (fdd1 != FT_NONE)
+		Dev::registerDevice(new FloppyDrive(fdc, 1, fdd1));
+	fdc->reset();
+
+	Vector<Device*> fdds = Dev::findDevices("block.floppy");
+	for (u32int i = 0; i < fdds.size(); i++) {
+		Part::registerDevice((BlockDevice*)fdds[i]);	//TODO
+	}
+
+	new Thread(floppyMotorTimer, true);
+}
+
+String FloppyController::getClass() {
+	return "controller.floppy";
+}
+
+String FloppyController::getName() {
+	String irq = String::number(m_irq);
+	return String("Floppy controller at IRQ ") += irq;
+}
+
+void FloppyController::checkInterrupt(int *st0, int *cyl) {
+	writeCmd(FC_SENSE_INTERRUPT);
+	*st0 = readData();
+	*cyl = readData();
+}
+
+void FloppyController::setDOR() {
+	u8int dor = 0x0C;
+	if (m_activeDrive == 1)
+		dor |= 0x01;
+	if (m_drives[0] != NULL and m_drives[0]->m_motorState != 0)
+		dor |= 0x10;
+	if (m_drives[1] != NULL and m_drives[1]->m_motorState != 0)
+		dor |= 0x20;
+	asm volatile ("cli");
+	outb(m_base + FR_DOR, dor);
+	if (m_first) {	//First time we set the DOR, controller initialized
+		Task::currentThread->waitIRQ(m_irq);
+		int st0, cyl;
+		checkInterrupt(&st0, &cyl);
+		m_first = false;
+	}
+	asm volatile ("sti");
+			//PANIC("test");
+}
+
+void FloppyController::setActiveDrive(u8int drive) {
+	m_driveMutex.waitLock();
+	m_activeDrive = drive;
+	setDOR();
+}
+
+void FloppyController::setNoActiveDrive() {
+	m_driveMutex.unlock();
+}
+
+bool FloppyController::writeCmd(u8int cmd) {
+	for (int i = 0; i < 600; i++) {
+		if (0x80 & inb(m_base + FR_MSR)) {
+			outb(m_base + FR_FIFO, cmd);
+			return true;
+		}
+		Task::currentThread->sleep(10);
+	}
+	return false;
+}
+
+u8int FloppyController::readData() {
+	for (int i = 0; i < 600; i++) {
+		if (0x80 & inb(m_base + FR_MSR)) {
+			return inb(m_base + FR_FIFO);
+		}
+		Task::currentThread->sleep(10);
+	}
+	return 0;
+}
+
+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;
+
+	for (int i = 0; i < 2; i++) {
+		if (m_drives[i] != NULL) {
+			if (!m_drives[i]->setup())
+				return false;
+		}
+	}
+	return true;
+}
diff --git a/Source/Kernel/Devices/Floppy/FloppyController.class.h b/Source/Kernel/Devices/Floppy/FloppyController.class.h
new file mode 100644
index 0000000..2d0104b
--- /dev/null
+++ b/Source/Kernel/Devices/Floppy/FloppyController.class.h
@@ -0,0 +1,81 @@
+#ifndef DEF_FLOPPYCONTROLLER_CLASS_H
+#define DEF_FLOPPYCONTROLLER_CLASS_H
+
+#include <Devices/Device.proto.h>
+#include <TaskManager/Mutex.class.h>
+
+#define FLOPPY_DMALEN 0x4800	//This is one cylinder
+
+//Floppy registers
+#define FR_DOR 2		//Digital Output Register
+#define FR_MSR 4		//Main Status Register
+#define FR_FIFO 5
+#define FR_CCR 7		//Configuration Control Register
+
+//Floppy commands
+#define FC_SPECIFY 3
+#define FC_SENSE_DRIVE_STATUS 4
+#define FC_WRITE_DATA 5
+#define FC_READ_DATA 6
+#define FC_RECALIBRATE 7
+#define FC_SENSE_INTERRUPT 8
+#define FC_SEEK 15
+
+//Floppy drives types
+#define FT_NONE 0
+#define FT_360K525 1		//360kB 5.25in
+#define FT_12M525 2			//1.2MB 5.25in
+#define FT_720K35 3			//720kB 3.5in
+#define FT_144M35 4			//1.44MB 3.5in
+#define FT_288M35 5			//2.88MB 3.5in
+
+//Floppy drive motor states
+#define FS_MOTOROFF 0
+#define FS_MOTORON 1
+#define FS_MOTORWAIT 2
+
+//Floppy transfer directions
+#define FD_READ 1
+#define FD_WRITE 2
+
+class FloppyDrive;
+
+class FloppyController : public Device {
+	friend class FloppyDrive;
+	private:
+	//For DMA
+	static Mutex dmaMutex;
+	static u8int dmabuff[FLOPPY_DMALEN];
+	static bool dmaInit(u8int direction, u32int length);	//Locks the DMA Mutex. Called on each r/w operation.
+	static void dmaRelease();	//Unlocks the DMA Mutex
+
+	//For the FDC
+	FloppyController(u32int base, u8int irq);	//Private constructor, called by Detect();
+
+	u8int m_activeDrive;
+	Mutex m_driveMutex;
+
+	u32int m_base;	//I/O port base
+	u8int m_irq;
+
+	bool m_first;
+
+	FloppyDrive* m_drives[2];
+
+	void checkInterrupt(int *st0, int *cyl);
+	void setDOR();			//Updates the Digital Output Register
+	void setActiveDrive(u8int drive);	//Locks driveMutex and sets activeDrive.
+	void setNoActiveDrive();			//Unlocks driveMutex
+	bool writeCmd(u8int cmd);			//Sends command to controller
+	u8int readData();					//Reads a byte from controller
+	bool reset();
+	
+	public:
+	static void detect();
+	String getClass();
+	String getName();
+
+};
+
+#endif
+
diff --git a/Source/Kernel/Devices/Floppy/FloppyDrive.class.cpp b/Source/Kernel/Devices/Floppy/FloppyDrive.class.cpp
new file mode 100644
index 0000000..0d1b5fc
--- /dev/null
+++ b/Source/Kernel/Devices/Floppy/FloppyDrive.class.cpp
@@ -0,0 +1,330 @@
+#include "FloppyDrive.class.h"
+#include "FloppyController.class.h"
+#include <TaskManager/Task.ns.h>
+#include <DeviceManager/Time.ns.h>
+
+using namespace Sys;
+
+FloppyDrive::FloppyDrive(FloppyController *fdc, u8int number, u8int type) {
+	m_fdc = fdc;
+	m_driveNumber = number;
+	m_driveType = type;
+	fdc->m_drives[number] = this;
+
+	switch (m_driveType) {
+		case FT_360K525:
+			m_cylinders = 40;
+			m_sectors = 9;
+			break;
+		case FT_12M525:
+			m_cylinders = 80;
+			m_sectors = 15;
+			break;
+		case FT_720K35:
+			m_cylinders = 80;
+			m_sectors = 9;
+			break;
+		case FT_288M35:
+			m_cylinders = 80;
+			m_sectors = 36;
+			break;
+		case FT_144M35:
+		default:		//all unknown types are 1.44M
+			m_cylinders = 80;
+			m_sectors = 18;
+			break;
+	}
+	m_buffCyl = 0xFFFF;	//Invalid cylinder
+	m_buffTime = 0;
+}
+
+String FloppyDrive::getClass() {
+	return "block.floppy";
+}
+
+String FloppyDrive::getName() {
+	char *fdTypes[] = {
+		"NONE",
+		"360kB 5.25in",
+		"1.2MB 5.25in",
+		"720kB 3.5in",
+		"1.44MB 3.5in",
+		"2.88MB 3.5in" };
+	String ret;
+	if (m_driveNumber == 0) {
+		ret = "Floppy drive #0 : ";
+	} else {
+		ret = "Floppy drive #1 : ";
+	}
+	ret += fdTypes[m_driveType];
+	return ret;
+}
+
+bool FloppyDrive::setup() {
+	m_fdc->setActiveDrive(m_driveNumber);
+
+	//Set transfer rate
+	u8int xfer_rate;
+	if (m_driveType == FT_360K525) {
+		xfer_rate = 2;
+	} else if (m_driveType == FT_720K35) {
+		xfer_rate = 1;
+	} else {
+		xfer_rate = 0;
+	}
+	outb(m_fdc->m_base + FR_CCR, xfer_rate);
+
+	m_fdc->writeCmd(FC_SPECIFY);
+	m_fdc->writeCmd(0xDF);
+	m_fdc->writeCmd(0x02);
+
+	bool ret = calibrate();
+
+	m_fdc->setNoActiveDrive();
+
+	return ret;
+}
+
+bool FloppyDrive::calibrate() {
+	if (!m_fdc->m_driveMutex.locked() or m_fdc->m_activeDrive != m_driveNumber) return false;
+
+	int st0, cyl = -1;
+
+	if (!setMotorState(true)) return false;
+
+	for (int i = 0; i < 10; i++) {
+		asm volatile("cli");
+		m_fdc->writeCmd(FC_RECALIBRATE);
+		m_fdc->writeCmd(m_driveNumber);
+
+		Task::currentThread->waitIRQ(m_fdc->m_irq);
+		m_fdc->checkInterrupt(&st0, &cyl);
+		asm volatile("sti");
+
+		if (st0 & 0xC0) {
+			continue;
+		}
+		if (!cyl) {	//Cylinder 0 reached
+			setMotorState(false);
+			return true;
+		}
+	}
+	setMotorState(false);
+	return false;
+}
+
+bool FloppyDrive::setMotorState(bool on) {
+	if (!m_fdc->m_driveMutex.locked() or m_fdc->m_activeDrive != m_driveNumber) return false;
+
+	if (on) {
+		if (m_motorState == FS_MOTOROFF) {
+			m_motorState = FS_MOTORON;
+			m_fdc->setDOR();
+			Task::currentThread->sleep(500);
+		}
+		m_motorState = FS_MOTORON;
+	} else {
+		m_motorState = FS_MOTORWAIT;
+		m_motorTimeout = 4;
+	}
+	return true;
+}
+
+bool FloppyDrive::killMotor() {
+	m_motorState = FS_MOTOROFF;
+	m_fdc->setDOR();
+	return true;
+}
+
+bool FloppyDrive::seek(u32int cyli, s32int head) {
+	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");
+		m_fdc->writeCmd(FC_SEEK);
+		m_fdc->writeCmd(head << 2);
+		m_fdc->writeCmd(cyl);
+
+		Task::currentThread->waitIRQ(m_fdc->m_irq);
+		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) {
+			setMotorState(false);
+			m_fdc->setNoActiveDrive();
+			return true;
+		}
+	}
+	setMotorState(false);
+	m_fdc->setNoActiveDrive();
+	return false;
+}
+
+bool FloppyDrive::doTrack(u32int cyl, u8int dir) {
+	if (!seek(cyl, 0)) return false;
+	if (!seek(cyl, 1)) return false;
+	m_fdc->setActiveDrive(m_driveNumber);
+
+	u8int cmd, flags = 0xC0;
+	switch (dir) {
+		case FD_READ:
+			cmd = FC_READ_DATA | flags;
+			break;
+		case FD_WRITE:
+			cmd = FC_WRITE_DATA | flags;
+			break;
+		default:
+			m_fdc->setNoActiveDrive();
+			return false;
+	}
+
+	for (int i = 0; i < 20; i++) {
+		setMotorState(true);
+
+		if (!FloppyController::dmaInit(dir, 0x4800)) {
+			m_fdc->setNoActiveDrive();
+			setMotorState(false);
+			return false;
+		}
+
+		Task::currentThread->sleep(100);
+
+		asm volatile("cli");
+		m_fdc->writeCmd(cmd);
+		m_fdc->writeCmd(m_driveNumber);	//Drive number & first head << 2
+		m_fdc->writeCmd(cyl);	//Cylinder
+		m_fdc->writeCmd(0);		//First head
+		m_fdc->writeCmd(1);		//First sector
+		m_fdc->writeCmd(2);
+		m_fdc->writeCmd(18);	//Number of sectors
+		m_fdc->writeCmd(0x1B);
+		m_fdc->writeCmd(0xFF);
+
+		Task::currentThread->waitIRQ(m_fdc->m_irq);
+
+		u8int st0, st1, st2, rcy, rhe, rse, bps;
+		st0 = m_fdc->readData();
+		st1 = m_fdc->readData();
+		st2 = m_fdc->readData();
+		rcy = m_fdc->readData();
+		rhe = m_fdc->readData();
+		rse = m_fdc->readData();
+		bps = m_fdc->readData();
+		asm volatile("sti");
+
+		int error = 0;
+	
+		if (st0 & 0xC0) error = 1;
+		if (st1 & 0x80) error = 1;	//End of cylinder
+		if (st0 & 0x08) error = 1;	//Drive not ready
+		if (st1 & 0x20)	error = 1;	//CRC error
+		if (st1 & 0x10) error = 1;	//Controller timeout
+		if (st1 & 0x04) error = 1;	//No data found
+		if ((st2|st1) & 0x01) error=1;	//No address mark found
+		if (st2 & 0x40) error = 1;	//Deleted address mark
+		if (st2 & 0x20) error = 1;	//CRC error in data
+		if (st2 & 0x10) error = 1;	//Wrong cylinder
+		if (st2 & 0x04) error = 1;	//uPD765 sector not found
+		if (st2 & 0x02) error = 1;	//Bad cylinder
+		if (bps != 0x2) error = 1;	//Wanted 512 bytes/sector, got (1<<(bps+7))
+		if (st1 & 0x02) error = 2;	//Not writable
+
+		if (!error) CMem::memcpy(m_buffer, FloppyController::dmabuff, FLOPPY_DMALEN);	//Copy data to internal buffer
+		FloppyController::dmaRelease();
+
+		if (!error) {
+			setMotorState(false);
+			m_fdc->setNoActiveDrive();
+			return true;
+		}
+		if (error > 1) {
+			//Not retrying
+			setMotorState(false);
+			m_fdc->setNoActiveDrive();
+			return false;
+		}
+	}
+	setMotorState(false);
+	return false;
+}
+
+bool FloppyDrive::readOnly() {
+	m_fdc->setActiveDrive(m_driveNumber);
+	asm volatile("cli");
+	m_fdc->writeCmd(FC_SENSE_DRIVE_STATUS);
+	m_fdc->writeCmd(m_driveNumber);
+	Task::currentThread->waitIRQ(m_fdc->m_irq);
+	u8int st3 = m_fdc->readData();
+	asm volatile("sti");
+
+	bool ret = (st3 & 0x80 ? true : false);
+
+	m_fdc->setNoActiveDrive();
+
+	return ret;
+}
+
+u64int FloppyDrive::blocks() {
+	return m_cylinders * m_sectors * 2;
+}
+
+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 (!doTrack(cylinder, FD_READ)) return false;
+			m_buffCyl = cylinder;
+			m_buffTime = Time::uptime();
+			memcpy(data, (const u8int*)(&m_buffer[offset]), 512);
+			return true;
+		}
+	} else {
+		m_buffCyl = 0xFFFF;	//Invalid cylinder
+		m_buffTime = 0;
+
+		u32int startcylinder = (startblock / (m_sectors * 2)), stoffset = startblock % (m_sectors * 2);
+		u32int endcylinder = ((startblock + count - 1) / (m_sectors * 2)), 
+			   endoffset = (startblock + count - 1) % (m_sectors * 2);
+		for (u32int i = startcylinder; i <= endcylinder; i++) {
+			if (!doTrack(i, FD_READ)) return false;	//Read cylinder
+			//Figure out what to memcpy in data
+			u32int start = 0;	//Where to start memcpying in buffer
+			if (i == startcylinder) start = stoffset * 512;
+			u32int count = FLOPPY_DMALEN - start;	//How much we must memcpy
+			if (i == endcylinder) count = (endoffset * 512) - start + 512;
+			u32int where = 0;
+			if (i != startcylinder) where = (i - startcylinder - 1) * (512 * 2 * m_sectors) + (stoffset * 512);
+			memcpy(&data[where], (const u8int*)(&m_buffer[start]), count);
+		}
+	}
+	return true;
+}
+
+bool FloppyDrive::writeBlocks(u64int start, u32int count, u8int *data) { //TODO
+	return false;
+}
+
+u32int FloppyDrive::blockSize() {
+	return 512;
+}
+
+u64int FloppyDrive::chs2lba(u32int cylinder, u32int head, u32int sector) {
+	return ((u64int)cylinder * (u64int)m_cylinders) + ((u64int)head + 2ULL) + (u64int)sector - 1ULL;
+}
diff --git a/Source/Kernel/Devices/Floppy/FloppyDrive.class.h b/Source/Kernel/Devices/Floppy/FloppyDrive.class.h
new file mode 100644
index 0000000..3ff832b
--- /dev/null
+++ b/Source/Kernel/Devices/Floppy/FloppyDrive.class.h
@@ -0,0 +1,41 @@
+#ifndef DEF_FLOPPYDRIVE_CLASS_H
+#define DEF_FLOPPYDRIVE_CLASS_H
+
+#include <Devices/BlockDevice.proto.h>
+
+#include "FloppyController.class.h"
+
+class FloppyDrive : public BlockDevice {
+	friend class FloppyController;
+	friend u32int floppyMotorTimer();
+	private:
+	FloppyDrive(FloppyController *fdc, u8int number, u8int type);	//Private constructor, called by FloppyController()
+
+	u8int m_motorState, m_motorTimeout, m_driveNumber, m_driveType;
+	u32int m_cylinders, m_sectors;
+	FloppyController *m_fdc;
+
+	u8int m_buffer[FLOPPY_DMALEN];
+	u32int m_buffCyl, m_buffTime;	//Used for keeping track of what is in buffer, this is a sort of cache system
+
+	bool setup();
+	bool calibrate();
+	bool setMotorState(bool on);
+	bool killMotor();
+	bool seek(u32int cyli, s32int head);
+	bool doTrack(u32int cyl, u8int dir);
+
+	public:
+	String getClass();
+	String getName();
+
+	bool readOnly();
+	u64int blocks();
+	bool readBlocks(u64int startblock, u32int count, u8int *data);
+	bool writeBlocks(u64int startblock, u32int count, u8int* data);
+	u32int blockSize();	//Always 512
+
+	u64int chs2lba(u32int cylinder, u32int head, u32int sector);
+};
+
+#endif