cachepc-linux

Fork of AMDESE/linux with modifications for CachePC side-channel attack
git clone https://git.sinitax.com/sinitax/cachepc-linux
Log | Files | Refs | README | LICENSE | sfeed.txt

stratix10-svc.c (32099B)


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