411 lines · c
1// SPDX-License-Identifier: GPL-2.02//3// Copyright 2019 Google LLC.4 5#include <linux/kernel.h>6#include <linux/module.h>7#include <linux/of.h>8#include <linux/platform_device.h>9#include <linux/remoteproc.h>10#include <linux/rpmsg/mtk_rpmsg.h>11#include <linux/slab.h>12#include <linux/workqueue.h>13 14#include "rpmsg_internal.h"15 16struct mtk_rpmsg_rproc_subdev {17 struct platform_device *pdev;18 struct mtk_rpmsg_info *info;19 struct rpmsg_endpoint *ns_ept;20 struct rproc_subdev subdev;21 22 struct work_struct register_work;23 struct list_head channels;24 struct mutex channels_lock;25};26 27#define to_mtk_subdev(d) container_of(d, struct mtk_rpmsg_rproc_subdev, subdev)28 29struct mtk_rpmsg_channel_info {30 struct rpmsg_channel_info info;31 bool registered;32 struct list_head list;33};34 35/**36 * struct rpmsg_ns_msg - dynamic name service announcement message37 * @name: name of remote service that is published38 * @addr: address of remote service that is published39 *40 * This message is sent across to publish a new service. When we receive these41 * messages, an appropriate rpmsg channel (i.e device) is created. In turn, the42 * ->probe() handler of the appropriate rpmsg driver will be invoked43 * (if/as-soon-as one is registered).44 */45struct rpmsg_ns_msg {46 char name[RPMSG_NAME_SIZE];47 u32 addr;48} __packed;49 50struct mtk_rpmsg_device {51 struct rpmsg_device rpdev;52 struct mtk_rpmsg_rproc_subdev *mtk_subdev;53};54 55struct mtk_rpmsg_endpoint {56 struct rpmsg_endpoint ept;57 struct mtk_rpmsg_rproc_subdev *mtk_subdev;58};59 60#define to_mtk_rpmsg_device(r) container_of(r, struct mtk_rpmsg_device, rpdev)61#define to_mtk_rpmsg_endpoint(r) container_of(r, struct mtk_rpmsg_endpoint, ept)62 63static const struct rpmsg_endpoint_ops mtk_rpmsg_endpoint_ops;64 65static void __mtk_ept_release(struct kref *kref)66{67 struct rpmsg_endpoint *ept = container_of(kref, struct rpmsg_endpoint,68 refcount);69 kfree(to_mtk_rpmsg_endpoint(ept));70}71 72static void mtk_rpmsg_ipi_handler(void *data, unsigned int len, void *priv)73{74 struct mtk_rpmsg_endpoint *mept = priv;75 struct rpmsg_endpoint *ept = &mept->ept;76 int ret;77 78 ret = (*ept->cb)(ept->rpdev, data, len, ept->priv, ept->addr);79 if (ret)80 dev_warn(&ept->rpdev->dev, "rpmsg handler return error = %d",81 ret);82}83 84static struct rpmsg_endpoint *85__mtk_create_ept(struct mtk_rpmsg_rproc_subdev *mtk_subdev,86 struct rpmsg_device *rpdev, rpmsg_rx_cb_t cb, void *priv,87 u32 id)88{89 struct mtk_rpmsg_endpoint *mept;90 struct rpmsg_endpoint *ept;91 struct platform_device *pdev = mtk_subdev->pdev;92 int ret;93 94 mept = kzalloc(sizeof(*mept), GFP_KERNEL);95 if (!mept)96 return NULL;97 mept->mtk_subdev = mtk_subdev;98 99 ept = &mept->ept;100 kref_init(&ept->refcount);101 102 ept->rpdev = rpdev;103 ept->cb = cb;104 ept->priv = priv;105 ept->ops = &mtk_rpmsg_endpoint_ops;106 ept->addr = id;107 108 ret = mtk_subdev->info->register_ipi(pdev, id, mtk_rpmsg_ipi_handler,109 mept);110 if (ret) {111 dev_err(&pdev->dev, "IPI register failed, id = %d", id);112 kref_put(&ept->refcount, __mtk_ept_release);113 return NULL;114 }115 116 return ept;117}118 119static struct rpmsg_endpoint *120mtk_rpmsg_create_ept(struct rpmsg_device *rpdev, rpmsg_rx_cb_t cb, void *priv,121 struct rpmsg_channel_info chinfo)122{123 struct mtk_rpmsg_rproc_subdev *mtk_subdev =124 to_mtk_rpmsg_device(rpdev)->mtk_subdev;125 126 return __mtk_create_ept(mtk_subdev, rpdev, cb, priv, chinfo.src);127}128 129static void mtk_rpmsg_destroy_ept(struct rpmsg_endpoint *ept)130{131 struct mtk_rpmsg_rproc_subdev *mtk_subdev =132 to_mtk_rpmsg_endpoint(ept)->mtk_subdev;133 134 mtk_subdev->info->unregister_ipi(mtk_subdev->pdev, ept->addr);135 kref_put(&ept->refcount, __mtk_ept_release);136}137 138static int mtk_rpmsg_send(struct rpmsg_endpoint *ept, void *data, int len)139{140 struct mtk_rpmsg_rproc_subdev *mtk_subdev =141 to_mtk_rpmsg_endpoint(ept)->mtk_subdev;142 143 return mtk_subdev->info->send_ipi(mtk_subdev->pdev, ept->addr, data,144 len, 0);145}146 147static int mtk_rpmsg_trysend(struct rpmsg_endpoint *ept, void *data, int len)148{149 struct mtk_rpmsg_rproc_subdev *mtk_subdev =150 to_mtk_rpmsg_endpoint(ept)->mtk_subdev;151 152 /*153 * TODO: This currently is same as mtk_rpmsg_send, and wait until SCP154 * received the last command.155 */156 return mtk_subdev->info->send_ipi(mtk_subdev->pdev, ept->addr, data,157 len, 0);158}159 160static const struct rpmsg_endpoint_ops mtk_rpmsg_endpoint_ops = {161 .destroy_ept = mtk_rpmsg_destroy_ept,162 .send = mtk_rpmsg_send,163 .trysend = mtk_rpmsg_trysend,164};165 166static void mtk_rpmsg_release_device(struct device *dev)167{168 struct rpmsg_device *rpdev = to_rpmsg_device(dev);169 struct mtk_rpmsg_device *mdev = to_mtk_rpmsg_device(rpdev);170 171 kfree(mdev);172}173 174static const struct rpmsg_device_ops mtk_rpmsg_device_ops = {175 .create_ept = mtk_rpmsg_create_ept,176};177 178static struct device_node *179mtk_rpmsg_match_device_subnode(struct device_node *node, const char *channel)180{181 struct device_node *child;182 const char *name;183 int ret;184 185 for_each_available_child_of_node(node, child) {186 ret = of_property_read_string(child, "mediatek,rpmsg-name", &name);187 if (ret)188 continue;189 190 if (strcmp(name, channel) == 0)191 return child;192 }193 194 return NULL;195}196 197static int mtk_rpmsg_register_device(struct mtk_rpmsg_rproc_subdev *mtk_subdev,198 struct rpmsg_channel_info *info)199{200 struct rpmsg_device *rpdev;201 struct mtk_rpmsg_device *mdev;202 struct platform_device *pdev = mtk_subdev->pdev;203 204 mdev = kzalloc(sizeof(*mdev), GFP_KERNEL);205 if (!mdev)206 return -ENOMEM;207 208 mdev->mtk_subdev = mtk_subdev;209 210 rpdev = &mdev->rpdev;211 rpdev->ops = &mtk_rpmsg_device_ops;212 rpdev->src = info->src;213 rpdev->dst = info->dst;214 strscpy(rpdev->id.name, info->name, RPMSG_NAME_SIZE);215 216 rpdev->dev.of_node =217 mtk_rpmsg_match_device_subnode(pdev->dev.of_node, info->name);218 rpdev->dev.parent = &pdev->dev;219 rpdev->dev.release = mtk_rpmsg_release_device;220 221 return rpmsg_register_device(rpdev);222}223 224static void mtk_register_device_work_function(struct work_struct *register_work)225{226 struct mtk_rpmsg_rproc_subdev *subdev = container_of(227 register_work, struct mtk_rpmsg_rproc_subdev, register_work);228 struct platform_device *pdev = subdev->pdev;229 struct mtk_rpmsg_channel_info *info;230 int ret;231 232 mutex_lock(&subdev->channels_lock);233 list_for_each_entry(info, &subdev->channels, list) {234 if (info->registered)235 continue;236 237 mutex_unlock(&subdev->channels_lock);238 ret = mtk_rpmsg_register_device(subdev, &info->info);239 mutex_lock(&subdev->channels_lock);240 if (ret) {241 dev_err(&pdev->dev, "Can't create rpmsg_device\n");242 continue;243 }244 245 info->registered = true;246 }247 mutex_unlock(&subdev->channels_lock);248}249 250static int mtk_rpmsg_create_device(struct mtk_rpmsg_rproc_subdev *mtk_subdev,251 char *name, u32 addr)252{253 struct mtk_rpmsg_channel_info *info;254 255 info = kzalloc(sizeof(*info), GFP_KERNEL);256 if (!info)257 return -ENOMEM;258 259 strscpy(info->info.name, name, RPMSG_NAME_SIZE);260 info->info.src = addr;261 info->info.dst = RPMSG_ADDR_ANY;262 mutex_lock(&mtk_subdev->channels_lock);263 list_add(&info->list, &mtk_subdev->channels);264 mutex_unlock(&mtk_subdev->channels_lock);265 266 schedule_work(&mtk_subdev->register_work);267 return 0;268}269 270static int mtk_rpmsg_ns_cb(struct rpmsg_device *rpdev, void *data, int len,271 void *priv, u32 src)272{273 struct rpmsg_ns_msg *msg = data;274 struct mtk_rpmsg_rproc_subdev *mtk_subdev = priv;275 struct device *dev = &mtk_subdev->pdev->dev;276 277 int ret;278 279 if (len != sizeof(*msg)) {280 dev_err(dev, "malformed ns msg (%d)\n", len);281 return -EINVAL;282 }283 284 /*285 * the name service ept does _not_ belong to a real rpmsg channel,286 * and is handled by the rpmsg bus itself.287 * for sanity reasons, make sure a valid rpdev has _not_ sneaked288 * in somehow.289 */290 if (rpdev) {291 dev_err(dev, "anomaly: ns ept has an rpdev handle\n");292 return -EINVAL;293 }294 295 /* don't trust the remote processor for null terminating the name */296 msg->name[RPMSG_NAME_SIZE - 1] = '\0';297 298 dev_info(dev, "creating channel %s addr 0x%x\n", msg->name, msg->addr);299 300 ret = mtk_rpmsg_create_device(mtk_subdev, msg->name, msg->addr);301 if (ret) {302 dev_err(dev, "create rpmsg device failed\n");303 return ret;304 }305 306 return 0;307}308 309static int mtk_rpmsg_prepare(struct rproc_subdev *subdev)310{311 struct mtk_rpmsg_rproc_subdev *mtk_subdev = to_mtk_subdev(subdev);312 313 /* a dedicated endpoint handles the name service msgs */314 if (mtk_subdev->info->ns_ipi_id >= 0) {315 mtk_subdev->ns_ept =316 __mtk_create_ept(mtk_subdev, NULL, mtk_rpmsg_ns_cb,317 mtk_subdev,318 mtk_subdev->info->ns_ipi_id);319 if (!mtk_subdev->ns_ept) {320 dev_err(&mtk_subdev->pdev->dev,321 "failed to create name service endpoint\n");322 return -ENOMEM;323 }324 }325 326 return 0;327}328 329static void mtk_rpmsg_unprepare(struct rproc_subdev *subdev)330{331 struct mtk_rpmsg_rproc_subdev *mtk_subdev = to_mtk_subdev(subdev);332 333 if (mtk_subdev->ns_ept) {334 mtk_rpmsg_destroy_ept(mtk_subdev->ns_ept);335 mtk_subdev->ns_ept = NULL;336 }337}338 339static void mtk_rpmsg_stop(struct rproc_subdev *subdev, bool crashed)340{341 struct mtk_rpmsg_channel_info *info, *next;342 struct mtk_rpmsg_rproc_subdev *mtk_subdev = to_mtk_subdev(subdev);343 struct device *dev = &mtk_subdev->pdev->dev;344 345 /*346 * Destroy the name service endpoint here, to avoid new channel being347 * created after the rpmsg_unregister_device loop below.348 */349 if (mtk_subdev->ns_ept) {350 mtk_rpmsg_destroy_ept(mtk_subdev->ns_ept);351 mtk_subdev->ns_ept = NULL;352 }353 354 cancel_work_sync(&mtk_subdev->register_work);355 356 mutex_lock(&mtk_subdev->channels_lock);357 list_for_each_entry(info, &mtk_subdev->channels, list) {358 if (!info->registered)359 continue;360 if (rpmsg_unregister_device(dev, &info->info)) {361 dev_warn(362 dev,363 "rpmsg_unregister_device failed for %s.%d.%d\n",364 info->info.name, info->info.src,365 info->info.dst);366 }367 }368 369 list_for_each_entry_safe(info, next,370 &mtk_subdev->channels, list) {371 list_del(&info->list);372 kfree(info);373 }374 mutex_unlock(&mtk_subdev->channels_lock);375}376 377struct rproc_subdev *378mtk_rpmsg_create_rproc_subdev(struct platform_device *pdev,379 struct mtk_rpmsg_info *info)380{381 struct mtk_rpmsg_rproc_subdev *mtk_subdev;382 383 mtk_subdev = kzalloc(sizeof(*mtk_subdev), GFP_KERNEL);384 if (!mtk_subdev)385 return NULL;386 387 mtk_subdev->pdev = pdev;388 mtk_subdev->subdev.prepare = mtk_rpmsg_prepare;389 mtk_subdev->subdev.stop = mtk_rpmsg_stop;390 mtk_subdev->subdev.unprepare = mtk_rpmsg_unprepare;391 mtk_subdev->info = info;392 INIT_LIST_HEAD(&mtk_subdev->channels);393 INIT_WORK(&mtk_subdev->register_work,394 mtk_register_device_work_function);395 mutex_init(&mtk_subdev->channels_lock);396 397 return &mtk_subdev->subdev;398}399EXPORT_SYMBOL_GPL(mtk_rpmsg_create_rproc_subdev);400 401void mtk_rpmsg_destroy_rproc_subdev(struct rproc_subdev *subdev)402{403 struct mtk_rpmsg_rproc_subdev *mtk_subdev = to_mtk_subdev(subdev);404 405 kfree(mtk_subdev);406}407EXPORT_SYMBOL_GPL(mtk_rpmsg_destroy_rproc_subdev);408 409MODULE_LICENSE("GPL v2");410MODULE_DESCRIPTION("MediaTek scp rpmsg driver");411