diff --git a/firmware/shared/spi.c b/firmware/shared/spi.c
index 8a74662..29854a0 100644
--- a/firmware/shared/spi.c
+++ b/firmware/shared/spi.c
@@ -14,6 +14,9 @@
  * currently alpha, uses interrupts, single master/single slave operation
  */
 
+uint8_t readbuf[4];
+uint8_t writebuf[4];
+uint8_t pos;
 
 
 
@@ -70,6 +73,7 @@ void spi_init(){
 #ifndef SPI_MASTER
     SPCR |= _BV(SPIE); //enable interrupts
 #endif
+    pos = 0;
 }
 
 
@@ -116,28 +120,25 @@ void spi_mst_write_read(uint8_t len, uint8_t *data){
 
 #ifndef SPI_MASTER
 
-void __attribute__((always_inline)) spi_sla_handle_packet(){
+void spi_sla_handle_packet(){
         // TODO: make slave not hangup in case of partial read
 	uint8_t opcode, addr, do_write;
 	uint16_t data;
-	SPDR = 0x53; // random ack
-	SPI_WAIT;
-	opcode = SPDR;
+    opcode = readbuf[0];
+    addr = readbuf[1];
 	do_write = spi_proto_needswrite(opcode);
-	SPI_WAIT;
-	addr = SPDR;
-	SPDR = 0x00; // buys some pause
 	if(do_write){
 		data = spi_proto_handlewrite(opcode, addr);
-		SPI_WAIT;
-		SPDR = data >> 8;
-		SPI_WAIT;
-		SPDR = data & 0xff;
+        writebuf[0]=opcode;
+        writebuf[1]=addr;
+        writebuf[2]=(data>>8)&0xff;
+        writebuf[3]=(data)&0xff;
 	} else {
-		SPI_WAIT;
-		data = SPDR << 8;
-		SPI_WAIT;
-		data |= SPDR;
+        writebuf[0]=0;
+        writebuf[1]=0;
+        writebuf[2]=0;
+        writebuf[3]=0;
+        data = (readbuf[3]<<8)&0xff | readbuf[4];
 		spi_proto_handleread(opcode, addr, data);
 	}
 }
@@ -145,13 +146,16 @@ void __attribute__((always_inline)) spi_sla_handle_packet(){
 
 
 ISR(SPI_STC_vect){
-/*    uint8_t a;
-    a = SPDR;
-    SPDR = ~a; //*/
-    SPCR &= ~(_BV(SPIE)); //disable spi interrupts
-    spi_sla_handle_packet(); 
-    SPCR |= _BV(SPIE);
-   //*/
+    SPDR = *(writebuf+pos);
+    *(readbuf+pos) = SPDR;
+    pos++;
+    if(SPI_SSIN & _BV(SPI_SS_PIN)){
+        pos=0;
+        SPCR &= ~(_BV(SPIE));
+        sei();
+        spi_sla_handle_packet();
+        SPCR |= _BV(SPIE);
+    }
 }
 #endif //not SPI_MASTER
 	
diff --git a/firmware/shared/spi_proto.c b/firmware/shared/spi_proto.c
index 2f43bdd..0017088 100644
--- a/firmware/shared/spi_proto.c
+++ b/firmware/shared/spi_proto.c
@@ -16,13 +16,18 @@ void talk_to_slave(uint8_t opcode, uint8_t addr, uint8_t flags)
 	spi_mst_end_packet();
 }
 
-uint16_t spi_master_communicate(uint8_t opcode, uint8_t address, uint16_t value)
+uint16_t spi_master_communicate(uint8_t opcode, uint8_t addr, uint16_t value)
 {
     uint8_t flags = spi_proto_needs(opcode);
+	write_data[0] = opcode;
+	write_data[1] = addr;
 	write_data[2] = value >> 8;
 	write_data[3] = value & 0xff;
-	talk_to_slave(opcode, address, flags);
-	return (read_data[0] << 8) | read_data[1];
+	spi_mst_write_read(4,write_data);
+    while(write_data[0] == 0){
+        spi_mst_write_read(4,write_data);
+    }
+	return (write_data[2] << 8) | read_data[3];
 }
 
 uint8_t spi_proto_needs(uint8_t opcode)
diff --git a/firmware/slavechip/spi_config.h b/firmware/slavechip/spi_config.h
index 30e155c..cb36b2f 100644
--- a/firmware/slavechip/spi_config.h
+++ b/firmware/slavechip/spi_config.h
@@ -3,5 +3,5 @@
 #ifdef SPI_MASTER
 #undef SPI_MASTER
 #endif //SPI_MASTER
-//#define SPI_SS_PORT D
-//#define SPI_SS_PIN 7
+#define SPI_SS_PORT B
+#define SPI_SS_PIN 2