Add missing page erase.
[bertos.git] / bertos / fs / battfs.c
index db15a3c8df430f8c784029f10abb630c4eec2342..9c72f74928c024bf331df8bde3d5118ba3d81482 100644 (file)
@@ -44,6 +44,8 @@
 #include <cfg/macros.h> /* MIN, MAX */
 #include <cpu/byteorder.h> /* cpu_to_xx */
 
+#define LOG_LEVEL       LOG_LVL_INFO
+#define LOG_FORMAT      LOG_FMT_VERBOSE
 #include <cfg/log.h>
 
 #include <string.h> /* memset, memmove */
@@ -55,7 +57,7 @@
  */
 INLINE void battfs_to_disk(struct BattFsPageHeader *hdr, uint8_t *buf)
 {
-       STATIC_ASSERT(BATTFS_HEADER_LEN == 10);
+       STATIC_ASSERT(BATTFS_HEADER_LEN == 12);
        buf[0] = hdr->inode;
 
        buf[1] = hdr->fill;
@@ -65,25 +67,22 @@ INLINE void battfs_to_disk(struct BattFsPageHeader *hdr, uint8_t *buf)
        buf[4] = hdr->pgoff >> 8;
 
        /*
-        * Sequence number is at least 1 bit longer than page address.
-        * Needed to take care of wraparonds.
+        * Sequence number is 40 bits long.
+        * No need to take care of wraparonds: the memory will die first!
         */
        buf[5] = hdr->seq;
        buf[6] = hdr->seq >> 8;
-
-       /*
-        * First bit used by seq.
-        * Unused bits are set to 1.
-        */
-       buf[7] = (hdr->seq >> 16) ? 0xFF : 0xFE;
+       buf[7] = hdr->seq >> 16;
+       buf[8] = hdr->seq >> 24;
+       buf[9] = hdr->seq >> 32;
 
        /*
         * This field must be the last one!
         * This is needed because if the page is only partially
         * written, we can use this to detect it.
         */
-       buf[8] = hdr->fcs;
-       buf[9] = hdr->fcs >> 8;
+       buf[10] = hdr->fcs;
+       buf[11] = hdr->fcs >> 8;
 }
 
 /**
@@ -92,12 +91,12 @@ INLINE void battfs_to_disk(struct BattFsPageHeader *hdr, uint8_t *buf)
  */
 INLINE void disk_to_battfs(uint8_t *buf, struct BattFsPageHeader *hdr)
 {
-       STATIC_ASSERT(BATTFS_HEADER_LEN == 10);
+       STATIC_ASSERT(BATTFS_HEADER_LEN == 12);
        hdr->inode = buf[0];
        hdr->fill = buf[2] << 8 | buf[1];
        hdr->pgoff = buf[4] << 8 | buf[3];
-       hdr->seq = (seq_t)(buf[7] & 0x01) << 16 | buf[6] << 8 | buf[5];
-       hdr->fcs = buf[9] << 8 | buf[8];
+       hdr->seq = (seq_t)buf[9] << 32 | (seq_t)buf[8] << 24 | (seq_t)buf[7] << 16 | buf[6] << 8 | buf[5];
+       hdr->fcs = buf[11] << 8 | buf[10];
 }
 
 /**
@@ -131,7 +130,7 @@ static bool battfs_readHeader(struct BattFsSuper *disk, pgcnt_t page, struct Bat
        if (disk->read(disk, page, disk->page_size - BATTFS_HEADER_LEN, buf, BATTFS_HEADER_LEN)
            != BATTFS_HEADER_LEN)
        {
-               TRACEMSG("Error: page[%d]\n", page);
+               LOG_ERR("Error: page[%d]\n", page);
                return false;
        }
 
@@ -157,10 +156,10 @@ static bool battfs_writeHeader(struct BattFsSuper *disk, pgcnt_t page, struct Ba
         * Header is actually a footer, and so
         * resides at page end.
         */
-       if (disk->write(disk, page, disk->page_size - BATTFS_HEADER_LEN, buf, BATTFS_HEADER_LEN)
-           != BATTFS_HEADER_LEN)
+       if (!(disk->bufferWrite(disk, disk->page_size - BATTFS_HEADER_LEN, buf, BATTFS_HEADER_LEN)
+           == BATTFS_HEADER_LEN && disk->save(disk, page)))
        {
-               TRACEMSG("Error: page[%d]\n", page);
+               LOG_ERR("Error: page[%d]\n", page);
                return false;
        }
        return true;
@@ -197,133 +196,6 @@ static void movePages(struct BattFsSuper *disk, pgcnt_t src, int offset)
        }
 }
 
-#if 0
-
-/**
- * Insert \a page at the bottom of page allocation array of \a disk.
- */
-static void insertFreePage(struct BattFsSuper *disk, pgcnt_t page)
-{
-       pgcnt_t free_pos = disk->page_count - 1;
-       ASSERT(disk->page_array[free_pos] == PAGE_UNSET_SENTINEL);
-       ASSERT(page <= free_pos);
-
-       disk->page_array[free_pos] = page;
-}
-
-/**
- * Mark \a page of \a disk as free.
- * \note free_next of \a disk is used as \a page free marker
- * and is increased by 1.
- */
-static bool battfs_markFree(struct BattFsSuper *disk, struct BattFsPageHeader *hdr, pgcnt_t page)
-{
-       uint8_t buf[BATTFS_HEADER_LEN];
-
-       hdr->mark = disk->free_next;
-       hdr->fcs_free = computeFcsFree(hdr);
-       battfs_to_disk(hdr, buf);
-
-       if (!disk->write(disk, page, disk->page_size - BATTFS_HEADER_LEN, buf, BATTFS_HEADER_LEN))
-       {
-               TRACEMSG("error marking page [%d]\n", page);
-               return false;
-       }
-       else
-       {
-               disk->free_next++;
-               return true;
-       }
-}
-
-/**
- * Determine free_start and free_next blocks for \a disk
- * using \a minl, \a maxl, \a minh, \a maxh.
- *
- * Mark_t is a type that has at least 1 bit more than
- * pgaddr_t. So all free blocks can be numbered using
- * at most half numbers of a mark_t type.
- * The free blocks algorithm increments by 1 the disk->free_next
- * every time a page becomes free. So the free block sequence is
- * guaranteed to be countiguous.
- * Only wrap arounds may happen, but due to half size sequence limitation,
- * there are only 4 possible situations:
- *
- * \verbatim
- *    |------lower half------|-------upper half-------|
- *
- * 1) |------minl*****maxl---|------------------------|
- * 2) |------minl********maxl|minh******maxh----------|
- * 3) |----------------------|----minh*******maxh-----|
- * 4) |minl******maxl--------|------------minh****maxh|
- * \endverbatim
- *
- * Situations 1 and 3 are easy to detect, while 2 and 4 require more care.
- */
-static void findFreeStartNext(struct BattFsSuper *disk, mark_t minl, mark_t maxl, mark_t minh, mark_t maxh)
-{
-       /* Determine free_start & free_next */
-       if (maxl >= minl)
-       {
-               /* Valid interval found in lower half */
-               if (maxh >= minh)
-               {
-                       /* Valid interval also found in upper half */
-                       if (maxl == minh - 1)
-                       {
-                               /* Interval starts in lower half and ends in upper */
-                               disk->free_start = minl;
-                               disk->free_next = maxh;
-                       }
-                       else
-                       {
-                               /* Interval starts in upper half and ends in lower */
-                               ASSERT(minl == 0);
-                               ASSERT(maxh == (MAX_PAGE_ADDR | MARK_HALF_SIZE));
-
-                               disk->free_start = minh;
-                               disk->free_next = maxl;
-                       }
-               }
-               else
-               {
-                       /*
-                        * Upper interval is invalid.
-                        * Use lower values.
-                        */
-
-                       disk->free_start = minl;
-                       disk->free_next = maxl;
-               }
-       }
-       else if (maxh >= minh)
-       {
-               /*
-                * Lower interval is invalid.
-                * Use upper values.
-                */
-               disk->free_start = minh;
-               disk->free_next = maxh;
-       }
-       else
-       {
-               /*
-                * No valid interval found.
-                * Hopefully the disk is brand new (or full).
-                */
-               TRACEMSG("No valid marked free block found, new disk or disk full\n");
-               disk->free_start = 0;
-               disk->free_next = -1; //to be increased later
-       }
-
-       /* free_next should contain the first usable address */
-       disk->free_next++;
-
-       TRACEMSG("Free markers:\n minl %u\n maxl %u\n minh %u\n maxh %u\n free_start %u\n free_next %u\n",
-               minl, maxl, minh, maxh, disk->free_start, disk->free_next);
-}
-#endif
-
 /**
  * Count number of pages per file on \a disk.
  * This information is registered in \a filelen_table.
@@ -360,6 +232,7 @@ static bool countDiskFilePages(struct BattFsSuper *disk, pgoff_t *filelen_table)
                        disk->free_page_start++;
                }
        }
+       LOG_INFO("free_bytes:%d, free_page_start:%d\n", disk->free_bytes, disk->free_page_start);
 
        return true;
 }
@@ -373,11 +246,10 @@ static bool countDiskFilePages(struct BattFsSuper *disk, pgoff_t *filelen_table)
  * inside file.
  * e.g. : at page array[0] you will find page address of the first page
  * of the first file (if present).
- * Free blocks are allocated after the last file, starting from invalid ones
- * and continuing with the marked free ones.
+ * Free blocks are allocated after the last file.
  *
  * \return true if ok, false on disk read errors.
- * \note The whole disk is scanned once.
+ * \note The whole disk is scanned at max twice.
  */
 static bool fillPageArray(struct BattFsSuper *disk, pgoff_t *filelen_table)
 {
@@ -393,17 +265,71 @@ static bool fillPageArray(struct BattFsSuper *disk, pgoff_t *filelen_table)
                if (hdr.fcs == computeFcs(&hdr))
                {
                        /* Compute array position */
-                       pgcnt_t array_pos_start = countPages(filelen_table, hdr.inode);
-                       pgcnt_t array_pos = array_pos_start + hdr.pgoff;
+                       pgcnt_t array_pos = countPages(filelen_table, hdr.inode);
+                       array_pos += hdr.pgoff;
+
 
-                       /* Find the first free position */
-                       while (disk->page_array[array_pos] != PAGE_UNSET_SENTINEL)
+                       /* Check if position is already used by another page of the same file */
+                       if (disk->page_array[array_pos] == PAGE_UNSET_SENTINEL)
+                               disk->page_array[array_pos] = page;
+                       else
                        {
-                               ASSERT(array_pos < array_pos_start + filelen_table[hdr.inode + 1]);
-                               array_pos++;
-                       }
+                               BattFsPageHeader hdr_prv;
+
+                               if (!battfs_readHeader(disk, disk->page_array[array_pos], &hdr_prv))
+                                       return false;
+
+                               /* Check header FCS */
+                               ASSERT(hdr_prv.fcs == computeFcs(&hdr_prv));
+
+                               /* Only the very same page with a different seq number can be here */
+                               ASSERT(hdr.inode == hdr_prv.inode);
+                               ASSERT(hdr.pgoff == hdr_prv.pgoff);
+                               ASSERT(hdr.seq != hdr_prv.seq);
+
+                               pgcnt_t new_page, old_page;
+                               fill_t old_fill;
+
+                               /*
+                                * Sequence number comparison: since
+                                * seq is 40 bits wide, it wraps once
+                                * every 1.1E12 times.
+                                * The memory will not live enough to
+                                * see a wraparound, so we can use a simple
+                                * compare here.
+                                */
+                               if (hdr.seq > hdr_prv.seq)
+                               {
+                                       /* Current header is newer than the previuos one */
+                                       old_page = disk->page_array[array_pos];
+                                       new_page = page;
+                                       old_fill = hdr_prv.fill;
+                               }
+                               else
+                               {
+                                       /* Previous header is newer than the current one */
+                                       old_page = page;
+                                       new_page = disk->page_array[array_pos];
+                                       old_fill = hdr.fill;
+                               }
+
+                               /* Set new page */
+                               disk->page_array[array_pos] = new_page;
+                               /* Add free space */
+                               disk->free_bytes += old_fill;
+                               /* Shift all array one position to the left, overwriting duplicate page */
+                               array_pos -= hdr.pgoff;
+                               array_pos += filelen_table[hdr.inode];
+                               movePages(disk, array_pos, -1);
+                               /* Move back all indexes */
+                               filelen_table[hdr.inode]--;
+                               disk->free_page_start--;
+                               curr_free_page--;
+                               /* Set old page as free */
+                               ASSERT(disk->page_array[curr_free_page] == PAGE_UNSET_SENTINEL);
+                               disk->page_array[curr_free_page++] = old_page;
 
-                       disk->page_array[array_pos] = page;
+                       }
                }
                else
                {
@@ -416,218 +342,45 @@ static bool fillPageArray(struct BattFsSuper *disk, pgoff_t *filelen_table)
        return true;
 }
 
-/**
- * Find the latest version of a page, starting from the
- * page supplied by \a page_array.
- * The pages are read from the disk until a different
- * inode or page offset is found.
- * The lastest version of the page is moved in the first
- * position of \a page_array.
- * \return the number of old versions of the page or PAGE_ERROR
- *         on disk read errors.
- */
-static pgcnt_t findLastVersion(struct BattFsSuper *disk, pgcnt_t *page_array)
-{
-       pgcnt_t *array_start = page_array;
-       BattFsPageHeader hdr;
-       if (!battfs_readHeader(disk, *page_array++, &hdr))
-               return PAGE_ERROR;
-
-       /* Free space: early bailout */
-       if (hdr.fcs != computeFcs(&hdr))
-               return 0;
-
-       /*
-        * If the first page is valid,
-        * inode and pg_off in the array are taken
-        * as the current page markers.
-        */
-       inode_t curr_inode = hdr.inode;
-       pgoff_t curr_pgoff = hdr.pgoff;
-
-       /* Temps used to find the sequence number range */
-       seq_t minl = HALF_SEQ - 1;
-       seq_t maxl = 0;
-       seq_t minh = MAX_SEQ;
-       seq_t maxh = MAX_SEQ;
-       pgcnt_t lpos = 0, hpos = 0, dup_cnt = 0;
-
-       /*
-        * Find min and max values for the two
-        * half of seq_num range.
-        * With this we can find seqnum wraparounds.
-        * seq_t is a type that has at least 1 bit more than
-        * pgaddr_t. So all version of a page blocks can be numbered using
-        * at most half numbers of a seq_t type.
-        * The sequence number algorithm increments by 1 the previous seq_num
-        * every time a page is rewritten. So the sequence is
-        * guaranteed to be countiguous.
-        * Only wrap arounds may happen, but due to half size sequence limitation,
-        * there are only 4 possible situations:
-        *
-        * \verbatim
-        *    |------lower half------|-------upper half-------|
-        *
-        * 1) |------minl*****maxl---|------------------------|
-        * 2) |------minl********maxl|minh******maxh----------|
-        * 3) |----------------------|----minh*******maxh-----|
-        * 4) |minl******maxl--------|------------minh****maxh|
-        * \endverbatim
-        *
-        * Situations 1 and 3 are easy to detect, while 2 and 4 require more care.
-        */
-       do
-       {
-               if (hdr.seq < HALF_SEQ)
-               {
-                       minl = MIN(minl, hdr.seq);
-                       if (hdr.seq > maxl)
-                       {
-                               maxl = hdr.seq;
-                               lpos = dup_cnt;
-                       }
-               }
-               else
-               {
-                       minh = MIN(minh, hdr.seq);
-                       if (hdr.seq > maxh)
-                       {
-                               maxh = hdr.seq;
-                               hpos = dup_cnt;
-                       }
-               }
-
-               if (!battfs_readHeader(disk, *page_array++, &hdr))
-                       return PAGE_ERROR;
-               dup_cnt++;
-       }
-       while (curr_inode == hdr.inode && curr_pgoff == hdr.pgoff && hdr.fcs == computeFcs(&hdr));
-
-
-       /* Return early if there is only one version of the current page */
-       if (dup_cnt == 1)
-               return 0;
-
-       /* Find the position in the array of the last version of the page */
-       pgcnt_t last_ver = hpos;
-       if (maxl >= minl)
-       {
-               /* Valid interval found in lower half */
-               if (maxh >= minh)
-               {
-                       /* Valid interval also found in upper half */
-                       if (maxl != minh - 1)
-                       {
-                               /* Interval starts in upper half and ends in lower */
-                               ASSERT(minl == 0);
-                               ASSERT(maxh == MAX_SEQ);
-
-                               last_ver = lpos;
-                       }
-               }
-               else
-                       /*
-                        * Upper interval is invalid.
-                        * Use lower values.
-                        */
-                       last_ver = lpos;
-       }
-
-       /* Put last page version at array start position */
-       SWAP(array_start[0], array_start[last_ver]);
-
-       return dup_cnt - 1;
-}
 
 /**
- * Collect old pages, removing empty spaces from \a pg_array, for a maximum len of \a pg_len.
- * Once the collect task is completed, copy \a old_cnt pages from \a old_pages at the
- * end of free space in pg_array.
+ * Flush the current \a disk buffer.
+ * \return true if ok, false on errors.
  */
-void collectOldPages(pgcnt_t *pg_array, pgcnt_t pg_len, pgcnt_t *old_pages, pgcnt_t old_cnt)
+static bool battfs_flushBuffer(struct BattFsSuper *disk)
 {
-       bool copy = false;
-       pgcnt_t gap = 0;
-
-       for (pgcnt_t curr_page = 0; curr_page < pg_len; pg_len++)
+       if (disk->cache_dirty)
        {
-               if (!copy)
-               {
-                       if (pg_array[curr_page] == PAGE_UNSET_SENTINEL)
-                               gap++;
-                       else
-                       {
-                               pg_array[curr_page - gap] = pg_array[curr_page];
-                               copy = true;
-                       }
-               }
-               else
-               {
-                       if (pg_array[curr_page] != PAGE_UNSET_SENTINEL)
-                               pg_array[curr_page - gap] = pg_array[curr_page];
-                       else
-                       {
-                               gap++;
-                               copy = false;
-                       }
-               }
-       }
-       ASSERT(gap == old_cnt);
-       pg_array += pg_len - old_cnt;
+               LOG_INFO("Flushing to disk page %d\n", disk->curr_page);
+               if (!disk->erase(disk, disk->curr_page))
+                       return false;
 
-       memcpy(pg_array, old_pages, old_cnt * sizeof(pgcnt_t));
+               if (!disk->save(disk, disk->curr_page))
+                       return false;
+               disk->cache_dirty = false;
+       }
+       return true;
 }
 
 /**
- * This function scan the page array of \a disk looking for
- * old versions of the same page.
- *
- * Only the last version is kept as valid, the old ones are inserted
- * in the free blocks heap.
- * \return true if ok, false on disk read errors.
- * \note The whole disk is scanned once.
+ * Load \a new_page from \a disk in disk page buffer.
+ * If a previuos page is still dirty in the buffer, will be
+ * flushed first.
+ * \return true if ok, false on errors.
  */
-static bool dropOldPages(struct BattFsSuper *disk)
+static bool battfs_loadPage(struct BattFsSuper *disk, pgcnt_t new_page)
 {
-       #define OLD_PAGE_BUFLEN 64
-       pgcnt_t old_pages[OLD_PAGE_BUFLEN];
-       pgcnt_t old_cnt = 0;
-
-       pgcnt_t *curr_page = disk->page_array;
-       pgcnt_t *collect_start = disk->page_array;
-       pgcnt_t collect_len = disk->page_count;
-       pgcnt_t dup_pages;
-
-       do
-       {
-               dup_pages = findLastVersion(disk, curr_page);
-               if (dup_pages == PAGE_ERROR)
-                       return false;
-               /* The first page is the last version */
-               curr_page++;
-               while (dup_pages--)
-               {
-                       if (old_cnt >= OLD_PAGE_BUFLEN)
-                       {
-                               collectOldPages(collect_start, collect_len, old_pages, old_cnt);
-                               collect_len -= old_cnt;
-                               disk->free_bytes += old_cnt * (disk->page_size - BATTFS_HEADER_LEN);
-                               disk->free_page_start -= old_cnt;
-                               curr_page -= old_cnt;
-                               collect_start = curr_page;
-                               old_cnt = 0;
-                       }
+       LOG_INFO("Loading page %d\n", new_page);
+       if (disk->curr_page == new_page)
+               return true;
 
-                       old_pages[old_cnt++] = *curr_page;
-                       *curr_page++ = PAGE_UNSET_SENTINEL;
-               }
-       }
-       while (curr_page < disk->page_array + disk->free_page_start);
+       if (!battfs_flushBuffer(disk))
+               return false;
 
-       collectOldPages(collect_start, collect_len, old_pages, old_cnt);
-       disk->free_bytes += old_cnt * (disk->page_size - BATTFS_HEADER_LEN);
-       disk->free_page_start -= old_cnt;
+       if (!disk->load(disk, new_page))
+               return false;
 
+       disk->curr_page = new_page;
        return true;
 }
 
@@ -647,13 +400,15 @@ bool battfs_init(struct BattFsSuper *disk)
        /* Init disk device */
        if (!disk->open(disk))
        {
-               TRACEMSG("open error\n");
+               LOG_ERR("open error\n");
                return false;
        }
 
        /* Disk open must set all of these */
        ASSERT(disk->read);
-       ASSERT(disk->write);
+       ASSERT(disk->load);
+       ASSERT(disk->bufferWrite);
+       ASSERT(disk->save);
        ASSERT(disk->erase);
        ASSERT(disk->close);
        ASSERT(disk->page_size);
@@ -669,7 +424,7 @@ bool battfs_init(struct BattFsSuper *disk)
        /* Count pages per file */
        if (!countDiskFilePages(disk, filelen_table))
        {
-               TRACEMSG("error counting file pages\n");
+               LOG_ERR("error counting file pages\n");
                return false;
        }
 
@@ -682,15 +437,15 @@ bool battfs_init(struct BattFsSuper *disk)
        /* Fill page allocation array using filelen_table */
        if (!fillPageArray(disk, filelen_table))
        {
-               TRACEMSG("error filling page array\n");
+               LOG_ERR("error filling page array\n");
                return false;
        }
+       #warning TODO: shuffle free blocks
 
-       if (!dropOldPages(disk))
-       {
-               LOG_ERR("error dropping old pages\n");
-               return false;
-       }
+       /* Initialize page buffer cache */
+       disk->cache_dirty = false;
+       disk->curr_page = 0;
+       disk->load(disk, disk->curr_page);
 
        /* Init list for opened files. */
        LIST_INIT(&disk->file_opened_list);
@@ -703,9 +458,12 @@ bool battfs_init(struct BattFsSuper *disk)
  */
 static int battfs_flush(struct KFile *fd)
 {
-       (void)fd;
-       #warning TODO
-       return 0;
+       BattFs *fdb = BATTFS_CAST(fd);
+
+       if (battfs_flushBuffer(fdb->disk))
+               return 0;
+       else
+               return EOF;
 }
 
 /**
@@ -721,6 +479,69 @@ static int battfs_fileclose(struct KFile *fd)
        return 0;
 }
 
+
+/**
+ * Write to file \a fd \a size bytes from \a buf.
+ * \return The number of bytes written.
+ */
+static size_t battfs_write(struct KFile *fd, const void *_buf, size_t size)
+{
+       BattFs *fdb = BATTFS_CAST(fd);
+       const uint8_t *buf = (const uint8_t *)_buf;
+
+       size_t total_write = 0;
+       pgoff_t pg_offset;
+       pgaddr_t addr_offset;
+       pgaddr_t wr_len;
+
+       size = MIN((kfile_off_t)size, fd->size - fd->seek_pos);
+
+       while (size)
+       {
+               #warning TODO: outside EOF?
+
+               pg_offset = fd->seek_pos / (fdb->disk->page_size - BATTFS_HEADER_LEN);
+               addr_offset = fd->seek_pos % (fdb->disk->page_size - BATTFS_HEADER_LEN);
+               wr_len = MIN(size, (size_t)(fdb->disk->page_size - BATTFS_HEADER_LEN - addr_offset));
+
+
+               if (fdb->start[pg_offset] != fdb->disk->curr_page)
+               {
+                       if (!battfs_loadPage(fdb->disk, fdb->start[pg_offset]))
+                       {
+                               #warning TODO set error?
+                               return total_write;
+                       }
+
+                       /* Get a free page */
+                       fdb->disk->curr_page = fdb->disk->page_array[fdb->disk->free_page_start];
+                       movePages(fdb->disk, fdb->disk->free_page_start + 1, -1);
+
+                       /* Insert previous page in free blocks list */
+                       fdb->disk->page_array[fdb->disk->page_count - 1] = fdb->start[pg_offset];
+                       /* Assign new page */
+                       fdb->start[pg_offset] = fdb->disk->curr_page;
+                       #warning TODO: hdr have to be updated!
+               }
+
+
+               if (fdb->disk->bufferWrite(fdb->disk, addr_offset, buf, wr_len) != wr_len)
+               {
+                       #warning TODO set error?
+               }
+               fdb->disk->cache_dirty = true;
+
+               size -= wr_len;
+               fd->seek_pos += wr_len;
+               total_write += wr_len;
+               buf += wr_len;
+               #warning TODO: hdr have to be updated!
+       }
+       return total_write;
+
+}
+
+
 /**
  * Read from file \a fd \a size bytes in \a buf.
  * \return The number of bytes read.
@@ -743,6 +564,10 @@ static size_t battfs_read(struct KFile *fd, void *_buf, size_t size)
                addr_offset = fd->seek_pos % (fdb->disk->page_size - BATTFS_HEADER_LEN);
                read_len = MIN(size, (size_t)(fdb->disk->page_size - BATTFS_HEADER_LEN - addr_offset));
 
+               /* Flush current page if needed */
+               if (fdb->start[pg_offset] == fdb->disk->curr_page)
+                       battfs_flushBuffer(fdb->disk);
+
                /* Read from disk */
                if (fdb->disk->read(fdb->disk, fdb->start[pg_offset], addr_offset, buf, read_len) != read_len)
                {
@@ -766,10 +591,10 @@ static size_t battfs_read(struct KFile *fd, void *_buf, size_t size)
 static pgcnt_t *findFile(BattFsSuper *disk, inode_t inode)
 {
        BattFsPageHeader hdr;
-       pgcnt_t first = 0, page, last = disk->page_count -1;
+       pgcnt_t first = 0, page, last = disk->free_page_start;
        fcs_t fcs;
 
-       while (first <= last)
+       while (first < last)
        {
                page = (first + last) / 2;
 
@@ -801,19 +626,19 @@ bool battfs_fileExists(BattFsSuper *disk, inode_t inode)
  * in disk->page_array. Size is written in \a size.
  * \return true if all s ok, false on disk read errors.
  */
-static bool countFileSize(BattFsSuper *disk, pgcnt_t *start, inode_t inode, file_size_t *size)
+static file_size_t countFileSize(BattFsSuper *disk, pgcnt_t *start, inode_t inode)
 {
-       *size = 0;
+       file_size_t size = 0;
        BattFsPageHeader hdr;
 
        for (;;)
        {
                if (!battfs_readHeader(disk, *start++, &hdr))
-                       return false;
+                       return EOF;
                if (hdr.fcs == computeFcs(&hdr) && hdr.inode == inode)
-                       *size += hdr.fill;
+                       size += hdr.fill;
                else
-                       return true;
+                       return size;
        }
 }
 
@@ -846,7 +671,7 @@ bool battfs_fileopen(BattFsSuper *disk, BattFs *fd, inode_t inode, filemode_t mo
        }
 
        /* Fill file size */
-       if (!countFileSize(disk, fd->start, inode, &fd->fd.size))
+       if ((fd->fd.size = countFileSize(disk, fd->start, inode)) == EOF)
                return false;
 
        /* Reset seek position */
@@ -871,10 +696,10 @@ bool battfs_fileopen(BattFsSuper *disk, BattFs *fd, inode_t inode, filemode_t mo
        fd->fd.read = battfs_read;
        fd->fd.reopen = kfile_genericReopen;
        fd->fd.seek = kfile_genericSeek;
+       fd->fd.write = battfs_write;
 
-#warning TODO battfs_write, battfs_error, battfs_clearerr
+#warning TODO battfs_error, battfs_clearerr
 #if 0
-       fd->fd.write = battfs_write;
        fd->fd.error = battfs_error;
        fd->fd.clearerr = battfs_clearerr;
 #endif
@@ -903,10 +728,16 @@ bool battfs_close(struct BattFsSuper *disk)
        return disk->close(disk) && (res == 0);
 }
 
+#if UNIT_TEST
 bool battfs_writeTestBlock(struct BattFsSuper *disk, pgcnt_t page, inode_t inode, seq_t seq, fill_t fill, pgoff_t pgoff)
 {
        BattFsPageHeader hdr;
 
+       /* Reset page to all 0xff */
+       uint8_t buf[disk->page_size];
+       memset(buf, 0xFF, disk->page_size);
+       disk->bufferWrite(disk, 0, buf, disk->page_size);
+
        hdr.inode = inode;
        hdr.fill = fill;
        hdr.pgoff = pgoff;
@@ -915,9 +746,10 @@ bool battfs_writeTestBlock(struct BattFsSuper *disk, pgcnt_t page, inode_t inode
 
        if (!battfs_writeHeader(disk, page, &hdr))
        {
-               TRACEMSG("error writing hdr\n");
+               LOG_ERR("error writing hdr\n");
                return false;
        }
 
        return true;
 }
+#endif