Commit | Line | Data |
---|---|---|
28644c80 DP |
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 | ||
18 | static int regcache_rbtree_write(struct regmap *map, unsigned int reg, | |
19 | unsigned int value); | |
20 | ||
21 | struct 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; | |
28644c80 DP |
26 | /* block of adjacent registers */ |
27 | void *block; | |
28 | /* number of registers available in the block */ | |
29 | unsigned int blklen; | |
30 | } __attribute__ ((packed)); | |
31 | ||
32 | struct regcache_rbtree_ctx { | |
33 | struct rb_root root; | |
34 | struct regcache_rbtree_node *cached_rbnode; | |
35 | }; | |
36 | ||
37 | static 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 | ||
45 | static unsigned int regcache_rbtree_get_register( | |
25ed1156 DP |
46 | struct regcache_rbtree_node *rbnode, unsigned int idx, |
47 | unsigned int word_size) | |
28644c80 | 48 | { |
c5713004 | 49 | return regcache_get_val(rbnode->block, idx, word_size); |
28644c80 DP |
50 | } |
51 | ||
52 | static void regcache_rbtree_set_register(struct regcache_rbtree_node *rbnode, | |
25ed1156 DP |
53 | unsigned int idx, unsigned int val, |
54 | unsigned int word_size) | |
28644c80 | 55 | { |
c5713004 | 56 | regcache_set_val(rbnode->block, idx, val, word_size); |
28644c80 DP |
57 | } |
58 | ||
3405addd LPC |
59 | static struct regcache_rbtree_node *regcache_rbtree_lookup(struct regmap *map, |
60 | unsigned int reg) | |
28644c80 | 61 | { |
3405addd | 62 | struct regcache_rbtree_ctx *rbtree_ctx = map->cache; |
28644c80 DP |
63 | struct rb_node *node; |
64 | struct regcache_rbtree_node *rbnode; | |
65 | unsigned int base_reg, top_reg; | |
66 | ||
3405addd LPC |
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; | |
28644c80 DP |
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); | |
3405addd LPC |
78 | if (reg >= base_reg && reg <= top_reg) { |
79 | rbtree_ctx->cached_rbnode = rbnode; | |
28644c80 | 80 | return rbnode; |
3405addd | 81 | } else if (reg > top_reg) { |
28644c80 | 82 | node = node->rb_right; |
3405addd | 83 | } else if (reg < base_reg) { |
28644c80 | 84 | node = node->rb_left; |
3405addd | 85 | } |
28644c80 DP |
86 | } |
87 | ||
88 | return NULL; | |
89 | } | |
90 | ||
91 | static 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 | ||
127 | static 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 | ||
151 | err: | |
152 | regcache_exit(map); | |
153 | return ret; | |
154 | } | |
155 | ||
156 | static 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 | ||
184 | static int regcache_rbtree_read(struct regmap *map, | |
185 | unsigned int reg, unsigned int *value) | |
186 | { | |
28644c80 | 187 | struct regcache_rbtree_node *rbnode; |
28644c80 DP |
188 | unsigned int reg_tmp; |
189 | ||
3405addd | 190 | rbnode = regcache_rbtree_lookup(map, reg); |
28644c80 DP |
191 | if (rbnode) { |
192 | reg_tmp = reg - rbnode->base_reg; | |
25ed1156 DP |
193 | *value = regcache_rbtree_get_register(rbnode, reg_tmp, |
194 | map->cache_word_size); | |
28644c80 | 195 | } else { |
6e6ace00 | 196 | return -ENOENT; |
28644c80 DP |
197 | } |
198 | ||
199 | return 0; | |
200 | } | |
201 | ||
202 | ||
203 | static int regcache_rbtree_insert_to_block(struct regcache_rbtree_node *rbnode, | |
204 | unsigned int pos, unsigned int reg, | |
25ed1156 | 205 | unsigned int value, unsigned int word_size) |
28644c80 DP |
206 | { |
207 | u8 *blk; | |
208 | ||
209 | blk = krealloc(rbnode->block, | |
25ed1156 | 210 | (rbnode->blklen + 1) * word_size, GFP_KERNEL); |
28644c80 DP |
211 | if (!blk) |
212 | return -ENOMEM; | |
213 | ||
214 | /* insert the register value in the correct place in the rbnode block */ | |
25ed1156 DP |
215 | memmove(blk + (pos + 1) * word_size, |
216 | blk + pos * word_size, | |
217 | (rbnode->blklen - pos) * word_size); | |
28644c80 DP |
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 | ||
25ed1156 | 225 | regcache_rbtree_set_register(rbnode, pos, value, word_size); |
28644c80 DP |
226 | return 0; |
227 | } | |
228 | ||
229 | static 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; | |
28644c80 DP |
237 | unsigned int pos; |
238 | int i; | |
239 | int ret; | |
240 | ||
241 | rbtree_ctx = map->cache; | |
28644c80 DP |
242 | /* if we can't locate it in the cached rbnode we'll have |
243 | * to traverse the rbtree looking for it. | |
244 | */ | |
3405addd | 245 | rbnode = regcache_rbtree_lookup(map, reg); |
28644c80 DP |
246 | if (rbnode) { |
247 | reg_tmp = reg - rbnode->base_reg; | |
25ed1156 DP |
248 | val = regcache_rbtree_get_register(rbnode, reg_tmp, |
249 | map->cache_word_size); | |
28644c80 DP |
250 | if (val == value) |
251 | return 0; | |
25ed1156 DP |
252 | regcache_rbtree_set_register(rbnode, reg_tmp, value, |
253 | map->cache_word_size); | |
28644c80 | 254 | } else { |
28644c80 DP |
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, | |
25ed1156 DP |
269 | reg, value, |
270 | map->cache_word_size); | |
28644c80 DP |
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; | |
25ed1156 | 287 | rbnode->block = kmalloc(rbnode->blklen * map->cache_word_size, |
28644c80 DP |
288 | GFP_KERNEL); |
289 | if (!rbnode->block) { | |
290 | kfree(rbnode); | |
291 | return -ENOMEM; | |
292 | } | |
25ed1156 | 293 | regcache_rbtree_set_register(rbnode, 0, value, map->cache_word_size); |
28644c80 DP |
294 | regcache_rbtree_insert(&rbtree_ctx->root, rbnode); |
295 | rbtree_ctx->cached_rbnode = rbnode; | |
296 | } | |
297 | ||
298 | return 0; | |
299 | } | |
300 | ||
301 | static 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; | |
25ed1156 DP |
316 | val = regcache_rbtree_get_register(rbnode, i, |
317 | map->cache_word_size); | |
28644c80 DP |
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; | |
13753a90 | 326 | ret = _regmap_write(map, regtmp, val); |
28644c80 DP |
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 | ||
338 | struct 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 | }; |