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