ipc: rpmsg_multi_instance: Re-organize the init function

The rpmsg_mi_ctx_init() function is long and complex. Split it into
several logical units.

Signed-off-by: Carlo Caione <ccaione@baylibre.com>

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;
+		return -ENODEV;
-		goto out;
 	}
 
-	 /* 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;
+		return -ENODEV;
-		goto out;
 	}
 
 	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;
+		return -ENODEV;
-		goto out;
 	}
 
-	/* 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;
+		return -ENOMEM;
-		goto out;
 	}
 
 	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;
+		return -ENOMEM;
-		goto out;
 	}
 
 	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;