alistair23-linux/drivers/hwtracing/coresight/coresight-catu.c

// SPDX-License-Identifier: GPL-2.0
/*
 * Copyright (C) 2018 Arm Limited. All rights reserved.
 *
 * Coresight Address Translation Unit support
 *
 * Author: Suzuki K Poulose <suzuki.poulose@arm.com>
 */

#include <linux/amba/bus.h>
#include <linux/device.h>
#include <linux/dma-mapping.h>
#include <linux/io.h>
#include <linux/kernel.h>
#include <linux/slab.h>

#include "coresight-catu.h"
#include "coresight-priv.h"
#include "coresight-tmc.h"

#define csdev_to_catu_drvdata(csdev)	\
	dev_get_drvdata(csdev->dev.parent)

/* Verbose output for CATU table contents */
#ifdef CATU_DEBUG
#define catu_dbg(x, ...) dev_dbg(x, __VA_ARGS__)
#else
#define catu_dbg(x, ...) do {} while (0)
#endif

DEFINE_CORESIGHT_DEVLIST(catu_devs, "catu");

struct catu_etr_buf {
	struct tmc_sg_table *catu_table;
	dma_addr_t sladdr;
};

/*
 * CATU uses a page size of 4KB for page tables as well as data pages.
 * Each 64bit entry in the table has the following format.
 *
 *	63			12	1  0
 *	------------------------------------
 *	|	 Address [63-12] | SBZ	| V|
 *	------------------------------------
 *
 * Where bit[0] V indicates if the address is valid or not.
 * Each 4K table pages have upto 256 data page pointers, taking upto 2K
 * size. There are two Link pointers, pointing to the previous and next
 * table pages respectively at the end of the 4K page. (i.e, entry 510
 * and 511).
 *  E.g, a table of two pages could look like :
 *
 *                 Table Page 0               Table Page 1
 * SLADDR ===> x------------------x  x--> x-----------------x
 * INADDR    ->|  Page 0      | V |  |    | Page 256    | V | <- INADDR+1M
 *             |------------------|  |    |-----------------|
 * INADDR+4K ->|  Page 1      | V |  |    |                 |
 *             |------------------|  |    |-----------------|
 *             |  Page 2      | V |  |    |                 |
 *             |------------------|  |    |-----------------|
 *             |   ...        | V |  |    |    ...          |
 *             |------------------|  |    |-----------------|
 * INADDR+1020K|  Page 255    | V |  |    |   Page 511  | V |
 * SLADDR+2K==>|------------------|  |    |-----------------|
 *             |  UNUSED      |   |  |    |                 |
 *             |------------------|  |    |                 |
 *             |  UNUSED      |   |  |    |                 |
 *             |------------------|  |    |                 |
 *             |    ...       |   |  |    |                 |
 *             |------------------|  |    |-----------------|
 *             |   IGNORED    | 0 |  |    | Table Page 0| 1 |
 *             |------------------|  |    |-----------------|
 *             |  Table Page 1| 1 |--x    | IGNORED     | 0 |
 *             x------------------x       x-----------------x
 * SLADDR+4K==>
 *
 * The base input address (used by the ETR, programmed in INADDR_{LO,HI})
 * must be aligned to 1MB (the size addressable by a single page table).
 * The CATU maps INADDR{LO:HI} to the first page in the table pointed
 * to by SLADDR{LO:HI} and so on.
 *
 */
typedef u64 cate_t;

#define CATU_PAGE_SHIFT		12
#define CATU_PAGE_SIZE		(1UL << CATU_PAGE_SHIFT)
#define CATU_PAGES_PER_SYSPAGE	(PAGE_SIZE / CATU_PAGE_SIZE)

/* Page pointers are only allocated in the first 2K half */
#define CATU_PTRS_PER_PAGE	((CATU_PAGE_SIZE >> 1) / sizeof(cate_t))
#define CATU_PTRS_PER_SYSPAGE	(CATU_PAGES_PER_SYSPAGE * CATU_PTRS_PER_PAGE)
#define CATU_LINK_PREV		((CATU_PAGE_SIZE / sizeof(cate_t)) - 2)
#define CATU_LINK_NEXT		((CATU_PAGE_SIZE / sizeof(cate_t)) - 1)

#define CATU_ADDR_SHIFT		12
#define CATU_ADDR_MASK		~(((cate_t)1 << CATU_ADDR_SHIFT) - 1)
#define CATU_ENTRY_VALID	((cate_t)0x1)
#define CATU_VALID_ENTRY(addr) \
	(((cate_t)(addr) & CATU_ADDR_MASK) | CATU_ENTRY_VALID)
#define CATU_ENTRY_ADDR(entry)	((cate_t)(entry) & ~((cate_t)CATU_ENTRY_VALID))

/* CATU expects the INADDR to be aligned to 1M. */
#define CATU_DEFAULT_INADDR	(1ULL << 20)

/*
 * catu_get_table : Retrieve the table pointers for the given @offset
 * within the buffer. The buffer is wrapped around to a valid offset.
 *
 * Returns : The CPU virtual address for the beginning of the table
 * containing the data page pointer for @offset. If @daddrp is not NULL,
 * @daddrp points the DMA address of the beginning of the table.
 */
static inline cate_t *catu_get_table(struct tmc_sg_table *catu_table,
				     unsigned long offset,
				     dma_addr_t *daddrp)
{
	unsigned long buf_size = tmc_sg_table_buf_size(catu_table);
	unsigned int table_nr, pg_idx, pg_offset;
	struct tmc_pages *table_pages = &catu_table->table_pages;
	void *ptr;

	/* Make sure offset is within the range */
	offset %= buf_size;

	/*
	 * Each table can address 1MB and a single kernel page can
	 * contain "CATU_PAGES_PER_SYSPAGE" CATU tables.
	 */
	table_nr = offset >> 20;
	/* Find the table page where the table_nr lies in */
	pg_idx = table_nr / CATU_PAGES_PER_SYSPAGE;
	pg_offset = (table_nr % CATU_PAGES_PER_SYSPAGE) * CATU_PAGE_SIZE;
	if (daddrp)
		*daddrp = table_pages->daddrs[pg_idx] + pg_offset;
	ptr = page_address(table_pages->pages[pg_idx]);
	return (cate_t *)((unsigned long)ptr + pg_offset);
}

#ifdef CATU_DEBUG
static void catu_dump_table(struct tmc_sg_table *catu_table)
{
	int i;
	cate_t *table;
	unsigned long table_end, buf_size, offset = 0;

	buf_size = tmc_sg_table_buf_size(catu_table);
	dev_dbg(catu_table->dev,
		"Dump table %p, tdaddr: %llx\n",
		catu_table, catu_table->table_daddr);

	while (offset < buf_size) {
		table_end = offset + SZ_1M < buf_size ?
			    offset + SZ_1M : buf_size;
		table = catu_get_table(catu_table, offset, NULL);
		for (i = 0; offset < table_end; i++, offset += CATU_PAGE_SIZE)
			dev_dbg(catu_table->dev, "%d: %llx\n", i, table[i]);
		dev_dbg(catu_table->dev, "Prev : %llx, Next: %llx\n",
			table[CATU_LINK_PREV], table[CATU_LINK_NEXT]);
		dev_dbg(catu_table->dev, "== End of sub-table ===");
	}
	dev_dbg(catu_table->dev, "== End of Table ===");
}

#else
static inline void catu_dump_table(struct tmc_sg_table *catu_table)
{
}
#endif

static inline cate_t catu_make_entry(dma_addr_t addr)
{
	return addr ? CATU_VALID_ENTRY(addr) : 0;
}

/*
 * catu_populate_table : Populate the given CATU table.
 * The table is always populated as a circular table.
 * i.e, the "prev" link of the "first" table points to the "last"
 * table and the "next" link of the "last" table points to the
 * "first" table. The buffer should be made linear by calling
 * catu_set_table().
 */
static void
catu_populate_table(struct tmc_sg_table *catu_table)
{
	int i;
	int sys_pidx;	/* Index to current system data page */
	int catu_pidx;	/* Index of CATU page within the system data page */
	unsigned long offset, buf_size, table_end;
	dma_addr_t data_daddr;
	dma_addr_t prev_taddr, next_taddr, cur_taddr;
	cate_t *table_ptr, *next_table;

	buf_size = tmc_sg_table_buf_size(catu_table);
	sys_pidx = catu_pidx = 0;
	offset = 0;

	table_ptr = catu_get_table(catu_table, 0, &cur_taddr);
	prev_taddr = 0;	/* Prev link for the first table */

	while (offset < buf_size) {
		/*
		 * The @offset is always 1M aligned here and we have an
		 * empty table @table_ptr to fill. Each table can address
		 * upto 1MB data buffer. The last table may have fewer
		 * entries if the buffer size is not aligned.
		 */
		table_end = (offset + SZ_1M) < buf_size ?
			    (offset + SZ_1M) : buf_size;
		for (i = 0; offset < table_end;
		     i++, offset += CATU_PAGE_SIZE) {

			data_daddr = catu_table->data_pages.daddrs[sys_pidx] +
				     catu_pidx * CATU_PAGE_SIZE;
			catu_dbg(catu_table->dev,
				"[table %5ld:%03d] 0x%llx\n",
				(offset >> 20), i, data_daddr);
			table_ptr[i] = catu_make_entry(data_daddr);
			/* Move the pointers for data pages */
			catu_pidx = (catu_pidx + 1) % CATU_PAGES_PER_SYSPAGE;
			if (catu_pidx == 0)
				sys_pidx++;
		}

		/*
		 * If we have finished all the valid entries, fill the rest of
		 * the table (i.e, last table page) with invalid entries,
		 * to fail the lookups.
		 */
		if (offset == buf_size) {
			memset(&table_ptr[i], 0,
			       sizeof(cate_t) * (CATU_PTRS_PER_PAGE - i));
			next_taddr = 0;
		} else {
			next_table = catu_get_table(catu_table,
						    offset, &next_taddr);
		}

		table_ptr[CATU_LINK_PREV] = catu_make_entry(prev_taddr);
		table_ptr[CATU_LINK_NEXT] = catu_make_entry(next_taddr);

		catu_dbg(catu_table->dev,
			"[table%5ld]: Cur: 0x%llx Prev: 0x%llx, Next: 0x%llx\n",
			(offset >> 20) - 1,  cur_taddr, prev_taddr, next_taddr);

		/* Update the prev/next addresses */
		if (next_taddr) {
			prev_taddr = cur_taddr;
			cur_taddr = next_taddr;
			table_ptr = next_table;
		}
	}

	/* Sync the table for device */
	tmc_sg_table_sync_table(catu_table);
}

static struct tmc_sg_table *
catu_init_sg_table(struct device *catu_dev, int node,
		   ssize_t size, void **pages)
{
	int nr_tpages;
	struct tmc_sg_table *catu_table;

	/*
	 * Each table can address upto 1MB and we can have
	 * CATU_PAGES_PER_SYSPAGE tables in a system page.
	 */
	nr_tpages = DIV_ROUND_UP(size, SZ_1M) / CATU_PAGES_PER_SYSPAGE;
	catu_table = tmc_alloc_sg_table(catu_dev, node, nr_tpages,
					size >> PAGE_SHIFT, pages);
	if (IS_ERR(catu_table))
		return catu_table;

	catu_populate_table(catu_table);
	dev_dbg(catu_dev,
		"Setup table %p, size %ldKB, %d table pages\n",
		catu_table, (unsigned long)size >> 10,  nr_tpages);
	catu_dump_table(catu_table);
	return catu_table;
}

static void catu_free_etr_buf(struct etr_buf *etr_buf)
{
	struct catu_etr_buf *catu_buf;

	if (!etr_buf || etr_buf->mode != ETR_MODE_CATU || !etr_buf->private)
		return;

	catu_buf = etr_buf->private;
	tmc_free_sg_table(catu_buf->catu_table);
	kfree(catu_buf);
}

static ssize_t catu_get_data_etr_buf(struct etr_buf *etr_buf, u64 offset,
				     size_t len, char **bufpp)
{
	struct catu_etr_buf *catu_buf = etr_buf->private;

	return tmc_sg_table_get_data(catu_buf->catu_table, offset, len, bufpp);
}

static void catu_sync_etr_buf(struct etr_buf *etr_buf, u64 rrp, u64 rwp)
{
	struct catu_etr_buf *catu_buf = etr_buf->private;
	struct tmc_sg_table *catu_table = catu_buf->catu_table;
	u64 r_offset, w_offset;

	/*
	 * ETR started off at etr_buf->hwaddr. Convert the RRP/RWP to
	 * offsets within the trace buffer.
	 */
	r_offset = rrp - etr_buf->hwaddr;
	w_offset = rwp - etr_buf->hwaddr;

	if (!etr_buf->full) {
		etr_buf->len = w_offset - r_offset;
		if (w_offset < r_offset)
			etr_buf->len += etr_buf->size;
	} else {
		etr_buf->len = etr_buf->size;
	}

	etr_buf->offset = r_offset;
	tmc_sg_table_sync_data_range(catu_table, r_offset, etr_buf->len);
}

static int catu_alloc_etr_buf(struct tmc_drvdata *tmc_drvdata,
			      struct etr_buf *etr_buf, int node, void **pages)
{
	struct coresight_device *csdev;
	struct tmc_sg_table *catu_table;
	struct catu_etr_buf *catu_buf;

	csdev = tmc_etr_get_catu_device(tmc_drvdata);
	if (!csdev)
		return -ENODEV;
	catu_buf = kzalloc(sizeof(*catu_buf), GFP_KERNEL);
	if (!catu_buf)
		return -ENOMEM;

	catu_table = catu_init_sg_table(&csdev->dev, node,
					etr_buf->size, pages);
	if (IS_ERR(catu_table)) {
		kfree(catu_buf);
		return PTR_ERR(catu_table);
	}

	etr_buf->mode = ETR_MODE_CATU;
	etr_buf->private = catu_buf;
	etr_buf->hwaddr = CATU_DEFAULT_INADDR;

	catu_buf->catu_table = catu_table;
	/* Get the table base address */
	catu_buf->sladdr = catu_table->table_daddr;

	return 0;
}

const struct etr_buf_operations etr_catu_buf_ops = {
	.alloc = catu_alloc_etr_buf,
	.free = catu_free_etr_buf,
	.sync = catu_sync_etr_buf,
	.get_data = catu_get_data_etr_buf,
};

coresight_simple_reg32(struct catu_drvdata, devid, CORESIGHT_DEVID);
coresight_simple_reg32(struct catu_drvdata, control, CATU_CONTROL);
coresight_simple_reg32(struct catu_drvdata, status, CATU_STATUS);
coresight_simple_reg32(struct catu_drvdata, mode, CATU_MODE);
coresight_simple_reg32(struct catu_drvdata, axictrl, CATU_AXICTRL);
coresight_simple_reg32(struct catu_drvdata, irqen, CATU_IRQEN);
coresight_simple_reg64(struct catu_drvdata, sladdr,
		       CATU_SLADDRLO, CATU_SLADDRHI);
coresight_simple_reg64(struct catu_drvdata, inaddr,
		       CATU_INADDRLO, CATU_INADDRHI);

static struct attribute *catu_mgmt_attrs[] = {
	&dev_attr_devid.attr,
	&dev_attr_control.attr,
	&dev_attr_status.attr,
	&dev_attr_mode.attr,
	&dev_attr_axictrl.attr,
	&dev_attr_irqen.attr,
	&dev_attr_sladdr.attr,
	&dev_attr_inaddr.attr,
	NULL,
};

static const struct attribute_group catu_mgmt_group = {
	.attrs = catu_mgmt_attrs,
	.name = "mgmt",
};

static const struct attribute_group *catu_groups[] = {
	&catu_mgmt_group,
	NULL,
};


static inline int catu_wait_for_ready(struct catu_drvdata *drvdata)
{
	return coresight_timeout(drvdata->base,
				 CATU_STATUS, CATU_STATUS_READY, 1);
}

static int catu_enable_hw(struct catu_drvdata *drvdata, void *data)
{
	int rc;
	u32 control, mode;
	struct etr_buf *etr_buf = data;
	struct device *dev = &drvdata->csdev->dev;

	if (catu_wait_for_ready(drvdata))
		dev_warn(dev, "Timeout while waiting for READY\n");

	control = catu_read_control(drvdata);
	if (control & BIT(CATU_CONTROL_ENABLE)) {
		dev_warn(dev, "CATU is already enabled\n");
		return -EBUSY;
	}

	rc = coresight_claim_device_unlocked(drvdata->base);
	if (rc)
		return rc;

	control |= BIT(CATU_CONTROL_ENABLE);

	if (etr_buf && etr_buf->mode == ETR_MODE_CATU) {
		struct catu_etr_buf *catu_buf = etr_buf->private;

		mode = CATU_MODE_TRANSLATE;
		catu_write_axictrl(drvdata, CATU_OS_AXICTRL);
		catu_write_sladdr(drvdata, catu_buf->sladdr);
		catu_write_inaddr(drvdata, CATU_DEFAULT_INADDR);
	} else {
		mode = CATU_MODE_PASS_THROUGH;
		catu_write_sladdr(drvdata, 0);
		catu_write_inaddr(drvdata, 0);
	}

	catu_write_irqen(drvdata, 0);
	catu_write_mode(drvdata, mode);
	catu_write_control(drvdata, control);
	dev_dbg(dev, "Enabled in %s mode\n",
		(mode == CATU_MODE_PASS_THROUGH) ?
		"Pass through" :
		"Translate");
	return 0;
}

static int catu_enable(struct coresight_device *csdev, void *data)
{
	int rc;
	struct catu_drvdata *catu_drvdata = csdev_to_catu_drvdata(csdev);

	CS_UNLOCK(catu_drvdata->base);
	rc = catu_enable_hw(catu_drvdata, data);
	CS_LOCK(catu_drvdata->base);
	return rc;
}

static int catu_disable_hw(struct catu_drvdata *drvdata)
{
	int rc = 0;
	struct device *dev = &drvdata->csdev->dev;

	catu_write_control(drvdata, 0);
	coresight_disclaim_device_unlocked(drvdata->base);
	if (catu_wait_for_ready(drvdata)) {
		dev_info(dev, "Timeout while waiting for READY\n");
		rc = -EAGAIN;
	}

	dev_dbg(dev, "Disabled\n");
	return rc;
}

static int catu_disable(struct coresight_device *csdev, void *__unused)
{
	int rc;
	struct catu_drvdata *catu_drvdata = csdev_to_catu_drvdata(csdev);

	CS_UNLOCK(catu_drvdata->base);
	rc = catu_disable_hw(catu_drvdata);
	CS_LOCK(catu_drvdata->base);
	return rc;
}

static const struct coresight_ops_helper catu_helper_ops = {
	.enable = catu_enable,
	.disable = catu_disable,
};

static const struct coresight_ops catu_ops = {
	.helper_ops = &catu_helper_ops,
};

static int catu_probe(struct amba_device *adev, const struct amba_id *id)
{
	int ret = 0;
	u32 dma_mask;
	struct catu_drvdata *drvdata;
	struct coresight_desc catu_desc;
	struct coresight_platform_data *pdata = NULL;
	struct device *dev = &adev->dev;
	void __iomem *base;

	catu_desc.name = coresight_alloc_device_name(&catu_devs, dev);
	if (!catu_desc.name)
		return -ENOMEM;

	drvdata = devm_kzalloc(dev, sizeof(*drvdata), GFP_KERNEL);
	if (!drvdata) {
		ret = -ENOMEM;
		goto out;
	}

	dev_set_drvdata(dev, drvdata);
	base = devm_ioremap_resource(dev, &adev->res);
	if (IS_ERR(base)) {
		ret = PTR_ERR(base);
		goto out;
	}

	/* Setup dma mask for the device */
	dma_mask = readl_relaxed(base + CORESIGHT_DEVID) & 0x3f;
	switch (dma_mask) {
	case 32:
	case 40:
	case 44:
	case 48:
	case 52:
	case 56:
	case 64:
		break;
	default:
		/* Default to the 40bits as supported by TMC-ETR */
		dma_mask = 40;
	}
	ret = dma_set_mask_and_coherent(dev, DMA_BIT_MASK(dma_mask));
	if (ret)
		goto out;

	pdata = coresight_get_platform_data(dev);
	if (IS_ERR(pdata)) {
		ret = PTR_ERR(pdata);
		goto out;
	}
	dev->platform_data = pdata;

	drvdata->base = base;
	catu_desc.pdata = pdata;
	catu_desc.dev = dev;
	catu_desc.groups = catu_groups;
	catu_desc.type = CORESIGHT_DEV_TYPE_HELPER;
	catu_desc.subtype.helper_subtype = CORESIGHT_DEV_SUBTYPE_HELPER_CATU;
	catu_desc.ops = &catu_ops;

	drvdata->csdev = coresight_register(&catu_desc);
	if (IS_ERR(drvdata->csdev))
		ret = PTR_ERR(drvdata->csdev);
	else
		pm_runtime_put(&adev->dev);
out:
	return ret;
}

static struct amba_id catu_ids[] = {
	{
		.id	= 0x000bb9ee,
		.mask	= 0x000fffff,
	},
	{},
};

static struct amba_driver catu_driver = {
	.drv = {
		.name			= "coresight-catu",
		.owner			= THIS_MODULE,
		.suppress_bind_attrs	= true,
	},
	.probe				= catu_probe,
	.id_table			= catu_ids,
};

builtin_amba_driver(catu_driver);