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