flight/pios/common/pios_flashfs_logfs.c

   1 /**
   2  ******************************************************************************
   3  * @file       pios_flashfs_logfs.c
   4  * @author     PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012
   5  * @addtogroup PIOS PIOS Core hardware abstraction layer
   6  * @{
   7  * @addtogroup PIOS_FLASHFS Flash Filesystem Function
   8  * @{
   9  * @brief Log Structured Filesystem for internal or external NOR Flash
  10  *****************************************************************************/
  11 /*
  12  * This program is free software; you can redistribute it and/or modify
  13  * it under the terms of the GNU General Public License as published by
  14  * the Free Software Foundation; either version 3 of the License, or
  15  * (at your option) any later version.
  16  *
  17  * This program is distributed in the hope that it will be useful, but
  18  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  19  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  20  * for more details.
  21  *
  22  * You should have received a copy of the GNU General Public License along
  23  * with this program; if not, write to the Free Software Foundation, Inc.,
  24  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  25  */
  26
  27
  28 #include "pios.h"
  29
  30 #ifdef PIOS_INCLUDE_FLASH
  31
  32 #include <stdbool.h>
  33 #include <openpilot.h>
  34 #include <pios_math.h>
  35 #include <pios_wdg.h>
  36 #include "pios_flashfs_logfs_priv.h"
  37
  38 /*
  39  * Filesystem state data tracked in RAM
  40  */
  41
  42 enum pios_flashfs_logfs_dev_magic {
  43     PIOS_FLASHFS_LOGFS_DEV_MAGIC = 0x94938201,
  44 };
  45
  46 struct logfs_state {
  47     enum pios_flashfs_logfs_dev_magic magic;
  48     const struct flashfs_logfs_cfg    *cfg;
  49     bool    mounted;
  50     uint8_t active_arena_id;
  51
  52     /* NOTE: num_active_slots + num_free_slots will not typically add
  53      *       up to the number of slots in the arena since some of the
  54      *       slots will be obsolete or otherwise invalidated
  55      */
  56     uint16_t num_free_slots; /* slots in free state */
  57     uint16_t num_active_slots; /* slots in active state */
  58
  59     /* Underlying flash driver glue */
  60     const struct pios_flash_driver *driver;
  61     uintptr_t flash_id;
  62 };
  63
  64 /*
  65  * Internal Utility functions
  66  */
  67
  68 /**
  69  * @brief Return the offset in flash of a particular slot within an arena
  70  * @return address of the requested slot
  71  */
  72 static uintptr_t logfs_get_addr(const struct logfs_state *logfs, uint8_t arena_id, uint16_t slot_id)
  73 {
  74     PIOS_Assert(arena_id < (logfs->cfg->total_fs_size / logfs->cfg->arena_size));
  75     PIOS_Assert(slot_id < (logfs->cfg->arena_size / logfs->cfg->slot_size));
  76
  77     return logfs->cfg->start_offset +
  78            (arena_id * logfs->cfg->arena_size) +
  79            (slot_id * logfs->cfg->slot_size);
  80 }
  81
  82 /*
  83  * The bits within these enum values must progress ONLY
  84  * from 1 -> 0 so that we can write later ones on top
  85  * of earlier ones in NOR flash without an erase cycle.
  86  */
  87 enum arena_state {
  88     /*
  89      * The STM32F30X flash subsystem is only capable of
  90      * writing words or halfwords. In this case we use halfwords.
  91      * In addition to that it is only capable to write to erased
  92      * cells (0xffff) or write a cell from anything to (0x0000).
  93      * To cope with this, the F3 needs carefully crafted enum values.
  94      * For this to work the underlying flash driver has to
  95      * check each halfword if it has changed before writing.
  96      */
  97     ARENA_STATE_ERASED   = 0xFFFFFFFF,
  98     ARENA_STATE_RESERVED = 0xE6E6FFFF,
  99     ARENA_STATE_ACTIVE   = 0xE6E66666,
 100     ARENA_STATE_OBSOLETE = 0x00000000,
 101 };
 102
 103 struct arena_header {
 104     uint32_t magic;
 105     enum arena_state state;
 106 } __attribute__((packed));
 107
 108
 109 /****************************************
 110 * Arena life-cycle transition functions
 111 ****************************************/
 112
 113 /**
 114  * @brief Erases all sectors within the given arena and sets arena to erased state.
 115  * @return 0 if success, < 0 on failure
 116  * @note Must be called while holding the flash transaction lock
 117  */
 118 static int32_t logfs_erase_arena(const struct logfs_state *logfs, uint8_t arena_id)
 119 {
 120     uintptr_t arena_addr = logfs_get_addr(logfs, arena_id, 0);
 121
 122     /* Erase all of the sectors in the arena */
 123     for (uint8_t sector_id = 0;
 124          sector_id < (logfs->cfg->arena_size / logfs->cfg->sector_size);
 125          sector_id++) {
 126         if (logfs->driver->erase_sector(logfs->flash_id,
 127                                         arena_addr + (sector_id * logfs->cfg->sector_size))) {
 128             return -1;
 129         }
 130     }
 131
 132     /* Mark this arena as fully erased */
 133     struct arena_header arena_hdr = {
 134         .magic = logfs->cfg->fs_magic,
 135         .state = ARENA_STATE_ERASED,
 136     };
 137
 138     if (logfs->driver->write_data(logfs->flash_id,
 139                                   arena_addr,
 140                                   (uint8_t *)&arena_hdr,
 141                                   sizeof(arena_hdr)) != 0) {
 142         return -2;
 143     }
 144
 145     /* Arena is ready to be activated */
 146     return 0;
 147 }
 148
 149 /**
 150  * @brief Marks the given arena as reserved so it can be filled.
 151  * @return 0 if success, < 0 on failure
 152  * @note Arena must have been previously erased before calling this
 153  * @note Must be called while holding the flash transaction lock
 154  */
 155 static int32_t logfs_reserve_arena(const struct logfs_state *logfs, uint8_t arena_id)
 156 {
 157     uintptr_t arena_addr = logfs_get_addr(logfs, arena_id, 0);
 158
 159     /* Read in the current arena header */
 160     struct arena_header arena_hdr;
 161
 162     if (logfs->driver->read_data(logfs->flash_id,
 163                                  arena_addr,
 164                                  (uint8_t *)&arena_hdr,
 165                                  sizeof(arena_hdr)) != 0) {
 166         return -1;
 167     }
 168     if (arena_hdr.state != ARENA_STATE_ERASED) {
 169         /* Arena was not erased, can't reserve it */
 170         return -2;
 171     }
 172
 173     /* Set the arena state to reserved */
 174     arena_hdr.state = ARENA_STATE_RESERVED;
 175
 176     /* Write the arena header back to flash */
 177     if (logfs->driver->write_data(logfs->flash_id,
 178                                   arena_addr,
 179                                   (uint8_t *)&arena_hdr,
 180                                   sizeof(arena_hdr)) != 0) {
 181         return -3;
 182     }
 183
 184     /* Arena is ready to be filled */
 185     return 0;
 186 }
 187
 188 /**
 189  * @brief Erases all arenas available to this filesystem instance
 190  * @return 0 if success, < 0 on failure
 191  * @note Must be called while holding the flash transaction lock
 192  */
 193 static int32_t logfs_erase_all_arenas(const struct logfs_state *logfs)
 194 {
 195     uint16_t num_arenas = logfs->cfg->total_fs_size / logfs->cfg->arena_size;
 196
 197     for (uint16_t arena = 0; arena < num_arenas; arena++) {
 198 #ifdef PIOS_LED_HEARTBEAT
 199         PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);
 200 #endif
 201 #ifdef PIOS_INCLUDE_WDG
 202         PIOS_WDG_Clear();
 203 #endif
 204         if (logfs_erase_arena(logfs, arena) != 0) {
 205             return -1;
 206         }
 207     }
 208
 209     return 0;
 210 }
 211
 212 /**
 213  * @brief Marks the given arena as active so it can be mounted.
 214  * @return 0 if success, < 0 on failure
 215  * @note Arena must have been previously erased or reserved before calling this
 216  * @note Must be called while holding the flash transaction lock
 217  */
 218 static int32_t logfs_activate_arena(const struct logfs_state *logfs, uint8_t arena_id)
 219 {
 220     uintptr_t arena_addr = logfs_get_addr(logfs, arena_id, 0);
 221
 222     /* Make sure this arena has been previously erased */
 223     struct arena_header arena_hdr;
 224
 225     if (logfs->driver->read_data(logfs->flash_id,
 226                                  arena_addr,
 227                                  (uint8_t *)&arena_hdr,
 228                                  sizeof(arena_hdr)) != 0) {
 229         /* Failed to read arena header */
 230         return -1;
 231     }
 232     if ((arena_hdr.state != ARENA_STATE_RESERVED) &&
 233         (arena_hdr.state != ARENA_STATE_ERASED)) {
 234         /* Arena was not erased or reserved, can't activate it */
 235         return -2;
 236     }
 237
 238     /* Mark this arena as active */
 239     arena_hdr.state = ARENA_STATE_ACTIVE;
 240     if (logfs->driver->write_data(logfs->flash_id,
 241                                   arena_addr,
 242                                   (uint8_t *)&arena_hdr,
 243                                   sizeof(arena_hdr)) != 0) {
 244         return -3;
 245     }
 246
 247     /* The arena is now activated and the log may be mounted */
 248     return 0;
 249 }
 250
 251 /**
 252  * @brief Marks the given arena as obsolete.
 253  * @return 0 if success, < 0 on failure
 254  * @note Arena must have been previously active before calling this
 255  * @note Must be called while holding the flash transaction lock
 256  */
 257 static int32_t logfs_obsolete_arena(const struct logfs_state *logfs, uint8_t arena_id)
 258 {
 259     uintptr_t arena_addr = logfs_get_addr(logfs, arena_id, 0);
 260
 261     /* We shouldn't be retiring the currently active arena */
 262     PIOS_Assert(!logfs->mounted);
 263
 264     /* Make sure this arena was previously active */
 265     struct arena_header arena_hdr;
 266     if (logfs->driver->read_data(logfs->flash_id,
 267                                  arena_addr,
 268                                  (uint8_t *)&arena_hdr,
 269                                  sizeof(arena_hdr)) != 0) {
 270         /* Failed to read arena header */
 271         return -1;
 272     }
 273
 274     if (arena_hdr.state != ARENA_STATE_ACTIVE) {
 275         /* Arena was not previously active, can't obsolete it */
 276         return -2;
 277     }
 278
 279     /* Mark this arena as obsolete */
 280     arena_hdr.state = ARENA_STATE_OBSOLETE;
 281     if (logfs->driver->write_data(logfs->flash_id,
 282                                   arena_addr,
 283                                   (uint8_t *)&arena_hdr,
 284                                   sizeof(arena_hdr)) != 0) {
 285         return -3;
 286     }
 287
 288     /* Arena is now obsoleted */
 289     return 0;
 290 }
 291
 292 /**
 293  * @brief Find the first active arena in flash
 294  * @return arena_id (>=0) of first active arena
 295  * @return -1 if no active arena is found
 296  * @return -2 if failed to read arena header
 297  * @note Must be called while holding the flash transaction lock
 298  */
 299 static int32_t logfs_find_active_arena(const struct logfs_state *logfs)
 300 {
 301     /* Search for the lowest numbered active arena */
 302     for (uint8_t arena_id = 0;
 303          arena_id < logfs->cfg->total_fs_size / logfs->cfg->arena_size;
 304          arena_id++) {
 305         uintptr_t arena_addr = logfs_get_addr(logfs, arena_id, 0);
 306         /* Load the arena header */
 307         struct arena_header arena_hdr;
 308         if (logfs->driver->read_data(logfs->flash_id,
 309                                      arena_addr,
 310                                      (uint8_t *)&arena_hdr,
 311                                      sizeof(arena_hdr)) != 0) {
 312             return -2;
 313         }
 314         if ((arena_hdr.state == ARENA_STATE_ACTIVE) &&
 315             (arena_hdr.magic == logfs->cfg->fs_magic)) {
 316             /* This is the first active arena */
 317             return arena_id;
 318         }
 319 #ifdef PIOS_INCLUDE_WDG
 320         PIOS_WDG_Clear();
 321 #endif
 322     }
 323
 324     /* Didn't find an active arena */
 325     return -1;
 326 }
 327
 328 /*
 329  * The bits within these enum values must progress ONLY
 330  * from 1 -> 0 so that we can write later ones on top
 331  * of earlier ones in NOR flash without an erase cycle.
 332  */
 333 enum slot_state {
 334     /*
 335      * The STM32F30X flash subsystem is only capable of
 336      * writing words or halfwords. In this case we use halfwords.
 337      * In addition to that it is only capable to write to erased
 338      * cells (0xffff) or write a cell from anything to (0x0000).
 339      * To cope with this, the F3 needs carfully crafted enum values.
 340      * For this to work the underlying flash driver has to
 341      * check each halfword if it has changed before writing.
 342      */
 343     SLOT_STATE_EMPTY    = 0xFFFFFFFF,
 344     SLOT_STATE_RESERVED = 0xFAFAFFFF,
 345     SLOT_STATE_ACTIVE   = 0xFAFAAAAA,
 346     SLOT_STATE_OBSOLETE = 0x00000000,
 347 };
 348
 349 struct slot_header {
 350     enum slot_state state;
 351     uint32_t obj_id;
 352     uint16_t obj_inst_id;
 353     uint16_t obj_size;
 354 } __attribute__((packed));
 355
 356 /* NOTE: Must be called while holding the flash transaction lock */
 357 static int32_t logfs_raw_copy_bytes(const struct logfs_state *logfs, uintptr_t src_addr, uint16_t src_size, uintptr_t dst_addr)
 358 {
 359 #define RAW_COPY_BLOCK_SIZE 16
 360     uint8_t data_block[RAW_COPY_BLOCK_SIZE];
 361
 362     while (src_size) {
 363         uint16_t blk_size;
 364         if (src_size >= RAW_COPY_BLOCK_SIZE) {
 365             /* Copy a full block */
 366             blk_size = RAW_COPY_BLOCK_SIZE;
 367         } else {
 368             /* Copy the remainder */
 369             blk_size = src_size;
 370         }
 371
 372         /* Read a block of data from source */
 373         if (logfs->driver->read_data(logfs->flash_id,
 374                                      src_addr,
 375                                      data_block,
 376                                      blk_size) != 0) {
 377             /* Failed to read next chunk from source */
 378             return -1;
 379         }
 380
 381         /* Write a block of data to destination */
 382         if (logfs->driver->write_data(logfs->flash_id,
 383                                       dst_addr,
 384                                       data_block,
 385                                       blk_size) != 0) {
 386             /* Failed to write chunk to destination */
 387             return -2;
 388         }
 389
 390         /* Update the src/dst pointers */
 391         src_size -= blk_size;
 392         src_addr += blk_size;
 393         dst_addr += blk_size;
 394     }
 395
 396     return 0;
 397 }
 398
 399 /*
 400  * Is the entire filesystem full?
 401  * true = all slots in the arena are in the ACTIVE state (ie. garbage collection won't free anything)
 402  * false = some slots in the arena are either currently free or could be free'd by garbage collection
 403  */
 404 static bool logfs_fs_is_full(const struct logfs_state *logfs)
 405 {
 406     return logfs->num_active_slots == (logfs->cfg->arena_size / logfs->cfg->slot_size) - 1;
 407 }
 408
 409 /*
 410  * Is the log full?
 411  * true = there are no unwritten slots left in the log (garbage collection may or may not help)
 412  * false = there are still some entirely unused slots left in the log
 413  */
 414 static bool logfs_log_is_full(const struct logfs_state *logfs)
 415 {
 416     return logfs->num_free_slots == 0;
 417 }
 418
 419 static int32_t logfs_unmount_log(struct logfs_state *logfs)
 420 {
 421     PIOS_Assert(logfs->mounted);
 422
 423     logfs->num_active_slots = 0;
 424     logfs->num_free_slots   = 0;
 425     logfs->mounted = false;
 426
 427     return 0;
 428 }
 429
 430 static int32_t logfs_mount_log(struct logfs_state *logfs, uint8_t arena_id)
 431 {
 432     PIOS_Assert(!logfs->mounted);
 433
 434     logfs->num_active_slots = 0;
 435     logfs->num_free_slots   = 0;
 436     logfs->active_arena_id  = arena_id;
 437
 438     /* Scan the log to find out how full it is */
 439     for (uint16_t slot_id = 1;
 440          slot_id < (logfs->cfg->arena_size / logfs->cfg->slot_size);
 441          slot_id++) {
 442         struct slot_header slot_hdr;
 443         uintptr_t slot_addr = logfs_get_addr(logfs, logfs->active_arena_id, slot_id);
 444         if (logfs->driver->read_data(logfs->flash_id,
 445                                      slot_addr,
 446                                      (uint8_t *)&slot_hdr,
 447                                      sizeof(slot_hdr)) != 0) {
 448             return -1;
 449         }
 450
 451         /*
 452          * Empty slots must be in a continguous block at the
 453          * end of the arena.
 454          */
 455         PIOS_Assert(slot_hdr.state == SLOT_STATE_EMPTY ||
 456                     logfs->num_free_slots == 0);
 457
 458         switch (slot_hdr.state) {
 459         case SLOT_STATE_EMPTY:
 460             logfs->num_free_slots++;
 461             break;
 462         case SLOT_STATE_ACTIVE:
 463             logfs->num_active_slots++;
 464             break;
 465         case SLOT_STATE_RESERVED:
 466         case SLOT_STATE_OBSOLETE:
 467             break;
 468         }
 469     }
 470
 471     /* Scan is complete, mark the arena mounted */
 472     logfs->active_arena_id = arena_id;
 473     logfs->mounted = true;
 474
 475     return 0;
 476 }
 477
 478 static bool PIOS_FLASHFS_Logfs_validate(const struct logfs_state *logfs)
 479 {
 480     return logfs && (logfs->magic == PIOS_FLASHFS_LOGFS_DEV_MAGIC);
 481 }
 482
 483 #if defined(PIOS_INCLUDE_FREERTOS)
 484 static struct logfs_state *PIOS_FLASHFS_Logfs_alloc(void)
 485 {
 486     struct logfs_state *logfs;
 487
 488     logfs = (struct logfs_state *)pios_malloc(sizeof(*logfs));
 489     if (!logfs) {
 490         return NULL;
 491     }
 492
 493     logfs->magic = PIOS_FLASHFS_LOGFS_DEV_MAGIC;
 494     return logfs;
 495 }
 496 static void PIOS_FLASHFS_Logfs_free(struct logfs_state *logfs)
 497 {
 498     /* Invalidate the magic */
 499     logfs->magic = ~PIOS_FLASHFS_LOGFS_DEV_MAGIC;
 500     vPortFree(logfs);
 501 }
 502 #else
 503 static struct logfs_state pios_flashfs_logfs_devs[PIOS_FLASHFS_LOGFS_MAX_DEVS];
 504 static uint8_t pios_flashfs_logfs_num_devs;
 505 static struct logfs_state *PIOS_FLASHFS_Logfs_alloc(void)
 506 {
 507     struct logfs_state *logfs;
 508
 509     if (pios_flashfs_logfs_num_devs >= PIOS_FLASHFS_LOGFS_MAX_DEVS) {
 510         return NULL;
 511     }
 512
 513     logfs = &pios_flashfs_logfs_devs[pios_flashfs_logfs_num_devs++];
 514     logfs->magic = PIOS_FLASHFS_LOGFS_DEV_MAGIC;
 515
 516     return logfs;
 517 }
 518 static void PIOS_FLASHFS_Logfs_free(struct logfs_state *logfs)
 519 {
 520     /* Invalidate the magic */
 521     logfs->magic = ~PIOS_FLASHFS_LOGFS_DEV_MAGIC;
 522
 523     /* Can't free the resources with this simple allocator */
 524 }
 525 #endif /* if defined(PIOS_INCLUDE_FREERTOS) */
 526
 527 /**
 528  * @brief Initialize the flash object setting FS
 529  * @return 0 if success, -1 if failure
 530  */
 531 int32_t PIOS_FLASHFS_Logfs_Init(uintptr_t *fs_id, const struct flashfs_logfs_cfg *cfg, const struct pios_flash_driver *driver, uintptr_t flash_id)
 532 {
 533     PIOS_Assert(cfg);
 534     PIOS_Assert(fs_id);
 535     PIOS_Assert(driver);
 536
 537     /* We must have at least 2 arenas for garbage collection to work */
 538     PIOS_Assert((cfg->total_fs_size / cfg->arena_size > 1));
 539
 540     /* Make sure the underlying flash driver provides the minimal set of required methods */
 541     PIOS_Assert(driver->start_transaction);
 542     PIOS_Assert(driver->end_transaction);
 543     PIOS_Assert(driver->erase_sector);
 544     PIOS_Assert(driver->write_data);
 545     PIOS_Assert(driver->read_data);
 546
 547     int8_t rc;
 548
 549     struct logfs_state *logfs;
 550
 551     logfs = (struct logfs_state *)PIOS_FLASHFS_Logfs_alloc();
 552     if (!logfs) {
 553         rc = -1;
 554         goto out_exit;
 555     }
 556
 557     /* Bind configuration parameters to this filesystem instance */
 558     logfs->cfg      = cfg;  /* filesystem configuration */
 559     logfs->driver   = driver; /* lower-level flash driver */
 560     logfs->flash_id = flash_id; /* lower-level flash device id */
 561     logfs->mounted  = false;
 562
 563     if (logfs->driver->start_transaction(logfs->flash_id) != 0) {
 564         rc = -1;
 565         goto out_exit;
 566     }
 567
 568     bool found = false;
 569     int32_t arena_id;
 570     for (uint8_t try = 0; !found && try < 2; try++) {
 571         /* Find the active arena */
 572         arena_id = logfs_find_active_arena(logfs);
 573         if (arena_id >= 0) {
 574             /* Found the active arena */
 575             found = true;
 576             break;
 577         } else {
 578             /* No active arena found, erase and activate arena 0 */
 579             if (logfs_erase_arena(logfs, 0) != 0) {
 580                 break;
 581             }
 582             if (logfs_activate_arena(logfs, 0) != 0) {
 583                 break;
 584             }
 585         }
 586     }
 587
 588     if (!found) {
 589         /* Still no active arena, something is broken */
 590         rc = -2;
 591         goto out_end_trans;
 592     }
 593
 594     /* We've found an active arena, mount it */
 595     if (logfs_mount_log(logfs, arena_id) != 0) {
 596         /* Failed to mount the log, something is broken */
 597         rc = -3;
 598         goto out_end_trans;
 599     }
 600
 601     /* Log has been mounted */
 602     rc     = 0;
 603
 604     *fs_id = (uintptr_t)logfs;
 605
 606 out_end_trans:
 607     logfs->driver->end_transaction(logfs->flash_id);
 608
 609 out_exit:
 610     return rc;
 611 }
 612
 613 int32_t PIOS_FLASHFS_Logfs_Destroy(uintptr_t fs_id)
 614 {
 615     int32_t rc;
 616
 617     struct logfs_state *logfs = (struct logfs_state *)fs_id;
 618
 619     if (!PIOS_FLASHFS_Logfs_validate(logfs)) {
 620         rc = -1;
 621         goto out_exit;
 622     }
 623
 624     PIOS_FLASHFS_Logfs_free(logfs);
 625     rc = 0;
 626
 627 out_exit:
 628     return rc;
 629 }
 630
 631 /* NOTE: Must be called while holding the flash transaction lock */
 632 static int32_t logfs_garbage_collect(struct logfs_state *logfs)
 633 {
 634     PIOS_Assert(logfs->mounted);
 635
 636     /* Source arena is the active arena */
 637     uint8_t src_arena_id = logfs->active_arena_id;
 638
 639     /* Compute destination arena */
 640     uint8_t dst_arena_id = (logfs->active_arena_id + 1) % (logfs->cfg->total_fs_size / logfs->cfg->arena_size);
 641
 642     /* Erase destination arena */
 643     if (logfs_erase_arena(logfs, dst_arena_id) != 0) {
 644         return -1;
 645     }
 646
 647     /* Reserve the destination arena so we can start filling it */
 648     if (logfs_reserve_arena(logfs, dst_arena_id) != 0) {
 649         /* Unable to reserve the arena */
 650         return -2;
 651     }
 652
 653     /* Copy active slots from active arena to destination arena */
 654     uint16_t dst_slot_id = 1;
 655     for (uint16_t src_slot_id = 1;
 656          src_slot_id < (logfs->cfg->arena_size / logfs->cfg->slot_size);
 657          src_slot_id++) {
 658         struct slot_header slot_hdr;
 659         uintptr_t src_addr = logfs_get_addr(logfs, src_arena_id, src_slot_id);
 660         if (logfs->driver->read_data(logfs->flash_id,
 661                                      src_addr,
 662                                      (uint8_t *)&slot_hdr,
 663                                      sizeof(slot_hdr)) != 0) {
 664             return -3;
 665         }
 666
 667         if (slot_hdr.state == SLOT_STATE_ACTIVE) {
 668             uintptr_t dst_addr = logfs_get_addr(logfs, dst_arena_id, dst_slot_id);
 669             if (logfs_raw_copy_bytes(logfs,
 670                                      src_addr,
 671                                      sizeof(slot_hdr) + slot_hdr.obj_size,
 672                                      dst_addr) != 0) {
 673                 /* Failed to copy all bytes */
 674                 return -4;
 675             }
 676             dst_slot_id++;
 677         }
 678 #ifdef PIOS_INCLUDE_WDG
 679         PIOS_WDG_Clear();
 680 #endif
 681     }
 682
 683     /* Activate the destination arena */
 684     if (logfs_activate_arena(logfs, dst_arena_id) != 0) {
 685         return -5;
 686     }
 687
 688     /* Unmount the source arena */
 689     if (logfs_unmount_log(logfs) != 0) {
 690         return -6;
 691     }
 692
 693     /* Obsolete the source arena */
 694     if (logfs_obsolete_arena(logfs, src_arena_id) != 0) {
 695         return -7;
 696     }
 697
 698     /* Mount the new arena */
 699     if (logfs_mount_log(logfs, dst_arena_id) != 0) {
 700         return -8;
 701     }
 702
 703     return 0;
 704 }
 705
 706 /* NOTE: Must be called while holding the flash transaction lock */
 707 static int16_t logfs_object_find_next(const struct logfs_state *logfs, struct slot_header *slot_hdr, uint16_t *curr_slot, uint32_t obj_id, uint16_t obj_inst_id)
 708 {
 709     PIOS_Assert(slot_hdr);
 710     PIOS_Assert(curr_slot);
 711
 712     /* First slot in the arena is reserved for arena header, skip it. */
 713     if (*curr_slot == 0) {
 714         *curr_slot = 1;
 715     }
 716
 717     for (uint16_t slot_id = *curr_slot;
 718          slot_id < (logfs->cfg->arena_size / logfs->cfg->slot_size);
 719          slot_id++) {
 720         uintptr_t slot_addr = logfs_get_addr(logfs, logfs->active_arena_id, slot_id);
 721
 722         if (logfs->driver->read_data(logfs->flash_id,
 723                                      slot_addr,
 724                                      (uint8_t *)slot_hdr,
 725                                      sizeof(*slot_hdr)) != 0) {
 726             return -2;
 727         }
 728         if (slot_hdr->state == SLOT_STATE_EMPTY) {
 729             /* We hit the end of the log */
 730             break;
 731         }
 732         if (slot_hdr->state == SLOT_STATE_ACTIVE &&
 733             slot_hdr->obj_id == obj_id &&
 734             slot_hdr->obj_inst_id == obj_inst_id) {
 735             /* Found what we were looking for */
 736             *curr_slot = slot_id;
 737             return 0;
 738         }
 739 #ifdef PIOS_INCLUDE_WDG
 740         PIOS_WDG_Clear();
 741 #endif
 742     }
 743
 744     /* No matching entry was found */
 745     return -1;
 746 }
 747
 748 /* NOTE: Must be called while holding the flash transaction lock */
 749 /* OPTIMIZE: could trust that there is at most one active version of every object and terminate the search when we find one */
 750 static int8_t logfs_delete_object(struct logfs_state *logfs, uint32_t obj_id, uint16_t obj_inst_id)
 751 {
 752     int8_t rc;
 753
 754     bool more = true;
 755     uint16_t curr_slot_id = 0;
 756
 757     do {
 758         struct slot_header slot_hdr;
 759         switch (logfs_object_find_next(logfs, &slot_hdr, &curr_slot_id, obj_id, obj_inst_id)) {
 760         case 0:
 761             /* Found a matching slot.  Obsolete it. */
 762             slot_hdr.state = SLOT_STATE_OBSOLETE;
 763             uintptr_t slot_addr = logfs_get_addr(logfs, logfs->active_arena_id, curr_slot_id);
 764
 765             if (logfs->driver->write_data(logfs->flash_id,
 766                                           slot_addr,
 767                                           (uint8_t *)&slot_hdr,
 768                                           sizeof(slot_hdr)) != 0) {
 769                 rc = -2;
 770                 goto out_exit;
 771             }
 772             /* Object has been successfully obsoleted and is no longer active */
 773             logfs->num_active_slots--;
 774             break;
 775         case -1:
 776             /* Search completed, object not found */
 777             more = false;
 778             rc   = 0;
 779             break;
 780         default:
 781             /* Error occurred during search */
 782             rc = -1;
 783             goto out_exit;
 784         }
 785     } while (more);
 786
 787 out_exit:
 788     return rc;
 789 }
 790
 791 /* NOTE: Must be called while holding the flash transaction lock */
 792 static int8_t logfs_reserve_free_slot(struct logfs_state *logfs, uint16_t *slot_id, struct slot_header *slot_hdr, uint32_t obj_id, uint16_t obj_inst_id, uint16_t obj_size)
 793 {
 794     PIOS_Assert(slot_id);
 795     PIOS_Assert(slot_hdr);
 796
 797     if (logfs->num_free_slots < 1) {
 798         /* No free slots to allocate */
 799         return -1;
 800     }
 801
 802     if (obj_size > (logfs->cfg->slot_size - sizeof(slot_hdr))) {
 803         /* This object is too big for the slot */
 804         return -2;
 805     }
 806
 807     uint16_t candidate_slot_id = (logfs->cfg->arena_size / logfs->cfg->slot_size) - logfs->num_free_slots;
 808     PIOS_Assert(candidate_slot_id > 0);
 809
 810     uintptr_t slot_addr = logfs_get_addr(logfs, logfs->active_arena_id, candidate_slot_id);
 811
 812     if (logfs->driver->read_data(logfs->flash_id,
 813                                  slot_addr,
 814                                  (uint8_t *)slot_hdr,
 815                                  sizeof(*slot_hdr)) != 0) {
 816         /* Failed to read slot header for candidate slot */
 817         return -3;
 818     }
 819
 820     if (slot_hdr->state != SLOT_STATE_EMPTY) {
 821         /* Candidate slot isn't empty!  Something is broken. */
 822         PIOS_DEBUG_Assert(0);
 823         return -4;
 824     }
 825
 826     /* Mark this slot as RESERVED */
 827     slot_hdr->state       = SLOT_STATE_RESERVED;
 828     slot_hdr->obj_id      = obj_id;
 829     slot_hdr->obj_inst_id = obj_inst_id;
 830     slot_hdr->obj_size    = obj_size;
 831
 832     if (logfs->driver->write_data(logfs->flash_id,
 833                                   slot_addr,
 834                                   (uint8_t *)slot_hdr,
 835                                   sizeof(*slot_hdr)) != 0) {
 836         /* Failed to write the slot header */
 837         return -5;
 838     }
 839
 840     /* FIXME: If the header write (above) failed, may have partially written data, thus corrupting that slot but we would have missed decrementing this counter */
 841     logfs->num_free_slots--;
 842
 843     *slot_id = candidate_slot_id;
 844     return 0;
 845 }
 846
 847 /* NOTE: Must be called while holding the flash transaction lock */
 848 static int8_t logfs_append_to_log(struct logfs_state *logfs, uint32_t obj_id, uint16_t obj_inst_id, uint8_t *obj_data, uint16_t obj_size)
 849 {
 850     /* Reserve a free slot for our new object */
 851     uint16_t free_slot_id;
 852     struct slot_header slot_hdr;
 853
 854     if (logfs_reserve_free_slot(logfs, &free_slot_id, &slot_hdr, obj_id, obj_inst_id, obj_size) != 0) {
 855         /* Failed to reserve a free slot */
 856         return -1;
 857     }
 858
 859     /* Compute slot address */
 860     uintptr_t slot_addr   = logfs_get_addr(logfs, logfs->active_arena_id, free_slot_id);
 861
 862     /* Write the data into the reserved slot, starting after the slot header */
 863     uintptr_t slot_offset = sizeof(slot_hdr);
 864     while (obj_size > 0) {
 865         /* Individual writes must fit entirely within a single page buffer. */
 866         uint16_t page_remaining = logfs->cfg->page_size - (slot_offset % logfs->cfg->page_size);
 867         uint16_t write_size     = MIN(obj_size, page_remaining);
 868         if (logfs->driver->write_data(logfs->flash_id,
 869                                       slot_addr + slot_offset,
 870                                       obj_data,
 871                                       write_size) != 0) {
 872             /* Failed to write the object data to the slot */
 873             return -2;
 874         }
 875
 876         /* Update our accounting */
 877         obj_data    += write_size;
 878         slot_offset += write_size;
 879         obj_size    -= write_size;
 880     }
 881
 882     /* Mark this slot active in one atomic step */
 883     slot_hdr.state = SLOT_STATE_ACTIVE;
 884     if (logfs->driver->write_data(logfs->flash_id,
 885                                   slot_addr,
 886                                   (uint8_t *)&slot_hdr,
 887                                   sizeof(slot_hdr)) != 0) {
 888         /* Failed to mark the slot active */
 889         return -4;
 890     }
 891
 892     /* Object has been successfully written to the slot */
 893     logfs->num_active_slots++;
 894     return 0;
 895 }
 896
 897
 898 /**********************************
 899  *
 900  * Provide a PIOS_FLASHFS_* driver
 901  *
 902  *********************************/
 903 #include "pios_flashfs.h" /* API for flash filesystem */
 904
 905 /**
 906  * @brief Saves one object instance to the filesystem
 907  * @param[in] fs_id The filesystem to use for this action
 908  * @param[in] obj UAVObject ID of the object to save
 909  * @param[in] obj_inst_id The instance number of the object being saved
 910  * @param[in] obj_data Contents of the object being saved
 911  * @param[in] obj_size Size of the object being saved
 912  * @return 0 if success or error code
 913  * @retval -1 if fs_id is not a valid filesystem instance
 914  * @retval -2 if failed to start transaction
 915  * @retval -3 if failure to delete any previous versions of the object
 916  * @retval -4 if filesystem is entirely full and garbage collection won't help
 917  * @retval -5 if garbage collection failed
 918  * @retval -6 if filesystem is full even after garbage collection should have freed space
 919  * @retval -7 if writing the new object to the filesystem failed
 920  */
 921 int32_t PIOS_FLASHFS_ObjSave(uintptr_t fs_id, uint32_t obj_id, uint16_t obj_inst_id, uint8_t *obj_data, uint16_t obj_size)
 922 {
 923     int8_t rc;
 924
 925     struct logfs_state *logfs = (struct logfs_state *)fs_id;
 926
 927     if (!PIOS_FLASHFS_Logfs_validate(logfs)) {
 928         rc = -1;
 929         goto out_exit;
 930     }
 931
 932     PIOS_Assert(obj_size <= (logfs->cfg->slot_size - sizeof(struct slot_header)));
 933
 934     if (logfs->driver->start_transaction(logfs->flash_id) != 0) {
 935         rc = -2;
 936         goto out_exit;
 937     }
 938
 939     if (logfs_delete_object(logfs, obj_id, obj_inst_id) != 0) {
 940         rc = -3;
 941         goto out_end_trans;
 942     }
 943
 944     /*
 945      * All old versions of this object + instance have been invalidated.
 946      * Write the new object.
 947      */
 948
 949     /* Check if the arena is entirely full. */
 950     if (logfs_fs_is_full(logfs)) {
 951         /* Note: Filesystem Full means we're full of *active* records so gc won't help at all. */
 952         rc = -4;
 953         goto out_end_trans;
 954     }
 955
 956     /* Is garbage collection required? */
 957     if (logfs_log_is_full(logfs)) {
 958         /* Note: Log Full means the log is full but may contain obsolete slots so gc may free some space */
 959         if (logfs_garbage_collect(logfs) != 0) {
 960             rc = -5;
 961             goto out_end_trans;
 962         }
 963         /* Check one more time just to be sure we actually free'd some space */
 964         if (logfs_log_is_full(logfs)) {
 965             /*
 966              * Log is still full even after gc!
 967              * NOTE: This should not happen since the filesystem wasn't full
 968              *       when we checked above so gc should have helped.
 969              */
 970             PIOS_DEBUG_Assert(0);
 971             rc = -6;
 972             goto out_end_trans;
 973         }
 974     }
 975
 976     /* We have room for our new object.  Append it to the log. */
 977     if (logfs_append_to_log(logfs, obj_id, obj_inst_id, obj_data, obj_size) != 0) {
 978         /* Error during append */
 979         rc = -7;
 980         goto out_end_trans;
 981     }
 982
 983     /* Object successfully written to the log */
 984     rc = 0;
 985
 986 out_end_trans:
 987     logfs->driver->end_transaction(logfs->flash_id);
 988
 989 out_exit:
 990     return rc;
 991 }
 992
 993 /**
 994  * @brief Load one object instance from the filesystem
 995  * @param[in] fs_id The filesystem to use for this action
 996  * @param[in] obj UAVObject ID of the object to load
 997  * @param[in] obj_inst_id The instance of the object to load
 998  * @param[in] obj_data Buffer to hold the contents of the loaded object
 999  * @param[in] obj_size Size of the object to be loaded
1000  * @return 0 if success or error code
1001  * @retval -1 if fs_id is not a valid filesystem instance
1002  * @retval -2 if failed to start transaction
1003  * @retval -3 if object not found in filesystem
1004  * @retval -4 if object size in filesystem does not exactly match buffer size
1005  * @retval -5 if reading the object data from flash fails
1006  */
1007 int32_t PIOS_FLASHFS_ObjLoad(uintptr_t fs_id, uint32_t obj_id, uint16_t obj_inst_id, uint8_t *obj_data, uint16_t obj_size)
1008 {
1009     int8_t rc;
1010
1011     struct logfs_state *logfs = (struct logfs_state *)fs_id;
1012
1013     if (!PIOS_FLASHFS_Logfs_validate(logfs)) {
1014         rc = -1;
1015         goto out_exit;
1016     }
1017
1018     PIOS_Assert(obj_size <= (logfs->cfg->slot_size - sizeof(struct slot_header)));
1019
1020     if (logfs->driver->start_transaction(logfs->flash_id) != 0) {
1021         rc = -2;
1022         goto out_exit;
1023     }
1024
1025     /* Find the object in the log */
1026     uint16_t slot_id = 0;
1027     struct slot_header slot_hdr;
1028     if (logfs_object_find_next(logfs, &slot_hdr, &slot_id, obj_id, obj_inst_id) != 0) {
1029         /* Object does not exist in fs */
1030         rc = -3;
1031         goto out_end_trans;
1032     }
1033
1034     /* Sanity check what we've found */
1035     if (slot_hdr.obj_size != obj_size) {
1036         /* Object sizes don't match.  Not safe to copy contents. */
1037         rc = -4;
1038         goto out_end_trans;
1039     }
1040
1041     /* Read the contents of the object from the log */
1042     if (obj_size > 0) {
1043         uintptr_t slot_addr = logfs_get_addr(logfs, logfs->active_arena_id, slot_id);
1044         if (logfs->driver->read_data(logfs->flash_id,
1045                                      slot_addr + sizeof(slot_hdr),
1046                                      (uint8_t *)obj_data,
1047                                      obj_size) != 0) {
1048             /* Failed to read object data from the log */
1049             rc = -5;
1050             goto out_end_trans;
1051         }
1052     }
1053
1054     /* Object successfully loaded */
1055     rc = 0;
1056
1057 out_end_trans:
1058     logfs->driver->end_transaction(logfs->flash_id);
1059
1060 out_exit:
1061     return rc;
1062 }
1063
1064 /**
1065  * @brief Delete one instance of an object from the filesystem
1066  * @param[in] fs_id The filesystem to use for this action
1067  * @param[in] obj UAVObject ID of the object to delete
1068  * @param[in] obj_inst_id The instance of the object to delete
1069  * @return 0 if success or error code
1070  * @retval -1 if fs_id is not a valid filesystem instance
1071  * @retval -2 if failed to start transaction
1072  * @retval -3 if failed to delete the object from the filesystem
1073  */
1074 int32_t PIOS_FLASHFS_ObjDelete(uintptr_t fs_id, uint32_t obj_id, uint16_t obj_inst_id)
1075 {
1076     int8_t rc;
1077
1078     struct logfs_state *logfs = (struct logfs_state *)fs_id;
1079
1080     if (!PIOS_FLASHFS_Logfs_validate(logfs)) {
1081         rc = -1;
1082         goto out_exit;
1083     }
1084
1085     if (logfs->driver->start_transaction(logfs->flash_id) != 0) {
1086         rc = -2;
1087         goto out_exit;
1088     }
1089
1090     if (logfs_delete_object(logfs, obj_id, obj_inst_id) != 0) {
1091         rc = -3;
1092         goto out_end_trans;
1093     }
1094
1095     /* Object successfully deleted from the log */
1096     rc = 0;
1097
1098 out_end_trans:
1099     logfs->driver->end_transaction(logfs->flash_id);
1100
1101 out_exit:
1102     return rc;
1103 }
1104
1105 /**
1106  * @brief Erases all filesystem arenas and activate the first arena
1107  * @param[in] fs_id The filesystem to use for this action
1108  * @return 0 if success or error code
1109  * @retval -1 if fs_id is not a valid filesystem instance
1110  * @retval -2 if failed to start transaction
1111  * @retval -3 if failed to erase all arenas
1112  * @retval -4 if failed to activate arena 0
1113  * @retval -5 if failed to mount arena 0
1114  */
1115 int32_t PIOS_FLASHFS_Format(uintptr_t fs_id)
1116 {
1117     int32_t rc;
1118
1119     struct logfs_state *logfs = (struct logfs_state *)fs_id;
1120
1121     if (!PIOS_FLASHFS_Logfs_validate(logfs)) {
1122         rc = -1;
1123         goto out_exit;
1124     }
1125
1126     if (logfs->mounted) {
1127         logfs_unmount_log(logfs);
1128     }
1129
1130     if (logfs->driver->start_transaction(logfs->flash_id) != 0) {
1131         rc = -2;
1132         goto out_exit;
1133     }
1134
1135     if (logfs_erase_all_arenas(logfs) != 0) {
1136         rc = -3;
1137         goto out_end_trans;
1138     }
1139
1140     /* Reinitialize arena 0 */
1141     if (logfs_activate_arena(logfs, 0) != 0) {
1142         rc = -4;
1143         goto out_end_trans;
1144     }
1145
1146     /* Mount arena 0 */
1147     if (logfs_mount_log(logfs, 0) != 0) {
1148         rc = -5;
1149         goto out_end_trans;
1150     }
1151
1152     /* Chip erased and log remounted successfully */
1153     rc = 0;
1154
1155 out_end_trans:
1156     logfs->driver->end_transaction(logfs->flash_id);
1157
1158 out_exit:
1159     return rc;
1160 }
1161 /**
1162  * @brief Returs stats for the filesystems
1163  * @param[in] fs_id The filesystem to use for this action
1164  * @return 0 if success or error code
1165  * @retval -1 if fs_id is not a valid filesystem instance
1166  */
1167 int32_t PIOS_FLASHFS_GetStats(uintptr_t fs_id, struct PIOS_FLASHFS_Stats *stats)
1168 {
1169     PIOS_Assert(stats);
1170     struct logfs_state *logfs = (struct logfs_state *)fs_id;
1171
1172     if (!PIOS_FLASHFS_Logfs_validate(logfs)) {
1173         return -1;
1174     }
1175     stats->num_active_slots = logfs->num_active_slots;
1176     stats->num_free_slots   = logfs->num_free_slots;
1177     return 0;
1178 }
1179 #endif /* PIOS_INCLUDE_FLASH */
1180
1181 /**
1182  * @}
1183  * @}
1184  */