extmod/vfs_blockdev: Factor out block device interface code.

5 files changed, 140 insertions, 85 deletions

diff --git a/extmod/vfs.h b/extmod/vfs.h
index 85b020faa..e626a14df 100644
--- a/extmod/vfs.h
+++ b/extmod/vfs.h
@@ -58,6 +58,7 @@ typedef struct _mp_vfs_proto_t {
 
 typedef struct _mp_vfs_blockdev_t {
     uint16_t flags;
+    size_t block_size;
     mp_obj_t readblocks[4];
     mp_obj_t writeblocks[4];
     // new protocol uses just ioctl, old uses sync (optional) and count
@@ -77,6 +78,11 @@ typedef struct _mp_vfs_mount_t {
     struct _mp_vfs_mount_t *next;
 } mp_vfs_mount_t;
 
+void mp_vfs_blockdev_init(mp_vfs_blockdev_t *self, mp_obj_t bdev);
+int mp_vfs_blockdev_read(mp_vfs_blockdev_t *self, size_t block_num, size_t num_blocks, uint8_t *buf);
+int mp_vfs_blockdev_write(mp_vfs_blockdev_t *self, size_t block_num, size_t num_blocks, const uint8_t *buf);
+mp_obj_t mp_vfs_blockdev_ioctl(mp_vfs_blockdev_t *self, uintptr_t cmd, uintptr_t arg);
+
 mp_vfs_mount_t *mp_vfs_lookup_path(const char *path, const char **path_out);
 mp_import_stat_t mp_vfs_import_stat(const char *path);
 mp_obj_t mp_vfs_mount(size_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args);
diff --git a/extmod/vfs_blockdev.c b/extmod/vfs_blockdev.c
new file mode 100644
index 000000000..0bc0fdebf
--- /dev/null
+++ b/extmod/vfs_blockdev.c
@@ -0,0 +1,112 @@
+/*
+ * This file is part of the MicroPython project, http://micropython.org/
+ *
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2013-2019 Damien P. George
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "py/runtime.h"
+#include "py/binary.h"
+#include "py/objarray.h"
+#include "py/mperrno.h"
+#include "extmod/vfs.h"
+
+#if MICROPY_VFS
+
+void mp_vfs_blockdev_init(mp_vfs_blockdev_t *self, mp_obj_t bdev) {
+    mp_load_method(bdev, MP_QSTR_readblocks, self->readblocks);
+    mp_load_method_maybe(bdev, MP_QSTR_writeblocks, self->writeblocks);
+    mp_load_method_maybe(bdev, MP_QSTR_ioctl, self->u.ioctl);
+    if (self->u.ioctl[0] != MP_OBJ_NULL) {
+        // Device supports new block protocol, so indicate it
+        self->flags |= MP_BLOCKDEV_FLAG_HAVE_IOCTL;
+    } else {
+        // No ioctl method, so assume the device uses the old block protocol
+        mp_load_method_maybe(bdev, MP_QSTR_sync, self->u.old.sync);
+        mp_load_method(bdev, MP_QSTR_count, self->u.old.count);
+    }
+}
+
+int mp_vfs_blockdev_read(mp_vfs_blockdev_t *self, size_t block_num, size_t num_blocks, uint8_t *buf) {
+    if (self->flags & MP_BLOCKDEV_FLAG_NATIVE) {
+        mp_uint_t (*f)(uint8_t*, uint32_t, uint32_t) = (void*)(uintptr_t)self->readblocks[2];
+        return f(buf, block_num, num_blocks);
+    } else {
+        mp_obj_array_t ar = {{&mp_type_bytearray}, BYTEARRAY_TYPECODE, 0, num_blocks * self->block_size, buf};
+        self->readblocks[2] = MP_OBJ_NEW_SMALL_INT(block_num);
+        self->readblocks[3] = MP_OBJ_FROM_PTR(&ar);
+        mp_call_method_n_kw(2, 0, self->readblocks);
+        // TODO handle error return
+        return 0;
+    }
+}
+
+int mp_vfs_blockdev_write(mp_vfs_blockdev_t *self, size_t block_num, size_t num_blocks, const uint8_t *buf) {
+    if (self->writeblocks[0] == MP_OBJ_NULL) {
+        // read-only block device
+        return -MP_EROFS;
+    }
+
+    if (self->flags & MP_BLOCKDEV_FLAG_NATIVE) {
+        mp_uint_t (*f)(const uint8_t*, uint32_t, uint32_t) = (void*)(uintptr_t)self->writeblocks[2];
+        return f(buf, block_num, num_blocks);
+    } else {
+        mp_obj_array_t ar = {{&mp_type_bytearray}, BYTEARRAY_TYPECODE, 0, num_blocks * self->block_size, (void*)buf};
+        self->writeblocks[2] = MP_OBJ_NEW_SMALL_INT(block_num);
+        self->writeblocks[3] = MP_OBJ_FROM_PTR(&ar);
+        mp_call_method_n_kw(2, 0, self->writeblocks);
+        // TODO handle error return
+        return 0;
+    }
+}
+
+mp_obj_t mp_vfs_blockdev_ioctl(mp_vfs_blockdev_t *self, uintptr_t cmd, uintptr_t arg) {
+    if (self->flags & MP_BLOCKDEV_FLAG_HAVE_IOCTL) {
+        // New protocol with ioctl
+        self->u.ioctl[2] = MP_OBJ_NEW_SMALL_INT(cmd);
+        self->u.ioctl[3] = MP_OBJ_NEW_SMALL_INT(arg);
+        return mp_call_method_n_kw(2, 0, self->u.ioctl);
+    } else {
+        // Old protocol with sync and count
+        switch (cmd) {
+            case BP_IOCTL_SYNC:
+                if (self->u.old.sync[0] != MP_OBJ_NULL) {
+                    mp_call_method_n_kw(0, 0, self->u.old.sync);
+                }
+                break;
+
+            case BP_IOCTL_SEC_COUNT:
+                return mp_call_method_n_kw(0, 0, self->u.old.count);
+
+            case BP_IOCTL_SEC_SIZE:
+                // Old protocol has fixed sector size of 512 bytes
+                break;
+
+            case BP_IOCTL_INIT:
+                // Old protocol doesn't have init
+                break;
+        }
+        return mp_const_none;
+    }
+}
+
+#endif // MICROPY_VFS
diff --git a/extmod/vfs_fat.c b/extmod/vfs_fat.c
index dcfb677b1..129b6cc66 100644
--- a/extmod/vfs_fat.c
+++ b/extmod/vfs_fat.c
@@ -68,21 +68,12 @@ STATIC mp_obj_t fat_vfs_make_new(const mp_obj_type_t *type, size_t n_args, size_
     // create new object
     fs_user_mount_t *vfs = m_new_obj(fs_user_mount_t);
     vfs->base.type = type;
-    vfs->blockdev.flags = MP_BLOCKDEV_FLAG_FREE_OBJ;
     vfs->fatfs.drv = vfs;
 
-    // load block protocol methods
-    mp_load_method(args[0], MP_QSTR_readblocks, vfs->blockdev.readblocks);
-    mp_load_method_maybe(args[0], MP_QSTR_writeblocks, vfs->blockdev.writeblocks);
-    mp_load_method_maybe(args[0], MP_QSTR_ioctl, vfs->blockdev.u.ioctl);
-    if (vfs->blockdev.u.ioctl[0] != MP_OBJ_NULL) {
-        // device supports new block protocol, so indicate it
-        vfs->blockdev.flags |= MP_BLOCKDEV_FLAG_HAVE_IOCTL;
-    } else {
-        // no ioctl method, so assume the device uses the old block protocol
-        mp_load_method_maybe(args[0], MP_QSTR_sync, vfs->blockdev.u.old.sync);
-        mp_load_method(args[0], MP_QSTR_count, vfs->blockdev.u.old.count);
-    }
+    // Initialise underlying block device
+    vfs->blockdev.flags = MP_BLOCKDEV_FLAG_FREE_OBJ;
+    vfs->blockdev.block_size = FF_MIN_SS; // default, will be populated by call to BP_IOCTL_SEC_SIZE
+    mp_vfs_blockdev_init(&vfs->blockdev, args[0]);
 
     // mount the block device so the VFS methods can be used
     FRESULT res = f_mount(&vfs->fatfs);
diff --git a/extmod/vfs_fat_diskio.c b/extmod/vfs_fat_diskio.c
index 25b131067..61a4d6da5 100644
--- a/extmod/vfs_fat_diskio.c
+++ b/extmod/vfs_fat_diskio.c
@@ -38,16 +38,11 @@
 #include "py/runtime.h"
 #include "py/binary.h"
 #include "py/objarray.h"
+#include "py/mperrno.h"
 #include "lib/oofatfs/ff.h"
 #include "lib/oofatfs/diskio.h"
 #include "extmod/vfs_fat.h"
 
-#if FF_MAX_SS == FF_MIN_SS
-#define SECSIZE(fs) (FF_MIN_SS)
-#else
-#define SECSIZE(fs) ((fs)->ssize)
-#endif
-
 typedef void *bdev_t;
 STATIC fs_user_mount_t *disk_get_device(void *bdev) {
     return (fs_user_mount_t*)bdev;
@@ -69,20 +64,9 @@ DRESULT disk_read (
         return RES_PARERR;
     }
 
-    if (vfs->blockdev.flags & MP_BLOCKDEV_FLAG_NATIVE) {
-        mp_uint_t (*f)(uint8_t*, uint32_t, uint32_t) = (void*)(uintptr_t)vfs->blockdev.readblocks[2];
-        if (f(buff, sector, count) != 0) {
-            return RES_ERROR;
-        }
-    } else {
-        mp_obj_array_t ar = {{&mp_type_bytearray}, BYTEARRAY_TYPECODE, 0, count * SECSIZE(&vfs->fatfs), buff};
-        vfs->blockdev.readblocks[2] = MP_OBJ_NEW_SMALL_INT(sector);
-        vfs->blockdev.readblocks[3] = MP_OBJ_FROM_PTR(&ar);
-        mp_call_method_n_kw(2, 0, vfs->blockdev.readblocks);
-        // TODO handle error return
-    }
+    int ret = mp_vfs_blockdev_read(&vfs->blockdev, sector, count, buff);
 
-    return RES_OK;
+    return ret == 0 ? RES_OK : RES_ERROR;
 }
 
 /*-----------------------------------------------------------------------*/
@@ -101,25 +85,14 @@ DRESULT disk_write (
         return RES_PARERR;
     }
 
-    if (vfs->blockdev.writeblocks[0] == MP_OBJ_NULL) {
+    int ret = mp_vfs_blockdev_write(&vfs->blockdev, sector, count, buff);
+
+    if (ret == -MP_EROFS) {
         // read-only block device
         return RES_WRPRT;
     }
 
-    if (vfs->blockdev.flags & MP_BLOCKDEV_FLAG_NATIVE) {
-        mp_uint_t (*f)(const uint8_t*, uint32_t, uint32_t) = (void*)(uintptr_t)vfs->blockdev.writeblocks[2];
-        if (f(buff, sector, count) != 0) {
-            return RES_ERROR;
-        }
-    } else {
-        mp_obj_array_t ar = {{&mp_type_bytearray}, BYTEARRAY_TYPECODE, 0, count * SECSIZE(&vfs->fatfs), (void*)buff};
-        vfs->blockdev.writeblocks[2] = MP_OBJ_NEW_SMALL_INT(sector);
-        vfs->blockdev.writeblocks[3] = MP_OBJ_FROM_PTR(&ar);
-        mp_call_method_n_kw(2, 0, vfs->blockdev.writeblocks);
-        // TODO handle error return
-    }
-
-    return RES_OK;
+    return ret == 0 ? RES_OK : RES_ERROR;
 }
 
 
@@ -139,42 +112,16 @@ DRESULT disk_ioctl (
     }
 
     // First part: call the relevant method of the underlying block device
+    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];
     mp_obj_t ret = mp_const_none;
-    if (vfs->blockdev.flags & MP_BLOCKDEV_FLAG_HAVE_IOCTL) {
-        // new protocol with ioctl
-        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->blockdev.u.ioctl[2] = MP_OBJ_NEW_SMALL_INT(bp_op);
-            vfs->blockdev.u.ioctl[3] = MP_OBJ_NEW_SMALL_INT(0); // unused
-            ret = mp_call_method_n_kw(2, 0, vfs->blockdev.u.ioctl);
-        }
-    } else {
-        // old protocol with sync and count
-        switch (cmd) {
-            case CTRL_SYNC:
-                if (vfs->blockdev.u.old.sync[0] != MP_OBJ_NULL) {
-                    mp_call_method_n_kw(0, 0, vfs->blockdev.u.old.sync);
-                }
-                break;
-
-            case GET_SECTOR_COUNT:
-                ret = mp_call_method_n_kw(0, 0, vfs->blockdev.u.old.count);
-                break;
-
-            case GET_SECTOR_SIZE:
-                // old protocol has fixed sector size of 512 bytes
-                break;
-
-            case IOCTL_INIT:
-                // old protocol doesn't have init
-                break;
-        }
+    if (bp_op != 0) {
+        ret = mp_vfs_blockdev_ioctl(&vfs->blockdev, bp_op, 0);
     }
 
     // Second part: convert the result for return
@@ -194,10 +141,8 @@ DRESULT disk_ioctl (
             } else {
                 *((WORD*)buff) = mp_obj_get_int(ret);
             }
-            #if FF_MAX_SS != FF_MIN_SS
             // need to store ssize because we use it in disk_read/disk_write
-            vfs->fatfs.ssize = *((WORD*)buff);
-            #endif
+            vfs->blockdev.block_size = *((WORD*)buff);
             return RES_OK;
         }
 
diff --git a/py/py.mk b/py/py.mk
index d7021d326..b08b3f80c 100644
--- a/py/py.mk
+++ b/py/py.mk
@@ -185,6 +185,7 @@ PY_EXTMOD_O_BASENAME = \
 	extmod/modwebrepl.o \
 	extmod/modframebuf.o \
 	extmod/vfs.o \
+	extmod/vfs_blockdev.o \
 	extmod/vfs_reader.o \
 	extmod/vfs_posix.o \
 	extmod/vfs_posix_file.o \