From bc0af1e53102d77895bf538b8e0feb59eb042c2e Mon Sep 17 00:00:00 2001 From: liaoao Date: Fri, 14 Jul 2023 17:16:44 +0800 Subject: [PATCH] rpmsgblk: add support for mmc_ioc_cmd/mmc_multi_ioc_cmd Signed-off-by: liaoao --- drivers/misc/rpmsgblk.c | 131 +++++++++++++++++++++++++++++++-- drivers/misc/rpmsgblk_server.c | 41 +++++++++++ 2 files changed, 167 insertions(+), 5 deletions(-) diff --git a/drivers/misc/rpmsgblk.c b/drivers/misc/rpmsgblk.c index 9448251472..7654f04206 100644 --- a/drivers/misc/rpmsgblk.c +++ b/drivers/misc/rpmsgblk.c @@ -38,6 +38,7 @@ #include #include #include +#include #include #include "rpmsgblk.h" @@ -86,7 +87,7 @@ static ssize_t rpmsgblk_write(FAR struct inode *inode, blkcnt_t start_sector, unsigned int nsectors); static int rpmsgblk_geometry(FAR struct inode *inode, FAR struct geometry *geometry); -static ssize_t rpmsgblk_ioctl_arglen(int cmd); +static ssize_t rpmsgblk_ioctl_arglen(int cmd, unsigned long arg); static int rpmsgblk_ioctl(FAR struct inode *inode, int cmd, unsigned long arg); #ifndef CONFIG_DISABLE_PSEUDOFS_OPERATIONS @@ -499,7 +500,7 @@ out: * ****************************************************************************/ -static ssize_t rpmsgblk_ioctl_arglen(int cmd) +static ssize_t rpmsgblk_ioctl_arglen(int cmd, unsigned long arg) { switch (cmd) { @@ -526,6 +527,32 @@ static ssize_t rpmsgblk_ioctl_arglen(int cmd) return sizeof(struct partition_info_s); case BIOC_BLKSSZGET: return sizeof(blksize_t); + case MMC_IOC_CMD: + { + FAR struct mmc_ioc_cmd *ioc = + (FAR struct mmc_ioc_cmd *)(uintptr_t)arg; + + return sizeof(struct mmc_ioc_cmd) + ioc->blksz * ioc->blocks; + } + + case MMC_IOC_MULTI_CMD: + { + FAR struct mmc_ioc_multi_cmd *mioc = + (FAR struct mmc_ioc_multi_cmd *)(uintptr_t)arg; + uint64_t num = mioc->num_of_cmds; + uint64_t i; + ssize_t arglen; + + arglen = sizeof(struct mmc_ioc_multi_cmd) + + num * sizeof(struct mmc_ioc_cmd); + for (i = 0; i < num; i++) + { + arglen += mioc->cmds[i].blksz * mioc->cmds[i].blocks; + } + + return arglen; + } + default: return -ENOTTY; } @@ -562,7 +589,7 @@ static int rpmsgblk_ioctl(FAR struct inode *inode, int cmd, /* Call our internal routine to perform the ioctl */ - arglen = rpmsgblk_ioctl_arglen(cmd); + arglen = rpmsgblk_ioctl_arglen(cmd, arg); if (arglen < 0) { return arglen; @@ -576,13 +603,56 @@ static int rpmsgblk_ioctl(FAR struct inode *inode, int cmd, return -ENOMEM; } + DEBUGASSERT(space > msglen); + msg->request = cmd; msg->arg = arg; msg->arglen = arglen; if (arglen > 0) { - memcpy(msg->buf, (FAR void *)(uintptr_t)arg, arglen); + /* If args include a usrspace buffer pointer, then the content of this + * pointer should also be copied into share memory and set after args. + */ + + if (cmd == MMC_IOC_CMD) + { + FAR struct mmc_ioc_cmd *ioc = + (FAR struct mmc_ioc_cmd *)(uintptr_t)arg; + + memcpy(msg->buf, (FAR void *)(uintptr_t)arg, sizeof(*ioc)); + if (ioc->data_ptr) + { + memcpy(msg->buf + sizeof(*ioc), + (FAR void *)(uintptr_t)ioc->data_ptr, + ioc->blksz * ioc->blocks); + } + } + else if (cmd == MMC_IOC_MULTI_CMD) + { + FAR struct mmc_ioc_multi_cmd *mioc = + (FAR struct mmc_ioc_multi_cmd *)(uintptr_t)arg; + uint64_t num = mioc->num_of_cmds; + ssize_t copy = sizeof(struct mmc_ioc_multi_cmd) + + num * sizeof(struct mmc_ioc_cmd); + uint64_t i; + + memcpy(msg->buf, (FAR void *)(uintptr_t)arg, copy); + for (i = 0; i < num; i++) + { + if (mioc->cmds[i].data_ptr) + { + memcpy(msg->buf + copy, + (FAR void *)(uintptr_t)mioc->cmds[i].data_ptr, + mioc->cmds[i].blksz * mioc->cmds[i].blocks); + copy += mioc->cmds[i].blksz * mioc->cmds[i].blocks; + } + } + } + else + { + memcpy(msg->buf, (FAR void *)(uintptr_t)arg, arglen); + } } return rpmsgblk_send_recv(priv, RPMSGBLK_IOCTL, false, &msg->header, @@ -878,7 +948,58 @@ static int rpmsgblk_ioctl_handler(FAR struct rpmsg_endpoint *ept, cookie->result = header->result; if (cookie->result >= 0 && rsp->arglen > 0) { - memcpy(cookie->data, rsp->buf, rsp->arglen); + switch (rsp->request) + { + case MMC_IOC_CMD: + { + FAR struct mmc_ioc_cmd *ioc = + (FAR struct mmc_ioc_cmd *)(uintptr_t)cookie->data; + + /* Copy struct mmc_ioc_cmd back to the usrspace buffer + * except data_ptr which is another buffer pointer + */ + + memcpy(ioc, rsp->buf, sizeof(*ioc) - sizeof(ioc->data_ptr)); + if (ioc->data_ptr) + { + memcpy((FAR void *)(uintptr_t)ioc->data_ptr, + rsp->buf + sizeof(*ioc), + ioc->blksz * ioc->blocks); + } + } + break; + + case MMC_IOC_MULTI_CMD: + { + FAR struct mmc_ioc_multi_cmd *mioc = + (FAR struct mmc_ioc_multi_cmd *)(uintptr_t)cookie->data; + FAR struct mmc_ioc_multi_cmd *mioc_rsp = + (FAR struct mmc_ioc_multi_cmd *)(uintptr_t)rsp->buf; + uint64_t num = mioc->num_of_cmds; + size_t off = sizeof(struct mmc_ioc_multi_cmd) + + num * sizeof(struct mmc_ioc_cmd); + uint64_t i; + + for (i = 0; i < num; i++) + { + memcpy(&mioc->cmds[i], &mioc_rsp->cmds[i], + sizeof(struct mmc_ioc_cmd) - + sizeof(mioc->cmds[i].data_ptr)); + if (mioc->cmds[i].data_ptr) + { + memcpy((FAR void *)(uintptr_t)mioc->cmds[i].data_ptr, + rsp->buf + off, + mioc->cmds[i].blksz * mioc->cmds[i].blocks); + off += mioc->cmds[i].blksz * mioc->cmds[i].blocks; + } + } + } + break; + + default: + memcpy(cookie->data, rsp->buf, rsp->arglen); + break; + } } return rpmsg_post(ept, &cookie->sem); diff --git a/drivers/misc/rpmsgblk_server.c b/drivers/misc/rpmsgblk_server.c index 210e227c8d..ab1db126b4 100644 --- a/drivers/misc/rpmsgblk_server.c +++ b/drivers/misc/rpmsgblk_server.c @@ -27,6 +27,7 @@ #include #include +#include #include #include @@ -277,6 +278,46 @@ static int rpmsgblk_ioctl_handler(FAR struct rpmsg_endpoint *ept, FAR struct rpmsgblk_server_s *server = ept->priv; FAR struct rpmsgblk_ioctl_s *msg = data; + switch (msg->request) + { + case MMC_IOC_CMD: + { + FAR struct mmc_ioc_cmd *ioc = + (FAR struct mmc_ioc_cmd *)(uintptr_t)msg->buf; + + if (ioc->data_ptr) + { + ioc->data_ptr = (uint64_t)(uintptr_t) + ((FAR uint8_t *)ioc + sizeof(*ioc)); + } + } + break; + + case MMC_IOC_MULTI_CMD: + { + FAR struct mmc_ioc_multi_cmd *mioc = + (FAR struct mmc_ioc_multi_cmd *)(uintptr_t)msg->buf; + uint64_t num = mioc->num_of_cmds; + size_t off = sizeof(struct mmc_ioc_multi_cmd) + + num * sizeof(struct mmc_ioc_cmd); + uint64_t i; + + for (i = 0; i < num; i++) + { + if (mioc->cmds[i].data_ptr) + { + mioc->cmds[i].data_ptr = (uint64_t)(uintptr_t) + ((FAR uint8_t *)mioc + off); + off += mioc->cmds[i].blksz * mioc->cmds[i].blocks; + } + } + } + break; + + default: + break; + } + msg->header.result = server->bops->ioctl(server->blknode, msg->request, msg->arglen > 0 ? (unsigned long)msg->buf :