#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <stdbool.h>
#include <errno.h>
#include <string.h>
#include <ctype.h>
#include <unistd.h>
#include <sys/stat.h>
#include <sys/mman.h>
#include <sys/neutrino.h>
#include <hw/inout.h>

uintptr_t iobase;

// Wait for device ready (returns true/false)
static int waitReady ()
{
   time_t t = time(NULL);
   uint8_t status;

   do {
      status = in8(iobase + 7);
      } while ((status & 0xC0) != (1<<6) && time(NULL) - t < 60);
   return (status & 0xC0) == (1<<6);
   }

// Erase a block of sectors
// Returns 0 if ok, or status << 8 | error registers
static unsigned erase (uint32_t lba, uint8_t count)
{
   uint8_t status;

   out8(iobase+2, count);
   out8(iobase+3, lba);
   out8(iobase+4, lba >> 8);
   out8(iobase+5, lba >> 16);
   out8(iobase+6, ((lba >> 24) & 0xf) | 0xe0);
   out8(iobase+7, 0xc0);
   if (!waitReady()) {
      printf("Could not execute erase!\n");
      exit(1);
      }
   status = in8(iobase + 7);
   //printf("  %.2x %.2x\n", status, in8(iobase+1));
   return (status & ((1<<5) | (1<<0))) ? (status << 8) | in8(iobase+1) : 0;
   }
   

int main (int argc, char *argv [])
{
   int i;

   iobase = mmap_device_io(8, 0x1f0);
   if (iobase == (uintptr_t)MAP_FAILED) {
      printf("Could not map control port (%x)\n", iobase);
      return 1;
      }
   if (ThreadCtl(_NTO_TCTL_IO, 0) == -1) {
      printf("ThreadCtl returned error %d, %s\n", errno, strerror(errno));
      return 1;
      }

   if (!waitReady()) {
      printf("Not ready!\n");
      exit(1);
      }
   for (i = 0;; i += 0x80) {
      printf("\rOffset %11llu", (uint64_t)i * 512);
      if (erase(i, 0x80)) break;
      }
   printf("\n");
   return 0;
   }