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