Steps 1 - 4 depend on several helper functions. These are the functions needed:

DetectDrives
This function, which is not in the Floppy class, detects what floppy drives are installed. It also calls Floppy::Initialize with the information it finds. It's set to use drive A. Except for setting the interrupt handler to call InterruptRaised, this is the only function you need to call to set up the floppy driver.

Initialize
Technically not a helper function, this sets several variables needed for the floppy driver. Since I don't yet have the ability to use new, all member functions are static, and Initialize essentially acts as a constructor. Pass it the drive index (0 is typically drive A, 1 drive B) and the drive type (currently not actually used -- 3.5" 1.44MB is assumed).

GetData
GetData is called to get a byte from the floppy controller’s data register. It returns true if it was successful, false if not. Note that this function may need to try multiple times before it successfully reads the data, since the CPU can be a lot faster than port I/O.

SendData
SendData sends a byte to the floppy controller’s data register. It, too, returns true if successful or false if not. It also may need to try more than once before being successful.

ReadStatus
ReadStatus issues a Check Interrupt Status command; it is used to check at which cylinder the drive is. It must be called after an interrupt (if I remember correctly, the floppy controller will not issue another interrupt until this is done, though I could be wrong).

ReadSectorStatus
ReadSectorStatus is called after reading a sector to make sure the correct sector was read. It returns true if the floppy drive is at the head and sector specified (and the sector size is correct).

InterruptRaised
InterruptRaised simply tells code waiting on an interrupt that it can continue. You should register this function with your interrupt handler to handle IRQ 6. Note that you will not see my code doing this, as the code on this site does not include the function that sets the interrupt handler (it's currently not in the Floppy class, and I currently don't provide my interrupt handling functions, anyway). You will probably want to add it in Floppy::Initialize or the function that calls DetectDrives. Depending on your interrupt handler, you may need to change the function definition -- mine requires the function to accept a byte parameter, so a function can tell which interrupt was raised if it is used for multiple interrupts.

Code

void DetectDrives() {
	byte DriveInfo;

	WriteByteToPort(0x70, 0x10);			// Request floppy drive information
	DriveInfo = ReadByteFromPort(0x71);		// Retrieve floppy drive information

	Floppy::Initialize(0, (Floppy::DriveType) ((DriveInfo & 0xF0) >> 4));
	//DriveB = (Floppy::DriveType) (DriveInfo & 0x0F);
}

void Floppy::Initialize(byte IndexOfDrive, DriveType TypeOfDrive) {
	DriveIndex = IndexOfDrive;
	Type = TypeOfDrive;
}


bool Floppy::ReadSectorStatus(byte Head, byte Sector, byte SectorSize) {
	byte TestCylinder, TestHead, TestSector, TestSectorSize;

	if (GetData(StatusRegister0)) {
		if (GetData(StatusRegister1)) {
			if (GetData(StatusRegister2)) {
				if (GetData(TestCylinder)) {
					if (GetData(TestHead)) {
						if (GetData(TestSector)) {
							if (GetData(TestSectorSize)) {
								return (TestCylinder == AtCylinder) && (TestHead == Head) && (TestSector == Sector) && (TestSectorSize == SectorSize);
							}
						}
					}
				}
			}
		}
	}

	return false;
}

bool Floppy::ReadStatus() {
	if (!SendData(cmdCheckInterruptStatus))
		return false;
	if (!GetData(StatusRegister0))
		return false;
	if(!GetData(AtCylinder))
		return false;

	return true;
}

bool Floppy::SendData(byte Data) {
	for (unsigned int Tries = 0; Tries < MaximumSendTries; Tries++) {
		if ((ReadByteFromPort(IOBase[ControllerIndex] + flpRMainStatusRegister) & (msrMainRequest | msrWaitingForDataIO)) >= msrMainRequest) {
			WriteByteToPort(IOBase[ControllerIndex] + flpRWDataRegister, Data);
			return true;
		}

		// Give the port some extra time
		ReadByteFromPort(0x80);
	}

	return false;
}

bool Floppy::GetData(byte & Data) {
	for (unsigned int Tries = 0; Tries < MaximumGetTries; Tries++) {
		if ((ReadByteFromPort(IOBase[ControllerIndex] + flpRMainStatusRegister) & (msrMainRequest | msrWaitingForDataIO)) == (msrMainRequest | msrWaitingForDataIO)) {
			Data = ReadByteFromPort(IOBase[ControllerIndex] + flpRWDataRegister);
			return true;
		}

		// Give the port some extra time
		ReadByteFromPort(0x80);
	}

	return false;
}

void Floppy::InterruptRaised(byte Interrupt) {
	PendingInterrupt = false;
}