From c84deff782deab453680eb34e188363a26f8b4b7 Mon Sep 17 00:00:00 2001
From: Veijo Pesonen <veijo.pesonen@arm.com>
Date: Tue, 18 Feb 2020 18:06:57 +0200
Subject: [PATCH] SFDP: consolidates check for addressability

Takes implementations found from SPIFBlockDevice and QSPIFBlockDevice
and makes those as part of the SFDP module.
---
 .../COMPONENT_QSPIF/QSPIFBlockDevice.cpp      |  4 ++--
 .../COMPONENT_SPIF/SPIFBlockDevice.cpp        |  4 ++--
 drivers/internal/SFDP.h                       |  9 ++++++++
 drivers/source/SFDP.cpp                       | 22 +++++++++++++++++++
 4 files changed, 35 insertions(+), 4 deletions(-)

diff --git a/components/storage/blockdevice/COMPONENT_QSPIF/QSPIFBlockDevice.cpp b/components/storage/blockdevice/COMPONENT_QSPIF/QSPIFBlockDevice.cpp
index f33cee08ac..ebc55a4cf6 100644
--- a/components/storage/blockdevice/COMPONENT_QSPIF/QSPIFBlockDevice.cpp
+++ b/components/storage/blockdevice/COMPONENT_QSPIF/QSPIFBlockDevice.cpp
@@ -630,8 +630,8 @@ int QSPIFBlockDevice::_sfdp_parse_basic_param_table(Callback<int(bd_addr_t, void
     }
 
     // Check that density is not greater than 4 gigabits (i.e. that addressing beyond 4 bytes is not required)
-    if ((param_table[7] & 0x80) != 0) {
-        tr_error("Init - verify flash density failed");
+    if (sfdp_detect_addressability(param_table, _sfdp_info.bptbl) < 0) {
+        tr_error("Verify 4byte addressing failed");
         return -1;
     }
 
diff --git a/components/storage/blockdevice/COMPONENT_SPIF/SPIFBlockDevice.cpp b/components/storage/blockdevice/COMPONENT_SPIF/SPIFBlockDevice.cpp
index b3918b121a..da9dce5efe 100644
--- a/components/storage/blockdevice/COMPONENT_SPIF/SPIFBlockDevice.cpp
+++ b/components/storage/blockdevice/COMPONENT_SPIF/SPIFBlockDevice.cpp
@@ -629,8 +629,8 @@ int SPIFBlockDevice::_sfdp_parse_basic_param_table(Callback<int(bd_addr_t, void
     }
 
     // Check address size, currently only supports 3byte addresses
-    if ((param_table[2] & 0x4) != 0 || (param_table[7] & 0x80) != 0) {
-        tr_error("init - verify 3byte addressing Failed");
+    if (sfdp_detect_addressability(param_table, _sfdp_info.bptbl) < 0) {
+        tr_error("Verify 3byte addressing failed");
         return -1;
     }
 
diff --git a/drivers/internal/SFDP.h b/drivers/internal/SFDP.h
index 794bc3b292..aa4794ce7d 100644
--- a/drivers/internal/SFDP.h
+++ b/drivers/internal/SFDP.h
@@ -150,6 +150,15 @@ int sfdp_iterate_next_largest_erase_type(uint8_t &bitfield,
  */
 int sfdp_detect_device_density(uint8_t *bptbl_ptr, sfdp_bptbl_info &bptbl_info);
 
+/** Detect is it possible to access the whole memory region
+ *
+ * @param bptbl_ptr Pointer to memory holding a Basic Parameter Table structure
+ * @param bptbl_info Basic Parameter Table information structure
+ *
+ * @return 0 on success, negative error code on failure
+ */
+int sfdp_detect_addressability(uint8_t *bptbl_ptr, sfdp_bptbl_info &bptbl_info);
+
 /** @}*/
 } /* namespace mbed */
 #endif
diff --git a/drivers/source/SFDP.cpp b/drivers/source/SFDP.cpp
index 1d9e747610..af7ab15a29 100644
--- a/drivers/source/SFDP.cpp
+++ b/drivers/source/SFDP.cpp
@@ -360,5 +360,27 @@ int sfdp_detect_device_density(uint8_t *bptbl_ptr, sfdp_bptbl_info &bptbl_info)
     return 0;
 }
 
+#if DEVICE_QSPI
+int sfdp_detect_addressability(uint8_t *bptbl_ptr, sfdp_bptbl_info &bptbl_info)
+{
+    // Check that density is not greater than 4 gigabits (i.e. that addressing beyond 4 bytes is not required)
+    if ((bptbl_ptr[7] & 0x80) != 0) {
+        return -1;
+    }
+
+    return 0;
+}
+#elif DEVICE_SPI
+int sfdp_detect_addressability(uint8_t *bptbl_ptr, sfdp_bptbl_info &bptbl_info)
+{
+    // Check address size, currently only supports 3byte addresses
+    if ((bptbl_ptr[2] & 0x4) != 0 || (bptbl_ptr[7] & 0x80) != 0) {
+        return -1;
+    }
+
+    return 0;
+}
+#endif
+
 } /* namespace mbed */
 #endif /* (DEVICE_SPI || DEVICE_QSPI) */