/*---------------------------------------------------------------------
        Copyright (C) 1997, Nintendo.
        
        File            motor.c
        Coded    by     Koji Mitsunari. Oct 11, 1996.
        Modified by     Koji Mitsunari. Feb 18, 1998.
        Comments        RUMBLE PAK , Initialize, start & stop
   
        $Id: motor.c,v 1.1.1.2 2002/10/29 08:06:44 blythe Exp $
   ---------------------------------------------------------------------*/

/**************************************************************************
 *
 *  $Revision: 1.1.1.2 $
 *  $Date: 2002/10/29 08:06:44 $
 *  $Source: /root/leakn64/depot/rf/sw/n64os20l/libultra/shared/motor/motor.c,v $
 *
 **************************************************************************/

#include "osint.h"
#include "controller.h"
#include "siint.h"

#define	M_ADDR		(0xc000/BLOCKSIZE)
#define BANK_ADDR	(0x8000/BLOCKSIZE)
#define	MOTOR_ID	0x80
#define	CHECK_ID	0xfe

s32 __osMotorAccess(OSPfs *, s32);

static OSPifRam        __MotorDataBuf[MAXCONTROLLERS];

#if	0
s32
osMotorStop(OSPfs *pfs)
{
  return (__osMotorAccess(pfs, 0)); 
}

s32
osMotorStart(OSPfs *pfs)
{
  return (__osMotorAccess(pfs, 1)); 
}
#endif

s32
__osMotorAccess(OSPfs *pfs, s32	flag)
{
  int	i;
  s32	ret;
  u8 			*ptr = (u8 *)(&__MotorDataBuf[pfs->channel]);

  if (!(pfs->status & PFS_MOTOR_INITIALIZED))
    return(PFS_ERR_INVALID);

  /* Block to get resource token */
  __osSiGetAccess();

  /* Set up request command format for all channels */

  __MotorDataBuf[pfs->channel].pifstatus = CONT_FORMAT;
  ptr += pfs->channel;

  for (i = 0; i < BLOCKSIZE ; i++)
    ((__OSContRamReadFormat *)ptr)->data[i] = (u8)flag;

  __osContLastCmd = CONT_ETC;

  __osSiRawStartDma(OS_WRITE, &__MotorDataBuf[pfs->channel]);
  (void)osRecvMesg(pfs->queue, (OSMesg *)NULL, OS_MESG_BLOCK);

  /* trigger pifmacro */
  
  ret = __osSiRawStartDma(OS_READ, &__MotorDataBuf[pfs->channel]);
  (void)osRecvMesg(pfs->queue, (OSMesg *)NULL, OS_MESG_BLOCK);

  /* check data CRC */

  ret = (s32)(((__OSContRamReadFormat *)ptr)->rxsize & CON_ERR_MASK);
  if (ret == 0) {
    if (flag == 0) {
      if (((__OSContRamReadFormat *)ptr)->datacrc != 0) {
	ret = PFS_ERR_CONTRFAIL;
      }
    } else {
      if (((__OSContRamReadFormat *)ptr)->datacrc != 0xeb) {
	ret = PFS_ERR_CONTRFAIL;
      }
    }
  }

  __osSiRelAccess();
  return (ret); 
}

static void
__osMakeMotorData(int channel, OSPifRam *mdata)
{
	u8 			*ptr = (u8 *)(mdata);
	__OSContRamReadFormat 	ramreadformat;
	int 			i;

	/* Setup ram read format */

	ramreadformat.dummy = 0xff;
        ramreadformat.txsize = 35;
        ramreadformat.rxsize = 1;
        ramreadformat.cmd = CONT_RAM_WRITE;
        ramreadformat.addrh = (u8)(M_ADDR >>3);
	ramreadformat.addrl = 
	  (u8)( (M_ADDR << 5) | __osContAddressCrc(M_ADDR)) ;

        /* setup ram read format */

	if (channel)
		for (i = 0; i < channel; i++)
			*ptr++ = 0;

        *((__OSContRamReadFormat *)ptr) = ramreadformat;

	ptr += sizeof(ramreadformat);
	*((u8 *)ptr) = FORMAT_END;
}

s32
osMotorInit(OSMesgQueue *mq, OSPfs *pfs, int channel)
{
	s32	ret;
	u8	temp[BLOCKSIZE];

	pfs->queue = mq;
	pfs->channel = channel;
	pfs->activebank = 255;		/* Dummy */
	pfs->status = 0;

	/* Change bank of Dummy ID */
	ret = __osPfsSelectBank(pfs, CHECK_ID);
	if (ret == PFS_ERR_NEW_PACK) ret = __osPfsSelectBank(pfs, MOTOR_ID);
	if (ret != 0) return(ret);

	/* Read bank of Dummy ID */
	ret =  __osContRamRead(mq, channel, BANK_ADDR, temp);
	if (ret == PFS_ERR_NEW_PACK) ret =  PFS_ERR_CONTRFAIL;
	if (ret != 0) {
	  return(ret);
	} else if (temp[BLOCKSIZE-1] == CHECK_ID) {
	  return(PFS_ERR_DEVICE);
	}
	
	/* Change bank of RUMBLE PAK ID */
	ret = __osPfsSelectBank(pfs, MOTOR_ID);
	if (ret == PFS_ERR_NEW_PACK) ret =  PFS_ERR_CONTRFAIL;
	if (ret != 0) return(ret);

	/* Read bank of RUMBLE PAK ID */
	ret =  __osContRamRead(mq, channel, BANK_ADDR, temp);
	if (ret == PFS_ERR_NEW_PACK) {
	  ret =  PFS_ERR_CONTRFAIL;
	}
	if (ret != 0) {
	  return(ret);
	} else if (temp[BLOCKSIZE-1] != MOTOR_ID) {
	  return(PFS_ERR_DEVICE);
	}

	if (!(pfs->status & PFS_MOTOR_INITIALIZED)) {
	  __osMakeMotorData(channel, &__MotorDataBuf[channel]);
	}

	pfs->status = PFS_MOTOR_INITIALIZED;
	
	return(0);
}