base/regmap/regcache-lzo.c

0001 // SPDX-License-Identifier: GPL-2.0
0002 //
0003 // Register cache access API - LZO caching support
0004 //
0005 // Copyright 2011 Wolfson Microelectronics plc
0006 //
0007 // Author: Dimitris Papastamos <dp@opensource.wolfsonmicro.com>
0008
0009 #include <linux/device.h>
0010 #include <linux/lzo.h>
0011 #include <linux/slab.h>
0012
0013 #include "internal.h"
0014
0015 static int regcache_lzo_exit(struct regmap *map);
0016
0017 struct regcache_lzo_ctx {
0018     void *wmem;
0019     void *dst;
0020     const void *src;
0021     size_t src_len;
0022     size_t dst_len;
0023     size_t decompressed_size;
0024     unsigned long *sync_bmp;
0025     int sync_bmp_nbits;
0026 };
0027
0028 #define LZO_BLOCK_NUM 8
0029 static int regcache_lzo_block_count(struct regmap *map)
0030 {
0031     return LZO_BLOCK_NUM;
0032 }
0033
0034 static int regcache_lzo_prepare(struct regcache_lzo_ctx *lzo_ctx)
0035 {
0036     lzo_ctx->wmem = kmalloc(LZO1X_MEM_COMPRESS, GFP_KERNEL);
0037     if (!lzo_ctx->wmem)
0038         return -ENOMEM;
0039     return 0;
0040 }
0041
0042 static int regcache_lzo_compress(struct regcache_lzo_ctx *lzo_ctx)
0043 {
0044     size_t compress_size;
0045     int ret;
0046
0047     ret = lzo1x_1_compress(lzo_ctx->src, lzo_ctx->src_len,
0048                    lzo_ctx->dst, &compress_size, lzo_ctx->wmem);
0049     if (ret != LZO_E_OK || compress_size > lzo_ctx->dst_len)
0050         return -EINVAL;
0051     lzo_ctx->dst_len = compress_size;
0052     return 0;
0053 }
0054
0055 static int regcache_lzo_decompress(struct regcache_lzo_ctx *lzo_ctx)
0056 {
0057     size_t dst_len;
0058     int ret;
0059
0060     dst_len = lzo_ctx->dst_len;
0061     ret = lzo1x_decompress_safe(lzo_ctx->src, lzo_ctx->src_len,
0062                     lzo_ctx->dst, &dst_len);
0063     if (ret != LZO_E_OK || dst_len != lzo_ctx->dst_len)
0064         return -EINVAL;
0065     return 0;
0066 }
0067
0068 static int regcache_lzo_compress_cache_block(struct regmap *map,
0069         struct regcache_lzo_ctx *lzo_ctx)
0070 {
0071     int ret;
0072
0073     lzo_ctx->dst_len = lzo1x_worst_compress(PAGE_SIZE);
0074     lzo_ctx->dst = kmalloc(lzo_ctx->dst_len, GFP_KERNEL);
0075     if (!lzo_ctx->dst) {
0076         lzo_ctx->dst_len = 0;
0077         return -ENOMEM;
0078     }
0079
0080     ret = regcache_lzo_compress(lzo_ctx);
0081     if (ret < 0)
0082         return ret;
0083     return 0;
0084 }
0085
0086 static int regcache_lzo_decompress_cache_block(struct regmap *map,
0087         struct regcache_lzo_ctx *lzo_ctx)
0088 {
0089     int ret;
0090
0091     lzo_ctx->dst_len = lzo_ctx->decompressed_size;
0092     lzo_ctx->dst = kmalloc(lzo_ctx->dst_len, GFP_KERNEL);
0093     if (!lzo_ctx->dst) {
0094         lzo_ctx->dst_len = 0;
0095         return -ENOMEM;
0096     }
0097
0098     ret = regcache_lzo_decompress(lzo_ctx);
0099     if (ret < 0)
0100         return ret;
0101     return 0;
0102 }
0103
0104 static inline int regcache_lzo_get_blkindex(struct regmap *map,
0105                         unsigned int reg)
0106 {
0107     return ((reg / map->reg_stride) * map->cache_word_size) /
0108         DIV_ROUND_UP(map->cache_size_raw,
0109                  regcache_lzo_block_count(map));
0110 }
0111
0112 static inline int regcache_lzo_get_blkpos(struct regmap *map,
0113                       unsigned int reg)
0114 {
0115     return (reg / map->reg_stride) %
0116             (DIV_ROUND_UP(map->cache_size_raw,
0117                   regcache_lzo_block_count(map)) /
0118              map->cache_word_size);
0119 }
0120
0121 static inline int regcache_lzo_get_blksize(struct regmap *map)
0122 {
0123     return DIV_ROUND_UP(map->cache_size_raw,
0124                 regcache_lzo_block_count(map));
0125 }
0126
0127 static int regcache_lzo_init(struct regmap *map)
0128 {
0129     struct regcache_lzo_ctx **lzo_blocks;
0130     size_t bmp_size;
0131     int ret, i, blksize, blkcount;
0132     const char *p, *end;
0133     unsigned long *sync_bmp;
0134
0135     ret = 0;
0136
0137     blkcount = regcache_lzo_block_count(map);
0138     map->cache = kcalloc(blkcount, sizeof(*lzo_blocks),
0139                  GFP_KERNEL);
0140     if (!map->cache)
0141         return -ENOMEM;
0142     lzo_blocks = map->cache;
0143
0144     /*
0145      * allocate a bitmap to be used when syncing the cache with
0146      * the hardware.  Each time a register is modified, the corresponding
0147      * bit is set in the bitmap, so we know that we have to sync
0148      * that register.
0149      */
0150     bmp_size = map->num_reg_defaults_raw;
0151     sync_bmp = bitmap_zalloc(bmp_size, GFP_KERNEL);
0152     if (!sync_bmp) {
0153         ret = -ENOMEM;
0154         goto err;
0155     }
0156
0157     /* allocate the lzo blocks and initialize them */
0158     for (i = 0; i < blkcount; i++) {
0159         lzo_blocks[i] = kzalloc(sizeof **lzo_blocks,
0160                     GFP_KERNEL);
0161         if (!lzo_blocks[i]) {
0162             bitmap_free(sync_bmp);
0163             ret = -ENOMEM;
0164             goto err;
0165         }
0166         lzo_blocks[i]->sync_bmp = sync_bmp;
0167         lzo_blocks[i]->sync_bmp_nbits = bmp_size;
0168         /* alloc the working space for the compressed block */
0169         ret = regcache_lzo_prepare(lzo_blocks[i]);
0170         if (ret < 0)
0171             goto err;
0172     }
0173
0174     blksize = regcache_lzo_get_blksize(map);
0175     p = map->reg_defaults_raw;
0176     end = map->reg_defaults_raw + map->cache_size_raw;
0177     /* compress the register map and fill the lzo blocks */
0178     for (i = 0; i < blkcount; i++, p += blksize) {
0179         lzo_blocks[i]->src = p;
0180         if (p + blksize > end)
0181             lzo_blocks[i]->src_len = end - p;
0182         else
0183             lzo_blocks[i]->src_len = blksize;
0184         ret = regcache_lzo_compress_cache_block(map,
0185                                lzo_blocks[i]);
0186         if (ret < 0)
0187             goto err;
0188         lzo_blocks[i]->decompressed_size =
0189             lzo_blocks[i]->src_len;
0190     }
0191
0192     return 0;
0193 err:
0194     regcache_lzo_exit(map);
0195     return ret;
0196 }
0197
0198 static int regcache_lzo_exit(struct regmap *map)
0199 {
0200     struct regcache_lzo_ctx **lzo_blocks;
0201     int i, blkcount;
0202
0203     lzo_blocks = map->cache;
0204     if (!lzo_blocks)
0205         return 0;
0206
0207     blkcount = regcache_lzo_block_count(map);
0208     /*
0209      * the pointer to the bitmap used for syncing the cache
0210      * is shared amongst all lzo_blocks.  Ensure it is freed
0211      * only once.
0212      */
0213     if (lzo_blocks[0])
0214         bitmap_free(lzo_blocks[0]->sync_bmp);
0215     for (i = 0; i < blkcount; i++) {
0216         if (lzo_blocks[i]) {
0217             kfree(lzo_blocks[i]->wmem);
0218             kfree(lzo_blocks[i]->dst);
0219         }
0220         /* each lzo_block is a pointer returned by kmalloc or NULL */
0221         kfree(lzo_blocks[i]);
0222     }
0223     kfree(lzo_blocks);
0224     map->cache = NULL;
0225     return 0;
0226 }
0227
0228 static int regcache_lzo_read(struct regmap *map,
0229                  unsigned int reg, unsigned int *value)
0230 {
0231     struct regcache_lzo_ctx *lzo_block, **lzo_blocks;
0232     int ret, blkindex, blkpos;
0233     size_t tmp_dst_len;
0234     void *tmp_dst;
0235
0236     /* index of the compressed lzo block */
0237     blkindex = regcache_lzo_get_blkindex(map, reg);
0238     /* register index within the decompressed block */
0239     blkpos = regcache_lzo_get_blkpos(map, reg);
0240     lzo_blocks = map->cache;
0241     lzo_block = lzo_blocks[blkindex];
0242
0243     /* save the pointer and length of the compressed block */
0244     tmp_dst = lzo_block->dst;
0245     tmp_dst_len = lzo_block->dst_len;
0246
0247     /* prepare the source to be the compressed block */
0248     lzo_block->src = lzo_block->dst;
0249     lzo_block->src_len = lzo_block->dst_len;
0250
0251     /* decompress the block */
0252     ret = regcache_lzo_decompress_cache_block(map, lzo_block);
0253     if (ret >= 0)
0254         /* fetch the value from the cache */
0255         *value = regcache_get_val(map, lzo_block->dst, blkpos);
0256
0257     kfree(lzo_block->dst);
0258     /* restore the pointer and length of the compressed block */
0259     lzo_block->dst = tmp_dst;
0260     lzo_block->dst_len = tmp_dst_len;
0261
0262     return ret;
0263 }
0264
0265 static int regcache_lzo_write(struct regmap *map,
0266                   unsigned int reg, unsigned int value)
0267 {
0268     struct regcache_lzo_ctx *lzo_block, **lzo_blocks;
0269     int ret, blkindex, blkpos;
0270     size_t tmp_dst_len;
0271     void *tmp_dst;
0272
0273     /* index of the compressed lzo block */
0274     blkindex = regcache_lzo_get_blkindex(map, reg);
0275     /* register index within the decompressed block */
0276     blkpos = regcache_lzo_get_blkpos(map, reg);
0277     lzo_blocks = map->cache;
0278     lzo_block = lzo_blocks[blkindex];
0279
0280     /* save the pointer and length of the compressed block */
0281     tmp_dst = lzo_block->dst;
0282     tmp_dst_len = lzo_block->dst_len;
0283
0284     /* prepare the source to be the compressed block */
0285     lzo_block->src = lzo_block->dst;
0286     lzo_block->src_len = lzo_block->dst_len;
0287
0288     /* decompress the block */
0289     ret = regcache_lzo_decompress_cache_block(map, lzo_block);
0290     if (ret < 0) {
0291         kfree(lzo_block->dst);
0292         goto out;
0293     }
0294
0295     /* write the new value to the cache */
0296     if (regcache_set_val(map, lzo_block->dst, blkpos, value)) {
0297         kfree(lzo_block->dst);
0298         goto out;
0299     }
0300
0301     /* prepare the source to be the decompressed block */
0302     lzo_block->src = lzo_block->dst;
0303     lzo_block->src_len = lzo_block->dst_len;
0304
0305     /* compress the block */
0306     ret = regcache_lzo_compress_cache_block(map, lzo_block);
0307     if (ret < 0) {
0308         kfree(lzo_block->dst);
0309         kfree(lzo_block->src);
0310         goto out;
0311     }
0312
0313     /* set the bit so we know we have to sync this register */
0314     set_bit(reg / map->reg_stride, lzo_block->sync_bmp);
0315     kfree(tmp_dst);
0316     kfree(lzo_block->src);
0317     return 0;
0318 out:
0319     lzo_block->dst = tmp_dst;
0320     lzo_block->dst_len = tmp_dst_len;
0321     return ret;
0322 }
0323
0324 static int regcache_lzo_sync(struct regmap *map, unsigned int min,
0325                  unsigned int max)
0326 {
0327     struct regcache_lzo_ctx **lzo_blocks;
0328     unsigned int val;
0329     int i;
0330     int ret;
0331
0332     lzo_blocks = map->cache;
0333     i = min;
0334     for_each_set_bit_from(i, lzo_blocks[0]->sync_bmp,
0335                   lzo_blocks[0]->sync_bmp_nbits) {
0336         if (i > max)
0337             continue;
0338
0339         ret = regcache_read(map, i, &val);
0340         if (ret)
0341             return ret;
0342
0343         /* Is this the hardware default?  If so skip. */
0344         ret = regcache_lookup_reg(map, i);
0345         if (ret > 0 && val == map->reg_defaults[ret].def)
0346             continue;
0347
0348         map->cache_bypass = true;
0349         ret = _regmap_write(map, i, val);
0350         map->cache_bypass = false;
0351         if (ret)
0352             return ret;
0353         dev_dbg(map->dev, "Synced register %#x, value %#x\n",
0354             i, val);
0355     }
0356
0357     return 0;
0358 }
0359
0360 struct regcache_ops regcache_lzo_ops = {
0361     .type = REGCACHE_COMPRESSED,
0362     .name = "lzo",
0363     .init = regcache_lzo_init,
0364     .exit = regcache_lzo_exit,
0365     .read = regcache_lzo_read,
0366     .write = regcache_lzo_write,
0367     .sync = regcache_lzo_sync
0368 };