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 void 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 guard(regmap)(map); 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 return 0; 172 } 173 174 DEFINE_SHOW_ATTRIBUTE(rbtree); 175 176 static void rbtree_debugfs_init(struct regmap *map) 177 { 178 debugfs_create_file("rbtree", 0400, map->debugfs, map, &rbtree_fops); 179 } 180 #endif 181 182 static int regcache_rbtree_init(struct regmap *map) 183 { 184 struct regcache_rbtree_ctx *rbtree_ctx; 185 186 map->cache = kmalloc_obj(*rbtree_ctx, map->alloc_flags); 187 if (!map->cache) 188 return -ENOMEM; 189 190 rbtree_ctx = map->cache; 191 rbtree_ctx->root = RB_ROOT; 192 rbtree_ctx->cached_rbnode = NULL; 193 194 return 0; 195 } 196 197 static void regcache_rbtree_exit(struct regmap *map) 198 { 199 struct rb_node *next; 200 struct regcache_rbtree_ctx *rbtree_ctx; 201 struct regcache_rbtree_node *rbtree_node; 202 203 /* if we've already been called then just return */ 204 rbtree_ctx = map->cache; 205 if (!rbtree_ctx) 206 return; 207 208 /* free up the rbtree */ 209 next = rb_first(&rbtree_ctx->root); 210 while (next) { 211 rbtree_node = rb_entry(next, struct regcache_rbtree_node, node); 212 next = rb_next(&rbtree_node->node); 213 rb_erase(&rbtree_node->node, &rbtree_ctx->root); 214 kfree(rbtree_node->cache_present); 215 kfree(rbtree_node->block); 216 kfree(rbtree_node); 217 } 218 219 /* release the resources */ 220 kfree(map->cache); 221 map->cache = NULL; 222 } 223 224 static int regcache_rbtree_populate(struct regmap *map) 225 { 226 unsigned int i; 227 int ret; 228 229 for (i = 0; i < map->num_reg_defaults; i++) { 230 ret = regcache_rbtree_write(map, 231 map->reg_defaults[i].reg, 232 map->reg_defaults[i].def); 233 if (ret) 234 return ret; 235 } 236 237 return 0; 238 } 239 240 static int regcache_rbtree_read(struct regmap *map, 241 unsigned int reg, unsigned int *value) 242 { 243 struct regcache_rbtree_node *rbnode; 244 unsigned int reg_tmp; 245 246 rbnode = regcache_rbtree_lookup(map, reg); 247 if (rbnode) { 248 reg_tmp = (reg - rbnode->base_reg) / map->reg_stride; 249 if (!test_bit(reg_tmp, rbnode->cache_present)) 250 return -ENOENT; 251 *value = regcache_rbtree_get_register(map, rbnode, reg_tmp); 252 } else { 253 return -ENOENT; 254 } 255 256 return 0; 257 } 258 259 260 static int regcache_rbtree_insert_to_block(struct regmap *map, 261 struct regcache_rbtree_node *rbnode, 262 unsigned int base_reg, 263 unsigned int top_reg, 264 unsigned int reg, 265 unsigned int value) 266 { 267 unsigned int blklen; 268 unsigned int pos, offset; 269 unsigned long *present; 270 u8 *blk; 271 272 blklen = (top_reg - base_reg) / map->reg_stride + 1; 273 pos = (reg - base_reg) / map->reg_stride; 274 offset = (rbnode->base_reg - base_reg) / map->reg_stride; 275 276 blk = krealloc_array(rbnode->block, blklen, map->cache_word_size, map->alloc_flags); 277 if (!blk) 278 return -ENOMEM; 279 280 rbnode->block = blk; 281 282 if (BITS_TO_LONGS(blklen) > BITS_TO_LONGS(rbnode->blklen)) { 283 present = krealloc_array(rbnode->cache_present, 284 BITS_TO_LONGS(blklen), sizeof(*present), 285 map->alloc_flags); 286 if (!present) 287 return -ENOMEM; 288 289 memset(present + BITS_TO_LONGS(rbnode->blklen), 0, 290 (BITS_TO_LONGS(blklen) - BITS_TO_LONGS(rbnode->blklen)) 291 * sizeof(*present)); 292 } else { 293 present = rbnode->cache_present; 294 } 295 296 /* insert the register value in the correct place in the rbnode block */ 297 if (pos == 0) { 298 memmove(blk + offset * map->cache_word_size, 299 blk, rbnode->blklen * map->cache_word_size); 300 bitmap_shift_left(present, present, offset, blklen); 301 } 302 303 /* update the rbnode block, its size and the base register */ 304 rbnode->blklen = blklen; 305 rbnode->base_reg = base_reg; 306 rbnode->cache_present = present; 307 308 regcache_rbtree_set_register(map, rbnode, pos, value); 309 return 0; 310 } 311 312 static struct regcache_rbtree_node * 313 regcache_rbtree_node_alloc(struct regmap *map, unsigned int reg) 314 { 315 struct regcache_rbtree_node *rbnode; 316 const struct regmap_range *range; 317 int i; 318 319 rbnode = kzalloc_obj(*rbnode, map->alloc_flags); 320 if (!rbnode) 321 return NULL; 322 323 /* If there is a read table then use it to guess at an allocation */ 324 if (map->rd_table) { 325 for (i = 0; i < map->rd_table->n_yes_ranges; i++) { 326 if (regmap_reg_in_range(reg, 327 &map->rd_table->yes_ranges[i])) 328 break; 329 } 330 331 if (i != map->rd_table->n_yes_ranges) { 332 range = &map->rd_table->yes_ranges[i]; 333 rbnode->blklen = (range->range_max - range->range_min) / 334 map->reg_stride + 1; 335 rbnode->base_reg = range->range_min; 336 } 337 } 338 339 if (!rbnode->blklen) { 340 rbnode->blklen = 1; 341 rbnode->base_reg = reg; 342 } 343 344 rbnode->block = kmalloc_array(rbnode->blklen, map->cache_word_size, 345 map->alloc_flags); 346 if (!rbnode->block) 347 goto err_free; 348 349 rbnode->cache_present = kcalloc(BITS_TO_LONGS(rbnode->blklen), 350 sizeof(*rbnode->cache_present), 351 map->alloc_flags); 352 if (!rbnode->cache_present) 353 goto err_free_block; 354 355 return rbnode; 356 357 err_free_block: 358 kfree(rbnode->block); 359 err_free: 360 kfree(rbnode); 361 return NULL; 362 } 363 364 static int regcache_rbtree_write(struct regmap *map, unsigned int reg, 365 unsigned int value) 366 { 367 struct regcache_rbtree_ctx *rbtree_ctx; 368 struct regcache_rbtree_node *rbnode, *rbnode_tmp; 369 struct rb_node *node; 370 unsigned int reg_tmp; 371 int ret; 372 373 rbtree_ctx = map->cache; 374 375 /* if we can't locate it in the cached rbnode we'll have 376 * to traverse the rbtree looking for it. 377 */ 378 rbnode = regcache_rbtree_lookup(map, reg); 379 if (rbnode) { 380 reg_tmp = (reg - rbnode->base_reg) / map->reg_stride; 381 regcache_rbtree_set_register(map, rbnode, reg_tmp, value); 382 } else { 383 unsigned int base_reg, top_reg; 384 unsigned int new_base_reg, new_top_reg; 385 unsigned int min, max; 386 unsigned int max_dist; 387 unsigned int dist, best_dist = UINT_MAX; 388 389 max_dist = map->reg_stride * sizeof(*rbnode_tmp) / 390 map->cache_word_size; 391 if (reg < max_dist) 392 min = 0; 393 else 394 min = reg - max_dist; 395 max = reg + max_dist; 396 397 /* look for an adjacent register to the one we are about to add */ 398 node = rbtree_ctx->root.rb_node; 399 while (node) { 400 rbnode_tmp = rb_entry(node, struct regcache_rbtree_node, 401 node); 402 403 regcache_rbtree_get_base_top_reg(map, rbnode_tmp, 404 &base_reg, &top_reg); 405 406 if (base_reg <= max && top_reg >= min) { 407 if (reg < base_reg) 408 dist = base_reg - reg; 409 else if (reg > top_reg) 410 dist = reg - top_reg; 411 else 412 dist = 0; 413 if (dist < best_dist) { 414 rbnode = rbnode_tmp; 415 best_dist = dist; 416 new_base_reg = min(reg, base_reg); 417 new_top_reg = max(reg, top_reg); 418 } 419 } 420 421 /* 422 * Keep looking, we want to choose the closest block, 423 * otherwise we might end up creating overlapping 424 * blocks, which breaks the rbtree. 425 */ 426 if (reg < base_reg) 427 node = node->rb_left; 428 else if (reg > top_reg) 429 node = node->rb_right; 430 else 431 break; 432 } 433 434 if (rbnode) { 435 ret = regcache_rbtree_insert_to_block(map, rbnode, 436 new_base_reg, 437 new_top_reg, reg, 438 value); 439 if (ret) 440 return ret; 441 rbtree_ctx->cached_rbnode = rbnode; 442 return 0; 443 } 444 445 /* We did not manage to find a place to insert it in 446 * an existing block so create a new rbnode. 447 */ 448 rbnode = regcache_rbtree_node_alloc(map, reg); 449 if (!rbnode) 450 return -ENOMEM; 451 regcache_rbtree_set_register(map, rbnode, 452 (reg - rbnode->base_reg) / map->reg_stride, 453 value); 454 regcache_rbtree_insert(map, &rbtree_ctx->root, rbnode); 455 rbtree_ctx->cached_rbnode = rbnode; 456 } 457 458 return 0; 459 } 460 461 static int regcache_rbtree_sync(struct regmap *map, unsigned int min, 462 unsigned int max) 463 { 464 struct regcache_rbtree_ctx *rbtree_ctx; 465 struct rb_node *node; 466 struct regcache_rbtree_node *rbnode; 467 unsigned int base_reg, top_reg; 468 unsigned int start, end; 469 int ret; 470 471 map->async = true; 472 473 rbtree_ctx = map->cache; 474 for (node = rb_first(&rbtree_ctx->root); node; node = rb_next(node)) { 475 rbnode = rb_entry(node, struct regcache_rbtree_node, node); 476 477 regcache_rbtree_get_base_top_reg(map, rbnode, &base_reg, 478 &top_reg); 479 if (base_reg > max) 480 break; 481 if (top_reg < min) 482 continue; 483 484 if (min > base_reg) 485 start = (min - base_reg) / map->reg_stride; 486 else 487 start = 0; 488 489 if (max < top_reg) 490 end = (max - base_reg) / map->reg_stride + 1; 491 else 492 end = rbnode->blklen; 493 494 ret = regcache_sync_block(map, rbnode->block, 495 rbnode->cache_present, 496 rbnode->base_reg, start, end); 497 if (ret != 0) 498 return ret; 499 } 500 501 map->async = false; 502 503 return regmap_async_complete(map); 504 } 505 506 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 rbtree_ctx = map->cache; 516 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 if (base_reg > max) 522 break; 523 if (top_reg < min) 524 continue; 525 526 if (min > base_reg) 527 start = (min - base_reg) / map->reg_stride; 528 else 529 start = 0; 530 531 if (max < top_reg) 532 end = (max - base_reg) / map->reg_stride + 1; 533 else 534 end = rbnode->blklen; 535 536 bitmap_clear(rbnode->cache_present, start, end - start); 537 } 538 539 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 .populate = regcache_rbtree_populate, 548 #ifdef CONFIG_DEBUG_FS 549 .debugfs_init = rbtree_debugfs_init, 550 #endif 551 .read = regcache_rbtree_read, 552 .write = regcache_rbtree_write, 553 .sync = regcache_rbtree_sync, 554 .drop = regcache_rbtree_drop, 555 }; 556