/* fl.c    Author: Raymond Michiels

This file contains a clever FDC for upto four floppy disk drives.

There are 5 entries:
 disk_init  = call to initialize buffers, etc.
 disk_sync  = call to clear all buffers
 disk_read  = call to read a sector
 disk_write = call to write a sector
 disk_quit  = call to finish all disk activity, and restore
              everything for the TRACKDISK-device. (Implies disk_sync)

NOTES: The Amiga minix floppy-task driver is based on the program.

       Compile with 16-bit integers.
*/

#include "type.h"
#include "cia.h"
#include "paula.h"

#define NR_SECTORS       9                       /* 720 Kb IBM disk */
#define NR_TRACKS       80                /* should be NR_CYLINDERS */
#define NR_SIDES         2
#define NR_DRIVES        4         /* maximum # of supported drives */
#define SECTOR_SIZE    512

#define H_ID             0           /* Offsets in the HEADER_FIELD */
#define H_TRACK          1
#define H_SIDE           2
#define H_SECTOR         3
#define H_LENGTH         4
#define H_CRC            5
#define H_SIZE           7

#define D_ID             0             /* Offsets in the DATA_FIELD */
#define D_DATA           1
#define D_CRC          513
#define D_SIZE         515

#define H_ID_MFM    0x5554                         /* uncoded: 0xFE */
#define H_ID_BIN      0xFE

#define D_ID_MFM    0x5545                         /* uncoded: 0xFB */
#define D_ID_BIN      0xFB

#define OK               0                   /* Error return values */
#define E_NO_DRIVE      -1
#define E_BAD_ARGS      -2
#define E_SYNC          -3
#define E_DISK_DMA      -4
#define E_NO_DISK       -5
#define E_WRONG_TRACK   -6
#define E_WR_PROT       -7
#define E_CRC           -8
#define E_BAD_DISK      -9

/* track= (GAP1 GAP2 SYNC HEADER GAP3 GAP4 SYNC DATA)+ */
/* sizes:   70   12    3     7    22   12    3   515  -> 644 */

#define GAP1_SIZE       70
#define GAP2_SIZE       12
#define GAP3_SIZE       22
#define GAP4_SIZE       12
#define SYNC_SIZE        3

#define H_OFFSET   (GAP1_SIZE + GAP2_SIZE + SYNC_SIZE)
#define D_OFFSET   (H_OFFSET + H_SIZE + GAP3_SIZE + GAP4_SIZE + SYNC_SIZE)

#define RAW_S_SIZE (GAP1_SIZE + GAP2_SIZE + SYNC_SIZE + H_SIZE + \
                     GAP3_SIZE + GAP4_SIZE + SYNC_SIZE + D_SIZE)

#define RAW_D_SIZE (SYNC_SIZE + H_SIZE + GAP3_SIZE + GAP4_SIZE + \
                     SYNC_SIZE + D_SIZE)

#define RAW_T_SIZE  0x1D00                 /* room for a full track */
#define BUFSIZE    (RAW_S_SIZE)*NR_SECTORS+2
#define GAP1_DATA   0x9254                         /* uncoded: 0x4E */
#define GAP2_DATA   0xAAAA                         /* uncoded: 0x00 */
#define GAP3_DATA   0x9254
#define GAP4_DATA   0xAAAA
#define SYNC_DATA   0x4489                         /* uncoded: 0xA1 */

#define MAX_RETRIES     10       /* max # of retries on disk access */
#define UNCALIBRATED    -1

#define ctrk  track[drive]                         /* current track */
#define DSK_SEL  DSK_SEL0<<drive            /* more general DSK_SEL */

PRIVATE WORD DiskBuf[BUFSIZE];  /* CHIPMEM-buffer for raw disk-data */
PRIVATE WORD DiskBuf2[RAW_T_SIZE];       /* CHIPMEM-buffer for read */
PRIVATE WORD oldADKCON;          /* temp usage to return to TRK_DSK */

PRIVATE int drive;                                 /* current drive */
PRIVATE int track[NR_DRIVES];       /* head position for each drive */
PRIVATE int side;                                   /* current side */
PRIVATE int wr_prot;            /* is current disk write-protected? */
PRIVATE int buf_valid=FALSE; /* does the buffer contain valid data? */
PRIVATE int buf_dirty=FALSE;      /* should the buffer be rewritten */
                                    /* in case of a disk_read call? */

PRIVATE int r;                    /* variable to temp. store errors */
PRIVATE int stepdelay=2000;

LONG reg_d2,reg_d3,reg_d4,reg_d5;        /* should be in "m68000.h" */
void *reg_a2,*reg_a3,*reg_a4,*reg_a5;


PRIVATE portBout(setclr,mask)     /* This make a cia port look like */
WORD setclr;                             /* a custom funny register */
BYTE mask;
{  if (setclr) *PortB |= mask;
   else *PortB &= ~mask;
}

PRIVATE start_motor()
{  portBout(SET,DSK_SEL);
   portBout(CLR,DSK_MOTOR);
   portBout(CLR,DSK_SEL);
}

PRIVATE stop_motor()
{  portBout(SET,DSK_MOTOR | DSK_SEL);
   portBout(CLR,DSK_SEL);
   portBout(SET,DSK_SEL);
}

PRIVATE lock()
{  *INTENA=CLR | INTEN;
}

PRIVATE unlock()
{  *INTENA=SET | INTEN;
}

PRIVATE movehead(dir)  /* dir=-1:one track down, dir=1:one track up */
int dir;
{  int i,count;

   portBout(CLR,DSK_SEL);
   portBout((dir==1)?CLR:SET,DSK_DIREC);
   portBout(CLR,DSK_STEP);
   portBout(SET,DSK_STEP);              /* give an active low pulse */
   count=stepdelay>>2;
   for (i=0;i<count;i++)
      ;
}

PRIVATE track0()                      /* return: are we on track 0? */
{  return ((*PortA & DSK_TRACK0)==0);
}

PRIVATE writeprotect()          /* return: is disk write protected? */
{  return ((*PortA & DSK_WPROT)==0);
}

PRIVATE read_track()

{  int offset,size,i=1,st;
   *DSKSYNC=SYNC_DATA;
   *ADKCON=(CLR | PRECOMPMASK | MSBSYNC);
   *ADKCON=(SET | (ctrk>39?PRECOMP140:PRECOMP0) |
                 MFMPREC | WORDSYNC | FAST);

   *DSKLEN=0;
   *DMACON=(SET | DSKEN);

   start_motor();
   while ((*PortA & DSK_RDY)&&(i!=0))
      i++;                             /* wait for motor to turn on */
   if (i==0) return(E_NO_DRIVE);

   *DSKPT=&DiskBuf2[0];
   *DSKLEN=*DSKLEN=(DMAEN | READ | RAW_T_SIZE);
   *INTREQ=DSKBLK;                        /* mark last intr as seen */
   i=1;
   while (!(*INTREQR & DSKBLK) && (i!=0))
      i++;                                          /* busy waiting */
   *INTREQ=DSKBLK;                             /* mark intr as seen */
   if (i==0) return(E_DISK_DMA);
   return (adjust_buffer());
}


PRIVATE write_track()
{  int i=1;
   *ADKCON=(CLR | PRECOMPMASK | MSBSYNC | WORDSYNC);
   *ADKCON=(SET | (ctrk>39?PRECOMP140:PRECOMP0) | MFMPREC | FAST);
   *DSKLEN=0;
   *DMACON=(SET | DSKEN);
   *DSKPT=&DiskBuf[0];

   start_motor();
   if (!(*PortA & DSK_RDY))
   {  if (*CIABICR);                              /* clear register */
      while (!(*CIABICR & INDEX)&&(i!=0))
         i++;
   }
   else while ((*PortA & DSK_RDY)&&(i!=0))
      i++;                 /* now we're right after the index pulse */
   if (i==0) return(E_NO_DRIVE);
   i=1;
   *DSKLEN=*DSKLEN=(DMAEN | WRITE | BUFSIZE);

   *INTREQ=DSKBLK;                        /* mark last intr as seen */
   while (!(*INTREQR & DSKBLK) && (i!=0))
      i++;                                          /* busy waiting */
   *DSKLEN=0;                /* disable diskDMA (just for security) */
   *INTREQ=DSKBLK;                             /* mark intr as seen */
   if (i==0) return(E_DISK_DMA);
   return (OK);
}


PRIVATE BYTE MFM2bin(code)
register WORD code;
{  register BYTE bin=0;
   register WORD c1,c2;

   for (c1=1,c2=1; c1<255; c1<<=1,c2<<=2)
      if (code&c2) bin|=c1;
   return bin;
}

PRIVATE WORD bin2MFM(offset,byte)
int offset;
BYTE byte;

{  register WORD byte2,code=0;
   register int ci,bi,bbi;

   byte2=byte|(DiskBuf[offset-1]<<8);

   for(ci=1,bi=1,bbi=3; bi<130; ci<<=2,bi<<=1,bbi<<=1)
   {  if (byte2&bi) code|=ci;                       /* MFM data-bit */
      if ((byte2&bbi)==0) code|=(ci<<1);             /* MFM tag-bit */
   }
   return (DiskBuf[offset]=code);
}

PRIVATE WORD crc(offset,count)
int offset,count;                /* The initial CRC is 0xFFFF, but  */
{  int i,c=0xCDB4;               /* c is the CRC after three 0xA1's */
   WORD j, byte;

   reg_d3=c;
   reg_d2=count-1;
   reg_a2=&DiskBuf[offset];
#asm
    movem.l  d1-d4/a2,-(sp)  ; All effort has been made to make
    move.l   _reg_a2,a2      ; the following assembler-routines as
    move.l   _reg_d2,d2      ; fast as possible.
    move.l   _reg_d3,d3      ; This did not allways result in better
.L4 clr.l    d4              ; code, but since it is used very often
    moveq    #14,d4          ; thre was not much of a choice.
    move.w   (a2)+,d1
.L5 btst     d4,d1           ; This routine takes about 40 ms for
    beq      .L1             ; a 512 byte block.
    eori.w   #$8000,d3
.L1 tst.w    d3
    bpl      .L2
    eori.w   #$0810,d3
.L2 rol.w    #1,d3
    subq.b   #2,d4
    bpl      .L5
    dbf      d2,.L4
    move.l   d3,_reg_d3
    movem.l  (sp)+,d1-d4/a2
#endasm
   c=reg_d3;

   return c;
}

PRIVATE raw2bin(offset,binbuf)        /*  MFM to binary + check CRC */
int offset;
BYTE binbuf[];
{  int c;

   reg_a2=&DiskBuf[offset+D_DATA];
   reg_a3=&binbuf[0];
#asm
    movem.l  D1-D5/A2-A3,-(sp)
    move.l   _reg_a2,a2    ; registers just as bin2raw.
    move.l   _reg_a3,a3
    move.l   #511,d2
    move.l   #$E295,d3
.N4 move.w   (a2)+,d5
    moveq    #7,d4
.N3 lsl.w    #2,d5
    bcc      .N1           ; bit is 0?
    eori.w   #$8000,d3
.N1 roxl.b   #1,d1
    tst.w    d3
    bpl      .N2
    eori     #$0810,d3
.N2 rol.w    #1,d3
    dbf      d4,.N3        ;next bit
    move.b   d1,(a3)+
    dbf      d2,.N4        ;next BYTE
    move.l   d3,_reg_d3
    movem.l  (sp)+,D1-D5/A2-A3
#endasm
   c=(WORD)reg_d3;
   if (MFM2bin(DiskBuf[offset+D_CRC]) != (BYTE)(c>>8) ) return (E_CRC);
   if (MFM2bin(DiskBuf[offset+D_CRC+1]) != (c&0xFF) ) return (E_CRC);
   return(OK);
}

PRIVATE bin2raw(binbuf,offset) /* convert data block to raw MFM+CRC */
BYTE binbuf[];
int offset;
{  WORD c;

   reg_a3=&binbuf[0];
   reg_a2=&DiskBuf[offset];
#asm
    movem.l  D1-D5/A2-A3,-(sp)
                           ; d1 = BYTE from binbuf
    move.l   _reg_a2,a2    ; d2 = counter for 512 bytes
    move.l   _reg_a3,a3    ; d3 = the CRC (after 0xA1 0xA1 0xA1 0xFB)
    move.l   #511,d2       ; d4 = bit count (7..0)
                           ; d5 = MFM-WORD
    move.l   #$E295,d3     ; a2 = address of next MFM-WORD
                           ; a3 = address of next bin-BYTE
    move.w   (a2)+,d5      ; get previous MFM-WORD (=DATA_ID)
.M4 moveq    #7,d4
    move.b   (a3)+,d1      ; get a BYTE
.M5 lsl.w    #2,d5         ; make room for T&D bits.
    lsl.b    #1,d1         ; bit to X/C
    bcc      .M3
    addq.w   #1,d5         ;  T=0,D=1
    eori.w   #$8000,d3     ; invert bit #15 (=bchg #15,d3)
    bra      .M1
.M3 btst     #2,d5         ; was previous bit also 0?
    bne      .M6           ;  T=0,D=0
    addq.w   #2,d5         ;  T=1,D=0
.M6 tst.w    d3            ; bit #15 in crc set?
.M1 bpl      .M2
    eori.w   #$0810,d3     ; yes, crc^=0x0810
.M2 rol.w    #1,d3         ; CRC: 0>1,1>2..14>15,15>0
    dbf      d4,.M5        ; next bit in BYTE
    move.w   d5,(a2)+      ; store MFM-WORD
    dbf      d2,.M4        ; next BYTE
    move.l   d3,_reg_d3
    movem.l  (sp)+,D1-D5/A2-A3
#endasm
   c=(WORD)reg_d3;
   bin2MFM(offset+D_CRC,(BYTE)(c>>8));
   bin2MFM(offset+D_CRC+1,(BYTE)(c&0xFF));
}

PRIVATE adjust_buffer()
{
  int count=0,p,st,tk=0;
  register int i,offset=0,tmpoff;
  register long *a1,*a2;
  char check[NR_SECTORS];

  for (st=0;st<NR_SECTORS;st++)
	check[st] = 0;
  while (offset < RAW_T_SIZE && count < NR_SECTORS) {
	while (DiskBuf2[offset] != SYNC_DATA) offset++;
	while (DiskBuf2[offset] == SYNC_DATA) offset++;
	if (DiskBuf2[offset] != H_ID_MFM)		/* Tough luck */
		continue;
	tmpoff = offset;	/* Save hoeder offset */
	st = MFM2bin(DiskBuf2[tmpoff + H_SECTOR])-1;
	offset += H_SIZE + GAP3_SIZE;

	while (DiskBuf2[offset] != SYNC_DATA) offset++;
	while (DiskBuf2[offset] == SYNC_DATA) offset++;
	if (DiskBuf2[offset] != D_ID_MFM)		/* Bad track */
		continue;
	if (check[st])
		continue;	/* Sector appeared twice on same track */
	check[st] = 1; count++;

	a1= (long*)&DiskBuf2[tmpoff];
	a2= (long*)&DiskBuf[H_OFFSET + st*RAW_S_SIZE];
	for (i=4;i>0;i--)		/* Copy block (2*4=8bytes) */
		*a2++ = *a1++;

	a1= (long*)&DiskBuf2[offset];
	a2= (long*)&DiskBuf[D_OFFSET + st*RAW_S_SIZE];
	for (i=258;i>0;i--)		/* Copy block (2*258=516bytes) */
		*a2++ = *a1++;

	offset += D_SIZE;
	tk += MFM2bin(DiskBuf2[tmpoff + H_TRACK]);
  }
  if (count < NR_SECTORS)
	for (st = 0; st < NR_SECTORS; st++) {
		putchar(check[st] ? '!' : '.');	/* DEBUG */
		if (!check[st])		/* Mark sector as bad */
			DiskBuf[D_OFFSET + D_DATA + st*RAW_S_SIZE]++;
	}
  else {
	if (tk%NR_SECTORS) return(OK);	/* Not really OK, but what else? */
	tk /= NR_SECTORS;
	if (ctrk != tk) {
		p = ctrk; ctrk = tk;
		seek(drive, p, side);
		return(E_WRONG_TRACK);    
  	}
  }
  return (OK);
}


PRIVATE seek(dr,tk,sd)
int dr,tk,sd;
{
   if ( (dr==drive) && (tk==ctrk) && (sd==side) ) return;
   if (buf_dirty) do_rdwt(WRITE);
   buf_valid=FALSE;
   drive=dr;
   side=sd;
   if (ctrk==UNCALIBRATED)
      if (r=recalibrate()) return (r);
   start_motor();
   while (ctrk<tk) movehead(1),ctrk++;
   while (ctrk>tk) movehead(-1),ctrk--;
   wr_prot=writeprotect();
   return (OK);
}

PRIVATE do_rdwt(act)                  /* READ or WRITE a full track */
int act;
{  int retries=MAX_RETRIES;

   r=OK;
   buf_valid=FALSE;
   portBout((side==0)?SET:CLR,DSK_SIDE);
   do
   { lock();	/* Some mysterious routine enables intrs. now and then */
     r=(act==READ)?read_track():write_track();
   } while (r && --retries);
   if (act==READ) buf_valid=(r==OK);
   buf_dirty=FALSE;
   return(r);
}

PRIVATE recalibrate()
{  int i;

   portBout(CLR,DSK_SEL);
   for (i=0;i<(2*NR_TRACKS) && !track0();i++)
      movehead(-1);
   if (!track0())
      return(E_NO_DRIVE);
   ctrk=0;
   return (OK);
}

PRIVATE phys_convert(blk,tk,sd,st)
int blk,*tk,*sd,*st;
{  if (blk<0 || blk>=(NR_TRACKS*NR_SIDES*NR_SECTORS) )
      return(E_BAD_ARGS);
   *tk=blk/(NR_SIDES*NR_SECTORS);
   *st=blk%NR_SECTORS+1;
   *sd=(blk%(NR_SIDES*NR_SECTORS))/NR_SECTORS;
   return(OK);
}

PRIVATE build_track()   /* build an empty track (params: ctrk,side) */ 
{  int st;
   register int i,p=0;
   WORD c;

   for (st=0;st<NR_SECTORS;st++)
   {
      for (i=0;i<GAP1_SIZE;i++)
         DiskBuf[p++]=GAP1_DATA;
      for (i=0;i<GAP2_SIZE;i++)
         DiskBuf[p++]=GAP2_DATA;

      for (i=0;i<SYNC_SIZE;i++)                      /* header-sync */ 
         DiskBuf[p++]=SYNC_DATA;
      bin2MFM(p+H_ID,H_ID_BIN);
      bin2MFM(p+H_TRACK,ctrk);
      bin2MFM(p+H_SIDE,side);
      bin2MFM(p+H_SECTOR,st+1);
      bin2MFM(p+H_LENGTH,2);     /* indicating a SECTOR_SIZE of 512 */
      c=crc(p,H_CRC);
      bin2MFM(p+H_CRC,c>>8);
      bin2MFM(p+H_CRC+1,c&0xFF);
      p+=H_CRC+2;

      for (i=0;i<GAP3_SIZE;i++)      /* gap between header and data */
         DiskBuf[p++]=GAP3_DATA;
      for (i=0;i<GAP4_SIZE;i++)
         DiskBuf[p++]=GAP4_DATA;

      for (i=0;i<SYNC_SIZE;i++)                        /* data-sync */
         DiskBuf[p++]=SYNC_DATA;
      bin2MFM(p+D_ID,D_ID_BIN);
      for (i=1;i<SECTOR_SIZE+1;i++)
         DiskBuf[p+i]=0xAAAA;          /* fill data block with 0x00 */
      DiskBuf[p+D_CRC]=0x5144;          /* pre-computed crc: 0xDA6E */
      DiskBuf[p+D_CRC+1]=0x9454;
      p+=D_CRC+2;
   }
   while (p<BUFSIZE)
      DiskBuf[p++]=GAP1_DATA;
}

PUBLIC disk_init()
{  int i;

   oldADKCON=*ADKCONR;
   build_track();
   for (drive=0;drive<NR_DRIVES;drive++)    
   {  track[drive]=UNCALIBRATED;
      portBout(SET,DSK_SEL);                 /* unselect all drives */
   }                                                    
   drive = 0;
}                                                     
                                                  
PUBLIC disk_quit()
{
   disk_sync();
   stop_motor();
   *ADKCON=~(SET | oldADKCON); /* Reset a funny register */
   *ADKCON=(SET | oldADKCON); 
   unlock();
}

PUBLIC disk_sync()
{  if (buf_dirty)
      if (r=do_rdwt(WRITE))
         printf("panic: disk_sync ran into error:%d\n",r);
}

PUBLIC disk_format(dr,t1,t2)            /* format tracks t1 upto t2 */
int dr,t1,t2;
{  int tk,sd;

   if (t1<0 || t1>t2 || t2>79) return (E_BAD_ARGS);
   track[dr]=UNCALIBRATED;                   /* force recalibration */
   buf_dirty=FALSE;
   for (tk=t1;tk<=t2;tk++)
      for (sd=0;sd<NR_SIDES;sd++)
      {  if (r=seek(dr,tk,sd)) return (r);
         build_track();
         if (r=do_rdwt(WRITE)) return (r);
      }
   return (OK);
}

PUBLIC disk_read(buffer,dr,blk)
BYTE *buffer;
int dr,blk;
{  int tk,sd,st;
   if (r=phys_convert(blk,&tk,&sd,&st)) return(r);
   if (r=seek(dr,tk,sd)) return (r);
   if (!buf_valid)
      if (r=do_rdwt(READ)) return (r);
   return (raw2bin((st-1)*RAW_S_SIZE+D_OFFSET,buffer));
}

PUBLIC disk_write(buffer,dr,blk)        /* The actual write is done */
BYTE *buffer;                    /* only in case of a seek or sync */
int dr,blk;
{  int tk,sd,st;
   if (r=phys_convert(blk,&tk,&sd,&st)) return(r);
   if (r=seek(dr,tk,sd)) return (r);
   if (wr_prot) return(E_WR_PROT);
   if (!buf_valid)
      if (r=do_rdwt(READ)) return (r);
   bin2raw(buffer,(st-1)*RAW_S_SIZE+D_OFFSET);
   buf_dirty=TRUE;
   return(OK);
}