From fb11b312c51cbc960b8d565bdf42b3516f595844 Mon Sep 17 00:00:00 2001
From: Alessio Morale <alessiomorale@gmail.com>
Date: Mon, 18 May 2015 21:26:40 +0200
Subject: [PATCH] REVONANO - Flash fs fixes

---
 flight/pios/common/pios_flash_eeprom.c    | 32 +++++++++------------
 flight/pios/common/pios_flashfs_objlist.c | 34 ++++++++---------------
 2 files changed, 24 insertions(+), 42 deletions(-)

diff --git a/flight/pios/common/pios_flash_eeprom.c b/flight/pios/common/pios_flash_eeprom.c
index 6d64229df..aabdbf5b3 100644
--- a/flight/pios/common/pios_flash_eeprom.c
+++ b/flight/pios/common/pios_flash_eeprom.c
@@ -173,32 +173,25 @@ int32_t PIOS_Flash_EEPROM_WriteSinglePage(struct flash_eeprom_dev *flash_dev, co
     uint8_t i2c_addr = flash_dev->i2c_address;
     uint32_t bus     = flash_dev->i2c_adapter;
 
-
     if (address > 0xFFFF) {
         return -2;
     }
-    uint16_t address16    = address & 0xFFFF;
+    uint16_t address16 = address & 0xFFFF;
 
-    uint8_t mem_address[] = {
-        (address16 & 0xff00) >> 8, // MSB
-        (address16 & 0xFF) // LSB
-    };
+    uint8_t tmp[len + 2];
+    tmp[0] = (address16 & 0xFF00) >> 8; // MSB
+    tmp[1] = (address16 & 0xFF); // LSB
+
+
+    memcpy(&tmp[2], buffer, len);
 
     const struct pios_i2c_txn txn_list[] = {
         {
             .info = __func__,
             .addr = i2c_addr,
             .rw   = PIOS_I2C_TXN_WRITE,
-            .len  = 2,
-            .buf  = mem_address,
-        }
-        ,
-        {
-            .info = __func__,
-            .addr = i2c_addr,
-            .rw   = PIOS_I2C_TXN_WRITE,
-            .len  = len,
-            .buf  = (uint8_t *)buffer,
+            .len  = len + 2,
+            .buf  = tmp,
         }
     };
 
@@ -223,21 +216,21 @@ int32_t PIOS_Flash_EEPROM_Read(struct flash_eeprom_dev *flash_dev, const uint32_
     // split the operation into several page operations
     uint8_t bytes_read     = 0;
 
-    const uint8_t page_len = flash_dev->cfg->page_len;
+    const uint16_t page_len = flash_dev->cfg->page_len;
 
     while (bytes_read < len) {
         // all is fine, wait until memory is not busy
         int32_t status;
         while ((status = PIOS_Flash_EEPROM_Busy(flash_dev)) == 1) {
 #ifdef PIOS_INCLUDE_FREERTOS
-            vTaskDelay(1);
+            vTaskDelay(0);
 #endif
         }
         // An error occurred while probing the status, return and report
         if (status < 0) {
             return status;
         }
-        uint8_t current_block_len  = len - bytes_read;
+        uint16_t current_block_len  = len - bytes_read;
         uint16_t index_within_page = (address16 + bytes_read) % page_len;
         // prevent overflowing the page boundary
         current_block_len = MIN(page_len - index_within_page, current_block_len);
@@ -248,6 +241,7 @@ int32_t PIOS_Flash_EEPROM_Read(struct flash_eeprom_dev *flash_dev, const uint32_
             return status;
         }
     }
+
     return 0;
 }
 
diff --git a/flight/pios/common/pios_flashfs_objlist.c b/flight/pios/common/pios_flashfs_objlist.c
index 1fbb66411..f123cbcf5 100644
--- a/flight/pios/common/pios_flashfs_objlist.c
+++ b/flight/pios/common/pios_flashfs_objlist.c
@@ -121,7 +121,6 @@ int32_t PIOS_FLASHFS_Init(uintptr_t *fs_id, const struct flashfs_cfg *cfg, const
 
     // Check for valid object table or create one
     uint32_t object_table_magic;
-    uint32_t magic_fail_count = 0;
     bool magic_good = false;
     *fs_id = (uintptr_t)dev;
 
@@ -130,18 +129,13 @@ int32_t PIOS_FLASHFS_Init(uintptr_t *fs_id, const struct flashfs_cfg *cfg, const
             return -1;
         }
         if (object_table_magic != dev->cfg->table_magic) {
-            if (magic_fail_count++ > MAX_BADMAGIC) {
-                if (PIOS_FLASHFS_Format(*fs_id) != 0) {
-                    return -1;
-                }
-#if defined(PIOS_LED_HEARTBEAT)
-                PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);
-#endif /* PIOS_LED_HEARTBEAT */
-                magic_fail_count = 0;
-                magic_good = true;
-            } else {
-                PIOS_DELAY_WaituS(1000);
+            if (PIOS_FLASHFS_Format(*fs_id) != 0) {
+                return -1;
             }
+#if defined(PIOS_LED_HEARTBEAT)
+            PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);
+#endif /* PIOS_LED_HEARTBEAT */
+            magic_good = true;
         } else {
             magic_good = true;
         }
@@ -182,21 +176,15 @@ int32_t PIOS_FLASHFS_Format(uintptr_t fs_id)
         return -1;
     }
 
-    if (!dev->driver->start_transaction(dev->flash_id)) {
+    if (dev->driver->start_transaction(dev->flash_id)) {
         return -1;
     }
 
-
-    if (dev->driver->erase_chip) {
-        if (dev->driver->erase_chip(dev->flash_id) != 0) {
-            dev->driver->end_transaction(dev->flash_id);
-            return -1;
-        }
-    }
-
     if (PIOS_FLASHFS_ClearObjectTableHeader(dev) != 0) {
+        dev->driver->end_transaction(dev->flash_id);
         return -1;
     }
+    dev->driver->end_transaction(dev->flash_id);
     return 0;
 }
 
@@ -227,7 +215,7 @@ static int32_t PIOS_FLASHFS_ClearObjectTableHeader(struct flashfs_dev *dev)
  * @brief Get the address of an object
  * @param obj UAVObjHandle for that object
  * @parma instId Instance id for that object
- * @return address if successful, -1 if not found
+ * @return address if successful, -1 if not found, -2 if failure occurred while reading
  */
 static int32_t PIOS_FLASHFS_GetObjAddress(struct flashfs_dev *dev, uint32_t objId, uint16_t instId)
 {
@@ -238,7 +226,7 @@ static int32_t PIOS_FLASHFS_GetObjAddress(struct flashfs_dev *dev, uint32_t objI
     while (addr < dev->cfg->obj_table_end) {
         // Read the instance data
         if (dev->driver->read_data(dev->flash_id, addr, (uint8_t *)&header, sizeof(header)) != 0) {
-            return -1;
+            return -2;
         }
         if (header.objMagic != dev->cfg->obj_magic) {
             break; // stop searching once hit first non-object header