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