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