Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 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> |
Paul Gortmaker | 51990e8 | 2012-01-22 11:23:42 -0500 | [diff] [blame] | 14 | #include <linux/device.h> |
Mark Brown | bad2ab4 | 2011-11-21 19:44:44 +0000 | [diff] [blame] | 15 | #include <linux/debugfs.h> |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 16 | #include <linux/rbtree.h> |
Mark Brown | bad2ab4 | 2011-11-21 19:44:44 +0000 | [diff] [blame] | 17 | #include <linux/seq_file.h> |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 18 | |
| 19 | #include "internal.h" |
| 20 | |
| 21 | static int regcache_rbtree_write(struct regmap *map, unsigned int reg, |
| 22 | unsigned int value); |
Lars-Peter Clausen | 462a185 | 2011-11-15 13:34:40 +0100 | [diff] [blame] | 23 | static int regcache_rbtree_exit(struct regmap *map); |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 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; |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 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; |
Dimitris Papastamos | 0c7ed856 | 2013-03-15 14:54:35 +0000 | [diff] [blame] | 39 | unsigned long *reg_present; |
| 40 | unsigned int reg_present_nbits; |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 41 | }; |
| 42 | |
| 43 | static inline void regcache_rbtree_get_base_top_reg( |
Stephen Warren | f01ee60 | 2012-04-09 13:40:24 -0600 | [diff] [blame] | 44 | struct regmap *map, |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 45 | struct regcache_rbtree_node *rbnode, |
| 46 | unsigned int *base, unsigned int *top) |
| 47 | { |
| 48 | *base = rbnode->base_reg; |
Stephen Warren | f01ee60 | 2012-04-09 13:40:24 -0600 | [diff] [blame] | 49 | *top = rbnode->base_reg + ((rbnode->blklen - 1) * map->reg_stride); |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 50 | } |
| 51 | |
Mark Brown | 879082c | 2013-02-21 18:03:13 +0000 | [diff] [blame] | 52 | static unsigned int regcache_rbtree_get_register(struct regmap *map, |
| 53 | struct regcache_rbtree_node *rbnode, unsigned int idx) |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 54 | { |
Mark Brown | 879082c | 2013-02-21 18:03:13 +0000 | [diff] [blame] | 55 | return regcache_get_val(map, rbnode->block, idx); |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 56 | } |
| 57 | |
Mark Brown | 879082c | 2013-02-21 18:03:13 +0000 | [diff] [blame] | 58 | static void regcache_rbtree_set_register(struct regmap *map, |
| 59 | struct regcache_rbtree_node *rbnode, |
| 60 | unsigned int idx, unsigned int val) |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 61 | { |
Mark Brown | 879082c | 2013-02-21 18:03:13 +0000 | [diff] [blame] | 62 | regcache_set_val(map, rbnode->block, idx, val); |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 63 | } |
| 64 | |
Lars-Peter Clausen | 3405add | 2011-09-27 20:15:38 +0200 | [diff] [blame] | 65 | static struct regcache_rbtree_node *regcache_rbtree_lookup(struct regmap *map, |
Mark Brown | 879082c | 2013-02-21 18:03:13 +0000 | [diff] [blame] | 66 | unsigned int reg) |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 67 | { |
Lars-Peter Clausen | 3405add | 2011-09-27 20:15:38 +0200 | [diff] [blame] | 68 | struct regcache_rbtree_ctx *rbtree_ctx = map->cache; |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 69 | struct rb_node *node; |
| 70 | struct regcache_rbtree_node *rbnode; |
| 71 | unsigned int base_reg, top_reg; |
| 72 | |
Lars-Peter Clausen | 3405add | 2011-09-27 20:15:38 +0200 | [diff] [blame] | 73 | rbnode = rbtree_ctx->cached_rbnode; |
| 74 | if (rbnode) { |
Stephen Warren | f01ee60 | 2012-04-09 13:40:24 -0600 | [diff] [blame] | 75 | regcache_rbtree_get_base_top_reg(map, rbnode, &base_reg, |
| 76 | &top_reg); |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 77 | if (reg >= base_reg && reg <= top_reg) |
| 78 | return rbnode; |
Lars-Peter Clausen | 3405add | 2011-09-27 20:15:38 +0200 | [diff] [blame] | 79 | } |
| 80 | |
| 81 | node = rbtree_ctx->root.rb_node; |
| 82 | while (node) { |
| 83 | rbnode = container_of(node, struct regcache_rbtree_node, node); |
Stephen Warren | f01ee60 | 2012-04-09 13:40:24 -0600 | [diff] [blame] | 84 | regcache_rbtree_get_base_top_reg(map, rbnode, &base_reg, |
| 85 | &top_reg); |
Lars-Peter Clausen | 3405add | 2011-09-27 20:15:38 +0200 | [diff] [blame] | 86 | if (reg >= base_reg && reg <= top_reg) { |
| 87 | rbtree_ctx->cached_rbnode = rbnode; |
| 88 | return rbnode; |
| 89 | } else if (reg > top_reg) { |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 90 | node = node->rb_right; |
Lars-Peter Clausen | 3405add | 2011-09-27 20:15:38 +0200 | [diff] [blame] | 91 | } else if (reg < base_reg) { |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 92 | node = node->rb_left; |
Lars-Peter Clausen | 3405add | 2011-09-27 20:15:38 +0200 | [diff] [blame] | 93 | } |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 94 | } |
| 95 | |
| 96 | return NULL; |
| 97 | } |
| 98 | |
Stephen Warren | f01ee60 | 2012-04-09 13:40:24 -0600 | [diff] [blame] | 99 | static int regcache_rbtree_insert(struct regmap *map, struct rb_root *root, |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 100 | struct regcache_rbtree_node *rbnode) |
| 101 | { |
| 102 | struct rb_node **new, *parent; |
| 103 | struct regcache_rbtree_node *rbnode_tmp; |
| 104 | unsigned int base_reg_tmp, top_reg_tmp; |
| 105 | unsigned int base_reg; |
| 106 | |
| 107 | parent = NULL; |
| 108 | new = &root->rb_node; |
| 109 | while (*new) { |
| 110 | rbnode_tmp = container_of(*new, struct regcache_rbtree_node, |
| 111 | node); |
| 112 | /* base and top registers of the current rbnode */ |
Stephen Warren | f01ee60 | 2012-04-09 13:40:24 -0600 | [diff] [blame] | 113 | regcache_rbtree_get_base_top_reg(map, rbnode_tmp, &base_reg_tmp, |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 114 | &top_reg_tmp); |
| 115 | /* base register of the rbnode to be added */ |
| 116 | base_reg = rbnode->base_reg; |
| 117 | parent = *new; |
| 118 | /* if this register has already been inserted, just return */ |
| 119 | if (base_reg >= base_reg_tmp && |
| 120 | base_reg <= top_reg_tmp) |
| 121 | return 0; |
| 122 | else if (base_reg > top_reg_tmp) |
| 123 | new = &((*new)->rb_right); |
| 124 | else if (base_reg < base_reg_tmp) |
| 125 | new = &((*new)->rb_left); |
| 126 | } |
| 127 | |
| 128 | /* insert the node into the rbtree */ |
| 129 | rb_link_node(&rbnode->node, parent, new); |
| 130 | rb_insert_color(&rbnode->node, root); |
| 131 | |
| 132 | return 1; |
| 133 | } |
| 134 | |
Mark Brown | bad2ab4 | 2011-11-21 19:44:44 +0000 | [diff] [blame] | 135 | #ifdef CONFIG_DEBUG_FS |
| 136 | static int rbtree_show(struct seq_file *s, void *ignored) |
| 137 | { |
| 138 | struct regmap *map = s->private; |
| 139 | struct regcache_rbtree_ctx *rbtree_ctx = map->cache; |
| 140 | struct regcache_rbtree_node *n; |
| 141 | struct rb_node *node; |
| 142 | unsigned int base, top; |
Dimitris Papastamos | a42277c | 2013-03-12 17:26:49 +0000 | [diff] [blame] | 143 | size_t mem_size; |
Mark Brown | bad2ab4 | 2011-11-21 19:44:44 +0000 | [diff] [blame] | 144 | int nodes = 0; |
| 145 | int registers = 0; |
Stephen Warren | f01ee60 | 2012-04-09 13:40:24 -0600 | [diff] [blame] | 146 | int this_registers, average; |
Mark Brown | bad2ab4 | 2011-11-21 19:44:44 +0000 | [diff] [blame] | 147 | |
Stephen Warren | bacdbe0 | 2012-04-04 15:48:28 -0600 | [diff] [blame] | 148 | map->lock(map); |
Mark Brown | bad2ab4 | 2011-11-21 19:44:44 +0000 | [diff] [blame] | 149 | |
Dimitris Papastamos | a42277c | 2013-03-12 17:26:49 +0000 | [diff] [blame] | 150 | mem_size = sizeof(*rbtree_ctx); |
Dimitris Papastamos | 0c7ed856 | 2013-03-15 14:54:35 +0000 | [diff] [blame] | 151 | mem_size += BITS_TO_LONGS(rbtree_ctx->reg_present_nbits) * sizeof(long); |
Dimitris Papastamos | a42277c | 2013-03-12 17:26:49 +0000 | [diff] [blame] | 152 | |
Mark Brown | bad2ab4 | 2011-11-21 19:44:44 +0000 | [diff] [blame] | 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); |
Dimitris Papastamos | a42277c | 2013-03-12 17:26:49 +0000 | [diff] [blame] | 156 | mem_size += sizeof(*n); |
| 157 | mem_size += (n->blklen * map->cache_word_size); |
Mark Brown | bad2ab4 | 2011-11-21 19:44:44 +0000 | [diff] [blame] | 158 | |
Stephen Warren | f01ee60 | 2012-04-09 13:40:24 -0600 | [diff] [blame] | 159 | regcache_rbtree_get_base_top_reg(map, n, &base, &top); |
| 160 | this_registers = ((top - base) / map->reg_stride) + 1; |
| 161 | seq_printf(s, "%x-%x (%d)\n", base, top, this_registers); |
Mark Brown | bad2ab4 | 2011-11-21 19:44:44 +0000 | [diff] [blame] | 162 | |
| 163 | nodes++; |
Stephen Warren | f01ee60 | 2012-04-09 13:40:24 -0600 | [diff] [blame] | 164 | registers += this_registers; |
Mark Brown | bad2ab4 | 2011-11-21 19:44:44 +0000 | [diff] [blame] | 165 | } |
| 166 | |
Stephen Warren | c04c1b9 | 2012-04-04 15:48:33 -0600 | [diff] [blame] | 167 | if (nodes) |
| 168 | average = registers / nodes; |
| 169 | else |
| 170 | average = 0; |
| 171 | |
Dimitris Papastamos | a42277c | 2013-03-12 17:26:49 +0000 | [diff] [blame] | 172 | seq_printf(s, "%d nodes, %d registers, average %d registers, used %zu bytes\n", |
| 173 | nodes, registers, average, mem_size); |
Mark Brown | bad2ab4 | 2011-11-21 19:44:44 +0000 | [diff] [blame] | 174 | |
Stephen Warren | bacdbe0 | 2012-04-04 15:48:28 -0600 | [diff] [blame] | 175 | map->unlock(map); |
Mark Brown | bad2ab4 | 2011-11-21 19:44:44 +0000 | [diff] [blame] | 176 | |
| 177 | return 0; |
| 178 | } |
| 179 | |
| 180 | static int rbtree_open(struct inode *inode, struct file *file) |
| 181 | { |
| 182 | return single_open(file, rbtree_show, inode->i_private); |
| 183 | } |
| 184 | |
| 185 | static const struct file_operations rbtree_fops = { |
| 186 | .open = rbtree_open, |
| 187 | .read = seq_read, |
| 188 | .llseek = seq_lseek, |
| 189 | .release = single_release, |
| 190 | }; |
Mark Brown | cce585c | 2011-11-22 11:33:31 +0000 | [diff] [blame] | 191 | |
| 192 | static void rbtree_debugfs_init(struct regmap *map) |
| 193 | { |
| 194 | debugfs_create_file("rbtree", 0400, map->debugfs, map, &rbtree_fops); |
| 195 | } |
| 196 | #else |
| 197 | static void rbtree_debugfs_init(struct regmap *map) |
| 198 | { |
| 199 | } |
Mark Brown | bad2ab4 | 2011-11-21 19:44:44 +0000 | [diff] [blame] | 200 | #endif |
| 201 | |
Dimitris Papastamos | 0c7ed856 | 2013-03-15 14:54:35 +0000 | [diff] [blame] | 202 | static int enlarge_reg_present_bitmap(struct regmap *map, unsigned int reg) |
| 203 | { |
| 204 | struct regcache_rbtree_ctx *rbtree_ctx; |
| 205 | unsigned long *reg_present; |
| 206 | unsigned int reg_present_size; |
| 207 | unsigned int nregs; |
| 208 | int i; |
| 209 | |
| 210 | rbtree_ctx = map->cache; |
| 211 | nregs = reg + 1; |
| 212 | reg_present_size = BITS_TO_LONGS(nregs); |
| 213 | reg_present_size *= sizeof(long); |
| 214 | |
| 215 | if (!rbtree_ctx->reg_present) { |
| 216 | reg_present = kmalloc(reg_present_size, GFP_KERNEL); |
| 217 | if (!reg_present) |
| 218 | return -ENOMEM; |
| 219 | bitmap_zero(reg_present, nregs); |
| 220 | rbtree_ctx->reg_present = reg_present; |
| 221 | rbtree_ctx->reg_present_nbits = nregs; |
| 222 | return 0; |
| 223 | } |
| 224 | |
| 225 | if (nregs > rbtree_ctx->reg_present_nbits) { |
| 226 | reg_present = krealloc(rbtree_ctx->reg_present, |
| 227 | reg_present_size, GFP_KERNEL); |
| 228 | if (!reg_present) |
| 229 | return -ENOMEM; |
| 230 | for (i = 0; i < nregs; i++) |
| 231 | if (i >= rbtree_ctx->reg_present_nbits) |
| 232 | clear_bit(i, reg_present); |
| 233 | rbtree_ctx->reg_present = reg_present; |
| 234 | rbtree_ctx->reg_present_nbits = nregs; |
| 235 | } |
| 236 | |
| 237 | return 0; |
| 238 | } |
| 239 | |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 240 | static int regcache_rbtree_init(struct regmap *map) |
| 241 | { |
| 242 | struct regcache_rbtree_ctx *rbtree_ctx; |
| 243 | int i; |
| 244 | int ret; |
| 245 | |
| 246 | map->cache = kmalloc(sizeof *rbtree_ctx, GFP_KERNEL); |
| 247 | if (!map->cache) |
| 248 | return -ENOMEM; |
| 249 | |
| 250 | rbtree_ctx = map->cache; |
| 251 | rbtree_ctx->root = RB_ROOT; |
| 252 | rbtree_ctx->cached_rbnode = NULL; |
Dimitris Papastamos | 0c7ed856 | 2013-03-15 14:54:35 +0000 | [diff] [blame] | 253 | rbtree_ctx->reg_present = NULL; |
| 254 | rbtree_ctx->reg_present_nbits = 0; |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 255 | |
| 256 | for (i = 0; i < map->num_reg_defaults; i++) { |
| 257 | ret = regcache_rbtree_write(map, |
| 258 | map->reg_defaults[i].reg, |
| 259 | map->reg_defaults[i].def); |
| 260 | if (ret) |
| 261 | goto err; |
| 262 | } |
| 263 | |
Mark Brown | cce585c | 2011-11-22 11:33:31 +0000 | [diff] [blame] | 264 | rbtree_debugfs_init(map); |
Mark Brown | bad2ab4 | 2011-11-21 19:44:44 +0000 | [diff] [blame] | 265 | |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 266 | return 0; |
| 267 | |
| 268 | err: |
Lars-Peter Clausen | 462a185 | 2011-11-15 13:34:40 +0100 | [diff] [blame] | 269 | regcache_rbtree_exit(map); |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 270 | return ret; |
| 271 | } |
| 272 | |
| 273 | static int regcache_rbtree_exit(struct regmap *map) |
| 274 | { |
| 275 | struct rb_node *next; |
| 276 | struct regcache_rbtree_ctx *rbtree_ctx; |
| 277 | struct regcache_rbtree_node *rbtree_node; |
| 278 | |
| 279 | /* if we've already been called then just return */ |
| 280 | rbtree_ctx = map->cache; |
| 281 | if (!rbtree_ctx) |
| 282 | return 0; |
| 283 | |
Dimitris Papastamos | 0c7ed856 | 2013-03-15 14:54:35 +0000 | [diff] [blame] | 284 | kfree(rbtree_ctx->reg_present); |
| 285 | |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 286 | /* free up the rbtree */ |
| 287 | next = rb_first(&rbtree_ctx->root); |
| 288 | while (next) { |
| 289 | rbtree_node = rb_entry(next, struct regcache_rbtree_node, node); |
| 290 | next = rb_next(&rbtree_node->node); |
| 291 | rb_erase(&rbtree_node->node, &rbtree_ctx->root); |
| 292 | kfree(rbtree_node->block); |
| 293 | kfree(rbtree_node); |
| 294 | } |
| 295 | |
| 296 | /* release the resources */ |
| 297 | kfree(map->cache); |
| 298 | map->cache = NULL; |
| 299 | |
| 300 | return 0; |
| 301 | } |
| 302 | |
Dimitris Papastamos | 0c7ed856 | 2013-03-15 14:54:35 +0000 | [diff] [blame] | 303 | static int regcache_reg_present(struct regmap *map, unsigned int reg) |
| 304 | { |
| 305 | struct regcache_rbtree_ctx *rbtree_ctx; |
| 306 | |
| 307 | rbtree_ctx = map->cache; |
| 308 | if (!(rbtree_ctx->reg_present[BIT_WORD(reg)] & BIT_MASK(reg))) |
| 309 | return 0; |
| 310 | return 1; |
| 311 | |
| 312 | } |
| 313 | |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 314 | static int regcache_rbtree_read(struct regmap *map, |
| 315 | unsigned int reg, unsigned int *value) |
| 316 | { |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 317 | struct regcache_rbtree_node *rbnode; |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 318 | unsigned int reg_tmp; |
| 319 | |
Lars-Peter Clausen | 3405add | 2011-09-27 20:15:38 +0200 | [diff] [blame] | 320 | rbnode = regcache_rbtree_lookup(map, reg); |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 321 | if (rbnode) { |
Stephen Warren | f01ee60 | 2012-04-09 13:40:24 -0600 | [diff] [blame] | 322 | reg_tmp = (reg - rbnode->base_reg) / map->reg_stride; |
Dimitris Papastamos | 0c7ed856 | 2013-03-15 14:54:35 +0000 | [diff] [blame] | 323 | if (!regcache_reg_present(map, reg)) |
| 324 | return -ENOENT; |
Mark Brown | 879082c | 2013-02-21 18:03:13 +0000 | [diff] [blame] | 325 | *value = regcache_rbtree_get_register(map, rbnode, reg_tmp); |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 326 | } else { |
Mark Brown | 6e6ace0 | 2011-10-09 13:23:31 +0100 | [diff] [blame] | 327 | return -ENOENT; |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 328 | } |
| 329 | |
| 330 | return 0; |
| 331 | } |
| 332 | |
| 333 | |
Mark Brown | 879082c | 2013-02-21 18:03:13 +0000 | [diff] [blame] | 334 | static int regcache_rbtree_insert_to_block(struct regmap *map, |
| 335 | struct regcache_rbtree_node *rbnode, |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 336 | unsigned int pos, unsigned int reg, |
Mark Brown | 879082c | 2013-02-21 18:03:13 +0000 | [diff] [blame] | 337 | unsigned int value) |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 338 | { |
| 339 | u8 *blk; |
| 340 | |
| 341 | blk = krealloc(rbnode->block, |
Mark Brown | 879082c | 2013-02-21 18:03:13 +0000 | [diff] [blame] | 342 | (rbnode->blklen + 1) * map->cache_word_size, |
| 343 | GFP_KERNEL); |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 344 | if (!blk) |
| 345 | return -ENOMEM; |
| 346 | |
| 347 | /* insert the register value in the correct place in the rbnode block */ |
Mark Brown | 879082c | 2013-02-21 18:03:13 +0000 | [diff] [blame] | 348 | memmove(blk + (pos + 1) * map->cache_word_size, |
| 349 | blk + pos * map->cache_word_size, |
| 350 | (rbnode->blklen - pos) * map->cache_word_size); |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 351 | |
| 352 | /* update the rbnode block, its size and the base register */ |
| 353 | rbnode->block = blk; |
| 354 | rbnode->blklen++; |
| 355 | if (!pos) |
| 356 | rbnode->base_reg = reg; |
| 357 | |
Mark Brown | 879082c | 2013-02-21 18:03:13 +0000 | [diff] [blame] | 358 | regcache_rbtree_set_register(map, rbnode, pos, value); |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 359 | return 0; |
| 360 | } |
| 361 | |
| 362 | static int regcache_rbtree_write(struct regmap *map, unsigned int reg, |
| 363 | unsigned int value) |
| 364 | { |
| 365 | struct regcache_rbtree_ctx *rbtree_ctx; |
| 366 | struct regcache_rbtree_node *rbnode, *rbnode_tmp; |
| 367 | struct rb_node *node; |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 368 | unsigned int reg_tmp; |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 369 | unsigned int pos; |
| 370 | int i; |
| 371 | int ret; |
| 372 | |
| 373 | rbtree_ctx = map->cache; |
Dimitris Papastamos | 0c7ed856 | 2013-03-15 14:54:35 +0000 | [diff] [blame] | 374 | /* update the reg_present bitmap, make space if necessary */ |
| 375 | ret = enlarge_reg_present_bitmap(map, reg); |
| 376 | if (ret < 0) |
| 377 | return ret; |
| 378 | set_bit(reg, rbtree_ctx->reg_present); |
| 379 | |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 380 | /* if we can't locate it in the cached rbnode we'll have |
| 381 | * to traverse the rbtree looking for it. |
| 382 | */ |
Lars-Peter Clausen | 3405add | 2011-09-27 20:15:38 +0200 | [diff] [blame] | 383 | rbnode = regcache_rbtree_lookup(map, reg); |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 384 | if (rbnode) { |
Stephen Warren | f01ee60 | 2012-04-09 13:40:24 -0600 | [diff] [blame] | 385 | reg_tmp = (reg - rbnode->base_reg) / map->reg_stride; |
Mark Brown | 879082c | 2013-02-21 18:03:13 +0000 | [diff] [blame] | 386 | regcache_rbtree_set_register(map, rbnode, reg_tmp, value); |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 387 | } else { |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 388 | /* look for an adjacent register to the one we are about to add */ |
| 389 | for (node = rb_first(&rbtree_ctx->root); node; |
| 390 | node = rb_next(node)) { |
Stephen Warren | f01ee60 | 2012-04-09 13:40:24 -0600 | [diff] [blame] | 391 | rbnode_tmp = rb_entry(node, struct regcache_rbtree_node, |
| 392 | node); |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 393 | for (i = 0; i < rbnode_tmp->blklen; i++) { |
Stephen Warren | f01ee60 | 2012-04-09 13:40:24 -0600 | [diff] [blame] | 394 | reg_tmp = rbnode_tmp->base_reg + |
| 395 | (i * map->reg_stride); |
| 396 | if (abs(reg_tmp - reg) != map->reg_stride) |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 397 | continue; |
| 398 | /* decide where in the block to place our register */ |
Stephen Warren | f01ee60 | 2012-04-09 13:40:24 -0600 | [diff] [blame] | 399 | if (reg_tmp + map->reg_stride == reg) |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 400 | pos = i + 1; |
| 401 | else |
| 402 | pos = i; |
Mark Brown | 879082c | 2013-02-21 18:03:13 +0000 | [diff] [blame] | 403 | ret = regcache_rbtree_insert_to_block(map, |
| 404 | rbnode_tmp, |
| 405 | pos, reg, |
| 406 | value); |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 407 | if (ret) |
| 408 | return ret; |
| 409 | rbtree_ctx->cached_rbnode = rbnode_tmp; |
| 410 | return 0; |
| 411 | } |
| 412 | } |
| 413 | /* we did not manage to find a place to insert it in an existing |
| 414 | * block so create a new rbnode with a single register in its block. |
| 415 | * This block will get populated further if any other adjacent |
| 416 | * registers get modified in the future. |
| 417 | */ |
| 418 | rbnode = kzalloc(sizeof *rbnode, GFP_KERNEL); |
| 419 | if (!rbnode) |
| 420 | return -ENOMEM; |
Dimitris Papastamos | 0c7ed856 | 2013-03-15 14:54:35 +0000 | [diff] [blame] | 421 | rbnode->blklen = sizeof(*rbnode); |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 422 | rbnode->base_reg = reg; |
Dimitris Papastamos | 25ed115 | 2011-09-27 11:25:07 +0100 | [diff] [blame] | 423 | rbnode->block = kmalloc(rbnode->blklen * map->cache_word_size, |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 424 | GFP_KERNEL); |
| 425 | if (!rbnode->block) { |
| 426 | kfree(rbnode); |
| 427 | return -ENOMEM; |
| 428 | } |
Mark Brown | 879082c | 2013-02-21 18:03:13 +0000 | [diff] [blame] | 429 | regcache_rbtree_set_register(map, rbnode, 0, value); |
Stephen Warren | f01ee60 | 2012-04-09 13:40:24 -0600 | [diff] [blame] | 430 | regcache_rbtree_insert(map, &rbtree_ctx->root, rbnode); |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 431 | rbtree_ctx->cached_rbnode = rbnode; |
| 432 | } |
| 433 | |
| 434 | return 0; |
| 435 | } |
| 436 | |
Mark Brown | ac8d91c | 2012-02-23 19:31:04 +0000 | [diff] [blame] | 437 | static int regcache_rbtree_sync(struct regmap *map, unsigned int min, |
| 438 | unsigned int max) |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 439 | { |
| 440 | struct regcache_rbtree_ctx *rbtree_ctx; |
| 441 | struct rb_node *node; |
| 442 | struct regcache_rbtree_node *rbnode; |
| 443 | unsigned int regtmp; |
Mark Brown | b03622a | 2011-10-09 12:54:25 +0100 | [diff] [blame] | 444 | unsigned int val; |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 445 | int ret; |
Mark Brown | ac8d91c | 2012-02-23 19:31:04 +0000 | [diff] [blame] | 446 | int i, base, end; |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 447 | |
| 448 | rbtree_ctx = map->cache; |
| 449 | for (node = rb_first(&rbtree_ctx->root); node; node = rb_next(node)) { |
| 450 | rbnode = rb_entry(node, struct regcache_rbtree_node, node); |
Mark Brown | ac8d91c | 2012-02-23 19:31:04 +0000 | [diff] [blame] | 451 | |
| 452 | if (rbnode->base_reg < min) |
| 453 | continue; |
| 454 | if (rbnode->base_reg > max) |
| 455 | break; |
| 456 | if (rbnode->base_reg + rbnode->blklen < min) |
| 457 | continue; |
| 458 | |
Mark Brown | f9353e7 | 2012-03-05 23:28:49 +0000 | [diff] [blame] | 459 | if (min > rbnode->base_reg) |
Mark Brown | ac8d91c | 2012-02-23 19:31:04 +0000 | [diff] [blame] | 460 | base = min - rbnode->base_reg; |
| 461 | else |
| 462 | base = 0; |
| 463 | |
| 464 | if (max < rbnode->base_reg + rbnode->blklen) |
| 465 | end = rbnode->base_reg + rbnode->blklen - max; |
| 466 | else |
| 467 | end = rbnode->blklen; |
| 468 | |
| 469 | for (i = base; i < end; i++) { |
Stephen Warren | f01ee60 | 2012-04-09 13:40:24 -0600 | [diff] [blame] | 470 | regtmp = rbnode->base_reg + (i * map->reg_stride); |
Dimitris Papastamos | 0c7ed856 | 2013-03-15 14:54:35 +0000 | [diff] [blame] | 471 | |
| 472 | if (!regcache_reg_present(map, regtmp)) |
| 473 | continue; |
| 474 | |
Mark Brown | 879082c | 2013-02-21 18:03:13 +0000 | [diff] [blame] | 475 | val = regcache_rbtree_get_register(map, rbnode, i); |
Mark Brown | b03622a | 2011-10-09 12:54:25 +0100 | [diff] [blame] | 476 | |
| 477 | /* Is this the hardware default? If so skip. */ |
Lars-Peter Clausen | 4b4e9e4 | 2012-03-23 11:04:57 +0100 | [diff] [blame] | 478 | ret = regcache_lookup_reg(map, regtmp); |
Mark Brown | 994f5db | 2012-03-05 23:31:39 +0000 | [diff] [blame] | 479 | if (ret >= 0 && val == map->reg_defaults[ret].def) |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 480 | continue; |
Mark Brown | b03622a | 2011-10-09 12:54:25 +0100 | [diff] [blame] | 481 | |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 482 | map->cache_bypass = 1; |
Dimitris Papastamos | 13753a9 | 2011-09-29 14:36:25 +0100 | [diff] [blame] | 483 | ret = _regmap_write(map, regtmp, val); |
Dimitris Papastamos | 28644c80 | 2011-09-19 14:34:02 +0100 | [diff] [blame] | 484 | map->cache_bypass = 0; |
| 485 | if (ret) |
| 486 | return ret; |
| 487 | dev_dbg(map->dev, "Synced register %#x, value %#x\n", |
| 488 | regtmp, val); |
| 489 | } |
| 490 | } |
| 491 | |
| 492 | return 0; |
| 493 | } |
| 494 | |
| 495 | struct regcache_ops regcache_rbtree_ops = { |
| 496 | .type = REGCACHE_RBTREE, |
| 497 | .name = "rbtree", |
| 498 | .init = regcache_rbtree_init, |
| 499 | .exit = regcache_rbtree_exit, |
| 500 | .read = regcache_rbtree_read, |
| 501 | .write = regcache_rbtree_write, |
| 502 | .sync = regcache_rbtree_sync |
| 503 | }; |