From 564abb01a5526bd2d981791401e28774c7dcfe4c Mon Sep 17 00:00:00 2001 From: Damien George Date: Sat, 16 Jun 2018 18:21:42 +1000 Subject: [PATCH] extmod/vfs_fat_diskio: Factor disk ioctl code to reduce code size. Functionality is unchanged. --- extmod/vfs_fat_diskio.c | 192 ++++++++++++++-------------------------- 1 file changed, 68 insertions(+), 124 deletions(-) diff --git a/extmod/vfs_fat_diskio.c b/extmod/vfs_fat_diskio.c index 712038f3e9..fa013d2786 100644 --- a/extmod/vfs_fat_diskio.c +++ b/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) { - 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,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: { - 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; - - case IOCTL_STATUS: - *((DSTATUS*)buff) = disk_status(pdrv); - return RES_OK; - - default: - return RES_PARERR; + // old protocol doesn't have init + break; } } + + // 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