regmap: cache Fix regcache-rbtree sync
[linux-2.6.git] / drivers / base / regmap / regcache-rbtree.c
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
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
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
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
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
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
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
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
181 static void rbtree_debugfs_init(struct regmap *map)
182 {
183         debugfs_create_file("rbtree", 0400, map->debugfs, map, &rbtree_fops);
184 }
185 #else
186 static void rbtree_debugfs_init(struct regmap *map)
187 {
188 }
189 #endif
190
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
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
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
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
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
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 };