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, 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(idx, rbnode->cache_present); 59 regcache_set_val(map, 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_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 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_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 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, 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 176 DEFINE_SHOW_ATTRIBUTE(rbtree); 177 178 static void rbtree_debugfs_init(struct regmap *map) 179 { 180 debugfs_create_file("rbtree", 0400, map->debugfs, map, &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(sizeof *rbtree_ctx, map->alloc_flags); 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 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(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 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, 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_array(rbnode->block, blklen, map->cache_word_size, map->alloc_flags); 279 if (!blk) 280 return -ENOMEM; 281 282 rbnode->block = blk; 283 284 if (BITS_TO_LONGS(blklen) > BITS_TO_LONGS(rbnode->blklen)) { 285 present = krealloc_array(rbnode->cache_present, 286 BITS_TO_LONGS(blklen), sizeof(*present), 287 map->alloc_flags); 288 if (!present) 289 return -ENOMEM; 290 291 memset(present + BITS_TO_LONGS(rbnode->blklen), 0, 292 (BITS_TO_LONGS(blklen) - BITS_TO_LONGS(rbnode->blklen)) 293 * sizeof(*present)); 294 } else { 295 present = rbnode->cache_present; 296 } 297 298 /* insert the register value in the correct place in the rbnode block */ 299 if (pos == 0) { 300 memmove(blk + offset * map->cache_word_size, 301 blk, rbnode->blklen * map->cache_word_size); 302 bitmap_shift_left(present, present, offset, blklen); 303 } 304 305 /* update the rbnode block, its size and the base register */ 306 rbnode->blklen = blklen; 307 rbnode->base_reg = base_reg; 308 rbnode->cache_present = present; 309 310 regcache_rbtree_set_register(map, rbnode, pos, value); 311 return 0; 312 } 313 314 static struct regcache_rbtree_node * 315 regcache_rbtree_node_alloc(struct regmap *map, unsigned int reg) 316 { 317 struct regcache_rbtree_node *rbnode; 318 const struct regmap_range *range; 319 int i; 320 321 rbnode = kzalloc(sizeof(*rbnode), map->alloc_flags); 322 if (!rbnode) 323 return NULL; 324 325 /* If there is a read table then use it to guess at an allocation */ 326 if (map->rd_table) { 327 for (i = 0; i < map->rd_table->n_yes_ranges; i++) { 328 if (regmap_reg_in_range(reg, 329 &map->rd_table->yes_ranges[i])) 330 break; 331 } 332 333 if (i != map->rd_table->n_yes_ranges) { 334 range = &map->rd_table->yes_ranges[i]; 335 rbnode->blklen = (range->range_max - range->range_min) / 336 map->reg_stride + 1; 337 rbnode->base_reg = range->range_min; 338 } 339 } 340 341 if (!rbnode->blklen) { 342 rbnode->blklen = 1; 343 rbnode->base_reg = reg; 344 } 345 346 rbnode->block = kmalloc_array(rbnode->blklen, map->cache_word_size, 347 map->alloc_flags); 348 if (!rbnode->block) 349 goto err_free; 350 351 rbnode->cache_present = kcalloc(BITS_TO_LONGS(rbnode->blklen), 352 sizeof(*rbnode->cache_present), 353 map->alloc_flags); 354 if (!rbnode->cache_present) 355 goto err_free_block; 356 357 return rbnode; 358 359 err_free_block: 360 kfree(rbnode->block); 361 err_free: 362 kfree(rbnode); 363 return NULL; 364 } 365 366 static int regcache_rbtree_write(struct regmap *map, unsigned int reg, 367 unsigned int value) 368 { 369 struct regcache_rbtree_ctx *rbtree_ctx; 370 struct regcache_rbtree_node *rbnode, *rbnode_tmp; 371 struct rb_node *node; 372 unsigned int reg_tmp; 373 int ret; 374 375 rbtree_ctx = map->cache; 376 377 /* if we can't locate it in the cached rbnode we'll have 378 * to traverse the rbtree looking for it. 379 */ 380 rbnode = regcache_rbtree_lookup(map, reg); 381 if (rbnode) { 382 reg_tmp = (reg - rbnode->base_reg) / map->reg_stride; 383 regcache_rbtree_set_register(map, rbnode, reg_tmp, value); 384 } else { 385 unsigned int base_reg, top_reg; 386 unsigned int new_base_reg, new_top_reg; 387 unsigned int min, max; 388 unsigned int max_dist; 389 unsigned int dist, best_dist = UINT_MAX; 390 391 max_dist = map->reg_stride * sizeof(*rbnode_tmp) / 392 map->cache_word_size; 393 if (reg < max_dist) 394 min = 0; 395 else 396 min = reg - max_dist; 397 max = reg + max_dist; 398 399 /* look for an adjacent register to the one we are about to add */ 400 node = rbtree_ctx->root.rb_node; 401 while (node) { 402 rbnode_tmp = rb_entry(node, struct regcache_rbtree_node, 403 node); 404 405 regcache_rbtree_get_base_top_reg(map, rbnode_tmp, 406 &base_reg, &top_reg); 407 408 if (base_reg <= max && top_reg >= min) { 409 if (reg < base_reg) 410 dist = base_reg - reg; 411 else if (reg > top_reg) 412 dist = reg - top_reg; 413 else 414 dist = 0; 415 if (dist < best_dist) { 416 rbnode = rbnode_tmp; 417 best_dist = dist; 418 new_base_reg = min(reg, base_reg); 419 new_top_reg = max(reg, top_reg); 420 } 421 } 422 423 /* 424 * Keep looking, we want to choose the closest block, 425 * otherwise we might end up creating overlapping 426 * blocks, which breaks the rbtree. 427 */ 428 if (reg < base_reg) 429 node = node->rb_left; 430 else if (reg > top_reg) 431 node = node->rb_right; 432 else 433 break; 434 } 435 436 if (rbnode) { 437 ret = regcache_rbtree_insert_to_block(map, rbnode, 438 new_base_reg, 439 new_top_reg, reg, 440 value); 441 if (ret) 442 return ret; 443 rbtree_ctx->cached_rbnode = rbnode; 444 return 0; 445 } 446 447 /* We did not manage to find a place to insert it in 448 * an existing block so create a new rbnode. 449 */ 450 rbnode = regcache_rbtree_node_alloc(map, reg); 451 if (!rbnode) 452 return -ENOMEM; 453 regcache_rbtree_set_register(map, rbnode, 454 (reg - rbnode->base_reg) / map->reg_stride, 455 value); 456 regcache_rbtree_insert(map, &rbtree_ctx->root, rbnode); 457 rbtree_ctx->cached_rbnode = rbnode; 458 } 459 460 return 0; 461 } 462 463 static int regcache_rbtree_sync(struct regmap *map, unsigned int min, 464 unsigned int max) 465 { 466 struct regcache_rbtree_ctx *rbtree_ctx; 467 struct rb_node *node; 468 struct regcache_rbtree_node *rbnode; 469 unsigned int base_reg, top_reg; 470 unsigned int start, end; 471 int ret; 472 473 map->async = true; 474 475 rbtree_ctx = map->cache; 476 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 if (base_reg > max) 482 break; 483 if (top_reg < min) 484 continue; 485 486 if (min > base_reg) 487 start = (min - base_reg) / map->reg_stride; 488 else 489 start = 0; 490 491 if (max < top_reg) 492 end = (max - base_reg) / map->reg_stride + 1; 493 else 494 end = rbnode->blklen; 495 496 ret = regcache_sync_block(map, rbnode->block, 497 rbnode->cache_present, 498 rbnode->base_reg, start, end); 499 if (ret != 0) 500 return ret; 501 } 502 503 map->async = false; 504 505 return regmap_async_complete(map); 506 } 507 508 static int regcache_rbtree_drop(struct regmap *map, unsigned int min, 509 unsigned int max) 510 { 511 struct regcache_rbtree_ctx *rbtree_ctx; 512 struct regcache_rbtree_node *rbnode; 513 struct rb_node *node; 514 unsigned int base_reg, top_reg; 515 unsigned int start, end; 516 517 rbtree_ctx = map->cache; 518 for (node = rb_first(&rbtree_ctx->root); node; node = rb_next(node)) { 519 rbnode = rb_entry(node, struct regcache_rbtree_node, node); 520 521 regcache_rbtree_get_base_top_reg(map, rbnode, &base_reg, 522 &top_reg); 523 if (base_reg > max) 524 break; 525 if (top_reg < min) 526 continue; 527 528 if (min > base_reg) 529 start = (min - base_reg) / map->reg_stride; 530 else 531 start = 0; 532 533 if (max < top_reg) 534 end = (max - base_reg) / map->reg_stride + 1; 535 else 536 end = rbnode->blklen; 537 538 bitmap_clear(rbnode->cache_present, start, end - start); 539 } 540 541 return 0; 542 } 543 544 struct regcache_ops regcache_rbtree_ops = { 545 .type = REGCACHE_RBTREE, 546 .name = "rbtree", 547 .init = regcache_rbtree_init, 548 .exit = regcache_rbtree_exit, 549 #ifdef CONFIG_DEBUG_FS 550 .debugfs_init = rbtree_debugfs_init, 551 #endif 552 .read = regcache_rbtree_read, 553 .write = regcache_rbtree_write, 554 .sync = regcache_rbtree_sync, 555 .drop = regcache_rbtree_drop, 556 }; 557