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