for (i=0; i<40; i+=2) {
    Tmp=*(Buffer+54+i);
    *(Buffer+54+i)=*(Buffer+55+i);
    *(Buffer+55+i)=Tmp;
  }
  memcpy(DriveInfo->Model, Buffer+54, 80);
  // Terminate string...
  DriveInfo->Model[40]='\0';
  // Return the error register...
  return ReadBYTE(CMD, 1);
}

//----------------------------------------------------------------------------
// Read one sector, identified by drive, head, track and sector
// Returns contents of the Error Register (0x00 is no error detected)
//----------------------------------------------------------------------------

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

  // Fetch the sector...
  MSBDATA=ALLINPUT;
  LSBDATA=ALLINPUT;
  // Select address and activate CS
  SetAddress(CMD, 0);
  // Two bytes at a time
  for (i=0; i<512; i+=2) {
    nDIOR=0;
    *(Buffer+i)=LSBDATA;
    *(Buffer+i+1)=MSBDATA;
    nDIOR=1;
  }
  // Disable CS
  nCS1FX=1;
  nCS3FX=1;
  // Return the error register...
  return ReadBYTE(CMD, 1);
}

//----------------------------------------------------------------------------
// Write one sector, identified by drive, head, track and sector
// Returns contents of the Error Register (0x00 is no error detected)
//----------------------------------------------------------------------------

unsigned char WriteSector(unsigned char Drive,
                unsigned char Head, unsigned int Track, unsigned char Sector,
                unsigned char *Buffer) {
  unsigned int i;
  // Prepare parameters...
  WriteBYTE(CMD,6, 0xA0+(Drive ? 0x10:00)+Head); // CHS mode/Drive/Head
  WriteBYTE(CMD,5, Track>>8);  // MSB of track
  WriteBYTE(CMD,4, Track);     // LSB of track
  WriteBYTE(CMD,3, Sector);    // sector
  WriteBYTE(CMD,2, 0x01);      // 1 sector
  // Issue write sector command...
  WriteBYTE(CMD,7, 0x30);      // Write sector(s) command
  Timer10mSec=10000;
  while ((ReadBYTE(CMD,7) & 0x08)!=0x08 && Timer10mSec); // Wait for DRQ or ti
meout
  if (Timer10mSec==0) return 0xFF;

  // Select address and activate CS
  SetAddress(CMD, 0);
  // write sector data...
  for (i=0; i<512; i+=2) {
    LSBDATA=*(Buffer+i);
    MSBDATA=*(Buffer+i+1);
    nDIOW=0;
    nDIOW=1;
  }
  // Disable CS
  nCS1FX=1;
  nCS3FX=1;
  Timer10mSec=10000;
  while ((ReadBYTE(CMD,7) & 0xC0)!=0x40 && Timer10mSec); // Wait for DRDY and
NOT BUSY
  if (Timer10mSec==0) return 0xFF;                       //   or timeout

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

//----------------------------------------------------------------------------
// Set drive mode (STANDBY, IDLE)
//----------------------------------------------------------------------------
#define STANDBY 0
#define IDLE    1
#define SLEEP   2

unsigned char SetMode(bit DriveNo, unsigned char Mode, bit PwrDown) {
  WriteBYTE(CMD, 6, 0xA0 + (DriveNo ? 0x10:0x00)); // Select drive
  WriteBYTE(CMD, 2, (PwrDown ? 0x01:0x00)); // Enable automatic power down
  switch (Mode) {
    case STANDBY: WriteBYTE(CMD,7, 0xE2); break;
    case IDLE:    WriteBYTE(CMD,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 rese
t
    // but only to go in Idle mode, But on a Conner CFA170A it's nessecary wit
h
    // a reset)
    case SLEEP:   WriteBYTE(CMD,7, 0xE6); break;
  }
  Timer10mSec=10000;
  while ((ReadBYTE(CMD,7) & 0xC0)!=0x40 && Timer10mSec); // Wait for DRDY & NO
T BUSY
  if (Timer10mSec==0) return 0xFF;                       //   or timeout

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

//----------------------------------------------------------------------------
// Show all IDE registers
//----------------------------------------------------------------------------

void ShowRegisters(bit DriveNo) {
  WriteBYTE(CMD,6, 0xA0 + (DriveNo ? 0x10:0x00)); // Select drive
  printf("Reg 0=%02BX 1=%02BX 2=%02BX 3=%02BX 4=%02BX 5=%02BX 6=%02BX 7=%02BX
\n",
    ReadBYTE(CMD, 0), ReadBYTE(CMD, 1), ReadBYTE(CMD, 2), ReadBYTE(CMD, 3),
    ReadBYTE(CMD, 4), ReadBYTE(CMD, 5), ReadBYTE(CMD, 6), ReadBYTE(CMD, 7));
}

//----------------------------------------------------------------------------
// Main part
//----------------------------------------------------------------------------
void main() {
  unsigned int  i;
  char cmdbuf[15];
  tdefDriveInfo DriveInfo;
  int Head=0, Track=0, Sector=1;

 //------------------------------
 // Initialize outputs...
 //------------------------------
  nCS1FX=1;
  nCS3FX=1;
  DA0   =0;
  DA1   =0;
  DA2   =0;
  nDIOR =1;
  nDIOW =1;
  RESET =0; // Reset drive
 //------------------------------
 // configure inputs...
 //------------------------------
  nDASP   =1;
  LSBDATA =ALLINPUT;
  MSBDATA =ALLINPUT;
  INTRQ   =1;
  IORDY   =1;
 //------------------------------
 // Configure onchip resources...
 //------------------------------
  TMOD |= 0x01;               // TMOD: timer 0, mode 1, 16 timer
  TH0   = 0xDC;               // Set timeout period for Timer 0
  TL0   = 0x00;               // to 10 mSec timeout
  ET0   = 1;                  // Enable Timer 0 interrupts
  TR0   = 1;                  // Start timer 0

  SCON  = 0x50;               // SCON: mode 1, 8-bit UART, enable rcvr    */
  TMOD |= 0x20;               // TMOD: timer 1, mode 2, 8-bit reload      */
  TH1   = 0xFD;               // TH1:  reload value for 9.6 Kbaud         */
  PCON  = 0x00;               // PCON: Double baudrate, 19.2 Kbaud        */
  TR1   = 1;                  // TR1:  timer 1 run                        */
//ES    = 1;                  // ES:   Enable serial port int             */
  TI    = 1;                  // TI:   set TI to send first char of UART  */
  EA    = 1;                  // Global int enable

 //---------------------------------------------------------------------------
-
 // Let things settle, and after that, remove RESET from drive
 //---------------------------------------------------------------------------
-
  Delay(10);
  RESET=1;    // Remove reset to drive
 //---------------------------------------------------------------------------
-
 // ?
 //---------------------------------------------------------------------------
-
  printf("--------------------------------------------\n");
  printf("---------IDE EXERCISER (c) CHG 1996---------\n");
  printf("--------------------------------------------\n");
  ShowInputs();

  printf("Waiting for Initial DRDY & NOT BUSY from drive\n");
  WriteBYTE(CMD,6, 0xA0); // Set drive/Head register, ie. select drive 0
  while ((ReadBYTE(CMD,7) & 0xC0)!=0x40); // Wait for DRDY and NOT BUSY
  printf("Drive is ready!\n");

  printf("Spinning drive up (Going to IDLE mode)\n");
  printf("SetMode=%02bX\n", SetMode(DRIVE0, IDLE, TRUE));
  printf("Drive is ready!\n");

  printf("ReadSector=%02bX\n", ReadSector(DRIVE0, 0, 0, 1, SectorBuffer));
  HexDump(SectorBuffer, 512);
  for (i=0; i<512; i++) SectorBuffer[i]=i;

  printf("WriteSector=%02bX\n", WriteSector(DRIVE0, 0, 0, 1, SectorBuffer));
  printf("ReadSector=%02bX\n", ReadSector(DRIVE0, 0, 0, 1, SectorBuffer));
  HexDump(SectorBuffer, 512);

  while (1) {
    printf ("\nCommand: ");
    gets(cmdbuf, sizeof(cmdbuf));
    for (i=0; cmdbuf[i]!=0; i++) cmdbuf[i] = toupper(cmdbuf[i]);
    for (i = 0; cmdbuf[i] == ' '; i++);        /* skip blanks
*/

    switch (cmdbuf[i]) {
      case 'S': SetMode(DRIVE0, STANDBY, TRUE); break;
      case 'I': SetMode(DRIVE0, IDLE, TRUE);    break;
      case 'D': IdentifyDrive(DRIVE0, SectorBuffer, &DriveInfo);
                HexDump(SectorBuffer, 256);
                printf("Model         = %s\n", DriveInfo.Model);
                printf("Heads         = %bu\n",DriveInfo.Heads);
                printf("Tracks        = %u\n", DriveInfo.Tracks);
                printf("Sectors/Track = %u\n", DriveInfo.SectorsPerTrack);

        break;
      case 'R': ShowRegisters(DRIVE0); break;
      case '?':
                printf("--------------------------------------------\n");
                printf("---------IDE EXERCISER (c) CHG 1996---------\n");
                printf("--------------------------------------------\n");
                printf("Commands:\n");
                printf("  S    go to Standby mode\n");
                printf("  I    go to Idle mode\n");
                printf("  D    Identify drive\n");
                printf("  R    Show all registers\n");
                printf("  G <Head>,<Track>,<Sector> Read sector\n");
                printf("--------------------------------------------\n");
        break;
      case 'G':
        sscanf (&cmdbuf[i+1], "%d,%d,%d",
                 &Head, &Track, &Sector);
        printf("ReadSector=%02bX\n", ReadSector(DRIVE0, Head, Track, Sector, S
ectorBuffer));
        HexDump(SectorBuffer, 512);
        printf("Head=%d, Track=%d, Sector=%d\n", Head, Track, Sector);
        break;
    }
  }
}