1 // SPDX-License-Identifier: GPL-2.0
2 /*
3 * Copyright (C) 2017-2018, Intel Corporation
4 */
5
6 #include <linux/completion.h>
7 #include <linux/delay.h>
8 #include <linux/genalloc.h>
9 #include <linux/io.h>
10 #include <linux/kfifo.h>
11 #include <linux/kthread.h>
12 #include <linux/module.h>
13 #include <linux/mutex.h>
14 #include <linux/of.h>
15 #include <linux/of_platform.h>
16 #include <linux/platform_device.h>
17 #include <linux/slab.h>
18 #include <linux/spinlock.h>
19 #include <linux/firmware/intel/stratix10-smc.h>
20 #include <linux/firmware/intel/stratix10-svc-client.h>
21 #include <linux/types.h>
22
23 /**
24 * SVC_NUM_DATA_IN_FIFO - number of struct stratix10_svc_data in the FIFO
25 *
26 * SVC_NUM_CHANNEL - number of channel supported by service layer driver
27 *
28 * FPGA_CONFIG_DATA_CLAIM_TIMEOUT_MS - claim back the submitted buffer(s)
29 * from the secure world for FPGA manager to reuse, or to free the buffer(s)
30 * when all bit-stream data had be send.
31 *
32 * FPGA_CONFIG_STATUS_TIMEOUT_SEC - poll the FPGA configuration status,
33 * service layer will return error to FPGA manager when timeout occurs,
34 * timeout is set to 30 seconds (30 * 1000) at Intel Stratix10 SoC.
35 */
36 #define SVC_NUM_DATA_IN_FIFO 32
37 #define SVC_NUM_CHANNEL 3
38 #define FPGA_CONFIG_DATA_CLAIM_TIMEOUT_MS 200
39 #define FPGA_CONFIG_STATUS_TIMEOUT_SEC 30
40 #define BYTE_TO_WORD_SIZE 4
41
42 /* stratix10 service layer clients */
43 #define STRATIX10_RSU "stratix10-rsu"
44 #define INTEL_FCS "intel-fcs"
45
46 typedef void (svc_invoke_fn)(unsigned long, unsigned long, unsigned long,
47 unsigned long, unsigned long, unsigned long,
48 unsigned long, unsigned long,
49 struct arm_smccc_res *);
50 struct stratix10_svc_chan;
51
52 /**
53 * struct stratix10_svc - svc private data
54 * @stratix10_svc_rsu: pointer to stratix10 RSU device
55 */
56 struct stratix10_svc {
57 struct platform_device *stratix10_svc_rsu;
58 struct platform_device *intel_svc_fcs;
59 };
60
61 /**
62 * struct stratix10_svc_sh_memory - service shared memory structure
63 * @sync_complete: state for a completion
64 * @addr: physical address of shared memory block
65 * @size: size of shared memory block
66 * @invoke_fn: function to issue secure monitor or hypervisor call
67 *
68 * This struct is used to save physical address and size of shared memory
69 * block. The shared memory blocked is allocated by secure monitor software
70 * at secure world.
71 *
72 * Service layer driver uses the physical address and size to create a memory
73 * pool, then allocates data buffer from that memory pool for service client.
74 */
75 struct stratix10_svc_sh_memory {
76 struct completion sync_complete;
77 unsigned long addr;
78 unsigned long size;
79 svc_invoke_fn *invoke_fn;
80 };
81
82 /**
83 * struct stratix10_svc_data_mem - service memory structure
84 * @vaddr: virtual address
85 * @paddr: physical address
86 * @size: size of memory
87 * @node: link list head node
88 *
89 * This struct is used in a list that keeps track of buffers which have
90 * been allocated or freed from the memory pool. Service layer driver also
91 * uses this struct to transfer physical address to virtual address.
92 */
93 struct stratix10_svc_data_mem {
94 void *vaddr;
95 phys_addr_t paddr;
96 size_t size;
97 struct list_head node;
98 };
99
100 /**
101 * struct stratix10_svc_data - service data structure
102 * @chan: service channel
103 * @paddr: physical address of to be processed payload
104 * @size: to be processed playload size
105 * @paddr_output: physical address of processed payload
106 * @size_output: processed payload size
107 * @command: service command requested by client
108 * @flag: configuration type (full or partial)
109 * @arg: args to be passed via registers and not physically mapped buffers
110 *
111 * This struct is used in service FIFO for inter-process communication.
112 */
113 struct stratix10_svc_data {
114 struct stratix10_svc_chan *chan;
115 phys_addr_t paddr;
116 size_t size;
117 phys_addr_t paddr_output;
118 size_t size_output;
119 u32 command;
120 u32 flag;
121 u64 arg[3];
122 };
123
124 /**
125 * struct stratix10_svc_controller - service controller
126 * @dev: device
127 * @chans: array of service channels
128 * @num_chans: number of channels in 'chans' array
129 * @num_active_client: number of active service client
130 * @node: list management
131 * @genpool: memory pool pointing to the memory region
132 * @task: pointer to the thread task which handles SMC or HVC call
133 * @svc_fifo: a queue for storing service message data
134 * @complete_status: state for completion
135 * @svc_fifo_lock: protect access to service message data queue
136 * @invoke_fn: function to issue secure monitor call or hypervisor call
137 * @svc: manages the list of client svc drivers
138 *
139 * This struct is used to create communication channels for service clients, to
140 * handle secure monitor or hypervisor call.
141 */
142 struct stratix10_svc_controller {
143 struct device *dev;
144 struct stratix10_svc_chan *chans;
145 int num_chans;
146 int num_active_client;
147 struct list_head node;
148 struct gen_pool *genpool;
149 struct task_struct *task;
150 struct kfifo svc_fifo;
151 struct completion complete_status;
152 spinlock_t svc_fifo_lock;
153 svc_invoke_fn *invoke_fn;
154 struct stratix10_svc *svc;
155 };
156
157 /**
158 * struct stratix10_svc_chan - service communication channel
159 * @ctrl: pointer to service controller which is the provider of this channel
160 * @scl: pointer to service client which owns the channel
161 * @name: service client name associated with the channel
162 * @lock: protect access to the channel
163 *
164 * This struct is used by service client to communicate with service layer, each
165 * service client has its own channel created by service controller.
166 */
167 struct stratix10_svc_chan {
168 struct stratix10_svc_controller *ctrl;
169 struct stratix10_svc_client *scl;
170 char *name;
171 spinlock_t lock;
172 };
173
174 static LIST_HEAD(svc_ctrl);
175 static LIST_HEAD(svc_data_mem);
176
177 /**
178 * svc_pa_to_va() - translate physical address to virtual address
179 * @addr: to be translated physical address
180 *
181 * Return: valid virtual address or NULL if the provided physical
182 * address doesn't exist.
183 */
svc_pa_to_va(unsigned long addr)184 static void *svc_pa_to_va(unsigned long addr)
185 {
186 struct stratix10_svc_data_mem *pmem;
187
188 pr_debug("claim back P-addr=0x%016x\n", (unsigned int)addr);
189 list_for_each_entry(pmem, &svc_data_mem, node)
190 if (pmem->paddr == addr)
191 return pmem->vaddr;
192
193 /* physical address is not found */
194 return NULL;
195 }
196
197 /**
198 * svc_thread_cmd_data_claim() - claim back buffer from the secure world
199 * @ctrl: pointer to service layer controller
200 * @p_data: pointer to service data structure
201 * @cb_data: pointer to callback data structure to service client
202 *
203 * Claim back the submitted buffers from the secure world and pass buffer
204 * back to service client (FPGA manager, etc) for reuse.
205 */
svc_thread_cmd_data_claim(struct stratix10_svc_controller * ctrl,struct stratix10_svc_data * p_data,struct stratix10_svc_cb_data * cb_data)206 static void svc_thread_cmd_data_claim(struct stratix10_svc_controller *ctrl,
207 struct stratix10_svc_data *p_data,
208 struct stratix10_svc_cb_data *cb_data)
209 {
210 struct arm_smccc_res res;
211 unsigned long timeout;
212
213 reinit_completion(&ctrl->complete_status);
214 timeout = msecs_to_jiffies(FPGA_CONFIG_DATA_CLAIM_TIMEOUT_MS);
215
216 pr_debug("%s: claim back the submitted buffer\n", __func__);
217 do {
218 ctrl->invoke_fn(INTEL_SIP_SMC_FPGA_CONFIG_COMPLETED_WRITE,
219 0, 0, 0, 0, 0, 0, 0, &res);
220
221 if (res.a0 == INTEL_SIP_SMC_STATUS_OK) {
222 if (!res.a1) {
223 complete(&ctrl->complete_status);
224 break;
225 }
226 cb_data->status = BIT(SVC_STATUS_BUFFER_DONE);
227 cb_data->kaddr1 = svc_pa_to_va(res.a1);
228 cb_data->kaddr2 = (res.a2) ?
229 svc_pa_to_va(res.a2) : NULL;
230 cb_data->kaddr3 = (res.a3) ?
231 svc_pa_to_va(res.a3) : NULL;
232 p_data->chan->scl->receive_cb(p_data->chan->scl,
233 cb_data);
234 } else {
235 pr_debug("%s: secure world busy, polling again\n",
236 __func__);
237 }
238 } while (res.a0 == INTEL_SIP_SMC_STATUS_OK ||
239 res.a0 == INTEL_SIP_SMC_STATUS_BUSY ||
240 wait_for_completion_timeout(&ctrl->complete_status, timeout));
241 }
242
243 /**
244 * svc_thread_cmd_config_status() - check configuration status
245 * @ctrl: pointer to service layer controller
246 * @p_data: pointer to service data structure
247 * @cb_data: pointer to callback data structure to service client
248 *
249 * Check whether the secure firmware at secure world has finished the FPGA
250 * configuration, and then inform FPGA manager the configuration status.
251 */
svc_thread_cmd_config_status(struct stratix10_svc_controller * ctrl,struct stratix10_svc_data * p_data,struct stratix10_svc_cb_data * cb_data)252 static void svc_thread_cmd_config_status(struct stratix10_svc_controller *ctrl,
253 struct stratix10_svc_data *p_data,
254 struct stratix10_svc_cb_data *cb_data)
255 {
256 struct arm_smccc_res res;
257 int count_in_sec;
258 unsigned long a0, a1, a2;
259
260 cb_data->kaddr1 = NULL;
261 cb_data->kaddr2 = NULL;
262 cb_data->kaddr3 = NULL;
263 cb_data->status = BIT(SVC_STATUS_ERROR);
264
265 pr_debug("%s: polling config status\n", __func__);
266
267 a0 = INTEL_SIP_SMC_FPGA_CONFIG_ISDONE;
268 a1 = (unsigned long)p_data->paddr;
269 a2 = (unsigned long)p_data->size;
270
271 if (p_data->command == COMMAND_POLL_SERVICE_STATUS)
272 a0 = INTEL_SIP_SMC_SERVICE_COMPLETED;
273
274 count_in_sec = FPGA_CONFIG_STATUS_TIMEOUT_SEC;
275 while (count_in_sec) {
276 ctrl->invoke_fn(a0, a1, a2, 0, 0, 0, 0, 0, &res);
277 if ((res.a0 == INTEL_SIP_SMC_STATUS_OK) ||
278 (res.a0 == INTEL_SIP_SMC_STATUS_ERROR) ||
279 (res.a0 == INTEL_SIP_SMC_STATUS_REJECTED))
280 break;
281
282 /*
283 * request is still in progress, wait one second then
284 * poll again
285 */
286 msleep(1000);
287 count_in_sec--;
288 }
289
290 if (!count_in_sec) {
291 pr_err("%s: poll status timeout\n", __func__);
292 cb_data->status = BIT(SVC_STATUS_BUSY);
293 } else if (res.a0 == INTEL_SIP_SMC_STATUS_OK) {
294 cb_data->status = BIT(SVC_STATUS_COMPLETED);
295 cb_data->kaddr2 = (res.a2) ?
296 svc_pa_to_va(res.a2) : NULL;
297 cb_data->kaddr3 = (res.a3) ? &res.a3 : NULL;
298 } else {
299 pr_err("%s: poll status error\n", __func__);
300 cb_data->kaddr1 = &res.a1;
301 cb_data->kaddr2 = (res.a2) ?
302 svc_pa_to_va(res.a2) : NULL;
303 cb_data->kaddr3 = (res.a3) ? &res.a3 : NULL;
304 cb_data->status = BIT(SVC_STATUS_ERROR);
305 }
306
307 p_data->chan->scl->receive_cb(p_data->chan->scl, cb_data);
308 }
309
310 /**
311 * svc_thread_recv_status_ok() - handle the successful status
312 * @p_data: pointer to service data structure
313 * @cb_data: pointer to callback data structure to service client
314 * @res: result from SMC or HVC call
315 *
316 * Send back the correspond status to the service clients.
317 */
svc_thread_recv_status_ok(struct stratix10_svc_data * p_data,struct stratix10_svc_cb_data * cb_data,struct arm_smccc_res res)318 static void svc_thread_recv_status_ok(struct stratix10_svc_data *p_data,
319 struct stratix10_svc_cb_data *cb_data,
320 struct arm_smccc_res res)
321 {
322 cb_data->kaddr1 = NULL;
323 cb_data->kaddr2 = NULL;
324 cb_data->kaddr3 = NULL;
325
326 switch (p_data->command) {
327 case COMMAND_RECONFIG:
328 case COMMAND_RSU_UPDATE:
329 case COMMAND_RSU_NOTIFY:
330 case COMMAND_FCS_REQUEST_SERVICE:
331 case COMMAND_FCS_SEND_CERTIFICATE:
332 case COMMAND_FCS_DATA_ENCRYPTION:
333 case COMMAND_FCS_DATA_DECRYPTION:
334 cb_data->status = BIT(SVC_STATUS_OK);
335 break;
336 case COMMAND_RECONFIG_DATA_SUBMIT:
337 cb_data->status = BIT(SVC_STATUS_BUFFER_SUBMITTED);
338 break;
339 case COMMAND_RECONFIG_STATUS:
340 cb_data->status = BIT(SVC_STATUS_COMPLETED);
341 break;
342 case COMMAND_RSU_RETRY:
343 case COMMAND_RSU_MAX_RETRY:
344 case COMMAND_RSU_DCMF_STATUS:
345 case COMMAND_FIRMWARE_VERSION:
346 cb_data->status = BIT(SVC_STATUS_OK);
347 cb_data->kaddr1 = &res.a1;
348 break;
349 case COMMAND_SMC_SVC_VERSION:
350 cb_data->status = BIT(SVC_STATUS_OK);
351 cb_data->kaddr1 = &res.a1;
352 cb_data->kaddr2 = &res.a2;
353 break;
354 case COMMAND_RSU_DCMF_VERSION:
355 cb_data->status = BIT(SVC_STATUS_OK);
356 cb_data->kaddr1 = &res.a1;
357 cb_data->kaddr2 = &res.a2;
358 break;
359 case COMMAND_FCS_RANDOM_NUMBER_GEN:
360 case COMMAND_FCS_GET_PROVISION_DATA:
361 case COMMAND_POLL_SERVICE_STATUS:
362 cb_data->status = BIT(SVC_STATUS_OK);
363 cb_data->kaddr1 = &res.a1;
364 cb_data->kaddr2 = svc_pa_to_va(res.a2);
365 cb_data->kaddr3 = &res.a3;
366 break;
367 case COMMAND_MBOX_SEND_CMD:
368 cb_data->status = BIT(SVC_STATUS_OK);
369 cb_data->kaddr1 = &res.a1;
370 /* SDM return size in u8. Convert size to u32 word */
371 res.a2 = res.a2 * BYTE_TO_WORD_SIZE;
372 cb_data->kaddr2 = &res.a2;
373 break;
374 default:
375 pr_warn("it shouldn't happen\n");
376 break;
377 }
378
379 pr_debug("%s: call receive_cb\n", __func__);
380 p_data->chan->scl->receive_cb(p_data->chan->scl, cb_data);
381 }
382
383 /**
384 * svc_normal_to_secure_thread() - the function to run in the kthread
385 * @data: data pointer for kthread function
386 *
387 * Service layer driver creates stratix10_svc_smc_hvc_call kthread on CPU
388 * node 0, its function stratix10_svc_secure_call_thread is used to handle
389 * SMC or HVC calls between kernel driver and secure monitor software.
390 *
391 * Return: 0 for success or -ENOMEM on error.
392 */
svc_normal_to_secure_thread(void * data)393 static int svc_normal_to_secure_thread(void *data)
394 {
395 struct stratix10_svc_controller
396 *ctrl = (struct stratix10_svc_controller *)data;
397 struct stratix10_svc_data *pdata;
398 struct stratix10_svc_cb_data *cbdata;
399 struct arm_smccc_res res;
400 unsigned long a0, a1, a2, a3, a4, a5, a6, a7;
401 int ret_fifo = 0;
402
403 pdata = kmalloc(sizeof(*pdata), GFP_KERNEL);
404 if (!pdata)
405 return -ENOMEM;
406
407 cbdata = kmalloc(sizeof(*cbdata), GFP_KERNEL);
408 if (!cbdata) {
409 kfree(pdata);
410 return -ENOMEM;
411 }
412
413 /* default set, to remove build warning */
414 a0 = INTEL_SIP_SMC_FPGA_CONFIG_LOOPBACK;
415 a1 = 0;
416 a2 = 0;
417 a3 = 0;
418 a4 = 0;
419 a5 = 0;
420 a6 = 0;
421 a7 = 0;
422
423 pr_debug("smc_hvc_shm_thread is running\n");
424
425 while (!kthread_should_stop()) {
426 ret_fifo = kfifo_out_spinlocked(&ctrl->svc_fifo,
427 pdata, sizeof(*pdata),
428 &ctrl->svc_fifo_lock);
429
430 if (!ret_fifo)
431 continue;
432
433 pr_debug("get from FIFO pa=0x%016x, command=%u, size=%u\n",
434 (unsigned int)pdata->paddr, pdata->command,
435 (unsigned int)pdata->size);
436
437 switch (pdata->command) {
438 case COMMAND_RECONFIG_DATA_CLAIM:
439 svc_thread_cmd_data_claim(ctrl, pdata, cbdata);
440 continue;
441 case COMMAND_RECONFIG:
442 a0 = INTEL_SIP_SMC_FPGA_CONFIG_START;
443 pr_debug("conf_type=%u\n", (unsigned int)pdata->flag);
444 a1 = pdata->flag;
445 a2 = 0;
446 break;
447 case COMMAND_RECONFIG_DATA_SUBMIT:
448 a0 = INTEL_SIP_SMC_FPGA_CONFIG_WRITE;
449 a1 = (unsigned long)pdata->paddr;
450 a2 = (unsigned long)pdata->size;
451 break;
452 case COMMAND_RECONFIG_STATUS:
453 a0 = INTEL_SIP_SMC_FPGA_CONFIG_ISDONE;
454 a1 = 0;
455 a2 = 0;
456 break;
457 case COMMAND_RSU_STATUS:
458 a0 = INTEL_SIP_SMC_RSU_STATUS;
459 a1 = 0;
460 a2 = 0;
461 break;
462 case COMMAND_RSU_UPDATE:
463 a0 = INTEL_SIP_SMC_RSU_UPDATE;
464 a1 = pdata->arg[0];
465 a2 = 0;
466 break;
467 case COMMAND_RSU_NOTIFY:
468 a0 = INTEL_SIP_SMC_RSU_NOTIFY;
469 a1 = pdata->arg[0];
470 a2 = 0;
471 break;
472 case COMMAND_RSU_RETRY:
473 a0 = INTEL_SIP_SMC_RSU_RETRY_COUNTER;
474 a1 = 0;
475 a2 = 0;
476 break;
477 case COMMAND_RSU_MAX_RETRY:
478 a0 = INTEL_SIP_SMC_RSU_MAX_RETRY;
479 a1 = 0;
480 a2 = 0;
481 break;
482 case COMMAND_RSU_DCMF_VERSION:
483 a0 = INTEL_SIP_SMC_RSU_DCMF_VERSION;
484 a1 = 0;
485 a2 = 0;
486 break;
487 case COMMAND_FIRMWARE_VERSION:
488 a0 = INTEL_SIP_SMC_FIRMWARE_VERSION;
489 a1 = 0;
490 a2 = 0;
491 break;
492
493 /* for FCS */
494 case COMMAND_FCS_DATA_ENCRYPTION:
495 a0 = INTEL_SIP_SMC_FCS_CRYPTION;
496 a1 = 1;
497 a2 = (unsigned long)pdata->paddr;
498 a3 = (unsigned long)pdata->size;
499 a4 = (unsigned long)pdata->paddr_output;
500 a5 = (unsigned long)pdata->size_output;
501 break;
502 case COMMAND_FCS_DATA_DECRYPTION:
503 a0 = INTEL_SIP_SMC_FCS_CRYPTION;
504 a1 = 0;
505 a2 = (unsigned long)pdata->paddr;
506 a3 = (unsigned long)pdata->size;
507 a4 = (unsigned long)pdata->paddr_output;
508 a5 = (unsigned long)pdata->size_output;
509 break;
510 case COMMAND_FCS_RANDOM_NUMBER_GEN:
511 a0 = INTEL_SIP_SMC_FCS_RANDOM_NUMBER;
512 a1 = (unsigned long)pdata->paddr;
513 a2 = 0;
514 break;
515 case COMMAND_FCS_REQUEST_SERVICE:
516 a0 = INTEL_SIP_SMC_FCS_SERVICE_REQUEST;
517 a1 = (unsigned long)pdata->paddr;
518 a2 = (unsigned long)pdata->size;
519 break;
520 case COMMAND_FCS_SEND_CERTIFICATE:
521 a0 = INTEL_SIP_SMC_FCS_SEND_CERTIFICATE;
522 a1 = (unsigned long)pdata->paddr;
523 a2 = (unsigned long)pdata->size;
524 break;
525 case COMMAND_FCS_GET_PROVISION_DATA:
526 a0 = INTEL_SIP_SMC_FCS_GET_PROVISION_DATA;
527 a1 = (unsigned long)pdata->paddr;
528 a2 = 0;
529 break;
530
531 /* for polling */
532 case COMMAND_POLL_SERVICE_STATUS:
533 a0 = INTEL_SIP_SMC_SERVICE_COMPLETED;
534 a1 = (unsigned long)pdata->paddr;
535 a2 = (unsigned long)pdata->size;
536 break;
537 case COMMAND_RSU_DCMF_STATUS:
538 a0 = INTEL_SIP_SMC_RSU_DCMF_STATUS;
539 a1 = 0;
540 a2 = 0;
541 break;
542 case COMMAND_SMC_SVC_VERSION:
543 a0 = INTEL_SIP_SMC_SVC_VERSION;
544 a1 = 0;
545 a2 = 0;
546 break;
547 case COMMAND_MBOX_SEND_CMD:
548 a0 = INTEL_SIP_SMC_MBOX_SEND_CMD;
549 a1 = pdata->arg[0];
550 a2 = (unsigned long)pdata->paddr;
551 a3 = (unsigned long)pdata->size / BYTE_TO_WORD_SIZE;
552 a4 = pdata->arg[1];
553 a5 = (unsigned long)pdata->paddr_output;
554 a6 = (unsigned long)pdata->size_output / BYTE_TO_WORD_SIZE;
555 break;
556 default:
557 pr_warn("it shouldn't happen\n");
558 break;
559 }
560 pr_debug("%s: before SMC call -- a0=0x%016x a1=0x%016x",
561 __func__,
562 (unsigned int)a0,
563 (unsigned int)a1);
564 pr_debug(" a2=0x%016x\n", (unsigned int)a2);
565 pr_debug(" a3=0x%016x\n", (unsigned int)a3);
566 pr_debug(" a4=0x%016x\n", (unsigned int)a4);
567 pr_debug(" a5=0x%016x\n", (unsigned int)a5);
568 ctrl->invoke_fn(a0, a1, a2, a3, a4, a5, a6, a7, &res);
569
570 pr_debug("%s: after SMC call -- res.a0=0x%016x",
571 __func__, (unsigned int)res.a0);
572 pr_debug(" res.a1=0x%016x, res.a2=0x%016x",
573 (unsigned int)res.a1, (unsigned int)res.a2);
574 pr_debug(" res.a3=0x%016x\n", (unsigned int)res.a3);
575
576 if (pdata->command == COMMAND_RSU_STATUS) {
577 if (res.a0 == INTEL_SIP_SMC_RSU_ERROR)
578 cbdata->status = BIT(SVC_STATUS_ERROR);
579 else
580 cbdata->status = BIT(SVC_STATUS_OK);
581
582 cbdata->kaddr1 = &res;
583 cbdata->kaddr2 = NULL;
584 cbdata->kaddr3 = NULL;
585 pdata->chan->scl->receive_cb(pdata->chan->scl, cbdata);
586 continue;
587 }
588
589 switch (res.a0) {
590 case INTEL_SIP_SMC_STATUS_OK:
591 svc_thread_recv_status_ok(pdata, cbdata, res);
592 break;
593 case INTEL_SIP_SMC_STATUS_BUSY:
594 switch (pdata->command) {
595 case COMMAND_RECONFIG_DATA_SUBMIT:
596 svc_thread_cmd_data_claim(ctrl,
597 pdata, cbdata);
598 break;
599 case COMMAND_RECONFIG_STATUS:
600 case COMMAND_POLL_SERVICE_STATUS:
601 svc_thread_cmd_config_status(ctrl,
602 pdata, cbdata);
603 break;
604 default:
605 pr_warn("it shouldn't happen\n");
606 break;
607 }
608 break;
609 case INTEL_SIP_SMC_STATUS_REJECTED:
610 pr_debug("%s: STATUS_REJECTED\n", __func__);
611 /* for FCS */
612 switch (pdata->command) {
613 case COMMAND_FCS_REQUEST_SERVICE:
614 case COMMAND_FCS_SEND_CERTIFICATE:
615 case COMMAND_FCS_GET_PROVISION_DATA:
616 case COMMAND_FCS_DATA_ENCRYPTION:
617 case COMMAND_FCS_DATA_DECRYPTION:
618 case COMMAND_FCS_RANDOM_NUMBER_GEN:
619 case COMMAND_MBOX_SEND_CMD:
620 cbdata->status = BIT(SVC_STATUS_INVALID_PARAM);
621 cbdata->kaddr1 = NULL;
622 cbdata->kaddr2 = NULL;
623 cbdata->kaddr3 = NULL;
624 pdata->chan->scl->receive_cb(pdata->chan->scl,
625 cbdata);
626 break;
627 }
628 break;
629 case INTEL_SIP_SMC_STATUS_ERROR:
630 case INTEL_SIP_SMC_RSU_ERROR:
631 pr_err("%s: STATUS_ERROR\n", __func__);
632 cbdata->status = BIT(SVC_STATUS_ERROR);
633 cbdata->kaddr1 = &res.a1;
634 cbdata->kaddr2 = (res.a2) ?
635 svc_pa_to_va(res.a2) : NULL;
636 cbdata->kaddr3 = (res.a3) ? &res.a3 : NULL;
637 pdata->chan->scl->receive_cb(pdata->chan->scl, cbdata);
638 break;
639 default:
640 pr_warn("Secure firmware doesn't support...\n");
641
642 /*
643 * be compatible with older version firmware which
644 * doesn't support newer RSU commands
645 */
646 if ((pdata->command != COMMAND_RSU_UPDATE) &&
647 (pdata->command != COMMAND_RSU_STATUS)) {
648 cbdata->status =
649 BIT(SVC_STATUS_NO_SUPPORT);
650 cbdata->kaddr1 = NULL;
651 cbdata->kaddr2 = NULL;
652 cbdata->kaddr3 = NULL;
653 pdata->chan->scl->receive_cb(
654 pdata->chan->scl, cbdata);
655 }
656 break;
657
658 }
659 }
660
661 kfree(cbdata);
662 kfree(pdata);
663
664 return 0;
665 }
666
667 /**
668 * svc_normal_to_secure_shm_thread() - the function to run in the kthread
669 * @data: data pointer for kthread function
670 *
671 * Service layer driver creates stratix10_svc_smc_hvc_shm kthread on CPU
672 * node 0, its function stratix10_svc_secure_shm_thread is used to query the
673 * physical address of memory block reserved by secure monitor software at
674 * secure world.
675 *
676 * svc_normal_to_secure_shm_thread() terminates directly since it is a
677 * standlone thread for which no one will call kthread_stop() or return when
678 * 'kthread_should_stop()' is true.
679 */
svc_normal_to_secure_shm_thread(void * data)680 static int svc_normal_to_secure_shm_thread(void *data)
681 {
682 struct stratix10_svc_sh_memory
683 *sh_mem = (struct stratix10_svc_sh_memory *)data;
684 struct arm_smccc_res res;
685
686 /* SMC or HVC call to get shared memory info from secure world */
687 sh_mem->invoke_fn(INTEL_SIP_SMC_FPGA_CONFIG_GET_MEM,
688 0, 0, 0, 0, 0, 0, 0, &res);
689 if (res.a0 == INTEL_SIP_SMC_STATUS_OK) {
690 sh_mem->addr = res.a1;
691 sh_mem->size = res.a2;
692 } else {
693 pr_err("%s: after SMC call -- res.a0=0x%016x", __func__,
694 (unsigned int)res.a0);
695 sh_mem->addr = 0;
696 sh_mem->size = 0;
697 }
698
699 complete(&sh_mem->sync_complete);
700 return 0;
701 }
702
703 /**
704 * svc_get_sh_memory() - get memory block reserved by secure monitor SW
705 * @pdev: pointer to service layer device
706 * @sh_memory: pointer to service shared memory structure
707 *
708 * Return: zero for successfully getting the physical address of memory block
709 * reserved by secure monitor software, or negative value on error.
710 */
svc_get_sh_memory(struct platform_device * pdev,struct stratix10_svc_sh_memory * sh_memory)711 static int svc_get_sh_memory(struct platform_device *pdev,
712 struct stratix10_svc_sh_memory *sh_memory)
713 {
714 struct device *dev = &pdev->dev;
715 struct task_struct *sh_memory_task;
716 unsigned int cpu = 0;
717
718 init_completion(&sh_memory->sync_complete);
719
720 /* smc or hvc call happens on cpu 0 bound kthread */
721 sh_memory_task = kthread_create_on_node(svc_normal_to_secure_shm_thread,
722 (void *)sh_memory,
723 cpu_to_node(cpu),
724 "svc_smc_hvc_shm_thread");
725 if (IS_ERR(sh_memory_task)) {
726 dev_err(dev, "fail to create stratix10_svc_smc_shm_thread\n");
727 return -EINVAL;
728 }
729
730 wake_up_process(sh_memory_task);
731
732 if (!wait_for_completion_timeout(&sh_memory->sync_complete, 10 * HZ)) {
733 dev_err(dev,
734 "timeout to get sh-memory paras from secure world\n");
735 return -ETIMEDOUT;
736 }
737
738 if (!sh_memory->addr || !sh_memory->size) {
739 dev_err(dev,
740 "failed to get shared memory info from secure world\n");
741 return -ENOMEM;
742 }
743
744 dev_dbg(dev, "SM software provides paddr: 0x%016x, size: 0x%08x\n",
745 (unsigned int)sh_memory->addr,
746 (unsigned int)sh_memory->size);
747
748 return 0;
749 }
750
751 /**
752 * svc_create_memory_pool() - create a memory pool from reserved memory block
753 * @pdev: pointer to service layer device
754 * @sh_memory: pointer to service shared memory structure
755 *
756 * Return: pool allocated from reserved memory block or ERR_PTR() on error.
757 */
758 static struct gen_pool *
svc_create_memory_pool(struct platform_device * pdev,struct stratix10_svc_sh_memory * sh_memory)759 svc_create_memory_pool(struct platform_device *pdev,
760 struct stratix10_svc_sh_memory *sh_memory)
761 {
762 struct device *dev = &pdev->dev;
763 struct gen_pool *genpool;
764 unsigned long vaddr;
765 phys_addr_t paddr;
766 size_t size;
767 phys_addr_t begin;
768 phys_addr_t end;
769 void *va;
770 size_t page_mask = PAGE_SIZE - 1;
771 int min_alloc_order = 3;
772 int ret;
773
774 begin = roundup(sh_memory->addr, PAGE_SIZE);
775 end = rounddown(sh_memory->addr + sh_memory->size, PAGE_SIZE);
776 paddr = begin;
777 size = end - begin;
778 va = devm_memremap(dev, paddr, size, MEMREMAP_WC);
779 if (IS_ERR(va)) {
780 dev_err(dev, "fail to remap shared memory\n");
781 return ERR_PTR(-EINVAL);
782 }
783 vaddr = (unsigned long)va;
784 dev_dbg(dev,
785 "reserved memory vaddr: %p, paddr: 0x%16x size: 0x%8x\n",
786 va, (unsigned int)paddr, (unsigned int)size);
787 if ((vaddr & page_mask) || (paddr & page_mask) ||
788 (size & page_mask)) {
789 dev_err(dev, "page is not aligned\n");
790 return ERR_PTR(-EINVAL);
791 }
792 genpool = gen_pool_create(min_alloc_order, -1);
793 if (!genpool) {
794 dev_err(dev, "fail to create genpool\n");
795 return ERR_PTR(-ENOMEM);
796 }
797 gen_pool_set_algo(genpool, gen_pool_best_fit, NULL);
798 ret = gen_pool_add_virt(genpool, vaddr, paddr, size, -1);
799 if (ret) {
800 dev_err(dev, "fail to add memory chunk to the pool\n");
801 gen_pool_destroy(genpool);
802 return ERR_PTR(ret);
803 }
804
805 return genpool;
806 }
807
808 /**
809 * svc_smccc_smc() - secure monitor call between normal and secure world
810 * @a0: argument passed in registers 0
811 * @a1: argument passed in registers 1
812 * @a2: argument passed in registers 2
813 * @a3: argument passed in registers 3
814 * @a4: argument passed in registers 4
815 * @a5: argument passed in registers 5
816 * @a6: argument passed in registers 6
817 * @a7: argument passed in registers 7
818 * @res: result values from register 0 to 3
819 */
svc_smccc_smc(unsigned long a0,unsigned long a1,unsigned long a2,unsigned long a3,unsigned long a4,unsigned long a5,unsigned long a6,unsigned long a7,struct arm_smccc_res * res)820 static void svc_smccc_smc(unsigned long a0, unsigned long a1,
821 unsigned long a2, unsigned long a3,
822 unsigned long a4, unsigned long a5,
823 unsigned long a6, unsigned long a7,
824 struct arm_smccc_res *res)
825 {
826 arm_smccc_smc(a0, a1, a2, a3, a4, a5, a6, a7, res);
827 }
828
829 /**
830 * svc_smccc_hvc() - hypervisor call between normal and secure world
831 * @a0: argument passed in registers 0
832 * @a1: argument passed in registers 1
833 * @a2: argument passed in registers 2
834 * @a3: argument passed in registers 3
835 * @a4: argument passed in registers 4
836 * @a5: argument passed in registers 5
837 * @a6: argument passed in registers 6
838 * @a7: argument passed in registers 7
839 * @res: result values from register 0 to 3
840 */
svc_smccc_hvc(unsigned long a0,unsigned long a1,unsigned long a2,unsigned long a3,unsigned long a4,unsigned long a5,unsigned long a6,unsigned long a7,struct arm_smccc_res * res)841 static void svc_smccc_hvc(unsigned long a0, unsigned long a1,
842 unsigned long a2, unsigned long a3,
843 unsigned long a4, unsigned long a5,
844 unsigned long a6, unsigned long a7,
845 struct arm_smccc_res *res)
846 {
847 arm_smccc_hvc(a0, a1, a2, a3, a4, a5, a6, a7, res);
848 }
849
850 /**
851 * get_invoke_func() - invoke SMC or HVC call
852 * @dev: pointer to device
853 *
854 * Return: function pointer to svc_smccc_smc or svc_smccc_hvc.
855 */
get_invoke_func(struct device * dev)856 static svc_invoke_fn *get_invoke_func(struct device *dev)
857 {
858 const char *method;
859
860 if (of_property_read_string(dev->of_node, "method", &method)) {
861 dev_warn(dev, "missing \"method\" property\n");
862 return ERR_PTR(-ENXIO);
863 }
864
865 if (!strcmp(method, "smc"))
866 return svc_smccc_smc;
867 if (!strcmp(method, "hvc"))
868 return svc_smccc_hvc;
869
870 dev_warn(dev, "invalid \"method\" property: %s\n", method);
871
872 return ERR_PTR(-EINVAL);
873 }
874
875 /**
876 * stratix10_svc_request_channel_byname() - request a service channel
877 * @client: pointer to service client
878 * @name: service client name
879 *
880 * This function is used by service client to request a service channel.
881 *
882 * Return: a pointer to channel assigned to the client on success,
883 * or ERR_PTR() on error.
884 */
stratix10_svc_request_channel_byname(struct stratix10_svc_client * client,const char * name)885 struct stratix10_svc_chan *stratix10_svc_request_channel_byname(
886 struct stratix10_svc_client *client, const char *name)
887 {
888 struct device *dev = client->dev;
889 struct stratix10_svc_controller *controller;
890 struct stratix10_svc_chan *chan = NULL;
891 unsigned long flag;
892 int i;
893
894 /* if probe was called after client's, or error on probe */
895 if (list_empty(&svc_ctrl))
896 return ERR_PTR(-EPROBE_DEFER);
897
898 controller = list_first_entry(&svc_ctrl,
899 struct stratix10_svc_controller, node);
900 for (i = 0; i < SVC_NUM_CHANNEL; i++) {
901 if (!strcmp(controller->chans[i].name, name)) {
902 chan = &controller->chans[i];
903 break;
904 }
905 }
906
907 /* if there was no channel match */
908 if (i == SVC_NUM_CHANNEL) {
909 dev_err(dev, "%s: channel not allocated\n", __func__);
910 return ERR_PTR(-EINVAL);
911 }
912
913 if (chan->scl || !try_module_get(controller->dev->driver->owner)) {
914 dev_dbg(dev, "%s: svc not free\n", __func__);
915 return ERR_PTR(-EBUSY);
916 }
917
918 spin_lock_irqsave(&chan->lock, flag);
919 chan->scl = client;
920 chan->ctrl->num_active_client++;
921 spin_unlock_irqrestore(&chan->lock, flag);
922
923 return chan;
924 }
925 EXPORT_SYMBOL_GPL(stratix10_svc_request_channel_byname);
926
927 /**
928 * stratix10_svc_free_channel() - free service channel
929 * @chan: service channel to be freed
930 *
931 * This function is used by service client to free a service channel.
932 */
stratix10_svc_free_channel(struct stratix10_svc_chan * chan)933 void stratix10_svc_free_channel(struct stratix10_svc_chan *chan)
934 {
935 unsigned long flag;
936
937 spin_lock_irqsave(&chan->lock, flag);
938 chan->scl = NULL;
939 chan->ctrl->num_active_client--;
940 module_put(chan->ctrl->dev->driver->owner);
941 spin_unlock_irqrestore(&chan->lock, flag);
942 }
943 EXPORT_SYMBOL_GPL(stratix10_svc_free_channel);
944
945 /**
946 * stratix10_svc_send() - send a message data to the remote
947 * @chan: service channel assigned to the client
948 * @msg: message data to be sent, in the format of
949 * "struct stratix10_svc_client_msg"
950 *
951 * This function is used by service client to add a message to the service
952 * layer driver's queue for being sent to the secure world.
953 *
954 * Return: 0 for success, -ENOMEM or -ENOBUFS on error.
955 */
stratix10_svc_send(struct stratix10_svc_chan * chan,void * msg)956 int stratix10_svc_send(struct stratix10_svc_chan *chan, void *msg)
957 {
958 struct stratix10_svc_client_msg
959 *p_msg = (struct stratix10_svc_client_msg *)msg;
960 struct stratix10_svc_data_mem *p_mem;
961 struct stratix10_svc_data *p_data;
962 int ret = 0;
963 unsigned int cpu = 0;
964
965 p_data = kzalloc(sizeof(*p_data), GFP_KERNEL);
966 if (!p_data)
967 return -ENOMEM;
968
969 /* first client will create kernel thread */
970 if (!chan->ctrl->task) {
971 chan->ctrl->task =
972 kthread_run_on_cpu(svc_normal_to_secure_thread,
973 (void *)chan->ctrl,
974 cpu, "svc_smc_hvc_thread");
975 if (IS_ERR(chan->ctrl->task)) {
976 dev_err(chan->ctrl->dev,
977 "failed to create svc_smc_hvc_thread\n");
978 kfree(p_data);
979 return -EINVAL;
980 }
981 }
982
983 pr_debug("%s: sent P-va=%p, P-com=%x, P-size=%u\n", __func__,
984 p_msg->payload, p_msg->command,
985 (unsigned int)p_msg->payload_length);
986
987 if (list_empty(&svc_data_mem)) {
988 if (p_msg->command == COMMAND_RECONFIG) {
989 struct stratix10_svc_command_config_type *ct =
990 (struct stratix10_svc_command_config_type *)
991 p_msg->payload;
992 p_data->flag = ct->flags;
993 }
994 } else {
995 list_for_each_entry(p_mem, &svc_data_mem, node)
996 if (p_mem->vaddr == p_msg->payload) {
997 p_data->paddr = p_mem->paddr;
998 p_data->size = p_msg->payload_length;
999 break;
1000 }
1001 if (p_msg->payload_output) {
1002 list_for_each_entry(p_mem, &svc_data_mem, node)
1003 if (p_mem->vaddr == p_msg->payload_output) {
1004 p_data->paddr_output =
1005 p_mem->paddr;
1006 p_data->size_output =
1007 p_msg->payload_length_output;
1008 break;
1009 }
1010 }
1011 }
1012
1013 p_data->command = p_msg->command;
1014 p_data->arg[0] = p_msg->arg[0];
1015 p_data->arg[1] = p_msg->arg[1];
1016 p_data->arg[2] = p_msg->arg[2];
1017 p_data->size = p_msg->payload_length;
1018 p_data->chan = chan;
1019 pr_debug("%s: put to FIFO pa=0x%016x, cmd=%x, size=%u\n", __func__,
1020 (unsigned int)p_data->paddr, p_data->command,
1021 (unsigned int)p_data->size);
1022 ret = kfifo_in_spinlocked(&chan->ctrl->svc_fifo, p_data,
1023 sizeof(*p_data),
1024 &chan->ctrl->svc_fifo_lock);
1025
1026 kfree(p_data);
1027
1028 if (!ret)
1029 return -ENOBUFS;
1030
1031 return 0;
1032 }
1033 EXPORT_SYMBOL_GPL(stratix10_svc_send);
1034
1035 /**
1036 * stratix10_svc_done() - complete service request transactions
1037 * @chan: service channel assigned to the client
1038 *
1039 * This function should be called when client has finished its request
1040 * or there is an error in the request process. It allows the service layer
1041 * to stop the running thread to have maximize savings in kernel resources.
1042 */
stratix10_svc_done(struct stratix10_svc_chan * chan)1043 void stratix10_svc_done(struct stratix10_svc_chan *chan)
1044 {
1045 /* stop thread when thread is running AND only one active client */
1046 if (chan->ctrl->task && chan->ctrl->num_active_client <= 1) {
1047 pr_debug("svc_smc_hvc_shm_thread is stopped\n");
1048 kthread_stop(chan->ctrl->task);
1049 chan->ctrl->task = NULL;
1050 }
1051 }
1052 EXPORT_SYMBOL_GPL(stratix10_svc_done);
1053
1054 /**
1055 * stratix10_svc_allocate_memory() - allocate memory
1056 * @chan: service channel assigned to the client
1057 * @size: memory size requested by a specific service client
1058 *
1059 * Service layer allocates the requested number of bytes buffer from the
1060 * memory pool, service client uses this function to get allocated buffers.
1061 *
1062 * Return: address of allocated memory on success, or ERR_PTR() on error.
1063 */
stratix10_svc_allocate_memory(struct stratix10_svc_chan * chan,size_t size)1064 void *stratix10_svc_allocate_memory(struct stratix10_svc_chan *chan,
1065 size_t size)
1066 {
1067 struct stratix10_svc_data_mem *pmem;
1068 unsigned long va;
1069 phys_addr_t pa;
1070 struct gen_pool *genpool = chan->ctrl->genpool;
1071 size_t s = roundup(size, 1 << genpool->min_alloc_order);
1072
1073 pmem = devm_kzalloc(chan->ctrl->dev, sizeof(*pmem), GFP_KERNEL);
1074 if (!pmem)
1075 return ERR_PTR(-ENOMEM);
1076
1077 va = gen_pool_alloc(genpool, s);
1078 if (!va)
1079 return ERR_PTR(-ENOMEM);
1080
1081 memset((void *)va, 0, s);
1082 pa = gen_pool_virt_to_phys(genpool, va);
1083
1084 pmem->vaddr = (void *)va;
1085 pmem->paddr = pa;
1086 pmem->size = s;
1087 list_add_tail(&pmem->node, &svc_data_mem);
1088 pr_debug("%s: va=%p, pa=0x%016x\n", __func__,
1089 pmem->vaddr, (unsigned int)pmem->paddr);
1090
1091 return (void *)va;
1092 }
1093 EXPORT_SYMBOL_GPL(stratix10_svc_allocate_memory);
1094
1095 /**
1096 * stratix10_svc_free_memory() - free allocated memory
1097 * @chan: service channel assigned to the client
1098 * @kaddr: memory to be freed
1099 *
1100 * This function is used by service client to free allocated buffers.
1101 */
stratix10_svc_free_memory(struct stratix10_svc_chan * chan,void * kaddr)1102 void stratix10_svc_free_memory(struct stratix10_svc_chan *chan, void *kaddr)
1103 {
1104 struct stratix10_svc_data_mem *pmem;
1105
1106 list_for_each_entry(pmem, &svc_data_mem, node)
1107 if (pmem->vaddr == kaddr) {
1108 gen_pool_free(chan->ctrl->genpool,
1109 (unsigned long)kaddr, pmem->size);
1110 pmem->vaddr = NULL;
1111 list_del(&pmem->node);
1112 return;
1113 }
1114
1115 list_del(&svc_data_mem);
1116 }
1117 EXPORT_SYMBOL_GPL(stratix10_svc_free_memory);
1118
1119 static const struct of_device_id stratix10_svc_drv_match[] = {
1120 {.compatible = "intel,stratix10-svc"},
1121 {.compatible = "intel,agilex-svc"},
1122 {},
1123 };
1124
stratix10_svc_drv_probe(struct platform_device * pdev)1125 static int stratix10_svc_drv_probe(struct platform_device *pdev)
1126 {
1127 struct device *dev = &pdev->dev;
1128 struct stratix10_svc_controller *controller;
1129 struct stratix10_svc_chan *chans;
1130 struct gen_pool *genpool;
1131 struct stratix10_svc_sh_memory *sh_memory;
1132 struct stratix10_svc *svc;
1133
1134 svc_invoke_fn *invoke_fn;
1135 size_t fifo_size;
1136 int ret;
1137
1138 /* get SMC or HVC function */
1139 invoke_fn = get_invoke_func(dev);
1140 if (IS_ERR(invoke_fn))
1141 return -EINVAL;
1142
1143 sh_memory = devm_kzalloc(dev, sizeof(*sh_memory), GFP_KERNEL);
1144 if (!sh_memory)
1145 return -ENOMEM;
1146
1147 sh_memory->invoke_fn = invoke_fn;
1148 ret = svc_get_sh_memory(pdev, sh_memory);
1149 if (ret)
1150 return ret;
1151
1152 genpool = svc_create_memory_pool(pdev, sh_memory);
1153 if (IS_ERR(genpool))
1154 return PTR_ERR(genpool);
1155
1156 /* allocate service controller and supporting channel */
1157 controller = devm_kzalloc(dev, sizeof(*controller), GFP_KERNEL);
1158 if (!controller) {
1159 ret = -ENOMEM;
1160 goto err_destroy_pool;
1161 }
1162
1163 chans = devm_kmalloc_array(dev, SVC_NUM_CHANNEL,
1164 sizeof(*chans), GFP_KERNEL | __GFP_ZERO);
1165 if (!chans) {
1166 ret = -ENOMEM;
1167 goto err_destroy_pool;
1168 }
1169
1170 controller->dev = dev;
1171 controller->num_chans = SVC_NUM_CHANNEL;
1172 controller->num_active_client = 0;
1173 controller->chans = chans;
1174 controller->genpool = genpool;
1175 controller->task = NULL;
1176 controller->invoke_fn = invoke_fn;
1177 init_completion(&controller->complete_status);
1178
1179 fifo_size = sizeof(struct stratix10_svc_data) * SVC_NUM_DATA_IN_FIFO;
1180 ret = kfifo_alloc(&controller->svc_fifo, fifo_size, GFP_KERNEL);
1181 if (ret) {
1182 dev_err(dev, "failed to allocate FIFO\n");
1183 goto err_destroy_pool;
1184 }
1185 spin_lock_init(&controller->svc_fifo_lock);
1186
1187 chans[0].scl = NULL;
1188 chans[0].ctrl = controller;
1189 chans[0].name = SVC_CLIENT_FPGA;
1190 spin_lock_init(&chans[0].lock);
1191
1192 chans[1].scl = NULL;
1193 chans[1].ctrl = controller;
1194 chans[1].name = SVC_CLIENT_RSU;
1195 spin_lock_init(&chans[1].lock);
1196
1197 chans[2].scl = NULL;
1198 chans[2].ctrl = controller;
1199 chans[2].name = SVC_CLIENT_FCS;
1200 spin_lock_init(&chans[2].lock);
1201
1202 list_add_tail(&controller->node, &svc_ctrl);
1203 platform_set_drvdata(pdev, controller);
1204
1205 /* add svc client device(s) */
1206 svc = devm_kzalloc(dev, sizeof(*svc), GFP_KERNEL);
1207 if (!svc) {
1208 ret = -ENOMEM;
1209 goto err_free_kfifo;
1210 }
1211 controller->svc = svc;
1212
1213 svc->stratix10_svc_rsu = platform_device_alloc(STRATIX10_RSU, 0);
1214 if (!svc->stratix10_svc_rsu) {
1215 dev_err(dev, "failed to allocate %s device\n", STRATIX10_RSU);
1216 ret = -ENOMEM;
1217 goto err_free_kfifo;
1218 }
1219
1220 ret = platform_device_add(svc->stratix10_svc_rsu);
1221 if (ret) {
1222 platform_device_put(svc->stratix10_svc_rsu);
1223 goto err_free_kfifo;
1224 }
1225
1226 svc->intel_svc_fcs = platform_device_alloc(INTEL_FCS, 1);
1227 if (!svc->intel_svc_fcs) {
1228 dev_err(dev, "failed to allocate %s device\n", INTEL_FCS);
1229 ret = -ENOMEM;
1230 goto err_unregister_rsu_dev;
1231 }
1232
1233 ret = platform_device_add(svc->intel_svc_fcs);
1234 if (ret) {
1235 platform_device_put(svc->intel_svc_fcs);
1236 goto err_unregister_rsu_dev;
1237 }
1238
1239 ret = of_platform_default_populate(dev_of_node(dev), NULL, dev);
1240 if (ret)
1241 goto err_unregister_fcs_dev;
1242
1243 pr_info("Intel Service Layer Driver Initialized\n");
1244
1245 return 0;
1246
1247 err_unregister_fcs_dev:
1248 platform_device_unregister(svc->intel_svc_fcs);
1249 err_unregister_rsu_dev:
1250 platform_device_unregister(svc->stratix10_svc_rsu);
1251 err_free_kfifo:
1252 kfifo_free(&controller->svc_fifo);
1253 err_destroy_pool:
1254 gen_pool_destroy(genpool);
1255 return ret;
1256 }
1257
stratix10_svc_drv_remove(struct platform_device * pdev)1258 static void stratix10_svc_drv_remove(struct platform_device *pdev)
1259 {
1260 struct stratix10_svc_controller *ctrl = platform_get_drvdata(pdev);
1261 struct stratix10_svc *svc = ctrl->svc;
1262
1263 of_platform_depopulate(ctrl->dev);
1264
1265 platform_device_unregister(svc->intel_svc_fcs);
1266 platform_device_unregister(svc->stratix10_svc_rsu);
1267
1268 kfifo_free(&ctrl->svc_fifo);
1269 if (ctrl->task) {
1270 kthread_stop(ctrl->task);
1271 ctrl->task = NULL;
1272 }
1273 if (ctrl->genpool)
1274 gen_pool_destroy(ctrl->genpool);
1275 list_del(&ctrl->node);
1276 }
1277
1278 static struct platform_driver stratix10_svc_driver = {
1279 .probe = stratix10_svc_drv_probe,
1280 .remove = stratix10_svc_drv_remove,
1281 .driver = {
1282 .name = "stratix10-svc",
1283 .of_match_table = stratix10_svc_drv_match,
1284 },
1285 };
1286
stratix10_svc_init(void)1287 static int __init stratix10_svc_init(void)
1288 {
1289 struct device_node *fw_np;
1290 struct device_node *np;
1291 int ret;
1292
1293 fw_np = of_find_node_by_name(NULL, "firmware");
1294 if (!fw_np)
1295 return -ENODEV;
1296
1297 np = of_find_matching_node(fw_np, stratix10_svc_drv_match);
1298 if (!np)
1299 return -ENODEV;
1300
1301 of_node_put(np);
1302 ret = of_platform_populate(fw_np, stratix10_svc_drv_match, NULL, NULL);
1303 if (ret)
1304 return ret;
1305
1306 return platform_driver_register(&stratix10_svc_driver);
1307 }
1308
stratix10_svc_exit(void)1309 static void __exit stratix10_svc_exit(void)
1310 {
1311 return platform_driver_unregister(&stratix10_svc_driver);
1312 }
1313
1314 subsys_initcall(stratix10_svc_init);
1315 module_exit(stratix10_svc_exit);
1316
1317 MODULE_LICENSE("GPL v2");
1318 MODULE_DESCRIPTION("Intel Stratix10 Service Layer Driver");
1319 MODULE_AUTHOR("Richard Gong <richard.gong@intel.com>");
1320 MODULE_ALIAS("platform:stratix10-svc");
1321