fix: [usbmass_bd] misalignment workaround

ps2dev · Apr 16, 2024 · ae42d16 · ae42d16
1 parent 6433719
commit ae42d16
Showing 1 changed file with 45 additions and 4 deletions.
diff --git a/iop/usb/usbmass_bd/src/scsi.c b/iop/usb/usbmass_bd/src/scsi.c
@@ -185,26 +185,48 @@ static int scsi_read(struct block_device *bd, u64 sector, void *buffer, u16 coun
 {
     struct scsi_interface *scsi = (struct scsi_interface *)bd->priv;
     u16 sc_remaining            = count;
+    unsigned int sectorSize     = bd->sectorSize;
+    void *misalign_buffer       = NULL;
     int retries;
 
     M_DEBUG("%s: sector=0x%08x%08x, count=%d\n", __func__, U64_2XU32(&sector), count);
 
+    if (((uiptr)buffer & 3) != 0) {
+        /* Slow misalignment workaround */
+        misalign_buffer = __builtin_alloca(sectorSize + 4);
+
+        if (((uiptr)misalign_buffer & 3) != 0)
+        {
+            misalign_buffer = (u8 *)misalign_buffer + (4 - ((uiptr)misalign_buffer & 3));
+        }
+    }
+
     while (sc_remaining > 0) {
         u16 sc = sc_remaining > scsi->max_sectors ? scsi->max_sectors : sc_remaining;
+        void *dst_buffer = buffer;
+
+        if (misalign_buffer != NULL) {
+            dst_buffer = misalign_buffer;
+            sc = 1;
+        }
 
         for (retries = SCSI_MAX_RETRIES; retries > 0; retries--) {
-            if (scsi_cmd_rw_sector(bd, sector, buffer, sc, 0) == 0)
+            if (scsi_cmd_rw_sector(bd, sector, dst_buffer, sc, 0) == 0)
                 break;
         }
 
+        if (misalign_buffer != NULL) {
+            memcpy(buffer, misalign_buffer, sectorSize);
+        }
+
         if (retries == 0) {
             M_PRINTF("ERROR: unable to read sector after %d tries (sector=0x%08x%08x, count=%d)\n", SCSI_MAX_RETRIES, U64_2XU32(&sector), count);
             return -EIO;
         }
 
         sc_remaining -= sc;
         sector += sc;
-        buffer = (u8 *)buffer + (sc * bd->sectorSize);
+        buffer = (u8 *)buffer + (sc * sectorSize);
     }
 
     return count;
@@ -214,15 +236,34 @@ static int scsi_write(struct block_device *bd, u64 sector, const void *buffer, u
 {
     struct scsi_interface *scsi = (struct scsi_interface *)bd->priv;
     u16 sc_remaining            = count;
+    unsigned int sectorSize     = bd->sectorSize;
+    void *misalign_buffer       = NULL;
     int retries;
 
     M_DEBUG("%s: sector=0x%08x%08x, count=%d\n", __func__, U64_2XU32(&sector), count);
 
+    if (((uiptr)buffer & 3) != 0) {
+        /* Slow misalignment workaround */
+        misalign_buffer = __builtin_alloca(sectorSize + 4);
+
+        if (((uiptr)misalign_buffer & 3) != 0)
+        {
+            misalign_buffer = (u8 *)misalign_buffer + (4 - ((uiptr)misalign_buffer & 3));
+        }
+    }
+
     while (sc_remaining > 0) {
         u16 sc = sc_remaining > scsi->max_sectors ? scsi->max_sectors : sc_remaining;
+        const void *dst_buffer = buffer;
+
+        if (misalign_buffer != NULL) {
+            memcpy(misalign_buffer, buffer, sectorSize);
+            dst_buffer = misalign_buffer;
+            sc = 1;
+        }
 
         for (retries = SCSI_MAX_RETRIES; retries > 0; retries--) {
-            if (scsi_cmd_rw_sector(bd, sector, buffer, sc, 1) == 0)
+            if (scsi_cmd_rw_sector(bd, sector, dst_buffer, sc, 1) == 0)
                 break;
         }
 
@@ -233,7 +274,7 @@ static int scsi_write(struct block_device *bd, u64 sector, const void *buffer, u
 
         sc_remaining -= sc;
         sector += sc;
-        buffer = (u8 *)buffer + (sc * bd->sectorSize);
+        buffer = (u8 *)buffer + (sc * sectorSize);
     }
 
     return count;