diff --git a/fw/Core/Inc/config.h b/fw/Core/Inc/config.h
index d75e37f..c398e99 100644
--- a/fw/Core/Inc/config.h
+++ b/fw/Core/Inc/config.h
@@ -10,6 +10,19 @@
 
 /* TODO: add comments to everything */
 
+/* EXAMPLE of USAGE */
+/*
+config_t new_config;
+new_config.led_co2_alert_limit1 = 1000;
+new_config.led_co2_alert_limit2 = 2000;
+new_config.led_on = 1;
+new_config.modbus_addr = 0x11;
+config_write(&new_config);
+
+config_t config;
+config_read(&config);
+*/
+
 #include "stdint.h"
 #include "stm32l0xx.h"
 /* DESCRIPTION OF THE DATA STRUCTURE */
@@ -36,7 +49,7 @@
  * Device configuration data can be accessed using config_t struct.
  */
 #define	MODBUS_ADDR_LENGTH						2
-#define CONFIG_LED_ON_LENGTH					1
+#define CONFIG_LED_ON_LENGTH					2
 #define CONFIG_LED_ALERT1_LENGTH				2
 #define CONFIG_LED_ALERT2_LENGTH				2
 #define VENDOR_NAME_LENGTH						64
@@ -71,6 +84,9 @@
 #define EEPROM_WRITE_ERROR						-4
 #define EEPROM_ADDR_ERROR						-5
 
+#define SYSTICK_FREQ_HZ							12000000
+#define EEPROM_TIMEOUT_MAX_MS_INV				200
+
 typedef struct
 {
 	/* DEVICE CONFIG */
diff --git a/fw/Core/Src/config.c b/fw/Core/Src/config.c
index cee056c..f809f09 100644
--- a/fw/Core/Src/config.c
+++ b/fw/Core/Src/config.c
@@ -20,10 +20,10 @@ static int8_t eeprom_program_halfword(uint32_t addr, uint16_t ee_data);
 
 int8_t config_read(config_t *config)
 {
-	config->modbus_addr = *(uint16_t *) CONFIG_EEPROM_ADDR_MODBUS_ADDR;
-	config->led_on = *(uint8_t *) CONFIG_EEPROM_ADDR_LED_ON_ADDR;
-	config->led_co2_alert_limit1 = *(uint16_t *) CONFIG_EEPROM_ADDR_LED_ALERT1_ADDR;
-	config->led_co2_alert_limit2 = *(uint16_t *) CONFIG_EEPROM_ADDR_LED_ALERT2_ADDR;
+	config->modbus_addr = *(uint16_t *) (CONFIG_EEPROM_ADDR_MODBUS_ADDR);
+	config->led_on = *(uint16_t *) (CONFIG_EEPROM_ADDR_LED_ON_ADDR);
+	config->led_co2_alert_limit1 = *(uint16_t *) (CONFIG_EEPROM_ADDR_LED_ALERT1_ADDR);
+	config->led_co2_alert_limit2 = *(uint16_t *) (CONFIG_EEPROM_ADDR_LED_ALERT2_ADDR);
 	return CONFIG_OK;
 }
 
@@ -71,22 +71,56 @@ int8_t config_write(config_t *config)
 
 static int8_t eeprom_lock(void)
 {
-   while ((FLASH->SR & FLASH_SR_BSY) != 0) /* Wait for FLASH to be free */
-   {
-/* TODO:  insert timeout test */
-   }
+	uint32_t tick_start = SysTick->VAL;
+  	while ((FLASH->SR & FLASH_SR_BSY) != 0) /* Wait for FLASH to be free */
+  	{
+ 	   /* Timeout test */
+ 	   /* The maximum writing time is 3.94ms (half-word) */
+  		uint32_t tick_last = SysTick->VAL;
+  		uint32_t tick_diff;
+  		if (tick_start <= tick_last)
+  		{
+  			tick_diff = tick_last - tick_start;
+  		} else
+  		{
+  			tick_diff = (0xFFFFFFFF - tick_last) + tick_start;
+  		}
 
-   FLASH->PECR = FLASH->PECR & ~(FLASH_PECR_ERRIE | FLASH_PECR_EOPIE); /* disable flash interrupts */
-   FLASH->PECR = FLASH->PECR | FLASH_PECR_PELOCK; /* Lock memory with PELOCK */
+ 		/* If the time difference is more than 5ms */
+  		if (tick_diff >= (uint32_t)((uint32_t)SYSTICK_FREQ_HZ*(uint32_t)EEPROM_TIMEOUT_MAX_MS_INV))
+  		{
+  			return EEPROM_LOCK_ERROR;
+  		}
+  	}
 
-   return EEPROM_OK;
+  	FLASH->PECR = FLASH->PECR & ~(FLASH_PECR_ERRIE | FLASH_PECR_EOPIE); /* disable flash interrupts */
+  	FLASH->PECR = FLASH->PECR | FLASH_PECR_PELOCK; /* Lock memory with PELOCK */
+
+  	return EEPROM_OK;
 }
 
 static int8_t eeprom_unlock(void)
 {
+	uint32_t tick_start = SysTick->VAL;
    while ((FLASH->SR & FLASH_SR_BSY) != 0) /* Wait for FLASH to be free */
    {
-/*  TODO: insert timeout test */
+	   /* Timeout test */
+	   /* The maximum writing time is 3.94ms (half-word) */
+ 		uint32_t tick_last = SysTick->VAL;
+ 		uint32_t tick_diff;
+ 		if (tick_start <= tick_last)
+ 		{
+ 			tick_diff = tick_last - tick_start;
+ 		} else
+ 		{
+ 			tick_diff = (0xFFFFFFFF - tick_last) + tick_start;
+ 		}
+
+ 		/* If the time difference is more than 5ms */
+  		if (tick_diff >= (uint32_t)((uint32_t)SYSTICK_FREQ_HZ*(uint32_t)EEPROM_TIMEOUT_MAX_MS_INV))
+ 		{
+ 			return EEPROM_UNLOCK_ERROR;
+ 		}
    }
    if ((FLASH->PECR & FLASH_PECR_PELOCK) != 0) /* If PELOCK is locked */
    {
@@ -103,8 +137,6 @@ static int8_t eeprom_program_byte(uint32_t addr, uint8_t ee_data)
 	if ((EEPROM_ADDR_START <= addr) && (addr <= EEPROM_ADDR_END - 1))
 	{
 		*(uint8_t *)(addr) = ee_data; /* write data to EEPROM */
-		/* TODO: Why is not the WFI triggering on FLASH Interrupt if it is supposed to be enabled? */
-		//__WFI();
 		if (*(uint8_t *)(addr) != ee_data)
 		{
 			return EEPROM_WRITE_ERROR;
@@ -120,8 +152,6 @@ static int8_t eeprom_program_halfword(uint32_t addr, uint16_t ee_data)
 	if ((EEPROM_ADDR_START <= addr) && (addr <= EEPROM_ADDR_END - 2))
 	{
 		*(uint16_t *)(addr) = ee_data; /* write data to EEPROM */
-		/* TODO: Why is not the WFI triggering on FLASH Interrupt if it is supposed to be enabled? */
-		//__WFI();
 		if (*(uint16_t *)(addr) != ee_data)
 		{
 			return EEPROM_WRITE_ERROR;
diff --git a/fw/iaq_wired_sensor Debug.launch b/fw/iaq_wired_sensor Debug.launch
index 61bf8b8..647065f 100644
--- a/fw/iaq_wired_sensor Debug.launch	
+++ b/fw/iaq_wired_sensor Debug.launch	
@@ -68,6 +68,6 @@
     <listAttribute key="org.eclipse.debug.core.MAPPED_RESOURCE_TYPES">
         <listEntry value="4"/>
     </listAttribute>
-    <stringAttribute key="org.eclipse.dsf.launch.MEMORY_BLOCKS" value="&lt;?xml version=&quot;1.0&quot; encoding=&quot;UTF-8&quot; standalone=&quot;no&quot;?&gt;&#10;&lt;memoryBlockExpressionList context=&quot;reserved-for-future-use&quot;/&gt;&#10;"/>
+    <stringAttribute key="org.eclipse.dsf.launch.MEMORY_BLOCKS" value="&lt;?xml version=&quot;1.0&quot; encoding=&quot;UTF-8&quot; standalone=&quot;no&quot;?&gt;&#10;&lt;memoryBlockExpressionList context=&quot;reserved-for-future-use&quot;&gt;&#10;    &lt;gdbmemoryBlockExpression address=&quot;134742016&quot; label=&quot;0x08080000&quot;/&gt;&#10;    &lt;gdbmemoryBlockExpression address=&quot;134742020&quot; label=&quot;0x08080004&quot;/&gt;&#10;&lt;/memoryBlockExpressionList&gt;&#10;"/>
     <stringAttribute key="process_factory_id" value="org.eclipse.cdt.dsf.gdb.GdbProcessFactory"/>
 </launchConfiguration>