/* floppy.c - floppy disk driver */

#include <system.h>
#include <floppy.h>
#include <dma.h>
#include <timer.h>


unsigned char floppy_flags = 0;
unsigned char floppy_num_drives = 0;
unsigned char floppy_drive_type;
unsigned char floppy_state = FLOPPY_STATE_IDLE;
unsigned char floppy_waiting = 0;
unsigned char floppy_requests = 0;
unsigned char ST0, ST1, ST2, ST3;

struct floppy_request request;


static inline void send_floppy_byte(unsigned char byte) {
	unsigned char status;
	do IN_PORT_PP(PORT_FLOPPY_STATUS, status); while((status & 0xC0) != 0x80);
	OUT_PORT(byte, PORT_FLOPPY_COMMAND_DATA);
}

static inline unsigned char recv_floppy_byte() {
	unsigned char status;
	do IN_PORT_PP(PORT_FLOPPY_STATUS, status); while((status & 0xD0) != 0xD0);
	IN_PORT_PP(PORT_FLOPPY_COMMAND_DATA, status);
	return status;
}

static inline void recalibrate(unsigned char drive) {
	floppy_state = FLOPPY_STATE_RECALIBRATE;
	floppy_waiting = 1;
	send_floppy_byte(0x07);
	send_floppy_byte(drive);
}

void test() {
	read_floppy_sectors(0, 0, 0, 1, 1);
}

void init_floppy() {
	unsigned short info;
	unsigned char i;
	
	
	//Kill motor
	OUT_PORT(0x0C, PORT_FLOPPY_DOR);
	
	//List drives
	asm(
		"mov $0x28,%%ax\n"
		"mov %%ax,%%fs\n"
		"mov %%fs:0x0410,%0"
		: "=a" (info)
	);
	
	if(info & 1) {
		floppy_num_drives = ((info >> 6) & 3) + 1;
		if(floppy_num_drives > 2) floppy_num_drives = 2;
		if(floppy_num_drives == 1) print("1 floppy drive found\r\n");
		else print("2 floppy drives found\r\n");
	}
	else {
		print(" No floppy drives found...\r\n");
		return;
	}
	
	
	OUT_PORT(0x10, 0x70);
	IN_PORT(0x71, floppy_drive_type);
	
	if((floppy_drive_type >> 4) & 0x0F) {
		print("   fd0: ");
		if(((floppy_drive_type >> 4) & 0x0F) > 5) print("unknown");
		else print(floppy_drive_types[(floppy_drive_type >> 4) & 0x0F]);
		print("\r\n");
	}
	
	if(floppy_drive_type & 0x0F) {
		print("   fd1: ");
		if((floppy_drive_type & 0x0F) > 5) print("unknown");
		else print(floppy_drive_types[floppy_drive_type & 0x0F]);
		print("\r\n");
	}
	
	// configure
	
	/*OUT_PORT(0x13, PORT_FLOPPY_COMMAND_DATA);
	OUT_PORT(0, PORT_FLOPPY_COMMAND_DATA);
	OUT_PORT(0x1F, PORT_FLOPPY_COMMAND_DATA);
	OUT_PORT(0, PORT_FLOPPY_COMMAND_DATA);
	
	// specify
	
	OUT_PORT(0x03, PORT_FLOPPY_COMMAND_DATA);
	OUT_PORT(0xCF, PORT_FLOPPY_COMMAND_DATA);
	OUT_PORT(0x06, PORT_FLOPPY_COMMAND_DATA);*/
}

int read_floppy_sectors(unsigned char drive, unsigned char cylinder, unsigned char head, unsigned char sector, unsigned char sector_count) {
	unsigned char i;
	
	// test parameters
	if(drive != 0 && drive != 1) return 1;
	else if((drive == 0) && (((floppy_drive_type >> 4) & 0x0F) != 4)) return 2;
	else if((drive == 1) && ((floppy_drive_type & 0x0F) != 4)) return 2;
	
	if(!sector_count) return 3;
	
	if(floppy_state == FLOPPY_STATE_IDLE) {
		
		//Activate drive motor
		OUT_PORT((1 << (4 + drive)) | 0x0C, PORT_FLOPPY_DOR);
		
		print("TEST\n");
		
		sleep(5000);
		
		print("TEST\n");
		
		//Recalibrate
		//recalibrate(drive);
	}
	
	return 0;
}

void floppy_proc() {
	unsigned char i;
	
	/*IN_PORT(PORT_FLOPPY_STATUS, i);
	print_hex_byte(i);*/
	//dma_clear_ff();
	//IN_PORT(0x04, i);
	//print_hex_byte(i);
	//IN_PORT(0x04, i);
	//print_hex_byte(i);
	/*IN_PORT(0x81, i);
	print_hex_byte(i);*/
	//IN_PORT(0x05, i);
	//print_hex_byte(i);
	/*IN_PORT(0x05, i);
	print_hex_byte(i);*/
	
	if(floppy_waiting) return;
	
	if(floppy_state == FLOPPY_STATE_RECALIBRATE) {
		floppy_state = FLOPPY_STATE_READ;
		floppy_waiting = 1;
		 
		dma_disable(2);
		dma_clear_ff();
		dma_set_mode(2, DMA_MODE_SINGLE | DMA_MODE_READ | DMA_MODE_INC_ADDR);
		dma_set_base_addr(2, (void*)0x200000);
		dma_set_word_count(2, 0xFFFF);
		dma_enable(2);
		
		send_floppy_byte(0xE6);
		send_floppy_byte(0x00);
		send_floppy_byte(0x00);
		send_floppy_byte(0x00);
		send_floppy_byte(0x02);
		send_floppy_byte(0x02);
		send_floppy_byte(0x01);
		send_floppy_byte(0x1B);
		send_floppy_byte(0xFF);
	}
}

void floppy_IRQ() {
	//floppy_waiting = 0;
	switch(floppy_state) {
		case FLOPPY_STATE_RECALIBRATE:
			print("Recalibrate FDC interrupt\n");
			//IN_PORT(PORT_FLOPPY_STATUS, i);
			//print_hex_byte(i);
			//floppy_state = FLOPPY_STATE_READ;
 			//floppy_waiting = 0;
			send_floppy_byte(0x08);
			//print_hex_byte(recv_floppy_byte());
			//print_hex_byte(recv_floppy_byte());
			ST0 = recv_floppy_byte();
			ST1 = recv_floppy_byte();
			
			/*dma_disable(2);
			dma_clear_ff();
			dma_set_mode(2, DMA_MODE_SINGLE | DMA_MODE_READ | DMA_MODE_INC_ADDR);
			dma_set_base_addr(2, (void*)0x100000);
			dma_set_word_count(2, 512);
			dma_enable(2);
			
			send_floppy_byte(0xE6);
			send_floppy_byte(0x00);
			send_floppy_byte(0x00);
			send_floppy_byte(0x00);
			send_floppy_byte(0x01);
			send_floppy_byte(0x02);
			send_floppy_byte(0x01);
			send_floppy_byte(0x1B);
			send_floppy_byte(0xFF);*/
	
			break;
		case FLOPPY_STATE_READ:
			print("Read FDC interrupt\n");
			floppy_state = FLOPPY_STATE_IDLE;
			print_hex_long(*(unsigned long*)0x1FFB00);
			break;
		default:
			print("Unexpected FDC interrupt!\n");
	}
}