1 files changed, 51 insertions, 83 deletions

diff --git a/drivers/avr/i2c_slave.c b/drivers/avr/i2c_slave.c
index 3edf85b12..27696ca01 100755
--- a/drivers/avr/i2c_slave.c
+++ b/drivers/avr/i2c_slave.c
@@ -5,96 +5,64 @@
 #include <avr/io.h>
 #include <util/twi.h>
 #include <avr/interrupt.h>
+#include <stdbool.h>
 
 #include "i2c_slave.h"
 
 void i2c_init(uint8_t address){
-        // load address into TWI address register
-        TWAR = (address << 1);
-        // set the TWCR to enable address matching and enable TWI, clear TWINT, enable TWI interrupt
-        TWCR = (1<<TWIE) | (1<<TWEA) | (1<<TWINT) | (1<<TWEN);
+    // load address into TWI address register
+    TWAR = (address << 1);
+    // set the TWCR to enable address matching and enable TWI, clear TWINT, enable TWI interrupt
+    TWCR = (1 << TWIE) | (1 << TWEA) | (1 << TWINT) | (1 << TWEN);
 }
 
 void i2c_stop(void){
-        // clear acknowledge and enable bits
-        TWCR &= ~( (1<<TWEA) | (1<<TWEN) );
+    // clear acknowledge and enable bits
+    TWCR &= ~((1 << TWEA) | (1 << TWEN));
 }
 
 ISR(TWI_vect){
-        
-        // temporary stores the received data
-        uint8_t data;
-        
-        // own address has been acknowledged
-        if( (TWSR & 0xF8) == TW_SR_SLA_ACK ){  
-                buffer_address = 0xFF;
-                // clear TWI interrupt flag, prepare to receive next byte and acknowledge
-                TWCR |= (1<<TWIE) | (1<<TWINT) | (1<<TWEA) | (1<<TWEN); 
-        }
-        else if( (TWSR & 0xF8) == TW_SR_DATA_ACK ){ // data has been received in slave receiver mode
-                
-                // save the received byte inside data 
-                data = TWDR;
-                
-                // check wether an address has already been transmitted or not
-                if(buffer_address == 0xFF){
-                        
-                        buffer_address = data; 
-                        
-                        // clear TWI interrupt flag, prepare to receive next byte and acknowledge
-                        TWCR |= (1<<TWIE) | (1<<TWINT) | (1<<TWEA) | (1<<TWEN); 
-                }
-                else{ // if a databyte has already been received
-                        
-                        // store the data at the current address
-                        rxbuffer[buffer_address] = data;
-                        
-                        // increment the buffer address
-                        buffer_address++;
-                        
-                        // if there is still enough space inside the buffer
-                        if(buffer_address < 0xFF){
-                                // clear TWI interrupt flag, prepare to receive next byte and acknowledge
-                                TWCR |= (1<<TWIE) | (1<<TWINT) | (1<<TWEA) | (1<<TWEN); 
-                        }
-                        else{
-                                // Don't acknowledge
-                                TWCR &= ~(1<<TWEA); 
-                                // clear TWI interrupt flag, prepare to receive last byte.
-                                TWCR |= (1<<TWIE) | (1<<TWINT) | (1<<TWEN); 
-                        }
-                }
-        }
-        else if( (TWSR & 0xF8) == TW_ST_DATA_ACK ){ // device has been addressed to be a transmitter
-                
-                // copy data from TWDR to the temporary memory
-                data = TWDR;
-                
-                // if no buffer read address has been sent yet
-                if( buffer_address == 0xFF ){
-                        buffer_address = data;
-                }
-                
-                // copy the specified buffer address into the TWDR register for transmission
-                TWDR = txbuffer[buffer_address];
-                // increment buffer read address
-                buffer_address++;
-                
-                // if there is another buffer address that can be sent
-                if(buffer_address < 0xFF){
-                        // clear TWI interrupt flag, prepare to send next byte and receive acknowledge
-                        TWCR |= (1<<TWIE) | (1<<TWINT) | (1<<TWEA) | (1<<TWEN); 
-                }
-                else{
-                        // Don't acknowledge
-                        TWCR &= ~(1<<TWEA); 
-                        // clear TWI interrupt flag, prepare to receive last byte.
-                        TWCR |= (1<<TWIE) | (1<<TWINT) | (1<<TWEN); 
-                }
-                
-        }
-        else{
-                // if none of the above apply prepare TWI to be addressed again
-                TWCR |= (1<<TWIE) | (1<<TWEA) | (1<<TWEN);
-        } 
-}
+    uint8_t ack = 1;
+    // temporary stores the received data
+    //uint8_t data;
+    
+    switch(TW_STATUS){
+        case TW_SR_SLA_ACK:
+            // The device is now a slave receiver
+            slave_has_register_set = false;
+            break;
+
+        case TW_SR_DATA_ACK:
+            // This device is a slave receiver and has received data
+            // First byte is the location then the bytes will be writen in buffer with auto-incriment
+            if(!slave_has_register_set){
+                buffer_address = TWDR;
+
+                if (buffer_address >= RX_BUFFER_SIZE){ // address out of bounds dont ack
+                    ack = 0;
+                    buffer_address = 0;
+                }
+                slave_has_register_set = true; // address has been receaved now fill in buffer
+            } else {
+                rxbuffer[buffer_address] = TWDR;
+                buffer_address++;
+            }
+            break;
+
+        case TW_ST_SLA_ACK:
+        case TW_ST_DATA_ACK:
+            // This device is a slave transmitter and master has requested data
+            TWDR = txbuffer[buffer_address];
+            buffer_address++;
+            break;
+
+        case TW_BUS_ERROR:
+            // We got an error, reset i2c
+            TWCR = 0;
+        default:
+            break;
+    }
+
+    // Reset i2c state mahcine to be ready for next interrupt
+    TWCR |= (1 << TWIE) | (1 << TWINT) | (ack << TWEA) | (1 << TWEN);
+}
\ No newline at end of file