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