diff --git a/README.md b/README.md
index d391857..16a870a 100644
--- a/README.md
+++ b/README.md
@@ -10,10 +10,10 @@ Currently the following AVR MCUs are supported:
 
 AVR MCU | Flash bytes used (.text + .data) | Bootloader region size
 --- | --- | ---
-atmega8 | 868 (0x364) | 512 words
-atmega88 | 920 (0x398) | 512 words
-atmega168 | 972 (0x3CC) | 512 words
-atmega328p | 972 (0x3CC) | 512 words
+atmega8 | 872 (0x368) | 512 words
+atmega88 | 918 (0x396) | 512 words
+atmega168 | 970 (0x3CA) | 512 words
+atmega328p | 970 (0x3CA) | 512 words
 (Compiled on Ubuntu 18.04 LTS (gcc 5.4.0 / avr-libc 2.0.0) with EEPROM and LED support)
 
 
diff --git a/main.c b/main.c
index 66a8f92..8ee42ed 100644
--- a/main.c
+++ b/main.c
@@ -358,6 +358,7 @@ static uint8_t TWI_data_read(uint8_t bcnt)
 ISR(TWI_vect)
 {
     static uint8_t bcnt;
+    uint8_t control = TWCR;
 
     switch (TWSR & 0xF8)
     {
@@ -365,18 +366,13 @@ ISR(TWI_vect)
         case 0x60:
             bcnt = 0;
             LED_RT_ON();
-            TWCR |= (1<<TWINT) | (1<<TWEA);
             break;
 
         /* prev. SLA+W, data received, ACK returned -> receive data and ACK */
         case 0x80:
-            if (TWI_data_write(bcnt++, TWDR))
+            if (TWI_data_write(bcnt++, TWDR) == 0x00)
             {
-                TWCR |= (1<<TWINT) | (1<<TWEA);
-            }
-            else
-            {
-                TWCR |= (1<<TWINT);
+                control &= ~(1<<TWEA);
                 bcnt = 0;
             }
             break;
@@ -389,22 +385,25 @@ ISR(TWI_vect)
         /* prev. SLA+R, data sent, ACK returned -> send data */
         case 0xB8:
             TWDR = TWI_data_read(bcnt++);
-            TWCR |= (1<<TWINT) | (1<<TWEA);
             break;
 
+        /* prev. SLA+W, data received, NACK returned -> IDLE */
+        case 0x88:
         /* STOP or repeated START -> IDLE */
         case 0xA0:
         /* prev. SLA+R, data sent, NACK returned -> IDLE */
         case 0xC0:
             LED_RT_OFF();
-            TWCR |= (1<<TWINT) | (1<<TWEA);
+            control |= (1<<TWEA);
             break;
 
-        /* illegal state -> reset hardware */
-        case 0xF8:
-            TWCR |= (1<<TWINT) | (1<<TWSTO) | (1<<TWEA);
+        /* illegal state(s) -> reset hardware */
+        default:
+            control |= (1<<TWSTO);
             break;
     }
+
+    TWCR = (1<<TWINT) | control;
 } /* TWI_vect */