blob: e210a6d1406a5798b60a49791ae8132cf4a8594a [file] [log] [blame]
Dimitris Papastamos2cbbb572011-09-19 14:34:03 +01001/*
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>
Paul Gortmaker51990e82012-01-22 11:23:42 -050014#include <linux/device.h>
Dimitris Papastamos2cbbb572011-09-19 14:34:03 +010015#include <linux/lzo.h>
16
17#include "internal.h"
18
Lars-Peter Clausenc2b1ecd2011-11-15 13:34:41 +010019static int regcache_lzo_exit(struct regmap *map);
20
Dimitris Papastamos2cbbb572011-09-19 14:34:03 +010021struct 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
Mark Brown82732bd2011-10-18 00:37:00 +010033static int regcache_lzo_block_count(struct regmap *map)
Dimitris Papastamos2cbbb572011-09-19 14:34:03 +010034{
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{
Stephen Warrenf01ee602012-04-09 13:40:24 -0600111 return ((reg / map->reg_stride) * map->cache_word_size) /
Mark Brown82732bd2011-10-18 00:37:00 +0100112 DIV_ROUND_UP(map->cache_size_raw,
113 regcache_lzo_block_count(map));
Dimitris Papastamos2cbbb572011-09-19 14:34:03 +0100114}
115
116static inline int regcache_lzo_get_blkpos(struct regmap *map,
117 unsigned int reg)
118{
Stephen Warrenf01ee602012-04-09 13:40:24 -0600119 return (reg / map->reg_stride) %
120 (DIV_ROUND_UP(map->cache_size_raw,
121 regcache_lzo_block_count(map)) /
122 map->cache_word_size);
Dimitris Papastamos2cbbb572011-09-19 14:34:03 +0100123}
124
125static inline int regcache_lzo_get_blksize(struct regmap *map)
126{
Mark Brown82732bd2011-10-18 00:37:00 +0100127 return DIV_ROUND_UP(map->cache_size_raw,
128 regcache_lzo_block_count(map));
Dimitris Papastamos2cbbb572011-09-19 14:34:03 +0100129}
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
Mark Brown82732bd2011-10-18 00:37:00 +0100141 blkcount = regcache_lzo_block_count(map);
Dimitris Papastamos2cbbb572011-09-19 14:34:03 +0100142 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:
Lars-Peter Clausenc2b1ecd2011-11-15 13:34:41 +0100200 regcache_lzo_exit(map);
Dimitris Papastamos2cbbb572011-09-19 14:34:03 +0100201 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
Mark Brown82732bd2011-10-18 00:37:00 +0100213 blkcount = regcache_lzo_block_count(map);
Dimitris Papastamos2cbbb572011-09-19 14:34:03 +0100214 /*
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
Dimitris Papastamos2cbbb572011-09-19 14:34:03 +0100242 /* 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 */
Mark Brown879082c2013-02-21 18:03:13 +0000263 *value = regcache_get_val(map, lzo_block->dst, blkpos);
Dimitris Papastamos2cbbb572011-09-19 14:34:03 +0100264
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;
Mark Brown6e6ace02011-10-09 13:23:31 +0100269
270 return ret;
Dimitris Papastamos2cbbb572011-09-19 14:34:03 +0100271}
272
273static 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 */
Mark Brown879082c2013-02-21 18:03:13 +0000306 if (regcache_set_val(map, lzo_block->dst, blkpos, value)) {
Dimitris Papastamos2cbbb572011-09-19 14:34:03 +0100307 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 */
Stephen Warrenf01ee602012-04-09 13:40:24 -0600324 set_bit(reg / map->reg_stride, lzo_block->sync_bmp);
Dimitris Papastamos2cbbb572011-09-19 14:34:03 +0100325 kfree(tmp_dst);
326 kfree(lzo_block->src);
327 return 0;
328out:
329 lzo_block->dst = tmp_dst;
330 lzo_block->dst_len = tmp_dst_len;
331 return ret;
332}
333
Mark Brownac8d91c2012-02-23 19:31:04 +0000334static int regcache_lzo_sync(struct regmap *map, unsigned int min,
335 unsigned int max)
Dimitris Papastamos2cbbb572011-09-19 14:34:03 +0100336{
337 struct regcache_lzo_ctx **lzo_blocks;
338 unsigned int val;
339 int i;
340 int ret;
341
342 lzo_blocks = map->cache;
Mark Brownac8d91c2012-02-23 19:31:04 +0000343 i = min;
Akinobu Mita0a329d22012-03-23 15:02:04 -0700344 for_each_set_bit_from(i, lzo_blocks[0]->sync_bmp,
Mark Brownac8d91c2012-02-23 19:31:04 +0000345 lzo_blocks[0]->sync_bmp_nbits) {
346 if (i > max)
347 continue;
348
Dimitris Papastamos2cbbb572011-09-19 14:34:03 +0100349 ret = regcache_read(map, i, &val);
350 if (ret)
351 return ret;
Mark Browna3c37742012-02-23 20:11:58 +0000352
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
Dimitris Papastamos2cbbb572011-09-19 14:34:03 +0100358 map->cache_bypass = 1;
Dimitris Papastamos13753a92011-09-29 14:36:25 +0100359 ret = _regmap_write(map, i, val);
Dimitris Papastamos2cbbb572011-09-19 14:34:03 +0100360 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
370struct regcache_ops regcache_lzo_ops = {
Mark Brown50b776f2011-11-02 15:00:03 +0000371 .type = REGCACHE_COMPRESSED,
Dimitris Papastamos2cbbb572011-09-19 14:34:03 +0100372 .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};