drivers/base/regmap/regcache-rbtree.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 //
   3 // Register cache access API - rbtree caching support
   4 //
   5 // Copyright 2011 Wolfson Microelectronics plc
   6 //
   7 // Author: Dimitris Papastamos <dp@opensource.wolfsonmicro.com>
   8
   9 #include <linux/debugfs.h>
  10 #include <linux/device.h>
  11 #include <linux/rbtree.h>
  12 #include <linux/seq_file.h>
  13 #include <linux/slab.h>
  14
  15 #include "internal.h"
  16
  17 static int regcache_rbtree_write(struct regmap *map, unsigned int reg,
  18                                  unsigned int value);
  19 static int regcache_rbtree_exit(struct regmap *map);
  20
  21 struct regcache_rbtree_node {
  22         /* block of adjacent registers */
  23         void *block;
  24         /* Which registers are present */
  25         long *cache_present;
  26         /* base register handled by this block */
  27         unsigned int base_reg;
  28         /* number of registers available in the block */
  29         unsigned int blklen;
  30         /* the actual rbtree node holding this block */
  31         struct rb_node node;
  32 };
  33
  34 struct regcache_rbtree_ctx {
  35         struct rb_root root;
  36         struct regcache_rbtree_node *cached_rbnode;
  37 };
  38
  39 static inline void regcache_rbtree_get_base_top_reg(
  40         struct regmap *map,
  41         struct regcache_rbtree_node *rbnode,
  42         unsigned int *base, unsigned int *top)
  43 {
  44         *base = rbnode->base_reg;
  45         *top = rbnode->base_reg + ((rbnode->blklen - 1) * map->reg_stride);
  46 }
  47
  48 static unsigned int regcache_rbtree_get_register(struct regmap *map,
  49         struct regcache_rbtree_node *rbnode, unsigned int idx)
  50 {
  51         return regcache_get_val(map, rbnode->block, idx);
  52 }
  53
  54 static void regcache_rbtree_set_register(struct regmap *map,
  55                                          struct regcache_rbtree_node *rbnode,
  56                                          unsigned int idx, unsigned int val)
  57 {
  58         set_bit(idx, rbnode->cache_present);
  59         regcache_set_val(map, rbnode->block, idx, val);
  60 }
  61
  62 static struct regcache_rbtree_node *regcache_rbtree_lookup(struct regmap *map,
  63                                                            unsigned int reg)
  64 {
  65         struct regcache_rbtree_ctx *rbtree_ctx = map->cache;
  66         struct rb_node *node;
  67         struct regcache_rbtree_node *rbnode;
  68         unsigned int base_reg, top_reg;
  69
  70         rbnode = rbtree_ctx->cached_rbnode;
  71         if (rbnode) {
  72                 regcache_rbtree_get_base_top_reg(map, rbnode, &base_reg,
  73                                                  &top_reg);
  74                 if (reg >= base_reg && reg <= top_reg)
  75                         return rbnode;
  76         }
  77
  78         node = rbtree_ctx->root.rb_node;
  79         while (node) {
  80                 rbnode = rb_entry(node, struct regcache_rbtree_node, node);
  81                 regcache_rbtree_get_base_top_reg(map, rbnode, &base_reg,
  82                                                  &top_reg);
  83                 if (reg >= base_reg && reg <= top_reg) {
  84                         rbtree_ctx->cached_rbnode = rbnode;
  85                         return rbnode;
  86                 } else if (reg > top_reg) {
  87                         node = node->rb_right;
  88                 } else if (reg < base_reg) {
  89                         node = node->rb_left;
  90                 }
  91         }
  92
  93         return NULL;
  94 }
  95
  96 static int regcache_rbtree_insert(struct regmap *map, struct rb_root *root,
  97                                   struct regcache_rbtree_node *rbnode)
  98 {
  99         struct rb_node **new, *parent;
 100         struct regcache_rbtree_node *rbnode_tmp;
 101         unsigned int base_reg_tmp, top_reg_tmp;
 102         unsigned int base_reg;
 103
 104         parent = NULL;
 105         new = &root->rb_node;
 106         while (*new) {
 107                 rbnode_tmp = rb_entry(*new, struct regcache_rbtree_node, node);
 108                 /* base and top registers of the current rbnode */
 109                 regcache_rbtree_get_base_top_reg(map, rbnode_tmp, &base_reg_tmp,
 110                                                  &top_reg_tmp);
 111                 /* base register of the rbnode to be added */
 112                 base_reg = rbnode->base_reg;
 113                 parent = *new;
 114                 /* if this register has already been inserted, just return */
 115                 if (base_reg >= base_reg_tmp &&
 116                     base_reg <= top_reg_tmp)
 117                         return 0;
 118                 else if (base_reg > top_reg_tmp)
 119                         new = &((*new)->rb_right);
 120                 else if (base_reg < base_reg_tmp)
 121                         new = &((*new)->rb_left);
 122         }
 123
 124         /* insert the node into the rbtree */
 125         rb_link_node(&rbnode->node, parent, new);
 126         rb_insert_color(&rbnode->node, root);
 127
 128         return 1;
 129 }
 130
 131 #ifdef CONFIG_DEBUG_FS
 132 static int rbtree_show(struct seq_file *s, void *ignored)
 133 {
 134         struct regmap *map = s->private;
 135         struct regcache_rbtree_ctx *rbtree_ctx = map->cache;
 136         struct regcache_rbtree_node *n;
 137         struct rb_node *node;
 138         unsigned int base, top;
 139         size_t mem_size;
 140         int nodes = 0;
 141         int registers = 0;
 142         int this_registers, average;
 143
 144         map->lock(map->lock_arg);
 145
 146         mem_size = sizeof(*rbtree_ctx);
 147
 148         for (node = rb_first(&rbtree_ctx->root); node != NULL;
 149              node = rb_next(node)) {
 150                 n = rb_entry(node, struct regcache_rbtree_node, node);
 151                 mem_size += sizeof(*n);
 152                 mem_size += (n->blklen * map->cache_word_size);
 153                 mem_size += BITS_TO_LONGS(n->blklen) * sizeof(long);
 154
 155                 regcache_rbtree_get_base_top_reg(map, n, &base, &top);
 156                 this_registers = ((top - base) / map->reg_stride) + 1;
 157                 seq_printf(s, "%x-%x (%d)\n", base, top, this_registers);
 158
 159                 nodes++;
 160                 registers += this_registers;
 161         }
 162
 163         if (nodes)
 164                 average = registers / nodes;
 165         else
 166                 average = 0;
 167
 168         seq_printf(s, "%d nodes, %d registers, average %d registers, used %zu bytes\n",
 169                    nodes, registers, average, mem_size);
 170
 171         map->unlock(map->lock_arg);
 172
 173         return 0;
 174 }
 175
 176 DEFINE_SHOW_ATTRIBUTE(rbtree);
 177
 178 static void rbtree_debugfs_init(struct regmap *map)
 179 {
 180         debugfs_create_file("rbtree", 0400, map->debugfs, map, &rbtree_fops);
 181 }
 182 #endif
 183
 184 static int regcache_rbtree_init(struct regmap *map)
 185 {
 186         struct regcache_rbtree_ctx *rbtree_ctx;
 187         int i;
 188         int ret;
 189
 190         map->cache = kmalloc(sizeof *rbtree_ctx, GFP_KERNEL);
 191         if (!map->cache)
 192                 return -ENOMEM;
 193
 194         rbtree_ctx = map->cache;
 195         rbtree_ctx->root = RB_ROOT;
 196         rbtree_ctx->cached_rbnode = NULL;
 197
 198         for (i = 0; i < map->num_reg_defaults; i++) {
 199                 ret = regcache_rbtree_write(map,
 200                                             map->reg_defaults[i].reg,
 201                                             map->reg_defaults[i].def);
 202                 if (ret)
 203                         goto err;
 204         }
 205
 206         return 0;
 207
 208 err:
 209         regcache_rbtree_exit(map);
 210         return ret;
 211 }
 212
 213 static int regcache_rbtree_exit(struct regmap *map)
 214 {
 215         struct rb_node *next;
 216         struct regcache_rbtree_ctx *rbtree_ctx;
 217         struct regcache_rbtree_node *rbtree_node;
 218
 219         /* if we've already been called then just return */
 220         rbtree_ctx = map->cache;
 221         if (!rbtree_ctx)
 222                 return 0;
 223
 224         /* free up the rbtree */
 225         next = rb_first(&rbtree_ctx->root);
 226         while (next) {
 227                 rbtree_node = rb_entry(next, struct regcache_rbtree_node, node);
 228                 next = rb_next(&rbtree_node->node);
 229                 rb_erase(&rbtree_node->node, &rbtree_ctx->root);
 230                 kfree(rbtree_node->cache_present);
 231                 kfree(rbtree_node->block);
 232                 kfree(rbtree_node);
 233         }
 234
 235         /* release the resources */
 236         kfree(map->cache);
 237         map->cache = NULL;
 238
 239         return 0;
 240 }
 241
 242 static int regcache_rbtree_read(struct regmap *map,
 243                                 unsigned int reg, unsigned int *value)
 244 {
 245         struct regcache_rbtree_node *rbnode;
 246         unsigned int reg_tmp;
 247
 248         rbnode = regcache_rbtree_lookup(map, reg);
 249         if (rbnode) {
 250                 reg_tmp = (reg - rbnode->base_reg) / map->reg_stride;
 251                 if (!test_bit(reg_tmp, rbnode->cache_present))
 252                         return -ENOENT;
 253                 *value = regcache_rbtree_get_register(map, rbnode, reg_tmp);
 254         } else {
 255                 return -ENOENT;
 256         }
 257
 258         return 0;
 259 }
 260
 261
 262 static int regcache_rbtree_insert_to_block(struct regmap *map,
 263                                            struct regcache_rbtree_node *rbnode,
 264                                            unsigned int base_reg,
 265                                            unsigned int top_reg,
 266                                            unsigned int reg,
 267                                            unsigned int value)
 268 {
 269         unsigned int blklen;
 270         unsigned int pos, offset;
 271         unsigned long *present;
 272         u8 *blk;
 273
 274         blklen = (top_reg - base_reg) / map->reg_stride + 1;
 275         pos = (reg - base_reg) / map->reg_stride;
 276         offset = (rbnode->base_reg - base_reg) / map->reg_stride;
 277
 278         blk = krealloc(rbnode->block,
 279                        blklen * map->cache_word_size,
 280                        GFP_KERNEL);
 281         if (!blk)
 282                 return -ENOMEM;
 283
 284         rbnode->block = blk;
 285
 286         if (BITS_TO_LONGS(blklen) > BITS_TO_LONGS(rbnode->blklen)) {
 287                 present = krealloc(rbnode->cache_present,
 288                                    BITS_TO_LONGS(blklen) * sizeof(*present),
 289                                    GFP_KERNEL);
 290                 if (!present)
 291                         return -ENOMEM;
 292
 293                 memset(present + BITS_TO_LONGS(rbnode->blklen), 0,
 294                        (BITS_TO_LONGS(blklen) - BITS_TO_LONGS(rbnode->blklen))
 295                        * sizeof(*present));
 296         } else {
 297                 present = rbnode->cache_present;
 298         }
 299
 300         /* insert the register value in the correct place in the rbnode block */
 301         if (pos == 0) {
 302                 memmove(blk + offset * map->cache_word_size,
 303                         blk, rbnode->blklen * map->cache_word_size);
 304                 bitmap_shift_left(present, present, offset, blklen);
 305         }
 306
 307         /* update the rbnode block, its size and the base register */
 308         rbnode->blklen = blklen;
 309         rbnode->base_reg = base_reg;
 310         rbnode->cache_present = present;
 311
 312         regcache_rbtree_set_register(map, rbnode, pos, value);
 313         return 0;
 314 }
 315
 316 static struct regcache_rbtree_node *
 317 regcache_rbtree_node_alloc(struct regmap *map, unsigned int reg)
 318 {
 319         struct regcache_rbtree_node *rbnode;
 320         const struct regmap_range *range;
 321         int i;
 322
 323         rbnode = kzalloc(sizeof(*rbnode), GFP_KERNEL);
 324         if (!rbnode)
 325                 return NULL;
 326
 327         /* If there is a read table then use it to guess at an allocation */
 328         if (map->rd_table) {
 329                 for (i = 0; i < map->rd_table->n_yes_ranges; i++) {
 330                         if (regmap_reg_in_range(reg,
 331                                                 &map->rd_table->yes_ranges[i]))
 332                                 break;
 333                 }
 334
 335                 if (i != map->rd_table->n_yes_ranges) {
 336                         range = &map->rd_table->yes_ranges[i];
 337                         rbnode->blklen = (range->range_max - range->range_min) /
 338                                 map->reg_stride + 1;
 339                         rbnode->base_reg = range->range_min;
 340                 }
 341         }
 342
 343         if (!rbnode->blklen) {
 344                 rbnode->blklen = 1;
 345                 rbnode->base_reg = reg;
 346         }
 347
 348         rbnode->block = kmalloc_array(rbnode->blklen, map->cache_word_size,
 349                                       GFP_KERNEL);
 350         if (!rbnode->block)
 351                 goto err_free;
 352
 353         rbnode->cache_present = kcalloc(BITS_TO_LONGS(rbnode->blklen),
 354                                         sizeof(*rbnode->cache_present),
 355                                         GFP_KERNEL);
 356         if (!rbnode->cache_present)
 357                 goto err_free_block;
 358
 359         return rbnode;
 360
 361 err_free_block:
 362         kfree(rbnode->block);
 363 err_free:
 364         kfree(rbnode);
 365         return NULL;
 366 }
 367
 368 static int regcache_rbtree_write(struct regmap *map, unsigned int reg,
 369                                  unsigned int value)
 370 {
 371         struct regcache_rbtree_ctx *rbtree_ctx;
 372         struct regcache_rbtree_node *rbnode, *rbnode_tmp;
 373         struct rb_node *node;
 374         unsigned int reg_tmp;
 375         int ret;
 376
 377         rbtree_ctx = map->cache;
 378
 379         /* if we can't locate it in the cached rbnode we'll have
 380          * to traverse the rbtree looking for it.
 381          */
 382         rbnode = regcache_rbtree_lookup(map, reg);
 383         if (rbnode) {
 384                 reg_tmp = (reg - rbnode->base_reg) / map->reg_stride;
 385                 regcache_rbtree_set_register(map, rbnode, reg_tmp, value);
 386         } else {
 387                 unsigned int base_reg, top_reg;
 388                 unsigned int new_base_reg, new_top_reg;
 389                 unsigned int min, max;
 390                 unsigned int max_dist;
 391                 unsigned int dist, best_dist = UINT_MAX;
 392
 393                 max_dist = map->reg_stride * sizeof(*rbnode_tmp) /
 394                         map->cache_word_size;
 395                 if (reg < max_dist)
 396                         min = 0;
 397                 else
 398                         min = reg - max_dist;
 399                 max = reg + max_dist;
 400
 401                 /* look for an adjacent register to the one we are about to add */
 402                 node = rbtree_ctx->root.rb_node;
 403                 while (node) {
 404                         rbnode_tmp = rb_entry(node, struct regcache_rbtree_node,
 405                                               node);
 406
 407                         regcache_rbtree_get_base_top_reg(map, rbnode_tmp,
 408                                 &base_reg, &top_reg);
 409
 410                         if (base_reg <= max && top_reg >= min) {
 411                                 if (reg < base_reg)
 412                                         dist = base_reg - reg;
 413                                 else if (reg > top_reg)
 414                                         dist = reg - top_reg;
 415                                 else
 416                                         dist = 0;
 417                                 if (dist < best_dist) {
 418                                         rbnode = rbnode_tmp;
 419                                         best_dist = dist;
 420                                         new_base_reg = min(reg, base_reg);
 421                                         new_top_reg = max(reg, top_reg);
 422                                 }
 423                         }
 424
 425                         /*
 426                          * Keep looking, we want to choose the closest block,
 427                          * otherwise we might end up creating overlapping
 428                          * blocks, which breaks the rbtree.
 429                          */
 430                         if (reg < base_reg)
 431                                 node = node->rb_left;
 432                         else if (reg > top_reg)
 433                                 node = node->rb_right;
 434                         else
 435                                 break;
 436                 }
 437
 438                 if (rbnode) {
 439                         ret = regcache_rbtree_insert_to_block(map, rbnode,
 440                                                               new_base_reg,
 441                                                               new_top_reg, reg,
 442                                                               value);
 443                         if (ret)
 444                                 return ret;
 445                         rbtree_ctx->cached_rbnode = rbnode;
 446                         return 0;
 447                 }
 448
 449                 /* We did not manage to find a place to insert it in
 450                  * an existing block so create a new rbnode.
 451                  */
 452                 rbnode = regcache_rbtree_node_alloc(map, reg);
 453                 if (!rbnode)
 454                         return -ENOMEM;
 455                 regcache_rbtree_set_register(map, rbnode,
 456                                              reg - rbnode->base_reg, value);
 457                 regcache_rbtree_insert(map, &rbtree_ctx->root, rbnode);
 458                 rbtree_ctx->cached_rbnode = rbnode;
 459         }
 460
 461         return 0;
 462 }
 463
 464 static int regcache_rbtree_sync(struct regmap *map, unsigned int min,
 465                                 unsigned int max)
 466 {
 467         struct regcache_rbtree_ctx *rbtree_ctx;
 468         struct rb_node *node;
 469         struct regcache_rbtree_node *rbnode;
 470         unsigned int base_reg, top_reg;
 471         unsigned int start, end;
 472         int ret;
 473
 474         rbtree_ctx = map->cache;
 475         for (node = rb_first(&rbtree_ctx->root); node; node = rb_next(node)) {
 476                 rbnode = rb_entry(node, struct regcache_rbtree_node, node);
 477
 478                 regcache_rbtree_get_base_top_reg(map, rbnode, &base_reg,
 479                         &top_reg);
 480                 if (base_reg > max)
 481                         break;
 482                 if (top_reg < min)
 483                         continue;
 484
 485                 if (min > base_reg)
 486                         start = (min - base_reg) / map->reg_stride;
 487                 else
 488                         start = 0;
 489
 490                 if (max < top_reg)
 491                         end = (max - base_reg) / map->reg_stride + 1;
 492                 else
 493                         end = rbnode->blklen;
 494
 495                 ret = regcache_sync_block(map, rbnode->block,
 496                                           rbnode->cache_present,
 497                                           rbnode->base_reg, start, end);
 498                 if (ret != 0)
 499                         return ret;
 500         }
 501
 502         return regmap_async_complete(map);
 503 }
 504
 505 static int regcache_rbtree_drop(struct regmap *map, unsigned int min,
 506                                 unsigned int max)
 507 {
 508         struct regcache_rbtree_ctx *rbtree_ctx;
 509         struct regcache_rbtree_node *rbnode;
 510         struct rb_node *node;
 511         unsigned int base_reg, top_reg;
 512         unsigned int start, end;
 513
 514         rbtree_ctx = map->cache;
 515         for (node = rb_first(&rbtree_ctx->root); node; node = rb_next(node)) {
 516                 rbnode = rb_entry(node, struct regcache_rbtree_node, node);
 517
 518                 regcache_rbtree_get_base_top_reg(map, rbnode, &base_reg,
 519                         &top_reg);
 520                 if (base_reg > max)
 521                         break;
 522                 if (top_reg < min)
 523                         continue;
 524
 525                 if (min > base_reg)
 526                         start = (min - base_reg) / map->reg_stride;
 527                 else
 528                         start = 0;
 529
 530                 if (max < top_reg)
 531                         end = (max - base_reg) / map->reg_stride + 1;
 532                 else
 533                         end = rbnode->blklen;
 534
 535                 bitmap_clear(rbnode->cache_present, start, end - start);
 536         }
 537
 538         return 0;
 539 }
 540
 541 struct regcache_ops regcache_rbtree_ops = {
 542         .type = REGCACHE_RBTREE,
 543         .name = "rbtree",
 544         .init = regcache_rbtree_init,
 545         .exit = regcache_rbtree_exit,
 546 #ifdef CONFIG_DEBUG_FS
 547         .debugfs_init = rbtree_debugfs_init,
 548 #endif
 549         .read = regcache_rbtree_read,
 550         .write = regcache_rbtree_write,
 551         .sync = regcache_rbtree_sync,
 552         .drop = regcache_rbtree_drop,
 553 };