/****************************************************************************/
/* Summary  : I2C library for Invensense MotionApps on Persistor CF2	    */
/* Filename : twim.c							    */
/* Author   : Robert Herlien (rah)					    */
/* Project  : BEDS (Benthic Event Detection System)			    */
/* $Revision$			*/
/* Created  : 04/30/2012						    */
/* Note     : Named twim.c to emulate the AVR TWIM module		    */
/****************************************************************************/
/* Modification History:						    */
/* 30apr2012 rah - created						    */
/****************************************************************************/

#include <mbariTypes.h>			/* MBARI type definitions	    */
#include <mbariConst.h>			/* MBARI constants		    */
#include <cfxpico.h>			/* Persistor PicoDOS definitions    */
#include <stdio.h>
#include <stdlib.h>
#include <twim.h>
#include <beds.h>			/* BEDS controller definitions	    */
#include <io.h>				/* BEDS I/O definitions		    */
#include <syslog.h>			/* System logger		    */

#define PORTQS		(QSM_BASE_ADDR + 0x15)
#define PQSPAR		(QSM_BASE_ADDR + 0x16)
#define DDRQS		(QSM_BASE_ADDR + 0x17)

/* Note these definitions are here for using /PCS0 and /PCS1 for SCL and SDA*/
/* But we mostly use assembly language to tweak the same bits, so you must */
/* make sure that the constants in the assembly routines match these pins  */
#define SCL_PIN		21
#define SDA_PIN		19


/************************************************************************/
/* Function    : twimInit						*/
/* Purpose     : Initialize this module					*/
/* Inputs      : None							*/
/* Outputs     : None							*/
/************************************************************************/
void twimInit(void)
{
    execstr("SET SYS.QPBCS=32");
    PIOSet(SCL_PIN);
    PIOSet(SDA_PIN);

} /* twimInit() */


/************************************************************************/
/* Function    : twimSetSafe						*/
/* Purpose     : Set safe starting condition for I2C module		*/
/* Inputs      : None							*/
/* Outputs     : None							*/
/************************************************************************/
void twimSetSafe(void)
{
    PIOSet(SCL_PIN);
    PIOSet(SDA_PIN);

} /* twimSetSafe() */


/************************************************************************/
/* Function    : twimPwrOffState					*/
/* Purpose     : Set I2C pins to correct state when module is powered off*/
/* Inputs      : None							*/
/* Outputs     : None							*/
/* Comment     : When we power off the MPU module, we need the I2C lines*/
/*		 low, or we end up powering chip from the I2C lines	*/
/************************************************************************/
void twimPwrOffState(void)
{
    PIOClear(SCL_PIN);
    PIOClear(SDA_PIN);

} /* twimPwrOffState() */


/************************************************************************/
/* Function    : sendStartBit						*/
/* Purpose     : Send start bit						*/
/* Inputs      : None							*/
/* Outputs     : None							*/
/************************************************************************/
asm static void sendStartBit(void)
{
    andi.b	#0xe7, PQSPAR		//Set SCL, SDA as I/O
    ori.b	#0x18, PORTQS		//Output 1's on SCL, SDA
    ori.b	#0x18, DDRQS		//Set SCL, SDA as outputs
    andi.b	#0xef, PORTQS		//Neg edge on SDA
    andi.b	#0xf7, PORTQS		//Neg edge on SCL
    ori.b	#0x10, PORTQS		//Leave SDA high
    rts

} /* sendStartBit() */


/************************************************************************/
/* Function    : sendStopBit						*/
/* Purpose     : Send stop bit						*/
/* Inputs      : None							*/
/* Outputs     : None							*/
/************************************************************************/
asm static void sendStopBit(void)
{
    andi.b	#0xef, PORTQS		//SDA low
    ori.b	#0x08, PORTQS		//SCL high
    ori.b	#0x10, PORTQS		//SDA high
    rts

} /* sendStopBit() */


/************************************************************************/
/* Function    : writeOneByte						*/
/* Purpose     : Write one byte to the MPU6050				*/
/* Inputs      : Byte to send						*/
/* Outputs     : TRUE if ACKed OK, else FALSE				*/
/* Comments    : d0 = return code (ackOK)				*/
/* 		 d1 = byte to send					*/
/*		 d2 = loop counter					*/
/*		 a0 = ptr to PORTQS					*/
/************************************************************************/
asm static bool writeOneByte(ushort byte:__d1)
{
    move.l	d2, -(sp)
    move.l	a0, -(sp)

    moveq	#-1, d0			//Init return to TRUE
    moveq	#7, d2			//Loop count (must be 1 less than rqd)
    lea		PORTQS, a0		//Point a0 to PORTQS
writeLoop:
    lsl.b	#1, d1
    bcc		write0
    ori.b	#0x10, (a0)		//If data bit 1, set SDA
    bra		write1
write0:
    andi.b	#0xef, (a0)		//else clear SDA
write1:
    ori.b	#0x8, (a0)		//SCL high
    nop
    andi.b	#0xf7, (a0)		//SCL low
    dbra	d2, writeLoop

    andi.b	#0xef, (a0)		//SDA low
    ori.b	#0x08, (a0)		//SCL high
    andi.b	#0xef, DDRQS		//Set SDA pin as input
    btst	#4, (a0)		//Read SDA, check for ACK
    beq		wackOK			//0 bit on SDA indicates ACK
    clr.l	d0			//If no ACK, set rtn code to FALSE
wackOK:
    andi.b	#0xf7, (a0)		//SCL low
    ori.b	#0x10, (a0)		//SDA high
    ori.b	#0x18, DDRQS		//SDA is output again

    move.l	(sp)+, a0
    move.l	(sp)+, d2
    rts

} /* writeOneByte() */


/************************************************************************/
/* Function    : readOneByte						*/
/* Purpose     : Read one byte from the MPU6050				*/
/* Inputs      : None							*/
/* Outputs     : Byte read						*/
/* Comments    : d0 = byte read						*/
/*		 d3 = loop counter					*/
/*		 a0 = ptr to PORTQS					*/
/************************************************************************/
asm static uint16_t readOneByte(void)
{
    move.l	d3, -(sp)
    move.l	a0, -(sp)
    moveq	#0, d0			//Init byte read to 0
    moveq	#7, d3			//Loop count (must be 1 less than rqd)
    lea		PORTQS, a0		//Point a0 to PORTQS
    andi.b	#0xe7, (a0)		//SCL, SDA low
    andi.b	#0xef, DDRQS		//Set SDA as input
readLoop:
    lsl.b	#1, d0			//Shift byte read
    ori.b	#0x8, (a0)		//SCL high
    btst	#4, (a0)		//Read SDA
    beq		read0
    ori.b	#1, d0			//If SDA high, set lsb of byte read
read0:
    andi.b	#0xf7, (a0)		//SCL low
    dbra	d3, readLoop

    ori.b	#0x10, (a0)		//SDA high
    ori.b	#0x10, DDRQS		//Set SDA back as output
    move.l	(sp)+, a0
    move.l	(sp)+, d3
    rts

} /* readOneByte() */


/************************************************************************/
/* Function    : sendAck						*/
/* Purpose     : Send ACK bit						*/
/* Inputs      : None							*/
/* Outputs     : None							*/
/************************************************************************/
asm static void sendAck(void)
{
    andi.b	#0xe7, PORTQS		//SCL, SDA low
    ori.b	#0x08, PORTQS		//SCL high
    andi.b	#0xf7, PORTQS		//SCL low
    ori.b	#0x10, PORTQS		//SDA high
    rts

} /* sendAck() */


/************************************************************************/
/* Function    : sendNack						*/
/* Purpose     : Send NACK bit						*/
/* Inputs      : None							*/
/* Outputs     : None							*/
/************************************************************************/
asm static void sendNack(void)
{
    ori.b	#0x10, PORTQS		//SDA high
    andi.b	#0xf7, PORTQS		//SCL low
    ori.b	#0x08, PORTQS		//SCL high
    andi.b	#0xf7, PORTQS		//SCL low
    rts

} /* sendNack() */


/************************************************************************/
/* Function    : earlyAbort						*/
/* Purpose     : Error return						*/
/* Inputs      : None							*/
/* Outputs     : None							*/
/************************************************************************/
static int16_t earlyAbort(int16_t errCode)
{
    sendStopBit();
    return(errCode);

} /* earlyAbort() */


/************************************************************************/
/* Function    : twim_write						*/
/* Purpose     : Write register data via I2C				*/
/* Inputs      : Device addr, register addr, data ptr, count		*/
/* Outputs     : Status return code					*/
/************************************************************************/
int16_t twim_write(uint16_t dev, uint8_t reg, const Byte *datap, size_t count)
{
    uint16_t	i, sr;
    bool	rtn;
	
    sr = IEVSaveSRThenDisable();
#ifdef DEBUG_I2C
    PIOSet(I2C_IO);
#endif
    sendStartBit();
    rtn = writeOneByte((dev<<1) & 0xfe);
    IEVRestoreSavedSR(sr);
    if (!rtn)
	return(earlyAbort(DEVADDR_ERR));

    sr = IEVSaveSRThenDisable();
    rtn = writeOneByte(reg);
    IEVRestoreSavedSR(sr);
    if (!rtn)
	return(earlyAbort(REGADDR_ERR));

    for (i = 0; i < count; i++)
    {
	// Create interrupt windows between bytes
	sr = IEVSaveSRThenDisable();
	rtn = writeOneByte(datap[i]);
	IEVRestoreSavedSR(sr);
	if (!rtn)
	    return(earlyAbort(DATA_ERR));
    }

    sr = IEVSaveSRThenDisable();
    sendStopBit();
#ifdef DEBUG_I2C
    PIOClear(I2C_IO);
#endif
    IEVRestoreSavedSR(sr);
//    sysLogPrintf("twim_write reg %#x count %d value %#x",
//		 (unsigned int)reg, count, (unsigned int)(*datap));
    return(STATUS_OK);

} /* twim_write() */


/************************************************************************/
/* Function    : twim_read						*/
/* Purpose     : Read register data via I2C				*/
/* Inputs      : Device addr, register addr, data ptr, count		*/
/* Outputs     : STATUS_OK or STATUS_ERR				*/
/************************************************************************/
int16_t twim_read(uint16_t dev, uint8_t reg, Byte *datap, size_t count)
{
    uint16_t	i, sr;
    bool	rtn;

    sr = IEVSaveSRThenDisable();
#ifdef DEBUG_I2C
    PIOSet(I2C_IO);
#endif
    sendStartBit();
    rtn = writeOneByte((dev<<1) & 0xfe);
    IEVRestoreSavedSR(sr);
    if (!rtn)
	return(earlyAbort(DEVADDR_ERR));

    sr = IEVSaveSRThenDisable();
    rtn = writeOneByte(reg);
    IEVRestoreSavedSR(sr);
    if (!rtn)
	return(earlyAbort(REGADDR_ERR));

    sr = IEVSaveSRThenDisable();
    sendStartBit();
    rtn = writeOneByte((dev<<1) | 1);
    IEVRestoreSavedSR(sr);
    if (!rtn)
	return(earlyAbort(DEV2ADDR_ERR));

    for (i = 0; i < count-1; i++)
    {
	sr = IEVSaveSRThenDisable();
	datap[i] = (Byte)readOneByte();
	sendAck();
	IEVRestoreSavedSR(sr);
    }

    sr = IEVSaveSRThenDisable();
    datap[count-1] = (Byte)readOneByte();
    sendNack();
    sendStopBit();
#ifdef DEBUG_I2C
    PIOClear(I2C_IO);
#endif
    IEVRestoreSavedSR(sr);
//    sysLogPrintf("twim_read reg %#x count %d value %#x",
//		 (unsigned int)reg, count, (unsigned int)(*datap));
    return(STATUS_OK);

} /* twim_read() */