From 7c692f5196fbcfe957e024207a6340dc55ee06fa Mon Sep 17 00:00:00 2001
From: Veijo Pesonen <veijo.pesonen@arm.com>
Date: Wed, 19 Feb 2020 15:43:26 +0200
Subject: [PATCH] SPIFBlockDevice: Refactor vendor specific workarounds

To follow the same convention as with QSPIFBlockDevice there was need to
create a separate function for handling vendor specific workarounds.
---
 .../COMPONENT_QSPIF/QSPIFBlockDevice.cpp      |  2 +-
 .../COMPONENT_QSPIF/QSPIFBlockDevice.h        |  6 +--
 .../COMPONENT_SPIF/SPIFBlockDevice.cpp        | 48 ++++++++++++-------
 .../COMPONENT_SPIF/SPIFBlockDevice.h          |  3 ++
 4 files changed, 38 insertions(+), 21 deletions(-)

diff --git a/components/storage/blockdevice/COMPONENT_QSPIF/QSPIFBlockDevice.cpp b/components/storage/blockdevice/COMPONENT_QSPIF/QSPIFBlockDevice.cpp
index 6362a9230b..e22902450b 100644
--- a/components/storage/blockdevice/COMPONENT_QSPIF/QSPIFBlockDevice.cpp
+++ b/components/storage/blockdevice/COMPONENT_QSPIF/QSPIFBlockDevice.cpp
@@ -226,7 +226,7 @@ int QSPIFBlockDevice::init()
         goto exit_point;
     }
 
-    if (0 != _handle_vendor_quirks()) {
+    if (_handle_vendor_quirks() < 0) {
         tr_error("Init - Could not read vendor id");
         status = QSPIF_BD_ERROR_DEVICE_ERROR;
         goto exit_point;
diff --git a/components/storage/blockdevice/COMPONENT_QSPIF/QSPIFBlockDevice.h b/components/storage/blockdevice/COMPONENT_QSPIF/QSPIFBlockDevice.h
index d518a82094..51ac2a09ef 100644
--- a/components/storage/blockdevice/COMPONENT_QSPIF/QSPIFBlockDevice.h
+++ b/components/storage/blockdevice/COMPONENT_QSPIF/QSPIFBlockDevice.h
@@ -306,6 +306,9 @@ private:
     // Enable Fast Mode - for flash chips with low power default
     int _enable_fast_mode();
 
+    // Query vendor ID and handle special behavior that isn't covered by SFDP data
+    int _handle_vendor_quirks();
+
     /****************************************/
     /* SFDP Detection and Parsing Functions */
     /****************************************/
@@ -329,9 +332,6 @@ private:
     // Detect 4-byte addressing mode and enable it if supported
     int _sfdp_detect_and_enable_4byte_addressing(uint8_t *basic_param_table_ptr, int basic_param_table_size);
 
-    // Query vendor ID and handle special behavior that isn't covered by SFDP data
-    int _handle_vendor_quirks();
-
     /***********************/
     /* Utilities Functions */
     /***********************/
diff --git a/components/storage/blockdevice/COMPONENT_SPIF/SPIFBlockDevice.cpp b/components/storage/blockdevice/COMPONENT_SPIF/SPIFBlockDevice.cpp
index f8864b4005..d1bc1fde6a 100644
--- a/components/storage/blockdevice/COMPONENT_SPIF/SPIFBlockDevice.cpp
+++ b/components/storage/blockdevice/COMPONENT_SPIF/SPIFBlockDevice.cpp
@@ -115,10 +115,7 @@ SPIFBlockDevice::SPIFBlockDevice(PinName mosi, PinName miso, PinName sclk, PinNa
 
 int SPIFBlockDevice::init()
 {
-    uint8_t vendor_device_ids[4];
-    size_t data_length = 3;
     int status = SPIF_BD_ERROR_OK;
-    spif_bd_error spi_status = SPIF_BD_ERROR_OK;
 
     _mutex->lock();
 
@@ -141,24 +138,12 @@ int SPIFBlockDevice::init()
         tr_debug("Initialize flash memory OK");
     }
 
-    /* Read Manufacturer ID (1byte), and Device ID (2bytes)*/
-    spi_status = _spi_send_general_command(SPIF_RDID, SPI_NO_ADDRESS_COMMAND, NULL, 0, (char *)vendor_device_ids,
-                                           data_length);
-    if (spi_status != SPIF_BD_ERROR_OK) {
-        tr_error("init - Read Vendor ID Failed");
+    if (_handle_vendor_quirks() < 0) {
+        tr_error("Init - Could not read vendor id");
         status = SPIF_BD_ERROR_DEVICE_ERROR;
         goto exit_point;
     }
 
-    switch (vendor_device_ids[0]) {
-        case 0xbf:
-            // SST devices come preset with block protection
-            // enabled for some regions, issue global protection unlock to clear
-            _set_write_enable();
-            _spi_send_general_command(SPIF_ULBPR, SPI_NO_ADDRESS_COMMAND, NULL, 0, NULL, 0);
-            break;
-    }
-
     //Synchronize Device
     if (false == _is_mem_ready()) {
         tr_error("init - _is_mem_ready Failed");
@@ -764,3 +749,32 @@ int SPIFBlockDevice::_set_write_enable()
     } while (false);
     return status;
 }
+
+int SPIFBlockDevice::_handle_vendor_quirks()
+{
+    uint8_t vendor_device_ids[4];
+    size_t data_length = 3;
+
+    /* Read Manufacturer ID (1byte), and Device ID (2bytes)*/
+    spif_bd_error spi_status = _spi_send_general_command(SPIF_RDID, SPI_NO_ADDRESS_COMMAND, NULL, 0,
+                                                         (char *)vendor_device_ids,
+                                                         data_length);
+
+    if (spi_status != SPIF_BD_ERROR_OK) {
+        tr_error("Read Vendor ID Failed");
+        return -1;
+    }
+
+    tr_debug("Vendor device ID = 0x%x 0x%x 0x%x", vendor_device_ids[0], vendor_device_ids[1], vendor_device_ids[2]);
+
+    switch (vendor_device_ids[0]) {
+        case 0xbf:
+            // SST devices come preset with block protection
+            // enabled for some regions, issue global protection unlock to clear
+            _set_write_enable();
+            _spi_send_general_command(SPIF_ULBPR, SPI_NO_ADDRESS_COMMAND, NULL, 0, NULL, 0);
+            break;
+    }
+
+    return 0;
+}
diff --git a/components/storage/blockdevice/COMPONENT_SPIF/SPIFBlockDevice.h b/components/storage/blockdevice/COMPONENT_SPIF/SPIFBlockDevice.h
index e5bece0862..b5c1bdeb8f 100644
--- a/components/storage/blockdevice/COMPONENT_SPIF/SPIFBlockDevice.h
+++ b/components/storage/blockdevice/COMPONENT_SPIF/SPIFBlockDevice.h
@@ -260,6 +260,9 @@ private:
     // Wait on status register until write not-in-progress
     bool _is_mem_ready();
 
+    // Query vendor ID and handle special behavior that isn't covered by SFDP data
+    int _handle_vendor_quirks();
+
 private:
     // Master side hardware
     mbed::SPI _spi;