SFDP: Add support for multiple configurations and sector maps

A Sector Map Parameter Table contains a sequence of the following
descriptors:
* (Optional) configuration detection command descriptors, one for
each command to run to determine the current configuration. This
exists only if the flash layout is configurable.
* Sector map descriptors, one for each possible configuration. On
a flash device with a non-configurable layout, there is only one
such descriptor.

Previously we only supported the non-configurable case with a single
descriptor. This commit adds support for multiple configurations.

storage/blockdevice/source/SFDP.cpp

@@ -268,7 +268,7 @@ int sfdp_parse_sector_map_table(Callback<int(bd_addr_t, sfdp_cmd_addr_size_t, ui
      * - sector map configuration detection commands
      * - configurations
      * - regions in each configuration
-     *  is variable -> the size of this table is variable
+     * are variable -> the size of this table is variable
      */
     auto smptbl_buff = std::make_unique<uint8_t[]>(sfdp_info.smptbl.size);
     if (!smptbl_buff) {
@@ -291,13 +291,96 @@ int sfdp_parse_sector_map_table(Callback<int(bd_addr_t, sfdp_cmd_addr_size_t, ui
         return -1;
     }
 
-    // Currently we support only Single Map Descriptor
-    if (!((smptbl_buff[0] & 0x3) == 0x03) && (smptbl_buff[1] == 0x0)) {
-        tr_error("Sector Map: Supporting Only Single Map Descriptor (not map commands)");
+    // Loop through all descriptors: configuration command descriptor and sector map descriptors.
+    // Command descriptors are optional. If any exist, they always precede sector map descriptors.
+    uint8_t *table = smptbl_buff.get();
+    uint8_t *descriptor = table;
+    const size_t min_descriptor_size = 8; // two DWORDs
+
+    // Part 1: configuration detection command descriptors (optional).
+    // If any exist, run their commands to determine the active configuration's ID.
+    // Otherwise the configuration ID is 0x00.
+    uint8_t active_config_id = 0x00;
+    // If any configuration detection command descriptors exist, the table starts with one.
+    if (!(descriptor[0] & 0x02)) {
+        while (descriptor + min_descriptor_size <= table + sfdp_info.smptbl.size) { // table boundary
+            if (descriptor[0] & 0x02) {
+                tr_error("Sector Map: Expecting a configuration detection command descriptor");
+                return -1;
+            }
+
+            uint8_t instruction = descriptor[1];
+            uint8_t dummy_cycles = descriptor[2] & 0x0F;
+            auto addr_size = static_cast<sfdp_cmd_addr_size_t>(descriptor[2] >> 6);
+            uint8_t mask = descriptor[3];
+            uint32_t cmd_addr;
+            memcpy(&cmd_addr, &descriptor[4], sizeof(cmd_addr)); // bytes 4-7 of the descriptor
+
+            uint8_t rx;
+            status = sfdp_reader(cmd_addr, addr_size, instruction, dummy_cycles, &rx, sizeof(rx));
+            if (status < 0) {
+                tr_error("Sector Map: Configuration detection command failed");
+                return -1;
+            }
+
+            // Shift existing bits to the left, so we can add the newly detected bit
+            active_config_id <<= 1;
+
+            // The mask may apply to any bit of rx, so we can't directly combine
+            // (rx & mask) with active_config_id. Instead, treat (rx & mask) as a boolean.
+            if (rx & mask) {
+                active_config_id |= 0x01;
+            }
+
+            // Each command descriptor has a fixed size of two DWORDS.
+            if (descriptor[0] & 0x01) { // the current command descriptor is the last one
+                descriptor += min_descriptor_size; // next descriptor
+                break;
+            }
+            descriptor += min_descriptor_size; // next descriptor
+        }
+    }
+
+    // Part 2: sector map descriptors.
+    // Find the descriptor that matches the active configuration, or use the only one
+    // if the flash is not configurable.
+    // Each sector map descriptor is two DWORDs or larger. Use two DOWRDs as a loop condition
+    // to avoid buffer overflow, then deduce the actual descriptor size from the descriptor header.
+    uint8_t *enabled_map = nullptr;
+    while (descriptor + min_descriptor_size <= table + sfdp_info.smptbl.size) { // table boundary
+        if (!(descriptor[0] & 0x02)) { // not a sector map descriptor
+            tr_error("Sector Map: Expecting a sector map descriptor");
+            return -1;
+        }
+
+        size_t regions = descriptor[2] + 1; // Region ID starts at 0
+        size_t current_descriptor_size = (1 /*header*/ + regions) * 4 /*DWORD*/;
+        if (descriptor + current_descriptor_size > table + sfdp_info.smptbl.size) {
+            tr_error("Sector Map: Incomplete sector map descriptor at the end of the table");
+            return -1;
+        }
+
+        if (descriptor[1] == active_config_id) {
+            // matching sector map found
+            enabled_map = descriptor;
+            break;
+        }
+
+        if (descriptor[0] & 0x01) {
+            // last command descriptor: do not loop any further
+            break;
+        }
+
+        descriptor += current_descriptor_size; // next descriptor
+    }
+
+    if (!enabled_map) {
+        tr_error("Sector Map: No sector map matches the current configuration");
         return -1;
     }
 
-    sfdp_info.smptbl.region_cnt = smptbl_buff[2] + 1;
+    // Process the currently active enabled map
+    sfdp_info.smptbl.region_cnt = enabled_map[2] + 1;
     if (sfdp_info.smptbl.region_cnt > SFDP_SECTOR_MAP_MAX_REGIONS) {
         tr_error("Sector Map: Supporting up to %d regions, current setup to %d regions - fail",
                  SFDP_SECTOR_MAP_MAX_REGIONS,
@@ -308,10 +391,10 @@ int sfdp_parse_sector_map_table(Callback<int(bd_addr_t, sfdp_cmd_addr_size_t, ui
     // Loop through Regions and set for each one: size, supported erase types, high boundary offset
     // Calculate minimum Common Erase Type for all Regions
     for (auto idx = 0; idx < sfdp_info.smptbl.region_cnt; idx++) {
-        tmp_region_size = ((*((uint32_t *)&smptbl_buff[(idx + 1) * 4])) >> 8) & 0x00FFFFFF; // bits 9-32
+        tmp_region_size = ((*((uint32_t *)&enabled_map[(idx + 1) * 4])) >> 8) & 0x00FFFFFF; // bits 9-32
         sfdp_info.smptbl.region_size[idx] = (tmp_region_size + 1) * 256; // Region size is 0 based multiple of 256 bytes;
 
-        sfdp_info.smptbl.region_erase_types_bitfld[idx] = smptbl_buff[(idx + 1) * 4] & 0x0F; // bits 1-4
+        sfdp_info.smptbl.region_erase_types_bitfld[idx] = enabled_map[(idx + 1) * 4] & 0x0F; // bits 1-4
 
         min_common_erase_type_bits &= sfdp_info.smptbl.region_erase_types_bitfld[idx];
 
@@ -335,7 +418,6 @@ int sfdp_parse_sector_map_table(Callback<int(bd_addr_t, sfdp_cmd_addr_size_t, ui
     return 0;
 }
 
-
 size_t sfdp_detect_page_size(uint8_t *basic_param_table_ptr, size_t basic_param_table_size)
 {
     constexpr int SFDP_BASIC_PARAM_TABLE_PAGE_SIZE = 40;

storage/blockdevice/tests/UNITTESTS/SFDP/test_sfdp.cpp

@@ -19,14 +19,50 @@
 #include "blockdevice/internal/SFDP.h"
 
 using ::testing::_;
+using ::testing::Expectation;
 using ::testing::MockFunction;
 using ::testing::Return;
 
+// The following data is used by multiple test cases.
+
 /**
  * The Sector Map Parameter Table of Cypress S25FS512S:
  * https://www.cypress.com/file/216376/download Table 71.
  */
 static const mbed::bd_addr_t sector_map_start_addr = 0xD81000;
+static const mbed::bd_addr_t register_CR1NV = 0x000002;
+static const mbed::bd_addr_t register_CR3NV = 0x000004;
+static const uint8_t sector_map_multiple_descriptors[] = {
+    // Detect 1
+    0xFC, 0x65, 0xFF, 0x08,
+    0x04, 0x00, 0x00, 0x00,
+
+    // Detect 2
+    0xFC, 0x65, 0xFF, 0x04,
+    0x02, 0x00, 0x00, 0x00,
+
+    // Detect 3
+    0xFD, 0x65, 0xFF, 0x02,
+    0x04, 0x00, 0x00, 0x00,
+
+    // Config 1
+    0xFE, 0x01, 0x02, 0xFF,     // header
+    0xF1, 0x7F, 0x00, 0x00,     // region 0
+    0xF4, 0x7F, 0x03, 0x00,     // region 1
+    0xF4, 0xFF, 0xFB, 0x03,     // region 2
+
+    // No Config 2
+
+    // Config 3
+    0xFE, 0x03, 0x02, 0xFF,     // header
+    0xF4, 0xFF, 0xFB, 0x03,     // region 0
+    0xF4, 0x7F, 0x03, 0x00,     // region 1
+    0xF1, 0x7F, 0x00, 0x00,     // region 2
+
+    // Config 4
+    0xFF, 0x05, 0x00, 0xFF,     // header
+    0xF4, 0xFF, 0xFF, 0x03      // region 0
+};
 
 /**
  * Based on Cypress S25FS512S, modified to have one descriptor,
@@ -54,7 +90,19 @@ class TestSFDP : public testing::Test {
             return mock_return;
         }
 
-        memcpy(buff, sector_descriptors, sector_descriptors_size);
+        // The following register values give Configuration ID = 0x03.
+        uint8_t *out = static_cast<uint8_t*>(buff);
+        switch (addr) {
+            case sector_map_start_addr:
+                memcpy(buff, sector_descriptors, sector_descriptors_size);
+                break;
+            case register_CR1NV:
+                out[0] = 0x04;
+                break;
+            case register_CR3NV:
+                out[0] = 0x02;
+                break;
+        }
         return 0;
     };
 
@@ -291,3 +339,73 @@ TEST_F(TestSFDP, TestMoreRegionsThanSupported)
 
     EXPECT_EQ(-1, sfdp_parse_sector_map_table(sfdp_reader_callback, header_info));
 }
+
+/**
+ * When a Sector Map Parameter Table has multiple configuration detection
+ * commands and sector maps, sfdp_parse_sector_map_table() runs all commands
+ * to find the active configuration and selects the matching sector map.
+ */
+TEST_F(TestSFDP, TestMultipleSectorConfigs)
+{
+    mbed::sfdp_hdr_info header_info;
+    set_sector_map_param_table(
+        header_info.smptbl,
+        sector_map_multiple_descriptors,
+        sizeof(sector_map_multiple_descriptors)
+    );
+
+    // First call: get all detection command and sector map descriptors
+    Expectation call_1 = EXPECT_CALL(
+        sfdp_reader_mock,
+        Call(
+            sector_map_start_addr,
+            mbed::SFDP_READ_CMD_ADDR_TYPE,
+            mbed::SFDP_READ_CMD_INST,
+            mbed::SFDP_READ_CMD_DUMMY_CYCLES,
+            _,
+            sizeof(sector_map_multiple_descriptors)
+        )
+    ).Times(1).WillOnce(Return(0));
+
+    // Second call: detect bit-0 of configuration
+    Expectation call_2 = EXPECT_CALL(
+        sfdp_reader_mock,
+        Call(register_CR3NV, mbed::SFDP_CMD_ADDR_SIZE_VARIABLE, 0x65, mbed::SFDP_CMD_DUMMY_CYCLES_VARIABLE, _, 1)
+    ).Times(1).After(call_1).WillOnce(Return(0));
+
+    // Third call: detect bit-1 of configuration
+    Expectation call_3 = EXPECT_CALL(
+        sfdp_reader_mock,
+        Call(register_CR1NV, mbed::SFDP_CMD_ADDR_SIZE_VARIABLE, 0x65, mbed::SFDP_CMD_DUMMY_CYCLES_VARIABLE, _, 1)
+    ).Times(1).After(call_2).WillOnce(Return(0));
+
+    // Fourth call: detect bit-2 of configuration
+    Expectation call_4 = EXPECT_CALL(
+        sfdp_reader_mock,
+        Call(register_CR3NV, mbed::SFDP_CMD_ADDR_SIZE_VARIABLE, 0x65, mbed::SFDP_CMD_DUMMY_CYCLES_VARIABLE, _, 1)
+    ).Times(1).After(call_3).WillOnce(Return(0));
+
+    EXPECT_EQ(0, sfdp_parse_sector_map_table(sfdp_reader_callback, header_info));
+
+    // Expecting sector map for Configuration ID = 0x03:
+    // Three regions
+    EXPECT_EQ(3, header_info.smptbl.region_cnt);
+
+    // Region 0: erase type 3 (256KB erase)
+    // Range: first 64 MB minus 256 KB
+    EXPECT_EQ(64_MB - 256_KB, header_info.smptbl.region_size[0]);
+    EXPECT_EQ(64_MB - 256_KB - 1_B, header_info.smptbl.region_high_boundary[0]);
+    EXPECT_EQ(1 << (3 - 1), header_info.smptbl.region_erase_types_bitfld[0]);
+
+    // Region 1: erase type 3 (256KB erase, which also covers 32KB from Region 2)
+    // Range: between Region 0 and Region 2
+    EXPECT_EQ(256_KB - 32_KB, header_info.smptbl.region_size[1]);
+    EXPECT_EQ(64_MB - 32_KB - 1_B, header_info.smptbl.region_high_boundary[1]);
+    EXPECT_EQ(1 << (3 - 1), header_info.smptbl.region_erase_types_bitfld[1]);
+
+    // Region 2: erase type 1 (4KB erase)
+    // Range: last 32 KB
+    EXPECT_EQ(32_KB, header_info.smptbl.region_size[2]);
+    EXPECT_EQ(64_MB - 1_B, header_info.smptbl.region_high_boundary[2]);
+    EXPECT_EQ(1 << (1 - 1), header_info.smptbl.region_erase_types_bitfld[2]);
+}