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

Functionality is unchanged.

extmod/vfs_fat_diskio.c

@@ -53,59 +53,6 @@ STATIC fs_user_mount_t *disk_get_device(void *bdev) {
     return (fs_user_mount_t*)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) {
+        static const uint8_t op_map[8] = {
-            case CTRL_SYNC:
+            [CTRL_SYNC] = BP_IOCTL_SYNC,
-                vfs->u.ioctl[2] = MP_OBJ_NEW_SMALL_INT(BP_IOCTL_SYNC);
+            [GET_SECTOR_COUNT] = BP_IOCTL_SEC_COUNT,
-                vfs->u.ioctl[3] = MP_OBJ_NEW_SMALL_INT(0); // unused
+            [GET_SECTOR_SIZE] = BP_IOCTL_SEC_SIZE,
-                mp_call_method_n_kw(2, 0, vfs->u.ioctl);
+            [IOCTL_INIT] = BP_IOCTL_INIT,
-                return RES_OK;
+        };
-
+        uint8_t bp_op = op_map[cmd & 7];
-            case GET_SECTOR_COUNT: {
+        if (bp_op != 0) {
-                vfs->u.ioctl[2] = MP_OBJ_NEW_SMALL_INT(BP_IOCTL_SEC_COUNT);
+            vfs->u.ioctl[2] = MP_OBJ_NEW_SMALL_INT(bp_op);
-                vfs->u.ioctl[3] = MP_OBJ_NEW_SMALL_INT(0); // unused
+            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);
+            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;
         }
     } else {
         // old protocol with sync and count
@@ -247,38 +161,68 @@ 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: {
+            case GET_SECTOR_COUNT:
-                mp_obj_t ret = mp_call_method_n_kw(0, 0, vfs->u.old.count);
+                ret = mp_call_method_n_kw(0, 0, vfs->u.old.count);
-                *((DWORD*)buff) = mp_obj_get_int(ret);
+                break;
-                return RES_OK;
-            }
 
             case GET_SECTOR_SIZE:
-                *((WORD*)buff) = 512; // old protocol had fixed sector size
+                // old protocol has fixed sector size of 512 bytes
-                #if _MAX_SS != _MIN_SS
+                break;
-                // 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;
 
             case IOCTL_INIT:
-                *((DSTATUS*)buff) = disk_initialize(pdrv);
+                // old protocol doesn't have init
-                return RES_OK;
+                break;
-
-            case IOCTL_STATUS:
-                *((DSTATUS*)buff) = disk_status(pdrv);
-                return RES_OK;
-
-            default:
-                return RES_PARERR;
         }
     }
+
+    // Second part: convert the result for return
+    switch (cmd) {
+        case CTRL_SYNC:
+            return RES_OK;
+
+        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;
+    }
 }
 
 #endif // MICROPY_VFS && MICROPY_VFS_FAT