regcache-rbtree.c revision 879082c9fe6e8fbddf787170eee605e4be138d0f 
1/*
2 * Register cache access API - rbtree caching support
3 *
4 * Copyright 2011 Wolfson Microelectronics plc
5 *
6 * Author: Dimitris Papastamos <dp@opensource.wolfsonmicro.com>
7 *
8 * This program is free software; you can redistribute it and/or modify
9 * it under the terms of the GNU General Public License version 2 as
10 * published by the Free Software Foundation.
11 */
12
13#include <linux/slab.h>
14#include <linux/device.h>
15#include <linux/debugfs.h>
16#include <linux/rbtree.h>
17#include <linux/seq_file.h>
18
19#include "internal.h"
20
21static int regcache_rbtree_write(struct regmap *map, unsigned int reg,
22				 unsigned int value);
23static int regcache_rbtree_exit(struct regmap *map);
24
25struct regcache_rbtree_node {
26	/* the actual rbtree node holding this block */
27	struct rb_node node;
28	/* base register handled by this block */
29	unsigned int base_reg;
30	/* block of adjacent registers */
31	void *block;
32	/* number of registers available in the block */
33	unsigned int blklen;
34} __attribute__ ((packed));
35
36struct regcache_rbtree_ctx {
37	struct rb_root root;
38	struct regcache_rbtree_node *cached_rbnode;
39};
40
41static inline void regcache_rbtree_get_base_top_reg(
42	struct regmap *map,
43	struct regcache_rbtree_node *rbnode,
44	unsigned int *base, unsigned int *top)
45{
46	*base = rbnode->base_reg;
47	*top = rbnode->base_reg + ((rbnode->blklen - 1) * map->reg_stride);
48}
49
50static unsigned int regcache_rbtree_get_register(struct regmap *map,
51	struct regcache_rbtree_node *rbnode, unsigned int idx)
52{
53	return regcache_get_val(map, rbnode->block, idx);
54}
55
56static void regcache_rbtree_set_register(struct regmap *map,
57					 struct regcache_rbtree_node *rbnode,
58					 unsigned int idx, unsigned int val)
59{
60	regcache_set_val(map, rbnode->block, idx, val);
61}
62
63static struct regcache_rbtree_node *regcache_rbtree_lookup(struct regmap *map,
64							   unsigned int reg)
65{
66	struct regcache_rbtree_ctx *rbtree_ctx = map->cache;
67	struct rb_node *node;
68	struct regcache_rbtree_node *rbnode;
69	unsigned int base_reg, top_reg;
70
71	rbnode = rbtree_ctx->cached_rbnode;
72	if (rbnode) {
73		regcache_rbtree_get_base_top_reg(map, rbnode, &base_reg,
74						 &top_reg);
75		if (reg >= base_reg && reg <= top_reg)
76			return rbnode;
77	}
78
79	node = rbtree_ctx->root.rb_node;
80	while (node) {
81		rbnode = container_of(node, struct regcache_rbtree_node, node);
82		regcache_rbtree_get_base_top_reg(map, rbnode, &base_reg,
83						 &top_reg);
84		if (reg >= base_reg && reg <= top_reg) {
85			rbtree_ctx->cached_rbnode = rbnode;
86			return rbnode;
87		} else if (reg > top_reg) {
88			node = node->rb_right;
89		} else if (reg < base_reg) {
90			node = node->rb_left;
91		}
92	}
93
94	return NULL;
95}
96
97static int regcache_rbtree_insert(struct regmap *map, struct rb_root *root,
98				  struct regcache_rbtree_node *rbnode)
99{
100	struct rb_node **new, *parent;
101	struct regcache_rbtree_node *rbnode_tmp;
102	unsigned int base_reg_tmp, top_reg_tmp;
103	unsigned int base_reg;
104
105	parent = NULL;
106	new = &root->rb_node;
107	while (*new) {
108		rbnode_tmp = container_of(*new, struct regcache_rbtree_node,
109					  node);
110		/* base and top registers of the current rbnode */
111		regcache_rbtree_get_base_top_reg(map, rbnode_tmp, &base_reg_tmp,
112						 &top_reg_tmp);
113		/* base register of the rbnode to be added */
114		base_reg = rbnode->base_reg;
115		parent = *new;
116		/* if this register has already been inserted, just return */
117		if (base_reg >= base_reg_tmp &&
118		    base_reg <= top_reg_tmp)
119			return 0;
120		else if (base_reg > top_reg_tmp)
121			new = &((*new)->rb_right);
122		else if (base_reg < base_reg_tmp)
123			new = &((*new)->rb_left);
124	}
125
126	/* insert the node into the rbtree */
127	rb_link_node(&rbnode->node, parent, new);
128	rb_insert_color(&rbnode->node, root);
129
130	return 1;
131}
132
133#ifdef CONFIG_DEBUG_FS
134static int rbtree_show(struct seq_file *s, void *ignored)
135{
136	struct regmap *map = s->private;
137	struct regcache_rbtree_ctx *rbtree_ctx = map->cache;
138	struct regcache_rbtree_node *n;
139	struct rb_node *node;
140	unsigned int base, top;
141	int nodes = 0;
142	int registers = 0;
143	int this_registers, average;
144
145	map->lock(map);
146
147	for (node = rb_first(&rbtree_ctx->root); node != NULL;
148	     node = rb_next(node)) {
149		n = container_of(node, struct regcache_rbtree_node, node);
150
151		regcache_rbtree_get_base_top_reg(map, n, &base, &top);
152		this_registers = ((top - base) / map->reg_stride) + 1;
153		seq_printf(s, "%x-%x (%d)\n", base, top, this_registers);
154
155		nodes++;
156		registers += this_registers;
157	}
158
159	if (nodes)
160		average = registers / nodes;
161	else
162		average = 0;
163
164	seq_printf(s, "%d nodes, %d registers, average %d registers\n",
165		   nodes, registers, average);
166
167	map->unlock(map);
168
169	return 0;
170}
171
172static int rbtree_open(struct inode *inode, struct file *file)
173{
174	return single_open(file, rbtree_show, inode->i_private);
175}
176
177static const struct file_operations rbtree_fops = {
178	.open		= rbtree_open,
179	.read		= seq_read,
180	.llseek		= seq_lseek,
181	.release	= single_release,
182};
183
184static void rbtree_debugfs_init(struct regmap *map)
185{
186	debugfs_create_file("rbtree", 0400, map->debugfs, map, &rbtree_fops);
187}
188#else
189static void rbtree_debugfs_init(struct regmap *map)
190{
191}
192#endif
193
194static int regcache_rbtree_init(struct regmap *map)
195{
196	struct regcache_rbtree_ctx *rbtree_ctx;
197	int i;
198	int ret;
199
200	map->cache = kmalloc(sizeof *rbtree_ctx, GFP_KERNEL);
201	if (!map->cache)
202		return -ENOMEM;
203
204	rbtree_ctx = map->cache;
205	rbtree_ctx->root = RB_ROOT;
206	rbtree_ctx->cached_rbnode = NULL;
207
208	for (i = 0; i < map->num_reg_defaults; i++) {
209		ret = regcache_rbtree_write(map,
210					    map->reg_defaults[i].reg,
211					    map->reg_defaults[i].def);
212		if (ret)
213			goto err;
214	}
215
216	rbtree_debugfs_init(map);
217
218	return 0;
219
220err:
221	regcache_rbtree_exit(map);
222	return ret;
223}
224
225static int regcache_rbtree_exit(struct regmap *map)
226{
227	struct rb_node *next;
228	struct regcache_rbtree_ctx *rbtree_ctx;
229	struct regcache_rbtree_node *rbtree_node;
230
231	/* if we've already been called then just return */
232	rbtree_ctx = map->cache;
233	if (!rbtree_ctx)
234		return 0;
235
236	/* free up the rbtree */
237	next = rb_first(&rbtree_ctx->root);
238	while (next) {
239		rbtree_node = rb_entry(next, struct regcache_rbtree_node, node);
240		next = rb_next(&rbtree_node->node);
241		rb_erase(&rbtree_node->node, &rbtree_ctx->root);
242		kfree(rbtree_node->block);
243		kfree(rbtree_node);
244	}
245
246	/* release the resources */
247	kfree(map->cache);
248	map->cache = NULL;
249
250	return 0;
251}
252
253static int regcache_rbtree_read(struct regmap *map,
254				unsigned int reg, unsigned int *value)
255{
256	struct regcache_rbtree_node *rbnode;
257	unsigned int reg_tmp;
258
259	rbnode = regcache_rbtree_lookup(map, reg);
260	if (rbnode) {
261		reg_tmp = (reg - rbnode->base_reg) / map->reg_stride;
262		*value = regcache_rbtree_get_register(map, rbnode, reg_tmp);
263	} else {
264		return -ENOENT;
265	}
266
267	return 0;
268}
269
270
271static int regcache_rbtree_insert_to_block(struct regmap *map,
272					   struct regcache_rbtree_node *rbnode,
273					   unsigned int pos, unsigned int reg,
274					   unsigned int value)
275{
276	u8 *blk;
277
278	blk = krealloc(rbnode->block,
279		       (rbnode->blklen + 1) * map->cache_word_size,
280		       GFP_KERNEL);
281	if (!blk)
282		return -ENOMEM;
283
284	/* insert the register value in the correct place in the rbnode block */
285	memmove(blk + (pos + 1) * map->cache_word_size,
286		blk + pos * map->cache_word_size,
287		(rbnode->blklen - pos) * map->cache_word_size);
288
289	/* update the rbnode block, its size and the base register */
290	rbnode->block = blk;
291	rbnode->blklen++;
292	if (!pos)
293		rbnode->base_reg = reg;
294
295	regcache_rbtree_set_register(map, rbnode, pos, value);
296	return 0;
297}
298
299static int regcache_rbtree_write(struct regmap *map, unsigned int reg,
300				 unsigned int value)
301{
302	struct regcache_rbtree_ctx *rbtree_ctx;
303	struct regcache_rbtree_node *rbnode, *rbnode_tmp;
304	struct rb_node *node;
305	unsigned int reg_tmp;
306	unsigned int pos;
307	int i;
308	int ret;
309
310	rbtree_ctx = map->cache;
311	/* if we can't locate it in the cached rbnode we'll have
312	 * to traverse the rbtree looking for it.
313	 */
314	rbnode = regcache_rbtree_lookup(map, reg);
315	if (rbnode) {
316		reg_tmp = (reg - rbnode->base_reg) / map->reg_stride;
317		regcache_rbtree_set_register(map, rbnode, reg_tmp, value);
318	} else {
319		/* look for an adjacent register to the one we are about to add */
320		for (node = rb_first(&rbtree_ctx->root); node;
321		     node = rb_next(node)) {
322			rbnode_tmp = rb_entry(node, struct regcache_rbtree_node,
323					      node);
324			for (i = 0; i < rbnode_tmp->blklen; i++) {
325				reg_tmp = rbnode_tmp->base_reg +
326						(i * map->reg_stride);
327				if (abs(reg_tmp - reg) != map->reg_stride)
328					continue;
329				/* decide where in the block to place our register */
330				if (reg_tmp + map->reg_stride == reg)
331					pos = i + 1;
332				else
333					pos = i;
334				ret = regcache_rbtree_insert_to_block(map,
335								      rbnode_tmp,
336								      pos, reg,
337								      value);
338				if (ret)
339					return ret;
340				rbtree_ctx->cached_rbnode = rbnode_tmp;
341				return 0;
342			}
343		}
344		/* we did not manage to find a place to insert it in an existing
345		 * block so create a new rbnode with a single register in its block.
346		 * This block will get populated further if any other adjacent
347		 * registers get modified in the future.
348		 */
349		rbnode = kzalloc(sizeof *rbnode, GFP_KERNEL);
350		if (!rbnode)
351			return -ENOMEM;
352		rbnode->blklen = 1;
353		rbnode->base_reg = reg;
354		rbnode->block = kmalloc(rbnode->blklen * map->cache_word_size,
355					GFP_KERNEL);
356		if (!rbnode->block) {
357			kfree(rbnode);
358			return -ENOMEM;
359		}
360		regcache_rbtree_set_register(map, rbnode, 0, value);
361		regcache_rbtree_insert(map, &rbtree_ctx->root, rbnode);
362		rbtree_ctx->cached_rbnode = rbnode;
363	}
364
365	return 0;
366}
367
368static int regcache_rbtree_sync(struct regmap *map, unsigned int min,
369				unsigned int max)
370{
371	struct regcache_rbtree_ctx *rbtree_ctx;
372	struct rb_node *node;
373	struct regcache_rbtree_node *rbnode;
374	unsigned int regtmp;
375	unsigned int val;
376	int ret;
377	int i, base, end;
378
379	rbtree_ctx = map->cache;
380	for (node = rb_first(&rbtree_ctx->root); node; node = rb_next(node)) {
381		rbnode = rb_entry(node, struct regcache_rbtree_node, node);
382
383		if (rbnode->base_reg < min)
384			continue;
385		if (rbnode->base_reg > max)
386			break;
387		if (rbnode->base_reg + rbnode->blklen < min)
388			continue;
389
390		if (min > rbnode->base_reg)
391			base = min - rbnode->base_reg;
392		else
393			base = 0;
394
395		if (max < rbnode->base_reg + rbnode->blklen)
396			end = rbnode->base_reg + rbnode->blklen - max;
397		else
398			end = rbnode->blklen;
399
400		for (i = base; i < end; i++) {
401			regtmp = rbnode->base_reg + (i * map->reg_stride);
402			val = regcache_rbtree_get_register(map, rbnode, i);
403
404			/* Is this the hardware default?  If so skip. */
405			ret = regcache_lookup_reg(map, regtmp);
406			if (ret >= 0 && val == map->reg_defaults[ret].def)
407				continue;
408
409			map->cache_bypass = 1;
410			ret = _regmap_write(map, regtmp, val);
411			map->cache_bypass = 0;
412			if (ret)
413				return ret;
414			dev_dbg(map->dev, "Synced register %#x, value %#x\n",
415				regtmp, val);
416		}
417	}
418
419	return 0;
420}
421
422struct regcache_ops regcache_rbtree_ops = {
423	.type = REGCACHE_RBTREE,
424	.name = "rbtree",
425	.init = regcache_rbtree_init,
426	.exit = regcache_rbtree_exit,
427	.read = regcache_rbtree_read,
428	.write = regcache_rbtree_write,
429	.sync = regcache_rbtree_sync
430};
431