void TWI_ConfigureMaster(AT91S_TWI *pTwi, unsigned int twck, unsigned int mck)
{
unsigned int ckdiv = 0;
unsigned int cldiv;
unsigned char ok = 0;

SANITY_CHECK(pTwi);

// Reset the TWI
pTwi->TWI_CR = AT91C_TWI_SWRST;
pTwi->TWI_RHR;

pTwi->TWI_CR = AT91C_TWI_MSDIS;

pTwi->TWI_CR = AT91C_TWI_MSEN;

// Configure clock
while (!ok) {

cldiv = ((mck / (2 * twck)) - 3) / power(2, ckdiv);

if (cldiv <= 255) {

ok = 1;
}
else {

ckdiv++;
}
}

pTwi->TWI_CWGR = 0;
pTwi->TWI_CWGR = (ckdiv << 16) | (cldiv << 8) | cldiv;
}

void init_TWI(void)
{
PIO_Configure(TWIpins,1);
PMC_EnablePeripheral(AT91C_ID_TWI);
*AT91C_TWI_IDR = 0xFFFFF;
TWI_ConfigureMaster(AT91C_BASE_TWI, Clock, BOARD_MCK);
AIC_DisableIT(AT91C_ID_TWI);
}

void TWI_StartRead(
AT91S_TWI *pTwi,
unsigned char address,
unsigned int iaddress,
unsigned char isize)
{
SANITY_CHECK(pTwi);
SANITY_CHECK((address & 0x80) == 0);
SANITY_CHECK((iaddress & 0xFF000000) == 0);
SANITY_CHECK(isize < 4);

// Set slave address and number of internal address bytes
pTwi->TWI_MMR = 0;
pTwi->TWI_MMR = (isize << 8) | AT91C_TWI_MREAD | (address << 16);

// Set internal address bytes
pTwi->TWI_IADR = 0;
pTwi->TWI_IADR = iaddress;

// Send START condition
pTwi->TWI_CR = AT91C_TWI_START;
}