Skip to content

Commit 564abb0

Browse files
dpgeorgedpgeorge
committed
extmod/vfs_fat_diskio: Factor disk ioctl code to reduce code size.
Functionality is unchanged.
1 parent 1747d15 commit 564abb0

1 file changed

Lines changed: 66 additions & 122 deletions

File tree

‎extmod/vfs_fat_diskio.c‎

Lines changed: 66 additions & 122 deletions
Original file line numberDiff line numberDiff line change
@@ -53,59 +53,6 @@ STATIC fs_user_mount_t *disk_get_device(void *bdev) {
5353
return (fs_user_mount_t*)bdev;
5454
}
5555

56-
/*-----------------------------------------------------------------------*/
57-
/* Initialize a Drive */
58-
/*-----------------------------------------------------------------------*/
59-
60-
STATIC
61-
DSTATUS disk_initialize (
62-
bdev_t pdrv /* Physical drive nmuber (0..) */
63-
)
64-
{
65-
fs_user_mount_t *vfs = disk_get_device(pdrv);
66-
if (vfs == NULL) {
67-
return STA_NOINIT;
68-
}
69-
70-
if (vfs->flags & FSUSER_HAVE_IOCTL) {
71-
// new protocol with ioctl; call ioctl(INIT, 0)
72-
vfs->u.ioctl[2] = MP_OBJ_NEW_SMALL_INT(BP_IOCTL_INIT);
73-
vfs->u.ioctl[3] = MP_OBJ_NEW_SMALL_INT(0); // unused
74-
mp_obj_t ret = mp_call_method_n_kw(2, 0, vfs->u.ioctl);
75-
if (ret != mp_const_none && MP_OBJ_SMALL_INT_VALUE(ret) != 0) {
76-
// error initialising
77-
return STA_NOINIT;
78-
}
79-
}
80-
81-
if (vfs->writeblocks[0] == MP_OBJ_NULL) {
82-
return STA_PROTECT;
83-
} else {
84-
return 0;
85-
}
86-
}
87-
88-
/*-----------------------------------------------------------------------*/
89-
/* Get Disk Status */
90-
/*-----------------------------------------------------------------------*/
91-
92-
STATIC
93-
DSTATUS disk_status (
94-
bdev_t pdrv /* Physical drive nmuber (0..) */
95-
)
96-
{
97-
fs_user_mount_t *vfs = disk_get_device(pdrv);
98-
if (vfs == NULL) {
99-
return STA_NOINIT;
100-
}
101-
102-
if (vfs->writeblocks[0] == MP_OBJ_NULL) {
103-
return STA_PROTECT;
104-
} else {
105-
return 0;
106-
}
107-
}
108-
10956
/*-----------------------------------------------------------------------*/
11057
/* Read Sector(s) */
11158
/*-----------------------------------------------------------------------*/
@@ -191,54 +138,21 @@ DRESULT disk_ioctl (
191138
return RES_PARERR;
192139
}
193140

141+
// First part: call the relevant method of the underlying block device
142+
mp_obj_t ret = mp_const_none;
194143
if (vfs->flags & FSUSER_HAVE_IOCTL) {
195144
// new protocol with ioctl
196-
switch (cmd) {
197-
case CTRL_SYNC:
198-
vfs->u.ioctl[2] = MP_OBJ_NEW_SMALL_INT(BP_IOCTL_SYNC);
199-
vfs->u.ioctl[3] = MP_OBJ_NEW_SMALL_INT(0); // unused
200-
mp_call_method_n_kw(2, 0, vfs->u.ioctl);
201-
return RES_OK;
202-
203-
case GET_SECTOR_COUNT: {
204-
vfs->u.ioctl[2] = MP_OBJ_NEW_SMALL_INT(BP_IOCTL_SEC_COUNT);
205-
vfs->u.ioctl[3] = MP_OBJ_NEW_SMALL_INT(0); // unused
206-
mp_obj_t ret = mp_call_method_n_kw(2, 0, vfs->u.ioctl);
207-
*((DWORD*)buff) = mp_obj_get_int(ret);
208-
return RES_OK;
209-
}
210-
211-
case GET_SECTOR_SIZE: {
212-
vfs->u.ioctl[2] = MP_OBJ_NEW_SMALL_INT(BP_IOCTL_SEC_SIZE);
213-
vfs->u.ioctl[3] = MP_OBJ_NEW_SMALL_INT(0); // unused
214-
mp_obj_t ret = mp_call_method_n_kw(2, 0, vfs->u.ioctl);
215-
if (ret == mp_const_none) {
216-
// Default sector size
217-
*((WORD*)buff) = 512;
218-
} else {
219-
*((WORD*)buff) = mp_obj_get_int(ret);
220-
}
221-
#if _MAX_SS != _MIN_SS
222-
// need to store ssize because we use it in disk_read/disk_write
223-
vfs->fatfs.ssize = *((WORD*)buff);
224-
#endif
225-
return RES_OK;
226-
}
227-
228-
case GET_BLOCK_SIZE:
229-
*((DWORD*)buff) = 1; // erase block size in units of sector size
230-
return RES_OK;
231-
232-
case IOCTL_INIT:
233-
*((DSTATUS*)buff) = disk_initialize(pdrv);
234-
return RES_OK;
235-
236-
case IOCTL_STATUS:
237-
*((DSTATUS*)buff) = disk_status(pdrv);
238-
return RES_OK;
239-
240-
default:
241-
return RES_PARERR;
145+
static const uint8_t op_map[8] = {
146+
[CTRL_SYNC] = BP_IOCTL_SYNC,
147+
[GET_SECTOR_COUNT] = BP_IOCTL_SEC_COUNT,
148+
[GET_SECTOR_SIZE] = BP_IOCTL_SEC_SIZE,
149+
[IOCTL_INIT] = BP_IOCTL_INIT,
150+
};
151+
uint8_t bp_op = op_map[cmd & 7];
152+
if (bp_op != 0) {
153+
vfs->u.ioctl[2] = MP_OBJ_NEW_SMALL_INT(bp_op);
154+
vfs->u.ioctl[3] = MP_OBJ_NEW_SMALL_INT(0); // unused
155+
ret = mp_call_method_n_kw(2, 0, vfs->u.ioctl);
242156
}
243157
} else {
244158
// old protocol with sync and count
@@ -247,37 +161,67 @@ DRESULT disk_ioctl (
247161
if (vfs->u.old.sync[0] != MP_OBJ_NULL) {
248162
mp_call_method_n_kw(0, 0, vfs->u.old.sync);
249163
}
250-
return RES_OK;
164+
break;
251165

252-
case GET_SECTOR_COUNT: {
253-
mp_obj_t ret = mp_call_method_n_kw(0, 0, vfs->u.old.count);
254-
*((DWORD*)buff) = mp_obj_get_int(ret);
255-
return RES_OK;
256-
}
166+
case GET_SECTOR_COUNT:
167+
ret = mp_call_method_n_kw(0, 0, vfs->u.old.count);
168+
break;
257169

258170
case GET_SECTOR_SIZE:
259-
*((WORD*)buff) = 512; // old protocol had fixed sector size
260-
#if _MAX_SS != _MIN_SS
261-
// need to store ssize because we use it in disk_read/disk_write
262-
vfs->fatfs.ssize = 512;
263-
#endif
264-
return RES_OK;
265-
266-
case GET_BLOCK_SIZE:
267-
*((DWORD*)buff) = 1; // erase block size in units of sector size
268-
return RES_OK;
171+
// old protocol has fixed sector size of 512 bytes
172+
break;
269173

270174
case IOCTL_INIT:
271-
*((DSTATUS*)buff) = disk_initialize(pdrv);
272-
return RES_OK;
175+
// old protocol doesn't have init
176+
break;
177+
}
178+
}
273179

274-
case IOCTL_STATUS:
275-
*((DSTATUS*)buff) = disk_status(pdrv);
276-
return RES_OK;
180+
// Second part: convert the result for return
181+
switch (cmd) {
182+
case CTRL_SYNC:
183+
return RES_OK;
277184

278-
default:
279-
return RES_PARERR;
185+
case GET_SECTOR_COUNT: {
186+
*((DWORD*)buff) = mp_obj_get_int(ret);
187+
return RES_OK;
280188
}
189+
190+
case GET_SECTOR_SIZE: {
191+
if (ret == mp_const_none) {
192+
// Default sector size
193+
*((WORD*)buff) = 512;
194+
} else {
195+
*((WORD*)buff) = mp_obj_get_int(ret);
196+
}
197+
#if _MAX_SS != _MIN_SS
198+
// need to store ssize because we use it in disk_read/disk_write
199+
vfs->fatfs.ssize = *((WORD*)buff);
200+
#endif
201+
return RES_OK;
202+
}
203+
204+
case GET_BLOCK_SIZE:
205+
*((DWORD*)buff) = 1; // erase block size in units of sector size
206+
return RES_OK;
207+
208+
case IOCTL_INIT:
209+
case IOCTL_STATUS: {
210+
DSTATUS stat;
211+
if (ret != mp_const_none && MP_OBJ_SMALL_INT_VALUE(ret) != 0) {
212+
// error initialising
213+
stat = STA_NOINIT;
214+
} else if (vfs->writeblocks[0] == MP_OBJ_NULL) {
215+
stat = STA_PROTECT;
216+
} else {
217+
stat = 0;
218+
}
219+
*((DSTATUS*)buff) = stat;
220+
return RES_OK;
221+
}
222+
223+
default:
224+
return RES_PARERR;
281225
}
282226
}
283227

0 commit comments

Comments
 (0)