/*
 * JR-IDE Project
 * - (c) 2017 Alan Hightower
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "netpi.h"

#include <sys/ioctl.h>
#include <sys/mman.h>

#include <poll.h>
#include <libgen.h>
#include <signal.h>

#include <linux/i2c.h>
#include <linux/i2c-dev.h>

#include "machxo2_api.h"


#define DRIVE_FW_VERSION     "20161102"

int verbose   = 0;
int serial_no = 0;
int fw_ver_major = 0, fw_ver_minor = 0;

ide_regs_t ide;

#define IFIFO_SIZE  1024 // words
#define OFIFO_SIZE  1024 // words

static drive_t *master = NULL;
static drive_t *slave  = NULL;


// First byte of the SPI payload is always the command action
#define SPI_CMD_NOP          0    // No operation
#define SPI_CMD_FIFO_READ    4    // Read IDE register file
#define SPI_CMD_FIFO_WRITE   5    // Update IDE regsiter file
#define SPI_CMD_REG_READ     6    // Read IDE payload data from host
#define SPI_CMD_REG_WRITE    7    // Write IDE payload data to host


/*
 * While there are two general purpose pins routed to the CPLD
 * only one is in use at the moment.  It is level sensitive and
 * active low.  It is set by the CPLD every time the IDE command
 * register is written and cleared by this program and pushed
 * via ide_update.  These functions setup the PI GPIO subsystem
 * to configure it as an input, read it status, and potentially
 * sleep until it's value changes (idle function of main loop). 
 */

static int gpioa_fd = -1;

static void gpio_term (void)
{
	if (gpioa_fd >= 0)
		close (gpioa_fd);

	gpioa_fd = -1;

	return;
}

static int gpio_init (void)
{
	if ((gpioa_fd = open ("/sys/class/gpio/gpio24/value", O_RDWR)) < 0)
	{
		LOGERR("Unable to open BCM24 GPIO - %s", strerror (errno));

		gpio_term ();

		return -1;
	}

	return 0;
}

static int irq_pending = 0;

static int gpio_update (void)
{
	char ch = 0;

	if ((lseek (gpioa_fd, 0L, SEEK_SET) == 0L) &&
	    (read (gpioa_fd, &ch, 1) == 1))
		irq_pending = (ch == '0');
	else
		return -1;

	return 0;
}

static int gpio_sleep (void)
{
	struct timeval tv;

	gettimeofday (&tv, NULL);

	struct pollfd fds;

	fds.fd      = gpioa_fd;
	fds.events  = POLLPRI;
	fds.revents = 0;

	poll (&fds, 1, 1000 - (tv.tv_usec / 1000));

	return gpio_update ();
}


/*
 * Utility function to dump transfer buffers if verbose > 2
 */
void hexdump (uint32_t offset, void *data, int len)
{
	int i, addr = 0;
	uint8_t *ptr = (uint8_t *) data;

	while (len > 0)
	{
		printf (" 0x%08x:", (unsigned int) (addr + offset));

		for (i = 0; i < 16; i++)
		{
			if (i < len) {
				printf (" %02x", ptr[i]); }
			else
				puts ("   ");

			if (i == 7)
				putc (' ', stdout);
		}

		putc (' ', stdout);

		for (i = 0; i < 16; i++)
		{
			if (i < len) {
				printf ("%c", isalnum(ptr[i]) ? ptr[i] : '.'); }

			if (i == 7)
				putc (' ', stdout);
		}

		len -= 16;
		ptr += 16;
		addr += 16;

		putc ('\n', stdout);
	}

	return;
}


/*
 * Central to this design is the ability to upgrade the CPLD code
 * without using an external programming device.  I chose the MachXO2
 * due to intimate familiarity and pre-developed legacy support tools;
 * like this firmware update API.
 *
 * The MachXO2 allows update of on-board firmware over I2C - even if
 * the device is blank from the factory.  The firmware image is always
 * embedded in this executable image so the firmware and this code
 * will always be in lock-step as long as the -u option is used.
 *
 * In the future, I plan on switching to an ice40HX series as there
 * is a fully open synthesis toolchain that could run directly on the
 * Raspberry PI.
 */

static int i2c_fd = -1;

static int _i2c_exchange (void *ctx, uint8_t *out, uint8_t out_len, uint8_t *in, uint8_t in_len)
{
	struct i2c_rdwr_ioctl_data list;
	struct i2c_msg msg[2];

	list.msgs  = msg;
	list.nmsgs = 0;

	if (out_len)
	{
		msg[list.nmsgs].addr  = I2C_EFB_ADDR;
		msg[list.nmsgs].flags = 0;
		msg[list.nmsgs].len   = out_len;
		msg[list.nmsgs].buf   = out;

		list.nmsgs++;
	}

	if (in_len)
	{
		msg[list.nmsgs].addr  = I2C_EFB_ADDR;
		msg[list.nmsgs].flags = I2C_M_RD | I2C_M_NOSTART;
		msg[list.nmsgs].len   = in_len;
		msg[list.nmsgs].buf   = in;

		list.nmsgs++;
	}

	int rc = ioctl (i2c_fd, I2C_RDWR, &list);

	return (rc == (int) list.nmsgs) ? 0 : -1;
}

extern const uint8_t _binary_firmware_jpf_start[];

static int _cpld_version_up (void)
{
	int sts = -1;

	xo2_handle_t *xo2 = NULL;

	do
	{
		if ((i2c_fd = open (I2C_EFB_DEVICE, O_RDWR)) < 0)
		{
			LOGERR("Unable to open '%s' - %s", I2C_EFB_DEVICE,
			       strerror (errno));
			break;
		}

		if (!(xo2 = xo2_open (_i2c_exchange, NULL)))
		{
			LOGERR("Unable to initialize XO2:EFB");

			break;
		}

		uint32_t dev_usercode = 0xdeadbeaf, img_usercode = 0xdeadbeaf;
		uint32_t dev_deviceid = 0xdeadbeaf, img_deviceid = 0xdeadbeaf;

		xo2_file_info (_binary_firmware_jpf_start, &img_usercode, &img_deviceid);

		if (xo2_read_device_id (xo2, &dev_deviceid))
		{
			LOGERR("Unable to read device ID");

			break;
		}

		if (dev_deviceid != img_deviceid)
		{
			LOGERR("Incompatible device ID (dev=%08xx/img=%08x)",
			       dev_deviceid, img_deviceid);
			break;
		}

		if (xo2_read_user_code (xo2, &dev_usercode))
		{
			LOGERR("Unable to read user code");

			break;
		}

		if ((dev_usercode ^ img_usercode) & 0x0000ffff)
		{
			LOGDBG("Programming CPLD version %d.%02d\n",
			       (img_usercode >> 8) & 0xff, img_usercode & 0xff);

			uint32_t mode = XO2_MODE_ERASE_CONFIG
			              | XO2_MODE_ERASE_UFM
			              | XO2_MODE_ERASE_FEATROW
			              | XO2_MODE_PROGRAM_CONFIG
			              | XO2_MODE_PROGRAM_UFM
			              | XO2_MODE_PROGRAM_FEATROW
			              | XO2_MODE_VERIFY_CONFIG
			              | XO2_MODE_VERIFY_UFM
			              | XO2_MODE_VERIFY_FEATROW
			              | XO2_MODE_OFFLINE;

			if (xo2_program (xo2, _binary_firmware_jpf_start, mode))
				LOGERR("Unable to program PLD image");
		}

		sts = 0;

	} while (0);

	if (xo2)
		xo2_close (xo2);

	if (i2c_fd >= 0)
	{
		close (i2c_fd);

		i2c_fd = -1;
	}

	return sts;
}

// Update local copy of IDE register file from CPLD
int ide_refresh (void)
{
	LOGDB3 ("IDE Refresh Registers\n");

	ide.spi_cmd  = SPI_CMD_REG_READ;
	ide.spi_arg0 = 0;
	ide.spi_arg1 = 0;
	ide.spi_arg2 = 0;

	if (spi_xfer (&ide, sizeof (ide_regs_t)) != sizeof (ide_regs_t))
		return -1;

	if ((fw_ver_major == 0) && ide.spi_arg0)
	{
		fw_ver_major = ide.spi_arg0;
		fw_ver_minor = ide.spi_arg1;

		LOGDBG("CPLD Firmware version %d.%02d\n", fw_ver_major, fw_ver_minor);
	}

	return 0;
}


// Push local copy of IDE register file to CPLD
int ide_update (int iadj, int oadj)
{
	LOGDB2 ("IDE Update Registers\n");

	ide.ififo_cnt = iadj;
	ide.ofifo_cnt = oadj;

	ide.spi_cmd  = SPI_CMD_REG_WRITE;
	ide.spi_arg0 = 0;
	ide.spi_arg1 = 0;
	ide.spi_arg2 = 0;
	ide.IRQPIn   = 1;  // disable pending IRQ to Pi

	if (spi_xfer (&ide, sizeof (ide_regs_t)) != sizeof (ide_regs_t))
		return -1;

	ide.ififo_cnt += iadj;
	ide.ofifo_cnt += oadj;

	return 0;
}


/*
 * _fixup_LBA is a general helper function that a) translates supplied
 * CHS geometry to LBA if the LBA flag isn't set, and b) translates
 * the supplied or translated LBA number to starting byte offset
 * and ending location based on supplied transfer size.
 */

static void _fixup_LBA (uint64_t *offset, uint32_t *words, uint32_t *lastsec)
{
	drive_t *drive = ide.SLAVE ? slave : master;

	if (!ide.LBAM)
	{
		uint32_t sector =  ide.lba        & 0xff;
		uint32_t head   = (ide.lba >> 24) & 0x0f;
		uint32_t cyl    = (ide.lba >>  8) & 0xffff;

		ide.lba = (cyl * drive->heads + head) * drive->spt + (sector - 1);
	}

	uint32_t scount =  ide.secz ? ide.secz : 256;

	*offset  = ((uint64_t) ide.lba << 9);
	*words   = scount << 8;
	*lastsec = ide.lba + scount - 1;

	return;
}






static void _reset_signature (void)
{
		ide.SRST = 0;
		ide.IENn = 0;

		ide.secz = 1;
		ide.lba  = 1;

		return;
}






/*
 * IDE transfers are fundamentally half-duplex - that is each
 * command code has an implicit direction and common IDE data
 * status flags are used to indicate buffer fullness.
 *
 * svc_buffer is a common (in & out) holding area for data
 * gathered from the CPLD or queued to the CPLD.  The following
 * functions handle keeping the FIFO full or drained based on
 * the pending counts in svc_iwords/svc_owords (only one can be
 * non-zero.
 *
 * It may seem odd, but there are two separate FIFOs for IN/OUT
 * in the CPLD.  This is only because a) it simplifys multiplexing
 * the end points of the FIFO, and b) there is enough FIFO space
 * to not have to work about resource constraints.
 *
 * Each FIFO (direction) has 1024 words = 2048 bytes; enough to
 * hold 4 sectors.  <256 word transfer sizes are not currently
 * used (all padded to 256), but the code shouldn't be too far
 * off from complete (testing mostly).
 *
 * The 128 KB max size below comes from a max sector transfer
 * count of 256. (256 * 512 = 128KB)
 *
 * FIFO availability is entirely maintained on the CPLD side.
 * We push adjustment values during ide_update in theses functions
 * to adjust the value maintained by the CPLD during IDE update.
 */


// Write data to the IDE PI->CPLD FIFO
int ide_write (void *data, int words)
{
	int len = words << 1;

	drive_t *drive = ide.SLAVE ? slave : master;

	uint8_t *block = (uint8_t *) alloca (len + 5);

	block[0] = SPI_CMD_FIFO_WRITE;
	block[1] = 0; // arg:0
	block[2] = (words >> 8) & 0xff;
	block[3] = (words     ) & 0xff;

	memcpy (&block[4], data, len);

	block[len + 4] = 0; // dummy byte

	if (spi_xfer (block, len + 5) != (len + 5))
		return -1;

	drive->read_diff  += words;
	drive->read_total += words;

	return words;
}


// Read data from the IDE CPLD->PI FIFO
int ide_read (void *data, int words)
{
	int len = words << 1;

	drive_t *drive = ide.SLAVE ? slave : master;

	uint8_t *block = (uint8_t *) alloca (len + 5);

	block[0] = SPI_CMD_FIFO_READ;
	block[1] = 0; // arg:0
	block[2] = (words >> 8) & 0xff;
	block[3] = (words     ) & 0xff;

	if (spi_xfer (block, len + 5) != (len + 5))
		return -1;

	memcpy (data, &block[5], len);

	drive->write_diff  += words;
	drive->write_total += words;

	return words;
}


// Holding area and status for high level data transfers
static uint8_t svc_buffer[128 * 1024], svc_cmd = 0, svc_sub = 0;
static int svc_iwords = 0, svc_owords = 0, svc_offset = 0;
static drive_t *svc_drive = NULL;

static void _proc_buffer_complete (void);

void data_in_setup (uint8_t cmd, uint8_t sub, int words)
{
	LOGDB2("Out-bound FIFO: Expecting %d words\n", words);

	svc_offset = 0;
	svc_owords = words;
	svc_cmd    = cmd;
	svc_sub    = sub;

	ide.DRQ  = DRQ_MODE_WRITESEC;

	return;
}

void *data_out_setup (int words)
{
	(void) words;

	return svc_buffer;
}

void data_out_commit (int words)
{
	LOGDB2("In-bound FIFO: Queued %d words\n", words);

	svc_offset = 0;
	svc_iwords = words;

	ide.DRQ = DRQ_MODE_READSEC;

	return;
}

// Service CPLD->PI data fifo (read from persepective of this prog)
static void service_datain (void)
{
	while (svc_owords)
	{
		/*
		 * OFIFO count is the number of words of free available
		 * space in the CPLD->PI FIFO.  Like the IFIFO check
		 * above, the XT-IDE logic will look at DRQ only before
		 * blasting 256 words into the fifo with rep movsw.  So
		 * any partial sector fragment may leave the FIFO without
		 * enough space to accept a full sector.  So only pull
		 * data out if there is a full sector to transfer.  And
		 * only atomically adjust by multiples of 256 words.
		 */

		if ((OFIFO_SIZE - ide.ofifo_cnt) < 256)
		{
			LOGDB2("Out-bound FIFO: empty - pausing\n");

			break;
		}

		int words = (svc_owords > 256) ? 256 : svc_owords;

		// Future proof for < 256 transfer sizes

		if (words > (OFIFO_SIZE - ide.ofifo_cnt))
			words = OFIFO_SIZE - ide.ofifo_cnt;

		LOGDB2("Out-bound FIFO: Reading %d words\n", words);

		ide_read (&svc_buffer[svc_offset << 1], words);

		if (verbose > 2)
			hexdump (svc_offset, &svc_buffer[svc_offset << 1], words << 1);

		svc_offset += words;
		svc_owords -= words;

		if (svc_owords == 0)
			ide.DRDY = 1;

		ide_update (0, words);
	}

	if (svc_owords == 0)
		_proc_buffer_complete ();

	return;
}


// Service PI->CPLD data fifo (write from persepective of this prog)
static void service_dataout (void)
{
	while (svc_iwords)
	{
		/*
		 * Don't write anything until there is enough space
		 * hold an entire sector.  The way the XT-IDE BIOS
		 * specifically works is as long as the DRQ flag is
		 * set, it will rep movsw 256 times.  If there is a
		 * partial sector, it will under-flow.  Writing any-
		 * thing less than a full sector here creates a
		 * timing opportunity if we cannot keep up to have
		 * only a sector fragment in the IFIFO.  This is
		 * bad.
		 *
		 * Data availability will not be reflected until we
		 * update IDE regs below with a FIFO adjustment
		 * value.
		 */

		if (ide.ififo_cnt >= (IFIFO_SIZE - 255))
		{
			LOGDB2("In-bound FIFO: full - pausing\n");

			break;
		}

		int words = (svc_iwords > 256) ? 256 : svc_iwords;

		if (words > (IFIFO_SIZE - ide.ififo_cnt))
			words = IFIFO_SIZE - ide.ififo_cnt;

		LOGDB2("In-bound FIFO: Writing %d words\n", words);

		ide_write (&svc_buffer[svc_offset << 1], words);

		svc_offset  += words;
		svc_iwords  -= words;

		ide_update (words, 0);
	}

	return;
}


static struct timeval stat_tv = { 0, 0 };

// Print thoughput numbers every seconds as long as xfers running
static void update_stats (void)
{
	struct timeval now;

	gettimeofday (&now, NULL);

	time_t delta = now.tv_sec - stat_tv.tv_sec;

	if (delta)
	{
		if (delta == 1)
		{
			if (master && (master->read_diff || master->write_diff))
			{
				LOGDBG ("STAT [MASTER]: Write=%d B/s - Read=%d B/s]\n",
				        master->write_diff << 1, master->read_diff << 1);
			}

			if (slave && (slave->read_diff || slave->write_diff))
			{
				LOGDBG ("STAT [SLAVE]:  Write=%d B/s - Read=%d B/s]\n",
				        slave->write_diff << 1, slave->read_diff << 1);
			}
		}

		if (master)
		{
			master->read_diff  = 0;
			master->write_diff = 0;
		}

		if (slave)
		{
			slave->read_diff  = 0;
			slave->write_diff = 0;
		}

		memcpy (&stat_tv, &now, sizeof (struct timeval));
	}

	return;
}


/*
 * Currently both hardware and software support limit ATA spec
 * support to ATAPI-4 / PIO mode only.  This function creates
 * an ID sector based on the supplied drive profile.
 *
 * Some of the limitations is lack of software support for LBA48,
 * lack of hardware implementation for interrupt generation (hw
 * supports it) and lack of hardware signal support for DMA/UDMA.
 */

static void _flip_words (uint16_t *word, int count)
{
	while (count)
	{
		uint16_t new = ((*word & 0xff) << 8) | ((*word >> 8) & 0xff);

		*word = new;

		count--;
		word++;
	}

	return;
}

static uint8_t ide_multiple = 1;

static void _create_idsector (uint16_t *sector, drive_t *drive)
{
	memset (sector, 0, 512);

	sector[0] = 0x044a;           // General config: ATA, non-removable
	sector[1] = drive->cyls;      // Number of logical cylinders
	sector[3] = drive->heads;     // Number of logical heads
	sector[6] = drive->spt;       // Number of logical sectors per track

	sprintf ((char *) &sector[10], "%-20d", serial_no);
	sprintf ((char *) &sector[23], "%-8s",  DRIVE_FW_VERSION);
	sprintf ((char *) &sector[27], "%-40s", drive->label);

	_flip_words (&sector[10], 10);
	_flip_words (&sector[23], 4);
	_flip_words (&sector[27], 20);

	sector[47] = 0x8004;          // Maximum # sectors per R/W multiple
	sector[49] = 0x0700;          // Capabilities - LBA / no IORDY
	sector[50] = 0x4000;          // Capabilities -
	sector[51] = 0x0200;          // PIO-2 data transfer mode
	sector[53] = 0x0007;          // 54-58, 64-70, 88 words valid
	sector[54] = drive->cyls;     // Current number of cylinders
	sector[55] = drive->heads;    // Current number of heads
	sector[56] = drive->spt;      // Current number of sectors per track
	sector[57] = drive->sectors & 0xffff;  // capacity in sectors
	sector[58] = (drive->sectors >> 16) & 0xffff;
	sector[59] = 0x0100 | ide_multiple; // Mutiple sector valid
	sector[60] = sector[57];      // number of user addressible sectors low
	sector[61] = sector[58];      // number of user addressible sectors high
	sector[64] = 0x0000;          // Does not support PIO3&4
	sector[65] = 120;             // (ns) Minimum multiword DMA time
	sector[66] = 120;             // (ns) Manufacturers suggested DMA time
	sector[67] = 120;             // (ns) Minimum PIO time w/o IORDY
	sector[68] = 120;             // (ns) Minimum PIO time w/ IORDY
	sector[80] = 0x0004;          // Supports ATA/ATAPI-4
	sector[81] = 0x0017;          // ATAPI-4 Version T13 1153D revision 17
 	sector[82] = 0x7000;          // Sp CMDs: NOP/READ/WRITE
	sector[83] = 0x4000;          // Sp CMDs:
	sector[84] = 0x4000;          // Sp CMDs:
	sector[85] = 0x7000;          // En CMDs: NOP/READ/WRITE
	sector[86] = 0x4000;          // En CMDs:
	sector[87] = 0x4000;          // En CMDs:

	return;
}


static void _proc_buffer_complete (void)
{
	LOGDB2("Proc buffer complete\n");

	switch (svc_cmd)
	{
	case IDE_CMD_WRITE_SEC:
	case IDE_CMD_WRITE_SEC_NORETRY:
	case IDE_CMD_WRITE_MULTIPLE:

		if (svc_drive)
		{
			write (svc_drive->fd, svc_buffer, svc_offset << 1);
			fdatasync (svc_drive->fd);


// error handling


		}

		break;

	case IDE_CMD_REDIRECTOR:
		_handle_redirect_complete (svc_sub, svc_buffer, svc_offset << 1);
		break;

	default:
		LOGDBG("Buffer received (%d) - unknown IDE command %02Xh\n",
			  svc_offset, (int) svc_cmd);
		break;
	}

	svc_cmd = 0;
	svc_sub = 0;
	svc_drive = NULL;

	return;
}


/*
 * This is where all the magic happens.  It is the handler for
 * when the IDE command register goes from 0 -> CMD.  The IDE
 * command is processes here and dispatched for other functions
 * to cleanup.
 */

static void process_ide_command (void)
{
	uint32_t words = 0;
	uint64_t offset = 0;
	uint32_t lastsec = 0;

	drive_t *drive = ide.SLAVE ? slave : master;

	/*
	 * Default good status for all handled commands
	 *   unless overridden by the default switch handler below
	 */

	ide.BSY  = 0;  // Command complete - not busy
	ide.DSC  = 1;  // Seek complete
	ide.DRDY = 1;  // Drive ready
	ide.ABRT = 0;  // Command not-aborted
	ide.DF   = 0;  // Drive not faulted
	ide.IDNF = 0;  // Sector found
	ide.IRQACT = 0;
	ide.DRQ  = DRQ_MODE_OFF;


	/*
	 * Abort commands issued to non-existent drives even
	 *   though this doesn't work this way in the real world
	 *   as a missing but selected drive would time-out.
	 */

	if (ide.cmd && ((ide.cmd & 0xf0) != 0x80)) // exclude vendor
	{
		if ((!ide.SLAVE && (master == NULL)) ||
		    ( ide.SLAVE && (slave  == NULL)))
		{			
			LOGDBG("Aborting CMD %02Xh to %s - does not exist\n",
				  ide.cmd, ide.SLAVE ? "SLAVE" : "MASTER");

			ide.DRDY = 0; // Drive not ready
			ide.ABRT = 1; // Command aborted

			ide.cmd = 0;

			ide_update (0, 0);

			return;
		}
	}


	LOGDBG("IDE-CMD: %s ", ide.SLAVE ? "[SLAVE ]" : "[MASTER]");

	switch (ide.cmd)
	{
	// ******** COMMAND ONLY MODE (no data transfer) ********

	case IDE_CMD_NULL:
		LOGDBG("NOP\n");
		break;

	case IDE_CMD_VERIFY_SEC:
	case IDE_CMD_VERIFY_SEC_NORETRY:
	{
		_fixup_LBA (&offset, &words, &lastsec);

		LOGDBG("READ VERIFY %s(LBA 0x%x / count %d)\n",
		       (ide.cmd == IDE_CMD_VERIFY_SEC_NORETRY) ?
		       "NO-RETRY " : "", ide.lba, ide.secz ? ide.secz : 256);

		if (lastsec > drive->sectors)
		{
			LOGDBG("   - Last sector past end of device\n");

			ide.DSC  = 0;  // Seek not complete
			ide.IDNF = 1;  // Sector not found

			break;
		}

		break;
	}

	case IDE_CMD_SEEK ... (IDE_CMD_SEEK + 0x0f):
	{
		ide.secz = 1; // always assume 1 block for sanity check

		_fixup_LBA (&offset, &words, &lastsec);

		LOGDBG("SEEK step=%d (LBA 0x%x)\n", ide.cmd & 0x0f, ide.lba);

		if (lastsec > drive->sectors)
		{
			LOGDBG("   - Last sector past end of device\n");

			ide.DSC  = 0;  // Seek not complete
			ide.IDNF = 1;  // Sector not found

			break;
		}

		break;
	}

	case IDE_CMD_EXEC_DIAG:
	{
		LOGDBG("EXECUTE DIAGNOSTICS (%sIRQ)\n",
		       ide.IENn ? "NOn-" : "");

		/*
		 * Special in that the error register should contain a
		 * code after BSY completion rather than the normal bits.
		 * This is handled by using the EMUX bit to control how
		 * the error register is presented to the host.
		 *
		 *    0 = Normal error return bits
		 *    1 = Present the feature reg instead (set here)
		 *
		 * Diagnostic return codes:
		 *
		 *    01h = no error detected
		 *    03h = sector buffer error
		 */

		ide.feat = 0x01;
		ide.EMUX = 1;       // Cleared on next command write
		ide.IRQACT = 1;

		_reset_signature ();

		break;
	}

	case IDE_CMD_SET_DRIVE_PARM:
	{
		int spt   = ide.lba & 0xff;
		int heads = (ide.lba >> 24) & 0x0f;

//		drive->spt   = spt;
//		drive->heads = heads;
		drive->cyls  = drive->sectors / (spt * heads);

		LOGDBG("INITIALIZE DEVICE PARAMETERS (SPT=%d/H=%d) = %d\n",
		       spt, heads, drive->cyls);

		break;
	}

	case IDE_CMD_FLUSH_CACHE:

		LOGDBG("FLUSH CACHE\n");

		fdatasync (drive->fd);

		break;

	case IDE_CMD_SET_FEATURES:

		LOGDBG("SET FEATURES: ");

		switch (ide.feat)
		{
		case 0x02:
			LOGDBG("ENABLE WRITE CACHE\n");
			// Always enabled
			break;

		case 0x82:
			LOGDBG("DISABLE WRITE CACHE\n");
			// Ignored
			break;

		case 0x05:
			LOGDBG("ENABLE ADVANCED POWER MANAGEMENT\n");
			// Always handled internally
			break;

		case 0xAA:
			LOGDBG("ENABLE READ LOOK-AHEAD\n");
			// Always enabled
			break;

		case 0x03:
			LOGDBG("SET TRANSFER MODE: %02Xh\n", ide.secz);

			if (ide.secz >= 0x20) // no DMA support
			{
				ide.DRDY = 0; // Drive not ready
				ide.ABRT = 1; // Command aborted
			}

			break;

		default:
			LOGDBG("Unhandled %02Xh\n", ide.feat);

			ide.DRDY = 0; // Drive not ready
			ide.ABRT = 1; // Command aborted

			break;
		}

		break;


	case IDE_CMD_GET_MAX_ADDRESS:

		LOGDBG("READ NATIVE MAX ADDRESS\n");

		if (ide.LBAM)
		{
			ide.lba = drive->sectors;
		}
		else
		{
			uint32_t max_sec  = (drive->spt < 0x100) ? drive->spt - 1 : 0xff;
			uint32_t max_cyl  = (drive->cyls < 0x10000) ? drive->cyls - 1 : 0xffff;
			uint32_t max_head = (drive->heads < 0x10) ? drive->heads - 1 : 0xf;

			ide.lba = (max_head << 24) | (max_cyl << 8) | max_sec;
		}

		break;


	case IDE_CMD_SET_MAX_ADDRESS:

		LOGDBG("SET MAX ADDRESS");

		if (ide.secz & 1)
		{
			LOGDBG("\n***** WARNING: Persistent bit set and unsupported *****\n")
		}

		if (ide.LBAM)
		{
			if (ide.lba >= drive->sectors)
			{
				ide.DRDY = 0; // Drive not ready
				ide.ABRT = 1; // Command aborted

				LOGDBG(" - exceeds capacity (%u)\n", ide.lba);

				break;
			}

			drive->sectors = ide.lba;

			LOGDBG(" - set to %u sectors\n", drive->sectors);
		}
		else
		{
			uint32_t spt   = ( ide.lba        & 0xff) + 1;
			uint32_t heads = ((ide.lba >> 24) & 0x0f) + 1;
			uint32_t cyls  = ((ide.lba >>  8) & 0xffff) + 1;

			ide.lba = spt * heads * cyls;

			if (ide.lba >= drive->sectors)
			{
				ide.DRDY = 0; // Drive not ready
				ide.ABRT = 1; // Command aborted

				LOGDBG(" - exceeds capacity (%u)\n", ide.lba);

				break;
			}

			drive->spt   = spt;
			drive->heads = heads;
			drive->cyls  = cyls;

			LOGDBG(" - set to CHS %u/%d/%d (%u sectors)\n",
				  cyls, heads, spt, drive->sectors);
		}

		break;


	case IDE_CMD_SET_MULTIPLE_MODE:

		LOGDBG("SET MULTIPLE MODE (%d sectors / block)\n", ide.secz);

		ide_multiple = ide.secz;

		break;


	// ******** PIO-IN MODE ********

	case IDE_CMD_READ_SEC:
	case IDE_CMD_READ_SEC_NORETRY:
	{
		_fixup_LBA (&offset, &words, &lastsec);

		LOGDBG("READ %s(LBA 0x%x / count %d) [PIO-IN]\n",
		       (ide.cmd == IDE_CMD_READ_SEC_NORETRY) ?
		       "NO-RETRY " : "", ide.lba,
		       ide.secz ? ide.secz : 256);

		if (lastsec > drive->sectors)
		{
			LOGDBG("   - Last sector past end of device\n");

			ide.DSC  = 0;  // Seek not complete
			ide.IDNF = 1;  // Sector not found

			break;
		}

// fix me - error handling

		lseek (drive->fd, offset, SEEK_SET);
		read (drive->fd, svc_buffer, words << 1);

		data_out_commit (words);

		break;
	}

	case IDE_CMD_READ_MULTIPLE:
	{
		_fixup_LBA (&offset, &words, &lastsec);

		LOGDBG("READ MULTIPLE (LBA 0x%x / count %d (%d)) [PIO-IN]\n",
		       ide.lba, ide.secz ? ide.secz : 256, ide_multiple);

		if (lastsec > drive->sectors)
		{
			LOGDBG("   - Last sector past end of device\n");

			ide.DSC  = 0;  // Seek not complete
			ide.IDNF = 1;  // Sector not found

			break;
		}

// fix me - error handling

		lseek (drive->fd, offset, SEEK_SET);
		read (drive->fd, svc_buffer, words << 1);

		data_out_commit (words);

		break;
	}

	case IDE_CMD_PULL_REQUEST:
	{
		char *path = NULL;

		switch (ide.feat)
		{
		case 0: path = "bin/optbios.bin"; break;

		default:
		case 1: path = "bin/pimount.com"; break;

		case 2: path = "bin/picon.com"; break;
		}

		LOGDBG("PULL REQUEST - %s [PIO-IN]\n", path);

		int fd = open (path, O_RDONLY);

		if (fd < 0)
		{
			LOGDBG("   - Unable to open %s\n", path);

			ide.DSC  = 0;  // Seek not complete
			ide.IDNF = 1;  // Sector not found

			break;
		}

		ide.lba = (read (fd, svc_buffer, sizeof (svc_buffer)) + 1) / 2;

		close (fd);

		data_out_commit (ide.lba);

		break;
	}

	case IDE_CMD_IDENTIFY:
	{
		LOGDBG("IDENTIFY DEVICE [PIO-IN]\n");

		_create_idsector ((uint16_t *) svc_buffer, drive);

		LOGDB2("In-bound FIFO: Queued 256 words\n");

		svc_offset = 0;
		svc_iwords = 256;

		ide.DRQ = DRQ_MODE_READSEC;

		break;
	}


	// ******** PIO-OUT MODE ********

	case IDE_CMD_WRITE_SEC:
	case IDE_CMD_WRITE_SEC_NORETRY:
	{
		_fixup_LBA (&offset, &words, &lastsec);

		LOGDBG("WRITE %s(LBA 0x%x / count %d) [PIO-OUT]\n",
		       (ide.cmd == IDE_CMD_WRITE_SEC_NORETRY) ?
		       "NO-RETRY" : "", ide.lba,
		       ide.secz ? ide.secz : 256);

		if (lastsec > drive->sectors)
		{
			LOGDBG("   - Last sector past end of device\n");

			ide.DSC  = 0;  // Seek not complete
			ide.IDNF = 1;  // Sector not found

			break;
		}

		if (drive->readonly)
		{
			LOGDBG("   - Drive is read-only\n");

			ide.DF = 1;  // Drive faulted

			break;
		}

// fix me - error handling

		lseek (drive->fd, offset, SEEK_SET);

		svc_drive = ide.SLAVE ? slave : master;

		data_in_setup (ide.cmd, 0, words);

		break;
	}


	case IDE_CMD_WRITE_MULTIPLE:
	{
		_fixup_LBA (&offset, &words, &lastsec);

		LOGDBG("WRITE MULTIPLE (LBA 0x%x / count %d (%d)) [PIO-OUT]\n",
		       ide.lba, ide.secz ? ide.secz : 256, ide_multiple);

		if (lastsec > drive->sectors)
		{
			LOGDBG("   - Last sector past end of device\n");

			ide.DSC  = 0;  // Seek not complete
			ide.IDNF = 1;  // Sector not found

			break;
		}

		if (drive->readonly)
		{
			LOGDBG("   - Drive is read-only\n");

			ide.DF = 1;  // Drive faulted

			break;
		}

// fix me - error handling

		lseek (drive->fd, offset, SEEK_SET);

		svc_drive = ide.SLAVE ? slave : master;

		data_in_setup (ide.cmd, 0, words);

		break;
	}


	// ******** DOS REDIRECTOR SERVICE ********

	case IDE_CMD_REDIRECTOR:
		_handle_redirect_command ();
		break;


	// ******** ABORT MODE ********

	default:
		LOGDBG("Unhandled IDE CMD %02Xh\n", ide.cmd);

		ide.DRDY = 0; // Drive not ready
		ide.ABRT = 1; // Command aborted

		break;
	}

  	ide.cmd = 0; // Clear active command

	ide_update (0, 0);

	return;
}


static void _program_usage (const char *name)
{
	fprintf (stderr,
	         "\nUsage: %s <options>\n\n"
	         "Options:\n\n"
	         "\t<-v|--verbose>       Increase logging verbosity\n"
	         "\t<-u|--upgrade>       Perform CPLD version/upgrade check\n"
	         "\t<-c|--config>        Specify path to config file\n"
	         "\n", name);
	return;
}


static int done = 0;

static void _signal_handler (int signo)
{
	done = 1;

	return;
}


int main (int argc, char *argv[])
{
	const char *progname = basename (argv[0]);
	char *config_path = "./netpi.cfg";
	int upgrade_cpld = 0;
	int sts = -1;

	argc--; argv++;

	while (argc)
	{
		if ((strcmp (argv[0], "-v") == 0) ||
		    (strcmp (argv[0], "--verbose") == 0))
		{
			verbose++;
		}
		else if ((strcmp (argv[0], "-u") == 0) ||
		         (strcmp (argv[0], "--upgrade") == 0))
		{
			upgrade_cpld = 1;
		}
		else if (((strcmp (argv[0], "-c") == 0) ||
		          (strcmp (argv[0], "--config") == 0)) && (argc > 1))
		{
			argc--; argv++;

			config_path = argv[0];
		}
		else
		{
			_program_usage (progname);

			return -1;
		}

		argc--; argv++;
	}









	serial_no = 666;





	if (config_load (config_path))
		return -1;

	master = config_drive_init (1);
	slave  = config_drive_init (0);

	init_redirector ();

	do
	{
		if (upgrade_cpld && _cpld_version_up())
			break;

		if (spi_init (1, SPI_BITRATE))
			break;

		if (gpio_init ())
			break;

		// register graceful exit signals

		struct sigaction sigvector;

		memset (&sigvector, 0, sizeof (sigvector));

		sigvector.sa_handler = _signal_handler;
		sigvector.sa_flags = 0;
		sigemptyset (&sigvector.sa_mask);

		sigaction (SIGINT,  &sigvector, NULL);
		sigaction (SIGTERM, &sigvector, NULL);
		sigaction (SIGQUIT, &sigvector, NULL);


		// Main loop

		while (!done)
		{
			ide_refresh ();

			update_stats ();

			// Always process reset requests first

			if (ide.SRSTP)
			{
				LOGDBG("Processing %s reset request\n", ide.SRST ? "SW" : "HW");

				memset (&ide, 0, sizeof (ide_regs_t));

				ide.DRDY   = 1;
				ide.IRQPIn = 1;

				ide.lba = 1; // sector 1 non-LBA

				svc_offset = 0;
				svc_iwords = 0;
				svc_owords = 0;

				ide_update (0, 0);
			}
			else if (svc_iwords)
				service_dataout ();
			else if (svc_owords)
				service_datain ();
			else if (ide.cmd)
				process_ide_command ();
			else
				gpio_sleep ();
		}

		sts = 0;

	} while (0);

	LOGDBG("Shutting down\n");

	if (master)
		config_drive_term (master);

	if (slave)
		config_drive_term (slave);

	gpio_term ();
	spi_term ();

	config_free ();

	return sts;
}