extmod/vfs_fat_diskio: Factor disk ioctl code to reduce code size.

Functionality is unchanged.

1 files changed, 66 insertions, 122 deletions

diff --git a/extmod/vfs_fat_diskio.c b/extmod/vfs_fat_diskio.c
index 712038f3e..fa013d278 100644
--- a/extmod/vfs_fat_diskio.c
+++ b/extmod/vfs_fat_diskio.c
@@ -54,59 +54,6 @@ STATIC fs_user_mount_t *disk_get_device(void *bdev) {
 }
 
 /*-----------------------------------------------------------------------*/
-/* Initialize a Drive                                                    */
-/*-----------------------------------------------------------------------*/
-
-STATIC
-DSTATUS disk_initialize (
-    bdev_t pdrv              /* Physical drive nmuber (0..) */
-)
-{
-    fs_user_mount_t *vfs = disk_get_device(pdrv);
-    if (vfs == NULL) {
-        return STA_NOINIT;
-    }
-
-    if (vfs->flags & FSUSER_HAVE_IOCTL) {
-        // new protocol with ioctl; call ioctl(INIT, 0)
-        vfs->u.ioctl[2] = MP_OBJ_NEW_SMALL_INT(BP_IOCTL_INIT);
-        vfs->u.ioctl[3] = MP_OBJ_NEW_SMALL_INT(0); // unused
-        mp_obj_t ret = mp_call_method_n_kw(2, 0, vfs->u.ioctl);
-        if (ret != mp_const_none && MP_OBJ_SMALL_INT_VALUE(ret) != 0) {
-            // error initialising
-            return STA_NOINIT;
-        }
-    }
-
-    if (vfs->writeblocks[0] == MP_OBJ_NULL) {
-        return STA_PROTECT;
-    } else {
-        return 0;
-    }
-}
-
-/*-----------------------------------------------------------------------*/
-/* Get Disk Status                                                       */
-/*-----------------------------------------------------------------------*/
-
-STATIC
-DSTATUS disk_status (
-    bdev_t pdrv      /* Physical drive nmuber (0..) */
-)
-{
-    fs_user_mount_t *vfs = disk_get_device(pdrv);
-    if (vfs == NULL) {
-        return STA_NOINIT;
-    }
-
-    if (vfs->writeblocks[0] == MP_OBJ_NULL) {
-        return STA_PROTECT;
-    } else {
-        return 0;
-    }
-}
-
-/*-----------------------------------------------------------------------*/
 /* Read Sector(s)                                                        */
 /*-----------------------------------------------------------------------*/
 
@@ -191,54 +138,21 @@ DRESULT disk_ioctl (
         return RES_PARERR;
     }
 
+    // First part: call the relevant method of the underlying block device
+    mp_obj_t ret = mp_const_none;
     if (vfs->flags & FSUSER_HAVE_IOCTL) {
         // new protocol with ioctl
-        switch (cmd) {
-            case CTRL_SYNC:
-                vfs->u.ioctl[2] = MP_OBJ_NEW_SMALL_INT(BP_IOCTL_SYNC);
-                vfs->u.ioctl[3] = MP_OBJ_NEW_SMALL_INT(0); // unused
-                mp_call_method_n_kw(2, 0, vfs->u.ioctl);
-                return RES_OK;
-
-            case GET_SECTOR_COUNT: {
-                vfs->u.ioctl[2] = MP_OBJ_NEW_SMALL_INT(BP_IOCTL_SEC_COUNT);
-                vfs->u.ioctl[3] = MP_OBJ_NEW_SMALL_INT(0); // unused
-                mp_obj_t ret = mp_call_method_n_kw(2, 0, vfs->u.ioctl);
-                *((DWORD*)buff) = mp_obj_get_int(ret);
-                return RES_OK;
-            }
-
-            case GET_SECTOR_SIZE: {
-                vfs->u.ioctl[2] = MP_OBJ_NEW_SMALL_INT(BP_IOCTL_SEC_SIZE);
-                vfs->u.ioctl[3] = MP_OBJ_NEW_SMALL_INT(0); // unused
-                mp_obj_t ret = mp_call_method_n_kw(2, 0, vfs->u.ioctl);
-                if (ret == mp_const_none) {
-                    // Default sector size
-                    *((WORD*)buff) = 512;
-                } else {
-                    *((WORD*)buff) = mp_obj_get_int(ret);
-                }
-                #if _MAX_SS != _MIN_SS
-                // need to store ssize because we use it in disk_read/disk_write
-                vfs->fatfs.ssize = *((WORD*)buff);
-                #endif
-                return RES_OK;
-            }
-
-            case GET_BLOCK_SIZE:
-                *((DWORD*)buff) = 1; // erase block size in units of sector size
-                return RES_OK;
-
-            case IOCTL_INIT:
-                *((DSTATUS*)buff) = disk_initialize(pdrv);
-                return RES_OK;
-
-            case IOCTL_STATUS:
-                *((DSTATUS*)buff) = disk_status(pdrv);
-                return RES_OK;
-
-            default:
-                return RES_PARERR;
+        static const uint8_t op_map[8] = {
+            [CTRL_SYNC] = BP_IOCTL_SYNC,
+            [GET_SECTOR_COUNT] = BP_IOCTL_SEC_COUNT,
+            [GET_SECTOR_SIZE] = BP_IOCTL_SEC_SIZE,
+            [IOCTL_INIT] = BP_IOCTL_INIT,
+        };
+        uint8_t bp_op = op_map[cmd & 7];
+        if (bp_op != 0) {
+            vfs->u.ioctl[2] = MP_OBJ_NEW_SMALL_INT(bp_op);
+            vfs->u.ioctl[3] = MP_OBJ_NEW_SMALL_INT(0); // unused
+            ret = mp_call_method_n_kw(2, 0, vfs->u.ioctl);
         }
     } else {
         // old protocol with sync and count
@@ -247,37 +161,67 @@ DRESULT disk_ioctl (
                 if (vfs->u.old.sync[0] != MP_OBJ_NULL) {
                     mp_call_method_n_kw(0, 0, vfs->u.old.sync);
                 }
-                return RES_OK;
+                break;
 
-            case GET_SECTOR_COUNT: {
-                mp_obj_t ret = mp_call_method_n_kw(0, 0, vfs->u.old.count);
-                *((DWORD*)buff) = mp_obj_get_int(ret);
-                return RES_OK;
-            }
+            case GET_SECTOR_COUNT:
+                ret = mp_call_method_n_kw(0, 0, vfs->u.old.count);
+                break;
 
             case GET_SECTOR_SIZE:
-                *((WORD*)buff) = 512; // old protocol had fixed sector size
-                #if _MAX_SS != _MIN_SS
-                // need to store ssize because we use it in disk_read/disk_write
-                vfs->fatfs.ssize = 512;
-                #endif
-                return RES_OK;
-
-            case GET_BLOCK_SIZE:
-                *((DWORD*)buff) = 1; // erase block size in units of sector size
-                return RES_OK;
+                // old protocol has fixed sector size of 512 bytes
+                break;
 
             case IOCTL_INIT:
-                *((DSTATUS*)buff) = disk_initialize(pdrv);
-                return RES_OK;
+                // old protocol doesn't have init
+                break;
+        }
+    }
 
-            case IOCTL_STATUS:
-                *((DSTATUS*)buff) = disk_status(pdrv);
-                return RES_OK;
+    // Second part: convert the result for return
+    switch (cmd) {
+        case CTRL_SYNC:
+            return RES_OK;
 
-            default:
-                return RES_PARERR;
+        case GET_SECTOR_COUNT: {
+            *((DWORD*)buff) = mp_obj_get_int(ret);
+            return RES_OK;
         }
+
+        case GET_SECTOR_SIZE: {
+            if (ret == mp_const_none) {
+                // Default sector size
+                *((WORD*)buff) = 512;
+            } else {
+                *((WORD*)buff) = mp_obj_get_int(ret);
+            }
+            #if _MAX_SS != _MIN_SS
+            // need to store ssize because we use it in disk_read/disk_write
+            vfs->fatfs.ssize = *((WORD*)buff);
+            #endif
+            return RES_OK;
+        }
+
+        case GET_BLOCK_SIZE:
+            *((DWORD*)buff) = 1; // erase block size in units of sector size
+            return RES_OK;
+
+        case IOCTL_INIT:
+        case IOCTL_STATUS: {
+            DSTATUS stat;
+            if (ret != mp_const_none && MP_OBJ_SMALL_INT_VALUE(ret) != 0) {
+                // error initialising
+                stat = STA_NOINIT;
+            } else if (vfs->writeblocks[0] == MP_OBJ_NULL) {
+                stat = STA_PROTECT;
+            } else {
+                stat = 0;
+            }
+            *((DSTATUS*)buff) = stat;
+            return RES_OK;
+        }
+
+        default:
+            return RES_PARERR;
     }
 }