diff --git a/os/hal/lib/complex/mfs/mfs.c b/os/hal/lib/complex/mfs/mfs.c
index 87ff1f365..8cede8416 100644
--- a/os/hal/lib/complex/mfs/mfs.c
+++ b/os/hal/lib/complex/mfs/mfs.c
@@ -196,6 +196,7 @@ static mfs_error_t mfs_flash_write(MFSDriver *mfsp,
       return MFS_ERR_FLASH_FAILURE;
     }
     n -= chunk;
+    offset += (flash_offset_t)chunk;
     wp += chunk;
   }
 #endif
diff --git a/test/mfs/configuration.xml b/test/mfs/configuration.xml
index 3b8a2b56c..58ee9c858 100644
--- a/test/mfs/configuration.xml
+++ b/test/mfs/configuration.xml
@@ -534,14 +534,16 @@ remaining = (size_t)flashGetSectorOffset(mfscfg1.flashp, mfscfg1.bank0_start) +
             (size_t)mfscfg1.bank_size - (size_t)mfs1.next_offset;
 test_assert(remaining >= sizeof (mfs_data_header_t), "not enough space");
 
-if (remaining > sizeof (mfs_data_header_t) * 2U) {
-  err = mfsWriteRecord(&mfs1, MFS_CFG_MAX_RECORDS - 1U,
-                       remaining - (sizeof (mfs_data_header_t) * 2U),
+if (remaining > sizeof (mfs_data_header_t) * 2) {
+  err = mfsWriteRecord(&mfs1, MFS_CFG_MAX_RECORDS,
+                       remaining - (sizeof (mfs_data_header_t) * 2),
                        pattern512);
   test_assert(err == MFS_NO_ERROR, "error filling remaining space");
+  err = mfsEraseRecord(&mfs1, MFS_CFG_MAX_RECORDS);
+  test_assert(err == MFS_NO_ERROR, "error filling remaining space");
 }
 else {
-  if (remaining == sizeof (mfs_data_header_t) * 2U) {
+  if (remaining == sizeof (mfs_data_header_t) * 2) {
     err = mfsEraseRecord(&mfs1, 2);
     test_assert(err == MFS_NO_ERROR, "error filling remaining space");
   }
diff --git a/test/mfs/source/test/mfs_test_sequence_001.c b/test/mfs/source/test/mfs_test_sequence_001.c
index 5ff4e7958..9eb86c635 100644
--- a/test/mfs/source/test/mfs_test_sequence_001.c
+++ b/test/mfs/source/test/mfs_test_sequence_001.c
@@ -452,14 +452,16 @@ static void mfs_test_001_005_execute(void) {
                 (size_t)mfscfg1.bank_size - (size_t)mfs1.next_offset;
     test_assert(remaining >= sizeof (mfs_data_header_t), "not enough space");
 
-    if (remaining > sizeof (mfs_data_header_t) * 2U) {
-      err = mfsWriteRecord(&mfs1, MFS_CFG_MAX_RECORDS - 1U,
-                           remaining - (sizeof (mfs_data_header_t) * 2U),
+    if (remaining > sizeof (mfs_data_header_t) * 2) {
+      err = mfsWriteRecord(&mfs1, MFS_CFG_MAX_RECORDS,
+                           remaining - (sizeof (mfs_data_header_t) * 2),
                            pattern512);
       test_assert(err == MFS_NO_ERROR, "error filling remaining space");
+      err = mfsEraseRecord(&mfs1, MFS_CFG_MAX_RECORDS);
+      test_assert(err == MFS_NO_ERROR, "error filling remaining space");
     }
     else {
-      if (remaining == sizeof (mfs_data_header_t) * 2U) {
+      if (remaining == sizeof (mfs_data_header_t) * 2) {
         err = mfsEraseRecord(&mfs1, 2);
         test_assert(err == MFS_NO_ERROR, "error filling remaining space");
       }