__inline void AT91F_TWI_myConfigure(AT91PS_TWI pTWI)
{
	AT91C_BASE_AIC->AIC_IDCR = (1<<AT91C_ID_TWI);
	
	AT91C_BASE_PIOA->PIO_PDR = (1<<3) | (1<<4);
	AT91C_BASE_PIOA->PIO_MDER = (1<<3) | (1<<4);
	
	AT91C_BASE_PMC->PMC_PCER = (1<<AT91C_ID_TWI);

    //* Disable interrupts
	pTWI->TWI_IDR = (unsigned int) -1;

	//* Set Clock	s. Datenblatt S. 287
	pTWI->TWI_CWGR = (2 << 16) | (50 << 8) | (50 << 0);

    //* Reset peripheral
	pTWI->TWI_CR = AT91C_TWI_SWRST;
	
	//* Set Master mode
	pTWI->TWI_CR = AT91C_TWI_MSEN;
}

__inline void AT91F_TWI_SetDeviceAddress(AT91PS_TWI pTWI,char devaddr)
{
	pTWI->TWI_MMR = ((devaddr & 127) << 16) | (1 << 8);
}

__inline void AT91F_TWI_SetReadMode(AT91PS_TWI pTWI)
{
	pTWI->TWI_MMR |= (1 << 12);
}

__inline void AT91F_TWI_SetWriteMode(AT91PS_TWI pTWI)
{
	pTWI->TWI_MMR |= (1 << 12);
	pTWI->TWI_MMR ^= (1 << 12);
}

__inline void AT91F_TWI_WriteData(AT91PS_TWI pTWI,char iaddr,char data)
{
	pTWI->TWI_IADR = (int) iaddr;
	while((pTWI->TWI_SR & (1 << 2))==0);	//* Warten bis Transferregister bereit
	pTWI->TWI_THR = (int) data;
	pTWI->TWI_CR = (1 << 0);				//* Startbefehl senden
	while((pTWI->TWI_SR & (1 << 2))==0);	//* Warten bis Transferregister leer
	pTWI->TWI_CR = (1 << 1);				//* Stopbefehl senden
	while((pTWI->TWI_SR & (1 << 0))==0);	//* Warten bis Transfer abgeschlossen
}

__inline int AT91F_TWI_ReadData(AT91PS_TWI pTWI,char iaddr)
{
	pTWI->TWI_IADR = iaddr;
	pTWI->TWI_CR = (1 << 0);				//* Startbefehl senden
	while((pTWI->TWI_SR & (1 << 1))==0);	//* Warten bis Daten eingegangen sind
	pTWI->TWI_CR = (1 << 1);				//* Stopbefehl senden
	while((pTWI->TWI_SR & (1 << 0))==0);	//* Warten bis Transfer abgeschlossen
	return pTWI->TWI_RHR;
}