SFDP: consolidates device density detection

Combines implementations found from SPIFBlockDevice and QSPIFBlockDevice
and makes it as part of the SFDP module.

components/storage/blockdevice/COMPONENT_QSPIF/QSPIFBlockDevice.cpp

@@ -635,12 +635,10 @@ int QSPIFBlockDevice::_sfdp_parse_basic_param_table(Callback<int(bd_addr_t, void
         return -1;
     }
 
-    // Get device density (stored in bits - 1)
+    if (sfdp_detect_device_density(param_table, _sfdp_info.bptbl) < 0) {
-    uint32_t density_bits = ((param_table[7] << 24) |
+        tr_error("Detecting device density failed");
-                             (param_table[6] << 16) |
+        return -1;
-                             (param_table[5] << 8)  |
+    }
-                             param_table[4]);
-    sfdp_info.bptbl.device_size_bytes = (density_bits + 1) / 8;
 
     // Set Page Size (QSPI write must be done on Page limits)
     _page_size_bytes = sfdp_detect_page_size(param_table, sfdp_info.bptbl.size);

components/storage/blockdevice/COMPONENT_SPIF/SPIFBlockDevice.cpp

@@ -634,14 +634,10 @@ int SPIFBlockDevice::_sfdp_parse_basic_param_table(Callback<int(bd_addr_t, void
         return -1;
     }
 
-    // Get device density (stored in bits - 1)
+    if (sfdp_detect_device_density(param_table, _sfdp_info.bptbl) < 0) {
-    uint32_t density_bits = (
+        tr_error("Detecting device density failed");
-                                (param_table[7] << 24) |
+        return -1;
-                                (param_table[6] << 16) |
+    }
-                                (param_table[5] << 8) |
-                                param_table[4]);
-    sfdp_info.bptbl.device_size_bytes = (density_bits + 1) / 8;
-    tr_debug("Density bits: %" PRIu32 " , device size: %llu bytes", density_bits, sfdp_info.bptbl.device_size_bytes);
 
     // Set Default read/program/erase Instructions
     _read_instruction = SPIF_READ;

drivers/internal/SFDP.h

@@ -141,6 +141,15 @@ int sfdp_iterate_next_largest_erase_type(uint8_t &bitfield,
                                          int region,
                                          const sfdp_smptbl_info &smptbl);
 
+/** Detect device density
+ *
+ * @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_device_density(uint8_t *bptbl_ptr, sfdp_bptbl_info &bptbl_info);
+
 /** @}*/
 } /* namespace mbed */
 #endif

drivers/source/SFDP.cpp

@@ -344,5 +344,21 @@ int sfdp_iterate_next_largest_erase_type(uint8_t &bitfield,
     return largest_erase_type;
 }
 
+int sfdp_detect_device_density(uint8_t *bptbl_ptr, sfdp_bptbl_info &bptbl_info)
+{
+    // stored in bits - 1
+    uint32_t density_bits = (
+                                (bptbl_ptr[7] << 24) |
+                                (bptbl_ptr[6] << 16) |
+                                (bptbl_ptr[5] << 8) |
+                                bptbl_ptr[4]);
+
+    bptbl_info.device_size_bytes = (density_bits + 1) / 8;
+
+    tr_info("Density bits: %" PRIu32 " , device size: %llu bytes", density_bits, bptbl_info.device_size_bytes);
+
+    return 0;
+}
+
 } /* namespace mbed */
 #endif /* (DEVICE_SPI || DEVICE_QSPI) */