Dimitris Papastamos | 2cbbb57 | 2011-09-19 14:34:03 +0100 | [diff] [blame] | 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> |
Paul Gortmaker | 51990e8 | 2012-01-22 11:23:42 -0500 | [diff] [blame] | 14 | #include <linux/device.h> |
Dimitris Papastamos | 2cbbb57 | 2011-09-19 14:34:03 +0100 | [diff] [blame] | 15 | #include <linux/lzo.h> |
| 16 | |
| 17 | #include "internal.h" |
| 18 | |
Lars-Peter Clausen | c2b1ecd | 2011-11-15 13:34:41 +0100 | [diff] [blame] | 19 | static int regcache_lzo_exit(struct regmap *map); |
| 20 | |
Dimitris Papastamos | 2cbbb57 | 2011-09-19 14:34:03 +0100 | [diff] [blame] | 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 |
Mark Brown | 82732bd | 2011-10-18 00:37:00 +0100 | [diff] [blame] | 33 | static int regcache_lzo_block_count(struct regmap *map) |
Dimitris Papastamos | 2cbbb57 | 2011-09-19 14:34:03 +0100 | [diff] [blame] | 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 | { |
Stephen Warren | f01ee60 | 2012-04-09 13:40:24 -0600 | [diff] [blame] | 111 | return ((reg / map->reg_stride) * map->cache_word_size) / |
Mark Brown | 82732bd | 2011-10-18 00:37:00 +0100 | [diff] [blame] | 112 | DIV_ROUND_UP(map->cache_size_raw, |
| 113 | regcache_lzo_block_count(map)); |
Dimitris Papastamos | 2cbbb57 | 2011-09-19 14:34:03 +0100 | [diff] [blame] | 114 | } |
| 115 | |
| 116 | static inline int regcache_lzo_get_blkpos(struct regmap *map, |
| 117 | unsigned int reg) |
| 118 | { |
Stephen Warren | f01ee60 | 2012-04-09 13:40:24 -0600 | [diff] [blame] | 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); |
Dimitris Papastamos | 2cbbb57 | 2011-09-19 14:34:03 +0100 | [diff] [blame] | 123 | } |
| 124 | |
| 125 | static inline int regcache_lzo_get_blksize(struct regmap *map) |
| 126 | { |
Mark Brown | 82732bd | 2011-10-18 00:37:00 +0100 | [diff] [blame] | 127 | return DIV_ROUND_UP(map->cache_size_raw, |
| 128 | regcache_lzo_block_count(map)); |
Dimitris Papastamos | 2cbbb57 | 2011-09-19 14:34:03 +0100 | [diff] [blame] | 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 | |
Mark Brown | 82732bd | 2011-10-18 00:37:00 +0100 | [diff] [blame] | 141 | blkcount = regcache_lzo_block_count(map); |
Dimitris Papastamos | 2cbbb57 | 2011-09-19 14:34:03 +0100 | [diff] [blame] | 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: |
Lars-Peter Clausen | c2b1ecd | 2011-11-15 13:34:41 +0100 | [diff] [blame] | 200 | regcache_lzo_exit(map); |
Dimitris Papastamos | 2cbbb57 | 2011-09-19 14:34:03 +0100 | [diff] [blame] | 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 | |
Mark Brown | 82732bd | 2011-10-18 00:37:00 +0100 | [diff] [blame] | 213 | blkcount = regcache_lzo_block_count(map); |
Dimitris Papastamos | 2cbbb57 | 2011-09-19 14:34:03 +0100 | [diff] [blame] | 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 | |
Dimitris Papastamos | 2cbbb57 | 2011-09-19 14:34:03 +0100 | [diff] [blame] | 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 */ |
Mark Brown | 879082c | 2013-02-21 18:03:13 +0000 | [diff] [blame] | 263 | *value = regcache_get_val(map, lzo_block->dst, blkpos); |
Dimitris Papastamos | 2cbbb57 | 2011-09-19 14:34:03 +0100 | [diff] [blame] | 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; |
Mark Brown | 6e6ace0 | 2011-10-09 13:23:31 +0100 | [diff] [blame] | 269 | |
| 270 | return ret; |
Dimitris Papastamos | 2cbbb57 | 2011-09-19 14:34:03 +0100 | [diff] [blame] | 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 */ |
Mark Brown | 879082c | 2013-02-21 18:03:13 +0000 | [diff] [blame] | 306 | if (regcache_set_val(map, lzo_block->dst, blkpos, value)) { |
Dimitris Papastamos | 2cbbb57 | 2011-09-19 14:34:03 +0100 | [diff] [blame] | 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 */ |
Stephen Warren | f01ee60 | 2012-04-09 13:40:24 -0600 | [diff] [blame] | 324 | set_bit(reg / map->reg_stride, lzo_block->sync_bmp); |
Dimitris Papastamos | 2cbbb57 | 2011-09-19 14:34:03 +0100 | [diff] [blame] | 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 | |
Mark Brown | ac8d91c | 2012-02-23 19:31:04 +0000 | [diff] [blame] | 334 | static int regcache_lzo_sync(struct regmap *map, unsigned int min, |
| 335 | unsigned int max) |
Dimitris Papastamos | 2cbbb57 | 2011-09-19 14:34:03 +0100 | [diff] [blame] | 336 | { |
| 337 | struct regcache_lzo_ctx **lzo_blocks; |
| 338 | unsigned int val; |
| 339 | int i; |
| 340 | int ret; |
| 341 | |
| 342 | lzo_blocks = map->cache; |
Mark Brown | ac8d91c | 2012-02-23 19:31:04 +0000 | [diff] [blame] | 343 | i = min; |
Akinobu Mita | 0a329d2 | 2012-03-23 15:02:04 -0700 | [diff] [blame] | 344 | for_each_set_bit_from(i, lzo_blocks[0]->sync_bmp, |
Mark Brown | ac8d91c | 2012-02-23 19:31:04 +0000 | [diff] [blame] | 345 | lzo_blocks[0]->sync_bmp_nbits) { |
| 346 | if (i > max) |
| 347 | continue; |
| 348 | |
Dimitris Papastamos | 2cbbb57 | 2011-09-19 14:34:03 +0100 | [diff] [blame] | 349 | ret = regcache_read(map, i, &val); |
| 350 | if (ret) |
| 351 | return ret; |
Mark Brown | a3c3774 | 2012-02-23 20:11:58 +0000 | [diff] [blame] | 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 | |
Dimitris Papastamos | 2cbbb57 | 2011-09-19 14:34:03 +0100 | [diff] [blame] | 358 | map->cache_bypass = 1; |
Dimitris Papastamos | 13753a9 | 2011-09-29 14:36:25 +0100 | [diff] [blame] | 359 | ret = _regmap_write(map, i, val); |
Dimitris Papastamos | 2cbbb57 | 2011-09-19 14:34:03 +0100 | [diff] [blame] | 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 = { |
Mark Brown | 50b776f | 2011-11-02 15:00:03 +0000 | [diff] [blame] | 371 | .type = REGCACHE_COMPRESSED, |
Dimitris Papastamos | 2cbbb57 | 2011-09-19 14:34:03 +0100 | [diff] [blame] | 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 | }; |