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;
}

//==============================================================================
//  Функция             : AT91F_SetTwiClock
//  Описание            :
//  входные параметры   :
//  выходные параметры  :
//==============================================================================
void AT91F_SetTwiClock(void)
{
  int sclock;

  /* Here, CKDIV = 1 and CHDIV=CLDIV  ==> CLDIV = CHDIV = 1/4*((Fmclk/FTWI) -6)*/

  sclock = (10*MCK /AT91C_TWI_CLOCK);
  if (sclock % 10 >= 5)
      sclock = (sclock /10) - 5;
  else
      sclock = (sclock /10)- 6;
  sclock = (sclock + (4 - sclock %4)) >> 2;    // div 4

  AT91C_BASE_TWI->TWI_CWGR    = ( 1<<16 ) | (sclock << 8) | sclock;
}
//==============================================================================
//  Функция             : AT91F_TWI_Open
//  Описание            :
//  входные параметры   :
//  выходные параметры  :
//==============================================================================
void AT91F_TWI_Open(void)
{
  AT91F_TWI_CfgPIO();                     // configure TWI PIOs
  AT91F_TWI_CfgPMC();                     // enable clock TWI
  AT91F_TWI_Configure (AT91C_BASE_TWI);   // configure in MASTER mode
  AT91F_SetTwiClock();
}
//==============================================================================
//  Функция             : AT91F_TWI_WriteByte
//  Описание            :
//  входные параметры   :
//  выходные параметры  :
//==============================================================================
int AT91F_TWI_WriteByte(const AT91PS_TWI pTwi ,int mode, int int_address, char *data2send, int nb)
{
  unsigned int status,counter=0,error=0;

  // Set TWI Internal Address Register
  if ((mode & AT91C_TWI_IADRSZ) != 0) pTwi->TWI_IADR = int_address;

  // Set the TWI Master Mode Register
  pTwi->TWI_MMR = mode & ~AT91C_TWI_MREAD;
  if(nb <2)
  {
    pTwi->TWI_CR = AT91C_TWI_START | AT91C_TWI_MSEN | AT91C_TWI_STOP;
    pTwi->TWI_THR = *data2send;
  }
  else
  {
    // Set the TWI Master Mode Register
    for(counter=0;counter<nb;counter++)
    {
      pTwi->TWI_CR = AT91C_TWI_START | AT91C_TWI_MSEN;
      if (counter == (nb - 1)) pTwi->TWI_CR = AT91C_TWI_STOP;
      status = pTwi->TWI_SR;
      if ((status & ERROR) == ERROR) error++;
      while (!(status & AT91C_TWI_TXRDY))
      {
        status = pTwi->TWI_SR;
        if ((status & ERROR) == ERROR) error++;
      }
      pTwi->TWI_THR = *(data2send+counter);
   }
  }
  status = pTwi->TWI_SR;
  if ((status & ERROR) == ERROR) error++;
  while (!(status & AT91C_TWI_TXCOMP))
  {
    status = pTwi->TWI_SR;
    if ((status & ERROR) == ERROR) error++;
  }
  return error;
}
//==============================================================================
//  Функция             : AT91F_TWI_ReadByte
//  Описание            :
//  входные параметры   :
//  выходные параметры  :
//==============================================================================
int AT91F_TWI_ReadByte(const AT91PS_TWI pTwi ,int mode, int int_address, char *data, int nb)
{
  unsigned int status,counter=0,error=0;

  // Set TWI Internal Address Register
  if ((mode & AT91C_TWI_IADRSZ) != 0) pTwi->TWI_IADR = int_address;

  // Set the TWI Master Mode Register
  pTwi->TWI_MMR = mode | AT91C_TWI_MREAD;

  // Start transfer
  if (nb == 1)
  {
    pTwi->TWI_CR = AT91C_TWI_START | AT91C_TWI_STOP;
    status = pTwi->TWI_SR;
    if ((status & ERROR) == ERROR) error++;
    while (!(status & AT91C_TWI_TXCOMP))
    {
      status = pTwi->TWI_SR;
      if ((status & ERROR) == ERROR) error++;
    }
    *(data) = pTwi->TWI_RHR;
   }
   else
   {
     pTwi->TWI_CR = AT91C_TWI_START | AT91C_TWI_MSEN;
     status = pTwi->TWI_SR;
     if ((status & ERROR) == ERROR) error++;

     // Wait transfer is finished
     while (!(status & AT91C_TWI_TXCOMP))
     {
       status = pTwi->TWI_SR;
       if ((status & ERROR )== ERROR) error++;
        if(status & AT91C_TWI_RXRDY)
        {
      *(data+counter++) = pTwi->TWI_RHR;
      if (counter == (nb - 1)) pTwi->TWI_CR = AT91C_TWI_STOP;
    }
     }
   }
   return 0;
}
//==============================================================================
//  Функция             :
//  Описание            :
//  входные параметры   :
//  выходные параметры  :
//==============================================================================
void TWI_ReadByte(const AT91PS_TWI pTwi ,int mode, char *data, int nb)
{
  unsigned int status,counter=0;
  // Set TWI Internal Address Register
  // without internal address ??????
  //if ((mode & AT91C_TWI_IADRSZ) != 0) pTwi->TWI_IADR = int_address;

  // Set the TWI Master Mode Register
  // Use MS 7 bits as mode ??????
  pTwi->TWI_MMR = mode | AT91C_TWI_MREAD;

  // Start transfer
  if (nb == 1)
  {
    pTwi->TWI_CR = AT91C_TWI_START | AT91C_TWI_STOP;
    status = pTwi->TWI_SR;
    //if ((status & ERROR) == ERROR) error++;
    while (!(status & AT91C_TWI_TXCOMP));
    *(data) = pTwi->TWI_RHR;
  }
  else
  {
    pTwi->TWI_CR = AT91C_TWI_START | AT91C_TWI_MSEN;
    status = pTwi->TWI_SR;
    // Wait transfer is finished
    while (!(status & AT91C_TWI_TXCOMP))
    {
      AT91F_DBGU_Printk("\n\rTEST_AT91F_TWI_ReadByte_1");
      if(status & AT91C_TWI_RXRDY)
      {
         *(data+counter++) = pTwi->TWI_RHR;
         AT91F_DBGU_Printk("\n\rTEST_AT91F_TWI_ReadByte_2");
         if (counter == (nb - 1)) pTwi->TWI_CR = AT91C_TWI_STOP;
         //AT91F_DBGU_Printk("\n\rTEST_AT91F_TWI_ReadByte_3");
      }
    }
  }
}