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.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 | |
20 | static struct rpmsg_device_id cdx_rpmsg_id_table[] = { |
21 | { .name = "mcdi_ipc" }, |
22 | { }, |
23 | }; |
24 | MODULE_DEVICE_TABLE(rpmsg, cdx_rpmsg_id_table); |
25 | |
26 | int 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(size: 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(ept: cdx_mcdi->ept, data: send_buf, len: hdr_len + sdu_len); |
41 | kfree(objp: send_buf); |
42 | |
43 | return ret; |
44 | } |
45 | |
46 | static 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(np: dev->of_node, phandle_name: "xlnx,rproc" , index: 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(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(rproc: rp); |
73 | if (ret) { |
74 | dev_err(&pdev->dev, "Failed to attach to remote processor\n" ); |
75 | rproc_put(rproc: rp); |
76 | goto pdev_err; |
77 | } |
78 | |
79 | cdx_mcdi->r5_rproc = rp; |
80 | pdev_err: |
81 | of_node_put(node: r5_core_node); |
82 | return ret; |
83 | } |
84 | |
85 | static 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(rproc: cdx_mcdi->r5_rproc); |
94 | rproc_put(rproc: cdx_mcdi->r5_rproc); |
95 | } |
96 | |
97 | static 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(dev: &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: cdx_mcdi, outbuf: (struct cdx_dword *)data, len); |
107 | |
108 | return 0; |
109 | } |
110 | |
111 | static 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(dev: &cdx_mcdi->rpdev->dev); |
118 | cdx_rpmsg_post_probe(cdx: cdx_c); |
119 | } |
120 | |
121 | static 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(p: chinfo.name, q: cdx_rpmsg_id_table[0].name, |
133 | strlen(cdx_rpmsg_id_table[0].name)); |
134 | |
135 | cdx_mcdi->ept = rpmsg_create_ept(rpdev, cb: cdx_rpmsg_cb, NULL, chinfo); |
136 | if (!cdx_mcdi->ept) { |
137 | dev_err_probe(dev: &rpdev->dev, err: -ENXIO, |
138 | fmt: "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(dev: &rpdev->dev, data: cdx_c); |
145 | |
146 | schedule_work(work: &cdx_mcdi->work); |
147 | return 0; |
148 | } |
149 | |
150 | static void cdx_rpmsg_remove(struct rpmsg_device *rpdev) |
151 | { |
152 | struct cdx_controller *cdx_c = dev_get_drvdata(dev: &rpdev->dev); |
153 | struct cdx_mcdi *cdx_mcdi = cdx_c->priv; |
154 | |
155 | flush_work(work: &cdx_mcdi->work); |
156 | cdx_rpmsg_pre_remove(cdx: cdx_c); |
157 | |
158 | rpmsg_destroy_ept(cdx_mcdi->ept); |
159 | dev_set_drvdata(dev: &rpdev->dev, NULL); |
160 | } |
161 | |
162 | static 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 | |
170 | int 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 | |
198 | void cdx_destroy_rpmsg(struct platform_device *pdev) |
199 | { |
200 | unregister_rpmsg_driver(drv: &cdx_rpmsg_driver); |
201 | |
202 | cdx_detach_to_r5(pdev); |
203 | } |
204 | |