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