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 /* the actual rbtree node holding this block */ 27 struct rb_node node; 28 /* base register handled by this block */ 29 unsigned int base_reg; 30 /* block of adjacent registers */ 31 void *block; 32 /* number of registers available in the block */ 33 unsigned int blklen; 34 } __attribute__ ((packed)); 35 36 struct regcache_rbtree_ctx { 37 struct rb_root root; 38 struct regcache_rbtree_node *cached_rbnode; 39 }; 40 41 static inline void regcache_rbtree_get_base_top_reg( 42 struct regmap *map, 43 struct regcache_rbtree_node *rbnode, 44 unsigned int *base, unsigned int *top) 45 { 46 *base = rbnode->base_reg; 47 *top = rbnode->base_reg + ((rbnode->blklen - 1) * map->reg_stride); 48 } 49 50 static unsigned int regcache_rbtree_get_register( 51 struct regcache_rbtree_node *rbnode, unsigned int idx, 52 unsigned int word_size) 53 { 54 return regcache_get_val(rbnode->block, idx, word_size); 55 } 56 57 static void regcache_rbtree_set_register(struct regcache_rbtree_node *rbnode, 58 unsigned int idx, unsigned int val, 59 unsigned int word_size) 60 { 61 regcache_set_val(rbnode->block, idx, val, word_size); 62 } 63 64 static struct regcache_rbtree_node *regcache_rbtree_lookup(struct regmap *map, 65 unsigned int reg) 66 { 67 struct regcache_rbtree_ctx *rbtree_ctx = map->cache; 68 struct rb_node *node; 69 struct regcache_rbtree_node *rbnode; 70 unsigned int base_reg, top_reg; 71 72 rbnode = rbtree_ctx->cached_rbnode; 73 if (rbnode) { 74 regcache_rbtree_get_base_top_reg(map, rbnode, &base_reg, 75 &top_reg); 76 if (reg >= base_reg && reg <= top_reg) 77 return rbnode; 78 } 79 80 node = rbtree_ctx->root.rb_node; 81 while (node) { 82 rbnode = container_of(node, struct regcache_rbtree_node, node); 83 regcache_rbtree_get_base_top_reg(map, rbnode, &base_reg, 84 &top_reg); 85 if (reg >= base_reg && reg <= top_reg) { 86 rbtree_ctx->cached_rbnode = rbnode; 87 return rbnode; 88 } else if (reg > top_reg) { 89 node = node->rb_right; 90 } else if (reg < base_reg) { 91 node = node->rb_left; 92 } 93 } 94 95 return NULL; 96 } 97 98 static int regcache_rbtree_insert(struct regmap *map, struct rb_root *root, 99 struct regcache_rbtree_node *rbnode) 100 { 101 struct rb_node **new, *parent; 102 struct regcache_rbtree_node *rbnode_tmp; 103 unsigned int base_reg_tmp, top_reg_tmp; 104 unsigned int base_reg; 105 106 parent = NULL; 107 new = &root->rb_node; 108 while (*new) { 109 rbnode_tmp = container_of(*new, struct regcache_rbtree_node, 110 node); 111 /* base and top registers of the current rbnode */ 112 regcache_rbtree_get_base_top_reg(map, rbnode_tmp, &base_reg_tmp, 113 &top_reg_tmp); 114 /* base register of the rbnode to be added */ 115 base_reg = rbnode->base_reg; 116 parent = *new; 117 /* if this register has already been inserted, just return */ 118 if (base_reg >= base_reg_tmp && 119 base_reg <= top_reg_tmp) 120 return 0; 121 else if (base_reg > top_reg_tmp) 122 new = &((*new)->rb_right); 123 else if (base_reg < base_reg_tmp) 124 new = &((*new)->rb_left); 125 } 126 127 /* insert the node into the rbtree */ 128 rb_link_node(&rbnode->node, parent, new); 129 rb_insert_color(&rbnode->node, root); 130 131 return 1; 132 } 133 134 #ifdef CONFIG_DEBUG_FS 135 static int rbtree_show(struct seq_file *s, void *ignored) 136 { 137 struct regmap *map = s->private; 138 struct regcache_rbtree_ctx *rbtree_ctx = map->cache; 139 struct regcache_rbtree_node *n; 140 struct rb_node *node; 141 unsigned int base, top; 142 int nodes = 0; 143 int registers = 0; 144 int this_registers, average; 145 146 map->lock(map); 147 148 for (node = rb_first(&rbtree_ctx->root); node != NULL; 149 node = rb_next(node)) { 150 n = container_of(node, struct regcache_rbtree_node, node); 151 152 regcache_rbtree_get_base_top_reg(map, n, &base, &top); 153 this_registers = ((top - base) / map->reg_stride) + 1; 154 seq_printf(s, "%x-%x (%d)\n", base, top, this_registers); 155 156 nodes++; 157 registers += this_registers; 158 } 159 160 if (nodes) 161 average = registers / nodes; 162 else 163 average = 0; 164 165 seq_printf(s, "%d nodes, %d registers, average %d registers\n", 166 nodes, registers, average); 167 168 map->unlock(map); 169 170 return 0; 171 } 172 173 static int rbtree_open(struct inode *inode, struct file *file) 174 { 175 return single_open(file, rbtree_show, inode->i_private); 176 } 177 178 static const struct file_operations rbtree_fops = { 179 .open = rbtree_open, 180 .read = seq_read, 181 .llseek = seq_lseek, 182 .release = single_release, 183 }; 184 185 static void rbtree_debugfs_init(struct regmap *map) 186 { 187 debugfs_create_file("rbtree", 0400, map->debugfs, map, &rbtree_fops); 188 } 189 #else 190 static void rbtree_debugfs_init(struct regmap *map) 191 { 192 } 193 #endif 194 195 static int regcache_rbtree_init(struct regmap *map) 196 { 197 struct regcache_rbtree_ctx *rbtree_ctx; 198 int i; 199 int ret; 200 201 map->cache = kmalloc(sizeof *rbtree_ctx, GFP_KERNEL); 202 if (!map->cache) 203 return -ENOMEM; 204 205 rbtree_ctx = map->cache; 206 rbtree_ctx->root = RB_ROOT; 207 rbtree_ctx->cached_rbnode = NULL; 208 209 for (i = 0; i < map->num_reg_defaults; i++) { 210 ret = regcache_rbtree_write(map, 211 map->reg_defaults[i].reg, 212 map->reg_defaults[i].def); 213 if (ret) 214 goto err; 215 } 216 217 rbtree_debugfs_init(map); 218 219 return 0; 220 221 err: 222 regcache_rbtree_exit(map); 223 return ret; 224 } 225 226 static int regcache_rbtree_exit(struct regmap *map) 227 { 228 struct rb_node *next; 229 struct regcache_rbtree_ctx *rbtree_ctx; 230 struct regcache_rbtree_node *rbtree_node; 231 232 /* if we've already been called then just return */ 233 rbtree_ctx = map->cache; 234 if (!rbtree_ctx) 235 return 0; 236 237 /* free up the rbtree */ 238 next = rb_first(&rbtree_ctx->root); 239 while (next) { 240 rbtree_node = rb_entry(next, struct regcache_rbtree_node, node); 241 next = rb_next(&rbtree_node->node); 242 rb_erase(&rbtree_node->node, &rbtree_ctx->root); 243 kfree(rbtree_node->block); 244 kfree(rbtree_node); 245 } 246 247 /* release the resources */ 248 kfree(map->cache); 249 map->cache = NULL; 250 251 return 0; 252 } 253 254 static int regcache_rbtree_read(struct regmap *map, 255 unsigned int reg, unsigned int *value) 256 { 257 struct regcache_rbtree_node *rbnode; 258 unsigned int reg_tmp; 259 260 rbnode = regcache_rbtree_lookup(map, reg); 261 if (rbnode) { 262 reg_tmp = (reg - rbnode->base_reg) / map->reg_stride; 263 *value = regcache_rbtree_get_register(rbnode, reg_tmp, 264 map->cache_word_size); 265 } else { 266 return -ENOENT; 267 } 268 269 return 0; 270 } 271 272 273 static int regcache_rbtree_insert_to_block(struct regcache_rbtree_node *rbnode, 274 unsigned int pos, unsigned int reg, 275 unsigned int value, unsigned int word_size) 276 { 277 u8 *blk; 278 279 blk = krealloc(rbnode->block, 280 (rbnode->blklen + 1) * word_size, GFP_KERNEL); 281 if (!blk) 282 return -ENOMEM; 283 284 /* insert the register value in the correct place in the rbnode block */ 285 memmove(blk + (pos + 1) * word_size, 286 blk + pos * word_size, 287 (rbnode->blklen - pos) * word_size); 288 289 /* update the rbnode block, its size and the base register */ 290 rbnode->block = blk; 291 rbnode->blklen++; 292 if (!pos) 293 rbnode->base_reg = reg; 294 295 regcache_rbtree_set_register(rbnode, pos, value, word_size); 296 return 0; 297 } 298 299 static int regcache_rbtree_write(struct regmap *map, unsigned int reg, 300 unsigned int value) 301 { 302 struct regcache_rbtree_ctx *rbtree_ctx; 303 struct regcache_rbtree_node *rbnode, *rbnode_tmp; 304 struct rb_node *node; 305 unsigned int val; 306 unsigned int reg_tmp; 307 unsigned int pos; 308 int i; 309 int ret; 310 311 rbtree_ctx = map->cache; 312 /* if we can't locate it in the cached rbnode we'll have 313 * to traverse the rbtree looking for it. 314 */ 315 rbnode = regcache_rbtree_lookup(map, reg); 316 if (rbnode) { 317 reg_tmp = (reg - rbnode->base_reg) / map->reg_stride; 318 val = regcache_rbtree_get_register(rbnode, reg_tmp, 319 map->cache_word_size); 320 if (val == value) 321 return 0; 322 regcache_rbtree_set_register(rbnode, reg_tmp, value, 323 map->cache_word_size); 324 } else { 325 /* look for an adjacent register to the one we are about to add */ 326 for (node = rb_first(&rbtree_ctx->root); node; 327 node = rb_next(node)) { 328 rbnode_tmp = rb_entry(node, struct regcache_rbtree_node, 329 node); 330 for (i = 0; i < rbnode_tmp->blklen; i++) { 331 reg_tmp = rbnode_tmp->base_reg + 332 (i * map->reg_stride); 333 if (abs(reg_tmp - reg) != map->reg_stride) 334 continue; 335 /* decide where in the block to place our register */ 336 if (reg_tmp + map->reg_stride == reg) 337 pos = i + 1; 338 else 339 pos = i; 340 ret = regcache_rbtree_insert_to_block(rbnode_tmp, pos, 341 reg, value, 342 map->cache_word_size); 343 if (ret) 344 return ret; 345 rbtree_ctx->cached_rbnode = rbnode_tmp; 346 return 0; 347 } 348 } 349 /* we did not manage to find a place to insert it in an existing 350 * block so create a new rbnode with a single register in its block. 351 * This block will get populated further if any other adjacent 352 * registers get modified in the future. 353 */ 354 rbnode = kzalloc(sizeof *rbnode, GFP_KERNEL); 355 if (!rbnode) 356 return -ENOMEM; 357 rbnode->blklen = 1; 358 rbnode->base_reg = reg; 359 rbnode->block = kmalloc(rbnode->blklen * map->cache_word_size, 360 GFP_KERNEL); 361 if (!rbnode->block) { 362 kfree(rbnode); 363 return -ENOMEM; 364 } 365 regcache_rbtree_set_register(rbnode, 0, value, map->cache_word_size); 366 regcache_rbtree_insert(map, &rbtree_ctx->root, rbnode); 367 rbtree_ctx->cached_rbnode = rbnode; 368 } 369 370 return 0; 371 } 372 373 static int regcache_rbtree_sync(struct regmap *map, unsigned int min, 374 unsigned int max) 375 { 376 struct regcache_rbtree_ctx *rbtree_ctx; 377 struct rb_node *node; 378 struct regcache_rbtree_node *rbnode; 379 unsigned int regtmp; 380 unsigned int val; 381 int ret; 382 int i, base, end; 383 384 rbtree_ctx = map->cache; 385 for (node = rb_first(&rbtree_ctx->root); node; node = rb_next(node)) { 386 rbnode = rb_entry(node, struct regcache_rbtree_node, node); 387 388 if (rbnode->base_reg < min) 389 continue; 390 if (rbnode->base_reg > max) 391 break; 392 if (rbnode->base_reg + rbnode->blklen < min) 393 continue; 394 395 if (min > rbnode->base_reg) 396 base = min - rbnode->base_reg; 397 else 398 base = 0; 399 400 if (max < rbnode->base_reg + rbnode->blklen) 401 end = rbnode->base_reg + rbnode->blklen - max; 402 else 403 end = rbnode->blklen; 404 405 for (i = base; i < end; i++) { 406 regtmp = rbnode->base_reg + (i * map->reg_stride); 407 val = regcache_rbtree_get_register(rbnode, i, 408 map->cache_word_size); 409 410 /* Is this the hardware default? If so skip. */ 411 ret = regcache_lookup_reg(map, regtmp); 412 if (ret >= 0 && val == map->reg_defaults[ret].def) 413 continue; 414 415 map->cache_bypass = 1; 416 ret = _regmap_write(map, regtmp, val); 417 map->cache_bypass = 0; 418 if (ret) 419 return ret; 420 dev_dbg(map->dev, "Synced register %#x, value %#x\n", 421 regtmp, val); 422 } 423 } 424 425 return 0; 426 } 427 428 struct regcache_ops regcache_rbtree_ops = { 429 .type = REGCACHE_RBTREE, 430 .name = "rbtree", 431 .init = regcache_rbtree_init, 432 .exit = regcache_rbtree_exit, 433 .read = regcache_rbtree_read, 434 .write = regcache_rbtree_write, 435 .sync = regcache_rbtree_sync 436 }; 437