main.c

// SPDX-License-Identifier: GPL-2.0
/* Copyright(c) Advanced Micro Devices, Inc */

#include <linux/module.h>
#include <linux/auxiliary_bus.h>
#include <linux/pci.h>
#include <linux/vmalloc.h>
#include <linux/bitfield.h>

#include <uapi/fwctl/fwctl.h>
#include <uapi/fwctl/pds.h>
#include <linux/fwctl.h>

#include <linux/pds/pds_common.h>
#include <linux/pds/pds_core_if.h>
#include <linux/pds/pds_adminq.h>
#include <linux/pds/pds_auxbus.h>

struct pdsfc_uctx {
	struct fwctl_uctx uctx;
	u32 uctx_caps;
};

struct pdsfc_rpc_endpoint_info {
	u32 endpoint;
	dma_addr_t operations_pa;
	struct pds_fwctl_query_data *operations;
	struct mutex lock;	/* lock for endpoint info management */
};

struct pdsfc_dev {
	struct fwctl_device fwctl;
	struct pds_auxiliary_dev *padev;
	u32 caps;
	struct pds_fwctl_ident ident;
	dma_addr_t endpoints_pa;
	struct pds_fwctl_query_data *endpoints;
	struct pdsfc_rpc_endpoint_info *endpoint_info;
};

static int pdsfc_open_uctx(struct fwctl_uctx *uctx)
{
	struct pdsfc_dev *pdsfc = container_of(uctx->fwctl, struct pdsfc_dev, fwctl);
	struct pdsfc_uctx *pdsfc_uctx = container_of(uctx, struct pdsfc_uctx, uctx);

	pdsfc_uctx->uctx_caps = pdsfc->caps;

	return 0;
}

static void pdsfc_close_uctx(struct fwctl_uctx *uctx)
{
}

static void *pdsfc_info(struct fwctl_uctx *uctx, size_t *length)
{
	struct pdsfc_uctx *pdsfc_uctx = container_of(uctx, struct pdsfc_uctx, uctx);
	struct fwctl_info_pds *info;

	info = kzalloc(sizeof(*info), GFP_KERNEL);
	if (!info)
		return ERR_PTR(-ENOMEM);

	info->uctx_caps = pdsfc_uctx->uctx_caps;

	return info;
}

static int pdsfc_identify(struct pdsfc_dev *pdsfc)
{
	struct device *dev = &pdsfc->fwctl.dev;
	union pds_core_adminq_comp comp = {0};
	union pds_core_adminq_cmd cmd;
	struct pds_fwctl_ident *ident;
	dma_addr_t ident_pa;
	int err;

	ident = dma_alloc_coherent(dev->parent, sizeof(*ident), &ident_pa, GFP_KERNEL);
	if (!ident) {
		dev_err(dev, "Failed to map ident buffer\n");
		return -ENOMEM;
	}

	cmd = (union pds_core_adminq_cmd) {
		.fwctl_ident = {
			.opcode = PDS_FWCTL_CMD_IDENT,
			.version = 0,
			.len = cpu_to_le32(sizeof(*ident)),
			.ident_pa = cpu_to_le64(ident_pa),
		}
	};

	err = pds_client_adminq_cmd(pdsfc->padev, &cmd, sizeof(cmd), &comp, 0);
	if (err)
		dev_err(dev, "Failed to send adminq cmd opcode: %u err: %d\n",
			cmd.fwctl_ident.opcode, err);
	else
		pdsfc->ident = *ident;

	dma_free_coherent(dev->parent, sizeof(*ident), ident, ident_pa);

	return err;
}

static void pdsfc_free_endpoints(struct pdsfc_dev *pdsfc)
{
	struct device *dev = &pdsfc->fwctl.dev;
	u32 num_endpoints;
	int i;

	if (!pdsfc->endpoints)
		return;

	num_endpoints = le32_to_cpu(pdsfc->endpoints->num_entries);
	for (i = 0; pdsfc->endpoint_info && i < num_endpoints; i++)
		mutex_destroy(&pdsfc->endpoint_info[i].lock);
	vfree(pdsfc->endpoint_info);
	pdsfc->endpoint_info = NULL;
	dma_free_coherent(dev->parent, PAGE_SIZE,
			  pdsfc->endpoints, pdsfc->endpoints_pa);
	pdsfc->endpoints = NULL;
	pdsfc->endpoints_pa = DMA_MAPPING_ERROR;
}

static void pdsfc_free_operations(struct pdsfc_dev *pdsfc)
{
	struct device *dev = &pdsfc->fwctl.dev;
	u32 num_endpoints;
	int i;

	num_endpoints = le32_to_cpu(pdsfc->endpoints->num_entries);
	for (i = 0; i < num_endpoints; i++) {
		struct pdsfc_rpc_endpoint_info *ei = &pdsfc->endpoint_info[i];

		if (ei->operations) {
			dma_free_coherent(dev->parent, PAGE_SIZE,
					  ei->operations, ei->operations_pa);
			ei->operations = NULL;
			ei->operations_pa = DMA_MAPPING_ERROR;
		}
	}
}

static struct pds_fwctl_query_data *pdsfc_get_endpoints(struct pdsfc_dev *pdsfc,
							dma_addr_t *pa)
{
	struct device *dev = &pdsfc->fwctl.dev;
	union pds_core_adminq_comp comp = {0};
	struct pds_fwctl_query_data *data;
	union pds_core_adminq_cmd cmd;
	dma_addr_t data_pa;
	int err;

	data = dma_alloc_coherent(dev->parent, PAGE_SIZE, &data_pa, GFP_KERNEL);
	if (!data) {
		dev_err(dev, "Failed to map endpoint list\n");
		return ERR_PTR(-ENOMEM);
	}

	cmd = (union pds_core_adminq_cmd) {
		.fwctl_query = {
			.opcode = PDS_FWCTL_CMD_QUERY,
			.entity = PDS_FWCTL_RPC_ROOT,
			.version = 0,
			.query_data_buf_len = cpu_to_le32(PAGE_SIZE),
			.query_data_buf_pa = cpu_to_le64(data_pa),
		}
	};

	err = pds_client_adminq_cmd(pdsfc->padev, &cmd, sizeof(cmd), &comp, 0);
	if (err) {
		dev_err(dev, "Failed to send adminq cmd opcode: %u entity: %u err: %d\n",
			cmd.fwctl_query.opcode, cmd.fwctl_query.entity, err);
		dma_free_coherent(dev->parent, PAGE_SIZE, data, data_pa);
		return ERR_PTR(err);
	}

	*pa = data_pa;

	return data;
}

static int pdsfc_init_endpoints(struct pdsfc_dev *pdsfc)
{
	struct pds_fwctl_query_data_endpoint *ep_entry;
	u32 num_endpoints;
	int i;

	pdsfc->endpoints = pdsfc_get_endpoints(pdsfc, &pdsfc->endpoints_pa);
	if (IS_ERR(pdsfc->endpoints))
		return PTR_ERR(pdsfc->endpoints);

	num_endpoints = le32_to_cpu(pdsfc->endpoints->num_entries);
	pdsfc->endpoint_info = vcalloc(num_endpoints,
				       sizeof(*pdsfc->endpoint_info));
	if (!pdsfc->endpoint_info) {
		pdsfc_free_endpoints(pdsfc);
		return -ENOMEM;
	}

	ep_entry = (struct pds_fwctl_query_data_endpoint *)pdsfc->endpoints->entries;
	for (i = 0; i < num_endpoints; i++) {
		mutex_init(&pdsfc->endpoint_info[i].lock);
		pdsfc->endpoint_info[i].endpoint = le32_to_cpu(ep_entry[i].id);
	}

	return 0;
}

static struct pds_fwctl_query_data *pdsfc_get_operations(struct pdsfc_dev *pdsfc,
							 dma_addr_t *pa, u32 ep)
{
	struct pds_fwctl_query_data_operation *entries;
	struct device *dev = &pdsfc->fwctl.dev;
	union pds_core_adminq_comp comp = {0};
	struct pds_fwctl_query_data *data;
	union pds_core_adminq_cmd cmd;
	dma_addr_t data_pa;
	u32 num_entries;
	int err;
	int i;

	/* Query the operations list for the given endpoint */
	data = dma_alloc_coherent(dev->parent, PAGE_SIZE, &data_pa, GFP_KERNEL);
	if (!data) {
		dev_err(dev, "Failed to map operations list\n");
		return ERR_PTR(-ENOMEM);
	}

	cmd = (union pds_core_adminq_cmd) {
		.fwctl_query = {
			.opcode = PDS_FWCTL_CMD_QUERY,
			.entity = PDS_FWCTL_RPC_ENDPOINT,
			.version = 0,
			.query_data_buf_len = cpu_to_le32(PAGE_SIZE),
			.query_data_buf_pa = cpu_to_le64(data_pa),
			.ep = cpu_to_le32(ep),
		}
	};

	err = pds_client_adminq_cmd(pdsfc->padev, &cmd, sizeof(cmd), &comp, 0);
	if (err) {
		dev_err(dev, "Failed to send adminq cmd opcode: %u entity: %u err: %d\n",
			cmd.fwctl_query.opcode, cmd.fwctl_query.entity, err);
		dma_free_coherent(dev->parent, PAGE_SIZE, data, data_pa);
		return ERR_PTR(err);
	}

	*pa = data_pa;

	entries = (struct pds_fwctl_query_data_operation *)data->entries;
	num_entries = le32_to_cpu(data->num_entries);
	dev_dbg(dev, "num_entries %d\n", num_entries);
	for (i = 0; i < num_entries; i++) {

		/* Translate FW command attribute to fwctl scope */
		switch (entries[i].scope) {
		case PDSFC_FW_CMD_ATTR_READ:
		case PDSFC_FW_CMD_ATTR_WRITE:
		case PDSFC_FW_CMD_ATTR_SYNC:
			entries[i].scope = FWCTL_RPC_CONFIGURATION;
			break;
		case PDSFC_FW_CMD_ATTR_DEBUG_READ:
			entries[i].scope = FWCTL_RPC_DEBUG_READ_ONLY;
			break;
		case PDSFC_FW_CMD_ATTR_DEBUG_WRITE:
			entries[i].scope = FWCTL_RPC_DEBUG_WRITE;
			break;
		default:
			entries[i].scope = FWCTL_RPC_DEBUG_WRITE_FULL;
			break;
		}
		dev_dbg(dev, "endpoint %d operation: id %x scope %d\n",
			ep, le32_to_cpu(entries[i].id), entries[i].scope);
	}

	return data;
}

static int pdsfc_validate_rpc(struct pdsfc_dev *pdsfc,
			      struct fwctl_rpc_pds *rpc,
			      enum fwctl_rpc_scope scope)
{
	struct pds_fwctl_query_data_operation *op_entry;
	struct pdsfc_rpc_endpoint_info *ep_info = NULL;
	struct device *dev = &pdsfc->fwctl.dev;
	u32 num_entries;
	int i;

	/* validate rpc in_len & out_len based
	 * on ident.max_req_sz & max_resp_sz
	 */
	if (rpc->in.len > le32_to_cpu(pdsfc->ident.max_req_sz)) {
		dev_dbg(dev, "Invalid request size %u, max %u\n",
			rpc->in.len, le32_to_cpu(pdsfc->ident.max_req_sz));
		return -EINVAL;
	}

	if (rpc->out.len > le32_to_cpu(pdsfc->ident.max_resp_sz)) {
		dev_dbg(dev, "Invalid response size %u, max %u\n",
			rpc->out.len, le32_to_cpu(pdsfc->ident.max_resp_sz));
		return -EINVAL;
	}

	num_entries = le32_to_cpu(pdsfc->endpoints->num_entries);
	for (i = 0; i < num_entries; i++) {
		if (pdsfc->endpoint_info[i].endpoint == rpc->in.ep) {
			ep_info = &pdsfc->endpoint_info[i];
			break;
		}
	}
	if (!ep_info) {
		dev_dbg(dev, "Invalid endpoint %d\n", rpc->in.ep);
		return -EINVAL;
	}

	/* query and cache this endpoint's operations */
	mutex_lock(&ep_info->lock);
	if (!ep_info->operations) {
		struct pds_fwctl_query_data *operations;

		operations = pdsfc_get_operations(pdsfc,
						  &ep_info->operations_pa,
						  rpc->in.ep);
		if (IS_ERR(operations)) {
			mutex_unlock(&ep_info->lock);
			return -ENOMEM;
		}
		ep_info->operations = operations;
	}
	mutex_unlock(&ep_info->lock);

	/* reject unsupported and/or out of scope commands */
	op_entry = (struct pds_fwctl_query_data_operation *)ep_info->operations->entries;
	num_entries = le32_to_cpu(ep_info->operations->num_entries);
	for (i = 0; i < num_entries; i++) {
		if (PDS_FWCTL_RPC_OPCODE_CMP(rpc->in.op, le32_to_cpu(op_entry[i].id))) {
			if (scope < op_entry[i].scope)
				return -EPERM;
			return 0;
		}
	}

	dev_dbg(dev, "Invalid operation %d for endpoint %d\n", rpc->in.op, rpc->in.ep);

	return -EINVAL;
}

static void *pdsfc_fw_rpc(struct fwctl_uctx *uctx, enum fwctl_rpc_scope scope,
			  void *in, size_t in_len, size_t *out_len)
{
	struct pdsfc_dev *pdsfc = container_of(uctx->fwctl, struct pdsfc_dev, fwctl);
	struct device *dev = &uctx->fwctl->dev;
	union pds_core_adminq_comp comp = {0};
	dma_addr_t out_payload_dma_addr = 0;
	dma_addr_t in_payload_dma_addr = 0;
	struct fwctl_rpc_pds *rpc = in;
	union pds_core_adminq_cmd cmd;
	void *out_payload = NULL;
	void *in_payload = NULL;
	void *out = NULL;
	int err;

	err = pdsfc_validate_rpc(pdsfc, rpc, scope);
	if (err)
		return ERR_PTR(err);

	if (rpc->in.len > 0) {
		in_payload = kzalloc(rpc->in.len, GFP_KERNEL);
		if (!in_payload) {
			dev_err(dev, "Failed to allocate in_payload\n");
			err = -ENOMEM;
			goto err_out;
		}

		if (copy_from_user(in_payload, u64_to_user_ptr(rpc->in.payload),
				   rpc->in.len)) {
			dev_dbg(dev, "Failed to copy in_payload from user\n");
			err = -EFAULT;
			goto err_in_payload;
		}

		in_payload_dma_addr = dma_map_single(dev->parent, in_payload,
						     rpc->in.len, DMA_TO_DEVICE);
		err = dma_mapping_error(dev->parent, in_payload_dma_addr);
		if (err) {
			dev_dbg(dev, "Failed to map in_payload\n");
			goto err_in_payload;
		}
	}

	if (rpc->out.len > 0) {
		out_payload = kzalloc(rpc->out.len, GFP_KERNEL);
		if (!out_payload) {
			dev_dbg(dev, "Failed to allocate out_payload\n");
			err = -ENOMEM;
			goto err_out_payload;
		}

		out_payload_dma_addr = dma_map_single(dev->parent, out_payload,
						      rpc->out.len, DMA_FROM_DEVICE);
		err = dma_mapping_error(dev->parent, out_payload_dma_addr);
		if (err) {
			dev_dbg(dev, "Failed to map out_payload\n");
			goto err_out_payload;
		}
	}

	cmd = (union pds_core_adminq_cmd) {
		.fwctl_rpc = {
			.opcode = PDS_FWCTL_CMD_RPC,
			.flags = cpu_to_le16(PDS_FWCTL_RPC_IND_REQ | PDS_FWCTL_RPC_IND_RESP),
			.ep = cpu_to_le32(rpc->in.ep),
			.op = cpu_to_le32(rpc->in.op),
			.req_pa = cpu_to_le64(in_payload_dma_addr),
			.req_sz = cpu_to_le32(rpc->in.len),
			.resp_pa = cpu_to_le64(out_payload_dma_addr),
			.resp_sz = cpu_to_le32(rpc->out.len),
		}
	};

	err = pds_client_adminq_cmd(pdsfc->padev, &cmd, sizeof(cmd), &comp, 0);
	if (err) {
		dev_dbg(dev, "%s: ep %d op %x req_pa %llx req_sz %d req_sg %d resp_pa %llx resp_sz %d resp_sg %d err %d\n",
			__func__, rpc->in.ep, rpc->in.op,
			cmd.fwctl_rpc.req_pa, cmd.fwctl_rpc.req_sz, cmd.fwctl_rpc.req_sg_elems,
			cmd.fwctl_rpc.resp_pa, cmd.fwctl_rpc.resp_sz, cmd.fwctl_rpc.resp_sg_elems,
			err);
		goto done;
	}

	dynamic_hex_dump("out ", DUMP_PREFIX_OFFSET, 16, 1, out_payload, rpc->out.len, true);

	if (copy_to_user(u64_to_user_ptr(rpc->out.payload), out_payload, rpc->out.len)) {
		dev_dbg(dev, "Failed to copy out_payload to user\n");
		out = ERR_PTR(-EFAULT);
		goto done;
	}

	rpc->out.retval = le32_to_cpu(comp.fwctl_rpc.err);
	*out_len = in_len;
	out = in;

done:
	if (out_payload_dma_addr)
		dma_unmap_single(dev->parent, out_payload_dma_addr,
				 rpc->out.len, DMA_FROM_DEVICE);
err_out_payload:
	kfree(out_payload);

	if (in_payload_dma_addr)
		dma_unmap_single(dev->parent, in_payload_dma_addr,
				 rpc->in.len, DMA_TO_DEVICE);
err_in_payload:
	kfree(in_payload);
err_out:
	if (err)
		return ERR_PTR(err);

	return out;
}

static const struct fwctl_ops pdsfc_ops = {
	.device_type = FWCTL_DEVICE_TYPE_PDS,
	.uctx_size = sizeof(struct pdsfc_uctx),
	.open_uctx = pdsfc_open_uctx,
	.close_uctx = pdsfc_close_uctx,
	.info = pdsfc_info,
	.fw_rpc = pdsfc_fw_rpc,
};

static int pdsfc_probe(struct auxiliary_device *adev,
		       const struct auxiliary_device_id *id)
{
	struct pds_auxiliary_dev *padev =
			container_of(adev, struct pds_auxiliary_dev, aux_dev);
	struct device *dev = &adev->dev;
	struct pdsfc_dev *pdsfc;
	int err;

	pdsfc = fwctl_alloc_device(&padev->vf_pdev->dev, &pdsfc_ops,
				   struct pdsfc_dev, fwctl);
	if (!pdsfc)
		return dev_err_probe(dev, -ENOMEM, "Failed to allocate fwctl device struct\n");
	pdsfc->padev = padev;

	err = pdsfc_identify(pdsfc);
	if (err) {
		fwctl_put(&pdsfc->fwctl);
		return dev_err_probe(dev, err, "Failed to identify device\n");
	}

	err = pdsfc_init_endpoints(pdsfc);
	if (err) {
		fwctl_put(&pdsfc->fwctl);
		return dev_err_probe(dev, err, "Failed to init endpoints\n");
	}

	pdsfc->caps = PDS_FWCTL_QUERY_CAP | PDS_FWCTL_SEND_CAP;

	err = fwctl_register(&pdsfc->fwctl);
	if (err) {
		pdsfc_free_endpoints(pdsfc);
		fwctl_put(&pdsfc->fwctl);
		return dev_err_probe(dev, err, "Failed to register device\n");
	}

	auxiliary_set_drvdata(adev, pdsfc);

	return 0;
}

static void pdsfc_remove(struct auxiliary_device *adev)
{
	struct pdsfc_dev *pdsfc = auxiliary_get_drvdata(adev);

	fwctl_unregister(&pdsfc->fwctl);
	pdsfc_free_operations(pdsfc);
	pdsfc_free_endpoints(pdsfc);

	fwctl_put(&pdsfc->fwctl);
}

static const struct auxiliary_device_id pdsfc_id_table[] = {
	{.name = PDS_CORE_DRV_NAME "." PDS_DEV_TYPE_FWCTL_STR },
	{}
};
MODULE_DEVICE_TABLE(auxiliary, pdsfc_id_table);

static struct auxiliary_driver pdsfc_driver = {
	.name = "pds_fwctl",
	.probe = pdsfc_probe,
	.remove = pdsfc_remove,
	.id_table = pdsfc_id_table,
};

module_auxiliary_driver(pdsfc_driver);

MODULE_IMPORT_NS("FWCTL");
MODULE_DESCRIPTION("pds fwctl driver");
MODULE_AUTHOR("Shannon Nelson <[email protected]>");
MODULE_AUTHOR("Brett Creeley <[email protected]>");
MODULE_LICENSE("GPL");