Commit | Line | Data |
---|---|---|
2cbbb579 DP |
1 | /* |
2 | * Register cache access API - LZO 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> | |
51990e82 | 14 | #include <linux/device.h> |
2cbbb579 DP |
15 | #include <linux/lzo.h> |
16 | ||
17 | #include "internal.h" | |
18 | ||
c2b1ecd1 LPC |
19 | static int regcache_lzo_exit(struct regmap *map); |
20 | ||
2cbbb579 DP |
21 | struct regcache_lzo_ctx { |
22 | void *wmem; | |
23 | void *dst; | |
24 | const void *src; | |
25 | size_t src_len; | |
26 | size_t dst_len; | |
27 | size_t decompressed_size; | |
28 | unsigned long *sync_bmp; | |
29 | int sync_bmp_nbits; | |
30 | }; | |
31 | ||
32 | #define LZO_BLOCK_NUM 8 | |
82732bdd | 33 | static int regcache_lzo_block_count(struct regmap *map) |
2cbbb579 DP |
34 | { |
35 | return LZO_BLOCK_NUM; | |
36 | } | |
37 | ||
38 | static int regcache_lzo_prepare(struct regcache_lzo_ctx *lzo_ctx) | |
39 | { | |
40 | lzo_ctx->wmem = kmalloc(LZO1X_MEM_COMPRESS, GFP_KERNEL); | |
41 | if (!lzo_ctx->wmem) | |
42 | return -ENOMEM; | |
43 | return 0; | |
44 | } | |
45 | ||
46 | static int regcache_lzo_compress(struct regcache_lzo_ctx *lzo_ctx) | |
47 | { | |
48 | size_t compress_size; | |
49 | int ret; | |
50 | ||
51 | ret = lzo1x_1_compress(lzo_ctx->src, lzo_ctx->src_len, | |
52 | lzo_ctx->dst, &compress_size, lzo_ctx->wmem); | |
53 | if (ret != LZO_E_OK || compress_size > lzo_ctx->dst_len) | |
54 | return -EINVAL; | |
55 | lzo_ctx->dst_len = compress_size; | |
56 | return 0; | |
57 | } | |
58 | ||
59 | static int regcache_lzo_decompress(struct regcache_lzo_ctx *lzo_ctx) | |
60 | { | |
61 | size_t dst_len; | |
62 | int ret; | |
63 | ||
64 | dst_len = lzo_ctx->dst_len; | |
65 | ret = lzo1x_decompress_safe(lzo_ctx->src, lzo_ctx->src_len, | |
66 | lzo_ctx->dst, &dst_len); | |
67 | if (ret != LZO_E_OK || dst_len != lzo_ctx->dst_len) | |
68 | return -EINVAL; | |
69 | return 0; | |
70 | } | |
71 | ||
72 | static int regcache_lzo_compress_cache_block(struct regmap *map, | |
73 | struct regcache_lzo_ctx *lzo_ctx) | |
74 | { | |
75 | int ret; | |
76 | ||
77 | lzo_ctx->dst_len = lzo1x_worst_compress(PAGE_SIZE); | |
78 | lzo_ctx->dst = kmalloc(lzo_ctx->dst_len, GFP_KERNEL); | |
79 | if (!lzo_ctx->dst) { | |
80 | lzo_ctx->dst_len = 0; | |
81 | return -ENOMEM; | |
82 | } | |
83 | ||
84 | ret = regcache_lzo_compress(lzo_ctx); | |
85 | if (ret < 0) | |
86 | return ret; | |
87 | return 0; | |
88 | } | |
89 | ||
90 | static int regcache_lzo_decompress_cache_block(struct regmap *map, | |
91 | struct regcache_lzo_ctx *lzo_ctx) | |
92 | { | |
93 | int ret; | |
94 | ||
95 | lzo_ctx->dst_len = lzo_ctx->decompressed_size; | |
96 | lzo_ctx->dst = kmalloc(lzo_ctx->dst_len, GFP_KERNEL); | |
97 | if (!lzo_ctx->dst) { | |
98 | lzo_ctx->dst_len = 0; | |
99 | return -ENOMEM; | |
100 | } | |
101 | ||
102 | ret = regcache_lzo_decompress(lzo_ctx); | |
103 | if (ret < 0) | |
104 | return ret; | |
105 | return 0; | |
106 | } | |
107 | ||
108 | static inline int regcache_lzo_get_blkindex(struct regmap *map, | |
109 | unsigned int reg) | |
110 | { | |
f01ee60f | 111 | return ((reg / map->reg_stride) * map->cache_word_size) / |
82732bdd MB |
112 | DIV_ROUND_UP(map->cache_size_raw, |
113 | regcache_lzo_block_count(map)); | |
2cbbb579 DP |
114 | } |
115 | ||
116 | static inline int regcache_lzo_get_blkpos(struct regmap *map, | |
117 | unsigned int reg) | |
118 | { | |
f01ee60f SW |
119 | return (reg / map->reg_stride) % |
120 | (DIV_ROUND_UP(map->cache_size_raw, | |
121 | regcache_lzo_block_count(map)) / | |
122 | map->cache_word_size); | |
2cbbb579 DP |
123 | } |
124 | ||
125 | static inline int regcache_lzo_get_blksize(struct regmap *map) | |
126 | { | |
82732bdd MB |
127 | return DIV_ROUND_UP(map->cache_size_raw, |
128 | regcache_lzo_block_count(map)); | |
2cbbb579 DP |
129 | } |
130 | ||
131 | static int regcache_lzo_init(struct regmap *map) | |
132 | { | |
133 | struct regcache_lzo_ctx **lzo_blocks; | |
134 | size_t bmp_size; | |
135 | int ret, i, blksize, blkcount; | |
136 | const char *p, *end; | |
137 | unsigned long *sync_bmp; | |
138 | ||
139 | ret = 0; | |
140 | ||
82732bdd | 141 | blkcount = regcache_lzo_block_count(map); |
2cbbb579 DP |
142 | map->cache = kzalloc(blkcount * sizeof *lzo_blocks, |
143 | GFP_KERNEL); | |
144 | if (!map->cache) | |
145 | return -ENOMEM; | |
146 | lzo_blocks = map->cache; | |
147 | ||
148 | /* | |
149 | * allocate a bitmap to be used when syncing the cache with | |
150 | * the hardware. Each time a register is modified, the corresponding | |
151 | * bit is set in the bitmap, so we know that we have to sync | |
152 | * that register. | |
153 | */ | |
154 | bmp_size = map->num_reg_defaults_raw; | |
155 | sync_bmp = kmalloc(BITS_TO_LONGS(bmp_size) * sizeof(long), | |
156 | GFP_KERNEL); | |
157 | if (!sync_bmp) { | |
158 | ret = -ENOMEM; | |
159 | goto err; | |
160 | } | |
161 | bitmap_zero(sync_bmp, bmp_size); | |
162 | ||
163 | /* allocate the lzo blocks and initialize them */ | |
164 | for (i = 0; i < blkcount; i++) { | |
165 | lzo_blocks[i] = kzalloc(sizeof **lzo_blocks, | |
166 | GFP_KERNEL); | |
167 | if (!lzo_blocks[i]) { | |
168 | kfree(sync_bmp); | |
169 | ret = -ENOMEM; | |
170 | goto err; | |
171 | } | |
172 | lzo_blocks[i]->sync_bmp = sync_bmp; | |
173 | lzo_blocks[i]->sync_bmp_nbits = bmp_size; | |
174 | /* alloc the working space for the compressed block */ | |
175 | ret = regcache_lzo_prepare(lzo_blocks[i]); | |
176 | if (ret < 0) | |
177 | goto err; | |
178 | } | |
179 | ||
180 | blksize = regcache_lzo_get_blksize(map); | |
181 | p = map->reg_defaults_raw; | |
182 | end = map->reg_defaults_raw + map->cache_size_raw; | |
183 | /* compress the register map and fill the lzo blocks */ | |
184 | for (i = 0; i < blkcount; i++, p += blksize) { | |
185 | lzo_blocks[i]->src = p; | |
186 | if (p + blksize > end) | |
187 | lzo_blocks[i]->src_len = end - p; | |
188 | else | |
189 | lzo_blocks[i]->src_len = blksize; | |
190 | ret = regcache_lzo_compress_cache_block(map, | |
191 | lzo_blocks[i]); | |
192 | if (ret < 0) | |
193 | goto err; | |
194 | lzo_blocks[i]->decompressed_size = | |
195 | lzo_blocks[i]->src_len; | |
196 | } | |
197 | ||
198 | return 0; | |
199 | err: | |
c2b1ecd1 | 200 | regcache_lzo_exit(map); |
2cbbb579 DP |
201 | return ret; |
202 | } | |
203 | ||
204 | static int regcache_lzo_exit(struct regmap *map) | |
205 | { | |
206 | struct regcache_lzo_ctx **lzo_blocks; | |
207 | int i, blkcount; | |
208 | ||
209 | lzo_blocks = map->cache; | |
210 | if (!lzo_blocks) | |
211 | return 0; | |
212 | ||
82732bdd | 213 | blkcount = regcache_lzo_block_count(map); |
2cbbb579 DP |
214 | /* |
215 | * the pointer to the bitmap used for syncing the cache | |
216 | * is shared amongst all lzo_blocks. Ensure it is freed | |
217 | * only once. | |
218 | */ | |
219 | if (lzo_blocks[0]) | |
220 | kfree(lzo_blocks[0]->sync_bmp); | |
221 | for (i = 0; i < blkcount; i++) { | |
222 | if (lzo_blocks[i]) { | |
223 | kfree(lzo_blocks[i]->wmem); | |
224 | kfree(lzo_blocks[i]->dst); | |
225 | } | |
226 | /* each lzo_block is a pointer returned by kmalloc or NULL */ | |
227 | kfree(lzo_blocks[i]); | |
228 | } | |
229 | kfree(lzo_blocks); | |
230 | map->cache = NULL; | |
231 | return 0; | |
232 | } | |
233 | ||
234 | static int regcache_lzo_read(struct regmap *map, | |
235 | unsigned int reg, unsigned int *value) | |
236 | { | |
237 | struct regcache_lzo_ctx *lzo_block, **lzo_blocks; | |
238 | int ret, blkindex, blkpos; | |
239 | size_t blksize, tmp_dst_len; | |
240 | void *tmp_dst; | |
241 | ||
2cbbb579 DP |
242 | /* index of the compressed lzo block */ |
243 | blkindex = regcache_lzo_get_blkindex(map, reg); | |
244 | /* register index within the decompressed block */ | |
245 | blkpos = regcache_lzo_get_blkpos(map, reg); | |
246 | /* size of the compressed block */ | |
247 | blksize = regcache_lzo_get_blksize(map); | |
248 | lzo_blocks = map->cache; | |
249 | lzo_block = lzo_blocks[blkindex]; | |
250 | ||
251 | /* save the pointer and length of the compressed block */ | |
252 | tmp_dst = lzo_block->dst; | |
253 | tmp_dst_len = lzo_block->dst_len; | |
254 | ||
255 | /* prepare the source to be the compressed block */ | |
256 | lzo_block->src = lzo_block->dst; | |
257 | lzo_block->src_len = lzo_block->dst_len; | |
258 | ||
259 | /* decompress the block */ | |
260 | ret = regcache_lzo_decompress_cache_block(map, lzo_block); | |
261 | if (ret >= 0) | |
262 | /* fetch the value from the cache */ | |
263 | *value = regcache_get_val(lzo_block->dst, blkpos, | |
264 | map->cache_word_size); | |
265 | ||
266 | kfree(lzo_block->dst); | |
267 | /* restore the pointer and length of the compressed block */ | |
268 | lzo_block->dst = tmp_dst; | |
269 | lzo_block->dst_len = tmp_dst_len; | |
6e6ace00 MB |
270 | |
271 | return ret; | |
2cbbb579 DP |
272 | } |
273 | ||
274 | static int regcache_lzo_write(struct regmap *map, | |
275 | unsigned int reg, unsigned int value) | |
276 | { | |
277 | struct regcache_lzo_ctx *lzo_block, **lzo_blocks; | |
278 | int ret, blkindex, blkpos; | |
279 | size_t blksize, tmp_dst_len; | |
280 | void *tmp_dst; | |
281 | ||
282 | /* index of the compressed lzo block */ | |
283 | blkindex = regcache_lzo_get_blkindex(map, reg); | |
284 | /* register index within the decompressed block */ | |
285 | blkpos = regcache_lzo_get_blkpos(map, reg); | |
286 | /* size of the compressed block */ | |
287 | blksize = regcache_lzo_get_blksize(map); | |
288 | lzo_blocks = map->cache; | |
289 | lzo_block = lzo_blocks[blkindex]; | |
290 | ||
291 | /* save the pointer and length of the compressed block */ | |
292 | tmp_dst = lzo_block->dst; | |
293 | tmp_dst_len = lzo_block->dst_len; | |
294 | ||
295 | /* prepare the source to be the compressed block */ | |
296 | lzo_block->src = lzo_block->dst; | |
297 | lzo_block->src_len = lzo_block->dst_len; | |
298 | ||
299 | /* decompress the block */ | |
300 | ret = regcache_lzo_decompress_cache_block(map, lzo_block); | |
301 | if (ret < 0) { | |
302 | kfree(lzo_block->dst); | |
303 | goto out; | |
304 | } | |
305 | ||
306 | /* write the new value to the cache */ | |
307 | if (regcache_set_val(lzo_block->dst, blkpos, value, | |
308 | map->cache_word_size)) { | |
309 | kfree(lzo_block->dst); | |
310 | goto out; | |
311 | } | |
312 | ||
313 | /* prepare the source to be the decompressed block */ | |
314 | lzo_block->src = lzo_block->dst; | |
315 | lzo_block->src_len = lzo_block->dst_len; | |
316 | ||
317 | /* compress the block */ | |
318 | ret = regcache_lzo_compress_cache_block(map, lzo_block); | |
319 | if (ret < 0) { | |
320 | kfree(lzo_block->dst); | |
321 | kfree(lzo_block->src); | |
322 | goto out; | |
323 | } | |
324 | ||
325 | /* set the bit so we know we have to sync this register */ | |
f01ee60f | 326 | set_bit(reg / map->reg_stride, lzo_block->sync_bmp); |
2cbbb579 DP |
327 | kfree(tmp_dst); |
328 | kfree(lzo_block->src); | |
329 | return 0; | |
330 | out: | |
331 | lzo_block->dst = tmp_dst; | |
332 | lzo_block->dst_len = tmp_dst_len; | |
333 | return ret; | |
334 | } | |
335 | ||
ac8d91c8 MB |
336 | static int regcache_lzo_sync(struct regmap *map, unsigned int min, |
337 | unsigned int max) | |
2cbbb579 DP |
338 | { |
339 | struct regcache_lzo_ctx **lzo_blocks; | |
340 | unsigned int val; | |
341 | int i; | |
342 | int ret; | |
343 | ||
344 | lzo_blocks = map->cache; | |
ac8d91c8 | 345 | i = min; |
0a329d2d | 346 | for_each_set_bit_from(i, lzo_blocks[0]->sync_bmp, |
ac8d91c8 MB |
347 | lzo_blocks[0]->sync_bmp_nbits) { |
348 | if (i > max) | |
349 | continue; | |
350 | ||
2cbbb579 DP |
351 | ret = regcache_read(map, i, &val); |
352 | if (ret) | |
353 | return ret; | |
a3c37741 MB |
354 | |
355 | /* Is this the hardware default? If so skip. */ | |
356 | ret = regcache_lookup_reg(map, i); | |
357 | if (ret > 0 && val == map->reg_defaults[ret].def) | |
358 | continue; | |
359 | ||
2cbbb579 | 360 | map->cache_bypass = 1; |
13753a90 | 361 | ret = _regmap_write(map, i, val); |
2cbbb579 DP |
362 | map->cache_bypass = 0; |
363 | if (ret) | |
364 | return ret; | |
365 | dev_dbg(map->dev, "Synced register %#x, value %#x\n", | |
366 | i, val); | |
367 | } | |
368 | ||
369 | return 0; | |
370 | } | |
371 | ||
372 | struct regcache_ops regcache_lzo_ops = { | |
50b776fc | 373 | .type = REGCACHE_COMPRESSED, |
2cbbb579 DP |
374 | .name = "lzo", |
375 | .init = regcache_lzo_init, | |
376 | .exit = regcache_lzo_exit, | |
377 | .read = regcache_lzo_read, | |
378 | .write = regcache_lzo_write, | |
379 | .sync = regcache_lzo_sync | |
380 | }; |