/*\
 *
 * SectorCache «Freeware» © Steven Norburn <sndev@bigfoot.com>.
 *
 *	This converts sector reads and writes, it also has a cache and more.
 *
 *	The cache was inspired by the great work of David Le Blanc on SmartDisk 1.3.
 *
 * History,
 *
 * 0.10ß (24.09.00)
 *	First release.
 *
\*/

#define __USE_SYSBASE

#include <proto/exec.h>
#include <proto/dos.h>

#include <exec/io.h>
#include <exec/errors.h>
#include <devices/newstyle.h>
#include <devices/scsidisk.h>
#include <devices/trackdisk.h>

#define PROG "SectorCache"
#define VERSION "«Freeware» 0.10ß"
#define DATE "24.09.01"
#define COPYRIGHT "© Steven Norburn <sndev@bigfoot.com>"

static char version_string[] =
	"$VER: "PROG" "VERSION" ("DATE") " COPYRIGHT;

static char template[] =
	"Device/A,Unit/A/N,"
	"E=EmuSize/N,D=DevSize/N,C=CacheBlock/N,L=LineLength/N,S=SetSize/N,"
	"ETD64/S,NSD64/S,TD64/S,SD=SCSIDirect/S,"
	"DType/N,Chip/S,"
	"NDR=NoDirectRead/S,NDW=NoDirectWrite/S,"
	"NC=NoConvert/S";

enum
{
	ARG_DEVICE,
	ARG_BU,
	ARG_E,
	ARG_D,
	ARG_C,
	ARG_L,
	ARG_S,
	ARG_ETD64,
	ARG_NSD64,
	ARG_TD64,
	ARG_SD,
	ARG_DTYPE,
	ARG_CHIP,
	ARG_NDR,
	ARG_NDW,
	ARG_NC,
	NUM_ARGS
};

LONG	arg_array[NUM_ARGS];

struct	ExecBase *SysBase;
struct	DosLibrary *DOSBase;

#define	TD_READ64	24
#define	TD_WRITE64	25
#define	TD_SEEK64	26
#define	TD_FORMAT64	27
#define	io_HighOffset	io_Actual

#define NSCMD_TDF_EXTCOM (1<<13) /* for internal use only! */

#define	SCSI_READ6	0x08
#define	SCSI_READ10	0x28
#define	SCSI_READ12	0xA8
#define	SCSI_READ16	0x88
#define	SCSI_WRITE6	0x0A
#define	SCSI_WRITE10	0x2A
#define	SCSI_WRITE12	0xAA
#define	SCSI_WRITE16	0x8A
#define	SCSI_INQUIRY	0x12 /* 00 00 00 38 00 */
#define	SCSI_READCAPACITY	0x25 /* 00 00 00 00 00 00 00 00 00 */

enum RW6 {
	RW6_OPCODE,
	RW6_LBA1,	/* Bits 7, 6, 5 RESERVED */
	RW6_LBA2,
	RW6_LBA3,
	RW6_TLEN1,
	RW6_CONTROL
};

enum RW10 {
	RW10_OPCODE,
	RW10_FLAGS,	/* Bit 4 = DPO, 3 = FUA, 0 = RELADR */
	RW10_LSB1, RW10_LBA2, RW10_LBA3, RW10_LBA4,
	RW10_RESERVED,
	RW10_TLEN1, RW10_TLEN2,
	RW10_CONTROL
};

enum RW12 {
	RW12_OPCODE,
	RW12_FLAGS,
	RW12_LSB1, RW12_LBA2, RW12_LBA3, RW12_LBA4,
	RW12_TLEN1, RW12_TLEN2, RW12_TLEN3, RW12_TLEN4,
	RW12_RESERVED,
	RW12_CONTROL
};
enum RW16 {
	RW16_OPCODE,
	RW16_FLAGS,
	RW16_LSB1, RW16_LBA2, RW16_LBA3, RW16_LBA4, RW16_LSB5, RW16_LBA6, RW16_LBA7, RW16_LBA8,
	RW16_TLEN1, RW16_TLEN2, RW16_TLEN3, RW16_TLEN4,
	RW16_RESERVED,
	RW16_CONTROL
};

#define pwrBase	9	/* This is the smallest sector size 512 */
#define secBase 1<<pwrBase

struct	Unit *unitID;

void (*oldbeginio)(register __a1 struct IOStdReq *, register __a6 struct Device *dev);

struct lineCache {
	ULONG	key;		/* Item key */
	ULONG	age;		/* AGE for LRU alg */
	UBYTE	*buffer;	/* [secCache << pwrBase] of DATA */
	ULONG	pad;
};


UWORD	pwrEmulate;		/* The Sector size for the Filesystem */
UWORD	secEmulate;

UWORD	pwrDevice;		/* The Sector size of the Device */
UWORD	secDevice;

UWORD	pwrCache;		/* The Sector size of the Cache */
UWORD	secCache;

UWORD	pwrLines;		/* The number the Caches */
UWORD	secLines;

UWORD	secSets;		/* The number of groups of Caches */

ULONG	cacheBytes;
ULONG	deviceBytes;


struct	IOStdReq *IO;		/* IO goes through here now. */
struct	SignalSemaphore ss;	/* To force single threading */

ULONG	counter;
UWORD	allocnum;

struct lineCache *cache;

struct DriveGeometry geo;

UWORD	readCmd, writeCmd, seekCmd, formatCmd;

ULONG	diskChangeNum, diskChangeLast;

ULONG	cachedRead, directRead,
	cachedWrite, directWrite;


 /*--------------*\
<*> Startup Code <*>
 \*--------------*/

int Main(void);

int __saveds main(void) /* __saveds Restore Globel Data Pointer ! */
{
int	ret = RETURN_FAIL;
struct	RDArgs *rdargs;

	SysBase = (*(struct ExecBase **) 4);

	if((DOSBase = (struct DosLibrary *)OpenLibrary(DOSNAME, 37L)))
	{
		if(!(rdargs = ReadArgs(template, arg_array, NULL))){
		//	Printf("Usage: "PROG" %s\n", template);
			Printf("\n%s\n\nUsage: "PROG" %s\n", &version_string[6], template);
		//	Printf("%s\n", &version_string[6]);
			ret = RETURN_WARN;
		}
		else{
			ret = Main();
			FreeArgs(rdargs);
		}
		CloseLibrary((struct Library *)DOSBase);
	}
	return(ret);
}


 /*--------*\
<*> old IO <*>
 \*--------*/

struct	SCSICmd scsiCmd;	/* where the actual SCSI command goes */
//UBYTE	scsiSense[20];		/* buffer for request sense data */
UBYTE	scsiOpcode[12];		/* Using 12 byte commands */

ULONG oldIO(UWORD command, ULONG sector, UBYTE *buffer, ULONG length)
{
ULONG	ret;
struct	MsgPort *port;
struct	Device *dev = IO->io_Device;	/* StormC bug fix ! */

	if(!(port = CreateMsgPort()))
		return(FALSE);

	IO->io_Message.mn_ReplyPort = port;
	IO->io_Flags = IOF_QUICK;

	if(command == SCSI_READ12 || command == SCSI_WRITE12) {
	ULONG	*sec = (ULONG *)(scsiOpcode+(RW12_LSB1));
	ULONG	*len = (ULONG *)(scsiOpcode+(RW12_TLEN1));

		*sec = sector >> pwrDevice;
		*len = length >> (pwrBase + pwrDevice);

		scsiOpcode[RW12_OPCODE] = command;
							/* set expected data direction */
		scsiCmd.scsi_Flags = (command == SCSI_WRITE12) ? SCSIF_WRITE : SCSIF_READ;

		if(scsiCmd.scsi_SenseLength != 0)	/* do automatic REQUEST_SENSE */
			scsiCmd.scsi_Flags |= SCSIF_AUTOSENSE;

		IO->io_Command = HD_SCSICMD;		/* the command we are sending */
		IO->io_Length = sizeof(struct SCSICmd);
		IO->io_Data = (APTR)&scsiCmd;

		scsiCmd.scsi_Data = (UWORD *)buffer;
		scsiCmd.scsi_Length = length;		/* how much we will accept */

		scsiCmd.scsi_Command = scsiOpcode;	/* issuing this SCSI command */
		scsiCmd.scsi_CmdLength = 12;		/* length of the command */

		oldbeginio(IO, dev);

		if( (IO->io_Flags & IOF_QUICK) == 0)
			WaitIO((IORequest *)IO);

		ret = scsiCmd.scsi_Actual;

	} else {

		IO->io_Command = command;

		IO->io_HighOffset = sector >> (32 - pwrBase);
		IO->io_Offset = sector << pwrBase;
		IO->io_Length = length;
		IO->io_Data = buffer;

		oldbeginio(IO, dev);

		if( (IO->io_Flags & IOF_QUICK) == 0)
			WaitIO((IORequest *)IO);

		ret = IO->io_Actual;
	}
	DeleteMsgPort(port);

	return(ret);
}


 /*------------------*\
<*> FreeCacheBuffers <*>
 \*------------------*/

ULONG	cacheFlush;

void FreeCacheBuffers(void)
{
struct	lineCache *lc;
UWORD	set;

	cacheFlush++;

	lc = cache;
	for(set = secSets << pwrLines; set != 0; lc++, set--)
		if(lc->buffer != NULL) {
			FreeMem(lc->buffer, cacheBytes);
			lc->buffer = NULL;
		}
}


 /*----------------*\
<*> Get Cache Line <*>
 \*----------------*/

ULONG	cacheMade;
ULONG	cacheHit;
ULONG	cacheMiss;

struct lineCache *GetCacheLine(UWORD rCmd, ULONG sector)
{
struct	lineCache *lc, *oldlc;
UWORD	set;
ULONG	age;
UWORD	oldest = 0;
ULONG	line = sector >> pwrCache;
ULONG	key = sector & ~(secCache - 1);

	if(diskChangeNum != diskChangeLast) {
		diskChangeLast = diskChangeNum;
		FreeCacheBuffers();
	}

	lc = &cache[line & (secLines - 1)];

	set = secSets;
	while(set != 0 && lc->buffer != NULL && lc->key != key) {

		/*\
		 * $FFFF - $FFFC = $0003, if counter has wrapped to zero,
		 * $0000 - $FFFC = $0004, so the age is still correct.
		\*/
		age = counter - lc->age;

		if (age > oldest)
			oldest = age, oldlc = lc;

		set--, lc += secLines;
	}

	if(set == 0) lc = oldlc;	/* Re-Use oldest Entry */

	if(lc->buffer == NULL) {
	ULONG memType = MEMF_PUBLIC;

		if(arg_array[ARG_CHIP])
			memType = MEMF_PUBLIC | MEMF_CHIP;

		if(!(lc->buffer = (UBYTE *)AllocMem(cacheBytes, memType)))
			return(NULL);

		set = 0;		/* Not already in Cache */
		cacheMade++;
	}

	lc->age = counter++;

	if(set == 0) {	/* if Not already in Cache */

		if(oldIO(rCmd, key, lc->buffer, cacheBytes) == 0) {
			lc->key = ~0;	// invalid key
			return(NULL);
		}

		lc->key = key;
		cacheMiss++;
	} else
		cacheHit++;

	return(lc);
}


 /*--------------------*\
<*> Update Cache Lines <*>
 \*--------------------*/

struct lineCache *UpdateCacheLines(ULONG sector, UBYTE *buffer, ULONG length)
{
struct	lineCache *lc;
UWORD	set;
ULONG	line = sector >> pwrCache;
ULONG	nextsector = sector & (secCache - 1);
ULONG	key = sector - nextsector;

ULONG	startbyte = nextsector << pwrBase;
ULONG	sizebytes = cacheBytes - startbyte;

	while(length != 0)
	{
		lc = &cache[line & (secLines - 1)];

		if(length < sizebytes)
			sizebytes = length;

		set = secSets;
		while(set != 0 && lc->buffer != NULL) {

			if(lc->key == key) {
				if(buffer != NULL) {
					lc->age = counter++;
					CopyMem(buffer, lc->buffer + startbyte, sizebytes);
				} else
					lc->key = ~0;	// invalid key
				break;
			}
			set--, lc += secLines;
		}
		line++;
		key += secCache;

		if(buffer != NULL) buffer += sizebytes;
		length -= sizebytes;

		startbyte = 0;
		sizebytes = cacheBytes;
	}
	return(lc);
}


 /*------------*\
<*> Cache Read <*>
 \*------------*/

ULONG CacheRead(UWORD rCmd, ULONG sector, UBYTE *buffer, ULONG length)
{
ULONG	actual = length;

	while(length != 0) {
	struct	lineCache *lc;
	ULONG	nextsector, startbyte, sizebytes, nextdevice;

		nextsector = sector & (secCache - 1);
		startbyte = nextsector << pwrBase;
		sizebytes = cacheBytes - startbyte;

		nextdevice = sector & (secDevice - 1);

		if(length < sizebytes)
			sizebytes = length;

		if(arg_array[ARG_NDR] == 0 && nextdevice == 0 && length > sizebytes) {
			/* Read lots of alined Sectors and update cache */

			sizebytes = length & ~(deviceBytes - 1);

			if((sizebytes = oldIO(rCmd, sector, buffer, sizebytes)) == 0)
				break;

			UpdateCacheLines(sector, buffer, sizebytes);

			buffer += sizebytes;
			length -= sizebytes;

			sector += (sizebytes >> pwrBase);

			directRead++;

		} else {
			/* Read a few Sectors from the alined cache */

			if((lc = GetCacheLine(rCmd, sector)) == NULL)
				break;

			CopyMem(lc->buffer + startbyte, buffer, sizebytes);

			buffer += sizebytes;
			length -= sizebytes;

			sector += secCache - nextsector;

			cachedRead++;
		}
	}
	return(actual - length);
}


 /*-------------*\
<*> Cache Write <*>
 \*-------------*/

ULONG CacheWrite(UWORD wCmd, UWORD rCmd, ULONG sector, UBYTE *buffer, ULONG length)
{
ULONG	actual = length;

	while(length != 0) {
	struct	lineCache *lc;
	ULONG	devicemask = deviceBytes - 1;
	ULONG	nextsector, startbyte, sizebytes, nextdevice;

		nextsector = sector & (secCache - 1);
		startbyte = nextsector << pwrBase;
		sizebytes = cacheBytes - startbyte;

		nextdevice = sector & (secDevice - 1);

		if(length < sizebytes)
			sizebytes = length;

		if(arg_array[ARG_NDW] == 0 && nextdevice == 0 && ((length & devicemask) == 0 || length > sizebytes)) {
			/* Write lots of alined Sectors and update cache */

			sizebytes = ((length - sizebytes) & ~(cacheBytes - 1)) + sizebytes;

			if((sizebytes = oldIO(wCmd, sector, buffer, sizebytes)) == 0)
				break;

			UpdateCacheLines(sector, buffer, sizebytes);

			buffer += sizebytes;
			length -= sizebytes;

			sector += (sizebytes >> pwrBase);

			directWrite++;

		} else {
			/* Write a few Sectors from the alined cache */

			if((lc = GetCacheLine(rCmd, sector)) == NULL)
				break;

			CopyMem(buffer, lc->buffer + startbyte, sizebytes);

			buffer += sizebytes;
			length -= sizebytes;

			startbyte = startbyte & ~devicemask;
			sizebytes = (devicemask + sizebytes) & ~devicemask;

			if(sizebytes != oldIO(wCmd, sector - nextdevice, lc->buffer + startbyte, sizebytes))
				break;

			sector += secCache - nextsector;

			cachedWrite++;
		}
	}
	return(actual-length);
}


 /*-----------------------------*\
<*> Release Semaphore and Reply <*>
 \*-----------------------------*/

void ReleaseSemaphoreReply(struct IOStdReq *req)
{
	req->io_Actual = IO->io_Actual;
	req->io_Error = IO->io_Error;

	if( (req->io_Flags & IOF_QUICK) == 0)
		ReplyMsg((struct Message *) req);

	ReleaseSemaphore(&ss);
}


 /*----------------------*\
<*> Get Geometry Command <*>
 \*----------------------*/

void GetGeometryCmd(UWORD gCmd, struct IOStdReq *req)
{
ULONG	sector = 0;
struct	DriveGeometry *geo = req->io_Data;
ULONG	length = req->io_Length;

	ObtainSemaphore(&ss);

	oldIO(gCmd, sector, (UBYTE *)geo, length);

	/*\
	 *  1. TotalSectors
	 *  2. Cylinders and CylSectors
	 *  3. Cylinders, Heads, and TrackSectors.
	 *  #1 is most accurate, #2 is less so, and #3 is least accurate.
	\*/
	geo->dg_SectorSize = secEmulate << pwrBase; 	/* in bytes */

	/* total # of sectors on drive */
	geo->dg_TotalSectors = geo->dg_TotalSectors << (pwrDevice - pwrEmulate);

	/* number of sectors/cylinder */
	geo->dg_CylSectors = geo->dg_CylSectors << (pwrDevice - pwrEmulate);

	/* number of sectors/track */
	geo->dg_TrackSectors = geo->dg_TrackSectors << (pwrDevice - pwrEmulate);

	 /* codes as defined in the SCSI-2 spec*/
	if(arg_array[ARG_DTYPE])
		geo->dg_DeviceType = *(ULONG*) arg_array[ARG_DTYPE];

	ReleaseSemaphoreReply(req);
}


 /*------------------*\
<*> Do SCSI Commands <*>
 \*------------------*/

void DoSCSICmd(UWORD sCmd, struct IOStdReq *req)
{
ULONG	sector, length;
struct SCSICmd *HDCmd = req->io_Data;
UBYTE	*scsi = HDCmd->scsi_Command;

UBYTE	opcode = scsi[0];
UBYTE	baseop;
	baseop = opcode; baseop &= ~0xA0; /* remove size */

	ObtainSemaphore(&ss);

	if(baseop == SCSI_READ6 || baseop == SCSI_WRITE6) {
	UBYTE	opsize;
	UBYTE	*buffer = (UBYTE *)HDCmd->scsi_Data;
	UWORD	rCmd, wCmd;

		if(HDCmd->scsi_Flags & (SCSIF_AUTOSENSE | SCSIF_OLDAUTOSENSE)) {

			scsiCmd.scsi_SenseData = HDCmd->scsi_SenseData;		/* where sense data will go */
			scsiCmd.scsi_SenseLength = HDCmd->scsi_SenseLength;	/* how much we will accept */
		}
		HDCmd->scsi_CmdActual = opcode;	/* actual Command returned */

		opsize = opcode; opsize &= 0xA0;

		if(opsize == (SCSI_READ6 & 0xA0)) {
		ULONG	*sec = (ULONG *)(scsi+(RW6_LBA1-1));
			sector = *sec & 0x1FFFFF;
			length = (ULONG)scsi[RW6_TLEN1];
			if(length == 0) length = 256;

		} else if(opsize == (SCSI_READ10 & 0xA0)) {
		ULONG	*sec = (ULONG *)(scsi+(RW10_LSB1));
			sector = *sec;
			length = (ULONG)((scsi[RW10_TLEN1] << 8) | scsi[RW10_TLEN2]);

		} else if(opsize == (SCSI_READ12 & 0xA0)) {
		ULONG	*sec = (ULONG *)(scsi+(RW12_LSB1));
		ULONG	*len = (ULONG *)(scsi+(RW12_TLEN1));

			sector = *sec;
			length = *len;

		} else if(opsize == (SCSI_READ16 & 0xA0)) {
		ULONG	*sec = (ULONG *)(scsi+(RW16_LSB1+4));
		ULONG	*len = (ULONG *)(scsi+(RW16_TLEN1));

			sector = *sec;
			length = *len;
		}

		sector = sector << pwrEmulate;
		length = length << (pwrBase + pwrEmulate);

		if(readCmd)
			rCmd = readCmd, wCmd = writeCmd;
		else
			rCmd = SCSI_READ12, wCmd = SCSI_WRITE12;

		if(baseop == SCSI_READ6)
			HDCmd->scsi_Actual = CacheRead(rCmd, sector, buffer, length);
		else
			HDCmd->scsi_Actual = CacheWrite(wCmd, rCmd, sector, buffer, length);

		HDCmd->scsi_Status = scsiCmd.scsi_Status;		/* SCSI status of command */
		HDCmd->scsi_SenseActual = scsiCmd.scsi_SenseActual;	/* how much has been sent */
		scsiCmd.scsi_SenseLength = 0;				/* how much we now will accept */

	} else {

		sector = 0;
		length = req->io_Length;

		oldIO(sCmd, sector, (UBYTE *)HDCmd, length);

		if(opcode == SCSI_INQUIRY && arg_array[ARG_DTYPE] && HDCmd->scsi_Actual >= 1) {
		UBYTE *inq = (UBYTE *)HDCmd->scsi_Data;

			 /* codes as defined in the SCSI-2 spec*/
			inq[0] = *(ULONG*)arg_array[ARG_DTYPE];

		} else if(opcode == SCSI_READCAPACITY && HDCmd->scsi_Actual >= 8) {
		struct rCapData {ULONG TotalSectors; ULONG SectorSize;} *rCap;

			rCap = (struct rCapData *)HDCmd->scsi_Data;

			/* total # of sectors on drive */
			rCap->TotalSectors = rCap->TotalSectors << (pwrDevice - pwrEmulate);
			rCap->SectorSize = secEmulate << pwrBase; 	/* in bytes */
		}
	}
	ReleaseSemaphoreReply(req);
}


 /*-------------*\
<*> My Begin IO <*>
 \*-------------*/

ULONG	ioCommands;
ULONG	ioComplete;

void __saveds mybeginio(register __a1 struct IOStdReq *req, register __a6 struct Device *dev)
{
UWORD	rawCmd, cmd, extFlag;
	rawCmd = req->io_Command;
	cmd = rawCmd & ~(NSCMD_TDF_EXTCOM | TDF_EXTCOM);
	extFlag = rawCmd - cmd;

	ioCommands++;

	if(req->io_Unit != unitID) {

		oldbeginio(req, dev);

	} else if(cmd == CMD_READ || cmd == NSCMD_TD_READ64 || cmd == TD_READ64) {
	ULONG	sector = req->io_Offset >> pwrBase;
	UBYTE	*buffer = (UBYTE *)req->io_Data;
	ULONG	length = req->io_Length;

		if(cmd != CMD_READ)	// It's a 64 bit command
		{
			sector = sector | (req->io_HighOffset << (32 - pwrBase));

			if(readCmd) rawCmd = readCmd;
		}

		ObtainSemaphore(&ss);

		if(extFlag == 0) {

			rawCmd &= ~(NSCMD_TDF_EXTCOM | TDF_EXTCOM);

		} else if(rawCmd & ~(NSCMD_TDF_EXTCOM | TDF_EXTCOM)) {

			((IOExtTD *)IO)->iotd_Count = ((IOExtTD *)req)->iotd_Count;

			if( ((IOExtTD *)req)->iotd_SecLabel != NULL ) rawCmd = 0;
		}

		if(rawCmd != 0) {

			IO->io_Actual = CacheRead(rawCmd, sector, buffer, length);
			ReleaseSemaphoreReply(req);

		} else {

			ReleaseSemaphore(&ss);
			oldbeginio(req, dev);
		}

	} else if(cmd == CMD_WRITE || cmd == NSCMD_TD_WRITE64 || cmd == TD_WRITE64) {
	ULONG	sector = req->io_Offset >> pwrBase;
	UBYTE	*buffer = (UBYTE *)req->io_Data;
	ULONG	length = req->io_Length;
	UWORD	rCmd = ETD_READ;

		if(cmd != CMD_WRITE)	// It's a 64 bit command
		{
			sector = sector | (req->io_HighOffset << (32 - pwrBase));

			if(readCmd) rCmd = readCmd, rawCmd = writeCmd;
		}

		ObtainSemaphore(&ss);

		if(extFlag == 0) {

			rCmd &= ~(NSCMD_TDF_EXTCOM | TDF_EXTCOM);
			rawCmd &= ~(NSCMD_TDF_EXTCOM | TDF_EXTCOM);

		} else if(rawCmd & ~(NSCMD_TDF_EXTCOM | TDF_EXTCOM)) {

			((IOExtTD *)IO)->iotd_Count = ((IOExtTD *)req)->iotd_Count;

			if( ((IOExtTD *)req)->iotd_SecLabel != NULL ) rawCmd = 0;
		}

		if(rawCmd != 0) {

			IO->io_Actual = CacheWrite(rawCmd, rCmd, sector, buffer, length);
			ReleaseSemaphoreReply(req);

		} else {

			UpdateCacheLines(sector, buffer, length);
			ReleaseSemaphore(&ss);
			oldbeginio(req, dev);
		}

	} else if(cmd == TD_FORMAT || cmd == NSCMD_TD_FORMAT64 || cmd == TD_FORMAT64) {
	ULONG	sector = req->io_Offset >> pwrBase;
	UBYTE	*buffer = (UBYTE *)req->io_Data;
	ULONG	length = req->io_Length;
	UWORD	rCmd = ETD_READ;
	UWORD	wCmd = ETD_WRITE;

		if(cmd != TD_FORMAT)	// It's a 64 bit command
		{
			sector = sector | (req->io_HighOffset << (32 - pwrBase));

			if(formatCmd) rawCmd = formatCmd;
			if(readCmd) rCmd = readCmd, wCmd = writeCmd;
		}

		if(pwrEmulate != pwrDevice) rawCmd = wCmd;

		ObtainSemaphore(&ss);

		if(extFlag == 0) {

			rCmd &= ~(NSCMD_TDF_EXTCOM | TDF_EXTCOM);
			wCmd &= ~(NSCMD_TDF_EXTCOM | TDF_EXTCOM);
			rawCmd &= ~(NSCMD_TDF_EXTCOM | TDF_EXTCOM);

		} else if(rawCmd & ~(NSCMD_TDF_EXTCOM | TDF_EXTCOM)) {

			((IOExtTD *)IO)->iotd_Count = ((IOExtTD *)req)->iotd_Count;
			((IOExtTD *)IO)->iotd_SecLabel = ((IOExtTD *)req)->iotd_SecLabel;

			if( ((IOExtTD *)req)->iotd_SecLabel != NULL ) wCmd = 0;
		}

		if(rawCmd == wCmd)
			IO->io_Actual = CacheWrite(rawCmd, rCmd, sector, buffer, length);
		else
			oldIO(rawCmd, sector, buffer, length);

		((IOExtTD *)IO)->iotd_SecLabel = NULL;

		UpdateCacheLines(sector, NULL, length);
		ReleaseSemaphoreReply(req);

	} else if(cmd == TD_SEEK || cmd == NSCMD_TD_SEEK64 || cmd == TD_SEEK64) {
	ULONG	sector = req->io_Offset >> pwrBase;
	UBYTE	*buffer = (UBYTE *)req->io_Data;
	ULONG	length = req->io_Length;

		if(cmd != TD_SEEK)	// It's a 64 bit command
		{
			sector = sector | (req->io_HighOffset << (32 - pwrBase));

			if(seekCmd) rawCmd = seekCmd;
		}

		ObtainSemaphore(&ss);

		if(extFlag == 0) {

			rawCmd &= ~(NSCMD_TDF_EXTCOM | TDF_EXTCOM);

		} else if(rawCmd & ~(NSCMD_TDF_EXTCOM | TDF_EXTCOM)) {

			((IOExtTD *)IO)->iotd_Count = ((IOExtTD *)req)->iotd_Count;
		}

		oldIO(rawCmd, sector, buffer, length);

		ReleaseSemaphoreReply(req);

	} else if(cmd == HD_SCSICMD) {

		DoSCSICmd(cmd, req);

	} else if(cmd == TD_GETGEOMETRY) {

		GetGeometryCmd(cmd, req);

	} else if(cmd == CMD_CLEAR) {

		ObtainSemaphore(&ss);

		FreeCacheBuffers();

		ReleaseSemaphore(&ss);
		oldbeginio(req, dev);

	} else { // Any other commands

		oldbeginio(req, dev);
	}

	ioComplete++;
}


 /*--------------*\
<*> Test Unit ID <*>
 \*--------------*/

int TestUnitID(char *device, int unit)
{
struct	IOStdReq *io;
int	ret = FALSE;

	if((io = CreateIORequest(CreateMsgPort(), sizeof(struct IOStdReq)))) {

		if(OpenDevice(device, unit, (struct IORequest *)io, 0) != NULL) {

			if(unitID == io->io_Unit) ret = TRUE;

			CloseDevice((struct IORequest *)io);
		}

		DeleteMsgPort(io->io_Message.mn_ReplyPort);
		DeleteIORequest(io);
	}
	return(ret);
}


 /*------------------------*\
<*> New Style Device Check <*>
 \*------------------------*/

#define	NSD	1<<0
#define	TD32	1<<1
#define	NSDSD	1<<2
#define	NSD64	1<<3
#define	ETD64	1<<4

#define	TD64	1<<5

#define	SCSIDirect	1<<6

int TestforNSD(struct IOStdReq *io)
{
struct	NSDeviceQueryResult nsdqr;
int	ret = 0;
UWORD	*cmdcheck;

	nsdqr.SizeAvailable = 0;
	nsdqr.DevQueryFormat = 0;

	io->io_Command = NSCMD_DEVICEQUERY;
	io->io_Length = sizeof(nsdqr);
	io->io_Data	= (APTR)&nsdqr;

	if ((SetSignal(0,0) & SIGBREAKF_CTRL_C)) return(0);

	if(!(DoIO((struct IORequest *)io)) &&
	   (io->io_Actual >= 16) &&
	   (io->io_Actual <= sizeof(nsdqr)) &&
	   (nsdqr.SizeAvailable == io->io_Actual))
	{
		/* Ok, this must be a new style device */
		ret = NSD;

		if(nsdqr.DeviceType == NSDEVTYPE_TRACKDISK)
		{
			ret = NSD | TD32;

			/* Device specific code follows */
			/* Is it safe to use 64 bits with this driver? We can reject
			 * bad mounts pretty easily via this check!
			 */
			for(cmdcheck = nsdqr.SupportedCommands; *cmdcheck; cmdcheck++)
			{
				if ((SetSignal(0,0) & SIGBREAKF_CTRL_C)) return(0);

				if(*cmdcheck == TD_READ64)
					ret |= TD64;

				if(*cmdcheck == HD_SCSICMD)
					ret |= NSDSD;

				if(*cmdcheck == NSCMD_TD_READ64)
					ret |= NSD64;

				if(*cmdcheck == NSCMD_ETD_READ64)
					/* According to specs, this implies NSD64 == TRUE! */
					ret |= ETD64;
			}
		}
	}
	return(ret);
}


 /*---------------------*\
<*> Track Disk 64 Check <*>
 \*---------------------*/

/*
it may optionally check whether the underlying driver actually supports
them by issuing a TD_READ64 request with io_Offset, io_HighOffset, and
io_Length all set to zero; if the underlying driver returns IOERR_NOCMD,
the filesystem-level application should report an error (e.g.  fail the
start-up packet in case of a filesystem).  For reasons of efficiency, this
check should be performed only _once_ upon start-up.
*/

int TestforTD64(struct IOStdReq *io)
{
int	ret = 0;

	io->io_Command = TD_READ64;

	io->io_Offset = 0;
	io->io_HighOffset = 0;
	io->io_Length  = 0;

	if ((SetSignal(0,0) & SIGBREAKF_CTRL_C)) return(0);

	if(DoIO((struct IORequest *)io) != IOERR_NOCMD) ret = TD64;

	return(ret);
}


 /*-------------------*\
<*> SCSI Direct Check <*>
 \*-------------------*/

int TestforSCSIDirect(struct IOStdReq *io)
{
int	ret = 0;

struct	SCSICmd Cmd;				/* where the actual SCSI command goes */

static	UBYTE TestReady[] = { 0,0,0,0,0,0 };

	io->io_Length = sizeof(struct SCSICmd);
	io->io_Data = (APTR)&Cmd;
	io->io_Command = HD_SCSICMD;		/* the command we are sending */

	Cmd.scsi_Data = NULL; 			/* where we put returned data */
	Cmd.scsi_Length = 0;			/* how much we will accept */
	Cmd.scsi_Command=(UBYTE *)TestReady;	/* issuing a TEST_READY command */
	Cmd.scsi_CmdLength = 6;			/* length of the command */
	Cmd.scsi_Actual = 1;			/* Dummy Value, TestReady should change to 0 */
	Cmd.scsi_Flags = SCSIF_NOSENSE|SCSIF_READ;

	if ((SetSignal(0,0) & SIGBREAKF_CTRL_C)) return(0);

	if((DoIO((struct IORequest *)io) != IOERR_NOCMD) && (Cmd.scsi_Actual == 0))
		ret = SCSIDirect;

	return(ret);
}


 /*--------------*\
<*> Disk Changed <*>
 \*--------------*/

void __saveds DiskChanged(void)
{
	diskChangeNum++;
}


 /*------*\
<*> MAIN <*>
 \*------*/

char *idCmdSCSI = "SCSI Direct (SD12)";
char *idCmdTD64 = "Track Disk 64 (TD64)";
char *idCmdNSD64 = "New Style (NSD64)";
char *idCmdETD64 = "New Style Enhanced (ETD64)";
struct	Interrupt diskChangeInt;

int Main(void)
{
struct	MsgPort *port;
ULONG	lineCacheSize;

char	*device;
ULONG	unit;
char	*idCmd = NULL;
ULONG	ret = RETURN_OK;
struct	IOStdReq *changeIO;

	device = (char *) arg_array [ ARG_DEVICE];
	unit = *(ULONG*) arg_array [ ARG_BU ];

	if(!(IO = CreateIORequest(port = CreateMsgPort(), sizeof(struct IOExtTD)))) {

		Printf("Couldn't Create IORequest Structure.\n");
		return(RETURN_FAIL);

	} else if(!(changeIO = CreateIORequest(CreateMsgPort(), sizeof(struct IOExtTD)))) {

		Printf("Couldn't Create Change Int IORequest Structure.\n");
		ret = RETURN_FAIL;

	} else if(OpenDevice(device, unit, (struct IORequest *)IO, 0)) {

		Printf("Couldn't open unit %ld on %s\n", unit, device);
		ret = RETURN_FAIL;

	} else if(OpenDevice(device, unit, (struct IORequest *)changeIO, 0)) {

		CloseDevice((struct IORequest *)IO);
		Printf("Couldn't open unit %ld on %s\n", unit, device);
		ret = RETURN_FAIL;

	} else {
		unitID = IO->io_Unit;

		diskChangeInt.is_Code = (void (*) ())DiskChanged;
		diskChangeInt.is_Node.ln_Type = NT_INTERRUPT;
		changeIO->io_Command = TD_ADDCHANGEINT;
		changeIO->io_Data  = &diskChangeInt;
		changeIO->io_Length  = sizeof(struct Interrupt);

		SendIO((struct IORequest *)changeIO);

		IO->io_Command = TD_GETGEOMETRY;
		IO->io_Data  = &geo;
		IO->io_Length  = sizeof(struct DriveGeometry);

		if(DoIO((struct IORequest *)IO) != 0)
			Printf("* Warning * Get Geometry has Failed !\n\n");

		/* The number of groups of Caches */

		secSets = (arg_array[ARG_S]) ? *(ULONG*) arg_array[ARG_S] : 8;

		/* The number the Cache Lines */

		pwrLines = (arg_array[ARG_L]) ? *(ULONG*) arg_array[ARG_L] : 5;

		/* The Sector size of the Cache */

		if(arg_array[ARG_C]) {
		ULONG sec =  *(ULONG*) arg_array[ARG_C], pwr = 0;
			sec = sec >> (pwrBase + 1);
			while(sec) sec = sec >> 1, pwr++;
			pwrCache = pwr;
		} else
			pwrCache = 3;

		/* The Sector size of the Device */

		{ULONG sec, pwr = 0;
			sec = (arg_array[ARG_D]) ? *(ULONG*) arg_array[ARG_D] : geo.dg_SectorSize;
			sec = sec >> (pwrBase + 1);
			while(sec) sec = sec >> 1, pwr++;
			pwrDevice = pwr;
		}

		/* The Sector size for the Filesystem */

		if(arg_array[ARG_E]) {
		ULONG sec =  *(ULONG*) arg_array[ARG_E], pwr = 0;
			sec = sec >> (pwrBase + 1);
			while(sec) sec = sec >> 1, pwr++;
			pwrEmulate = pwr;
		} else
			pwrEmulate = pwrDevice;


		if(pwrEmulate > pwrDevice) pwrDevice = pwrEmulate;
		if(pwrDevice > pwrCache) pwrCache = pwrDevice;
		if(secSets <= 0) secSets = 1;

		secEmulate = 1<<pwrEmulate;
		secDevice = 1<<pwrDevice;
		secCache = 1<<pwrCache;
		secLines = 1<<pwrLines;

		deviceBytes = secDevice << pwrBase;
		cacheBytes = secCache << pwrBase;

		lineCacheSize = sizeof(struct lineCache) * (secSets << pwrLines);

		if(TestUnitID(device, unit)) {
			Printf("Failed to verify Unit ID, Aborted !\n");
			ret = RETURN_FAIL;

		} else if(!(cache = AllocMem(lineCacheSize, MEMF_PUBLIC|MEMF_CLEAR))) {
			Printf("Couldn't Create Cache Line Structure.\n");
			ret = RETURN_FAIL;

		} else {
		int	nsd = TestforNSD(IO);
		char	*div = "";

			Printf("* Beta Release, Use at your Own Risk ! *\n\n");

			Printf("SectorCache");
			if(geo.dg_Flags & DGF_REMOVABLE)
				Printf(" Removable Media");
			Printf(", unit %ld on %s\n\n", unit, device);

			Printf("It's a ");

			if(nsd != 0)
			{
				Printf("New Style ");
				div = "& ";
			}

			if((nsd & TD32) != 0)
			{
				Printf("(TD");

				if(nsd & NSDSD)
					Printf(", SD");

				if(nsd & TD64)
					Printf(", TD64?");

				if(nsd & NSD64) {
					Printf(", NSD64");

					idCmd = idCmdNSD64,
					readCmd = NSCMD_TD_READ64,
					writeCmd = NSCMD_TD_WRITE64,
					seekCmd = NSCMD_TD_SEEK64,
					formatCmd = NSCMD_TD_FORMAT64;
				}

				if(nsd & ETD64) {
					Printf(", ETD64");

					idCmd = idCmdETD64,
					readCmd = NSCMD_ETD_READ64,
					writeCmd = NSCMD_ETD_WRITE64,
					seekCmd = NSCMD_ETD_SEEK64,
					formatCmd = NSCMD_ETD_FORMAT64;
				}

				Printf(") ");
				nsd = 0;
			}

			if(nsd == 0)
			{
				if(TestforTD64(IO)) {

					Printf("%sTrack Disk 64 ? ",div);
					div = "& ";

					if(idCmd == NULL)
						idCmd = idCmdTD64,
						readCmd = TD_READ64,
						writeCmd = TD_WRITE64,
						seekCmd = TD_SEEK64,
						formatCmd = TD_FORMAT64;
				}

				if(TestforSCSIDirect(IO)) {

					Printf("%sSCSI Direct ",div);

					if(idCmd == NULL)
						idCmd = idCmdSCSI,
						readCmd = SCSI_READ12,
						writeCmd = SCSI_WRITE12;
				}
			}
			Printf("device.\n");

			if(idCmd != NULL)
				Printf("The recommended 64 bit command type is %s.\n", idCmd);

			if(arg_array[ARG_ETD64])
				idCmd = idCmdETD64,
				readCmd = NSCMD_ETD_READ64,
				writeCmd = NSCMD_ETD_WRITE64,
				seekCmd = NSCMD_ETD_SEEK64,
				formatCmd = NSCMD_ETD_FORMAT64;

			else if(arg_array[ARG_NSD64])
				idCmd = idCmdNSD64,
				readCmd = NSCMD_TD_READ64,
				writeCmd = NSCMD_TD_WRITE64,
				seekCmd = NSCMD_TD_SEEK64,
				formatCmd = NSCMD_TD_FORMAT64;

			else if(arg_array[ARG_TD64])
				idCmd = idCmdTD64,
				readCmd = TD_READ64,
				writeCmd = TD_WRITE64,
				seekCmd = TD_SEEK64,
				formatCmd = TD_FORMAT64;

			else if(arg_array[ARG_SD])
				idCmd = idCmdSCSI,
				readCmd = SCSI_READ12,
				writeCmd = SCSI_WRITE12;

			else if(arg_array[ARG_NC])
				idCmd = NULL,
				readCmd = 0,
				writeCmd = 0,
				seekCmd = 0,
				formatCmd = 0;

			if(idCmd != NULL)
				Printf("Converting SCSI & 64 bit commands to %s.\n", idCmd);

			if(arg_array[ARG_DTYPE])
				Printf("Changing from a Type %ld to a Type %ld device.\n",
				geo.dg_DeviceType, *(ULONG*) arg_array[ARG_DTYPE]);

			Printf("\nEmulated Sector\t= %ld Bytes\n", secEmulate << pwrBase);
			Printf("Device Sector\t= %ld Bytes\n", deviceBytes);
			Printf("Cache Block\t= %ld Bytes\n", cacheBytes);
			Printf("Line Length\t= %ld Blocks\n", secLines);
			Printf("Set Size\t= %ld Lines\n", secSets);
			Printf("Maximum Cache\t= %ld Bytes\n", secSets << (pwrBase + pwrLines + pwrCache));
			Printf("Active on Device (0x%lx)\n", IO->io_Device);

			InitSemaphore(&ss);

			oldbeginio = SetFunction((struct Library *)IO->io_Device,DEV_BEGINIO,(APTR)mybeginio);

			Wait(SIGBREAKF_CTRL_C);

			Printf("Read\t\t: Cached = %ld, Direct = %ld\n", cachedRead, directRead);
			Printf("Write\t\t: Cached = %ld, Direct = %ld\n", cachedWrite, directWrite);
			Printf("Cache\t\t: Hit = %ld, Miss = %ld, Made = %ld, Flush = %ld\n", cacheHit, cacheMiss - cacheMade, cacheMade, cacheFlush);
			Printf("IO Commands\t= %ld\n", ioCommands);

			SetFunction((struct Library *)IO->io_Device,DEV_BEGINIO,(APTR)oldbeginio);

			Printf("Removed from, unit %ld on %s\n\n", unit, device);

			while(ioCommands != ioComplete) Delay(50);

			FreeCacheBuffers();
			FreeMem(cache, lineCacheSize);
		}
		changeIO->io_Command = TD_REMCHANGEINT;
		changeIO->io_Flags  = IOF_QUICK;

		if(DoIO((struct IORequest *)changeIO) != 0)
			Printf("Remove Change Int has Failed !\n");

		CloseDevice((struct IORequest *)changeIO);

		IO->io_Message.mn_ReplyPort = port;
		CloseDevice((struct IORequest *)IO);
	}

	if(changeIO) {

		DeleteMsgPort(changeIO->io_Message.mn_ReplyPort);
		DeleteIORequest(changeIO);
	}
	DeleteMsgPort(port);
	DeleteIORequest(IO);

	return(ret);
}