ata pio read and write code

#define C_READ_FUNC

#ifdef C_READ_FUNC

#define CMD_READ 0x20
#define CMD_WRITE 0x30
#define CMD_IDENTIFY 0xEC

#define BSY_FLAG 0x80
#define DRQ_FLAG 0x08
#define ERR_FLAG 0x01
bool driveAvailable = true;
int AtaLbaRead(uint32_t lba, void* dest_, uint8_t drive_num, uint8_t secCount)
{
	uint16_t max, base;
	uint8_t *buffer = (uint8_t*)dest_;
	uint8_t drive = 0xE0;
	
	switch(drive_num)
	{
		case 0: base = 0x01f0; break;
		case 1: base = 0x0170; break;
		case 2: base = 0x01e8; break;
		case 3: base = 0x0168; break;
		default: return -1;
	}
	if(drive_num % 2) drive |= 0x10;
	WritePort(base + 2, secCount);
	WritePort(base + 3, (uint8_t)((lba & 0x000000ff)));
	WritePort(base + 4, (uint8_t)((lba & 0x0000ff00) >> 8));
	WritePort(base + 5, (uint8_t)((lba & 0x00ff0000) >> 16));
	WritePort(base + 6, (uint8_t)((lba & 0x0f000000) >> 24) | drive);
	WritePort(base + 7, CMD_READ);
	
	int count = 0;
	for (int i = 0; i < secCount; i++) {
		while(ReadPort(base + 7) & BSY_FLAG)
		{
			count++;
			if (count > 1000000)
			{
				puts("Drive always busy! Quitting...");
				driveAvailable = false;
				return -1;
			}
		}
		
		max = 256;
		for(int as = 0; as < max; as++) 
		{
			uint16_t data = ReadPortW(base);
			*(uint16_t *)(buffer + as * 2 + i * 512) = data;
		}
	}
	return 0;
}

int AtaLbaReadS(uint32_t lba, void* dest_, uint8_t drive_num)
{
	return AtaLbaRead(lba, dest_, drive_num, 1);
}

#define AtaIdentifyData struct nshd_ata_identify_data
AtaIdentifyData
{
	uint16_t DataStructRev;
	uint16_t MultiwordDmaSupport;
	uint16_t UltraDmaSupport;
	uint64_t MaxLba;
	uint16_t FeatureSet;
	uint16_t SataFeatureSet;
	uint16_t SataReserved;
	uint16_t Reserved[10];
	uint16_t FeatureSet2;
	uint16_t FeatureSet3;
	uint16_t Reserved2[233];
	uint8_t IntegrityByte;
	uint8_t CheckSum;
}__attribute__((packed));

AtaIdentifyData* SendIdentifyCommand(uint8_t drive_num)
{
	uint16_t base;
	uint16_t drive = 0xa0;
	
	uint16_t* identify_struct = malloc(512);
	
	
	switch(drive_num)
	{
		case 0: base = 0x01f0; break;
		case 1: base = 0x0170; break;
		case 2: base = 0x01e8; break;
		case 3: base = 0x0168; break;
		default: 
			free(identify_struct);
			return NULL;
	}
	if(drive_num % 2) drive |= 0x10;
	WritePort(base + 6, drive);
	WritePort(base + 2, 0);
	WritePort(base + 3, 0);
	WritePort(base + 4, 0);
	WritePort(base + 5, 0);
	WritePort(base + 7, CMD_IDENTIFY);
	if(ReadPort(base + 7) == 0) 
	{
		free(identify_struct);
		return NULL;
	}
	else
	{
		int timeout = 200;
		while(ReadPort(base + 7) & BSY_FLAG)
		{
			timeout--;
			if(timeout <= 0) 
			{
				free(identify_struct);
				return NULL;
			}
		}
		if(ReadPort(base + 4) == 0 && ReadPort(base + 5) == 0) 
			goto is_ata_drive;
		free(identify_struct);
		return NULL; // This is not an ATA drive!
	is_ata_drive:;
		uint8_t state = 0x00;
		timeout = 200;
		do
		{
			state = ReadPort(base + 7);
			timeout--;
			if(timeout <= 0) 
			{
				free(identify_struct);
				return NULL;
			}
		}
		while((state & DRQ_FLAG) == 0 && (state & ERR_FLAG) == 0);
		// Wait until DRQ/ERR sets. ^ ^ ^
		// if ERR is clear
		if(!(state & ERR_FLAG))
		{
			// read data
			uint16_t* ids = identify_struct;
			for(int i = 0; i < 256; i++)
			{
				*ids++ = ReadPortW(base);
			}
			return (AtaIdentifyData*)identify_struct;
		}
		free(identify_struct);
		return NULL;
	}
}

int AtaLbaWrite(uint32_t lba, void* dest_, uint8_t drive_num, uint8_t secCount) {
	uint16_t max, base;
	uint8_t *buffer = (uint8_t*)dest_;
	uint8_t drive = 0xE0;
	
	switch(drive_num)
	{
		case 0: base = 0x01f0; break;
		case 1: base = 0x0170; break;
		case 2: base = 0x01e8; break;
		case 3: base = 0x0168; break;
		default: return -1;
	}
	if(drive_num % 2) drive |= 0x10;
	WritePort(base + 2, secCount);
	WritePort(base + 3, (uint8_t)((lba & 0x000000ff)));
	WritePort(base + 4, (uint8_t)((lba & 0x0000ff00) >> 8));
	WritePort(base + 5, (uint8_t)((lba & 0x00ff0000) >> 16));
	WritePort(base + 6, (uint8_t)((lba & 0x0f000000) >> 24) | drive);
	WritePort(base + 7, CMD_WRITE);
	
	int count = 0;
	for (int i = 0; i < secCount; i++) {
		while(ReadPort(base + 7) & BSY_FLAG)
		{
			count++;
			if (count > 1000000)
			{
				puts("Drive always busy! Quitting...");
				driveAvailable = false;
				return -1;
			}
		}
		
		max = 256;
		for(int as = 0; as < max; as++) 
		{
			WritePortW(base, *(uint16_t *)(buffer + as * 2 + i * 512));
			WritePort(0x80, 0x00); // delay
		}
	}
	WritePort(base + 7, 0xE7); // flush cache
	
	count = 0;
	while(ReadPort(base + 7) & BSY_FLAG)
	{
		count++;
		if (count > 1000000)
		{
			puts("Drive always busy! Quitting...");
			driveAvailable = false;
			return -1;
		}
	}
	return 0;
}
int AtaLbaWriteS(uint32_t lba, void* dest, uint8_t drive_num) {
	return AtaLbaWrite(lba, dest, drive_num, 1);
}
#else
extern void AtaLbaRead(uint32_t lba, void* dest, uint8_t sectorsToRead, uint8_t drive_num);
extern void AtaLbaReadS(uint32_t lba, void* dest, uint8_t drive_num);
extern void AtaLbaWrite(uint32_t lba, void* dest, uint8_t sectorsToRead, uint8_t drive_num);
extern void AtaLbaWriteS(uint32_t lba, void* dest, uint8_t drive_num);
#endif