diff --git a/subsys/ipc/rpmsg_multi_instance/rpmsg_multi_instance.c b/subsys/ipc/rpmsg_multi_instance/rpmsg_multi_instance.c index a7d9f80c7c9..4ed22003a67 100644 --- a/subsys/ipc/rpmsg_multi_instance/rpmsg_multi_instance.c +++ b/subsys/ipc/rpmsg_multi_instance/rpmsg_multi_instance.c @@ -201,69 +201,47 @@ static bool rpmsg_mi_config_verify(const struct rpmsg_mi_ctx_cfg *cfg) return true; } -int rpmsg_mi_ctx_init(struct rpmsg_mi_ctx *ctx, const struct rpmsg_mi_ctx_cfg *cfg) +static int libmetal_setup(struct rpmsg_mi_ctx *ctx) { struct metal_init_params metal_params = METAL_INIT_DEFAULTS; - size_t vring_size = VRING_SIZE_GET(cfg->shm->size); struct metal_device *device; - int err = 0; + int err; - if (!ctx || !cfg) { - return -EINVAL; - } - - if (!rpmsg_mi_config_verify(cfg)) { - return -EIO; - } - - LOG_DBG("RPMsg multiple instance initialization"); - - k_mutex_lock(&shm_mutex, K_FOREVER); - - /* Start IPM workqueue */ - k_work_queue_start(&ctx->ipm_work_q, cfg->ipm_stack_area, - cfg->ipm_stack_size, cfg->ipm_work_q_prio, NULL); - k_thread_name_set(&ctx->ipm_work_q.thread, cfg->ipm_thread_name); - k_work_init(&ctx->ipm_work, ipm_callback_process); - - ctx->name = cfg->name; - sys_slist_init(&ctx->endpoints); - - /* Configure share memory */ - rpmsg_mi_configure_shm(ctx, cfg); - - /* Libmetal setup */ err = metal_init(&metal_params); if (err) { LOG_ERR("metal_init: failed - error code %d", err); - goto out; + return err; } err = metal_register_generic_device(&ctx->shm_device); if (err) { LOG_ERR("Could not register shared memory device: %d", err); - goto out; + return err; } err = metal_device_open("generic", SHM_DEVICE_NAME, &device); if (err) { LOG_ERR("metal_device_open failed: %d", err); - goto out; + return err; } ctx->shm_io = metal_device_io_region(device, 0); if (!ctx->shm_io) { LOG_ERR("metal_device_io_region failed to get region"); - err = -ENODEV; - goto out; + return -ENODEV; } - /* IPM setup. */ + return 0; +} + +static int ipm_setup(struct rpmsg_mi_ctx *ctx, const struct rpmsg_mi_ctx_cfg *cfg) +{ + int err; + ctx->ipm_tx_handle = device_get_binding(cfg->ipm_tx_name); if (!ctx->ipm_tx_handle) { LOG_ERR("Could not get TX IPM device handle"); - err = -ENODEV; - goto out; + return -ENODEV; } ctx->ipm_tx_id = cfg->ipm_tx_id; @@ -271,11 +249,15 @@ int rpmsg_mi_ctx_init(struct rpmsg_mi_ctx *ctx, const struct rpmsg_mi_ctx_cfg *c ctx->ipm_rx_handle = device_get_binding(cfg->ipm_rx_name); if (!ctx->ipm_rx_handle) { LOG_ERR("Could not get RX IPM device handle"); - err = -ENODEV; - goto out; + return -ENODEV; } - /* Register IPM callback. This cb executes when msg has come. */ + k_work_queue_start(&ctx->ipm_work_q, cfg->ipm_stack_area, + cfg->ipm_stack_size, cfg->ipm_work_q_prio, NULL); + k_thread_name_set(&ctx->ipm_work_q.thread, cfg->ipm_thread_name); + + k_work_init(&ctx->ipm_work, ipm_callback_process); + ipm_register_callback(ctx->ipm_rx_handle, ipm_callback, ctx); err = ipm_set_enabled(ctx->ipm_rx_handle, 1); @@ -284,18 +266,21 @@ int rpmsg_mi_ctx_init(struct rpmsg_mi_ctx *ctx, const struct rpmsg_mi_ctx_cfg *c return err; } + return 0; +} + +static int vq_setup(struct rpmsg_mi_ctx *ctx, size_t vring_size) +{ ctx->vq[RPMSG_VQ_0] = virtqueue_allocate(vring_size); if (!ctx->vq[RPMSG_VQ_0]) { LOG_ERR("virtqueue_allocate failed to alloc vq[RPMSG_VQ_0]"); - err = -ENOMEM; - goto out; + return -ENOMEM; } ctx->vq[RPMSG_VQ_1] = virtqueue_allocate(vring_size); if (!ctx->vq[RPMSG_VQ_1]) { LOG_ERR("virtqueue_allocate failed to alloc vq[RPMSG_VQ_1]"); - err = -ENOMEM; - goto out; + return -ENOMEM; } ctx->rvrings[RPMSG_VQ_0].io = ctx->shm_io; @@ -317,6 +302,52 @@ int rpmsg_mi_ctx_init(struct rpmsg_mi_ctx *ctx, const struct rpmsg_mi_ctx_cfg *c ctx->vdev.func = &dispatch; ctx->vdev.vrings_info = &ctx->rvrings[0]; + return 0; +} + +int rpmsg_mi_ctx_init(struct rpmsg_mi_ctx *ctx, const struct rpmsg_mi_ctx_cfg *cfg) +{ + int err = 0; + + if (!ctx || !cfg) { + return -EINVAL; + } + + LOG_DBG("RPMsg multiple instance initialization"); + + if (!rpmsg_mi_config_verify(cfg)) { + return -EIO; + } + + k_mutex_lock(&shm_mutex, K_FOREVER); + + /* Configure shared memory */ + rpmsg_mi_configure_shm(ctx, cfg); + + /* Setup libmetal */ + err = libmetal_setup(ctx); + if (err) { + LOG_ERR("Failed to setup libmetal"); + goto out; + } + + /* Setup IPM */ + err = ipm_setup(ctx, cfg); + if (err) { + LOG_ERR("Failed to setup IPM"); + goto out; + } + + /* Setup VQs / VRINGs */ + err = vq_setup(ctx, VRING_SIZE_GET(cfg->shm->size)); + if (err) { + LOG_ERR("Failed to setup VQs / VRINGs"); + goto out; + } + + ctx->name = cfg->name; + sys_slist_init(&ctx->endpoints); + if (IS_ENABLED(CONFIG_RPMSG_MULTI_INSTANCE_MASTER)) { /* This step is only required if you are VirtIO device master. * Initialize the shared buffers pool. @@ -330,7 +361,6 @@ int rpmsg_mi_ctx_init(struct rpmsg_mi_ctx *ctx, const struct rpmsg_mi_ctx_cfg *c err = rpmsg_init_vdev(&ctx->rvdev, &ctx->vdev, NULL, ctx->shm_io, NULL); } - if (err) { LOG_ERR("RPMSG vdev initialization failed %d", err); goto out;