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