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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 * 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 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 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 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 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