drivers/rpmsg/rpmsg_ns.c

0001 // SPDX-License-Identifier: GPL-2.0
0002 /*
0003  * Copyright (C) STMicroelectronics 2020 - All Rights Reserved
0004  */
0005 #include <linux/device.h>
0006 #include <linux/kernel.h>
0007 #include <linux/module.h>
0008 #include <linux/rpmsg.h>
0009 #include <linux/rpmsg/ns.h>
0010 #include <linux/slab.h>
0011
0012 #include "rpmsg_internal.h"
0013
0014 /**
0015  * rpmsg_ns_register_device() - register name service device based on rpdev
0016  * @rpdev: prepared rpdev to be used for creating endpoints
0017  *
0018  * This function wraps rpmsg_register_device() preparing the rpdev for use as
0019  * basis for the rpmsg name service device.
0020  */
0021 int rpmsg_ns_register_device(struct rpmsg_device *rpdev)
0022 {
0023     rpdev->src = RPMSG_NS_ADDR;
0024     rpdev->dst = RPMSG_NS_ADDR;
0025
0026     return rpmsg_register_device_override(rpdev, "rpmsg_ns");
0027 }
0028 EXPORT_SYMBOL(rpmsg_ns_register_device);
0029
0030 /* invoked when a name service announcement arrives */
0031 static int rpmsg_ns_cb(struct rpmsg_device *rpdev, void *data, int len,
0032                void *priv, u32 src)
0033 {
0034     struct rpmsg_ns_msg *msg = data;
0035     struct rpmsg_device *newch;
0036     struct rpmsg_channel_info chinfo;
0037     struct device *dev = rpdev->dev.parent;
0038     int ret;
0039
0040 #if defined(CONFIG_DYNAMIC_DEBUG)
0041     dynamic_hex_dump("NS announcement: ", DUMP_PREFIX_NONE, 16, 1,
0042              data, len, true);
0043 #endif
0044
0045     if (len != sizeof(*msg)) {
0046         dev_err(dev, "malformed ns msg (%d)\n", len);
0047         return -EINVAL;
0048     }
0049
0050     /* don't trust the remote processor for null terminating the name */
0051     msg->name[RPMSG_NAME_SIZE - 1] = '\0';
0052
0053     strncpy(chinfo.name, msg->name, sizeof(chinfo.name));
0054     chinfo.src = RPMSG_ADDR_ANY;
0055     chinfo.dst = rpmsg32_to_cpu(rpdev, msg->addr);
0056
0057     dev_info(dev, "%sing channel %s addr 0x%x\n",
0058          rpmsg32_to_cpu(rpdev, msg->flags) & RPMSG_NS_DESTROY ?
0059          "destroy" : "creat", msg->name, chinfo.dst);
0060
0061     if (rpmsg32_to_cpu(rpdev, msg->flags) & RPMSG_NS_DESTROY) {
0062         ret = rpmsg_release_channel(rpdev, &chinfo);
0063         if (ret)
0064             dev_err(dev, "rpmsg_destroy_channel failed: %d\n", ret);
0065     } else {
0066         newch = rpmsg_create_channel(rpdev, &chinfo);
0067         if (!newch)
0068             dev_err(dev, "rpmsg_create_channel failed\n");
0069     }
0070
0071     return 0;
0072 }
0073
0074 static int rpmsg_ns_probe(struct rpmsg_device *rpdev)
0075 {
0076     struct rpmsg_endpoint *ns_ept;
0077     struct rpmsg_channel_info ns_chinfo = {
0078         .src = RPMSG_NS_ADDR,
0079         .dst = RPMSG_NS_ADDR,
0080         .name = "name_service",
0081     };
0082
0083     /*
0084      * Create the NS announcement service endpoint associated to the RPMsg
0085      * device. The endpoint will be automatically destroyed when the RPMsg
0086      * device will be deleted.
0087      */
0088     ns_ept = rpmsg_create_ept(rpdev, rpmsg_ns_cb, NULL, ns_chinfo);
0089     if (!ns_ept) {
0090         dev_err(&rpdev->dev, "failed to create the ns ept\n");
0091         return -ENOMEM;
0092     }
0093     rpdev->ept = ns_ept;
0094
0095     return 0;
0096 }
0097
0098 static struct rpmsg_driver rpmsg_ns_driver = {
0099     .drv.name = KBUILD_MODNAME,
0100     .probe = rpmsg_ns_probe,
0101 };
0102
0103 static int rpmsg_ns_init(void)
0104 {
0105     int ret;
0106
0107     ret = register_rpmsg_driver(&rpmsg_ns_driver);
0108     if (ret < 0)
0109         pr_err("%s: Failed to register rpmsg driver\n", __func__);
0110
0111     return ret;
0112 }
0113 postcore_initcall(rpmsg_ns_init);
0114
0115 static void rpmsg_ns_exit(void)
0116 {
0117     unregister_rpmsg_driver(&rpmsg_ns_driver);
0118 }
0119 module_exit(rpmsg_ns_exit);
0120
0121 MODULE_DESCRIPTION("Name service announcement rpmsg driver");
0122 MODULE_AUTHOR("Arnaud Pouliquen <arnaud.pouliquen@st.com>");
0123 MODULE_ALIAS("rpmsg:" KBUILD_MODNAME);
0124 MODULE_LICENSE("GPL v2");