From 9ed2f92cdceebfb82d3589d9252981e87e987c7b Mon Sep 17 00:00:00 2001
From: TuxSH <tuxsh@sfr.fr>
Date: Thu, 17 May 2018 19:53:42 +0200
Subject: [PATCH] [stage2] Fix some FS bugs

---
 fusee/fusee-secondary/src/device_partition.c |  4 +--
 fusee/fusee-secondary/src/device_partition.h |  4 +--
 fusee/fusee-secondary/src/switch_fs.c        | 36 +++++++++++++-------
 3 files changed, 28 insertions(+), 16 deletions(-)

diff --git a/fusee/fusee-secondary/src/device_partition.c b/fusee/fusee-secondary/src/device_partition.c
index 4fbfc120d..5d2534bdb 100644
--- a/fusee/fusee-secondary/src/device_partition.c
+++ b/fusee/fusee-secondary/src/device_partition.c
@@ -9,7 +9,7 @@ int device_partition_read_data(device_partition_t *devpart, void *dst, uint64_t
             return rc;
         }
     }
-    if (devpart->read_cipher != NULL) {
+    if (devpart->read_cipher != NULL && devpart->crypto_mode != DevicePartitionCryptoMode_None) {
         for (uint64_t i = 0; i < num_sectors; i += devpart->crypto_work_buffer_num_sectors) {
             uint64_t n = (i + devpart->crypto_work_buffer_num_sectors > num_sectors) ? (num_sectors - i) : devpart->crypto_work_buffer_num_sectors;
             rc = devpart->reader(devpart, devpart->crypto_work_buffer, sector + i, n);
@@ -38,7 +38,7 @@ int device_partition_write_data(device_partition_t *devpart, const void *src, ui
         }
     }
 
-    if (devpart->read_cipher != NULL) {
+    if (devpart->read_cipher != NULL && devpart->crypto_mode != DevicePartitionCryptoMode_None) {
         for (uint64_t i = 0; i < num_sectors; i += devpart->crypto_work_buffer_num_sectors) {
             uint64_t n = (i + devpart->crypto_work_buffer_num_sectors > num_sectors) ? (num_sectors - i) : devpart->crypto_work_buffer_num_sectors;
             memcpy(devpart->crypto_work_buffer, src + (size_t)(devpart->sector_size * i), (size_t)(devpart->sector_size * n)); 
diff --git a/fusee/fusee-secondary/src/device_partition.h b/fusee/fusee-secondary/src/device_partition.h
index 6e908f8a6..e0198d1fc 100644
--- a/fusee/fusee-secondary/src/device_partition.h
+++ b/fusee/fusee-secondary/src/device_partition.h
@@ -46,8 +46,8 @@ typedef struct device_partition_t {
     void *crypto_work_buffer; /* Work buffer for crypto. */
     uint64_t crypto_work_buffer_num_sectors; /* Size of the crypto work buffer in sectors. */
 
-    uint8_t keys[DEVPART_KEY_MAX][DEVPART_KEY_MAX_SIZE]; /* Key. */
-    uint8_t iv[DEVPART_IV_MAX_SIZE]; /* IV. */
+    uint8_t __attribute__((aligned(16))) keys[DEVPART_KEY_MAX][DEVPART_KEY_MAX_SIZE]; /* Key. */
+    uint8_t __attribute__((aligned(16))) iv[DEVPART_IV_MAX_SIZE]; /* IV. */
     bool initialized;
 } device_partition_t;
 
diff --git a/fusee/fusee-secondary/src/switch_fs.c b/fusee/fusee-secondary/src/switch_fs.c
index c1eb2be72..67a97fa26 100644
--- a/fusee/fusee-secondary/src/switch_fs.c
+++ b/fusee/fusee-secondary/src/switch_fs.c
@@ -41,19 +41,30 @@ static int mmc_partition_initialize(device_partition_t *devpart) {
         g_ahb_redirect_enabled = true;
     }
 
-    if (mmcpart->mmc == &g_sd_mmc && !g_sd_mmc_initialized) {
-        int rc = sdmmc_init(mmcpart->mmc, mmcpart->controller);
-        if (rc == 0) {
-            sdmmc_set_write_enable(mmcpart->mmc, SDMMC_WRITE_ENABLED);
-            devpart->initialized = g_sd_mmc_initialized = true;
+    if (mmcpart->mmc == &g_sd_mmc) {
+        if (!g_sd_mmc_initialized) {
+            int rc = sdmmc_init(mmcpart->mmc, mmcpart->controller);
+            if (rc == 0) {
+                sdmmc_set_write_enable(mmcpart->mmc, SDMMC_WRITE_ENABLED);
+                g_sd_mmc_initialized = true;
+            }
+            else {
+                return rc;
+            }
         }
-        return rc;
-    } else if (mmcpart->mmc == &g_nand_mmc && !g_nand_mmc_initialized) {
-        int rc = sdmmc_init(mmcpart->mmc, mmcpart->controller);
-        if (rc == 0) {
-            devpart->initialized = g_nand_mmc_initialized = true;
+        devpart->initialized = true;
+        return 0;
+    } else if (mmcpart->mmc == &g_nand_mmc) {
+        if (!g_nand_mmc_initialized) {
+            int rc = sdmmc_init(mmcpart->mmc, mmcpart->controller);
+            if (rc == 0) {
+                g_nand_mmc_initialized = true;
+            } else {
+                return rc;
+            }
         }
-        return rc;
+        devpart->initialized = true;
+        return 0;
     }
 
     return 0;
@@ -63,7 +74,7 @@ static void mmc_partition_finalize(device_partition_t *devpart) {
     free(devpart->crypto_work_buffer);
 }
 
-static enum sdmmc_partition g_current_emmc_partition = SDMMC_PARTITION_USER;
+static enum sdmmc_partition g_current_emmc_partition = (enum sdmmc_partition)-1;
 
 static int mmc_partition_read(device_partition_t *devpart, void *dst, uint64_t sector, uint64_t num_sectors) {
     mmc_partition_info_t *mmcpart = (mmc_partition_info_t *)devpart->device_struct;
@@ -192,6 +203,7 @@ static int switchfs_mount_partition_gpt_callback(const efi_entry_t *entry, void
             if (known_partitions[i].is_encrypted) {
                 devpart.read_cipher = switchfs_bis_crypto_decrypt;
                 devpart.write_cipher = switchfs_bis_crypto_encrypt;
+                devpart.crypto_mode = DevicePartitionCryptoMode_Xts;
             }
 
             if (known_partitions[i].is_fat) {