Loading...
Note: File does not exist in v3.1.
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/device.h>
15#include <linux/lzo.h>
16
17#include "internal.h"
18
19static int regcache_lzo_exit(struct regmap *map);
20
21struct 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
33static int regcache_lzo_block_count(struct regmap *map)
34{
35 return LZO_BLOCK_NUM;
36}
37
38static 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
46static 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
59static 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
72static 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
90static 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
108static inline int regcache_lzo_get_blkindex(struct regmap *map,
109 unsigned int reg)
110{
111 return ((reg / map->reg_stride) * map->cache_word_size) /
112 DIV_ROUND_UP(map->cache_size_raw,
113 regcache_lzo_block_count(map));
114}
115
116static inline int regcache_lzo_get_blkpos(struct regmap *map,
117 unsigned int reg)
118{
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);
123}
124
125static inline int regcache_lzo_get_blksize(struct regmap *map)
126{
127 return DIV_ROUND_UP(map->cache_size_raw,
128 regcache_lzo_block_count(map));
129}
130
131static 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
141 blkcount = regcache_lzo_block_count(map);
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;
199err:
200 regcache_lzo_exit(map);
201 return ret;
202}
203
204static 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
213 blkcount = regcache_lzo_block_count(map);
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
234static 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
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;
270
271 return ret;
272}
273
274static 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 */
326 set_bit(reg / map->reg_stride, lzo_block->sync_bmp);
327 kfree(tmp_dst);
328 kfree(lzo_block->src);
329 return 0;
330out:
331 lzo_block->dst = tmp_dst;
332 lzo_block->dst_len = tmp_dst_len;
333 return ret;
334}
335
336static int regcache_lzo_sync(struct regmap *map, unsigned int min,
337 unsigned int max)
338{
339 struct regcache_lzo_ctx **lzo_blocks;
340 unsigned int val;
341 int i;
342 int ret;
343
344 lzo_blocks = map->cache;
345 i = min;
346 for_each_set_bit_from(i, lzo_blocks[0]->sync_bmp,
347 lzo_blocks[0]->sync_bmp_nbits) {
348 if (i > max)
349 continue;
350
351 ret = regcache_read(map, i, &val);
352 if (ret)
353 return ret;
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
360 map->cache_bypass = 1;
361 ret = _regmap_write(map, i, val);
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
372struct regcache_ops regcache_lzo_ops = {
373 .type = REGCACHE_COMPRESSED,
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};