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 | 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 | |
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 | *base = rbnode->base_reg; |
45 | *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 | return regcache_get_val(map, base: rbnode->block, idx); |
52 | } |
53 | |
54 | static 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(nr: idx, addr: rbnode->cache_present); |
59 | regcache_set_val(map, base: rbnode->block, idx, val); |
60 | } |
61 | |
62 | static 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: &base_reg, |
73 | top: &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: &base_reg, |
82 | top: &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 | |
96 | 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 | 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: rbnode_tmp, base: &base_reg_tmp, |
110 | top: &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(node: &rbnode->node, parent, rb_link: new); |
126 | rb_insert_color(&rbnode->node, root); |
127 | |
128 | return 1; |
129 | } |
130 | |
131 | #ifdef CONFIG_DEBUG_FS |
132 | static 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, rbnode: n, base: &base, top: &top); |
156 | this_registers = ((top - base) / map->reg_stride) + 1; |
157 | seq_printf(m: s, fmt: "%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(m: s, fmt: "%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 | |
176 | DEFINE_SHOW_ATTRIBUTE(rbtree); |
177 | |
178 | static void rbtree_debugfs_init(struct regmap *map) |
179 | { |
180 | debugfs_create_file(name: "rbtree" , mode: 0400, parent: map->debugfs, data: map, fops: &rbtree_fops); |
181 | } |
182 | #endif |
183 | |
184 | static 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(size: 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 | reg: map->reg_defaults[i].reg, |
201 | value: map->reg_defaults[i].def); |
202 | if (ret) |
203 | goto err; |
204 | } |
205 | |
206 | return 0; |
207 | |
208 | err: |
209 | regcache_rbtree_exit(map); |
210 | return ret; |
211 | } |
212 | |
213 | 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 | 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(objp: rbtree_node->cache_present); |
231 | kfree(objp: rbtree_node->block); |
232 | kfree(objp: rbtree_node); |
233 | } |
234 | |
235 | /* release the resources */ |
236 | kfree(objp: map->cache); |
237 | map->cache = NULL; |
238 | |
239 | return 0; |
240 | } |
241 | |
242 | 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 | 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, idx: reg_tmp); |
254 | } else { |
255 | return -ENOENT; |
256 | } |
257 | |
258 | return 0; |
259 | } |
260 | |
261 | |
262 | 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 | 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(objp: rbnode->block, |
279 | new_size: blklen * map->cache_word_size, |
280 | flags: 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(objp: rbnode->cache_present, |
288 | BITS_TO_LONGS(blklen) * sizeof(*present), |
289 | flags: 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(dst: present, src: present, shift: offset, nbits: 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, idx: pos, val: value); |
313 | return 0; |
314 | } |
315 | |
316 | static struct regcache_rbtree_node * |
317 | regcache_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(size: sizeof(*rbnode), flags: 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 | range: &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(n: rbnode->blklen, size: map->cache_word_size, |
349 | flags: map->alloc_flags); |
350 | if (!rbnode->block) |
351 | goto err_free; |
352 | |
353 | rbnode->cache_present = kcalloc(BITS_TO_LONGS(rbnode->blklen), |
354 | size: sizeof(*rbnode->cache_present), |
355 | flags: map->alloc_flags); |
356 | if (!rbnode->cache_present) |
357 | goto err_free_block; |
358 | |
359 | return rbnode; |
360 | |
361 | err_free_block: |
362 | kfree(objp: rbnode->block); |
363 | err_free: |
364 | kfree(objp: rbnode); |
365 | return NULL; |
366 | } |
367 | |
368 | static 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, idx: reg_tmp, val: 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: rbnode_tmp, |
408 | base: &base_reg, top: &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 | base_reg: new_base_reg, |
441 | top_reg: 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 | idx: (reg - rbnode->base_reg) / map->reg_stride, |
457 | val: value); |
458 | regcache_rbtree_insert(map, root: &rbtree_ctx->root, rbnode); |
459 | rbtree_ctx->cached_rbnode = rbnode; |
460 | } |
461 | |
462 | return 0; |
463 | } |
464 | |
465 | 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 | 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: &base_reg, |
482 | top: &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, block: rbnode->block, |
499 | cache_present: rbnode->cache_present, |
500 | block_base: 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 | |
510 | static 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: &base_reg, |
524 | top: &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(map: rbnode->cache_present, start, nbits: end - start); |
541 | } |
542 | |
543 | return 0; |
544 | } |
545 | |
546 | struct 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 | |