diff --git a/drivers/mtd/rpmsgmtd.c b/drivers/mtd/rpmsgmtd.c index a3889da490..541f1aa2bd 100644 --- a/drivers/mtd/rpmsgmtd.c +++ b/drivers/mtd/rpmsgmtd.c @@ -74,7 +74,8 @@ struct rpmsgmtd_cookie_s static int rpmsgmtd_erase(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks); -static int rpmsgmtd_get_geometry(FAR struct rpmsgmtd_s *dev); +static int rpmsgmtd_get_geometry(FAR struct rpmsgmtd_s *dev, + FAR struct mtd_geometry_s *geometry); static ssize_t rpmsgmtd_bread(FAR struct mtd_dev_s *dev, off_t startblock, size_t nblocks, FAR uint8_t *buffer); static ssize_t rpmsgmtd_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, @@ -109,6 +110,9 @@ static int rpmsgmtd_bread_handler(FAR struct rpmsg_endpoint *ept, static int rpmsgmtd_read_handler(FAR struct rpmsg_endpoint *ept, FAR void *data, size_t len, uint32_t src, FAR void *priv); +static int rpmsgmtd_geometry_handler(FAR struct rpmsg_endpoint *ept, + FAR void *data, size_t len, + uint32_t src, FAR void *priv); static int rpmsgmtd_ioctl_handler(FAR struct rpmsg_endpoint *ept, FAR void *data, size_t len, uint32_t src, FAR void *priv); @@ -132,12 +136,13 @@ static void rpmsgmtd_ns_bound(struct rpmsg_endpoint *ept); static const rpmsg_ept_cb g_rpmsgmtd_handler[] = { - [RPMSGMTD_ERASE] = rpmsgmtd_default_handler, - [RPMSGMTD_BREAD] = rpmsgmtd_bread_handler, - [RPMSGMTD_BWRITE] = rpmsgmtd_default_handler, - [RPMSGMTD_READ] = rpmsgmtd_read_handler, - [RPMSGMTD_WRITE] = rpmsgmtd_default_handler, - [RPMSGMTD_IOCTL] = rpmsgmtd_ioctl_handler, + [RPMSGMTD_ERASE] = rpmsgmtd_default_handler, + [RPMSGMTD_BREAD] = rpmsgmtd_bread_handler, + [RPMSGMTD_BWRITE] = rpmsgmtd_default_handler, + [RPMSGMTD_READ] = rpmsgmtd_read_handler, + [RPMSGMTD_WRITE] = rpmsgmtd_default_handler, + [RPMSGMTD_GEOMETRY] = rpmsgmtd_geometry_handler, + [RPMSGMTD_IOCTL] = rpmsgmtd_ioctl_handler, }; /**************************************************************************** @@ -191,25 +196,53 @@ static int rpmsgmtd_erase(FAR struct mtd_dev_s *dev, off_t startblock, * ****************************************************************************/ -static int rpmsgmtd_get_geometry(FAR struct rpmsgmtd_s *dev) +static int rpmsgmtd_get_geometry(FAR struct rpmsgmtd_s *dev, + FAR struct mtd_geometry_s *geometry) { + FAR struct rpmsgmtd_s *priv = dev; + struct rpmsgmtd_geometry_s msg; int ret; - ret = nxmutex_lock(&dev->geolock); + /* Sanity checks */ + + DEBUGASSERT(priv != NULL); + + ret = nxmutex_lock(&priv->geolock); if (ret < 0) { return ret; } - if (dev->geo.blocksize == 0) - { - /* Get the server mtd device geometry */ + /* Return the perviously got geometry */ - ret = rpmsgmtd_ioctl(&dev->mtd, MTDIOC_GEOMETRY, - (unsigned long)&dev->geo); + if (priv->geo.blocksize != 0) + { + if (geometry != NULL) + { + memcpy(geometry, &priv->geo, sizeof(*geometry)); + } + + goto out; } - nxmutex_unlock(&dev->geolock); + ret = rpmsgmtd_send_recv(priv, RPMSGMTD_GEOMETRY, true, &msg.header, + sizeof(msg), NULL); + + if (ret >= 0) + { + priv->geo.blocksize = msg.blocksize; + priv->geo.erasesize = msg.erasesize; + priv->geo.neraseblocks = msg.neraseblocks; + strlcpy(priv->geo.model, msg.model, sizeof(priv->geo.model)); + + if (geometry != NULL) + { + memcpy(geometry, &priv->geo, sizeof(*geometry)); + } + } + +out: + nxmutex_unlock(&priv->geolock); return ret; } @@ -250,7 +283,7 @@ static ssize_t rpmsgmtd_bread(FAR struct mtd_dev_s *dev, off_t startblock, /* Get the server mtd geometry */ - ret = rpmsgmtd_get_geometry(priv); + ret = rpmsgmtd_get_geometry(priv, NULL); if (ret < 0) { ferr("Get geometry failed, ret=%d\n", ret); @@ -314,7 +347,7 @@ static ssize_t rpmsgmtd_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, /* Get the server mtd geometry */ - ret = rpmsgmtd_get_geometry(priv); + ret = rpmsgmtd_get_geometry(priv, NULL); if (ret < 0) { ferr("Get geometry failed, ret=%d\n", ret); @@ -552,8 +585,6 @@ static ssize_t rpmsgmtd_ioctl_arglen(int cmd) { switch (cmd) { - case MTDIOC_GEOMETRY: - return sizeof(struct mtd_geometry_s); case MTDIOC_PROTECT: case MTDIOC_UNPROTECT: return sizeof(struct mtd_protect_s); @@ -587,6 +618,11 @@ static int rpmsgmtd_ioctl(FAR struct mtd_dev_s *dev, int cmd, ssize_t arglen; size_t msglen; + if (cmd == MTDIOC_GEOMETRY) + { + return rpmsgmtd_get_geometry(priv, (FAR struct mtd_geometry_s *)arg); + } + /* Sanity checks */ DEBUGASSERT(priv != NULL); @@ -854,6 +890,42 @@ static int rpmsgmtd_read_handler(FAR struct rpmsg_endpoint *ept, return 0; } +/**************************************************************************** + * Name: rpmsgmtd_geometry_handler + * + * Description: + * Rpmsg-mtd geometry response handler, this function will be called to + * process the return message of rpmsgmtd_get_geometry(). + * + * Parameters: + * ept - The rpmsg endpoint + * data - The return message + * len - The return message length + * src - unknow + * priv - unknow + * + * Returned Values: + * Always OK + * + ****************************************************************************/ + +static int rpmsgmtd_geometry_handler(FAR struct rpmsg_endpoint *ept, + FAR void *data, size_t len, + uint32_t src, FAR void *priv) +{ + FAR struct rpmsgmtd_header_s *header = data; + FAR struct rpmsgmtd_cookie_s *cookie = + (FAR struct rpmsgmtd_cookie_s *)(uintptr_t)header->cookie; + + if (cookie->result >= 0) + { + memcpy(cookie->data, data, len); + } + + rpmsg_post(ept, &cookie->sem); + return 0; +} + /**************************************************************************** * Name: rpmsgmtd_ioctl_handler * diff --git a/drivers/mtd/rpmsgmtd.h b/drivers/mtd/rpmsgmtd.h index 39d01b068f..5a80375351 100644 --- a/drivers/mtd/rpmsgmtd.h +++ b/drivers/mtd/rpmsgmtd.h @@ -35,13 +35,15 @@ #define RPMSGMTD_NAME_PREFIX "rpmsgmtd-" #define RPMSGMTD_NAME_PREFIX_LEN 9 +#define RPMSGMTD_NAME_MAX 32 #define RPMSGMTD_ERASE 1 #define RPMSGMTD_BREAD 2 #define RPMSGMTD_BWRITE 3 #define RPMSGMTD_READ 4 #define RPMSGMTD_WRITE 5 -#define RPMSGMTD_IOCTL 6 +#define RPMSGMTD_GEOMETRY 6 +#define RPMSGMTD_IOCTL 7 /**************************************************************************** * Public Types @@ -82,6 +84,15 @@ begin_packed_struct struct rpmsgmtd_read_s #define rpmsgmtd_write_s rpmsgmtd_read_s +begin_packed_struct struct rpmsgmtd_geometry_s +{ + struct rpmsgmtd_header_s header; + uint32_t blocksize; + uint32_t erasesize; + uint32_t neraseblocks; + char model[RPMSGMTD_NAME_MAX + 1]; +} end_packed_struct; + begin_packed_struct struct rpmsgmtd_ioctl_s { struct rpmsgmtd_header_s header; diff --git a/drivers/mtd/rpmsgmtd_server.c b/drivers/mtd/rpmsgmtd_server.c index 7a9654d730..c0b5218a74 100644 --- a/drivers/mtd/rpmsgmtd_server.c +++ b/drivers/mtd/rpmsgmtd_server.c @@ -70,6 +70,9 @@ static int rpmsgmtd_read_handler(FAR struct rpmsg_endpoint *ept, static int rpmsgmtd_write_handler(FAR struct rpmsg_endpoint *ept, FAR void *data, size_t len, uint32_t src, FAR void *priv); +static int rpmsgmtd_geometry_handler(FAR struct rpmsg_endpoint *ept, + FAR void *data, size_t len, + uint32_t src, FAR void *priv); static int rpmsgmtd_ioctl_handler(FAR struct rpmsg_endpoint *ept, FAR void *data, size_t len, uint32_t src, FAR void *priv); @@ -93,12 +96,13 @@ static int rpmsgmtd_ept_cb(FAR struct rpmsg_endpoint *ept, static const rpmsg_ept_cb g_rpmsgmtd_handler[] = { - [RPMSGMTD_ERASE] = rpmsgmtd_erase_handler, - [RPMSGMTD_BREAD] = rpmsgmtd_bread_handler, - [RPMSGMTD_BWRITE] = rpmsgmtd_bwrite_handler, - [RPMSGMTD_READ] = rpmsgmtd_read_handler, - [RPMSGMTD_WRITE] = rpmsgmtd_write_handler, - [RPMSGMTD_IOCTL] = rpmsgmtd_ioctl_handler, + [RPMSGMTD_ERASE] = rpmsgmtd_erase_handler, + [RPMSGMTD_BREAD] = rpmsgmtd_bread_handler, + [RPMSGMTD_BWRITE] = rpmsgmtd_bwrite_handler, + [RPMSGMTD_READ] = rpmsgmtd_read_handler, + [RPMSGMTD_WRITE] = rpmsgmtd_write_handler, + [RPMSGMTD_GEOMETRY] = rpmsgmtd_geometry_handler, + [RPMSGMTD_IOCTL] = rpmsgmtd_ioctl_handler, }; /**************************************************************************** @@ -284,6 +288,39 @@ static int rpmsgmtd_write_handler(FAR struct rpmsg_endpoint *ept, return 0; } +/**************************************************************************** + * Name: rpmsgmtd_geometry_handler + ****************************************************************************/ + +static int rpmsgmtd_geometry_handler(FAR struct rpmsg_endpoint *ept, + FAR void *data, size_t len, + uint32_t src, FAR void *priv) +{ + FAR struct rpmsgmtd_server_s *server = ept->priv; + FAR struct rpmsgmtd_geometry_s *msg = data; + struct mtd_geometry_s geo; + + msg->header.result = MTD_IOCTL(server->dev, MTDIOC_GEOMETRY, + (unsigned long)&geo); + + if (msg->header.result < 0) + { + ferr("mtd get geometry result error: %" PRId32 "\n", + msg->header.result); + goto send; + } + + DEBUGASSERT(strlen(geo.model) <= RPMSGMTD_NAME_MAX); + + msg->blocksize = geo.blocksize; + msg->erasesize = geo.erasesize; + msg->neraseblocks = geo.neraseblocks; + strlcpy(msg->model, geo.model, sizeof(msg->model)); + +send: + return rpmsg_send(ept, msg, len); +} + /**************************************************************************** * Name: rpmsgmtd_ioctl_handler ****************************************************************************/