2 files changed, 95 insertions, 2 deletions
diff --git a/src/flash.c b/src/flash.c
new file mode 100644
index 0000000..b9ccf40
--- /dev/null
+++ b/src/flash.c
@@ -0,0 +1,93 @@
+#include "global.h"
+#include "gba/flash.h"
+#include "flash.h"
+
+extern u8 gFlashEnabled;
+
+extern FlashIntrFunc *sub_800B6E8(s32);
+extern void MemoryFill8(void *, u8, s32);
+extern void MemoryCopy8(void *, void *, s32);
+
+static u8 TryProgramSector(s32, u8 *);
+
+void InitFlash(void)
+{
+    if (!IdentifyFlash() && !SetFlashTimerIntr(2, sub_800B6E8(3)))
+        gFlashEnabled = TRUE;
+}
+
+void ClearFlashData(void)
+{
+    if (gFlashEnabled)
+        EraseFlashChip();
+}
+
+s32 ReadFlashData(s32 sector, u8 *dest, s32 size)
+{
+    if (gFlashEnabled)
+    {
+        u8 *_dest = dest;
+
+        while (size >= 0x1000)
+        {
+            ReadFlash(sector, 0, _dest, 0x1000);
+            sector++;
+            _dest += 0x1000;
+            size -= 0x1000;
+        }
+
+        if (size > 0)
+            ReadFlash(sector, 0, _dest, size);
+        return 0;
+    }
+
+    return 3;
+}
+
+s32 WriteFlashData(s32 sector, u8 *src, s32 size)
+{
+    u8 buffer[0x1000];
+
+    if (gFlashEnabled)
+    {
+        u8 flag = TRUE;
+        u8 *_src = src;
+
+        while (size >= 0x1000)
+        {
+            flag = TryProgramSector(sector, _src);
+            if (!flag)
+                break;
+            sector++;
+            _src += 0x1000;
+            size -= 0x1000;
+        }
+
+        if (flag)
+        {
+            if (size > 0)
+            {
+                MemoryFill8(buffer, 0xFF, 0x1000);
+                MemoryCopy8(buffer, _src, size);
+                flag = TryProgramSector(sector, buffer);
+            }
+            if (flag)
+                return 0;
+        }
+    }
+
+    return 3;
+}
+
+static u8 TryProgramSector(s32 sector, u8 *src)
+{
+    s32 tries = 3;
+
+    do
+    {
+        if (!ProgramFlashSector(sector, src) && !VerifyFlashSectorNBytes(sector, src, 0x1000))
+            return TRUE;
+    } while (tries-- > 0);
+
+    return FALSE;
+}
diff --git a/src/main.c b/src/main.c
index 59922dd..a756e28 100644
--- a/src/main.c
+++ b/src/main.c
@@ -1,5 +1,6 @@
 #include "global.h"
 #include "random.h"
+#include "flash.h"
 
 extern char ewram_start[];
 
@@ -26,7 +27,6 @@ extern char unk_code[];
 
 extern void sub_800CDA8(int);
 extern void sub_800B540(void);
-extern void sub_800CF0C(void);
 extern void sub_8004D78(void);
 extern void nullsub_9(void);
 extern void nullsub_6(void);
@@ -88,7 +88,7 @@ void AgbMain(void)
     REG_DISPCNT = 32704;
     sub_800CDA8(1);
     sub_800B540();
-    sub_800CF0C();
+    InitFlash();
     memcpy(seed, gUnknown_80B9BF1, 6);
     SeedRng(seed);
     sub_8004D78();