diff --git a/firmware/programmer/spi_flash.c b/firmware/programmer/spi_flash.c
index dce906b..1b27b9e 100644
--- a/firmware/programmer/spi_flash.c
+++ b/firmware/programmer/spi_flash.c
@@ -5,6 +5,7 @@
 
 #include "spi_flash.h"
 #include <stm32f10x.h>
+#include <stdbool.h>
 
 #define SPI_FLASH_CS_PIN GPIO_Pin_4
 #define SPI_FLASH_SCK_PIN GPIO_Pin_5
@@ -16,14 +17,11 @@
 #define FLASH_READY 0
 #define FLASH_BUSY  1
 #define FLASH_TIMEOUT 2
+#define FLASH_STATUS_INVALID_CMD 3
 
-/* 1st addressing cycle */
 #define ADDR_1st_CYCLE(ADDR) (uint8_t)((ADDR)& 0xFF)
-/* 2nd addressing cycle */
 #define ADDR_2nd_CYCLE(ADDR) (uint8_t)(((ADDR)& 0xFF00) >> 8)
-/* 3rd addressing cycle */
 #define ADDR_3rd_CYCLE(ADDR) (uint8_t)(((ADDR)& 0xFF0000) >> 16)
-/* 4th addressing cycle */
 #define ADDR_4th_CYCLE(ADDR) (uint8_t)(((ADDR)& 0xFF000000) >> 24)
 
 #define UNDEFINED_CMD 0xFF
@@ -40,35 +38,57 @@ typedef struct __attribute__((__packed__))
     uint8_t busy_bit;
     uint8_t busy_state;
     uint32_t freq;
+    uint8_t addr_bytes;
+    uint8_t enter_4byte_cmd;
+    uint8_t exit_4byte_cmd;
 } spi_conf_t;
 
 static spi_conf_t spi_conf;
 
+static uint8_t spi_flash_send_byte(uint8_t byte);
+static inline uint8_t spi_flash_read_byte(void);
+
+static void spi_flash_gpio_init(void);
+static void spi_flash_gpio_uninit(void);
+static inline void spi_flash_select_chip(void);
+static inline void spi_flash_deselect_chip(void);
+
+static uint16_t spi_flash_get_baud_rate_prescaler(uint32_t spi_freq_khz);
+
+static void spi_flash_send_address(uint32_t addr);
+
+static int spi_flash_init(void *conf, uint32_t conf_size);
+static void spi_flash_uninit(void);
+static uint32_t spi_flash_read_status(void);
+static uint32_t spi_flash_get_status(void);
+static void spi_flash_read_id(chip_id_t *chip_id);
+static void spi_flash_write_enable(void);
+static void spi_flash_write_page_async(uint8_t *buf, uint32_t page, uint32_t page_size);
+static uint32_t spi_flash_read_data(uint8_t *buf, uint32_t page, uint32_t page_offset, uint32_t data_size);
+static uint32_t spi_flash_read_page(uint8_t *buf, uint32_t page, uint32_t page_size);
+static uint32_t spi_flash_read_spare_data(uint8_t *buf, uint32_t page, uint32_t offset, uint32_t data_size);
+static uint32_t spi_flash_erase_block(uint32_t page);
+static inline bool spi_flash_is_bb_supported(void);
+
 static void spi_flash_gpio_init()
 {
     GPIO_InitTypeDef gpio_init;
 
     RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
-
-    /* Enable SPI peripheral clock */
     RCC_APB2PeriphClockCmd(RCC_APB2Periph_SPI1, ENABLE);
   
-    /* Configure SPI SCK pin */
     gpio_init.GPIO_Pin = SPI_FLASH_SCK_PIN;
     gpio_init.GPIO_Speed = GPIO_Speed_50MHz;
     gpio_init.GPIO_Mode = GPIO_Mode_AF_PP;
     GPIO_Init(GPIOA, &gpio_init);
 
-    /* Configure SPI MOSI pin */
     gpio_init.GPIO_Pin = SPI_FLASH_MOSI_PIN;
     GPIO_Init(GPIOA, &gpio_init);
 
-    /* Configure SPI MISO pin */
     gpio_init.GPIO_Pin = SPI_FLASH_MISO_PIN;
     gpio_init.GPIO_Mode = GPIO_Mode_IN_FLOATING;
     GPIO_Init(GPIOA, &gpio_init);
   
-    /* Configure SPI CS pin */
     gpio_init.GPIO_Pin = SPI_FLASH_CS_PIN;
     gpio_init.GPIO_Mode = GPIO_Mode_Out_PP;
     GPIO_Init(GPIOA, &gpio_init);
@@ -78,25 +98,11 @@ static void spi_flash_gpio_uninit()
 {
     GPIO_InitTypeDef gpio_init;
 
-    /* Disable SPI peripheral clock */
     RCC_APB2PeriphClockCmd(RCC_APB2Periph_SPI1, DISABLE);
 
-    /* Disable SPI SCK pin */
-    gpio_init.GPIO_Pin = SPI_FLASH_SCK_PIN;
+    gpio_init.GPIO_Pin = SPI_FLASH_SCK_PIN | SPI_FLASH_MISO_PIN | SPI_FLASH_MOSI_PIN | SPI_FLASH_CS_PIN;
     gpio_init.GPIO_Mode = GPIO_Mode_IN_FLOATING;
     GPIO_Init(GPIOA, &gpio_init);
-
-    /* Disable SPI MISO pin */
-    gpio_init.GPIO_Pin = SPI_FLASH_MISO_PIN;
-    GPIO_Init(GPIOA, &gpio_init);
-
-    /* Disable SPI MOSI pin */
-    gpio_init.GPIO_Pin = SPI_FLASH_MOSI_PIN;
-    GPIO_Init(GPIOA, &gpio_init);
-
-    /* Disable SPI CS pin */
-    gpio_init.GPIO_Pin = SPI_FLASH_CS_PIN;
-    GPIO_Init(GPIOA, &gpio_init);
 }
 
 static inline void spi_flash_select_chip()
@@ -131,6 +137,17 @@ static uint16_t spi_flash_get_baud_rate_prescaler(uint32_t spi_freq_khz)
         return SPI_BaudRatePrescaler_256;
 }
 
+static void spi_flash_send_address(uint32_t addr)
+{
+    if (spi_conf.addr_bytes == 4)
+    {
+        spi_flash_send_byte(ADDR_4th_CYCLE(addr));
+    }
+    spi_flash_send_byte(ADDR_3rd_CYCLE(addr));
+    spi_flash_send_byte(ADDR_2nd_CYCLE(addr));
+    spi_flash_send_byte(ADDR_1st_CYCLE(addr));
+}
+
 static int spi_flash_init(void *conf, uint32_t conf_size)
 {
     SPI_InitTypeDef spi_init;
@@ -139,24 +156,25 @@ static int spi_flash_init(void *conf, uint32_t conf_size)
         return -1; 
     spi_conf = *(spi_conf_t *)conf;
 
-    spi_flash_gpio_init();
+    if (spi_conf.addr_bytes != 3 && spi_conf.addr_bytes != 4)
+    {
+        spi_conf.addr_bytes = 3;
+    }
 
+    spi_flash_gpio_init();
     spi_flash_deselect_chip();
 
-    /* Configure SPI */
     spi_init.SPI_Direction = SPI_Direction_2Lines_FullDuplex;
     spi_init.SPI_Mode = SPI_Mode_Master;
     spi_init.SPI_DataSize = SPI_DataSize_8b;
     spi_init.SPI_CPOL = SPI_CPOL_High;
     spi_init.SPI_CPHA = SPI_CPHA_2Edge;
     spi_init.SPI_NSS = SPI_NSS_Soft;
-    spi_init.SPI_BaudRatePrescaler =
-        spi_flash_get_baud_rate_prescaler(spi_conf.freq);
+    spi_init.SPI_BaudRatePrescaler = spi_flash_get_baud_rate_prescaler(spi_conf.freq);
     spi_init.SPI_FirstBit = SPI_FirstBit_MSB;
     spi_init.SPI_CRCPolynomial = 7;
     SPI_Init(SPI1, &spi_init);
 
-    /* Enable SPI */
     SPI_Cmd(SPI1, ENABLE);
 
     return 0;
@@ -165,23 +183,14 @@ static int spi_flash_init(void *conf, uint32_t conf_size)
 static void spi_flash_uninit()
 {
     spi_flash_gpio_uninit();
-
-    /* Disable SPI */
     SPI_Cmd(SPI1, DISABLE);
 }
 
 static uint8_t spi_flash_send_byte(uint8_t byte)
 {
-    /* Loop while DR register in not emplty */
     while (SPI_I2S_GetFlagStatus(SPI1, SPI_I2S_FLAG_TXE) == RESET);
-
-    /* Send byte through the SPI1 peripheral to generate clock signal */
     SPI_I2S_SendData(SPI1, byte);
-
-    /* Wait to receive a byte */
     while (SPI_I2S_GetFlagStatus(SPI1, SPI_I2S_FLAG_RXNE) == RESET);
-
-    /* Return the byte read from the SPI bus */
     return SPI_I2S_ReceiveData(SPI1);
 }
 
@@ -197,10 +206,19 @@ static uint32_t spi_flash_read_status()
 
     spi_flash_select_chip();
 
-    spi_flash_send_byte(spi_conf.status_cmd);
+    if (spi_conf.addr_bytes == 4 && spi_conf.enter_4byte_cmd != UNDEFINED_CMD)
+    {
+        spi_flash_send_byte(spi_conf.enter_4byte_cmd);
+    }
 
+    spi_flash_send_byte(spi_conf.status_cmd);
     status = spi_flash_read_byte();
 
+    if (spi_conf.addr_bytes == 4 && spi_conf.exit_4byte_cmd != UNDEFINED_CMD)
+    {
+        spi_flash_send_byte(spi_conf.exit_4byte_cmd);
+    }
+
     if (spi_conf.busy_state == 1 && (status & (1 << spi_conf.busy_bit)))
         flash_status = FLASH_BUSY;
     else if (spi_conf.busy_state == 0 && !(status & (1 << spi_conf.busy_bit)))
@@ -217,7 +235,6 @@ static uint32_t spi_flash_get_status()
 
     status = spi_flash_read_status();
 
-    /* Wait for an operation to complete or a TIMEOUT to occur */
     while (status == FLASH_BUSY && timeout)
     {
         status = spi_flash_read_status();
@@ -235,7 +252,6 @@ static void spi_flash_read_id(chip_id_t *chip_id)
     spi_flash_select_chip();
 
     spi_flash_send_byte(spi_conf.read_id_cmd);
-
     chip_id->maker_id = spi_flash_read_byte();
     chip_id->device_id = spi_flash_read_byte();
     chip_id->third_id = spi_flash_read_byte();
@@ -254,61 +270,69 @@ static void spi_flash_write_enable()
     spi_flash_deselect_chip();
 }
 
-static void spi_flash_write_page_async(uint8_t *buf, uint32_t page,
-    uint32_t page_size)
+static void spi_flash_write_page_async(uint8_t *buf, uint32_t page, uint32_t page_size)
 {
     uint32_t i;
+    uint32_t addr = page << spi_conf.page_offset;
 
     spi_flash_write_enable();
-
     spi_flash_select_chip();
 
-    spi_flash_send_byte(spi_conf.write_cmd);
-
-    page = page << spi_conf.page_offset;
+    if (spi_conf.addr_bytes == 4 && spi_conf.enter_4byte_cmd != UNDEFINED_CMD)
+    {
+        spi_flash_send_byte(spi_conf.enter_4byte_cmd);
+    }
 
-    spi_flash_send_byte(ADDR_3rd_CYCLE(page));
-    spi_flash_send_byte(ADDR_2nd_CYCLE(page));
-    spi_flash_send_byte(ADDR_1st_CYCLE(page));
+    spi_flash_send_byte(spi_conf.write_cmd);
+    spi_flash_send_address(addr);
 
     for (i = 0; i < page_size; i++)
         spi_flash_send_byte(buf[i]);
 
+    if (spi_conf.addr_bytes == 4 && spi_conf.exit_4byte_cmd != UNDEFINED_CMD)
+    {
+        spi_flash_send_byte(spi_conf.exit_4byte_cmd);
+    }
+
     spi_flash_deselect_chip();
 }
 
-static uint32_t spi_flash_read_data(uint8_t *buf, uint32_t page,
-    uint32_t page_offset, uint32_t data_size)
+static uint32_t spi_flash_read_data(uint8_t *buf, uint32_t page, uint32_t page_offset, uint32_t data_size)
 {
-    uint32_t i, addr = (page << spi_conf.page_offset) + page_offset;
+    uint32_t i;
+    uint32_t addr = (page << spi_conf.page_offset) + page_offset;
 
     spi_flash_select_chip();
 
-    spi_flash_send_byte(spi_conf.read_cmd);
+    if (spi_conf.addr_bytes == 4 && spi_conf.enter_4byte_cmd != UNDEFINED_CMD)
+    {
+        spi_flash_send_byte(spi_conf.enter_4byte_cmd);
+    }
 
-    spi_flash_send_byte(ADDR_3rd_CYCLE(addr));
-    spi_flash_send_byte(ADDR_2nd_CYCLE(addr));
-    spi_flash_send_byte(ADDR_1st_CYCLE(addr));
+    spi_flash_send_byte(spi_conf.read_cmd);
+    spi_flash_send_address(addr);
 
-    /* AT45DB requires write of dummy byte after address */
     spi_flash_send_byte(FLASH_DUMMY_BYTE);
 
     for (i = 0; i < data_size; i++)
         buf[i] = spi_flash_read_byte();
 
+    if (spi_conf.addr_bytes == 4 && spi_conf.exit_4byte_cmd != UNDEFINED_CMD)
+    {
+        spi_flash_send_byte(spi_conf.exit_4byte_cmd);
+    }
+
     spi_flash_deselect_chip();
 
     return FLASH_READY;
 }
 
-static uint32_t spi_flash_read_page(uint8_t *buf, uint32_t page,
-    uint32_t page_size)
+static uint32_t spi_flash_read_page(uint8_t *buf, uint32_t page, uint32_t page_size)
 {
     return spi_flash_read_data(buf, page, 0, page_size);
 }
 
-static uint32_t spi_flash_read_spare_data(uint8_t *buf, uint32_t page,
-    uint32_t offset, uint32_t data_size)
+static uint32_t spi_flash_read_spare_data(uint8_t *buf, uint32_t page, uint32_t offset, uint32_t data_size)
 {
     return FLASH_STATUS_INVALID_CMD;
 }
@@ -318,14 +342,20 @@ static uint32_t spi_flash_erase_block(uint32_t page)
     uint32_t addr = page << spi_conf.page_offset;
 
     spi_flash_write_enable();
-
     spi_flash_select_chip();
 
+    if (spi_conf.addr_bytes == 4 && spi_conf.enter_4byte_cmd != UNDEFINED_CMD)
+    {
+        spi_flash_send_byte(spi_conf.enter_4byte_cmd);
+    }
+
     spi_flash_send_byte(spi_conf.erase_cmd);
+    spi_flash_send_address(addr);
 
-    spi_flash_send_byte(ADDR_3rd_CYCLE(addr));
-    spi_flash_send_byte(ADDR_2nd_CYCLE(addr));
-    spi_flash_send_byte(ADDR_1st_CYCLE(addr));
+    if (spi_conf.addr_bytes == 4 && spi_conf.exit_4byte_cmd != UNDEFINED_CMD)
+    {
+        spi_flash_send_byte(spi_conf.exit_4byte_cmd);
+    }
 
     spi_flash_deselect_chip();