X-Git-Url: https://git.ucc.asn.au/?a=blobdiff_plain;f=Kernel%2Fdrv%2Fata_x86.c;h=8d9e2e851da0db9e2b9d9bd55f31a9b61b6accbe;hb=5c46f86c5a8ceaa63a1a9919cf1f4d2889c6c233;hp=c8814d9a670d577f7d20a9db16cf0e6c196538fd;hpb=8f14093202765d2d363584da819b68ec8c11018c;p=tpg%2Facess2.git

diff --git a/Kernel/drv/ata_x86.c b/Kernel/drv/ata_x86.c
index c8814d9a..8d9e2e85 100644
--- a/Kernel/drv/ata_x86.c
+++ b/Kernel/drv/ata_x86.c
@@ -2,14 +2,17 @@
  * Acess2 IDE Harddisk Driver
  * drv/ide.c
  */
-#define DEBUG	0
+#define DEBUG	1
 #include <common.h>
 #include <modules.h>
 #include <vfs.h>
 #include <fs_devfs.h>
 #include <drv_pci.h>
 #include <tpl_drv_common.h>
-#include <drvutil.h>
+#include <tpl_drv_disk.h>
+
+// --- Flags ---
+#define START_BEFORE_CMD	0
 
 // === CONSTANTS ===
 #define	MAX_ATA_DISKS	4
@@ -92,28 +95,35 @@ typedef struct {
 // === PROTOTYPES ===
  int	ATA_Install();
  int	ATA_SetupIO();
-static void	ATA_SetupPartitions();
- void	ATA_SetupVFS();
+void	ATA_SetupPartitions();
+void	ATA_SetupVFS();
  int	ATA_ScanDisk(int Disk);
 void	ATA_ParseGPT(int Disk);
 void	ATA_ParseMBR(int Disk);
 void	ATA_int_MakePartition(tATA_Partition *Part, int Disk, int Num, Uint64 Start, Uint64 Length);
 Uint16	ATA_GetBasePort(int Disk);
+// Filesystem Interface
 char	*ATA_ReadDir(tVFS_Node *Node, int Pos);
 tVFS_Node	*ATA_FindDir(tVFS_Node *Node, char *Name);
 Uint64	ATA_ReadFS(tVFS_Node *Node, Uint64 Offset, Uint64 Length, void *Buffer);
+Uint64	ATA_WriteFS(tVFS_Node *Node, Uint64 Offset, Uint64 Length, void *Buffer);
  int	ATA_IOCtl(tVFS_Node *Node, int Id, void *Data);
-Uint	ATA_Read(Uint64 Address, Uint Count, void *Buffer, Uint Argument);
- int	ATA_ReadRaw(Uint8 Disk, Uint64 Address, Uint64 Count, void *Buffer);
+// Read/Write Interface/Quantiser
+Uint	ATA_ReadRaw(Uint64 Address, Uint Count, void *Buffer, Uint Disk);
+Uint	ATA_WriteRaw(Uint64 Address, Uint Count, void *Buffer, Uint Disk);
+// Read/Write DMA
  int	ATA_ReadDMA(Uint8 Disk, Uint64 Address, Uint Count, void *Buffer);
+ int	ATA_WriteDMA(Uint8 Disk, Uint64 Address, Uint Count, void *Buffer);
+// IRQs
 void	ATA_IRQHandlerPri(int unused);
 void	ATA_IRQHandlerSec(int unused);
+// Controller IO
 Uint8	ATA_int_BusMasterReadByte(int Ofs);
 void	ATA_int_BusMasterWriteByte(int Ofs, Uint8 Value);
 void	ATA_int_BusMasterWriteDWord(int Ofs, Uint32 Value);
 
 // === GLOBALS ===
-MODULE_DEFINE(0, 0x0032, i386ATA, ATA_Install, NULL);
+MODULE_DEFINE(0, 0x0032, i386ATA, ATA_Install, NULL, NULL);
 tDevFS_Driver	gATA_DriverInfo = {
 	NULL, "ata",
 	{
@@ -168,15 +178,14 @@ int ATA_Install()
 int ATA_SetupIO()
 {
 	 int	ent;
-	Uint	addr;
+	tPAddr	addr;
 	
 	ENTER("");
 	
 	// Get IDE Controller's PCI Entry
 	ent = PCI_GetDeviceByClass(0x0101, 0xFFFF, -1);
-	LOG("ent = %i\n", ent);
+	LOG("ent = %i", ent);
 	gATA_BusMasterBase = PCI_GetBAR4( ent );
-	LOG("gATA_BusMasterBase = 0x%x\n", gATA_BusMasterBase);
 	if( gATA_BusMasterBase == 0 ) {
 		Warning("It seems that there is no Bus Master Controller on this machine, get one");
 		LEAVE('i', 0);
@@ -188,6 +197,11 @@ int ATA_SetupIO()
 			gATA_BusMasterBasePtr = (void*)(0xC0000000|gATA_BusMasterBase);
 		else
 			gATA_BusMasterBasePtr = (void*)( MM_MapHWPage( gATA_BusMasterBase, 1 ) + (gATA_BusMasterBase&0xFFF) );
+		LOG("gATA_BusMasterBasePtr = %p", gATA_BusMasterBasePtr);
+	}
+	else {
+		// Bit 0 is left set as a flag to other functions
+		LOG("gATA_BusMasterBase = 0x%x", gATA_BusMasterBase & ~1);
 	}
 	
 	IRQ_AddHandler( gATA_IRQPri, ATA_IRQHandlerPri );
@@ -196,21 +210,26 @@ int ATA_SetupIO()
 	gATA_PRDTs[0].PBufAddr = MM_GetPhysAddr( (Uint)&gATA_Buffers[0] );
 	gATA_PRDTs[1].PBufAddr = MM_GetPhysAddr( (Uint)&gATA_Buffers[1] );
 	
-	LOG("gATA_PRDTs = {0x%x, 0x%x}", gATA_PRDTs[0].PBufAddr, gATA_PRDTs[1].PBufAddr);
+	LOG("gATA_PRDTs = {PBufAddr: 0x%x, PBufAddr: 0x%x}", gATA_PRDTs[0].PBufAddr, gATA_PRDTs[1].PBufAddr);
 	
 	addr = MM_GetPhysAddr( (Uint)&gATA_PRDTs[0] );
+	LOG("addr = 0x%x", addr);
 	ATA_int_BusMasterWriteDWord(4, addr);
 	addr = MM_GetPhysAddr( (Uint)&gATA_PRDTs[1] );
+	LOG("addr = 0x%x", addr);
 	ATA_int_BusMasterWriteDWord(12, addr);
 	
+	outb(IDE_PRI_BASE+1, 1);
+	outb(IDE_SEC_BASE+1, 1);
+	
 	LEAVE('i', 1);
 	return 1;
 }
 
 /**
- * \fn static void	ATA_SetupPartitions()
+ * \fn void ATA_SetupPartitions()
  */
-static void	ATA_SetupPartitions()
+void ATA_SetupPartitions()
 {
 	 int	i;
 	for( i = 0; i < MAX_ATA_DISKS; i ++ )
@@ -268,8 +287,12 @@ int ATA_ScanDisk(int Disk)
 	 int	i;
 	tVFS_Node	*node;
 	
+	ENTER("iDisk", Disk);
+	
 	base = ATA_GetBasePort( Disk );
 	
+	LOG("base = 0x%x", base);
+	
 	// Send Disk Selector
 	if(Disk == 1 || Disk == 3)
 		outb(base+6, 0xB0);
@@ -279,12 +302,18 @@ int ATA_ScanDisk(int Disk)
 	// Send IDENTIFY
 	outb(base+7, 0xEC);
 	val = inb(base+7);	// Read status
-	if(val == 0)	return 0;	// Disk does not exist
+	if(val == 0) {
+		LEAVE('i', 0);
+		return 0;	// Disk does not exist
+	}
 	
 	// Poll until BSY clears and DRQ sets or ERR is set
 	while( ((val & 0x80) || !(val & 0x08)) && !(val & 1))	val = inb(base+7);
 	
-	if(val & 1)	return 0;	// Error occured, so return false
+	if(val & 1) {
+		LEAVE('i', 0);
+		return 0;	// Error occured, so return false
+	}
 	
 	// Read Data
 	for(i=0;i<256;i++)	buf[i] = inw(base);
@@ -296,6 +325,8 @@ int ATA_ScanDisk(int Disk)
 		gATA_Disks[ Disk ].Sectors = identify->Sectors28;
 	
 	
+	LOG("gATA_Disks[ Disk ].Sectors = 0x%x", gATA_Disks[ Disk ].Sectors);
+	
 	if( gATA_Disks[ Disk ].Sectors / (2048*1024) )
 		Log("Disk %i: 0x%llx Sectors (%i GiB)", Disk,
 			gATA_Disks[ Disk ].Sectors, gATA_Disks[ Disk ].Sectors / (2048*1024));
@@ -321,11 +352,12 @@ int ATA_ScanDisk(int Disk)
 		= node->CTime = now();
 	
 	node->Read = ATA_ReadFS;
-	//node->Write = ATA_WriteFS;
+	node->Write = ATA_WriteFS;
 	node->IOCtl = ATA_IOCtl;
 
 
 	// --- Scan Partitions ---
+	LOG("Reading MBR");
 	// Read Boot Sector
 	ATA_ReadDMA( Disk, 0, 1, mbr );
 	
@@ -335,6 +367,7 @@ int ATA_ScanDisk(int Disk)
 	else	// No? Just parse the MBR
 		ATA_ParseMBR(Disk);
 	
+	LEAVE('i', 0);
 	return 1;
 }
 
@@ -357,6 +390,8 @@ void ATA_ParseMBR(int Disk)
 	tMBR	mbr;
 	Uint64	extendedLBA;
 	
+	ENTER("iDisk", Disk);
+	
 	// Read Boot Sector
 	ATA_ReadDMA( Disk, 0, 1, &mbr );
 	
@@ -372,6 +407,7 @@ void ATA_ParseMBR(int Disk)
 			)
 		{
 			if( mbr.Parts[i].SystemID == 0xF || mbr.Parts[i].SystemID == 5 ) {
+				LOG("Extended Partition");
 				if(extendedLBA != 0) {
 					Warning("Disk %i has multiple extended partitions, ignoring rest", Disk);
 					continue;
@@ -379,6 +415,7 @@ void ATA_ParseMBR(int Disk)
 				extendedLBA = mbr.Parts[i].LBAStart;
 				continue;
 			}
+			LOG("Primary Partition");
 			
 			gATA_Disks[Disk].NumPartitions ++;
 			continue;
@@ -426,6 +463,7 @@ void ATA_ParseMBR(int Disk)
 			}
 		}
 	}
+	LOG("gATA_Disks[Disk].NumPartitions = %i", gATA_Disks[Disk].NumPartitions);
 	
 	// Create patition array
 	gATA_Disks[Disk].Partitions = malloc( gATA_Disks[Disk].NumPartitions * sizeof(tATA_Partition) );
@@ -434,6 +472,7 @@ void ATA_ParseMBR(int Disk)
 	extendedLBA = 0;
 	for( i = 0; i < 4; i ++ )
 	{
+		Log("mbr.Parts[%i].SystemID = 0x%02x", i, mbr.Parts[i].SystemID);
 		if( mbr.Parts[i].SystemID == 0 )	continue;
 		if(	mbr.Parts[i].Boot == 0x0 || mbr.Parts[i].Boot == 0x80 )	// LBA 28
 		{
@@ -560,6 +599,8 @@ void ATA_ParseMBR(int Disk)
 			}
 		}
 	}
+	
+	LEAVE('-');
 }
 
 /**
@@ -585,6 +626,7 @@ void ATA_int_MakePartition(tATA_Partition *Part, int Disk, int Num, Uint64 Start
 	Part->Node.ImplPtr = Part->Name;
 	
 	Part->Node.Read = ATA_ReadFS;
+	Part->Node.Write = ATA_WriteFS;
 	Part->Node.IOCtl = ATA_IOCtl;
 	LOG("Made '%s' (&Node=%p)", Part->Name, &Part->Node);
 	LEAVE('-');
@@ -623,7 +665,11 @@ tVFS_Node *ATA_FindDir(tVFS_Node *Node, char *Name)
 	if(Name[0] < 'A' || Name[0] > 'A'+MAX_ATA_DISKS)
 		return NULL;
 	// Raw Disk
-	if(Name[1] == '\0')	return &gATA_Disks[Name[0]-'A'].Node;
+	if(Name[1] == '\0') {
+		if( gATA_Disks[Name[0]-'A'].Sectors == 0 )
+			return NULL;
+		return &gATA_Disks[Name[0]-'A'].Node;
+	}
 	
 	// Partitions
 	if(Name[1] < '0' || '9' < Name[1])	return NULL;
@@ -648,8 +694,29 @@ tVFS_Node *ATA_FindDir(tVFS_Node *Node, char *Name)
  */
 Uint64 ATA_ReadFS(tVFS_Node *Node, Uint64 Offset, Uint64 Length, void *Buffer)
 {
+	 int	disk = Node->Inode >> 8;
+	 int	part = Node->Inode & 0xFF;
+	
+	// Raw Disk Access
+	if(part == 0xFF)
+	{
+		if( Offset >= gATA_Disks[disk].Sectors * SECTOR_SIZE )
+			return 0;
+		if( Offset + Length > gATA_Disks[disk].Sectors*SECTOR_SIZE )
+			Length = gATA_Disks[disk].Sectors*SECTOR_SIZE - Offset;
+	}
+	// Partition
+	else
+	{
+		if( Offset >= gATA_Disks[disk].Partitions[part].Length * SECTOR_SIZE )
+			return 0;
+		if( Offset + Length > gATA_Disks[disk].Partitions[part].Length * SECTOR_SIZE )
+			Length = gATA_Disks[disk].Partitions[part].Length * SECTOR_SIZE - Offset;
+		Offset += gATA_Disks[disk].Partitions[part].Start * SECTOR_SIZE;
+	}
+	
 	//Log("ATA_ReadFS: (Node=%p, Offset=0x%llx, Length=0x%llx, Buffer=%p)", Node, Offset, Length, Buffer);
-	return DrvUtil_ReadBlock(Offset, Length, Buffer, ATA_Read, SECTOR_SIZE, Node->Inode);
+	return DrvUtil_ReadBlock(Offset, Length, Buffer, ATA_ReadRaw, SECTOR_SIZE, disk);
 }
 
 /**
@@ -657,7 +724,30 @@ Uint64 ATA_ReadFS(tVFS_Node *Node, Uint64 Offset, Uint64 Length, void *Buffer)
  */
 Uint64 ATA_WriteFS(tVFS_Node *Node, Uint64 Offset, Uint64 Length, void *Buffer)
 {
-	return 0;
+	 int	disk = Node->Inode >> 8;
+	 int	part = Node->Inode & 0xFF;
+	
+	// Raw Disk Access
+	if(part == 0xFF)
+	{
+		if( Offset >= gATA_Disks[disk].Sectors * SECTOR_SIZE )
+			return 0;
+		if( Offset + Length > gATA_Disks[disk].Sectors*SECTOR_SIZE )
+			Length = gATA_Disks[disk].Sectors*SECTOR_SIZE - Offset;
+	}
+	// Partition
+	else
+	{
+		if( Offset >= gATA_Disks[disk].Partitions[part].Length * SECTOR_SIZE )
+			return 0;
+		if( Offset + Length > gATA_Disks[disk].Partitions[part].Length * SECTOR_SIZE )
+			Length = gATA_Disks[disk].Partitions[part].Length * SECTOR_SIZE - Offset;
+		Offset += gATA_Disks[disk].Partitions[part].Start * SECTOR_SIZE;
+	}
+	
+	Log("ATA_WriteFS: (Node=%p, Offset=0x%llx, Length=0x%llx, Buffer=%p)", Node, Offset, Length, Buffer);
+	Debug_HexDump("ATA_WriteFS", Buffer, Length);
+	return DrvUtil_WriteBlock(Offset, Length, Buffer, ATA_ReadRaw, ATA_WriteRaw, SECTOR_SIZE, disk);
 }
 
 /**
@@ -675,71 +765,77 @@ int ATA_IOCtl(tVFS_Node *Node, int Id, void *Data)
 
 // --- Disk Access ---
 /**
- * \fn Uint ATA_Read(Uint64 Address, Uint Count, void *Buffer, Uint Argument)
+ * \fn Uint ATA_ReadRaw(Uint64 Address, Uint Count, void *Buffer, Uint Disk)
  */
-Uint ATA_Read(Uint64 Address, Uint Count, void *Buffer, Uint Argument)
+Uint ATA_ReadRaw(Uint64 Address, Uint Count, void *Buffer, Uint Disk)
 {
 	 int	ret;
-	 int	disk = Argument >> 8;
-	 int	part = Argument & 0xFF;
-	
-	// Raw Disk Access
-	if(part == 0xFF)
+	Uint	offset;
+	Uint	done = 0;
+	 
+	// Pass straight on to ATA_ReadDMAPage if we can
+	if(Count <= MAX_DMA_SECTORS)
 	{
-		if( Address >= gATA_Disks[disk].Sectors )	return 0;
-		if( Address + Count > gATA_Disks[disk].Sectors )
-			Count = gATA_Disks[disk].Sectors - Address;
-		
-		ret = ATA_ReadRaw(disk, Address, Count, Buffer);
-		if(ret == 1)
-			return Count;
-		return 0;
+		ret = ATA_ReadDMA(Disk, Address, Count, Buffer);
+		if(ret == 0)	return 0;
+		return Count;
 	}
 	
-	if( Address >= gATA_Disks[disk].Partitions[part].Length )	return 0;
-	if( Address + Count > gATA_Disks[disk].Partitions[part].Length )
-		Count = gATA_Disks[disk].Partitions[part].Length - Address;
-	
-	ret = ATA_ReadRaw(
-		disk,
-		gATA_Disks[disk].Partitions[part].Start + Address,
-		Count,
-		Buffer);
-	
-	if(ret == 1)	return Count;
-	
-	return 0;
+	// Else we will have to break up the transfer
+	offset = 0;
+	while(Count > MAX_DMA_SECTORS)
+	{
+		ret = ATA_ReadDMA(Disk, Address+offset, MAX_DMA_SECTORS, Buffer+offset);
+		// Check for errors
+		if(ret != 1)	return done;
+		// Change Position
+		done += MAX_DMA_SECTORS;
+		Count -= MAX_DMA_SECTORS;
+		offset += MAX_DMA_SECTORS*SECTOR_SIZE;
+	}
 	
+	ret = ATA_ReadDMA(Disk, Address+offset, Count, Buffer+offset);
+	if(ret != 1)	return 0;
+	return done+Count;
 }
+
 /**
- * \fn int ATA_ReadRaw(Uint8 Disk, Uint64 Address, Uint64 Count, void *Buffer)
+ * \fn Uint ATA_WriteRaw(Uint64 Address, Uint Count, void *Buffer, Uint Disk)
  */
-int ATA_ReadRaw(Uint8 Disk, Uint64 Address, Uint64 Count, void *Buffer)
+Uint ATA_WriteRaw(Uint64 Address, Uint Count, void *Buffer, Uint Disk)
 {
 	 int	ret;
 	Uint	offset;
+	Uint	done = 0;
 	 
-	// Pass straight on to ATA_ReadDMAPage if we can
+	// Pass straight on to ATA_WriteDMA if we can
 	if(Count <= MAX_DMA_SECTORS)
-		return ATA_ReadDMA(Disk, Address, Count, Buffer);
+	{
+		ret = ATA_WriteDMA(Disk, Address, Count, Buffer);
+		if(ret == 0)	return 0;
+		return Count;
+	}
 	
 	// Else we will have to break up the transfer
 	offset = 0;
 	while(Count > MAX_DMA_SECTORS)
 	{
-		ret = ATA_ReadDMA(Disk, Address+offset, MAX_DMA_SECTORS, Buffer+offset);
+		ret = ATA_WriteDMA(Disk, Address+offset, MAX_DMA_SECTORS, Buffer+offset);
 		// Check for errors
-		if(ret != 1)	return ret;
+		if(ret != 1)	return done;
 		// Change Position
+		done += MAX_DMA_SECTORS;
 		Count -= MAX_DMA_SECTORS;
 		offset += MAX_DMA_SECTORS*SECTOR_SIZE;
 	}
 	
-	return ATA_ReadDMA(Disk, Address+offset, Count, Buffer+offset);
+	ret = ATA_WriteDMA(Disk, Address+offset, Count, Buffer+offset);
+	if(ret != 1)	return 0;
+	return done+Count;
 }
 
 /**
- * \fn int ATA_ReadDMAPage(Uint8 Disk, Uint64 Address, Uint Count, void *Buffer)
+ * \fn int ATA_ReadDMA(Uint8 Disk, Uint64 Address, Uint Count, void *Buffer)
  */
 int ATA_ReadDMA(Uint8 Disk, Uint64 Address, Uint Count, void *Buffer)
 {
@@ -747,8 +843,15 @@ int ATA_ReadDMA(Uint8 Disk, Uint64 Address, Uint Count, void *Buffer)
 	 int	disk = Disk & 1;
 	Uint16	base;
 	
+	ENTER("iDisk XAddress iCount pBuffer", Disk, Address, Count, Buffer);
+	
 	// Check if the count is small enough
-	if(Count > MAX_DMA_SECTORS)	return 0;
+	if(Count > MAX_DMA_SECTORS) {
+		Warning("Passed too many sectors for a bulk DMA read (%i > %i)",
+			Count, MAX_DMA_SECTORS);
+		LEAVE('i');
+		return 0;
+	}
 	
 	// Get exclusive access to the disk controller
 	LOCK( &giaATA_ControllerLock[ cont ] );
@@ -759,6 +862,9 @@ int ATA_ReadDMA(Uint8 Disk, Uint64 Address, Uint Count, void *Buffer)
 	// Get Port Base
 	base = ATA_GetBasePort(Disk);
 	
+	// Reset IRQ Flag
+	gaATA_IRQs[cont] = 0;
+	
 	// Set up transfer
 	outb(base+0x01, 0x00);
 	if( Address > 0x0FFFFFFF )	// Use LBA48
@@ -778,16 +884,100 @@ int ATA_ReadDMA(Uint8 Disk, Uint64 Address, Uint Count, void *Buffer)
 	outb(base+0x03, (Uint8) Address);		// Low Addr
 	outb(base+0x04, (Uint8) (Address >> 8));	// Middle Addr
 	outb(base+0x05, (Uint8) (Address >> 16));	// High Addr
+	
+	LOG("Starting Transfer");
+	#if START_BEFORE_CMD
+	// Start transfer
+	ATA_int_BusMasterWriteByte( cont << 3, 9 );	// Read and start
+	if( Address > 0x0FFFFFFF )
+		outb(base+0x07, HDD_DMA_R48);	// Read Command (LBA48)
+	else
+		outb(base+0x07, HDD_DMA_R28);	// Read Command (LBA28)
+	#else
 	if( Address > 0x0FFFFFFF )
 		outb(base+0x07, HDD_DMA_R48);	// Read Command (LBA48)
 	else
 		outb(base+0x07, HDD_DMA_R28);	// Read Command (LBA28)
+	// Start transfer
+	ATA_int_BusMasterWriteByte( cont << 3, 9 );	// Read and start
+	#endif
+	
+	// Wait for transfer to complete
+	//ATA_int_BusMasterWriteByte( (cont << 3) + 2, 0x4 );
+	while( gaATA_IRQs[cont] == 0 ) {
+		//Uint8	val = ATA_int_BusMasterReadByte( (cont << 3) + 2, 0x4 );
+		//LOG("val = 0x%02x", val);
+		Threads_Yield();
+	}
+	
+	// Complete Transfer
+	ATA_int_BusMasterWriteByte( cont << 3, 0 );	// Write and stop
+	
+	LOG("Transfer Completed & Acknowledged");
+	
+	// Copy to destination buffer
+	memcpy( Buffer, gATA_Buffers[cont], Count*SECTOR_SIZE );
+	
+	// Release controller lock
+	RELEASE( &giaATA_ControllerLock[ cont ] );
+	
+	LEAVE('i', 1);
+	return 1;
+}
+
+/**
+ * \fn int ATA_WriteDMA(Uint8 Disk, Uint64 Address, Uint Count, void *Buffer)
+ */
+int ATA_WriteDMA(Uint8 Disk, Uint64 Address, Uint Count, void *Buffer)
+{
+	 int	cont = (Disk>>1)&1;	// Controller ID
+	 int	disk = Disk & 1;
+	Uint16	base;
+	
+	// Check if the count is small enough
+	if(Count > MAX_DMA_SECTORS)	return 0;
+	
+	// Get exclusive access to the disk controller
+	LOCK( &giaATA_ControllerLock[ cont ] );
+	
+	// Set Size
+	gATA_PRDTs[ cont ].Bytes = Count * SECTOR_SIZE;
+	
+	// Get Port Base
+	base = ATA_GetBasePort(Disk);
+	
+	// Set up transfer
+	outb(base+0x01, 0x00);
+	if( Address > 0x0FFFFFFF )	// Use LBA48
+	{
+		outb(base+0x6, 0x40 | (disk << 4));
+		outb(base+0x2, 0 >> 8);	// Upper Sector Count
+		outb(base+0x3, Address >> 24);	// Low 2 Addr
+		outb(base+0x3, Address >> 28);	// Mid 2 Addr
+		outb(base+0x3, Address >> 32);	// High 2 Addr
+	}
+	else
+	{
+		outb(base+0x06, 0xE0 | (disk << 4) | ((Address >> 24) & 0x0F));	//Disk,Magic,High addr
+	}
+	
+	outb(base+0x02, (Uint8) Count);		// Sector Count
+	outb(base+0x03, (Uint8) Address);		// Low Addr
+	outb(base+0x04, (Uint8) (Address >> 8));	// Middle Addr
+	outb(base+0x05, (Uint8) (Address >> 16));	// High Addr
+	if( Address > 0x0FFFFFFF )
+		outb(base+0x07, HDD_DMA_W48);	// Write Command (LBA48)
+	else
+		outb(base+0x07, HDD_DMA_W28);	// Write Command (LBA28)
 	
 	// Reset IRQ Flag
 	gaATA_IRQs[cont] = 0;
 	
+	// Copy to output buffer
+	memcpy( gATA_Buffers[cont], Buffer, Count*SECTOR_SIZE );
+	
 	// Start transfer
-	ATA_int_BusMasterWriteByte( cont << 3, 9 );	// Read and start
+	ATA_int_BusMasterWriteByte( cont << 3, 1 );	// Write and start
 	
 	// Wait for transfer to complete
 	while( gaATA_IRQs[cont] == 0 )	Threads_Yield();
@@ -795,9 +985,6 @@ int ATA_ReadDMA(Uint8 Disk, Uint64 Address, Uint Count, void *Buffer)
 	// Complete Transfer
 	ATA_int_BusMasterWriteByte( cont << 3, 0 );	// Write and stop
 	
-	// Copy to destination buffer
-	memcpy( Buffer, gATA_Buffers[cont], Count*SECTOR_SIZE );
-	
 	// Release controller lock
 	RELEASE( &giaATA_ControllerLock[ cont ] );
 	
@@ -813,8 +1000,9 @@ void ATA_IRQHandlerPri(int unused)
 	
 	// IRQ bit set for Primary Controller
 	val = ATA_int_BusMasterReadByte( 0x2 );
+	LOG("IRQ val = 0x%x", val);
 	if(val & 4) {
-		//Log(" ATA_IRQHandlerPri: IRQ hit (val = 0x%x)", val);
+		LOG("IRQ hit (val = 0x%x)", val);
 		ATA_int_BusMasterWriteByte( 0x2, 4 );
 		gaATA_IRQs[0] = 1;
 		return ;
@@ -830,7 +1018,7 @@ void ATA_IRQHandlerSec(int unused)
 	// IRQ bit set for Secondary Controller
 	val = ATA_int_BusMasterReadByte( 0xA );
 	if(val & 4) {
-		//Log(" ATA_IRQHandlerSec: IRQ hit (val = 0x%x)", val);
+		LOG("IRQ hit (val = 0x%x)", val);
 		ATA_int_BusMasterWriteByte( 0xA, 4 );
 		gaATA_IRQs[1] = 1;
 		return ;