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