diff --git a/TODO b/TODO
index ae60a18..38a3c67 100644
--- a/TODO
+++ b/TODO
@@ -1,2 +1 @@
-Fix EEPROM write - detection and read work (maybe algo failure?)
 Battery icon - how to calc percentage?
diff --git a/code/main.c b/code/main.c
index ff247e5..3d0bf81 100644
--- a/code/main.c
+++ b/code/main.c
@@ -109,7 +109,7 @@ uint8_t event_count  = 0;
 uint8_t *event_read  = events;
 uint8_t *event_write = events;
 
-uint32_t ms = 0;
+volatile uint32_t ms = 0;
 
 #ifdef ROT_REVERSE
 const uint8_t PROGMEM rot_table[16] =
@@ -749,6 +749,8 @@ void display_init()
 	{
 		display_send_data(buf, sizeof(buf));
 	}
+
+	i2c_stop();
 }
 
 void display_enable(uint8_t en)
@@ -819,6 +821,7 @@ void print_symbol(uint8_t symbol_idx, uint8_t x, uint8_t y, uint8_t invert)
 
 	display_set_area(x, x + CHAR_SIZE * 2 - 1, y * 2, y * 2 + 1);
 	display_send_data(unfolded_symbol, sizeof(unfolded_symbol));
+	i2c_stop();
 }
 
 void print_mm()
@@ -1061,11 +1064,13 @@ void print_battery_icon()
 		display_set_area(OLED_BATTERY_ICON_X1, OLED_BATTERY_ICON_X1 + OLED_BATTERY_ICON_SIZE - 1,
 			OLED_BATTERY_ICON_Y1, OLED_BATTERY_ICON_Y2);
 		display_send_data(buf, (OLED_BATTERY_ICON_Y2 - OLED_BATTERY_ICON_Y1 + 1) * OLED_BATTERY_ICON_SIZE);
+		i2c_stop();
 
 		// Draw right border
 		display_set_area(OLED_BATTERY_ICON_X2 - OLED_BATTERY_ICON_SIZE + 1, OLED_BATTERY_ICON_X2,
 			OLED_BATTERY_ICON_Y1, OLED_BATTERY_ICON_Y2);
 		display_send_data(buf, (OLED_BATTERY_ICON_Y2 - OLED_BATTERY_ICON_Y1 + 1) * OLED_BATTERY_ICON_SIZE);
+		i2c_stop();
 
 		for (i = 0; i < sizeof(buf); ++i)
 		{
@@ -1077,10 +1082,12 @@ void print_battery_icon()
 			OLED_BATTERY_ICON_X1 + OLED_BATTERY_ICON_SIZE * 2 - 1,
 			OLED_BATTERY_ICON_Y1, OLED_BATTERY_ICON_Y1);
 		display_send_data(buf, OLED_BATTERY_ICON_SIZE);
+		i2c_stop();
 		display_set_area(OLED_BATTERY_ICON_X2 - OLED_BATTERY_ICON_SIZE * 2 + 1,
 			OLED_BATTERY_ICON_X2 - OLED_BATTERY_ICON_SIZE,
 			OLED_BATTERY_ICON_Y1, OLED_BATTERY_ICON_Y1);
 		display_send_data(buf, OLED_BATTERY_ICON_SIZE);
+		i2c_stop();
 
 		for (i = 0; i < sizeof(buf); ++i)
 		{
@@ -1092,10 +1099,12 @@ void print_battery_icon()
 			OLED_BATTERY_ICON_X1 + OLED_BATTERY_ICON_SIZE * 2 - 1,
 			OLED_BATTERY_ICON_Y2, OLED_BATTERY_ICON_Y2);
 		display_send_data(buf, OLED_BATTERY_ICON_SIZE);
+		i2c_stop();
 		display_set_area(OLED_BATTERY_ICON_X2 - OLED_BATTERY_ICON_SIZE * 2 + 1,
 			OLED_BATTERY_ICON_X2 - OLED_BATTERY_ICON_SIZE,
 			OLED_BATTERY_ICON_Y2, OLED_BATTERY_ICON_Y2);
 		display_send_data(buf, OLED_BATTERY_ICON_SIZE);
+		i2c_stop();
 	}
 
 	// TODO: Calc battery percent
@@ -1151,6 +1160,7 @@ void print_battery_icon()
 			y, y);
 		display_send_data(buf, OLED_BATTERY_ICON_X2 - OLED_BATTERY_ICON_X1 -
 			OLED_BATTERY_ICON_SIZE * 4 + 1);
+		i2c_stop();
 	}
 
 // Buffer safety
@@ -1316,6 +1326,7 @@ int8_t write_eeprom_val(int8_t idx, uint32_t value, uint8_t direction)
 {
 	int8_t err;
 	uint8_t buf[4];
+	uint32_t write_complete_when;
 
 	if (idx < 0)
 	{
@@ -1336,6 +1347,10 @@ int8_t write_eeprom_val(int8_t idx, uint32_t value, uint8_t direction)
 		return -1;
 	}
 
+	// Wait for write to complete
+	write_complete_when = ms + 20;
+	while (ms < write_complete_when);
+
 	return 0;
 }