Remove overcomplicated code from I2CEEBlockDevice

components/storage/blockdevice/COMPONENT_I2CEE/I2CEEBlockDevice.cpp

@@ -62,45 +62,44 @@ int I2CEEBlockDevice::read(void *buffer, bd_addr_t addr, bd_size_t size)
     // Check the address and size fit onto the chip.
     MBED_ASSERT(is_valid_read(addr, size));
 
-    auto *charBuffer = reinterpret_cast<char *>(buffer);
+    auto *pBuffer = reinterpret_cast<char *>(buffer);
+
+    while (size > 0) {
+        uint32_t off = addr % _block;
+        uint32_t chunk = (off + size < _block) ? size : (_block - off);
 
-    auto const handler = [&](const bd_addr_t &pagedStart, const bd_size_t &pagedLength, const uint8_t &pagedDeviceAddress) -> int {
         _i2c->start();
 
-        if (1 != _i2c->write(pagedDeviceAddress))
+        if (1 != _i2c->write(get_paged_device_address(addr))) {
-        {
             return BD_ERROR_DEVICE_ERROR;
         }
 
-        if (!_address_is_eight_bit && 1 != _i2c->write((char)(pagedStart >> 8u)))
+        if (!_address_is_eight_bit && 1 != _i2c->write((char)(addr >> 8u))) {
-        {
             return BD_ERROR_DEVICE_ERROR;
         }
 
-        if (1 != _i2c->write((char)(pagedStart & 0xffu)))
+        if (1 != _i2c->write((char)(addr & 0xffu))) {
-        {
             return BD_ERROR_DEVICE_ERROR;
         }
 
         _i2c->stop();
 
         auto err = _sync();
-        if (err)
+
-        {
+        if (err) {
             return err;
         }
 
-        if (0 != _i2c->read(_i2c_addr, charBuffer, pagedLength))
+        if (0 != _i2c->read(_i2c_addr, pBuffer, chunk)) {
-        {
             return BD_ERROR_DEVICE_ERROR;
         }
 
-        charBuffer += size;
+        addr += chunk;
+        size -= chunk;
+        pBuffer += chunk;
+    }
 
-        return BD_ERROR_OK;
+    return BD_ERROR_OK;
-    };
-
-    return do_paged(addr, size, handler);
 }
 
 int I2CEEBlockDevice::program(const void *buffer, bd_addr_t addr, bd_size_t size)
@@ -108,53 +107,47 @@ int I2CEEBlockDevice::program(const void *buffer, bd_addr_t addr, bd_size_t size
     // Check the addr and size fit onto the chip.
     MBED_ASSERT(is_valid_program(addr, size));
 
-    auto const *charBuffer = reinterpret_cast<char const *>(buffer);
+    auto const *pBuffer = reinterpret_cast<char const *>(buffer);
 
-    auto const handler = [&](const bd_addr_t &pagedStart, const bd_size_t &pagedLength, const uint8_t &pagedDeviceAddress) -> int {
+    // While we have some more data to write.
-        // While we have some more data to write.
+    while (size > 0) {
-        while (size > 0)
+        uint32_t off = addr % _block;
-        {
+        uint32_t chunk = (off + size < _block) ? size : (_block - off);
-            uint32_t off = addr % _block;
-            uint32_t chunk = (off + size < _block) ? size : (_block - off);
 
-            _i2c->start();
+        _i2c->start();
 
-            if (1 != _i2c->write(pagedDeviceAddress)) {
+        if (1 != _i2c->write(get_paged_device_address(addr))) {
-                return BD_ERROR_DEVICE_ERROR;
+            return BD_ERROR_DEVICE_ERROR;
-            }
-
-            if (!_address_is_eight_bit && 1 != _i2c->write((char)(pagedStart >> 8u))) {
-                return BD_ERROR_DEVICE_ERROR;
-            }
-
-            if (1 != _i2c->write((char)(addr & 0xffu))) {
-                return BD_ERROR_DEVICE_ERROR;
-            }
-
-            for (unsigned i = 0; i < chunk; i++) {
-                if (1 != _i2c->write(charBuffer[i])) {
-                    return BD_ERROR_DEVICE_ERROR;
-                }
-            }
-
-            _i2c->stop();
-
-            int err = _sync();
-
-            if (err) {
-                return err;
-            }
-
-            addr += chunk;
-            size -= chunk;
-            charBuffer += chunk;
         }
 
-        return BD_ERROR_OK;
+        if (!_address_is_eight_bit && 1 != _i2c->write((char)(addr >> 8u))) {
-    };
+            return BD_ERROR_DEVICE_ERROR;
+        }
 
-    auto const originalSize = size;
+        if (1 != _i2c->write((char)(addr & 0xffu))) {
-    return do_paged(addr, originalSize, handler);
+            return BD_ERROR_DEVICE_ERROR;
+        }
+
+        for (unsigned i = 0; i < chunk; i++) {
+            if (1 != _i2c->write(pBuffer[i])) {
+                return BD_ERROR_DEVICE_ERROR;
+            }
+        }
+
+        _i2c->stop();
+
+        int err = _sync();
+
+        if (err) {
+            return err;
+        }
+
+        addr += chunk;
+        size -= chunk;
+        pBuffer += chunk;
+    }
+
+    return BD_ERROR_OK;
 }
 
 int I2CEEBlockDevice::erase(bd_addr_t addr, bd_size_t size)
@@ -204,68 +197,13 @@ const char *I2CEEBlockDevice::get_type() const
     return "I2CEE";
 }
 
-int I2CEEBlockDevice::do_paged(const bd_addr_t &startAddress,
+uint8_t I2CEEBlockDevice::get_paged_device_address(const bd_addr_t &address)
-                               const bd_size_t &length,
-                               const paged_handler &handler)
-{
-    // This helper is only used for eight bit mode.
-    if (!this->_address_is_eight_bit) {
-        return handler(startAddress, length, get_paged_device_address(0));
-    }
-
-    auto currentStartAddress = startAddress;
-
-    auto const pageSize = 256;
-    bd_size_t lengthDone = 0;
-    while (lengthDone != length) {
-        /* Integer division => Round down */
-        uint8_t const currentPage = currentStartAddress / pageSize;
-        bd_addr_t const nextPageBegin = (currentPage + 1) * pageSize;
-        bd_addr_t const currentReadEndAddressExclusive = std::min(nextPageBegin, startAddress + length);
-        bd_size_t const currentLength = currentReadEndAddressExclusive - currentStartAddress;
-        bd_addr_t const pagedBegin = currentStartAddress - (currentPage * pageSize);
-        uint8_t const pagedDeviceAddress = get_paged_device_address(currentPage);
-
-        auto const handlerReturn = handler(pagedBegin, currentLength, pagedDeviceAddress);
-        if (handlerReturn != BD_ERROR_OK) {
-            return handlerReturn;
-        }
-
-        currentStartAddress = currentReadEndAddressExclusive;
-        lengthDone += currentLength;
-    }
-
-    return BD_ERROR_OK;
-}
-
-uint8_t I2CEEBlockDevice::get_paged_device_address(const uint8_t &page)
 {
     if (!this->_address_is_eight_bit) {
         return this->_i2c_addr;
     } else {
-        // This method uses a dynamically created bit mask for the page given.
+        // Use the three least significant bits of the 2nd byte as the page
-        // This ensures compatibility with all sizes of ICs.
+        // The page will be bits 2-4 of the user defined addresses.
-        // E. g. the 512K variants have two user address bits and one page bit.
+        return this->_i2c_addr | ((address & 0x0700u) >> 7u);
-        // We don't want to forcefully override the two user address bits.
-
-        // Create a mask to cover all bits required to set page
-        // i starts at one because the LSB is used for R/W in I2C
-        uint8_t i = 1;
-        uint8_t addressMask = 0;
-        auto p = page;
-        while (p != 0u) {
-            addressMask |= (1u << i);
-            p >>= 1u;
-            i++;
-        }
-
-        uint8_t pagedDeviceAddress = this->_i2c_addr & static_cast<uint8_t>(~addressMask);
-        // Assert page < 0b111, because we don't have
-        // more bits for page encoding
-        // Don't actually write 0b111, this is a nonstandard extension.
-        MBED_ASSERT(page < 0x7);
-        pagedDeviceAddress |= static_cast<uint8_t>(page << 1u);
-
-        return pagedDeviceAddress;
     }
-}
+}

components/storage/blockdevice/COMPONENT_I2CEE/I2CEEBlockDevice.h

@@ -181,32 +181,15 @@ private:
 
     int _sync();
 
-    using paged_handler = std::function<int(const bd_addr_t &address, const bd_size_t &length, const uint8_t &deviceAddress)>;
-
-    /**
-     * Executes a handler across page boundaries for eight bit mode.
-     * When eight bit mode is disabled, the function does not do paging at all.
-     * When eight bit mode is enabled, this function splits the requested
-     * address space into multiple pages when needed.
-     * This is required when a read or write must be done across multiple pages.
-     *
-     * @param startAddress The address to start
-     * @param length The requested length of the operation
-     * @param handler The handler to execute
-     * @return Returns 0 when all calls to handler() return 0. Otherwise the
-     *         error code from the first non-zero handler() call.
-     */
-    int do_paged(const bd_addr_t &startAddress, const bd_size_t &length, const paged_handler &handler);
-
     /**
      * Gets the device's I2C address with respect to the requested page.
      * When eight bit mode is disabled, this function is a noop.
      * When eight bit mode is enabled, it sets the bits required for this bit
      * in the devices address. Other bits remain unchained.
-     * @param page The requested page
+     * @param address An address in the requested page.
      * @return The device's I2C address for that page
      */
-    uint8_t get_paged_device_address(const uint8_t &page);
+    uint8_t get_paged_device_address(const bd_addr_t &address);
 };