#include "option.h"
#include "def.h"
#include "44b.h"
#include "44blib.h"




void WriteBYTE(unsigned char cs, unsigned char adr, unsigned char dat)
{
  int add = ((adr&0x01)<<4) | ((adr>>1&0x01)<<5) | ((adr>>2&0x01)<<3) | (cs << 1);
  (*(volatile U8 *)(0x4000000+add)) = dat;
}

unsigned char ReadBYTE(unsigned char cs, unsigned char adr) 
{ 
  int add = ((adr&0x01)<<4) | ((adr>>1&0x01)<<5) | ((adr>>2&0x01)<<3) | (cs << 1);
  return (*(volatile U8 *)(0x4000000+add));
}

unsigned char SetMode(unsigned char DriveNo, unsigned char Mode,  unsigned char PwrDown) 
{
  WriteBYTE(1, 6, 0xA0 + (DriveNo ? 0x10:0x00)); // Select drive
  WriteBYTE(1, 2, (PwrDown ? 0x01:0x00)); // Enable automatic power down
  switch (Mode) {
    case 0:    WriteBYTE(1,7, 0xE2); break;
    case 1:    WriteBYTE(1,7, 0xE3); break;
    // NOTE: To recover from sleep, either issue a soft or hardware reset !
    // (But not on all drives, f.ex seagate ST3655A it's not nessecary to reset
    // but only to go in Idle mode, But on a Conner CFA170A it's nessecary with
    // a reset)
    case 2:   WriteBYTE(1,7, 0xE6); break;
  }
  while ((ReadBYTE(1,7) & 0xC0)!=0x40); // Wait for DRDY & NOT BUSY 
 
  // Return the error register...
  return ReadBYTE(1, 1);
}

unsigned char ReadSector(unsigned char Drive, 
                unsigned char Head, unsigned int Track, unsigned char Sector) 
{
  unsigned int i;
  // Prepare parameters...
  WriteBYTE(1,6, 0xA0+(Drive ? 0x10:00)+Head); // CHS mode/Drive/Head
  WriteBYTE(1,5, Track>>8);  // MSB of track
  WriteBYTE(1,4, Track);     // LSB of track
  WriteBYTE(1,3, Sector);    // sector
  WriteBYTE(1,2, 0x01);      // 1 sector
  // Issue read sector command...
  WriteBYTE(1,7, 0x20);      // Read sector(s) command
  while ((ReadBYTE(1,7) & 0x08)!=0x08); // Wait for DRQ or timeout

   for (i=0; i<256;i++) Uart_Printf("%x ", (*(volatile U16 *)0x4000002));
   return ReadBYTE(1, 1);
}



unsigned char WriteSector(unsigned char Drive, unsigned char Head, unsigned int Track, unsigned char Sector) 
{
  unsigned int i;

  while ((ReadBYTE(1,7) & 0x80)==0x80);

  // Prepare parameters...
  WriteBYTE(1,6, 0xA0+(Drive ? 0x10:00)+Head); // CHS mode/Drive/Head
  WriteBYTE(1,5, Track>>8);  // MSB of track
  WriteBYTE(1,4, Track);     // LSB of track
  WriteBYTE(1,3, Sector);    // sector
  WriteBYTE(1,2, 0x01);      // 1 sector

  while ((ReadBYTE(1,7) & 0x10)!=0x10 );
  // Issue write sector command...
  WriteBYTE(1,7, 0x30);      // Write sector(s) command

  while ((ReadBYTE(1,7) & 0x08)!=0x08 ) ;
 
  // write sector data...
  for (i=0; i<256; i++)  (*(volatile U16 *)0x4000002) = 0x4444;
  while ((ReadBYTE(1,7) & 0xC0)!=0xC0 ); // Wait for DRDY and NOT BUSY
 
  // Return the error register...
  return ReadBYTE(1, 1);
}

typedef struct 
{
  U8 Heads; 
  unsigned int Tracks;
  unsigned int SectorsPerTrack;
  U8 Model[41];
} tdefDriveInfo;

U8 IdentifyDrive(unsigned char DriveNo, tdefDriveInfo *DriveInfo) 
{
  unsigned int i; 
  U16 tv;
  U16 Buffer[256];

  WriteBYTE(1, 6, 0xA0 + (DriveNo ? 0x10:0x00)); // Select drive
  WriteBYTE(1, 1, 0);  
  WriteBYTE(1, 2, 1);  
  WriteBYTE(1, 3, 1);  
  WriteBYTE(1, 4, 0);  
  WriteBYTE(1, 5, 0);  
  WriteBYTE(1, 7, 0xEC);  

  
  while ((ReadBYTE(1,7) & 0x08)!=0x08); 

  // Two bytes at a time
  for (i=0; i<512; i+=2)   Buffer[i] = (*(volatile U16 *)0x4000002);

  // Extract drive info
  DriveInfo->Heads           = Buffer[6];
  DriveInfo->Tracks          = Buffer[2];
  DriveInfo->SectorsPerTrack = Buffer[12];

  // Swap bytes, because of 16 bit transfer...
  for (i=0; i<40; i+=2) 
  {
    tv = Buffer[54+i];
    DriveInfo->Model[i] = tv / 256;
    DriveInfo->Model[i+1] = tv % 256;
  }

  // Terminate string...
  DriveInfo->Model[40]='\0';

  Uart_Printf("Model         = %s\n\r", DriveInfo->Model);
  Uart_Printf("Heads         = %u\n\r", DriveInfo->Heads);
  Uart_Printf("Tracks        = %u\n\r", DriveInfo->Tracks);
  Uart_Printf("Sectors/Track = %u\n\r", DriveInfo->SectorsPerTrack);
  Uart_Printf("Model Size    = %uMB\n\r", DriveInfo->Heads * DriveInfo->Tracks * DriveInfo->SectorsPerTrack * 512 / 1024 / 1024);

  // Return the error register...
  return ReadBYTE(1, 1);
}


void WakeIDE()
{
  WriteBYTE(1, 6, 0xA0);
  while ((ReadBYTE(1,7) & 0xC0)!=0x40) ;  
}

void IdeTest()
{
  //int i; 
  //int a,b,c;
  //U32 size;
  tdefDriveInfo DriveInfo;
  Uart_Printf("\n\rIDE Test...\n\r");
  Uart_Printf("Waiting for Initial DRDY & NOT BUSY from drive\n\r"); 
  WakeIDE();
  Uart_Printf("Spinning drive up (Going to IDLE mode)\n\r");
  Uart_Printf("SetMode=0x%0.2x\n\r", SetMode(0, 1, 1));
  Uart_Printf("Drive is ready!\n\r");

  IdentifyDrive(0, &DriveInfo);
//  i = ReadSector(0, 0, 0, 1);




//************************************************************************
//  WARNING:  NEXT CODE CAN DELETE HARD ALL DATAS.
//************************************************************************



/*
   size = 16 * 1010 * 51;

    for (a=1;a<16;a++)
   {
     for (b=1;b<1010;b++)
     {
       for (c=1;c<52;c++) 
        {
          WriteSector(0,a-1,b-1,c); 
          ReadSector(0, a-1, b-1, c);

//          Delay(2000);
        }
         Uart_Printf("%x ", b);
     
      }
}
*/


//  Uart_Printf("ReadSector=0x%0.2x\n\r", i);
  


  Delay(10000);   
}

int Main()
{
   	
    
    rSYSCFG=CACHECFG;   // Using 8KB Cache//

    Port_Init();
    
    Led_Display(0x0);

    Uart_Init(0,57600);
    Delay(10);
    Uart_Select(0); //Select UART0

    Uart_Printf("\nHello,FS44B0X!");

    IdeTest();

	return 0;
}