#include "AT91SAM7S64.h"

#include "SAMtwi.h"


void TWIinit(void)
{
  // clock
  AT91C_BASE_PMC->PMC_PCER  = (1<<AT91C_ID_TWI); // TWI
  // pin
  AT91C_BASE_PIOA->PIO_PDR  = (1<<4)|(1<<3);
  AT91C_BASE_PIOA->PIO_ASR  = (1<<4)|(1<<3);    // PA4:TWCK , PA3:TWD
  AT91C_BASE_PIOA->PIO_MDER = (1<<4)|(1<<3);    // open drain
  AT91C_BASE_PIOA->PIO_PPUDR= (1<<4)|(1<<3);    // pull-up R
  
  AT91C_BASE_TWI->TWI_CR=AT91C_TWI_SWRST;        // SW reset
  AT91C_BASE_TWI->TWI_CR=AT91C_TWI_MSEN;         // TWI enable
  AT91C_BASE_TWI->TWI_CWGR=(1<<16)|(1497<<8)|(1497);  // about 8kHz @ 48MHz
 
  AT91C_BASE_TWI->TWI_IDR=0xFFFFFFFF;
}

void TWIsend(unsigned char mode,unsigned short addr,unsigned char *dat,unsigned char size)
{
 unsigned int s=0;

 if(!size) return;  // size = 0 
 
 //AT91C_BASE_TWI->TWI_CR=0x04;  // TWI enable

 if((mode&0x01)==TWI_1BYTE_SUBADDR)
    {
	 AT91C_BASE_TWI->TWI_MMR=(mode<<15)|(1<<8);
	 AT91C_BASE_TWI->TWI_IADR=addr&0x00FF;
	}						
 else
	{						
	 AT91C_BASE_TWI->TWI_MMR=((mode&0xFE)<<15)|(2<<8);
	 AT91C_BASE_TWI->TWI_IADR=addr;
    }
/*
  s=AT91C_BASE_TWI->TWI_SR; 
  AT91C_BASE_TWI->TWI_THR=*(dat++);
  AT91C_BASE_TWI->TWI_CR=AT91C_TWI_START;
  while(size-->1)
    {
	 while(!(s&AT91C_TWI_TXRDY)) s=AT91C_BASE_TWI->TWI_SR; 
	 AT91C_BASE_TWI->TWI_THR=*(dat++);
	}

   AT91C_BASE_TWI->TWI_CR=AT91C_TWI_STOP; // TWI STOP
   s=AT91C_BASE_TWI->TWI_SR;
   while(!(s&AT91C_TWI_TXCOMP))  s=AT91C_BASE_TWI->TWI_SR; 
*/   

 // in single byte... (datasheet page285)
 if(1==size) 
     {
	  AT91C_BASE_TWI->TWI_CR=AT91C_TWI_START | AT91C_TWI_STOP | AT91C_TWI_MSEN;
	  AT91C_BASE_TWI->TWI_THR=*dat;
	 }
 // multiple bytes	 
 else 
     {
      s=AT91C_BASE_TWI->TWI_SR;   
      AT91C_BASE_TWI->TWI_THR=*(dat++);
      AT91C_BASE_TWI->TWI_CR=1; // TWI START
 
      while((size--)>1)
        {
		 AT91C_BASE_TWI->TWI_CR=AT91C_TWI_START | AT91C_TWI_MSEN;
         s=AT91C_BASE_TWI->TWI_SR;
         while(!(s&AT91C_TWI_TXRDY)) s=AT91C_BASE_TWI->TWI_SR;
        AT91C_BASE_TWI->TWI_THR=*(dat++);  
        }      
 // last byte
      AT91C_BASE_TWI->TWI_CR=AT91C_TWI_STOP; // TWI STOP
         s=AT91C_BASE_TWI->TWI_SR;
         while(!(s&AT91C_TWI_TXRDY)) s=AT91C_BASE_TWI->TWI_SR;
        AT91C_BASE_TWI->TWI_THR=*dat;  
    }
	
 s=AT91C_BASE_TWI->TWI_SR;
 while(!(s&AT91C_TWI_TXCOMP))  s=AT91C_BASE_TWI->TWI_SR; 

  //AT91C_BASE_TWI->TWI_CR=0x08;  // TWI disable
}

void TWIread(unsigned char mode,unsigned short addr,unsigned char *dat,unsigned char size)
{
 unsigned int s=0;
 
 if(!size) return;

 //AT91C_BASE_TWI->TWI_CR=0x04;  // TWI enable

 if((mode&0x01)==TWI_1BYTE_SUBADDR)
    {
	 AT91C_BASE_TWI->TWI_MMR=(mode<<15)|(1<<12)|(1<<8);
	 AT91C_BASE_TWI->TWI_IADR=addr&0x00FF;
	}						
 else
	{						
	 AT91C_BASE_TWI->TWI_MMR=((mode&0xFE)<<15)|(1<<12)|(2<<8);
	 AT91C_BASE_TWI->TWI_IADR=addr;
    }
 /*
 AT91C_BASE_TWI->TWI_CR=AT91C_TWI_START;
 s=AT91C_BASE_TWI->TWI_SR;
   while(size-->1)
     {
	  while(!(s&AT91C_TWI_RXRDY)) s=AT91C_BASE_TWI->TWI_SR;
	  *(dat++)=AT91C_BASE_TWI->TWI_RHR;
	  *(dat)=s;
	 }

 AT91C_BASE_TWI->TWI_CR=AT91C_TWI_STOP;
 s=AT91C_BASE_TWI->TWI_SR;
 while(!(s&AT91C_TWI_RXRDY)) s=AT91C_BASE_TWI->TWI_SR;
 *dat=AT91C_BASE_TWI->TWI_RHR;
 */

	
 // single byte	
 if(1==size)
  	{
     AT91C_BASE_TWI->TWI_CR=AT91C_TWI_START | AT91C_TWI_STOP;   
      s=AT91C_BASE_TWI->TWI_SR;
      while(!(s&AT91C_TWI_TXCOMP))  s=AT91C_BASE_TWI->TWI_SR;
        *dat= AT91C_BASE_TWI->TWI_RHR;
    }
 // multiple bytes
 else
   {
     AT91C_BASE_TWI->TWI_CR=AT91C_TWI_START | AT91C_TWI_MSEN;
	 
        s=AT91C_BASE_TWI->TWI_SR;
        while(!(s&AT91C_TWI_TXCOMP))  // wait for stop 
         {
          s=AT91C_BASE_TWI->TWI_SR;	 
		    if(s&AT91C_TWI_RXRDY) 
			    {
				 *(dat++)=AT91C_BASE_TWI->TWI_RHR;
				 size--;
				 // last byte
				 if(1==size) AT91C_BASE_TWI->TWI_CR=AT91C_TWI_STOP;   
				}
		 } 
    }

  //AT91C_BASE_TWI->TWI_CR=0x08;  // TWI disable 
}