drivers/cdx/controller/cdx_rpmsg.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * Platform driver for CDX bus.
   4  *
   5  * Copyright (C) 2022-2023, Advanced Micro Devices, Inc.
   6  */
   7
   8 #include <linux/rpmsg.h>
   9 #include <linux/remoteproc.h>
  10 #include <linux/of_platform.h>
  11 #include <linux/cdx/cdx_bus.h>
  12 #include <linux/module.h>
  13
  14 #include "../cdx.h"
  15 #include "cdx_controller.h"
  16 #include "mcdi_functions.h"
  17 #include "mcdi.h"
  18
  19 static struct rpmsg_device_id cdx_rpmsg_id_table[] = {
  20         { .name = "mcdi_ipc" },
  21         { },
  22 };
  23 MODULE_DEVICE_TABLE(rpmsg, cdx_rpmsg_id_table);
  24
  25 int cdx_rpmsg_send(struct cdx_mcdi *cdx_mcdi,
  26                    const struct cdx_dword *hdr, size_t hdr_len,
  27                    const struct cdx_dword *sdu, size_t sdu_len)
  28 {
  29         unsigned char *send_buf;
  30         int ret;
  31
  32         send_buf = kzalloc(hdr_len + sdu_len, GFP_KERNEL);
  33         if (!send_buf)
  34                 return -ENOMEM;
  35
  36         memcpy(send_buf, hdr, hdr_len);
  37         memcpy(send_buf + hdr_len, sdu, sdu_len);
  38
  39         ret = rpmsg_send(cdx_mcdi->ept, send_buf, hdr_len + sdu_len);
  40         kfree(send_buf);
  41
  42         return ret;
  43 }
  44
  45 static int cdx_attach_to_rproc(struct platform_device *pdev)
  46 {
  47         struct device_node *r5_core_node;
  48         struct cdx_controller *cdx_c;
  49         struct cdx_mcdi *cdx_mcdi;
  50         struct device *dev;
  51         struct rproc *rp;
  52         int ret;
  53
  54         dev = &pdev->dev;
  55         cdx_c = platform_get_drvdata(pdev);
  56         cdx_mcdi = cdx_c->priv;
  57
  58         r5_core_node = of_parse_phandle(dev->of_node, "xlnx,rproc", 0);
  59         if (!r5_core_node) {
  60                 dev_err(&pdev->dev, "xlnx,rproc: invalid phandle\n");
  61                 return -EINVAL;
  62         }
  63
  64         rp = rproc_get_by_phandle(r5_core_node->phandle);
  65         if (!rp) {
  66                 ret = -EPROBE_DEFER;
  67                 goto pdev_err;
  68         }
  69
  70         /* Attach to remote processor */
  71         ret = rproc_boot(rp);
  72         if (ret) {
  73                 dev_err(&pdev->dev, "Failed to attach to remote processor\n");
  74                 rproc_put(rp);
  75                 goto pdev_err;
  76         }
  77
  78         cdx_mcdi->r5_rproc = rp;
  79 pdev_err:
  80         of_node_put(r5_core_node);
  81         return ret;
  82 }
  83
  84 static void cdx_detach_to_r5(struct platform_device *pdev)
  85 {
  86         struct cdx_controller *cdx_c;
  87         struct cdx_mcdi *cdx_mcdi;
  88
  89         cdx_c = platform_get_drvdata(pdev);
  90         cdx_mcdi = cdx_c->priv;
  91
  92         rproc_detach(cdx_mcdi->r5_rproc);
  93         rproc_put(cdx_mcdi->r5_rproc);
  94 }
  95
  96 static int cdx_rpmsg_cb(struct rpmsg_device *rpdev, void *data,
  97                         int len, void *priv, u32 src)
  98 {
  99         struct cdx_controller *cdx_c = dev_get_drvdata(&rpdev->dev);
 100         struct cdx_mcdi *cdx_mcdi = cdx_c->priv;
 101
 102         if (len > MCDI_BUF_LEN)
 103                 return -EINVAL;
 104
 105         cdx_mcdi_process_cmd(cdx_mcdi, (struct cdx_dword *)data, len);
 106
 107         return 0;
 108 }
 109
 110 static void cdx_rpmsg_post_probe_work(struct work_struct *work)
 111 {
 112         struct cdx_controller *cdx_c;
 113         struct cdx_mcdi *cdx_mcdi;
 114
 115         cdx_mcdi = container_of(work, struct cdx_mcdi, work);
 116         cdx_c = dev_get_drvdata(&cdx_mcdi->rpdev->dev);
 117         cdx_rpmsg_post_probe(cdx_c);
 118 }
 119
 120 static int cdx_rpmsg_probe(struct rpmsg_device *rpdev)
 121 {
 122         struct rpmsg_channel_info chinfo = {0};
 123         struct cdx_controller *cdx_c;
 124         struct cdx_mcdi *cdx_mcdi;
 125
 126         cdx_c = (struct cdx_controller *)cdx_rpmsg_id_table[0].driver_data;
 127         cdx_mcdi = cdx_c->priv;
 128
 129         chinfo.src = RPMSG_ADDR_ANY;
 130         chinfo.dst = rpdev->dst;
 131         strscpy(chinfo.name, cdx_rpmsg_id_table[0].name,
 132                 strlen(cdx_rpmsg_id_table[0].name));
 133
 134         cdx_mcdi->ept = rpmsg_create_ept(rpdev, cdx_rpmsg_cb, NULL, chinfo);
 135         if (!cdx_mcdi->ept) {
 136                 dev_err_probe(&rpdev->dev, -ENXIO,
 137                               "Failed to create ept for channel %s\n",
 138                               chinfo.name);
 139                 return -EINVAL;
 140         }
 141
 142         cdx_mcdi->rpdev = rpdev;
 143         dev_set_drvdata(&rpdev->dev, cdx_c);
 144
 145         schedule_work(&cdx_mcdi->work);
 146         return 0;
 147 }
 148
 149 static void cdx_rpmsg_remove(struct rpmsg_device *rpdev)
 150 {
 151         struct cdx_controller *cdx_c = dev_get_drvdata(&rpdev->dev);
 152         struct cdx_mcdi *cdx_mcdi = cdx_c->priv;
 153
 154         flush_work(&cdx_mcdi->work);
 155         cdx_rpmsg_pre_remove(cdx_c);
 156
 157         rpmsg_destroy_ept(cdx_mcdi->ept);
 158         dev_set_drvdata(&rpdev->dev, NULL);
 159 }
 160
 161 static struct rpmsg_driver cdx_rpmsg_driver = {
 162         .drv.name = KBUILD_MODNAME,
 163         .id_table = cdx_rpmsg_id_table,
 164         .probe = cdx_rpmsg_probe,
 165         .remove = cdx_rpmsg_remove,
 166         .callback = cdx_rpmsg_cb,
 167 };
 168
 169 int cdx_setup_rpmsg(struct platform_device *pdev)
 170 {
 171         struct cdx_controller *cdx_c;
 172         struct cdx_mcdi *cdx_mcdi;
 173         int ret;
 174
 175         /* Attach to remote processor */
 176         ret = cdx_attach_to_rproc(pdev);
 177         if (ret)
 178                 return ret;
 179
 180         cdx_c = platform_get_drvdata(pdev);
 181         cdx_mcdi = cdx_c->priv;
 182
 183         /* Register RPMsg driver */
 184         cdx_rpmsg_id_table[0].driver_data = (kernel_ulong_t)cdx_c;
 185
 186         INIT_WORK(&cdx_mcdi->work, cdx_rpmsg_post_probe_work);
 187         ret = register_rpmsg_driver(&cdx_rpmsg_driver);
 188         if (ret) {
 189                 dev_err(&pdev->dev,
 190                         "Failed to register cdx RPMsg driver: %d\n", ret);
 191                 cdx_detach_to_r5(pdev);
 192         }
 193
 194         return ret;
 195 }
 196
 197 void cdx_destroy_rpmsg(struct platform_device *pdev)
 198 {
 199         unregister_rpmsg_driver(&cdx_rpmsg_driver);
 200
 201         cdx_detach_to_r5(pdev);
 202 }