1 // SPDX-License-Identifier: GPL-2.0
2 /*
3  * Copyright (C) 2018 Arm Limited. All rights reserved.
4  *
5  * Coresight Address Translation Unit support
6  *
7  * Author: Suzuki K Poulose <suzuki.poulose@arm.com>
8  */
9 
10 #include <linux/amba/bus.h>
11 #include <linux/device.h>
12 #include <linux/dma-mapping.h>
13 #include <linux/io.h>
14 #include <linux/kernel.h>
15 #include <linux/slab.h>
16 
17 #include "coresight-catu.h"
18 #include "coresight-priv.h"
19 #include "coresight-tmc.h"
20 
21 #define csdev_to_catu_drvdata(csdev)	\
22 	dev_get_drvdata(csdev->dev.parent)
23 
24 /* Verbose output for CATU table contents */
25 #ifdef CATU_DEBUG
26 #define catu_dbg(x, ...) dev_dbg(x, __VA_ARGS__)
27 #else
28 #define catu_dbg(x, ...) do {} while (0)
29 #endif
30 
31 struct catu_etr_buf {
32 	struct tmc_sg_table *catu_table;
33 	dma_addr_t sladdr;
34 };
35 
36 /*
37  * CATU uses a page size of 4KB for page tables as well as data pages.
38  * Each 64bit entry in the table has the following format.
39  *
40  *	63			12	1  0
41  *	------------------------------------
42  *	|	 Address [63-12] | SBZ	| V|
43  *	------------------------------------
44  *
45  * Where bit[0] V indicates if the address is valid or not.
46  * Each 4K table pages have upto 256 data page pointers, taking upto 2K
47  * size. There are two Link pointers, pointing to the previous and next
48  * table pages respectively at the end of the 4K page. (i.e, entry 510
49  * and 511).
50  *  E.g, a table of two pages could look like :
51  *
52  *                 Table Page 0               Table Page 1
53  * SLADDR ===> x------------------x  x--> x-----------------x
54  * INADDR    ->|  Page 0      | V |  |    | Page 256    | V | <- INADDR+1M
55  *             |------------------|  |    |-----------------|
56  * INADDR+4K ->|  Page 1      | V |  |    |                 |
57  *             |------------------|  |    |-----------------|
58  *             |  Page 2      | V |  |    |                 |
59  *             |------------------|  |    |-----------------|
60  *             |   ...        | V |  |    |    ...          |
61  *             |------------------|  |    |-----------------|
62  * INADDR+1020K|  Page 255    | V |  |    |   Page 511  | V |
63  * SLADDR+2K==>|------------------|  |    |-----------------|
64  *             |  UNUSED      |   |  |    |                 |
65  *             |------------------|  |    |                 |
66  *             |  UNUSED      |   |  |    |                 |
67  *             |------------------|  |    |                 |
68  *             |    ...       |   |  |    |                 |
69  *             |------------------|  |    |-----------------|
70  *             |   IGNORED    | 0 |  |    | Table Page 0| 1 |
71  *             |------------------|  |    |-----------------|
72  *             |  Table Page 1| 1 |--x    | IGNORED     | 0 |
73  *             x------------------x       x-----------------x
74  * SLADDR+4K==>
75  *
76  * The base input address (used by the ETR, programmed in INADDR_{LO,HI})
77  * must be aligned to 1MB (the size addressable by a single page table).
78  * The CATU maps INADDR{LO:HI} to the first page in the table pointed
79  * to by SLADDR{LO:HI} and so on.
80  *
81  */
82 typedef u64 cate_t;
83 
84 #define CATU_PAGE_SHIFT		12
85 #define CATU_PAGE_SIZE		(1UL << CATU_PAGE_SHIFT)
86 #define CATU_PAGES_PER_SYSPAGE	(PAGE_SIZE / CATU_PAGE_SIZE)
87 
88 /* Page pointers are only allocated in the first 2K half */
89 #define CATU_PTRS_PER_PAGE	((CATU_PAGE_SIZE >> 1) / sizeof(cate_t))
90 #define CATU_PTRS_PER_SYSPAGE	(CATU_PAGES_PER_SYSPAGE * CATU_PTRS_PER_PAGE)
91 #define CATU_LINK_PREV		((CATU_PAGE_SIZE / sizeof(cate_t)) - 2)
92 #define CATU_LINK_NEXT		((CATU_PAGE_SIZE / sizeof(cate_t)) - 1)
93 
94 #define CATU_ADDR_SHIFT		12
95 #define CATU_ADDR_MASK		~(((cate_t)1 << CATU_ADDR_SHIFT) - 1)
96 #define CATU_ENTRY_VALID	((cate_t)0x1)
97 #define CATU_VALID_ENTRY(addr) \
98 	(((cate_t)(addr) & CATU_ADDR_MASK) | CATU_ENTRY_VALID)
99 #define CATU_ENTRY_ADDR(entry)	((cate_t)(entry) & ~((cate_t)CATU_ENTRY_VALID))
100 
101 /* CATU expects the INADDR to be aligned to 1M. */
102 #define CATU_DEFAULT_INADDR	(1ULL << 20)
103 
104 /*
105  * catu_get_table : Retrieve the table pointers for the given @offset
106  * within the buffer. The buffer is wrapped around to a valid offset.
107  *
108  * Returns : The CPU virtual address for the beginning of the table
109  * containing the data page pointer for @offset. If @daddrp is not NULL,
110  * @daddrp points the DMA address of the beginning of the table.
111  */
112 static inline cate_t *catu_get_table(struct tmc_sg_table *catu_table,
113 				     unsigned long offset,
114 				     dma_addr_t *daddrp)
115 {
116 	unsigned long buf_size = tmc_sg_table_buf_size(catu_table);
117 	unsigned int table_nr, pg_idx, pg_offset;
118 	struct tmc_pages *table_pages = &catu_table->table_pages;
119 	void *ptr;
120 
121 	/* Make sure offset is within the range */
122 	offset %= buf_size;
123 
124 	/*
125 	 * Each table can address 1MB and a single kernel page can
126 	 * contain "CATU_PAGES_PER_SYSPAGE" CATU tables.
127 	 */
128 	table_nr = offset >> 20;
129 	/* Find the table page where the table_nr lies in */
130 	pg_idx = table_nr / CATU_PAGES_PER_SYSPAGE;
131 	pg_offset = (table_nr % CATU_PAGES_PER_SYSPAGE) * CATU_PAGE_SIZE;
132 	if (daddrp)
133 		*daddrp = table_pages->daddrs[pg_idx] + pg_offset;
134 	ptr = page_address(table_pages->pages[pg_idx]);
135 	return (cate_t *)((unsigned long)ptr + pg_offset);
136 }
137 
138 #ifdef CATU_DEBUG
139 static void catu_dump_table(struct tmc_sg_table *catu_table)
140 {
141 	int i;
142 	cate_t *table;
143 	unsigned long table_end, buf_size, offset = 0;
144 
145 	buf_size = tmc_sg_table_buf_size(catu_table);
146 	dev_dbg(catu_table->dev,
147 		"Dump table %p, tdaddr: %llx\n",
148 		catu_table, catu_table->table_daddr);
149 
150 	while (offset < buf_size) {
151 		table_end = offset + SZ_1M < buf_size ?
152 			    offset + SZ_1M : buf_size;
153 		table = catu_get_table(catu_table, offset, NULL);
154 		for (i = 0; offset < table_end; i++, offset += CATU_PAGE_SIZE)
155 			dev_dbg(catu_table->dev, "%d: %llx\n", i, table[i]);
156 		dev_dbg(catu_table->dev, "Prev : %llx, Next: %llx\n",
157 			table[CATU_LINK_PREV], table[CATU_LINK_NEXT]);
158 		dev_dbg(catu_table->dev, "== End of sub-table ===");
159 	}
160 	dev_dbg(catu_table->dev, "== End of Table ===");
161 }
162 
163 #else
164 static inline void catu_dump_table(struct tmc_sg_table *catu_table)
165 {
166 }
167 #endif
168 
169 static inline cate_t catu_make_entry(dma_addr_t addr)
170 {
171 	return addr ? CATU_VALID_ENTRY(addr) : 0;
172 }
173 
174 /*
175  * catu_populate_table : Populate the given CATU table.
176  * The table is always populated as a circular table.
177  * i.e, the "prev" link of the "first" table points to the "last"
178  * table and the "next" link of the "last" table points to the
179  * "first" table. The buffer should be made linear by calling
180  * catu_set_table().
181  */
182 static void
183 catu_populate_table(struct tmc_sg_table *catu_table)
184 {
185 	int i;
186 	int sys_pidx;	/* Index to current system data page */
187 	int catu_pidx;	/* Index of CATU page within the system data page */
188 	unsigned long offset, buf_size, table_end;
189 	dma_addr_t data_daddr;
190 	dma_addr_t prev_taddr, next_taddr, cur_taddr;
191 	cate_t *table_ptr, *next_table;
192 
193 	buf_size = tmc_sg_table_buf_size(catu_table);
194 	sys_pidx = catu_pidx = 0;
195 	offset = 0;
196 
197 	table_ptr = catu_get_table(catu_table, 0, &cur_taddr);
198 	prev_taddr = 0;	/* Prev link for the first table */
199 
200 	while (offset < buf_size) {
201 		/*
202 		 * The @offset is always 1M aligned here and we have an
203 		 * empty table @table_ptr to fill. Each table can address
204 		 * upto 1MB data buffer. The last table may have fewer
205 		 * entries if the buffer size is not aligned.
206 		 */
207 		table_end = (offset + SZ_1M) < buf_size ?
208 			    (offset + SZ_1M) : buf_size;
209 		for (i = 0; offset < table_end;
210 		     i++, offset += CATU_PAGE_SIZE) {
211 
212 			data_daddr = catu_table->data_pages.daddrs[sys_pidx] +
213 				     catu_pidx * CATU_PAGE_SIZE;
214 			catu_dbg(catu_table->dev,
215 				"[table %5ld:%03d] 0x%llx\n",
216 				(offset >> 20), i, data_daddr);
217 			table_ptr[i] = catu_make_entry(data_daddr);
218 			/* Move the pointers for data pages */
219 			catu_pidx = (catu_pidx + 1) % CATU_PAGES_PER_SYSPAGE;
220 			if (catu_pidx == 0)
221 				sys_pidx++;
222 		}
223 
224 		/*
225 		 * If we have finished all the valid entries, fill the rest of
226 		 * the table (i.e, last table page) with invalid entries,
227 		 * to fail the lookups.
228 		 */
229 		if (offset == buf_size) {
230 			memset(&table_ptr[i], 0,
231 			       sizeof(cate_t) * (CATU_PTRS_PER_PAGE - i));
232 			next_taddr = 0;
233 		} else {
234 			next_table = catu_get_table(catu_table,
235 						    offset, &next_taddr);
236 		}
237 
238 		table_ptr[CATU_LINK_PREV] = catu_make_entry(prev_taddr);
239 		table_ptr[CATU_LINK_NEXT] = catu_make_entry(next_taddr);
240 
241 		catu_dbg(catu_table->dev,
242 			"[table%5ld]: Cur: 0x%llx Prev: 0x%llx, Next: 0x%llx\n",
243 			(offset >> 20) - 1,  cur_taddr, prev_taddr, next_taddr);
244 
245 		/* Update the prev/next addresses */
246 		if (next_taddr) {
247 			prev_taddr = cur_taddr;
248 			cur_taddr = next_taddr;
249 			table_ptr = next_table;
250 		}
251 	}
252 
253 	/* Sync the table for device */
254 	tmc_sg_table_sync_table(catu_table);
255 }
256 
257 static struct tmc_sg_table *
258 catu_init_sg_table(struct device *catu_dev, int node,
259 		   ssize_t size, void **pages)
260 {
261 	int nr_tpages;
262 	struct tmc_sg_table *catu_table;
263 
264 	/*
265 	 * Each table can address upto 1MB and we can have
266 	 * CATU_PAGES_PER_SYSPAGE tables in a system page.
267 	 */
268 	nr_tpages = DIV_ROUND_UP(size, SZ_1M) / CATU_PAGES_PER_SYSPAGE;
269 	catu_table = tmc_alloc_sg_table(catu_dev, node, nr_tpages,
270 					size >> PAGE_SHIFT, pages);
271 	if (IS_ERR(catu_table))
272 		return catu_table;
273 
274 	catu_populate_table(catu_table);
275 	dev_dbg(catu_dev,
276 		"Setup table %p, size %ldKB, %d table pages\n",
277 		catu_table, (unsigned long)size >> 10,  nr_tpages);
278 	catu_dump_table(catu_table);
279 	return catu_table;
280 }
281 
282 static void catu_free_etr_buf(struct etr_buf *etr_buf)
283 {
284 	struct catu_etr_buf *catu_buf;
285 
286 	if (!etr_buf || etr_buf->mode != ETR_MODE_CATU || !etr_buf->private)
287 		return;
288 
289 	catu_buf = etr_buf->private;
290 	tmc_free_sg_table(catu_buf->catu_table);
291 	kfree(catu_buf);
292 }
293 
294 static ssize_t catu_get_data_etr_buf(struct etr_buf *etr_buf, u64 offset,
295 				     size_t len, char **bufpp)
296 {
297 	struct catu_etr_buf *catu_buf = etr_buf->private;
298 
299 	return tmc_sg_table_get_data(catu_buf->catu_table, offset, len, bufpp);
300 }
301 
302 static void catu_sync_etr_buf(struct etr_buf *etr_buf, u64 rrp, u64 rwp)
303 {
304 	struct catu_etr_buf *catu_buf = etr_buf->private;
305 	struct tmc_sg_table *catu_table = catu_buf->catu_table;
306 	u64 r_offset, w_offset;
307 
308 	/*
309 	 * ETR started off at etr_buf->hwaddr. Convert the RRP/RWP to
310 	 * offsets within the trace buffer.
311 	 */
312 	r_offset = rrp - etr_buf->hwaddr;
313 	w_offset = rwp - etr_buf->hwaddr;
314 
315 	if (!etr_buf->full) {
316 		etr_buf->len = w_offset - r_offset;
317 		if (w_offset < r_offset)
318 			etr_buf->len += etr_buf->size;
319 	} else {
320 		etr_buf->len = etr_buf->size;
321 	}
322 
323 	etr_buf->offset = r_offset;
324 	tmc_sg_table_sync_data_range(catu_table, r_offset, etr_buf->len);
325 }
326 
327 static int catu_alloc_etr_buf(struct tmc_drvdata *tmc_drvdata,
328 			      struct etr_buf *etr_buf, int node, void **pages)
329 {
330 	struct coresight_device *csdev;
331 	struct device *catu_dev;
332 	struct tmc_sg_table *catu_table;
333 	struct catu_etr_buf *catu_buf;
334 
335 	csdev = tmc_etr_get_catu_device(tmc_drvdata);
336 	if (!csdev)
337 		return -ENODEV;
338 	catu_dev = csdev->dev.parent;
339 	catu_buf = kzalloc(sizeof(*catu_buf), GFP_KERNEL);
340 	if (!catu_buf)
341 		return -ENOMEM;
342 
343 	catu_table = catu_init_sg_table(catu_dev, node, etr_buf->size, pages);
344 	if (IS_ERR(catu_table)) {
345 		kfree(catu_buf);
346 		return PTR_ERR(catu_table);
347 	}
348 
349 	etr_buf->mode = ETR_MODE_CATU;
350 	etr_buf->private = catu_buf;
351 	etr_buf->hwaddr = CATU_DEFAULT_INADDR;
352 
353 	catu_buf->catu_table = catu_table;
354 	/* Get the table base address */
355 	catu_buf->sladdr = catu_table->table_daddr;
356 
357 	return 0;
358 }
359 
360 const struct etr_buf_operations etr_catu_buf_ops = {
361 	.alloc = catu_alloc_etr_buf,
362 	.free = catu_free_etr_buf,
363 	.sync = catu_sync_etr_buf,
364 	.get_data = catu_get_data_etr_buf,
365 };
366 
367 coresight_simple_reg32(struct catu_drvdata, devid, CORESIGHT_DEVID);
368 coresight_simple_reg32(struct catu_drvdata, control, CATU_CONTROL);
369 coresight_simple_reg32(struct catu_drvdata, status, CATU_STATUS);
370 coresight_simple_reg32(struct catu_drvdata, mode, CATU_MODE);
371 coresight_simple_reg32(struct catu_drvdata, axictrl, CATU_AXICTRL);
372 coresight_simple_reg32(struct catu_drvdata, irqen, CATU_IRQEN);
373 coresight_simple_reg64(struct catu_drvdata, sladdr,
374 		       CATU_SLADDRLO, CATU_SLADDRHI);
375 coresight_simple_reg64(struct catu_drvdata, inaddr,
376 		       CATU_INADDRLO, CATU_INADDRHI);
377 
378 static struct attribute *catu_mgmt_attrs[] = {
379 	&dev_attr_devid.attr,
380 	&dev_attr_control.attr,
381 	&dev_attr_status.attr,
382 	&dev_attr_mode.attr,
383 	&dev_attr_axictrl.attr,
384 	&dev_attr_irqen.attr,
385 	&dev_attr_sladdr.attr,
386 	&dev_attr_inaddr.attr,
387 	NULL,
388 };
389 
390 static const struct attribute_group catu_mgmt_group = {
391 	.attrs = catu_mgmt_attrs,
392 	.name = "mgmt",
393 };
394 
395 static const struct attribute_group *catu_groups[] = {
396 	&catu_mgmt_group,
397 	NULL,
398 };
399 
400 
401 static inline int catu_wait_for_ready(struct catu_drvdata *drvdata)
402 {
403 	return coresight_timeout(drvdata->base,
404 				 CATU_STATUS, CATU_STATUS_READY, 1);
405 }
406 
407 static int catu_enable_hw(struct catu_drvdata *drvdata, void *data)
408 {
409 	u32 control, mode;
410 	struct etr_buf *etr_buf = data;
411 
412 	if (catu_wait_for_ready(drvdata))
413 		dev_warn(drvdata->dev, "Timeout while waiting for READY\n");
414 
415 	control = catu_read_control(drvdata);
416 	if (control & BIT(CATU_CONTROL_ENABLE)) {
417 		dev_warn(drvdata->dev, "CATU is already enabled\n");
418 		return -EBUSY;
419 	}
420 
421 	control |= BIT(CATU_CONTROL_ENABLE);
422 
423 	if (etr_buf && etr_buf->mode == ETR_MODE_CATU) {
424 		struct catu_etr_buf *catu_buf = etr_buf->private;
425 
426 		mode = CATU_MODE_TRANSLATE;
427 		catu_write_axictrl(drvdata, CATU_OS_AXICTRL);
428 		catu_write_sladdr(drvdata, catu_buf->sladdr);
429 		catu_write_inaddr(drvdata, CATU_DEFAULT_INADDR);
430 	} else {
431 		mode = CATU_MODE_PASS_THROUGH;
432 		catu_write_sladdr(drvdata, 0);
433 		catu_write_inaddr(drvdata, 0);
434 	}
435 
436 	catu_write_irqen(drvdata, 0);
437 	catu_write_mode(drvdata, mode);
438 	catu_write_control(drvdata, control);
439 	dev_dbg(drvdata->dev, "Enabled in %s mode\n",
440 		(mode == CATU_MODE_PASS_THROUGH) ?
441 		"Pass through" :
442 		"Translate");
443 	return 0;
444 }
445 
446 static int catu_enable(struct coresight_device *csdev, void *data)
447 {
448 	int rc;
449 	struct catu_drvdata *catu_drvdata = csdev_to_catu_drvdata(csdev);
450 
451 	CS_UNLOCK(catu_drvdata->base);
452 	rc = catu_enable_hw(catu_drvdata, data);
453 	CS_LOCK(catu_drvdata->base);
454 	return rc;
455 }
456 
457 static int catu_disable_hw(struct catu_drvdata *drvdata)
458 {
459 	int rc = 0;
460 
461 	catu_write_control(drvdata, 0);
462 	if (catu_wait_for_ready(drvdata)) {
463 		dev_info(drvdata->dev, "Timeout while waiting for READY\n");
464 		rc = -EAGAIN;
465 	}
466 
467 	dev_dbg(drvdata->dev, "Disabled\n");
468 	return rc;
469 }
470 
471 static int catu_disable(struct coresight_device *csdev, void *__unused)
472 {
473 	int rc;
474 	struct catu_drvdata *catu_drvdata = csdev_to_catu_drvdata(csdev);
475 
476 	CS_UNLOCK(catu_drvdata->base);
477 	rc = catu_disable_hw(catu_drvdata);
478 	CS_LOCK(catu_drvdata->base);
479 	return rc;
480 }
481 
482 const struct coresight_ops_helper catu_helper_ops = {
483 	.enable = catu_enable,
484 	.disable = catu_disable,
485 };
486 
487 const struct coresight_ops catu_ops = {
488 	.helper_ops = &catu_helper_ops,
489 };
490 
491 static int catu_probe(struct amba_device *adev, const struct amba_id *id)
492 {
493 	int ret = 0;
494 	u32 dma_mask;
495 	struct catu_drvdata *drvdata;
496 	struct coresight_desc catu_desc;
497 	struct coresight_platform_data *pdata = NULL;
498 	struct device *dev = &adev->dev;
499 	struct device_node *np = dev->of_node;
500 	void __iomem *base;
501 
502 	if (np) {
503 		pdata = of_get_coresight_platform_data(dev, np);
504 		if (IS_ERR(pdata)) {
505 			ret = PTR_ERR(pdata);
506 			goto out;
507 		}
508 		dev->platform_data = pdata;
509 	}
510 
511 	drvdata = devm_kzalloc(dev, sizeof(*drvdata), GFP_KERNEL);
512 	if (!drvdata) {
513 		ret = -ENOMEM;
514 		goto out;
515 	}
516 
517 	drvdata->dev = dev;
518 	dev_set_drvdata(dev, drvdata);
519 	base = devm_ioremap_resource(dev, &adev->res);
520 	if (IS_ERR(base)) {
521 		ret = PTR_ERR(base);
522 		goto out;
523 	}
524 
525 	/* Setup dma mask for the device */
526 	dma_mask = readl_relaxed(base + CORESIGHT_DEVID) & 0x3f;
527 	switch (dma_mask) {
528 	case 32:
529 	case 40:
530 	case 44:
531 	case 48:
532 	case 52:
533 	case 56:
534 	case 64:
535 		break;
536 	default:
537 		/* Default to the 40bits as supported by TMC-ETR */
538 		dma_mask = 40;
539 	}
540 	ret = dma_set_mask_and_coherent(dev, DMA_BIT_MASK(dma_mask));
541 	if (ret)
542 		goto out;
543 
544 	drvdata->base = base;
545 	catu_desc.pdata = pdata;
546 	catu_desc.dev = dev;
547 	catu_desc.groups = catu_groups;
548 	catu_desc.type = CORESIGHT_DEV_TYPE_HELPER;
549 	catu_desc.subtype.helper_subtype = CORESIGHT_DEV_SUBTYPE_HELPER_CATU;
550 	catu_desc.ops = &catu_ops;
551 	drvdata->csdev = coresight_register(&catu_desc);
552 	if (IS_ERR(drvdata->csdev))
553 		ret = PTR_ERR(drvdata->csdev);
554 out:
555 	pm_runtime_put(&adev->dev);
556 	return ret;
557 }
558 
559 static struct amba_id catu_ids[] = {
560 	{
561 		.id	= 0x000bb9ee,
562 		.mask	= 0x000fffff,
563 	},
564 	{},
565 };
566 
567 static struct amba_driver catu_driver = {
568 	.drv = {
569 		.name			= "coresight-catu",
570 		.owner			= THIS_MODULE,
571 		.suppress_bind_attrs	= true,
572 	},
573 	.probe				= catu_probe,
574 	.id_table			= catu_ids,
575 };
576 
577 builtin_amba_driver(catu_driver);
578