/*
 * SmartDisk 1.4.2
 *
 * Created by David Le Blanc 29/10/91 (david@mlb.geomechanics.csiro.au)
 * Absolutely no copywrite. But if you improve it,
 * please send me a new version (with source!)
 * Enhanced by A. Arens (ari@tron.gun.de)
 *
 */

#include <exec/types.h>
#include <exec/memory.h>
#include <exec/io.h>
#include <exec/devices.h>
#include <exec/execbase.h>
#include <libraries/dos.h>
#include <libraries/dosextens.h>
#include <libraries/filehandler.h>

#include "string.h"
#include "stdlib.h"
#include "stdio.h"


void * AllocMem(), *FreeMem();
void DeletePort();
struct MsgPort* CreatePort();
struct IOStdReq * CreateExtIO();
void (*SetFunction())();


extern struct ExecBase *SysBase;

#if defined (__GNUC__) || defined(AZTEC_C)
#define OLDIO myoldio
void (* oldbeginio)();
void myoldio(struct IOStdReq *, struct Device *);
#else
#define OLDIO oldbeginio
#endif

struct MsgPort *dl_Task;	/* This is the FileSystem task we want	*/
struct IOStdReq *IO;		/* All IO goes through here now.	*/

struct SignalSemaphore *ss;	/* To force single threading	 */
				/* through the code responsible  */
				/* for updating the cache	 */

#define LOCK	 ObtainSemaphore(ss)
#define UNLOCK	 ReleaseSemaphore(ss)

ULONG counter;
int allocnum = 0 ;

/*
 * Constants. Adjust for adjusting cache size and shape.
 */

#define ITEM_SIZE 512		/* No change - sector size */

#define LINE_SIZE   4		/* Increase for prefetch */
#define SETS	    8		/* N-way set associative */
#define LINES	   32		/* N lines of cache	 */

/*
 * Bit fields. Adjust to match above constants.
 */

#define ITEM_BITS  2		/* 2^ITEM_BITS = LINE_SIZE */
#define LINE_BITS  5		/* 2^LINE_BITS = LINES	   */

#if !defined(__GNUC__) && !defined(AZTEC_C)
struct bitaddress {
   int key:32-LINE_BITS-ITEM_BITS,
       line:LINE_BITS,
       offset:ITEM_BITS ;
   } ;
#else
#ifdef __GNUC__
struct bitaddress {
   unsigned int key:	32-LINE_BITS-ITEM_BITS,
		line:	LINE_BITS,
		offset: ITEM_BITS;
   } ;
#else
/* Yes! Its true! MANX bitfields are broken... */
struct bitaddress {
   unsigned int offset: ITEM_BITS,
		line:	LINE_BITS,
		key:	32-LINE_BITS-ITEM_BITS;
   } ;
#endif
#endif

union sector {
   int sector ;
   struct bitaddress s ;
   } ;

struct cache_line {
   int key ;			 /* Item key */
   int age ;			 /* AGE for LRU alg */
   int valid ;
   char *buffer ;		 /* [LINE_SIZE][ITEM_SIZE] of DATA */
   } ;

struct cache_line cache[SETS][LINES] ;

/*
 * A buffer for the exec device to go through.
 */
char * globbuffer = NULL;

#define DEV_BEGINIO (-30)
#define DEV_ABORTIO (-36)


#if !defined(__GNUC__) && !defined(AZTEC_C)
void (*__asm oldbeginio)(register __a1 struct IOStdReq *,
			 register __a6 struct Device *);

#define CREATEPORT CreatePort(0,0)
#define DELETEPORT DeletePort
#else
#define CREATEPORT CreateP()
#define DELETEPORT DelP


static
#ifndef AZTEC_C
inline
#endif
struct MsgPort *
CreateP()
{
  int sigBit;
  struct MsgPort *port;
  void *FindTask();

  if ((sigBit = AllocSignal(-1)) == -1) return 0;

  port = (struct MsgPort *) AllocMem(sizeof (struct MsgPort),MEMF_PUBLIC|MEMF_CLEAR);
  if (! port)
  {
      FreeSignal (sigBit);
      return 0;
  }

  port->mp_Node.ln_Name = NULL;
  port->mp_Node.ln_Pri = 0;
  port->mp_Node.ln_Type = NT_MSGPORT;

  port->mp_Flags = PA_SIGNAL;
  port->mp_SigBit = sigBit;
  port->mp_SigTask = SysBase->ThisTask;

  NewList (&(port->mp_MsgList));

  return port;
}


static
#ifndef AZTEC_C
inline
#endif
void
DelP (struct MsgPort *port)
{
  /* make it difficult to reuse the port */
  port->mp_SigTask = (struct Task *)-1;
  port->mp_MsgList.lh_Head = (struct Node *)-1;

  FreeSignal(port->mp_SigBit);

  FreeMem ( port, sizeof(struct MsgPort));
}

_wb_parse()
{ return 1; }

Chk_Abort()
{ return 0; }


void mybeginio (struct IOStdReq *, struct Device *);

#ifdef AZTEC_C
#asm
.text
	xdef  _mybeginio
_mybeginio:
	movem.l d0-d7/a0-a6,-(sp)
	move.l	a6,-(sp)
	move.l	a1,-(sp)
	jsr	__mybeginio
	add.w	#8,sp
	movem.l (sp)+,d0-d7/a0-a6
	rts

	xdef _myoldio
_myoldio:
	movem.l d0-d7/a0-a6,-(sp)
	move.l	64(sp),a1
	move.l	68(sp),a6
	move.l	_oldbeginio,a0
	jsr	(a0)
	movem.l (sp)+,d0-d7/a0-a6
	rts
#endasm

#else	/* __GNUC__  */
asm("
.text
	.globl _mybeginio
_mybeginio:
	moveml #0xfffe,sp@-
	movel  a6,sp@-
	movel  a1,sp@-
	jbsr   __mybeginio
	addqw  #8,sp
	moveml sp@+,#0x7fff
	rts

	.globl _myoldio
_myoldio:
	moveml #0xfffe,sp@-
	movel  sp@(64),a1
	movel  sp@(68),a6
	movel  _oldbeginio,a0
	jbsr   a0@
	moveml sp@+,#0x7fff
	rts

");
#endif
#endif


/*
 * Scan for sector, and return the set it resides in.
 */
int
FindEntry(union sector *s) {

   register int set ;

   for (set = 0; set < SETS; set++)
      if (cache[set][s->s.line].valid) {
	 if (cache[set][s->s.line].key == s->s.key) {
	    cache[set][s->s.line].age = counter ++ ;
	    return set ;
	    }
	 }
      else
	 break ;

   return -1 ;
}


/*
 * Pick a set from the associated cache, and return
 * the set number. The cache memory is also allocated
 * before returning. The cache entry is marked VALID. so if you
 * can't fill it, remember to clear it!
 */
int
AllocCache(union sector *s) {
   int set ;
   int oldest ;
   int oldset ;
   int found ;
   int age ;

   oldset = 0 ;
   oldest = 0 ;
   found  = 0 ;

   for (set = 0; set < SETS; set++)
      if (cache[set][s->s.line].valid) {
	 if (cache[set][s->s.line].key != s->s.key) {
	    /*
	     * This 'age' calculation is complicated since normally,
	     * AGE = COUNTER - CACHE.AGE, however, if counter has
	     * wrapped to zero, such evaluation evaluates ages of < 0 and
	     * these entries will never be reselected for reuse.
	     *
	     * If counter wraps to zero, then CACHE.AGE is generally larger
	     * than COUNTER, so we evaluate age as the total of MAXINT-AGE
	     * plus the current value of the counter. IE, how much it took to
	     * wrap and reach the current position.
	     *
	     * Hence,
	     * AGE = ~0 - CACHE.AGE + COUNTER
	     */
	    age = cache[set][s->s.line].age ;
	    if (age > counter)
	       age = ((ULONG) ~0 - age) + counter ;
	    else
	       age = counter - age ;

	    if (age > oldest)
	       oldest = age, oldset = set ;
	    }
	 else {
	    found = 1 ;
	    break ; /* key = s.key. Line already in cache */
	    }
	 }
      else
	 break ; /* !valid. Found a free line */

   if (found) {
      return -1 ;
      }

   if (set == SETS)
      set = oldset ;

   cache[set][s->s.line].age = counter ++ ;
   cache[set][s->s.line].key = s->s.key ;

   /*
    * If no buffer, allocate one.
    */
   if (! cache[set][s->s.line].buffer )
      if (cache[set][s->s.line].buffer = AllocMem(LINE_SIZE << 9, MEMF_PUBLIC))
	 allocnum ++ ;

   /*
    * If STILL no buffer, return failure. Otherwise set the VALID flag.
    */
   if (cache[set][s->s.line].buffer )
      cache[set][s->s.line].valid = 1 ;
   else {
      cache[set][s->s.line].valid = 0 ; 	/* Allocation failed */
      return -1 ;
      }

   return set ;
}




/*
 * Allocate a line of cache and read it from disk.
 */
int
ReadCache(union sector *s) {
   struct MsgPort *port ;
   char *dest ;
   int	set ;

   LOCK;

   port = CREATEPORT ;

   if (!port) {
      UNLOCK ;
      return -1 ;
      }

   if (s->s.offset)
      s->s.offset = 0 ;

   IO->io_Message.mn_ReplyPort = port ;

   IO->io_Command = CMD_READ;
   IO->io_Offset = s->sector << 9 ;
   IO->io_Length = LINE_SIZE << 9 ;
   IO->io_Data = (APTR) globbuffer ;

   OLDIO(IO,IO->io_Device) ;

   WaitIO(IO) ;

   DELETEPORT(port) ;

   if (IO->io_Error) {
      UNLOCK ;
      return -1 ;
      }

   set = AllocCache(s) ;

   if (set < 0) {
      UNLOCK ;
      return -1 ;
      }

   dest = cache[set][s->s.line].buffer ;

   CopyMemQuick(globbuffer, dest, LINE_SIZE << 9) ;

   UNLOCK ;
   return 0 ;
}



int
ReadBufferToCache(int linestart,int unread,char *buffer) {

   union sector s ;
   struct MsgPort *port ;
   int set ;

   LOCK;
   s.sector = linestart ;

   port = CREATEPORT ;

   if (!port) {
      UNLOCK ;
      return -1 ;
      }

   IO->io_Message.mn_ReplyPort = port ;

   /*
    * Read enough to fill the buffer.
    */

   IO->io_Command = CMD_READ;
   IO->io_Offset = linestart << 9 ;
   IO->io_Length = unread << 9 ;
   IO->io_Data = (APTR) buffer ;

   OLDIO(IO,IO->io_Device) ;

   WaitIO(IO) ;

   DELETEPORT(port) ;

   if (IO->io_Error) {
      UNLOCK ;
      return -1 ;
      }

   while (unread) {
      set = AllocCache(&s) ;
      if (set < 0) {
	 UNLOCK ;
	 return -1 ;
	 }
      else {
	 CopyMemQuick(buffer,cache[set][s.s.line].buffer, LINE_SIZE << 9) ;
	 }

      s.sector += LINE_SIZE ;
      unread -= LINE_SIZE ;
      buffer += (LINE_SIZE << 9) ;
      }

   UNLOCK ;
   return 0 ;
}



/*
 * This functions checks the cache. If the sector is there, it returns
 * the buffer, otherwise it returns NULL.
 */
char *
FindCache(union sector *s,int set) {
   return & (cache[set][s->s.line].buffer[s->s.offset << 9 ] ) ;
}



/*
 * This function takes 'sector' and set, and decides if the next sector
 * is in the cache.
 */
int
NextEntry(union sector *s, int set) {
   int line ;

   line = s->s.line ;
   s->sector ++ ;

   if (line == s->s.line) {
      return set ;
      }
   else
      return FindEntry(s) ;
}



/*
 * Search for sector, and mark it invalid.
 */
void
ClearEntry(union sector *s,int set) {
   cache[set][s->s.line].valid = 0 ;
}



CacheUpdate(union sector *s, int seccount, char *buffer) {
   int set ;

   LOCK;

   while (seccount) {
      set = FindEntry(s) ;
      if (set >= 0) {
	 CopyMemQuick(buffer,FindCache(s,set), ITEM_SIZE) ;
	 cache[set][s->s.line].age = counter ++ ;
      }

      buffer += ( ITEM_SIZE ) ;
      seccount -- ;
      s->sector ++ ;
   }

   UNLOCK ;
   return 0 ;
}



#if !defined (__GNUC__) && !defined(AZTEC_C)
void __saveds __asm mybeginio(register __a1 struct IOStdReq *req,
			      register __a6 struct Device *dev) {
#else

void _mybeginio( register struct IOStdReq * req, register struct Device * dev )
{
#endif

   union sector s ;
   int	 set ;
   int	 command ;
   int	 secnum ;
   char  *source ;
   char  *buffer ;

   int	 unread ;
   int	 linestart ;

   /* Is this the correct caller (FileSystem) for us? */
   if (req->io_Message.mn_ReplyPort->mp_SigTask != dl_Task) {
      OLDIO(req,dev) ;
      return;
   }

   s.sector = req->io_Offset >> 9 ;
   secnum   = req->io_Length >> 9 ;
   command  = req->io_Command ;
   buffer   = (char *) req->io_Data ;

   if (command == CMD_WRITE)
      CacheUpdate(&s,secnum,buffer) ;

   if (command == CMD_READ) {

      while (secnum) {
	 set = FindEntry(&s) ;

	 if (set < 0) {
	    source = NULL ;
	    }
	 else {
	    source = FindCache(&s,set) ;
	    }

	 /*
	  * Scan copying buffers to the request.
	  */

	 while (secnum && source) {
	    CopyMemQuick(source,buffer,ITEM_SIZE) ;
	    buffer += ITEM_SIZE ;

	    secnum -- ;
	    if (secnum) {
	       set = NextEntry(&s, set) ;
	       if (set < 0) {
		  source = NULL ;
		  }
	       else {
		  source = FindCache(&s,set) ;
		  }
	       }
	    }

	 if (!secnum) { 				/* Done ? */
	    break ;
	    }

	 /*
	  * If we are in the middle of a line, read it in.
	  */
	 if (s.s.offset) {
	    int original = s.sector ;

	    s.s.offset = 0 ;
	    if (ReadCache(&s) < 0) {
	       }

	    s.sector = original ;
	    }
	 else {
	    /*
	     * Start scanning at next line.
	     */
	    unread = 0 ;

	    linestart = s.sector ;

	    /*
	     * Scan counting sectors that need reading.
	     */
	    while ((secnum>unread) && (set < 0)) {
	       s.sector = linestart + unread ;
	       set = FindEntry(&s) ;
	       /*
		* For efficiency, if a sector is not found, advance to
		* the next line instead of the next sector.
		*/
	       if (set < 0) {
		  unread += LINE_SIZE ;
		  }
	       }

	    if (unread > secnum) {
	       unread -= LINE_SIZE ;
	       }

	    if (unread) {
	       /*
		* Read the cache into the supplied buffer, and copy
		* it to cache memory.
		*/
	       ReadBufferToCache(linestart,unread,buffer) ;
	       buffer += ( unread << 9 ) ;
	       }
	     else {
	       /*
		* If there are more sectors, call 'ReadCache()' to get them.
		*/
	       ReadCache( (union sector *)&linestart) ;
	       }

	    /*
	     * Pick up where we left off.
	     */
	    s.sector = linestart + unread ;
	    secnum -= unread ;
	    if (secnum < 0)
	       break ;
	    }
	 }
      }

   if (command != CMD_READ)
      OLDIO(req,dev) ;
   else {
      req->io_Actual = req->io_Length ;
      if (!req->io_Flags & IOF_QUICK)
	 ReplyMsg((struct Message *) req) ;
   }
}



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

   int error ;
   int line,set ;
   int devopen ;
   struct MsgPort  *port ;
   char *device ;
   int unit ;
   int openflags = 0;
   struct DeviceList *devlist, *dlfirst;
   extern struct DosLibrary *DOSBase;
   struct MsgPort *DeviceProc();
   struct FileSysStartupMsg *fssm;

   port = NULL;
   devopen = 0 ;

   if (argc != 2) {
      printf ("Usage: SmartDisk device:\n") ;
      goto fail ;
   }
   dl_Task = DeviceProc(argv[1]);
   if (!dl_Task) {
noproc:
      printf ("SmartDisk: Cannot find device handler process: %s\n", argv[1]) ;
      goto fail ;
   }

   dlfirst = devlist = (struct DeviceList *)(((ULONG)((struct DosInfo *)(((ULONG)((struct RootNode *)DOSBase->dl_Root)->rn_Info)<<2))->di_DevInfo)<<2);
   while (devlist)
   {
      /* First, find each device that's active (dl_Task is not NULL) */
      if ((devlist->dl_Type == DLT_DEVICE) && (devlist->dl_Task == dl_Task))
	goto foundproc;
      devlist = (struct DeviceList *)(((ULONG)devlist->dl_Next) << 2);
   }
   goto noproc;

foundproc:
   fssm = (struct FileSysStartupMsg *)((ULONG)(((struct DeviceNode *)devlist)->dn_Startup)<<2);
   device = ((char *)((ULONG)(fssm->fssm_Device)<<2))+1;
   unit = fssm->fssm_Unit;
   openflags = fssm->fssm_Flags;

   globbuffer = AllocMem(LINE_SIZE << 9,
			((ULONG *)((ULONG)(fssm->fssm_Environ)<<2))[DE_MEMBUFTYPE]
			| MEMF_CLEAR);
   if (!globbuffer)
    goto fail;

   /* DeviceProc() returns a pointer to the MsgPort following the Task struct
      within the Process struct. This offset is used everywhere in DOS, but
      we now need the Task potion since we now determin callers by their
      Task in the redirected BeginIO routine. */
   dl_Task = (struct MsgPort *)(((char *)dl_Task)-sizeof(struct Task));

   port = CREATEPORT ;
   IO = CreateExtIO(port,sizeof(struct IOStdReq)) ;

   error = OpenDevice(device,unit,(struct IORequest *)IO, openflags) ;

   if (error) {
      printf ("Unable to open %s unit %d\n",device,unit) ;
      goto fail ;
      }
   else
      devopen = 1 ;

   ss = AllocMem(sizeof(struct SignalSemaphore), MEMF_PUBLIC|MEMF_CLEAR) ;

   if (!ss)
      goto fail ;

   SumLibrary( (struct Library *) IO->io_Device) ;

   InitSemaphore(ss) ;

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

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

   for (line = 0; line < LINES; line ++)
      for (set = 0; set < SETS; set ++)
	 if (cache[set][line].buffer) {
	    FreeMem( cache[set][line].buffer,LINE_SIZE << 9 ) ;
	    allocnum -- ;
	    }

   if (allocnum)
      printf("Allocation mismatch. %d buffers left lying around\n",allocnum) ;

fail:
   if (globbuffer)
      FreeMem(globbuffer,LINE_SIZE << 9 ) ;
   if (ss)
      FreeMem(ss,sizeof(struct SignalSemaphore)) ;
   if (devopen) {
      IO->io_Message.mn_ReplyPort = port ;
      CloseDevice((struct IORequest *)IO) ;
      }
   if (IO)
      DeleteExtIO((struct IORequest *)IO) ;
   if (port)
      DELETEPORT(port) ;

   return 0 ;
}