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