1// SPDX-License-Identifier: GPL-2.0
2//
3// Register cache access API - rbtree caching support
4//
5// Copyright 2011 Wolfson Microelectronics plc
6//
7// Author: Dimitris Papastamos <dp@opensource.wolfsonmicro.com>
8
9#include <linux/debugfs.h>
10#include <linux/device.h>
11#include <linux/rbtree.h>
12#include <linux/seq_file.h>
13#include <linux/slab.h>
14
15#include "internal.h"
16
17static int regcache_rbtree_write(struct regmap *map, unsigned int reg,
18				 unsigned int value);
19static int regcache_rbtree_exit(struct regmap *map);
20
21struct regcache_rbtree_node {
22	/* block of adjacent registers */
23	void *block;
24	/* Which registers are present */
25	unsigned long *cache_present;
26	/* base register handled by this block */
27	unsigned int base_reg;
28	/* number of registers available in the block */
29	unsigned int blklen;
30	/* the actual rbtree node holding this block */
31	struct rb_node node;
32};
33
34struct regcache_rbtree_ctx {
35	struct rb_root root;
36	struct regcache_rbtree_node *cached_rbnode;
37};
38
39static inline void regcache_rbtree_get_base_top_reg(
40	struct regmap *map,
41	struct regcache_rbtree_node *rbnode,
42	unsigned int *base, unsigned int *top)
43{
44	*base = rbnode->base_reg;
45	*top = rbnode->base_reg + ((rbnode->blklen - 1) * map->reg_stride);
46}
47
48static unsigned int regcache_rbtree_get_register(struct regmap *map,
49	struct regcache_rbtree_node *rbnode, unsigned int idx)
50{
51	return regcache_get_val(map, rbnode->block, idx);
52}
53
54static void regcache_rbtree_set_register(struct regmap *map,
55					 struct regcache_rbtree_node *rbnode,
56					 unsigned int idx, unsigned int val)
57{
58	set_bit(idx, rbnode->cache_present);
59	regcache_set_val(map, rbnode->block, idx, val);
60}
61
62static struct regcache_rbtree_node *regcache_rbtree_lookup(struct regmap *map,
63							   unsigned int reg)
64{
65	struct regcache_rbtree_ctx *rbtree_ctx = map->cache;
66	struct rb_node *node;
67	struct regcache_rbtree_node *rbnode;
68	unsigned int base_reg, top_reg;
69
70	rbnode = rbtree_ctx->cached_rbnode;
71	if (rbnode) {
72		regcache_rbtree_get_base_top_reg(map, rbnode, &base_reg,
73						 &top_reg);
74		if (reg >= base_reg && reg <= top_reg)
75			return rbnode;
76	}
77
78	node = rbtree_ctx->root.rb_node;
79	while (node) {
80		rbnode = rb_entry(node, struct regcache_rbtree_node, node);
81		regcache_rbtree_get_base_top_reg(map, rbnode, &base_reg,
82						 &top_reg);
83		if (reg >= base_reg && reg <= top_reg) {
84			rbtree_ctx->cached_rbnode = rbnode;
85			return rbnode;
86		} else if (reg > top_reg) {
87			node = node->rb_right;
88		} else if (reg < base_reg) {
89			node = node->rb_left;
90		}
91	}
92
93	return NULL;
94}
95
96static int regcache_rbtree_insert(struct regmap *map, struct rb_root *root,
97				  struct regcache_rbtree_node *rbnode)
98{
99	struct rb_node **new, *parent;
100	struct regcache_rbtree_node *rbnode_tmp;
101	unsigned int base_reg_tmp, top_reg_tmp;
102	unsigned int base_reg;
103
104	parent = NULL;
105	new = &root->rb_node;
106	while (*new) {
107		rbnode_tmp = rb_entry(*new, struct regcache_rbtree_node, node);
108		/* base and top registers of the current rbnode */
109		regcache_rbtree_get_base_top_reg(map, rbnode_tmp, &base_reg_tmp,
110						 &top_reg_tmp);
111		/* base register of the rbnode to be added */
112		base_reg = rbnode->base_reg;
113		parent = *new;
114		/* if this register has already been inserted, just return */
115		if (base_reg >= base_reg_tmp &&
116		    base_reg <= top_reg_tmp)
117			return 0;
118		else if (base_reg > top_reg_tmp)
119			new = &((*new)->rb_right);
120		else if (base_reg < base_reg_tmp)
121			new = &((*new)->rb_left);
122	}
123
124	/* insert the node into the rbtree */
125	rb_link_node(&rbnode->node, parent, new);
126	rb_insert_color(&rbnode->node, root);
127
128	return 1;
129}
130
131#ifdef CONFIG_DEBUG_FS
132static int rbtree_show(struct seq_file *s, void *ignored)
133{
134	struct regmap *map = s->private;
135	struct regcache_rbtree_ctx *rbtree_ctx = map->cache;
136	struct regcache_rbtree_node *n;
137	struct rb_node *node;
138	unsigned int base, top;
139	size_t mem_size;
140	int nodes = 0;
141	int registers = 0;
142	int this_registers, average;
143
144	map->lock(map->lock_arg);
145
146	mem_size = sizeof(*rbtree_ctx);
147
148	for (node = rb_first(&rbtree_ctx->root); node != NULL;
149	     node = rb_next(node)) {
150		n = rb_entry(node, struct regcache_rbtree_node, node);
151		mem_size += sizeof(*n);
152		mem_size += (n->blklen * map->cache_word_size);
153		mem_size += BITS_TO_LONGS(n->blklen) * sizeof(long);
154
155		regcache_rbtree_get_base_top_reg(map, n, &base, &top);
156		this_registers = ((top - base) / map->reg_stride) + 1;
157		seq_printf(s, "%x-%x (%d)\n", base, top, this_registers);
158
159		nodes++;
160		registers += this_registers;
161	}
162
163	if (nodes)
164		average = registers / nodes;
165	else
166		average = 0;
167
168	seq_printf(s, "%d nodes, %d registers, average %d registers, used %zu bytes\n",
169		   nodes, registers, average, mem_size);
170
171	map->unlock(map->lock_arg);
172
173	return 0;
174}
175
176DEFINE_SHOW_ATTRIBUTE(rbtree);
177
178static void rbtree_debugfs_init(struct regmap *map)
179{
180	debugfs_create_file("rbtree", 0400, map->debugfs, map, &rbtree_fops);
181}
182#endif
183
184static int regcache_rbtree_init(struct regmap *map)
185{
186	struct regcache_rbtree_ctx *rbtree_ctx;
187	int i;
188	int ret;
189
190	map->cache = kmalloc(sizeof *rbtree_ctx, GFP_KERNEL);
191	if (!map->cache)
192		return -ENOMEM;
193
194	rbtree_ctx = map->cache;
195	rbtree_ctx->root = RB_ROOT;
196	rbtree_ctx->cached_rbnode = NULL;
197
198	for (i = 0; i < map->num_reg_defaults; i++) {
199		ret = regcache_rbtree_write(map,
200					    map->reg_defaults[i].reg,
201					    map->reg_defaults[i].def);
202		if (ret)
203			goto err;
204	}
205
206	return 0;
207
208err:
209	regcache_rbtree_exit(map);
210	return ret;
211}
212
213static int regcache_rbtree_exit(struct regmap *map)
214{
215	struct rb_node *next;
216	struct regcache_rbtree_ctx *rbtree_ctx;
217	struct regcache_rbtree_node *rbtree_node;
218
219	/* if we've already been called then just return */
220	rbtree_ctx = map->cache;
221	if (!rbtree_ctx)
222		return 0;
223
224	/* free up the rbtree */
225	next = rb_first(&rbtree_ctx->root);
226	while (next) {
227		rbtree_node = rb_entry(next, struct regcache_rbtree_node, node);
228		next = rb_next(&rbtree_node->node);
229		rb_erase(&rbtree_node->node, &rbtree_ctx->root);
230		kfree(rbtree_node->cache_present);
231		kfree(rbtree_node->block);
232		kfree(rbtree_node);
233	}
234
235	/* release the resources */
236	kfree(map->cache);
237	map->cache = NULL;
238
239	return 0;
240}
241
242static int regcache_rbtree_read(struct regmap *map,
243				unsigned int reg, unsigned int *value)
244{
245	struct regcache_rbtree_node *rbnode;
246	unsigned int reg_tmp;
247
248	rbnode = regcache_rbtree_lookup(map, reg);
249	if (rbnode) {
250		reg_tmp = (reg - rbnode->base_reg) / map->reg_stride;
251		if (!test_bit(reg_tmp, rbnode->cache_present))
252			return -ENOENT;
253		*value = regcache_rbtree_get_register(map, rbnode, reg_tmp);
254	} else {
255		return -ENOENT;
256	}
257
258	return 0;
259}
260
261
262static int regcache_rbtree_insert_to_block(struct regmap *map,
263					   struct regcache_rbtree_node *rbnode,
264					   unsigned int base_reg,
265					   unsigned int top_reg,
266					   unsigned int reg,
267					   unsigned int value)
268{
269	unsigned int blklen;
270	unsigned int pos, offset;
271	unsigned long *present;
272	u8 *blk;
273
274	blklen = (top_reg - base_reg) / map->reg_stride + 1;
275	pos = (reg - base_reg) / map->reg_stride;
276	offset = (rbnode->base_reg - base_reg) / map->reg_stride;
277
278	blk = krealloc(rbnode->block,
279		       blklen * map->cache_word_size,
280		       map->alloc_flags);
281	if (!blk)
282		return -ENOMEM;
283
284	rbnode->block = blk;
285
286	if (BITS_TO_LONGS(blklen) > BITS_TO_LONGS(rbnode->blklen)) {
287		present = krealloc(rbnode->cache_present,
288				   BITS_TO_LONGS(blklen) * sizeof(*present),
289				   map->alloc_flags);
290		if (!present)
291			return -ENOMEM;
292
293		memset(present + BITS_TO_LONGS(rbnode->blklen), 0,
294		       (BITS_TO_LONGS(blklen) - BITS_TO_LONGS(rbnode->blklen))
295		       * sizeof(*present));
296	} else {
297		present = rbnode->cache_present;
298	}
299
300	/* insert the register value in the correct place in the rbnode block */
301	if (pos == 0) {
302		memmove(blk + offset * map->cache_word_size,
303			blk, rbnode->blklen * map->cache_word_size);
304		bitmap_shift_left(present, present, offset, blklen);
305	}
306
307	/* update the rbnode block, its size and the base register */
308	rbnode->blklen = blklen;
309	rbnode->base_reg = base_reg;
310	rbnode->cache_present = present;
311
312	regcache_rbtree_set_register(map, rbnode, pos, value);
313	return 0;
314}
315
316static struct regcache_rbtree_node *
317regcache_rbtree_node_alloc(struct regmap *map, unsigned int reg)
318{
319	struct regcache_rbtree_node *rbnode;
320	const struct regmap_range *range;
321	int i;
322
323	rbnode = kzalloc(sizeof(*rbnode), map->alloc_flags);
324	if (!rbnode)
325		return NULL;
326
327	/* If there is a read table then use it to guess at an allocation */
328	if (map->rd_table) {
329		for (i = 0; i < map->rd_table->n_yes_ranges; i++) {
330			if (regmap_reg_in_range(reg,
331						&map->rd_table->yes_ranges[i]))
332				break;
333		}
334
335		if (i != map->rd_table->n_yes_ranges) {
336			range = &map->rd_table->yes_ranges[i];
337			rbnode->blklen = (range->range_max - range->range_min) /
338				map->reg_stride	+ 1;
339			rbnode->base_reg = range->range_min;
340		}
341	}
342
343	if (!rbnode->blklen) {
344		rbnode->blklen = 1;
345		rbnode->base_reg = reg;
346	}
347
348	rbnode->block = kmalloc_array(rbnode->blklen, map->cache_word_size,
349				      map->alloc_flags);
350	if (!rbnode->block)
351		goto err_free;
352
353	rbnode->cache_present = kcalloc(BITS_TO_LONGS(rbnode->blklen),
354					sizeof(*rbnode->cache_present),
355					map->alloc_flags);
356	if (!rbnode->cache_present)
357		goto err_free_block;
358
359	return rbnode;
360
361err_free_block:
362	kfree(rbnode->block);
363err_free:
364	kfree(rbnode);
365	return NULL;
366}
367
368static int regcache_rbtree_write(struct regmap *map, unsigned int reg,
369				 unsigned int value)
370{
371	struct regcache_rbtree_ctx *rbtree_ctx;
372	struct regcache_rbtree_node *rbnode, *rbnode_tmp;
373	struct rb_node *node;
374	unsigned int reg_tmp;
375	int ret;
376
377	rbtree_ctx = map->cache;
378
379	/* if we can't locate it in the cached rbnode we'll have
380	 * to traverse the rbtree looking for it.
381	 */
382	rbnode = regcache_rbtree_lookup(map, reg);
383	if (rbnode) {
384		reg_tmp = (reg - rbnode->base_reg) / map->reg_stride;
385		regcache_rbtree_set_register(map, rbnode, reg_tmp, value);
386	} else {
387		unsigned int base_reg, top_reg;
388		unsigned int new_base_reg, new_top_reg;
389		unsigned int min, max;
390		unsigned int max_dist;
391		unsigned int dist, best_dist = UINT_MAX;
392
393		max_dist = map->reg_stride * sizeof(*rbnode_tmp) /
394			map->cache_word_size;
395		if (reg < max_dist)
396			min = 0;
397		else
398			min = reg - max_dist;
399		max = reg + max_dist;
400
401		/* look for an adjacent register to the one we are about to add */
402		node = rbtree_ctx->root.rb_node;
403		while (node) {
404			rbnode_tmp = rb_entry(node, struct regcache_rbtree_node,
405					      node);
406
407			regcache_rbtree_get_base_top_reg(map, rbnode_tmp,
408				&base_reg, &top_reg);
409
410			if (base_reg <= max && top_reg >= min) {
411				if (reg < base_reg)
412					dist = base_reg - reg;
413				else if (reg > top_reg)
414					dist = reg - top_reg;
415				else
416					dist = 0;
417				if (dist < best_dist) {
418					rbnode = rbnode_tmp;
419					best_dist = dist;
420					new_base_reg = min(reg, base_reg);
421					new_top_reg = max(reg, top_reg);
422				}
423			}
424
425			/*
426			 * Keep looking, we want to choose the closest block,
427			 * otherwise we might end up creating overlapping
428			 * blocks, which breaks the rbtree.
429			 */
430			if (reg < base_reg)
431				node = node->rb_left;
432			else if (reg > top_reg)
433				node = node->rb_right;
434			else
435				break;
436		}
437
438		if (rbnode) {
439			ret = regcache_rbtree_insert_to_block(map, rbnode,
440							      new_base_reg,
441							      new_top_reg, reg,
442							      value);
443			if (ret)
444				return ret;
445			rbtree_ctx->cached_rbnode = rbnode;
446			return 0;
447		}
448
449		/* We did not manage to find a place to insert it in
450		 * an existing block so create a new rbnode.
451		 */
452		rbnode = regcache_rbtree_node_alloc(map, reg);
453		if (!rbnode)
454			return -ENOMEM;
455		regcache_rbtree_set_register(map, rbnode,
456					     (reg - rbnode->base_reg) / map->reg_stride,
457					     value);
458		regcache_rbtree_insert(map, &rbtree_ctx->root, rbnode);
459		rbtree_ctx->cached_rbnode = rbnode;
460	}
461
462	return 0;
463}
464
465static int regcache_rbtree_sync(struct regmap *map, unsigned int min,
466				unsigned int max)
467{
468	struct regcache_rbtree_ctx *rbtree_ctx;
469	struct rb_node *node;
470	struct regcache_rbtree_node *rbnode;
471	unsigned int base_reg, top_reg;
472	unsigned int start, end;
473	int ret;
474
475	map->async = true;
476
477	rbtree_ctx = map->cache;
478	for (node = rb_first(&rbtree_ctx->root); node; node = rb_next(node)) {
479		rbnode = rb_entry(node, struct regcache_rbtree_node, node);
480
481		regcache_rbtree_get_base_top_reg(map, rbnode, &base_reg,
482			&top_reg);
483		if (base_reg > max)
484			break;
485		if (top_reg < min)
486			continue;
487
488		if (min > base_reg)
489			start = (min - base_reg) / map->reg_stride;
490		else
491			start = 0;
492
493		if (max < top_reg)
494			end = (max - base_reg) / map->reg_stride + 1;
495		else
496			end = rbnode->blklen;
497
498		ret = regcache_sync_block(map, rbnode->block,
499					  rbnode->cache_present,
500					  rbnode->base_reg, start, end);
501		if (ret != 0)
502			return ret;
503	}
504
505	map->async = false;
506
507	return regmap_async_complete(map);
508}
509
510static int regcache_rbtree_drop(struct regmap *map, unsigned int min,
511				unsigned int max)
512{
513	struct regcache_rbtree_ctx *rbtree_ctx;
514	struct regcache_rbtree_node *rbnode;
515	struct rb_node *node;
516	unsigned int base_reg, top_reg;
517	unsigned int start, end;
518
519	rbtree_ctx = map->cache;
520	for (node = rb_first(&rbtree_ctx->root); node; node = rb_next(node)) {
521		rbnode = rb_entry(node, struct regcache_rbtree_node, node);
522
523		regcache_rbtree_get_base_top_reg(map, rbnode, &base_reg,
524			&top_reg);
525		if (base_reg > max)
526			break;
527		if (top_reg < min)
528			continue;
529
530		if (min > base_reg)
531			start = (min - base_reg) / map->reg_stride;
532		else
533			start = 0;
534
535		if (max < top_reg)
536			end = (max - base_reg) / map->reg_stride + 1;
537		else
538			end = rbnode->blklen;
539
540		bitmap_clear(rbnode->cache_present, start, end - start);
541	}
542
543	return 0;
544}
545
546struct regcache_ops regcache_rbtree_ops = {
547	.type = REGCACHE_RBTREE,
548	.name = "rbtree",
549	.init = regcache_rbtree_init,
550	.exit = regcache_rbtree_exit,
551#ifdef CONFIG_DEBUG_FS
552	.debugfs_init = rbtree_debugfs_init,
553#endif
554	.read = regcache_rbtree_read,
555	.write = regcache_rbtree_write,
556	.sync = regcache_rbtree_sync,
557	.drop = regcache_rbtree_drop,
558};
559