drivers/firmware/stratix10-svc.c

   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  */
 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  */
 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  */
 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  */
 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  */
 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  */
 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  */
 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 *
 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  */
 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  */
 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  */
 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  */
 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  */
 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  */
 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  */
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  */
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  */
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
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
1254 static void 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
1272 static struct platform_driver stratix10_svc_driver = {
1273         .probe = stratix10_svc_drv_probe,
1274         .remove_new = stratix10_svc_drv_remove,
1275         .driver = {
1276                 .name = "stratix10-svc",
1277                 .of_match_table = stratix10_svc_drv_match,
1278         },
1279 };
1280
1281 static int __init stratix10_svc_init(void)
1282 {
1283         struct device_node *fw_np;
1284         struct device_node *np;
1285         int ret;
1286
1287         fw_np = of_find_node_by_name(NULL, "firmware");
1288         if (!fw_np)
1289                 return -ENODEV;
1290
1291         np = of_find_matching_node(fw_np, stratix10_svc_drv_match);
1292         if (!np)
1293                 return -ENODEV;
1294
1295         of_node_put(np);
1296         ret = of_platform_populate(fw_np, stratix10_svc_drv_match, NULL, NULL);
1297         if (ret)
1298                 return ret;
1299
1300         return platform_driver_register(&stratix10_svc_driver);
1301 }
1302
1303 static void __exit stratix10_svc_exit(void)
1304 {
1305         return platform_driver_unregister(&stratix10_svc_driver);
1306 }
1307
1308 subsys_initcall(stratix10_svc_init);
1309 module_exit(stratix10_svc_exit);
1310
1311 MODULE_LICENSE("GPL v2");
1312 MODULE_DESCRIPTION("Intel Stratix10 Service Layer Driver");
1313 MODULE_AUTHOR("Richard Gong <richard.gong@intel.com>");
1314 MODULE_ALIAS("platform:stratix10-svc");