/*
 * Main program for R4300 for a disk write program. Since this program is
 * using host io protocol and host io protocol uses Int 1 and cart int, we
 * have to do a certain tricky technique to share these interrupts between
 * host io and 64 disk drive. The method I used here is "changing" the 
 * interrupt service routine of Int 1 and the message queue that the message
 * from cartridge will be sent. See the source code of osLeoSwitch() for
 * more information.
 */
#include "hostio.h"
#include "../ddwrite.h"

#define KMC 1
#define KMC_DRV	1 /* 1.. use libgultra_rom, 0.. use libgultra_d */

/*
 * Stacks for the threads as well as message queues for synchronization
 * This stack is ridiculously large, and could also be reclaimed once
 * the main thread is started.
 */
u64	bootStack[STACKSIZE/sizeof(u64)];

static void	idle(void *);
static void	mainproc(void *);

static OSThread	idleThread;
static u64	idleThreadStack[STACKSIZE/sizeof(u64)];

static OSThread	mainThread;
static u64	mainThreadStack[5 * STACKSIZE/sizeof(u64)];

/*
 * Message queues
 */
#define NUM_PI_MSGS     8
static OSMesg PiMessages[NUM_PI_MSGS];
static OSMesg           dmaMessageBuf;
static OSMesgQueue PiMessageQ,dmaMessageQ;

static OSMesgQueue	diskQ;		/* post queue */
static OSMesg		diskQBuf;

#define NUM_CMD_MSGS	8
static OSMesg           cmdQBuf[8];

/*
 * Prototypes
 */
static	s32   	error_handle(s32, u8);
static	void 	getDiskInfo(ctrlblk *sdata);
static	s32 	getdata(void *addr, u32 nbytes);
static	void 	formatHook(void);
static	void 	infoHook(DDInfo *info);


/*
 * Definition for error handling routine
 */
#define	RETRY	0
#define	FATAL	1

/*
 * These variables are defined in each file to align 16bytes.
 * These are for communication to Indy.
 */

#if KMC /* { */

#if KMC_DRV /* { */

#define osSyncPrintf kmc_drv_printf

void kmc_drv_printf(const char *p, ...){}

#endif /* } */

#undef BUFSIZE
#define BUFSIZE		0x100000
#define rdramBuf	((u64 *)0x80200000)
#define DISK_ERROR	0xffffffff
#define DISK_INSERT	0xfffffffe
#define DISK_FORMAT	0xfffffffd

unsigned lba_end;
int kmc_init_flg=0;

extern void ptstart(void);

volatile unsigned *ptwtmode= (unsigned *)0xbff08014;

volatile unsigned *ptstat= (unsigned *)0xbff08004;
volatile unsigned *ptport= (unsigned *)0xbff08000;
volatile unsigned *n64piok= (unsigned *)0xa4600010;

static void putPT(unsigned c)
{
    while((*n64piok & 3)!=0) ;
    while((*ptstat & 4)==0) ;
    *ptport=c;
}

static unsigned getPT(void)
{
    while((*n64piok & 3)!=0) ;
    while((*ptstat & 2)==0) ;
    return *ptport;
}

void romCopy(const char *src, const char *dest, const int len)
{
    OSIoMesg dmaIoMesgBuf;
    OSMesg dummyMesg;
    
    /*
     * Always invalidate cache before dma'ing data into the buffer.
     * This is to prevent a flush of the cache in the future from 
     * potentially trashing some data that has just been dma'ed in.
     * Since you don't care if old data makes it from cache out to 
     * memory, you can use the cheaper osInvalDCache() instead of one
     * of the writeback commands
     */
    osInvalDCache((void *)dest, (s32) len);
    osPiStartDma(&dmaIoMesgBuf, OS_MESG_PRI_NORMAL, OS_READ,
                 (u32)src, (void *)dest, (u32)len, &dmaMessageQ);
    (void) osRecvMesg(&dmaMessageQ, &dummyMesg, OS_MESG_BLOCK);
}

void kmc_dd_status(u8 status, u8 sense, int errno)
{
    if(errno==1){
	putPT(DISK_INSERT);
	return;
    }
    if(errno==3){
	putPT(DISK_FORMAT);
	return;
    }
    putPT(DISK_ERROR);

    putPT(status);
    putPT(sense);

    putPT(errno);
    if(errno!=2){
	for(;;);
    }
}


unsigned kmc_type;
unsigned kmc_addr;
unsigned kmc_iplsize;

int leoid_flg;

#else /* }{ */
extern	u64	rdramBuf[];
#endif /* } */

s32 OS_CMD=OS_WRITE;

extern	ctrlblk	data;
extern	s32	transbyte;

void
boot(void)
{
  osInitialize();

#if KMC_DRV /* { */
  {
     unsigned tmp;

     tmp=*ptwtmode;
  }
#endif /* } */

  osCreateThread(&idleThread, 1, idle, (void *)0,
		 idleThreadStack+STACKSIZE/sizeof(u64), 10);
  osStartThread(&idleThread);
  
  /* never reached */
}

static void
idle(void *arg)
{
  /*
   * Create main thread
   */
  osCreateThread(&mainThread, 3, mainproc, arg,
		 mainThreadStack+STACKSIZE/sizeof(u64), 10);
  osStartThread(&mainThread);
  
  /*
   * Become the idle thread
   */
  osSetThreadPri(0, 0);
  
  for (;;);
}

/*
 * This is the main routine of the app.
 */
static void
mainproc(void *arg)
{
  LEOError	error;
  LEOCmd	cmdBlock;
  unsigned xfer_adr;
  s32		curLBA;
  u32		transLBA;
  u32		needLBAs;
  u32		bufferLBAs;
  u32		remain;
  u32		result;
  DDCmd		command;
  
  osCreateMesgQueue(&diskQ, &diskQBuf, 1);

  osCreatePiManager((OSPri)OS_PRIORITY_PIMGR, &PiMessageQ, PiMessages,
		    NUM_PI_MSGS);

  error = LeoCreateLeoManager((OSPri)OS_PRIORITY_LEOMGR-1,
			      (OSPri)OS_PRIORITY_LEOMGR,
			      cmdQBuf, NUM_CMD_MSGS);

  if (error == LEO_ERROR_DEVICE_COMMUNICATION_FAILURE)
  {
#if KMC /* { */
    putPT(DISK_ERROR);
#endif /* } */
    osSyncPrintf("leowrite: Drive doesn't exist\n");
    for(;;);
  }
  
  LeoResetClear();

  while(1)			/* main loop */
  {
#if KMC /* { */

    osCreateMesgQueue(&dmaMessageQ, &dmaMessageBuf, 1);

#if KMC_DRV /* { */
    putPT(0);
#endif /* } */

cmd_loop:
#if KMC_DRV /* { */
    leoid_flg=0;
    data.cmdFlags=0;
    data.type=getPT();			/* Disk type (not change -1) */
    data.offset=getPT();		/* LBA */
    data.filesize=getPT();		/* File size */
    data.address=(void **)getPT();	/* Start address (not change 0) */
    lba_end=getPT();			/* end of LBA */
    OS_CMD=getPT();
    if(OS_CMD & 2){
	data.cmdFlags |= DO_FORMAT;
    }
    if(OS_CMD & 4){
	data.cmdFlags |= SEND_ME_INFO;
	goto kmc_send_info;
    }
    if(OS_CMD & 8){
	data.cmdFlags |= SPECD_ID_FILE;
    }
    if(OS_CMD & 0x10){
	leoid_flg=1;
    }

    OS_CMD= (OS_CMD & 1) ? OS_READ:OS_WRITE;	/* 1.. OS_READ , 0.. OS_WRITE */

    {
	unsigned *dp;
	unsigned loop_ct;

	loop_ct=sizeof(LEODiskID)/sizeof(unsigned);
	dp=(unsigned *)&data.diskID;
	do{
	    *dp=getPT();
	    ++dp;
	    --loop_ct;
	}while(loop_ct) ;
    }

    if(data.offset==0xffffffff){
	goto kmc_end;
    }
#else /* }{ */
    data.type=0xffffffff;
    data.offset=6;
    data.filesize=0x1000;
    data.cmdFlags=0;
    data.diskID.serialNumber.lineNumber=0x7367690000000000ll;
    data.diskID.serialNumber.time.pad=0;
    data.diskID.serialNumber.time.yearlo=0x96;
    data.diskID.serialNumber.time.month=0x10;
    data.diskID.serialNumber.time.day=0x23;
    data.diskID.serialNumber.time.hour=0x16;
    data.diskID.serialNumber.time.minute=0x41;
    data.diskID.serialNumber.time.second=0x30;
#endif /* } */

    if(kmc_init_flg) goto kmc_next;
kmc_send_info:
#else /* }{ */
    /*
     * Send 'Ready' to Indy
     */
    osWriteHost(rdramBuf, 4);

    /*
     * First, we get the type, filesize and offset.
     */
    osReadHost(&data, sizeof(ctrlblk));

#endif /* } */

    if (data.cmdFlags & END_COMMAND)
    {
      /*
       * End of leowrite. Stop the motor of the drive
       */
      do
      {
	LeoSpdlMotor(&cmdBlock, SLEEP, &diskQ);
	osRecvMesg(&diskQ, NULL, OS_MESG_BLOCK);
    
	if ((GET_ERROR(cmdBlock) == 0) || 
	    (GET_ERROR(cmdBlock) == LEO_ERROR_MEDIUM_NOT_PRESENT))
	  break;
  	if (error_handle(GET_ERROR(cmdBlock), cmdBlock.header.status) == FATAL)
	{
	  osSyncPrintf("Fatal error\n");
	  for(;;);
	}
    
      } while(1);
    
#if !KMC /* { */
      /*
       * Send back the end command
       */
      osWriteHost(&data, sizeof(ctrlblk));
#endif /* } */
  
      /* Wait host to kill me */
      for(;;);

    }
  
    if (data.cmdFlags & SEND_ME_INFO)
    {
      getDiskInfo(&data);
    
      if ( !(data.cmdFlags & SYS_UNFORMATTED) ) /* if sys is unformatted, might hang */
      {
	/*
	 * Stop the motor of the drive
	 */
	do
	{
	  LeoSpdlMotor(&cmdBlock, SLEEP, &diskQ);
	  osRecvMesg(&diskQ, NULL, OS_MESG_BLOCK);
    
	  if ((GET_ERROR(cmdBlock) == 0) || 
	      (GET_ERROR(cmdBlock) == LEO_ERROR_MEDIUM_NOT_PRESENT))
	    break;
  	  if (error_handle(GET_ERROR(cmdBlock), cmdBlock.header.status) == FATAL)
	  {
	    osSyncPrintf("Fatal error\n");
	    for(;;);
	  }
    
	} while(1);
      }
#if KMC /* { */
      putPT(data.type);
      putPT((unsigned)data.address);
      putPT(data.offset);
      {
	unsigned *dp;
	unsigned loop_ct;

	loop_ct=sizeof(LEODiskID)/sizeof(unsigned);
	dp=(unsigned *)&data.diskID;
	do{
	    putPT(*dp);
	    ++dp;
	    --loop_ct;
	}while(loop_ct) ;
      }
#else /* }{ */
      osWriteHost(&data, sizeof(ctrlblk));
#endif /* } */
      continue;
      
    } /* if (SEND_ME_INFO) */
  
    /*
     * If the system area has never been written, format the disk.
     * Otherwise, change the area according as the specified parameters.
     */

#if KMC /* { */
    if(OS_CMD!=OS_WRITE){
	unsigned offset_bak,filesize_bak;
	offset_bak=data.offset;
	filesize_bak=data.filesize;
	getDiskInfo(&data);
	data.offset=offset_bak;
	data.filesize=filesize_bak;
	kmc_init_flg=1;
	goto kmc_next;
    }
#endif /* } */

    if(OS_CMD == OS_WRITE)
    {
	
	/*
	 * まずはフォーマット
	 */
	command.startLBA = (u32)data.offset;
	command.diskType = data.type;
	command.loadAddr = data.address;

	command.flags = 0;

#ifdef ID_NOT_CHANGE	

	/* idAddr を 0 にすると、ID を変更しない */
	command.idAddr = 0;

#else
	command.idAddr = &data.diskID;

	if (! (data.cmdFlags & SPECD_ID_FILE) )
	{
	    /* ID が指定されていないときは、シリアル番号のみ上書きする */
	    command.flags |= DDWRITE_FLG_IDSERIAL;
	    
	}
#endif
	
	if (data.offset == 0)
	    command.iplSize = (u32)((data.filesize + 19719) / 19720);
	else
	    command.iplSize = 0;
	
	command.writeBuf = (void *)rdramBuf;
	command.writeSize = BUFSIZE;
	command.getdata = getdata;
	command.totalSize = 0;	/* フォーマットだけしたいので、サイズは0 */
	if (data.cmdFlags & DO_FORMAT)
	    command.flags |= DDWRITE_FLG_FORMAT;
	command.formatHook = formatHook;
	command.infoHook = infoHook;

	/*
	 * フォーマットする
	 */
	do{
	    error = (LEOError)ddWrite(&command);
	    
	    if (error == 0)
		break;
	    
	    if (error_handle(error, 2) == FATAL)
	    {
		osSyncPrintf("Fatal error\n");
		for(;;);
	    }

	} while(1);
    }
    else
    {
	/*
	 * ID を読まないと、エラー47 が起き続けてしまう
	 */
	do{
	    LEODiskID dummyDiskID;

	    LeoReadDiskID(&cmdBlock, &dummyDiskID, &diskQ);
	    osRecvMesg(&diskQ, NULL, OS_MESG_BLOCK);

	    if(GET_ERROR(cmdBlock) == 0)
		break;
	    if (error_handle(GET_ERROR(cmdBlock), cmdBlock.header.status) == FATAL)
	    {
		osSyncPrintf("Fatal error\n");
		for(;;);
	    }
	} while(1);
    }
  
#if KMC /* { */
  kmc_init_flg=1;
kmc_next:
    if(leoid_flg){
	putPT(0);
	goto cmd_loop;
    }
#endif /* } */
    curLBA = data.offset;
#if KMC /* { */
    if(data.filesize!=0){
	do{
	    result = LeoByteToLBA(curLBA, data.filesize, (s32 *)&needLBAs);
	    if (result == 0) break;
	    if (error_handle(3, 2) == FATAL)
	    {
		osSyncPrintf("Fatal error\n");
		for(;;);
	    }
	} while(1);
	if(lba_end!=0){
	    if(lba_end<(data.offset+needLBAs-1)){
		data.filesize=0;
	    }
	}
    }
    if(data.filesize==0){
	do{
	    result = LeoLBAToByte(curLBA,needLBAs=lba_end - curLBA + 1, (s32 *)&data.filesize);
	    if(result == 0) break;
	    if (error_handle(3, 2) == FATAL)
	    {
		osSyncPrintf("Fatal error\n");
		for(;;);
	    }
	} while(1);
    }
#endif /* } */
    /*
     * If the filesize is larger than the size of the remained area,
     * we quit the operation here.
     */
    do{
      result = LeoLBAToByte(curLBA, NUM_LBAS - curLBA, (s32 *)&remain);

      if(result == 0)
	break;
      if (error_handle(3, 2) == FATAL)
      {
	osSyncPrintf("Fatal error\n");
	for(;;);
      }
    } while(1);
  
    if (remain < data.filesize)
    {
#if KMC /* { */
      kmc_dd_status(cmdBlock.header.status, cmdBlock.header.sense, 2);
      putPT(remain);
      putPT(data.filesize);
#else /* }{ */
      osSyncPrintf("file is too large: remaining disk space %d, filesize %d\n",
		   remain, data.filesize);
      transbyte = -1;
      osWriteHost(&transbyte, sizeof(transbyte));
#endif /* } */

      /* Wait host to kill me */
      for(;;);
    }
  
    /*
     * Calculate how many lbas are needed to write the file.
     */
    do{
      result = LeoByteToLBA(curLBA, data.filesize, (s32 *)&needLBAs);
      if (result == 0)
	break;
      if (error_handle(3, 2) == FATAL)
      {
	osSyncPrintf("Fatal error\n");
	for(;;);
      }
    } while(1);

    osSyncPrintf("Use from LBA%d to %u", curLBA,
		 curLBA + needLBAs - 1);
  
    osSyncPrintf("(%d LBA(s))\n", needLBAs);

    /*
     * Main loop
     */
#if KMC /* { */
    putPT(kmc_type);
    putPT(kmc_iplsize);
    putPT(kmc_addr);
    putPT(remain);
    putPT(needLBAs);
#endif /* } */

    if (OS_CMD == OS_WRITE)
    {
	
	/*
	 * ライトの準備
	 */
	command.startLBA = (u32)curLBA;
	command.diskType = -1;
	command.loadAddr = 0;
	command.idAddr = 0; 		/* ID は一回目で書いている */

	command.flags = 0;
	
	command.iplSize = 0;
	command.writeBuf = (void *)rdramBuf;
	command.writeSize = BUFSIZE;
	command.getdata = getdata;
	command.totalSize = data.filesize;
	command.formatHook = (void (*)(void))0; /* フォーマットされることはない */
	command.infoHook = (void (*)(DDInfo *))0;

	/*
	 * ライトする
	 */
	do{
	    error = (LEOError)ddWrite(&command);
	    
	    if (error == 0)
		break;
	    
	    if (error_handle(error, 2) == FATAL)
	    {
		osSyncPrintf("Fatal error\n");
		for(;;);
	    }

	} while(1);

	putPT(0);
	goto cmd_loop;
	
    }
    
    while(data.filesize > 0)
    {
      /*
       * Calculate how many lbas can be stored in the RDRAM buffer.
       */
      if (BUFSIZE > remain)
	bufferLBAs = NUM_LBAS - curLBA;
      else
      {
	do{
	  result = LeoByteToLBA(curLBA, BUFSIZE, (s32 *)&bufferLBAs);

	  if(result == 0)
	    break;
	  if (error_handle(3, 2) == FATAL)
	  {
	    osSyncPrintf("Fatal error\n");
	    for(;;);
	  }
	} while(1);

	/*
	 * Since BUFSIZE is smaller than the size of bufferLBAs,
	 * decrement bufferLBAs.
	 * XXX we might miss one loop (when BUFSIZE = bufferLBAs)
	 */
	bufferLBAs --;
      }

      /*
       * Calculate how many lbas and bytes should be transferred this
       * time.
       */

      transLBA = Min(needLBAs, bufferLBAs);
      do{
	result = LeoLBAToByte(curLBA, transLBA, (s32 *)&transbyte);

	if(result == 0)
	  break;
	if (error_handle(3, 2) == FATAL)
	{
	  osSyncPrintf("Fatal error\n");
	  for(;;);
	}
      } while(1);
    
      /*
       * Ask host to transfer <transbyte> bytes
       */
#if KMC /* { */
      putPT(transbyte);
      
#endif /* } */      

      /*
       * Write <transLBA> blocks to disk
       */
      do{
	LeoReadWrite(&cmdBlock, OS_CMD,
		     (u32)curLBA, (void *)rdramBuf, (u32)transLBA, &diskQ);
	osRecvMesg(&diskQ, NULL, OS_MESG_BLOCK);

	if(GET_ERROR(cmdBlock) == 0)
	  break;
	if (error_handle(GET_ERROR(cmdBlock), cmdBlock.header.status)== FATAL)
	{
	  osSyncPrintf("Fatal error\n");
	  for(;;);
	}
      } while(1);

      curLBA += transLBA;
      needLBAs -= transLBA;
      data.filesize -= transbyte;
      remain -= transbyte;
    
#if KMC /* { */
      if(OS_CMD==OS_READ){
	  unsigned size;
	  unsigned *s;

	  putPT(1);
	  osWritebackDCacheAll();
	  size=transbyte;
	  s=(unsigned *)rdramBuf;
	  while(size!=0){
	      putPT(*(s++));
	      size -= 4;
	  }
       }
#endif /* } */
    } /* while */

#if KMC /* { */
  putPT(0);
  goto cmd_loop;

kmc_end:
#endif /* } */
    /*
     * Send end mark to host
     */
#if KMC /* { */
    putPT(0);
#else /* }{ */
    transbyte = -1;
    osWriteHost(&transbyte, sizeof(transbyte));
#endif /* } */

  } /* while */
  
} /* mainproc() */

/*
 * Error handling routine. In this version, I don't check the ID blocks.
 * XXX need to fix
 */
s32 error_handle(s32 result, u8 status)
{
  LEOStatus	test;
  s32		res;
  
  /*
   * If the error is not one of these two, consider it as fatal.
   */
  if (!( (result == LEO_ERROR_MEDIUM_NOT_PRESENT) ||
      (result == LEO_ERROR_MEDIUM_MAY_HAVE_CHANGED ) )){
#if KMC /* { */
      kmc_dd_status(status, (u8)result, 0);
#endif /* } */
    return FATAL;
  }
  /*
   * Since this version doesn't check the ID, I don't care if the disk
   * might have been changed.
   */
  if (result == LEO_ERROR_MEDIUM_MAY_HAVE_CHANGED)
    return RETRY;
  
#if KMC /* { */
  kmc_dd_status(status, (u8)result, 1);
#endif /* } */
  osSyncPrintf("Insert a disk\n");

  /*
   *  Wait for the users to insert the disk.
   *  (test_unit_ready command doesn't check the system area).
   */
  do
  {
    res = LeoTestUnitReady(&test);
    
    if ( !( (res == LEO_ERROR_MEDIUM_NOT_PRESENT) ||
	   (res == LEO_ERROR_MEDIUM_MAY_HAVE_CHANGED) ) )
    {
#if KMC /* { */
      kmc_dd_status(status, (u8)result, 0);
#endif /* } */
      osSyncPrintf("error = 0x%x\n", res);
      return FATAL;
    }
  } while( res != LEO_ERROR_MEDIUM_MAY_HAVE_CHANGED );
  
  return RETRY;

} /* error_handle() */


#define	FORMATTED	0
#define	UNFORMATTED	1


void getDiskInfo(ctrlblk *sdata)
{
  LEOCmd        cmdBlock;
  LEOError      error = 0;
  DDInfo        info;
  s32           result;

  result = ddGetInfo(&info);

  if (result == LEO_ERROR_UNRECOVERED_READ_ERROR)
  {
    sdata->cmdFlags |= SYS_UNFORMATTED;
    return;
  }
  
  sdata->offset         = (s32)info.iplSize;
  sdata->type           = info.diskType;
  sdata->address        = info.loadAddr;

#if KMC /* { */
   kmc_type = sdata->type;
   kmc_addr = (unsigned)sdata->address;
   kmc_iplsize = sdata->offset;
#endif /* } */

  do{
    LeoReadDiskID(&cmdBlock, &sdata->diskID, &diskQ);
    osRecvMesg(&diskQ, (OSMesg)&error, OS_MESG_BLOCK);

    if(error == 0)
      return;
    else if(error == LEO_ERROR_UNRECOVERED_READ_ERROR)
    {
      sdata->cmdFlags |= ID_UNFORMATTED;
      return;
    }
    else if (error_handle(GET_ERROR(cmdBlock), cmdBlock.header.status) == FATAL)
    {
      osSyncPrintf("Fatal error\n");
      for(;;);
    }
  } while(1);

  return;
  
} /* getDiskInfo */


/*
 * getdata 関数。関数のポインタを ddwrite に渡す。
 */
static s32 getdata(void *addr, u32 nbytes)
{
    unsigned xfer_adr;

#if KMC /* { */
    putPT(nbytes);

    xfer_adr=getPT();
#if 0 /* { CPU */
    {
	unsigned size;
	unsigned *s,*d;

	size=nbytes;
	s=(unsigned *)xfer_adr;
	d=(unsigned *)addr;
	while(size!=0){
	    *(d++)=*(s++);
	    size -= 4;
	}
    }
#else /* }{ DMA */
    romCopy((char *)(xfer_adr-0xb0000000),(char *)addr, nbytes);
#endif /* } */
    osWritebackDCacheAll();
    putPT(1);

#else /* }{ */
    osWriteHost((void *)&nbytes, sizeof(nbytes));
    
    osReadHost(addr, nbytes);
    
#endif /* } */
    
    return 0;

}


static void formatHook(void)
{
    /*
     * ディスクがフォーマットされるときに、フックされる
     */
    kmc_dd_status(0, 0, 3);

    return;

}


static void infoHook(DDInfo *info)
{
    /*
     * ディスクの情報を、システム情報ライト後にフックして知らせる
     */
    kmc_type = info->diskType;
    kmc_addr = (u32)info->loadAddr;
    kmc_iplsize = info->iplSize;

    return;
}