From 8f6eb72ab4cedcfa667828097778d67e3f5e8729 Mon Sep 17 00:00:00 2001
From: dario <dario.vandenboom@arcor.de>
Date: Thu, 30 May 2024 11:07:10 +0200
Subject: [PATCH 01/13] Added simple logger

---
 include/sta/utils/logger.hpp |  73 +++++++++++++++++++++++++
 include/sta/utils/logger.tpp | 100 +++++++++++++++++++++++++++++++++++
 2 files changed, 173 insertions(+)
 create mode 100644 include/sta/utils/logger.hpp
 create mode 100644 include/sta/utils/logger.tpp

diff --git a/include/sta/utils/logger.hpp b/include/sta/utils/logger.hpp
new file mode 100644
index 0000000..5c5120f
--- /dev/null
+++ b/include/sta/utils/logger.hpp
@@ -0,0 +1,73 @@
+#ifndef STA_UTILS_LOGGER_HPP
+#define STA_UTILS_LOGGER_HPP
+
+#include <sta/drivers/w25qxx.hpp>
+#include <sta/debug/assert.hpp>
+
+#include <cstring>
+
+namespace sta
+{
+    template <typename T>
+    class Logger
+    {
+    public:
+        /**
+         * @brief Construct a new Logger object
+         * 
+         * @param flash 
+         * @param startAddr 
+         * @param endAddr 
+         */
+        Logger(W25Qxx * flash, uint32_t startSector, uint32_t endSector);
+
+        /**
+         * @brief Write a new data point to the flash chip. 
+         * @note If the total capacity of this logger was exceeded, it restarts at the first sector, overwriting its data.
+         * 
+         * @param data The data to write to the flash chip.
+         */
+        void write(T data);
+
+        /**
+         * @brief Clear the flash memory used by the logger.
+         * 
+         */
+        void clear();
+
+        /**
+         * @brief Get the number of data points currently written to the memory segment.
+         * 
+         * @return size_t The number of data points.
+         */
+        size_t occupied();
+
+        /**
+         * @brief Get the number of data points that can be written to the logger before an overflow occurs.
+         * 
+         * @return size_t The number of data points.
+         */
+        size_t remaining();
+
+        /**
+         * @brief Get the total number of data points that fit into the logger.
+         * 
+         * @return size_t The number of data points.
+         */
+        size_t length();
+
+        const T& operator[](std::size_t idx);
+    private:
+        W25Qxx * flash_;
+        uint32_t start_;
+        uint32_t end_;
+        uint32_t address_;
+
+        uint8_t buffer_[W25QXX_PAGE_SIZE];
+        uint8_t ptr_;
+    };
+} // namespace sta
+
+#include <sta/utils/logger.tpp>
+
+#endif // STA_UTILS_LOGGER_HPP
\ No newline at end of file
diff --git a/include/sta/utils/logger.tpp b/include/sta/utils/logger.tpp
new file mode 100644
index 0000000..8bc933c
--- /dev/null
+++ b/include/sta/utils/logger.tpp
@@ -0,0 +1,100 @@
+#ifndef STA_UTILS_LOGGER_TPP
+#define STA_UTILS_LOGGER_TPP
+
+#include <sta/debug/debug.hpp>
+#include <sta/drivers/w25qxx.hpp>
+
+namespace sta
+{
+    template <typename T>
+    Logger<T>::Logger(W25Qxx * flash, uint32_t startSec, uint32_t endSec)
+        : flash_{flash},
+          start_{startSec},
+          end_{endSec},
+          address_{start_ * W25QXX_SECTOR_SIZE},
+          buffer_{0x00, },
+          ptr_ {0}
+    {
+        STA_ASSERT(flash != nullptr);
+        STA_ASSERT(endAddr > startAddr);
+    }
+
+    template <typename T>
+    void Logger<T>::write(T data)
+    {
+        uint8_t * bytes = (uint8_t*)(&data);
+        uint8_t length = sizeof(data);
+
+        // Bytes remaining until the page is full.
+        uint8_t remaining = W25QXX_PAGE_SIZE - ptr_;
+
+        // If the written data exceeds the remaining bytes in the page.
+        if (ptr_ + length >= W25QXX_PAGE_SIZE)
+        {
+            std::memcpy(buffer_ + ptr_, bytes, remaining);
+            flash_->pageProgram(address_, buffer_, W25QXX_PAGE_SIZE);
+
+            bytes += remaining;
+            length -= remaining;
+            ptr_ = 0;
+
+            address_ += W25QXX_PAGE_SIZE;
+
+            // If a new sector was started, erase the new sector.
+            if (address_ % W25QXX_SECTOR_SIZE == 0)
+            {
+                // If the size of the logger segment was exceeded, restart at the first sector.
+                if (address_ / W25QXX_SECTOR_SIZE == end_)
+                {
+                    address_ = start_ * W25QXX_SECTOR_SIZE;
+                }
+
+                flash_->sectorErase(address_);
+            }
+        }
+
+        std::memcpy(buffer_ + ptr_, bytes, length);
+    }
+
+    template <typename T>
+    void Logger<T>::clear()
+    {
+        address_ = start_ * W25QXX_SECTOR_SIZE; 
+        ptr_ = 0;
+
+        for (uint32_t i = 0; i < end_ - start_; i++)
+        {
+            flash_->sectorErase((start_ + i) * W25QXX_SECTOR_SIZE);
+        }
+    }
+
+    template <typename T>
+    size_t Logger<T>::occupied()
+    {
+        return (address_ - start_ * W25QXX_SECTOR_SIZE) / sizeof(T);
+    }
+
+    template <typename T>
+    size_t Logger<T>::remaining()
+    {
+        return (end_  * W25QXX_SECTOR_SIZE - address_) / sizeof(T);
+    }
+
+    template <typename T>
+    size_t Logger<T>::length()
+    {
+        return (end_ - start_) * W25QXX_SECTOR_SIZE / sizeof(T);
+    }
+
+    template <typename T>
+    const T& Logger<T>::operator[](std::size_t idx)
+    {
+        uint32_t address = start_ * W25QXX_SECTOR_SIZE + idx * sizeof(T);
+        uint8_t buffer[sizeof(T)];
+        flash_->readData(address, buffer, sizeof(T));
+
+        return *((T*)buffer);
+    }
+} // namespace sta
+
+#endif // STA_UTILS_LOGGER_TPP

From fde868c9db44bea0323b33e737887dbd82ffc964 Mon Sep 17 00:00:00 2001
From: dario <dario.vandenboom@arcor.de>
Date: Thu, 30 May 2024 12:00:39 +0200
Subject: [PATCH 02/13] Updated logger

---
 include/sta/utils/logger.hpp | 12 +++++++++---
 1 file changed, 9 insertions(+), 3 deletions(-)

diff --git a/include/sta/utils/logger.hpp b/include/sta/utils/logger.hpp
index 5c5120f..a8bcd14 100644
--- a/include/sta/utils/logger.hpp
+++ b/include/sta/utils/logger.hpp
@@ -15,9 +15,9 @@ namespace sta
         /**
          * @brief Construct a new Logger object
          * 
-         * @param flash 
-         * @param startAddr 
-         * @param endAddr 
+         * @param flash The flash chip to use for logging.
+         * @param startSector The index of the start sector (4096 bytes).
+         * @param endAddr The index of the end sector (4096 bytes).
          */
         Logger(W25Qxx * flash, uint32_t startSector, uint32_t endSector);
 
@@ -56,6 +56,12 @@ namespace sta
          */
         size_t length();
 
+        /**
+         * @brief Allows reading a single data point from the segment.
+         * 
+         * @param idx The index of the segement.
+         * @return const T& The read data value.
+         */
         const T& operator[](std::size_t idx);
     private:
         W25Qxx * flash_;

From 9dd2a8da9cb660cb75cde3aaea60eb6b0af1ca39 Mon Sep 17 00:00:00 2001
From: dario <dario.vandenboom@arcor.de>
Date: Mon, 3 Jun 2024 22:23:00 +0200
Subject: [PATCH 03/13] Successful reading and writing

---
 include/sta/utils/logger.hpp |  8 ++++----
 include/sta/utils/logger.tpp | 28 +++++++++++++++++++++-------
 2 files changed, 25 insertions(+), 11 deletions(-)

diff --git a/include/sta/utils/logger.hpp b/include/sta/utils/logger.hpp
index a8bcd14..9f1be34 100644
--- a/include/sta/utils/logger.hpp
+++ b/include/sta/utils/logger.hpp
@@ -13,11 +13,11 @@ namespace sta
     {
     public:
         /**
-         * @brief Construct a new Logger object
+         * @brief Constructs a logger object which manages reading and writing data to a segment of a flash chip.
          * 
          * @param flash The flash chip to use for logging.
-         * @param startSector The index of the start sector (4096 bytes).
-         * @param endAddr The index of the end sector (4096 bytes).
+         * @param startSector The index of the start sector (1 LSB = 4096 bytes).
+         * @param endSector The index of the end sector (1 LSB = 4096 bytes).
          */
         Logger(W25Qxx * flash, uint32_t startSector, uint32_t endSector);
 
@@ -62,7 +62,7 @@ namespace sta
          * @param idx The index of the segement.
          * @return const T& The read data value.
          */
-        const T& operator[](std::size_t idx);
+        T operator[](std::size_t idx);
     private:
         W25Qxx * flash_;
         uint32_t start_;
diff --git a/include/sta/utils/logger.tpp b/include/sta/utils/logger.tpp
index 8bc933c..7078895 100644
--- a/include/sta/utils/logger.tpp
+++ b/include/sta/utils/logger.tpp
@@ -23,11 +23,13 @@ namespace sta
     void Logger<T>::write(T data)
     {
         uint8_t * bytes = (uint8_t*)(&data);
-        uint8_t length = sizeof(data);
+        uint8_t length = sizeof(T);
 
         // Bytes remaining until the page is full.
         uint8_t remaining = W25QXX_PAGE_SIZE - ptr_;
 
+        STA_DEBUG_PRINTF("Writing at address %d\n", address_+ptr_);
+
         // If the written data exceeds the remaining bytes in the page.
         if (ptr_ + length >= W25QXX_PAGE_SIZE)
         {
@@ -37,6 +39,7 @@ namespace sta
             bytes += remaining;
             length -= remaining;
             ptr_ = 0;
+            STA_DEBUG_PRINTLN("New PAGE!");
 
             address_ += W25QXX_PAGE_SIZE;
 
@@ -54,6 +57,7 @@ namespace sta
         }
 
         std::memcpy(buffer_ + ptr_, bytes, length);
+        ptr_ += length;
     }
 
     template <typename T>
@@ -62,9 +66,9 @@ namespace sta
         address_ = start_ * W25QXX_SECTOR_SIZE; 
         ptr_ = 0;
 
-        for (uint32_t i = 0; i < end_ - start_; i++)
+        for (uint32_t i = start_; i < end_; i++)
         {
-            flash_->sectorErase((start_ + i) * W25QXX_SECTOR_SIZE);
+            flash_->sectorErase(i * W25QXX_SECTOR_SIZE);
         }
     }
 
@@ -87,13 +91,23 @@ namespace sta
     }
 
     template <typename T>
-    const T& Logger<T>::operator[](std::size_t idx)
+    T Logger<T>::operator[](std::size_t idx)
     {
         uint32_t address = start_ * W25QXX_SECTOR_SIZE + idx * sizeof(T);
-        uint8_t buffer[sizeof(T)];
-        flash_->readData(address, buffer, sizeof(T));
 
-        return *((T*)buffer);
+        // If the requested element is in the cache, read it from there.
+        if (address / W25QXX_PAGE_SIZE == address_ / W25QXX_PAGE_SIZE)
+        {
+            STA_DEBUG_PRINTLN("Reading data from buffer!");
+            uint8_t * ptr = buffer_ + address % W25QXX_PAGE_SIZE;
+            return *reinterpret_cast<T*>(ptr);
+        }
+        else
+        {
+            uint8_t buffer[sizeof(T)];
+            flash_->readData(address, buffer, sizeof(T));
+            return *reinterpret_cast<T*>(buffer);
+        }
     }
 } // namespace sta
 

From 007ed4636374565ed650effb55b743cedf541623 Mon Sep 17 00:00:00 2001
From: dario <dario.vandenboom@arcor.de>
Date: Mon, 3 Jun 2024 22:49:04 +0200
Subject: [PATCH 04/13] Added mutex and more doxygen

---
 include/sta/drivers/w25qxx.hpp | 13 +++++++++----
 include/sta/utils/logger.tpp   | 16 ++++++++--------
 src/w25qxx.cpp                 |  4 +++-
 3 files changed, 20 insertions(+), 13 deletions(-)

diff --git a/include/sta/drivers/w25qxx.hpp b/include/sta/drivers/w25qxx.hpp
index 9d93c20..7031b7e 100644
--- a/include/sta/drivers/w25qxx.hpp
+++ b/include/sta/drivers/w25qxx.hpp
@@ -2,6 +2,8 @@
 #define STA_SENSORS_W25Q128_HPP
 
 #include <sta/bus/spi/device.hpp>
+#include <sta/mutex.hpp>
+
 #include <sta/drivers/w25qxx_defs.hpp>
 
 #include <functional>
@@ -60,12 +62,14 @@ namespace sta
         using DelayUsFunc = void (*)(uint32_t);
 
         /**
-         * @brief Construct a new W25Qxx object
+         * @brief Driver class for the W25QXX flash storage series.
          * 
-         * @param device 
-         * @param addrMode 
+         * @param device A SPI device handle from sta-core.
+         * @param delay A microsecond delay function.
+         * @param addrMode Choose between 24 Bit and 32 Bit addressing.
+         * @param mutex A mutex for thread safety if the flash chip is used by multiple threads. Defaults to a always free mutex.
          */
-        W25Qxx(SPIDevice * device, DelayUsFunc delay, AddressMode addrMode = AddressMode::_24BIT);
+        W25Qxx(SPIDevice * device, DelayUsFunc delay, AddressMode addrMode = AddressMode::_24BIT, Mutex * mutex = Mutex::ALWAYS_FREE);
 
         /**
          * @brief Initialize the flash chip.
@@ -318,6 +322,7 @@ namespace sta
     private:
         SPIDevice * device_;
         DelayUsFunc delay_;
+        Mutex * mutex_;
         ChipState state_;
         AddressMode addrMode_;
     };
diff --git a/include/sta/utils/logger.tpp b/include/sta/utils/logger.tpp
index 7078895..6816ff9 100644
--- a/include/sta/utils/logger.tpp
+++ b/include/sta/utils/logger.tpp
@@ -33,6 +33,12 @@ namespace sta
         // If the written data exceeds the remaining bytes in the page.
         if (ptr_ + length >= W25QXX_PAGE_SIZE)
         {
+            // If the segment is full, stop writing data to it.
+            if (address_ / W25QXX_SECTOR_SIZE == end_)
+            {
+                return;
+            }
+
             std::memcpy(buffer_ + ptr_, bytes, remaining);
             flash_->pageProgram(address_, buffer_, W25QXX_PAGE_SIZE);
 
@@ -43,15 +49,9 @@ namespace sta
 
             address_ += W25QXX_PAGE_SIZE;
 
-            // If a new sector was started, erase the new sector.
+            // If a new sector has to be started, erase the new sector.
             if (address_ % W25QXX_SECTOR_SIZE == 0)
             {
-                // If the size of the logger segment was exceeded, restart at the first sector.
-                if (address_ / W25QXX_SECTOR_SIZE == end_)
-                {
-                    address_ = start_ * W25QXX_SECTOR_SIZE;
-                }
-
                 flash_->sectorErase(address_);
             }
         }
@@ -81,7 +81,7 @@ namespace sta
     template <typename T>
     size_t Logger<T>::remaining()
     {
-        return (end_  * W25QXX_SECTOR_SIZE - address_) / sizeof(T);
+        return (end_ * W25QXX_SECTOR_SIZE - address_) / sizeof(T);
     }
 
     template <typename T>
diff --git a/src/w25qxx.cpp b/src/w25qxx.cpp
index 3351bbe..f8654d2 100644
--- a/src/w25qxx.cpp
+++ b/src/w25qxx.cpp
@@ -9,13 +9,15 @@
 
 namespace sta
 {
-    W25Qxx::W25Qxx(SPIDevice * device, DelayUsFunc delay, AddressMode addrMode /* = AddressMode::_24BIT */)
+    W25Qxx::W25Qxx(SPIDevice * device, DelayUsFunc delay, AddressMode addrMode /* = AddressMode::_24BIT */, Mutex * mutex /* = Mutex::ALWAYS_FREE */)
         : device_{device},
           delay_{delay},
+          mutex_{mutex},
           state_{ChipState::POWERED_DOWN},
           addrMode_{addrMode}
     {
         STA_ASSERT(device != nullptr);
+        STA_ASSERT(mutex != nullptr);
     }
 
     uint8_t W25Qxx::init()

From a15b2398ab507c5199fba61e0f034f05d7cff793 Mon Sep 17 00:00:00 2001
From: dario <dario.vandenboom@arcor.de>
Date: Tue, 4 Jun 2024 16:00:18 +0200
Subject: [PATCH 05/13] Added mutex protection and re-arranged some methods

---
 include/sta/drivers/w25qxx.hpp | 78 ++++++++++++++++++----------------
 src/w25qxx.cpp                 | 29 +++++++++++--
 2 files changed, 66 insertions(+), 41 deletions(-)

diff --git a/include/sta/drivers/w25qxx.hpp b/include/sta/drivers/w25qxx.hpp
index 7031b7e..4682f02 100644
--- a/include/sta/drivers/w25qxx.hpp
+++ b/include/sta/drivers/w25qxx.hpp
@@ -78,18 +78,38 @@ namespace sta
          */
         uint8_t init();
 
+        /**
+         * @brief Checks if the flash is busy writing or erasing.
+         * 
+         * @return bool Returns true if the flash is busy, false otherwise.
+         */
+        bool isBusy();
+
         uint32_t getChunkBytes(ChunkSize size);
 
+        /**
+         * @brief 
+         * 
+         * @note 
+         * 
+         * @param criterion 
+         * @param size 
+         * @return uint32_t 
+         */
+
+
         /**
          * @brief Find the first memory section not satisfying a given criterion using a binary search. 
          * 
-         * @note This function assume that there is a page n such that every page before n satisfies the criterion, while every page after that doesn't
+         * @note This function assumes that there is a page n such that every page before n satisfies the criterion, while every page after that doesn't.
          * 
          * @param criterion A function evaluating the criterion on a page.
          * @param size The size of the memory section. Has to be one of the predefined sizes.
+         * @param startAddr The start address of the segment to search. Defaults to the chip's start address.
+         * @param endAddr The end address of the segment to search. Defaults to the chip's end address.
          * @return uint32_t The last address such that the criterion is satisfied.
          */
-        uint32_t findLast(std::function<bool(uint8_t*)> criterion, ChunkSize size);
+        uint32_t findLast(std::function<bool(uint8_t*)> criterion, ChunkSize size, uint32_t startAddr = 0, uint32_t endAddr = W25QXX_32B_MEM_SIZE);
 
         /**
          * @brief Set the Address Mode object
@@ -150,41 +170,7 @@ namespace sta
 
         // reset device
 
-        // Extended address register read / write?
-
-        // Enter 4-Byte address mode
-
-        // Exit 4-Byte address mode
-    public:
-        /*
-         * Status registers.
-         */
-
-        /**
-         * @brief Read one of the flash's status registers.
-         * 
-         * @param regID The ID of the status register. Can only be 1, 2 or 3.
-         * @param status_byte A pointer to the variable to write the state into.
-         * @return uint8_t Returns 1 if successful, 0 otherwise.
-         */
-        uint8_t readStatusRegister(uint8_t regID, uint8_t * status_byte);
-
-        /**
-         * @brief Write into one of the chip's status registers.
-         * 
-         * @param regID The ID of the status register. Can only be 1, 2 or 3.
-         * @param status_byte The byte to write into the status register.
-         * @param nonvolatile If set to true, this setting will be restored after power off.
-         * @return uint8_t Returns 1 if successful, 0 otherwise.
-         */
-        uint8_t writeStatusRegister(uint8_t regID, uint8_t * status_byte, bool nonvolatile = false);
-
-        /**
-         * @brief Checks if the flash is busy writing or erasing.
-         * 
-         * @return bool Returns true if the flash is busy, false otherwise.
-         */
-        bool isBusy();
+        // Extended address register read / write?       
     public:
         /*
          * Read / Write operations
@@ -319,6 +305,24 @@ namespace sta
 
         uint8_t writeVolatileEnable();
 
+        /**
+         * @brief Read one of the flash's status registers.
+         * 
+         * @param regID The ID of the status register. Can only be 1, 2 or 3.
+         * @param status_byte A pointer to the variable to write the state into.
+         * @return uint8_t Returns 1 if successful, 0 otherwise.
+         */
+        uint8_t readStatusRegister(uint8_t regID, uint8_t * status_byte);
+
+        /**
+         * @brief Write into one of the chip's status registers.
+         * 
+         * @param regID The ID of the status register. Can only be 1, 2 or 3.
+         * @param status_byte The byte to write into the status register.
+         * @param nonvolatile If set to true, this setting will be restored after power off.
+         * @return uint8_t Returns 1 if successful, 0 otherwise.
+         */
+        uint8_t writeStatusRegister(uint8_t regID, uint8_t * status_byte, bool nonvolatile = false);
     private:
         SPIDevice * device_;
         DelayUsFunc delay_;
diff --git a/src/w25qxx.cpp b/src/w25qxx.cpp
index f8654d2..1518bae 100644
--- a/src/w25qxx.cpp
+++ b/src/w25qxx.cpp
@@ -22,6 +22,8 @@ namespace sta
 
     uint8_t W25Qxx::init()
     {
+        lock_guard<Mutex>(*mutex_);
+
     	powerDown();
         delay_(5);
 
@@ -70,12 +72,15 @@ namespace sta
         }
     }
 
-    uint32_t W25Qxx::findLast(std::function<bool(uint8_t*)> criterion, ChunkSize size)
+    uint32_t W25Qxx::findLast(std::function<bool(uint8_t*)> criterion, ChunkSize size, uint32_t startAddr /*= 0 */, uint32_t endAddr /* = W25QXX_32B_MEM_SIZE */)
     {
+        STA_ASSERT(startAddr <= endAddr);
+
+        lock_guard<Mutex>(*mutex_);
         uint32_t bytes = getChunkBytes(size);
 
-        uint32_t left   = 0;
-        uint32_t right  = (addrMode_ == AddressMode::_32BIT ? W25QXX_32B_MEM_SIZE : W25QXX_24B_MEM_SIZE) / bytes;
+        uint32_t left   = startAddr / bytes;
+        uint32_t right  = endAddr / bytes; // (addrMode_ == AddressMode::_32BIT ? W25QXX_32B_MEM_SIZE : W25QXX_24B_MEM_SIZE) / bytes;
         uint32_t middle;
 
         uint8_t * buffer = new uint8_t[bytes];
@@ -83,7 +88,6 @@ namespace sta
         while (left < right)
         {
             middle = (left + right) / 2;
-            STA_DEBUG_PRINTF("left=%d, middle=%d, right=%d", left, middle, right);
             readData(middle * bytes, buffer, bytes);
 
             if (criterion(buffer))
@@ -111,6 +115,7 @@ namespace sta
 
     uint8_t W25Qxx::setAddressMode(AddressMode addrMode)
     {
+        lock_guard<Mutex>(*mutex_);
         busWrite(W25QXX_4_BYTE_ADDR_ENABLE);
 
         while (isBusy()) {}
@@ -120,6 +125,8 @@ namespace sta
 
     AddressMode W25Qxx::getAddressMode()
     {   
+        lock_guard<Mutex>(*mutex_);
+
         uint8_t status;
         readStatusRegister(3, &status);
 
@@ -128,6 +135,8 @@ namespace sta
 
     uint8_t W25Qxx::getChipID()
     {
+        lock_guard<Mutex>(*mutex_);
+
         uint8_t buffer[4];
         busRead(W25QXX_RELEASE_POWER_DOWN, buffer, 3);
 
@@ -136,6 +145,8 @@ namespace sta
 
     uint8_t W25Qxx::getManufacturerID()
     {
+        lock_guard<Mutex>(*mutex_);
+
         uint8_t dummy[3] = {0, 0, 0};
         uint8_t id[2] = {0, 0};
 
@@ -146,6 +157,8 @@ namespace sta
 
     uint64_t W25Qxx::getUniqueID()
     {
+        lock_guard<Mutex>(*mutex_);
+
         uint8_t dummy[4];
         uint8_t id[8];
 
@@ -163,6 +176,8 @@ namespace sta
 
     bool W25Qxx::isBusy()
     {
+        lock_guard<Mutex>(*mutex_);
+
         uint8_t status = 0;
         readStatusRegister(1, &status);
 
@@ -171,6 +186,8 @@ namespace sta
 
     uint8_t W25Qxx::readData(uint32_t address, uint8_t * buffer, size_t length, bool fast /* = true */)
     {
+        lock_guard<Mutex>(*mutex_);
+
         uint8_t instruction = fast ? W25QXX_FAST_READ : W25QXX_READ;
 
         // In fast mode we have to send a 8 dummy clock cycles first.
@@ -221,6 +238,8 @@ namespace sta
 
     uint8_t W25Qxx::pageProgram(uint32_t address, uint8_t * buffer, size_t length)
     {
+        lock_guard<Mutex>(*mutex_);
+
         STA_ASSERT(length <= W25QXX_PAGE_SIZE);
     	while (isBusy()) {}
 
@@ -256,6 +275,8 @@ namespace sta
 
     uint8_t W25Qxx::sectorProgram(uint32_t address, uint8_t * buffer, size_t length)
     {
+        lock_guard<Mutex>(*mutex_);
+
         STA_ASSERT(length <= W25QXX_SECTOR_SIZE);
 
         uint32_t nPages = length / W25QXX_PAGE_SIZE;

From 33e034a429215af455a638c221ba7570615865e1 Mon Sep 17 00:00:00 2001
From: dario <dario.vandenboom@arcor.de>
Date: Tue, 4 Jun 2024 18:40:48 +0200
Subject: [PATCH 06/13] Updated logging, binary search to find starting point.

---
 include/sta/drivers/w25qxx_defs.hpp |  1 +
 include/sta/utils/logger.hpp        | 15 ++++---
 include/sta/utils/logger.tpp        | 61 ++++++++++++++++++++---------
 src/w25qxx.cpp                      |  3 ++
 4 files changed, 56 insertions(+), 24 deletions(-)

diff --git a/include/sta/drivers/w25qxx_defs.hpp b/include/sta/drivers/w25qxx_defs.hpp
index 64fc3b5..8f0b86a 100644
--- a/include/sta/drivers/w25qxx_defs.hpp
+++ b/include/sta/drivers/w25qxx_defs.hpp
@@ -61,6 +61,7 @@
 
 #define W25QXX_PAGE_SIZE            0x100
 #define W25QXX_SECTOR_SIZE          0x1000
+#define W25QXX_PAGE_PER_SECTOR      0x10
 #define W25QXX_BLOCK_32_KB_SIZE     0x8000
 #define W25QXX_BLOCK_64_KB_SIZE     0xF000
 
diff --git a/include/sta/utils/logger.hpp b/include/sta/utils/logger.hpp
index 9f1be34..4288a80 100644
--- a/include/sta/utils/logger.hpp
+++ b/include/sta/utils/logger.hpp
@@ -20,14 +20,15 @@ namespace sta
          * @param endSector The index of the end sector (1 LSB = 4096 bytes).
          */
         Logger(W25Qxx * flash, uint32_t startSector, uint32_t endSector);
-
+        
         /**
          * @brief Write a new data point to the flash chip. 
          * @note If the total capacity of this logger was exceeded, it restarts at the first sector, overwriting its data.
          * 
-         * @param data The data to write to the flash chip.
+         * @param data The data to write to the flash chip. 
+         * @return true if successful, false if the segment end was reached.
          */
-        void write(T data);
+        bool write(T data);
 
         /**
          * @brief Clear the flash memory used by the logger.
@@ -40,7 +41,7 @@ namespace sta
          * 
          * @return size_t The number of data points.
          */
-        size_t occupied();
+        size_t count();
 
         /**
          * @brief Get the number of data points that can be written to the logger before an overflow occurs.
@@ -54,16 +55,18 @@ namespace sta
          * 
          * @return size_t The number of data points.
          */
-        size_t length();
+        size_t capacity();
 
         /**
          * @brief Allows reading a single data point from the segment.
          * 
          * @param idx The index of the segement.
-         * @return const T& The read data value.
+         * @return T The read data value.
          */
         T operator[](std::size_t idx);
     private:
+        void findLast();
+
         W25Qxx * flash_;
         uint32_t start_;
         uint32_t end_;
diff --git a/include/sta/utils/logger.tpp b/include/sta/utils/logger.tpp
index 6816ff9..c0885b1 100644
--- a/include/sta/utils/logger.tpp
+++ b/include/sta/utils/logger.tpp
@@ -17,26 +17,52 @@ namespace sta
     {
         STA_ASSERT(flash != nullptr);
         STA_ASSERT(endAddr > startAddr);
+
+        // Jump to the last written page.
+        findLast();
     }
 
     template <typename T>
-    void Logger<T>::write(T data)
+    void Logger<T>::findLast()
     {
-        uint8_t * bytes = (uint8_t*)(&data);
+        address_ = this->flash_->findLast([](uint8_t * buffer) -> bool {
+            for (size_t i = 0; i < W25QXX_PAGE_SIZE; i++)
+            {
+                if (buffer[i] != 0xFF)
+                {
+                    return true;
+                }
+            }
+
+            return false;
+        }, sta::ChunkSize::PAGE, start_ * W25QXX_SECTOR_SIZE, end_ * W25QXX_SECTOR_SIZE);
+
+        STA_DEBUG_PRINTF("Starting at page with address %d\n", address_);
+    }
+
+    template <typename T>
+    bool Logger<T>::write(T data)
+    {
+        // If writing the data would exceed the segment length, return false and don't do anything.
+        if ((address_ + ptr_ + sizeof(T)) / W25QXX_SECTOR_SIZE == end_)
+            return false;
+
+        // Convert the data to a byte array.
+        uint8_t * bytes = reinterpret_cast<uint8_t*>(&data);
         uint8_t length = sizeof(T);
 
         // Bytes remaining until the page is full.
         uint8_t remaining = W25QXX_PAGE_SIZE - ptr_;
 
-        STA_DEBUG_PRINTF("Writing at address %d\n", address_+ptr_);
+        STA_DEBUG_PRINTF("Writing at address %d\n", address_ + ptr_);
 
         // If the written data exceeds the remaining bytes in the page.
         if (ptr_ + length >= W25QXX_PAGE_SIZE)
         {
-            // If the segment is full, stop writing data to it.
-            if (address_ / W25QXX_SECTOR_SIZE == end_)
+            // If the page to written is in a new sector, erase the new sector before writing to it.
+            if (address_ % W25QXX_SECTOR_SIZE == 0)
             {
-                return;
+                flash_->sectorErase(address_);
             }
 
             std::memcpy(buffer_ + ptr_, bytes, remaining);
@@ -45,19 +71,17 @@ namespace sta
             bytes += remaining;
             length -= remaining;
             ptr_ = 0;
-            STA_DEBUG_PRINTLN("New PAGE!");
-
             address_ += W25QXX_PAGE_SIZE;
-
-            // If a new sector has to be started, erase the new sector.
-            if (address_ % W25QXX_SECTOR_SIZE == 0)
-            {
-                flash_->sectorErase(address_);
-            }
         }
 
         std::memcpy(buffer_ + ptr_, bytes, length);
         ptr_ += length;
+
+        // If the end of the segment was reached, flush the temporally stored data.
+        if ((address_ + ptr_ + sizeof(T)) / W25QXX_SECTOR_SIZE == end_)
+            flash_->pageProgram(address_, buffer_, W25QXX_PAGE_SIZE);
+
+        return true;
     }
 
     template <typename T>
@@ -73,19 +97,19 @@ namespace sta
     }
 
     template <typename T>
-    size_t Logger<T>::occupied()
+    size_t Logger<T>::count()
     {
-        return (address_ - start_ * W25QXX_SECTOR_SIZE) / sizeof(T);
+        return ((address_ + ptr_) - start_ * W25QXX_SECTOR_SIZE) / sizeof(T);
     }
 
     template <typename T>
     size_t Logger<T>::remaining()
     {
-        return (end_ * W25QXX_SECTOR_SIZE - address_) / sizeof(T);
+        return (end_ * W25QXX_SECTOR_SIZE - (address_ + ptr_)) / sizeof(T);
     }
 
     template <typename T>
-    size_t Logger<T>::length()
+    size_t Logger<T>::capacity()
     {
         return (end_ - start_) * W25QXX_SECTOR_SIZE / sizeof(T);
     }
@@ -94,6 +118,7 @@ namespace sta
     T Logger<T>::operator[](std::size_t idx)
     {
         uint32_t address = start_ * W25QXX_SECTOR_SIZE + idx * sizeof(T);
+        STA_DEBUG_PRINTF("Reading from address %d\n", address);
 
         // If the requested element is in the cache, read it from there.
         if (address / W25QXX_PAGE_SIZE == address_ / W25QXX_PAGE_SIZE)
diff --git a/src/w25qxx.cpp b/src/w25qxx.cpp
index 1518bae..15031d4 100644
--- a/src/w25qxx.cpp
+++ b/src/w25qxx.cpp
@@ -88,6 +88,7 @@ namespace sta
         while (left < right)
         {
             middle = (left + right) / 2;
+            STA_DEBUG_PRINTF("left=%d, middle=%d, right=%d", left, middle, right);
             readData(middle * bytes, buffer, bytes);
 
             if (criterion(buffer))
@@ -101,11 +102,13 @@ namespace sta
         }
 
         middle = (left + right) / 2;
+        STA_DEBUG_PRINTLN(middle);
 
         readData(middle * bytes, buffer, bytes);
         if (criterion(buffer))
         {
             middle += 1;
+            STA_DEBUG_PRINTLN("PLUS ONE");
         }
 
         delete[] buffer;

From d378bda62cf82b41ab81bbdf1bcb7624013a1e2c Mon Sep 17 00:00:00 2001
From: dario <dario.vandenboom@arcor.de>
Date: Tue, 4 Jun 2024 22:05:23 +0200
Subject: [PATCH 07/13] cleanup in logger code

---
 include/sta/utils/logger.hpp | 16 ++++++++++++----
 include/sta/utils/logger.tpp | 12 ++++++++----
 2 files changed, 20 insertions(+), 8 deletions(-)

diff --git a/include/sta/utils/logger.hpp b/include/sta/utils/logger.hpp
index 4288a80..5491e23 100644
--- a/include/sta/utils/logger.hpp
+++ b/include/sta/utils/logger.hpp
@@ -58,12 +58,20 @@ namespace sta
         size_t capacity();
 
         /**
-         * @brief Allows reading a single data point from the segment.
+         * @brief Get the ith element stored in the flash storage.
          * 
-         * @param idx The index of the segement.
-         * @return T The read data value.
+         * @param i The index of the element to read. 
+         * @return T The ith element stored in the flash storage.
          */
-        T operator[](std::size_t idx);
+        T get(std::size_t i);
+
+        /**
+         * @brief Get the ith element stored in the flash storage.
+         * 
+         * @param i The index of the element to read. 
+         * @return T The ith element stored in the flash storage.
+         */
+        T operator[](std::size_t i);
     private:
         void findLast();
 
diff --git a/include/sta/utils/logger.tpp b/include/sta/utils/logger.tpp
index c0885b1..cad2da6 100644
--- a/include/sta/utils/logger.tpp
+++ b/include/sta/utils/logger.tpp
@@ -115,15 +115,13 @@ namespace sta
     }
 
     template <typename T>
-    T Logger<T>::operator[](std::size_t idx)
+    T Logger<T>::get(std::size_t i)
     {
-        uint32_t address = start_ * W25QXX_SECTOR_SIZE + idx * sizeof(T);
-        STA_DEBUG_PRINTF("Reading from address %d\n", address);
+        uint32_t address = start_ * W25QXX_SECTOR_SIZE + i * sizeof(T);
 
         // If the requested element is in the cache, read it from there.
         if (address / W25QXX_PAGE_SIZE == address_ / W25QXX_PAGE_SIZE)
         {
-            STA_DEBUG_PRINTLN("Reading data from buffer!");
             uint8_t * ptr = buffer_ + address % W25QXX_PAGE_SIZE;
             return *reinterpret_cast<T*>(ptr);
         }
@@ -134,6 +132,12 @@ namespace sta
             return *reinterpret_cast<T*>(buffer);
         }
     }
+
+    template <typename T>
+    T Logger<T>::operator[](std::size_t i)
+    {
+        return this->get(i);
+    }
 } // namespace sta
 
 #endif // STA_UTILS_LOGGER_TPP

From 9fc8bc2b45317f9a86bb06502d13a70dc0a83c20 Mon Sep 17 00:00:00 2001
From: dario <dario.vandenboom@arcor.de>
Date: Tue, 4 Jun 2024 22:51:45 +0200
Subject: [PATCH 08/13] Removed multiple debug prints

---
 include/sta/utils/logger.tpp | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/include/sta/utils/logger.tpp b/include/sta/utils/logger.tpp
index cad2da6..5eebdd3 100644
--- a/include/sta/utils/logger.tpp
+++ b/include/sta/utils/logger.tpp
@@ -54,8 +54,6 @@ namespace sta
         // Bytes remaining until the page is full.
         uint8_t remaining = W25QXX_PAGE_SIZE - ptr_;
 
-        STA_DEBUG_PRINTF("Writing at address %d\n", address_ + ptr_);
-
         // If the written data exceeds the remaining bytes in the page.
         if (ptr_ + length >= W25QXX_PAGE_SIZE)
         {
@@ -89,11 +87,16 @@ namespace sta
     {
         address_ = start_ * W25QXX_SECTOR_SIZE; 
         ptr_ = 0;
+    
+        flash_->sectorErase(address_);
 
+        /*
         for (uint32_t i = start_; i < end_; i++)
         {
             flash_->sectorErase(i * W25QXX_SECTOR_SIZE);
         }
+
+        */
     }
 
     template <typename T>

From d6b16dbff1236c7377e54c65a71f308be831e3a1 Mon Sep 17 00:00:00 2001
From: Lars Wilko Sentse <lars.wilko.sentse@rwth-aachen.de>
Date: Sun, 9 Jun 2024 12:32:13 +0200
Subject: [PATCH 09/13] fixed assert for start and end sector

---
 include/sta/drivers/w25qxx.hpp |  3 ++-
 include/sta/utils/logger.tpp   |  2 +-
 src/w25qxx.cpp                 | 22 +++++++++++-----------
 3 files changed, 14 insertions(+), 13 deletions(-)

diff --git a/include/sta/drivers/w25qxx.hpp b/include/sta/drivers/w25qxx.hpp
index 4682f02..3e8e962 100644
--- a/include/sta/drivers/w25qxx.hpp
+++ b/include/sta/drivers/w25qxx.hpp
@@ -2,6 +2,7 @@
 #define STA_SENSORS_W25Q128_HPP
 
 #include <sta/bus/spi/device.hpp>
+//#include <sta/devices/stm32/bus/spi.hpp>
 #include <sta/mutex.hpp>
 
 #include <sta/drivers/w25qxx_defs.hpp>
@@ -333,4 +334,4 @@ namespace sta
 } // namespace sta
 
 
-#endif // STA_SENSORS_W25Q128_HPP
\ No newline at end of file
+#endif // STA_SENSORS_W25Q128_HPP
diff --git a/include/sta/utils/logger.tpp b/include/sta/utils/logger.tpp
index 5eebdd3..4c1501c 100644
--- a/include/sta/utils/logger.tpp
+++ b/include/sta/utils/logger.tpp
@@ -16,7 +16,7 @@ namespace sta
           ptr_ {0}
     {
         STA_ASSERT(flash != nullptr);
-        STA_ASSERT(endAddr > startAddr);
+        STA_ASSERT(endSec > startSec);
 
         // Jump to the last written page.
         findLast();
diff --git a/src/w25qxx.cpp b/src/w25qxx.cpp
index 15031d4..3fd777c 100644
--- a/src/w25qxx.cpp
+++ b/src/w25qxx.cpp
@@ -22,7 +22,7 @@ namespace sta
 
     uint8_t W25Qxx::init()
     {
-        lock_guard<Mutex>(*mutex_);
+        lock_guard<Mutex>lock(*mutex_);
 
     	powerDown();
         delay_(5);
@@ -76,7 +76,7 @@ namespace sta
     {
         STA_ASSERT(startAddr <= endAddr);
 
-        lock_guard<Mutex>(*mutex_);
+        lock_guard<Mutex>lock(*mutex_);
         uint32_t bytes = getChunkBytes(size);
 
         uint32_t left   = startAddr / bytes;
@@ -118,7 +118,7 @@ namespace sta
 
     uint8_t W25Qxx::setAddressMode(AddressMode addrMode)
     {
-        lock_guard<Mutex>(*mutex_);
+        lock_guard<Mutex>lock(*mutex_);
         busWrite(W25QXX_4_BYTE_ADDR_ENABLE);
 
         while (isBusy()) {}
@@ -128,7 +128,7 @@ namespace sta
 
     AddressMode W25Qxx::getAddressMode()
     {   
-        lock_guard<Mutex>(*mutex_);
+        lock_guard<Mutex>lock(*mutex_);
 
         uint8_t status;
         readStatusRegister(3, &status);
@@ -138,7 +138,7 @@ namespace sta
 
     uint8_t W25Qxx::getChipID()
     {
-        lock_guard<Mutex>(*mutex_);
+        lock_guard<Mutex>lock(*mutex_);
 
         uint8_t buffer[4];
         busRead(W25QXX_RELEASE_POWER_DOWN, buffer, 3);
@@ -148,7 +148,7 @@ namespace sta
 
     uint8_t W25Qxx::getManufacturerID()
     {
-        lock_guard<Mutex>(*mutex_);
+        lock_guard<Mutex>lock(*mutex_);
 
         uint8_t dummy[3] = {0, 0, 0};
         uint8_t id[2] = {0, 0};
@@ -160,7 +160,7 @@ namespace sta
 
     uint64_t W25Qxx::getUniqueID()
     {
-        lock_guard<Mutex>(*mutex_);
+        lock_guard<Mutex>lock(*mutex_);
 
         uint8_t dummy[4];
         uint8_t id[8];
@@ -179,7 +179,7 @@ namespace sta
 
     bool W25Qxx::isBusy()
     {
-        lock_guard<Mutex>(*mutex_);
+        lock_guard<Mutex>lock(*mutex_);
 
         uint8_t status = 0;
         readStatusRegister(1, &status);
@@ -189,7 +189,7 @@ namespace sta
 
     uint8_t W25Qxx::readData(uint32_t address, uint8_t * buffer, size_t length, bool fast /* = true */)
     {
-        lock_guard<Mutex>(*mutex_);
+        lock_guard<Mutex>lock(*mutex_);
 
         uint8_t instruction = fast ? W25QXX_FAST_READ : W25QXX_READ;
 
@@ -241,7 +241,7 @@ namespace sta
 
     uint8_t W25Qxx::pageProgram(uint32_t address, uint8_t * buffer, size_t length)
     {
-        lock_guard<Mutex>(*mutex_);
+        lock_guard<Mutex>lock(*mutex_);
 
         STA_ASSERT(length <= W25QXX_PAGE_SIZE);
     	while (isBusy()) {}
@@ -278,7 +278,7 @@ namespace sta
 
     uint8_t W25Qxx::sectorProgram(uint32_t address, uint8_t * buffer, size_t length)
     {
-        lock_guard<Mutex>(*mutex_);
+        lock_guard<Mutex>lock(*mutex_);
 
         STA_ASSERT(length <= W25QXX_SECTOR_SIZE);
 

From 8bd5a9d26f29253d28f2291c81d7deb11a4ff873 Mon Sep 17 00:00:00 2001
From: dario <dario.vandenboom@arcor.de>
Date: Tue, 11 Jun 2024 10:24:04 +0200
Subject: [PATCH 10/13] Removed unnecessary debug prints

---
 src/w25qxx.cpp | 3 ---
 1 file changed, 3 deletions(-)

diff --git a/src/w25qxx.cpp b/src/w25qxx.cpp
index 15031d4..1518bae 100644
--- a/src/w25qxx.cpp
+++ b/src/w25qxx.cpp
@@ -88,7 +88,6 @@ namespace sta
         while (left < right)
         {
             middle = (left + right) / 2;
-            STA_DEBUG_PRINTF("left=%d, middle=%d, right=%d", left, middle, right);
             readData(middle * bytes, buffer, bytes);
 
             if (criterion(buffer))
@@ -102,13 +101,11 @@ namespace sta
         }
 
         middle = (left + right) / 2;
-        STA_DEBUG_PRINTLN(middle);
 
         readData(middle * bytes, buffer, bytes);
         if (criterion(buffer))
         {
             middle += 1;
-            STA_DEBUG_PRINTLN("PLUS ONE");
         }
 
         delete[] buffer;

From 789fa4cd3e21c650c46c442d7b66f3673000e241 Mon Sep 17 00:00:00 2001
From: dario <dario.vandenboom@arcor.de>
Date: Fri, 14 Jun 2024 16:39:42 +0200
Subject: [PATCH 11/13] A couple of bugfixes

---
 include/sta/drivers/w25qxx.hpp      |  2 +-
 include/sta/drivers/w25qxx_defs.hpp |  4 +-
 include/sta/utils/logger.hpp        |  4 +-
 include/sta/utils/logger.tpp        | 10 +----
 src/w25qxx.cpp                      | 68 +++++++++++++++--------------
 5 files changed, 42 insertions(+), 46 deletions(-)

diff --git a/include/sta/drivers/w25qxx.hpp b/include/sta/drivers/w25qxx.hpp
index 3e8e962..79c96c0 100644
--- a/include/sta/drivers/w25qxx.hpp
+++ b/include/sta/drivers/w25qxx.hpp
@@ -209,7 +209,7 @@ namespace sta
          * @remarks Afterwards, the device won't accept any instructions for a duration T_SE. This can be checked
          * by reading the busy bit.
          * 
-         * @param address The address of the sector to erase.
+         * @param address The number of the sector to erase. Here, 0 is the first sector, 1 the second and so on.
          * @return bool Returns 1 if the operation was successful, 0 otherwise.
          */
         uint8_t sectorErase(uint32_t address);
diff --git a/include/sta/drivers/w25qxx_defs.hpp b/include/sta/drivers/w25qxx_defs.hpp
index 8f0b86a..897b625 100644
--- a/include/sta/drivers/w25qxx_defs.hpp
+++ b/include/sta/drivers/w25qxx_defs.hpp
@@ -39,12 +39,14 @@
 #define W25QXX_PAGE_PROGAM          0x02
 #define W25QXX_QUAD_PAGE_PROGAM     0x32
 
-#define W25QXX_SECTOR_ERASE         0x21
+#define W25QXX_SECTOR_ERASE         0x20
 #define W25QXX_BLOCK_ERASE_32_KB    0x52
 #define W25QXX_BLOCK_ERASE_64_KB    0xD8
 
 #define W25QXX_READ                 0x03
+#define W25QXX_READ_32_BIT			0x13
 #define W25QXX_FAST_READ            0x0B
+#define W25QXX_FAST_READ_32_BIT		0x0C
 #define W25QXX_FAST_READ_DUAL_OUT   0x3B
 #define W25QXX_FAST_READ_QUAD_OUT   0x6B
 #define W25QXX_SFDP_REG             0x5A
diff --git a/include/sta/utils/logger.hpp b/include/sta/utils/logger.hpp
index 5491e23..8465c67 100644
--- a/include/sta/utils/logger.hpp
+++ b/include/sta/utils/logger.hpp
@@ -81,10 +81,10 @@ namespace sta
         uint32_t address_;
 
         uint8_t buffer_[W25QXX_PAGE_SIZE];
-        uint8_t ptr_;
+        uint32_t ptr_;
     };
 } // namespace sta
 
 #include <sta/utils/logger.tpp>
 
-#endif // STA_UTILS_LOGGER_HPP
\ No newline at end of file
+#endif // STA_UTILS_LOGGER_HPP
diff --git a/include/sta/utils/logger.tpp b/include/sta/utils/logger.tpp
index 4c1501c..f5db5af 100644
--- a/include/sta/utils/logger.tpp
+++ b/include/sta/utils/logger.tpp
@@ -49,7 +49,7 @@ namespace sta
 
         // Convert the data to a byte array.
         uint8_t * bytes = reinterpret_cast<uint8_t*>(&data);
-        uint8_t length = sizeof(T);
+        uint32_t length = sizeof(T);
 
         // Bytes remaining until the page is full.
         uint8_t remaining = W25QXX_PAGE_SIZE - ptr_;
@@ -89,14 +89,6 @@ namespace sta
         ptr_ = 0;
     
         flash_->sectorErase(address_);
-
-        /*
-        for (uint32_t i = start_; i < end_; i++)
-        {
-            flash_->sectorErase(i * W25QXX_SECTOR_SIZE);
-        }
-
-        */
     }
 
     template <typename T>
diff --git a/src/w25qxx.cpp b/src/w25qxx.cpp
index 737b940..66dcbcf 100644
--- a/src/w25qxx.cpp
+++ b/src/w25qxx.cpp
@@ -101,13 +101,6 @@ namespace sta
         }
 
         middle = (left + right) / 2;
-
-        readData(middle * bytes, buffer, bytes);
-        if (criterion(buffer))
-        {
-            middle += 1;
-        }
-
         delete[] buffer;
 
         return middle * bytes;
@@ -190,25 +183,34 @@ namespace sta
 
         uint8_t instruction = fast ? W25QXX_FAST_READ : W25QXX_READ;
 
-        // In fast mode we have to send a 8 dummy clock cycles first.
-        if (fast)
-        {
-            // TODO
-        }
-
         while (isBusy()) {}
 
         // Depending on address mode, send 3 bytes or 4 bytes.
         if (addrMode_ == AddressMode::_32BIT)
         {
-            uint8_t addrBuffer[4] = {
-                (uint8_t) (address >> 24),
-                (uint8_t) (address >> 16),
-                (uint8_t) (address >> 8),
-                (uint8_t) (address)
-            };
+        	if (fast)
+        	{
+        		uint8_t addrBuffer[5] = {
+					(uint8_t) (address >> 24),
+					(uint8_t) (address >> 16),
+					(uint8_t) (address >> 8),
+					(uint8_t) (address),
+					0x00 // Dummy byte for fast mode
+				};
 
-            return busRead(instruction, buffer, length, addrBuffer, 4);
+				return busRead(instruction, buffer, length, addrBuffer, 5);
+        	}
+        	else
+        	{
+        		uint8_t addrBuffer[5] = {
+					(uint8_t) (address >> 24),
+					(uint8_t) (address >> 16),
+					(uint8_t) (address >> 8),
+					(uint8_t) (address)
+				};
+
+				return busRead(instruction, buffer, length, addrBuffer, 4);
+        	}
         }
         else
         {
@@ -433,24 +435,24 @@ namespace sta
 
         if (addrMode_ == AddressMode::_32BIT)
         {
-            uint8_t addrBuffer[4] = {
-                (uint8_t) (address >> 24),
-                (uint8_t) (address >> 16),
-                (uint8_t) (address >> 8),
-                (uint8_t) (address)
-            };
+        	uint8_t addrBuffer[4] = {
+				(uint8_t) (address >> 24),
+				(uint8_t) (address >> 16),
+				(uint8_t) (address >> 8),
+				(uint8_t) (address)
+			};
 
-            return busWrite(W25QXX_SECTOR_ERASE, addrBuffer, 4);
+        	return busWrite(W25QXX_SECTOR_ERASE, addrBuffer, 4);
         }
         else
         {
-            uint8_t addrBuffer[3] = {
-                (uint8_t) (address >> 16),
-                (uint8_t) (address >> 8),
-                (uint8_t) (address)
-            };
+        	uint8_t addrBuffer[3] = {
+				(uint8_t) (address >> 16),
+				(uint8_t) (address >> 8),
+				(uint8_t) (address)
+			};
 
-            return busWrite(W25QXX_SECTOR_ERASE, addrBuffer, 3);
+        	return busWrite(W25QXX_SECTOR_ERASE, addrBuffer, 3);
         }
 
         return 1;

From 231f2ea06d318364b805e454898af47674df472f Mon Sep 17 00:00:00 2001
From: dario <dario.vandenboom@arcor.de>
Date: Sat, 15 Jun 2024 12:42:11 +0200
Subject: [PATCH 12/13] Working logger implementation and some fixes for driver
 code

---
 include/sta/drivers/w25qxx.hpp |  28 +------
 include/sta/utils/logger.hpp   |   7 +-
 include/sta/utils/logger.tpp   | 131 +++++++++++++++++++++++++--------
 src/w25qxx.cpp                 |  72 ++++--------------
 4 files changed, 123 insertions(+), 115 deletions(-)

diff --git a/include/sta/drivers/w25qxx.hpp b/include/sta/drivers/w25qxx.hpp
index 79c96c0..364ed62 100644
--- a/include/sta/drivers/w25qxx.hpp
+++ b/include/sta/drivers/w25qxx.hpp
@@ -86,32 +86,6 @@ namespace sta
          */
         bool isBusy();
 
-        uint32_t getChunkBytes(ChunkSize size);
-
-        /**
-         * @brief 
-         * 
-         * @note 
-         * 
-         * @param criterion 
-         * @param size 
-         * @return uint32_t 
-         */
-
-
-        /**
-         * @brief Find the first memory section not satisfying a given criterion using a binary search. 
-         * 
-         * @note This function assumes that there is a page n such that every page before n satisfies the criterion, while every page after that doesn't.
-         * 
-         * @param criterion A function evaluating the criterion on a page.
-         * @param size The size of the memory section. Has to be one of the predefined sizes.
-         * @param startAddr The start address of the segment to search. Defaults to the chip's start address.
-         * @param endAddr The end address of the segment to search. Defaults to the chip's end address.
-         * @return uint32_t The last address such that the criterion is satisfied.
-         */
-        uint32_t findLast(std::function<bool(uint8_t*)> criterion, ChunkSize size, uint32_t startAddr = 0, uint32_t endAddr = W25QXX_32B_MEM_SIZE);
-
         /**
          * @brief Set the Address Mode object
          * 
@@ -212,7 +186,7 @@ namespace sta
          * @param address The number of the sector to erase. Here, 0 is the first sector, 1 the second and so on.
          * @return bool Returns 1 if the operation was successful, 0 otherwise.
          */
-        uint8_t sectorErase(uint32_t address);
+        uint8_t sectorErase(uint32_t address, bool blocking = false);
 
         /**
          * @brief Sets all memory within a specified block (32/64 KByte) to 1s.
diff --git a/include/sta/utils/logger.hpp b/include/sta/utils/logger.hpp
index 8465c67..deed7a0 100644
--- a/include/sta/utils/logger.hpp
+++ b/include/sta/utils/logger.hpp
@@ -21,6 +21,10 @@ namespace sta
          */
         Logger(W25Qxx * flash, uint32_t startSector, uint32_t endSector);
         
+        void findLast();
+
+        bool write(T* data);
+
         /**
          * @brief Write a new data point to the flash chip. 
          * @note If the total capacity of this logger was exceeded, it restarts at the first sector, overwriting its data.
@@ -73,7 +77,7 @@ namespace sta
          */
         T operator[](std::size_t i);
     private:
-        void findLast();
+        bool searchCriterion(uint8_t * buffer);
 
         W25Qxx * flash_;
         uint32_t start_;
@@ -81,6 +85,7 @@ namespace sta
         uint32_t address_;
 
         uint8_t buffer_[W25QXX_PAGE_SIZE];
+        bool flushed_;
         uint32_t ptr_;
     };
 } // namespace sta
diff --git a/include/sta/utils/logger.tpp b/include/sta/utils/logger.tpp
index f5db5af..5bc4ee4 100644
--- a/include/sta/utils/logger.tpp
+++ b/include/sta/utils/logger.tpp
@@ -13,6 +13,7 @@ namespace sta
           end_{endSec},
           address_{start_ * W25QXX_SECTOR_SIZE},
           buffer_{0x00, },
+          flushed_{false},
           ptr_ {0}
     {
         STA_ASSERT(flash != nullptr);
@@ -23,41 +24,92 @@ namespace sta
     }
 
     template <typename T>
-    void Logger<T>::findLast()
+    bool Logger<T>::searchCriterion(uint8_t * buffer)
     {
-        address_ = this->flash_->findLast([](uint8_t * buffer) -> bool {
-            for (size_t i = 0; i < W25QXX_PAGE_SIZE; i++)
+        for (size_t i = 0; i < W25QXX_PAGE_SIZE; i++)
+        {
+            if (buffer[i] != 0xFF)
             {
-                if (buffer[i] != 0xFF)
-                {
-                    return true;
-                }
+                return false;
             }
-
-            return false;
-        }, sta::ChunkSize::PAGE, start_ * W25QXX_SECTOR_SIZE, end_ * W25QXX_SECTOR_SIZE);
-
-        STA_DEBUG_PRINTF("Starting at page with address %d\n", address_);
+        }
+        
+        return true;
     }
 
     template <typename T>
-    bool Logger<T>::write(T data)
+    void Logger<T>::findLast()
+    {
+        uint32_t left   = start_;
+        uint32_t right  = end_+1;
+        uint32_t middle;
+
+        uint8_t * buffer = new uint8_t[256];
+
+        while (left <= right)
+        {
+            middle = (left + right) / 2;
+            flash_->readData(middle * W25QXX_SECTOR_SIZE, buffer, W25QXX_PAGE_SIZE);
+            
+            if (middle == 0 && searchCriterion(buffer))
+            {
+            	break;
+            }
+
+            if (searchCriterion(buffer))
+            {
+            	if (middle == 0)
+            	{
+            		break;
+            	}
+            	
+            	flash_->readData((middle-1) * W25QXX_SECTOR_SIZE, buffer, W25QXX_PAGE_SIZE);
+            	if (!searchCriterion(buffer))
+            	{
+            		break;
+            	}
+            
+                right = middle; 
+            }
+            else
+            {
+                left = middle + 1;
+            }
+        }
+
+        middle = (left + right) / 2;
+        delete[] buffer;
+
+        address_ = middle * W25QXX_SECTOR_SIZE;
+        ptr_ = 0;
+    }
+
+    template <typename T>
+    bool Logger<T>::write(T* data)
     {
         // If writing the data would exceed the segment length, return false and don't do anything.
-        if ((address_ + ptr_ + sizeof(T)) / W25QXX_SECTOR_SIZE == end_)
+        if ((address_ + ptr_ + sizeof(T)) / W25QXX_SECTOR_SIZE >= end_)
+        {
+            // If the current page hasn't been uploaded yet, do it now.
+            if (!flushed_)
+            {
+                flash_->pageProgram(address_, buffer_, W25QXX_PAGE_SIZE);
+                flushed_ = true;
+            }
             return false;
+        }
 
         // Convert the data to a byte array.
-        uint8_t * bytes = reinterpret_cast<uint8_t*>(&data);
+        uint8_t * bytes = reinterpret_cast<uint8_t*>(data);
         uint32_t length = sizeof(T);
 
-        // Bytes remaining until the page is full.
-        uint8_t remaining = W25QXX_PAGE_SIZE - ptr_;
-
-        // If the written data exceeds the remaining bytes in the page.
-        if (ptr_ + length >= W25QXX_PAGE_SIZE)
-        {
-            // If the page to written is in a new sector, erase the new sector before writing to it.
+		// If the written data exceeds the remaining bytes in the page.
+		while (ptr_ + length >= W25QXX_PAGE_SIZE)
+		{
+	        // Bytes remaining until the page is full.
+			uint32_t remaining = W25QXX_PAGE_SIZE - ptr_;
+		
+			// If the page to written is in a new sector, erase the new sector before writing to it.
             if (address_ % W25QXX_SECTOR_SIZE == 0)
             {
                 flash_->sectorErase(address_);
@@ -70,25 +122,46 @@ namespace sta
             length -= remaining;
             ptr_ = 0;
             address_ += W25QXX_PAGE_SIZE;
-        }
+		}
 
         std::memcpy(buffer_ + ptr_, bytes, length);
         ptr_ += length;
 
-        // If the end of the segment was reached, flush the temporally stored data.
-        if ((address_ + ptr_ + sizeof(T)) / W25QXX_SECTOR_SIZE == end_)
-            flash_->pageProgram(address_, buffer_, W25QXX_PAGE_SIZE);
-
         return true;
     }
 
+    template <typename T>
+    bool Logger<T>::write(T data)
+    {
+        return write(&data);
+    }
+
     template <typename T>
     void Logger<T>::clear()
     {
+        uint32_t left   = start_;
+        uint32_t right  = end_+1;
+        uint32_t middle;
+
+        // Erase all sectors the binary search would check when trying to find the last written page.
+        while (left <= right)
+        {
+            middle = (left + right) / 2;
+            flash_->sectorErase(middle * W25QXX_SECTOR_SIZE, true);
+            right = middle;
+            
+            if (middle == 0)
+            	break;
+            	
+            flash_->sectorErase((middle-1) * W25QXX_SECTOR_SIZE, true);
+        }
+
+        middle = (left + right) / 2;
+        flash_->sectorErase(middle * W25QXX_SECTOR_SIZE, true);
+
         address_ = start_ * W25QXX_SECTOR_SIZE; 
         ptr_ = 0;
-    
-        flash_->sectorErase(address_);
+        flushed_ = false;
     }
 
     template <typename T>
diff --git a/src/w25qxx.cpp b/src/w25qxx.cpp
index 66dcbcf..4a62e68 100644
--- a/src/w25qxx.cpp
+++ b/src/w25qxx.cpp
@@ -50,62 +50,6 @@ namespace sta
         return 1;
     }
 
-    uint32_t W25Qxx::getChunkBytes(ChunkSize size)
-    {
-        switch (size)
-        {
-        case ChunkSize::PAGE:
-            return W25QXX_PAGE_SIZE;
-
-        case ChunkSize::SECTOR:
-            return W25QXX_SECTOR_SIZE;
-
-        case ChunkSize::BLOCK_32KB:
-            return W25QXX_BLOCK_32_KB_SIZE;
-
-        case ChunkSize::BLOCK_64KB:
-            return W25QXX_BLOCK_64_KB_SIZE;
-        
-        default:
-            STA_UNREACHABLE();
-            break;
-        }
-    }
-
-    uint32_t W25Qxx::findLast(std::function<bool(uint8_t*)> criterion, ChunkSize size, uint32_t startAddr /*= 0 */, uint32_t endAddr /* = W25QXX_32B_MEM_SIZE */)
-    {
-        STA_ASSERT(startAddr <= endAddr);
-
-        lock_guard<Mutex>lock(*mutex_);
-        uint32_t bytes = getChunkBytes(size);
-
-        uint32_t left   = startAddr / bytes;
-        uint32_t right  = endAddr / bytes; // (addrMode_ == AddressMode::_32BIT ? W25QXX_32B_MEM_SIZE : W25QXX_24B_MEM_SIZE) / bytes;
-        uint32_t middle;
-
-        uint8_t * buffer = new uint8_t[bytes];
-
-        while (left < right)
-        {
-            middle = (left + right) / 2;
-            readData(middle * bytes, buffer, bytes);
-
-            if (criterion(buffer))
-            {
-                left = middle + 1;
-            }
-            else
-            {
-                right = middle - 1;
-            }
-        }
-
-        middle = (left + right) / 2;
-        delete[] buffer;
-
-        return middle * bytes;
-    }
-
     uint8_t W25Qxx::setAddressMode(AddressMode addrMode)
     {
         lock_guard<Mutex>lock(*mutex_);
@@ -159,7 +103,7 @@ namespace sta
 
         uint64_t id_complete = 0;
 
-        for (size_t i; i < 8; i++)
+        for (size_t i = 0; i < 8; i++)
         {
             id_complete |= id[i] << (7-i) * 8;
         }
@@ -424,8 +368,15 @@ namespace sta
         return (0x02 & status) == 0x02;
     }
 
-    uint8_t W25Qxx::sectorErase(uint32_t address)
+    uint8_t W25Qxx::sectorErase(uint32_t address, bool blocking /* = false */)
     {
+    	if (address % W25QXX_SECTOR_SIZE != 0)
+    	{
+    		return 0;
+    	}
+
+    	while (isBusy()) {}
+
         if (!writeEnable())
         {
             return 0;
@@ -455,11 +406,16 @@ namespace sta
         	return busWrite(W25QXX_SECTOR_ERASE, addrBuffer, 3);
         }
 
+        if (blocking)
+        	delay_(200*1000);
+
         return 1;
     }
 
     uint8_t W25Qxx::blockErase(uint32_t address, BlockSize blockSize)
     {
+    	while (isBusy()) {}
+
         if (!writeEnable())
         {
             return 0;

From 1d3ba48d7cb1a2a199493fb0bca3240f5b7022a0 Mon Sep 17 00:00:00 2001
From: dario <dario.vandenboom@arcor.de>
Date: Sat, 15 Jun 2024 15:10:23 +0200
Subject: [PATCH 13/13] Added feedback

---
 include/sta/drivers/w25qxx.hpp |  2 --
 include/sta/utils/logger.hpp   | 26 +++++++++++++++++---------
 include/sta/utils/logger.tpp   |  6 ------
 3 files changed, 17 insertions(+), 17 deletions(-)

diff --git a/include/sta/drivers/w25qxx.hpp b/include/sta/drivers/w25qxx.hpp
index 364ed62..b6d04ad 100644
--- a/include/sta/drivers/w25qxx.hpp
+++ b/include/sta/drivers/w25qxx.hpp
@@ -2,9 +2,7 @@
 #define STA_SENSORS_W25Q128_HPP
 
 #include <sta/bus/spi/device.hpp>
-//#include <sta/devices/stm32/bus/spi.hpp>
 #include <sta/mutex.hpp>
-
 #include <sta/drivers/w25qxx_defs.hpp>
 
 #include <functional>
diff --git a/include/sta/utils/logger.hpp b/include/sta/utils/logger.hpp
index deed7a0..6a7e747 100644
--- a/include/sta/utils/logger.hpp
+++ b/include/sta/utils/logger.hpp
@@ -20,14 +20,17 @@ namespace sta
          * @param endSector The index of the end sector (1 LSB = 4096 bytes).
          */
         Logger(W25Qxx * flash, uint32_t startSector, uint32_t endSector);
-        
-        void findLast();
 
+        /**
+         * @brief Write a new data point to the flash chip as long as the segment's limit wasn't reached.
+         * 
+         * @param data A pointer to the data to write to the flash chip.
+         * @return true if successful, false if the segment end was reached.
+         */
         bool write(T* data);
 
         /**
-         * @brief Write a new data point to the flash chip. 
-         * @note If the total capacity of this logger was exceeded, it restarts at the first sector, overwriting its data.
+         * @brief Write a new data point to the flash chip as long as the segment's limit wasn't reached.
          * 
          * @param data The data to write to the flash chip. 
          * @return true if successful, false if the segment end was reached.
@@ -68,17 +71,22 @@ namespace sta
          * @return T The ith element stored in the flash storage.
          */
         T get(std::size_t i);
+    private:
+        /**
+         * @brief Find the first sector in the segment that was not written yet.
+         * 
+         */
+        void findLast();
 
         /**
-         * @brief Get the ith element stored in the flash storage.
+         * @brief A method that checks if a page is empty
          * 
-         * @param i The index of the element to read. 
-         * @return T The ith element stored in the flash storage.
+         * @param buffer A buffer of size 256 containing data from a page.
+         * @return true if the page is empty, false otherwise.
          */
-        T operator[](std::size_t i);
-    private:
         bool searchCriterion(uint8_t * buffer);
 
+    private:
         W25Qxx * flash_;
         uint32_t start_;
         uint32_t end_;
diff --git a/include/sta/utils/logger.tpp b/include/sta/utils/logger.tpp
index 5bc4ee4..6d8bbb5 100644
--- a/include/sta/utils/logger.tpp
+++ b/include/sta/utils/logger.tpp
@@ -200,12 +200,6 @@ namespace sta
             return *reinterpret_cast<T*>(buffer);
         }
     }
-
-    template <typename T>
-    T Logger<T>::operator[](std::size_t i)
-    {
-        return this->get(i);
-    }
 } // namespace sta
 
 #endif // STA_UTILS_LOGGER_TPP