]>
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 */ | |
879082c9 | 263 | *value = regcache_get_val(map, lzo_block->dst, blkpos); |
2cbbb579 DP |
264 | |
265 | kfree(lzo_block->dst); | |
266 | /* restore the pointer and length of the compressed block */ | |
267 | lzo_block->dst = tmp_dst; | |
268 | lzo_block->dst_len = tmp_dst_len; | |
6e6ace00 MB |
269 | |
270 | return ret; | |
2cbbb579 DP |
271 | } |
272 | ||
273 | static int regcache_lzo_write(struct regmap *map, | |
274 | unsigned int reg, unsigned int value) | |
275 | { | |
276 | struct regcache_lzo_ctx *lzo_block, **lzo_blocks; | |
277 | int ret, blkindex, blkpos; | |
278 | size_t blksize, tmp_dst_len; | |
279 | void *tmp_dst; | |
280 | ||
281 | /* index of the compressed lzo block */ | |
282 | blkindex = regcache_lzo_get_blkindex(map, reg); | |
283 | /* register index within the decompressed block */ | |
284 | blkpos = regcache_lzo_get_blkpos(map, reg); | |
285 | /* size of the compressed block */ | |
286 | blksize = regcache_lzo_get_blksize(map); | |
287 | lzo_blocks = map->cache; | |
288 | lzo_block = lzo_blocks[blkindex]; | |
289 | ||
290 | /* save the pointer and length of the compressed block */ | |
291 | tmp_dst = lzo_block->dst; | |
292 | tmp_dst_len = lzo_block->dst_len; | |
293 | ||
294 | /* prepare the source to be the compressed block */ | |
295 | lzo_block->src = lzo_block->dst; | |
296 | lzo_block->src_len = lzo_block->dst_len; | |
297 | ||
298 | /* decompress the block */ | |
299 | ret = regcache_lzo_decompress_cache_block(map, lzo_block); | |
300 | if (ret < 0) { | |
301 | kfree(lzo_block->dst); | |
302 | goto out; | |
303 | } | |
304 | ||
305 | /* write the new value to the cache */ | |
879082c9 | 306 | if (regcache_set_val(map, lzo_block->dst, blkpos, value)) { |
2cbbb579 DP |
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 */ | |
f01ee60f | 324 | set_bit(reg / map->reg_stride, lzo_block->sync_bmp); |
2cbbb579 DP |
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 | 343 | i = min; |
0a329d2d | 344 | for_each_set_bit_from(i, lzo_blocks[0]->sync_bmp, |
ac8d91c8 MB |
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; | |
a3c37741 MB |
352 | |
353 | /* Is this the hardware default? If so skip. */ | |
354 | ret = regcache_lookup_reg(map, i); | |
355 | if (ret > 0 && val == map->reg_defaults[ret].def) | |
356 | continue; | |
357 | ||
2cbbb579 | 358 | map->cache_bypass = 1; |
13753a90 | 359 | ret = _regmap_write(map, i, val); |
2cbbb579 DP |
360 | map->cache_bypass = 0; |
361 | if (ret) | |
362 | return ret; | |
363 | dev_dbg(map->dev, "Synced register %#x, value %#x\n", | |
364 | i, val); | |
365 | } | |
366 | ||
367 | return 0; | |
368 | } | |
369 | ||
370 | struct regcache_ops regcache_lzo_ops = { | |
50b776fc | 371 | .type = REGCACHE_COMPRESSED, |
2cbbb579 DP |
372 | .name = "lzo", |
373 | .init = regcache_lzo_init, | |
374 | .exit = regcache_lzo_exit, | |
375 | .read = regcache_lzo_read, | |
376 | .write = regcache_lzo_write, | |
377 | .sync = regcache_lzo_sync | |
378 | }; |