diff --git a/src/at91_twi.c b/src/at91_twi.c
index 03a8cf1..aa053be 100644
--- a/src/at91_twi.c
+++ b/src/at91_twi.c
@@ -34,7 +34,7 @@
 /*
  * while ((cldiv = ((MCK / (2 * TWI)) -3) / (1 << ckdiv)) > 255)
  *   	ckdiv++ ;
- * 
+ *
  * works for TWI >= 100kHz
  */
 #define TWI_CLK(x) (((MCK / (2 * (x))) -3)<<8 | ((MCK / (2 * (x))) -3))
@@ -56,34 +56,34 @@ static void twi_isr(void)
 	/* get status */
 	uint32_t status = *AT91C_TWI_SR;
 	status &= *AT91C_TWI_IMR;
-	
+
 	/* NACK - disable all interrupts and go to state TWI_ERROR */
 	if (status & AT91C_TWI_NACK) {
 		*AT91C_TWI_IDR = AT91C_TWI_TXCOMP | AT91C_TWI_RXRDY | AT91C_TWI_TXRDY | AT91C_TWI_NACK;
 		twi_state = TWI_ERROR;
-		
+
 		// TODO: TWI_BLMC_UPDATE?
 		return;
 	}
-	
+
 	/* tx register ready for new data */
 	if (status & AT91C_TWI_TXRDY) {
 		if (twi_count != twi_size) {
 			/* feed next byte */
 			*AT91C_TWI_THR = twi_data[twi_count++];
-			
+
 		} else {
 			/* wait for TXCOMP */
 			*AT91C_TWI_IDR = AT91C_TWI_RXRDY | AT91C_TWI_TXRDY;
 			*AT91C_TWI_IER = AT91C_TWI_TXCOMP | AT91C_TWI_NACK;
 		}
 	}
-	
+
 	/* rx register has data */
 	if (status & AT91C_TWI_RXRDY) {
 		/* get data */
 		twi_data[twi_count++] = *AT91C_TWI_RHR;
-		
+
 		/* transfer complete? */
 		if (twi_count == twi_size) {
 			/* send STOP and wait for TXCOMP */
@@ -92,29 +92,29 @@ static void twi_isr(void)
 			*AT91C_TWI_IER = AT91C_TWI_TXCOMP | AT91C_TWI_NACK;
 		}
 	}
-	
+
 	/* transfer really complete? */
 	if (status & AT91C_TWI_TXCOMP) {
-		
+
 		/* are we doing a blmc update? */
 		if (twi_state == TWI_BLMC_UPDATE) {
 			uint32_t addr = (*AT91C_TWI_MMR >> 16) & 0x7F;
 			if (addr != TWI_ADDR_BL4) {
 				/* increase address */
 				*AT91C_TWI_MMR += (1<<16);
-				
+
 				/* send next value to next blmc */
 				*AT91C_TWI_THR = *twi_data++;
 			} else {
-				// TODO: 	
+				// TODO:
 			}
-		
+
 		} else {
 			*AT91C_TWI_IDR = AT91C_TWI_TXCOMP | AT91C_TWI_RXRDY | AT91C_TWI_TXRDY | AT91C_TWI_NACK;
 			twi_state = TWI_IDLE;
 		}
 	}
-} 
+}
 
 uint32_t twi_setpwm(uint8_t *values)
 {
@@ -128,21 +128,21 @@ uint32_t twi_setpwm(uint8_t *values)
 
 	*AT91C_TWI_MMR = (TWI_ADDR_BL1 << 16) | AT91C_TWI_IADRSZ_1_BYTE;
 	*AT91C_TWI_IADR = CMD_SET_PWM;
-	
+
 	*AT91C_TWI_THR = *twi_data++;
 	*AT91C_TWI_IER = AT91C_TWI_TXRDY | AT91C_TWI_NACK;
-	
-	return 0;	
+
+	return 0;
 }
 
 uint32_t twi_cmd(uint8_t addr, struct blmc_cmd *cmd)
 {
 	if (twi_state != TWI_IDLE)
 		return 1;
-	
+
 	/* TODO: locking needed? */
 	twi_state = TWI_GENERIC_CMD;
-	
+
 	/* read transfer, or write transfer with payload */
 	if (cmd->mode & BLMC_CMD_READ || cmd->size != 0) {
 		/* set address, direction, argument count and command bytes */
@@ -156,7 +156,7 @@ uint32_t twi_cmd(uint8_t addr, struct blmc_cmd *cmd)
 		*AT91C_TWI_MMR = (addr << 16) | (cmd->mode & 0xFF) << 8;
 		*AT91C_TWI_IADR = (cmd->cmd) >> 8;
 	}
-				
+
 	/* isr needs data & size parameters */
 	twi_data = cmd->data;
 	twi_size = cmd->size;
@@ -165,14 +165,14 @@ uint32_t twi_cmd(uint8_t addr, struct blmc_cmd *cmd)
 	if (cmd->mode & BLMC_CMD_READ) {
 		*AT91C_TWI_CR = AT91C_TWI_START;
 		*AT91C_TWI_IER = AT91C_TWI_RXRDY | AT91C_TWI_NACK;
-		
+
 	} else {
 		*AT91C_TWI_THR = (twi_size != 0) ? cmd->data[twi_count++] : (cmd->cmd & 0xFF);
 		*AT91C_TWI_IER = AT91C_TWI_TXRDY | AT91C_TWI_NACK;
 	}
 
-	/* 
-	 * wait for end 
+	/*
+	 * wait for end
 	 * TODO: locking needed?
 	 * TODO: timeout?
 	 */
@@ -181,7 +181,7 @@ uint32_t twi_cmd(uint8_t addr, struct blmc_cmd *cmd)
 		twi_state = TWI_IDLE;
 		return 1;
 	}
-	
+
 	return 0;
 }
 
@@ -235,13 +235,17 @@ void at91_twi_init(void)
 
 	/* SDA & SCL from Peripheral A, Open Drain, no Pullup */
 	AT91S_PIO *pio = AT91C_BASE_PIOA;
-	pio->PIO_ASR = AT91C_PA3_TWD | AT91C_PA4_TWCK;
-	pio->PIO_PDR = AT91C_PA3_TWD | AT91C_PA4_TWCK;
+
+	/* do a software reset (bus not connected) */
+	*AT91C_TWI_CR = AT91C_TWI_SWRST;
+
 	pio->PIO_MDER = AT91C_PA3_TWD | AT91C_PA4_TWCK;
 	pio->PIO_PPUDR = AT91C_PA3_TWD | AT91C_PA4_TWCK;
+	pio->PIO_ASR = AT91C_PA3_TWD | AT91C_PA4_TWCK;
+	pio->PIO_PDR = AT91C_PA3_TWD | AT91C_PA4_TWCK;
 
 	/* set TWI Clock */
-	*AT91C_TWI_CWGR = TWI_CLK(200000) | (5<<16);
+	*AT91C_TWI_CWGR = TWI_CLK(400000); //| (5<<16);
 
 	/* disable all (known) interrupts */
 	*AT91C_TWI_IDR = AT91C_TWI_TXCOMP | AT91C_TWI_RXRDY | AT91C_TWI_TXRDY | AT91C_TWI_NACK;