Working logger implementation and some fixes for driver code

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.

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

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 false;
-                {
-                    return true;
-                }
             }
+        }
         
-            return false;
+        return true;
-        }, 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)
+    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.
+		// If the written data exceeds the remaining bytes in the page.
-        uint8_t remaining = W25QXX_PAGE_SIZE - ptr_;
+		while (ptr_ + length >= W25QXX_PAGE_SIZE)
+		{
+	        // Bytes remaining until the page is full.
+			uint32_t remaining = W25QXX_PAGE_SIZE - ptr_;
 		
-        // If the written data exceeds the remaining bytes in the page.
+			// If the page to written is in a new sector, erase the new sector before writing to it.
-        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 (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;
-    
+        flushed_ = false;
-        flash_->sectorErase(address_);
     }
 
     template <typename T>

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;