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