Skip to content
Snippets Groups Projects
Commit 564abb01 authored by Damien George's avatar Damien George
Browse files

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

Functionality is unchanged.
parent 1747d15c
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
extmod/vfs_fat_diskio.c
+ 66
− 122
View file @ 564abb01
......@@ -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,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;
}
}
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment