int AT91F_TWI_WriteSingle(const AT91PS_TWI pTwi,
                        int SlaveAddr,
                        char *data)
{
    unsigned int end = 0, status, err=0;

    /* Enable Master Mode */
  //  AT91C_BASE_TWI->TWI_CR = AT91C_TWI_MSEN;

    /* Set the TWI Master Mode Register */
    AT91C_BASE_TWI->TWI_MMR =  (SlaveAddr<<16 ) & ~AT91C_TWI_MREAD;

    /* Write the data to send into THR. Start conditionn DADDR and R/W bit
       are sent automatically */
    AT91C_BASE_TWI->TWI_THR = *data;
    
    AT91C_BASE_TWI->TWI_CR = AT91C_TWI_START;

    /* NACK errata handling */
    /* Do not poll the TWI_SR */
    /* Wait 3 x 9 TWCK pulse (max) 2 if IADRR not used, before reading TWI_SR */
    /* From 400Khz down to 1Khz, the time to wait will be in µs range.*/
    /* In this example the TWI period is 1/400KHz */
   AT91F_TWI_WaitMicroSecond (40);

    while (!end)
    {
      status = AT91C_BASE_TWI->TWI_SR;
      if ((status & AT91C_TWI_NACK) == AT91C_TWI_NACK)
      {
        err++;
        end=1;
      }
    /*  Wait for the Transmit ready is set */
      if ((status & AT91C_TWI_TXRDY) == AT91C_TWI_TXRDY)
        end=1;
    }
    AT91C_BASE_TWI->TWI_CR = AT91C_TWI_STOP;
    /* Wait for the Transmit complete is set */
    status = AT91C_BASE_TWI->TWI_SR;
    while (!(status & AT91C_TWI_TXCOMP))
      status = AT91C_BASE_TWI->TWI_SR;

    return err;
}

void AT91F_TWI_Open(int TwiClock)
{
    /* Configure TWI PIOs */
    AT91F_TWI_CfgPIO ();

    /* Configure PMC by enabling TWI clock */
    AT91F_TWI_CfgPMC ();

    /* Reset the TWI */
    AT91C_BASE_TWI->TWI_CR = AT91C_TWI_SWRST;

    
    /* Configure TWI in master mode */
    AT91F_TWI_Configure (AT91C_BASE_TWI);


    /* Set TWI Clock Waveform Generator Register */
  //AT91C_BASE_TWI->TWI_CWGR=0x047575;
    AT91F_SetTwiClock(TwiClock);
}

void AT91F_SetTwiClock(int TwiClock)
{
  unsigned int cldiv,ckdiv=1;

  /* CLDIV = ((Tlow x 2^CKDIV) -3) x Tmck */
  /* CHDIV = ((THigh x 2^CKDIV) -3) x Tmck */
  /* Only CLDIV is computed since CLDIV = CHDIV (50% duty cycle) */
          cldiv = ( (MCK/(2*TwiClock))-3 ) / pow(2,ckdiv);
  while (  cldiv> 255 )
  {
   ckdiv++;
   cldiv = ( (MCK/(2*TwiClock))-3 ) / pow(2,ckdiv);

  }
  AT91C_BASE_TWI->TWI_CWGR =(ckdiv<<16)|((unsigned int)cldiv << 8)|(unsigned int)cldiv;
}