diff --git a/Inc/flash.h b/Inc/flash.h
index a8bf7ad..0e94394 100644
--- a/Inc/flash.h
+++ b/Inc/flash.h
@@ -58,7 +58,7 @@ int flash_erase(uint8_t page, uint8_t page_count);
  * Copies `size` words from `src` to `dst`. Since this function is only called using constant values and previously checked values, the input parameters are not validated. The addresses must be aligned according
  * to the rules specified in the ST DM00083560 document.
  */
-int flash_copy(uint32_t* src, __IO uint32_t* dst, uint32_t size);
+int flash_copy(const uint32_t* src, __IO uint32_t* dst, uint32_t size);
 
 /**
  * Locks the flash again, preventing writing.
diff --git a/Inc/fs.h b/Inc/fs.h
index 89155e2..ddbc5f9 100644
--- a/Inc/fs.h
+++ b/Inc/fs.h
@@ -105,4 +105,16 @@ uint32_t* fs_swap_get_free();
  */
 uint32_t* fs_cache_get_free(uint32_t cache_start, int page_count, int entry_size);
 
+/**
+ * Finds the first free entry and writes the new value. If all pages are full, they are all erased and the new value is written at the beginning
+ * of the first page.
+ */
+int fs_replace_entry(uint32_t page_num, int page_count, int entry_size, const uint32_t *entry);
+
+/**
+ * Similar to fs_replace_entry but applicable to caches. In case the cache is full, only the oldest page is erased and the new entry is placed at
+ * the beginning of the erased page.
+ */
+int fs_cache_entry(uint32_t cache_start, int page_count, int entry_size, const uint32_t *entry);
+
 #endif /* FS_H_ */
diff --git a/Src/flash.c b/Src/flash.c
index 4ec70d2..af1265f 100644
--- a/Src/flash.c
+++ b/Src/flash.c
@@ -73,7 +73,7 @@ int flash_erase(uint8_t page, uint8_t page_count) {
   return 0;
 }
 
-int flash_copy(uint32_t *src, __IO uint32_t *dst, uint32_t size) {
+int flash_copy(const uint32_t *src, __IO uint32_t *dst, uint32_t size) {
   _flash_wait();
   _flash_clear_errors();
   int ret = 0;
diff --git a/Src/fs.c b/Src/fs.c
index b2241be..3df5f59 100644
--- a/Src/fs.c
+++ b/Src/fs.c
@@ -195,10 +195,10 @@ uint32_t* _fs_find_free_entry(uint32_t* page, int entry_size) {
 
 uint32_t* fs_find_free_entry(uint32_t page_num, int page_count, int entry_size) {
   for (int i = 0; i < page_count; i++) {
-    uint32_t* page = _fs_find_free_entry(FS_PAGE_IDX_ADDR(page_num, i), entry_size);
+    uint32_t* free = _fs_find_free_entry(FS_PAGE_IDX_ADDR(page_num, i), entry_size);
 
-    if (page) {
-      return page;
+    if (free) {
+      return free;
     }
   }
 
@@ -277,11 +277,13 @@ uint32_t * _fs_cache_free_oldest(uint32_t cache_start, int page_count) {
   if (flash_erase(FS_ABS_IDX_PAGE(cache_start, page_idx), 1) || flash_copy(header, addr, 2)) {
     addr = NULL;
     goto ret;
+  } else {
+    addr = &addr[2];
   }
 
 ret:
   flash_lock();
-  return &addr[2];
+  return addr;
 }
 
 uint32_t* fs_cache_get_free(uint32_t cache_start, int page_count, int entry_size) {
@@ -293,3 +295,58 @@ uint32_t* fs_cache_get_free(uint32_t cache_start, int page_count, int entry_size
 
   return free_addr;
 }
+
+int _fs_write_entry(uint32_t* addr, const uint32_t* entry, int entry_size) {
+  int res = 0;
+
+  if(flash_unlock() || flash_copy(entry, addr, entry_size)) res = -1;
+
+  flash_lock();
+  return res;
+}
+
+int _fs_rewrite_entry(uint32_t page_num, int page_count, int entry_size, const uint32_t *entry) {
+  uint32_t *free = fs_swap_get_free();
+  if (!free) return -1;
+
+  int res = -1;
+
+  if (flash_unlock()) goto ret;
+
+  uint32_t header[2];
+  uint32_t* page = FS_PAGE_IDX_ADDR(page_num, 0);
+  FS_HEADER(header, page[0], (page[1] + 1));
+
+  if (flash_copy(entry, &free[2], entry_size)) goto ret;
+  if (flash_copy(header, free, 2)) goto ret;
+
+  for (int i = 1; i < page_count; i++) {
+    free = fs_swap_get_free();
+    if (!free) goto ret;
+    page = FS_PAGE_IDX_ADDR(page_num, i);
+    FS_HEADER(header, page[0], (page[1] + 1));
+    if (flash_copy(header, free, 2)) goto ret;
+  }
+
+  res = 0;
+
+ret:
+  flash_lock();
+  return res;
+}
+
+int fs_replace_entry(uint32_t page_num, int page_count, int entry_size, const uint32_t *entry) {
+  uint32_t* free = fs_find_free_entry(page_num, page_count, entry_size);
+
+  if (free) {
+    return _fs_write_entry(free, entry, entry_size);
+  } else {
+    return _fs_rewrite_entry(page_num, page_count, entry_size, entry) || fs_commit();
+  }
+}
+
+int fs_cache_entry(uint32_t cache_start, int page_count, int entry_size, const uint32_t *entry) {
+  uint32_t* free = fs_cache_get_free(cache_start, page_count, entry_size);
+  if (!free) return -1;
+  return _fs_write_entry(free, entry, entry_size);
+}
diff --git a/tests/fs_test.c b/tests/fs_test.c
index b9563c6..493ebc7 100644
--- a/tests/fs_test.c
+++ b/tests/fs_test.c
@@ -33,7 +33,7 @@ int flash_erase(uint8_t page, uint8_t page_count) {
   return 0;
 }
 
-int flash_copy(uint32_t* src, __IO uint32_t* dst, uint32_t size) {
+int flash_copy(const uint32_t* src, __IO uint32_t* dst, uint32_t size) {
   TEST_CHECK(!(flash_locked || (((uintptr_t) dst) % 4) || (size % 2)));
 
   for(int i = 0; i < size; i += 2) {
@@ -250,6 +250,80 @@ void test_fs_cache_get_free(void) {
   TEST_CHECK(page[1] == (FS_KEY_CACHE_COUNT + 1));
 }
 
+void test_fs_replace_entry(void) {
+  TEST_CHECK(!fs_init());
+
+  uint32_t entry[] = { 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F, 0X10 };
+  uint32_t* page = FS_PAGE_IDX_ADDR(FS_COUNTERS_PAGE, 0);
+
+  TEST_CHECK(!fs_replace_entry(FS_COUNTERS_PAGE, FS_COUNTERS_PAGE, 4, entry));
+  TEST_CHECK(!memcmp(entry, &page[2], 16));
+
+  for (int i = 0; i < FS_KEY_CACHE_COUNT; i++) {
+    page = FS_PAGE_IDX_ADDR(FS_COUNTERS_PAGE, i);
+
+    for (int j = 2; j < FLASH_PAGE_SIZE/4; j++) {
+      page[j] = 0xeeeeeeee;
+    }
+  }
+
+  page = FS_PAGE_IDX_ADDR(FS_COUNTERS_PAGE, 0);
+
+  TEST_CHECK(!fs_replace_entry(FS_COUNTERS_PAGE, FS_COUNTERS_PAGE, 4, entry));
+  TEST_CHECK(!memcmp(entry, &page[2], 16));
+  TEST_CHECK(page[0] == FS_V1_PAGE_ID(FS_COUNTERS_ID, 0));
+  TEST_CHECK(page[1] == 1);
+
+  for (int j = 6; j < FLASH_PAGE_SIZE/4; j++) {
+    TEST_CHECK(page[j] == 0xffffffff);
+  }
+
+  for (int i = 1; i < FS_COUNTERS_PAGE; i++) {
+    page = FS_PAGE_IDX_ADDR(FS_COUNTERS_PAGE, i);
+    TEST_CHECK(page[0] == FS_V1_PAGE_ID(FS_COUNTERS_ID, i));
+    TEST_CHECK(page[1] == 1);
+
+    for (int j = 2; j < FLASH_PAGE_SIZE/4; j++) {
+      TEST_CHECK(page[j] == 0xffffffff);
+    }
+  }
+}
+
+void test_fs_cache_entry(void) {
+  TEST_CHECK(!fs_init());
+
+  uint32_t entry[] = { 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F, 0X10 };
+  uint32_t* page = FS_PAGE_IDX_ADDR(FS_KEY_CACHE_PAGE, 0);
+
+  TEST_CHECK(!fs_cache_entry(FS_KEY_CACHE_PAGE, FS_KEY_CACHE_COUNT, 4, entry));
+  TEST_CHECK(!memcmp(entry, &page[2], 16));
+
+  for (int i = 0; i < FS_KEY_CACHE_COUNT; i++) {
+    page = FS_PAGE_IDX_ADDR(FS_KEY_CACHE_PAGE, i);
+
+    for (int j = 2; j < FLASH_PAGE_SIZE/4; j++) {
+      page[j] = 0xeeeeeeee;
+    }
+  }
+
+  page = FS_PAGE_IDX_ADDR(FS_KEY_CACHE_PAGE, 0);
+
+  TEST_CHECK(!fs_cache_entry(FS_KEY_CACHE_PAGE, FS_KEY_CACHE_COUNT, 4, entry));
+  TEST_CHECK(!memcmp(entry, &page[2], 16));
+
+  for (int j = 6; j < FLASH_PAGE_SIZE/4; j++) {
+    TEST_CHECK(page[j] == 0xffffffff);
+  }
+
+  for (int i = 1; i < FS_KEY_CACHE_COUNT; i++) {
+    page = FS_PAGE_IDX_ADDR(FS_KEY_CACHE_PAGE, i);
+
+    for (int j = 2; j < FLASH_PAGE_SIZE/4; j++) {
+      TEST_CHECK(page[j] == 0xeeeeeeee);
+    }
+  }
+}
+
 TEST_LIST = {
    { "fs_init", test_fs_init },
    { "fs_commit", test_fs_commit },
@@ -257,5 +331,7 @@ TEST_LIST = {
    { "fs_find_last_entry", test_fs_find_last_entry },
    { "fs_swap_get_free", test_fs_swap_get_free },
    { "fs_cache_get_free", test_fs_cache_get_free },
+   { "fs_replace_entry", test_fs_replace_entry },
+   { "fs_cache_entry", test_fs_cache_entry },
    { 0 }
 };