LXR linux/drivers/base/regmap/regcache-lzo.c

   1// SPDX-License-Identifier: GPL-2.0
   2//
   3// Register cache access API - LZO caching support
   4//
   5// Copyright 2011 Wolfson Microelectronics plc
   6//
   7// Author: Dimitris Papastamos <dp@opensource.wolfsonmicro.com>
   8
   9#include <linux/device.h>
  10#include <linux/lzo.h>
  11#include <linux/slab.h>
  12
  13#include "internal.h"
  14
  15static int regcache_lzo_exit(struct regmap *map);
  16
  17struct regcache_lzo_ctx {
  18        void *wmem;
  19        void *dst;
  20        const void *src;
  21        size_t src_len;
  22        size_t dst_len;
  23        size_t decompressed_size;
  24        unsigned long *sync_bmp;
  25        int sync_bmp_nbits;
  26};
  27
  28#define LZO_BLOCK_NUM 8
  29static int regcache_lzo_block_count(struct regmap *map)
  30{
  31        return LZO_BLOCK_NUM;
  32}
  33
  34static int regcache_lzo_prepare(struct regcache_lzo_ctx *lzo_ctx)
  35{
  36        lzo_ctx->wmem = kmalloc(LZO1X_MEM_COMPRESS, GFP_KERNEL);
  37        if (!lzo_ctx->wmem)
  38                return -ENOMEM;
  39        return 0;
  40}
  41
  42static int regcache_lzo_compress(struct regcache_lzo_ctx *lzo_ctx)
  43{
  44        size_t compress_size;
  45        int ret;
  46
  47        ret = lzo1x_1_compress(lzo_ctx->src, lzo_ctx->src_len,
  48                               lzo_ctx->dst, &compress_size, lzo_ctx->wmem);
  49        if (ret != LZO_E_OK || compress_size > lzo_ctx->dst_len)
  50                return -EINVAL;
  51        lzo_ctx->dst_len = compress_size;
  52        return 0;
  53}
  54
  55static int regcache_lzo_decompress(struct regcache_lzo_ctx *lzo_ctx)
  56{
  57        size_t dst_len;
  58        int ret;
  59
  60        dst_len = lzo_ctx->dst_len;
  61        ret = lzo1x_decompress_safe(lzo_ctx->src, lzo_ctx->src_len,
  62                                    lzo_ctx->dst, &dst_len);
  63        if (ret != LZO_E_OK || dst_len != lzo_ctx->dst_len)
  64                return -EINVAL;
  65        return 0;
  66}
  67
  68static int regcache_lzo_compress_cache_block(struct regmap *map,
  69                struct regcache_lzo_ctx *lzo_ctx)
  70{
  71        int ret;
  72
  73        lzo_ctx->dst_len = lzo1x_worst_compress(PAGE_SIZE);
  74        lzo_ctx->dst = kmalloc(lzo_ctx->dst_len, GFP_KERNEL);
  75        if (!lzo_ctx->dst) {
  76                lzo_ctx->dst_len = 0;
  77                return -ENOMEM;
  78        }
  79
  80        ret = regcache_lzo_compress(lzo_ctx);
  81        if (ret < 0)
  82                return ret;
  83        return 0;
  84}
  85
  86static int regcache_lzo_decompress_cache_block(struct regmap *map,
  87                struct regcache_lzo_ctx *lzo_ctx)
  88{
  89        int ret;
  90
  91        lzo_ctx->dst_len = lzo_ctx->decompressed_size;
  92        lzo_ctx->dst = kmalloc(lzo_ctx->dst_len, GFP_KERNEL);
  93        if (!lzo_ctx->dst) {
  94                lzo_ctx->dst_len = 0;
  95                return -ENOMEM;
  96        }
  97
  98        ret = regcache_lzo_decompress(lzo_ctx);
  99        if (ret < 0)
 100                return ret;
 101        return 0;
 102}
 103
 104static inline int regcache_lzo_get_blkindex(struct regmap *map,
 105                                            unsigned int reg)
 106{
 107        return ((reg / map->reg_stride) * map->cache_word_size) /
 108                DIV_ROUND_UP(map->cache_size_raw,
 109                             regcache_lzo_block_count(map));
 110}
 111
 112static inline int regcache_lzo_get_blkpos(struct regmap *map,
 113                                          unsigned int reg)
 114{
 115        return (reg / map->reg_stride) %
 116                    (DIV_ROUND_UP(map->cache_size_raw,
 117                                  regcache_lzo_block_count(map)) /
 118                     map->cache_word_size);
 119}
 120
 121static inline int regcache_lzo_get_blksize(struct regmap *map)
 122{
 123        return DIV_ROUND_UP(map->cache_size_raw,
 124                            regcache_lzo_block_count(map));
 125}
 126
 127static int regcache_lzo_init(struct regmap *map)
 128{
 129        struct regcache_lzo_ctx **lzo_blocks;
 130        size_t bmp_size;
 131        int ret, i, blksize, blkcount;
 132        const char *p, *end;
 133        unsigned long *sync_bmp;
 134
 135        ret = 0;
 136
 137        blkcount = regcache_lzo_block_count(map);
 138        map->cache = kcalloc(blkcount, sizeof(*lzo_blocks),
 139                             GFP_KERNEL);
 140        if (!map->cache)
 141                return -ENOMEM;
 142        lzo_blocks = map->cache;
 143
 144        /*
 145         * allocate a bitmap to be used when syncing the cache with
 146         * the hardware.  Each time a register is modified, the corresponding
 147         * bit is set in the bitmap, so we know that we have to sync
 148         * that register.
 149         */
 150        bmp_size = map->num_reg_defaults_raw;
 151        sync_bmp = bitmap_zalloc(bmp_size, GFP_KERNEL);
 152        if (!sync_bmp) {
 153                ret = -ENOMEM;
 154                goto err;
 155        }
 156
 157        /* allocate the lzo blocks and initialize them */
 158        for (i = 0; i < blkcount; i++) {
 159                lzo_blocks[i] = kzalloc(sizeof **lzo_blocks,
 160                                        GFP_KERNEL);
 161                if (!lzo_blocks[i]) {
 162                        bitmap_free(sync_bmp);
 163                        ret = -ENOMEM;
 164                        goto err;
 165                }
 166                lzo_blocks[i]->sync_bmp = sync_bmp;
 167                lzo_blocks[i]->sync_bmp_nbits = bmp_size;
 168                /* alloc the working space for the compressed block */
 169                ret = regcache_lzo_prepare(lzo_blocks[i]);
 170                if (ret < 0)
 171                        goto err;
 172        }
 173
 174        blksize = regcache_lzo_get_blksize(map);
 175        p = map->reg_defaults_raw;
 176        end = map->reg_defaults_raw + map->cache_size_raw;
 177        /* compress the register map and fill the lzo blocks */
 178        for (i = 0; i < blkcount; i++, p += blksize) {
 179                lzo_blocks[i]->src = p;
 180                if (p + blksize > end)
 181                        lzo_blocks[i]->src_len = end - p;
 182                else
 183                        lzo_blocks[i]->src_len = blksize;
 184                ret = regcache_lzo_compress_cache_block(map,
 185                                                       lzo_blocks[i]);
 186                if (ret < 0)
 187                        goto err;
 188                lzo_blocks[i]->decompressed_size =
 189                        lzo_blocks[i]->src_len;
 190        }
 191
 192        return 0;
 193err:
 194        regcache_lzo_exit(map);
 195        return ret;
 196}
 197
 198static int regcache_lzo_exit(struct regmap *map)
 199{
 200        struct regcache_lzo_ctx **lzo_blocks;
 201        int i, blkcount;
 202
 203        lzo_blocks = map->cache;
 204        if (!lzo_blocks)
 205                return 0;
 206
 207        blkcount = regcache_lzo_block_count(map);
 208        /*
 209         * the pointer to the bitmap used for syncing the cache
 210         * is shared amongst all lzo_blocks.  Ensure it is freed
 211         * only once.
 212         */
 213        if (lzo_blocks[0])
 214                bitmap_free(lzo_blocks[0]->sync_bmp);
 215        for (i = 0; i < blkcount; i++) {
 216                if (lzo_blocks[i]) {
 217                        kfree(lzo_blocks[i]->wmem);
 218                        kfree(lzo_blocks[i]->dst);
 219                }
 220                /* each lzo_block is a pointer returned by kmalloc or NULL */
 221                kfree(lzo_blocks[i]);
 222        }
 223        kfree(lzo_blocks);
 224        map->cache = NULL;
 225        return 0;
 226}
 227
 228static int regcache_lzo_read(struct regmap *map,
 229                             unsigned int reg, unsigned int *value)
 230{
 231        struct regcache_lzo_ctx *lzo_block, **lzo_blocks;
 232        int ret, blkindex, blkpos;
 233        size_t tmp_dst_len;
 234        void *tmp_dst;
 235
 236        /* index of the compressed lzo block */
 237        blkindex = regcache_lzo_get_blkindex(map, reg);
 238        /* register index within the decompressed block */
 239        blkpos = regcache_lzo_get_blkpos(map, reg);
 240        lzo_blocks = map->cache;
 241        lzo_block = lzo_blocks[blkindex];
 242
 243        /* save the pointer and length of the compressed block */
 244        tmp_dst = lzo_block->dst;
 245        tmp_dst_len = lzo_block->dst_len;
 246
 247        /* prepare the source to be the compressed block */
 248        lzo_block->src = lzo_block->dst;
 249        lzo_block->src_len = lzo_block->dst_len;
 250
 251        /* decompress the block */
 252        ret = regcache_lzo_decompress_cache_block(map, lzo_block);
 253        if (ret >= 0)
 254                /* fetch the value from the cache */
 255                *value = regcache_get_val(map, lzo_block->dst, blkpos);
 256
 257        kfree(lzo_block->dst);
 258        /* restore the pointer and length of the compressed block */
 259        lzo_block->dst = tmp_dst;
 260        lzo_block->dst_len = tmp_dst_len;
 261
 262        return ret;
 263}
 264
 265static int regcache_lzo_write(struct regmap *map,
 266                              unsigned int reg, unsigned int value)
 267{
 268        struct regcache_lzo_ctx *lzo_block, **lzo_blocks;
 269        int ret, blkindex, blkpos;
 270        size_t tmp_dst_len;
 271        void *tmp_dst;
 272
 273        /* index of the compressed lzo block */
 274        blkindex = regcache_lzo_get_blkindex(map, reg);
 275        /* register index within the decompressed block */
 276        blkpos = regcache_lzo_get_blkpos(map, reg);
 277        lzo_blocks = map->cache;
 278        lzo_block = lzo_blocks[blkindex];
 279
 280        /* save the pointer and length of the compressed block */
 281        tmp_dst = lzo_block->dst;
 282        tmp_dst_len = lzo_block->dst_len;
 283
 284        /* prepare the source to be the compressed block */
 285        lzo_block->src = lzo_block->dst;
 286        lzo_block->src_len = lzo_block->dst_len;
 287
 288        /* decompress the block */
 289        ret = regcache_lzo_decompress_cache_block(map, lzo_block);
 290        if (ret < 0) {
 291                kfree(lzo_block->dst);
 292                goto out;
 293        }
 294
 295        /* write the new value to the cache */
 296        if (regcache_set_val(map, lzo_block->dst, blkpos, value)) {
 297                kfree(lzo_block->dst);
 298                goto out;
 299        }
 300
 301        /* prepare the source to be the decompressed block */
 302        lzo_block->src = lzo_block->dst;
 303        lzo_block->src_len = lzo_block->dst_len;
 304
 305        /* compress the block */
 306        ret = regcache_lzo_compress_cache_block(map, lzo_block);
 307        if (ret < 0) {
 308                kfree(lzo_block->dst);
 309                kfree(lzo_block->src);
 310                goto out;
 311        }
 312
 313        /* set the bit so we know we have to sync this register */
 314        set_bit(reg / map->reg_stride, lzo_block->sync_bmp);
 315        kfree(tmp_dst);
 316        kfree(lzo_block->src);
 317        return 0;
 318out:
 319        lzo_block->dst = tmp_dst;
 320        lzo_block->dst_len = tmp_dst_len;
 321        return ret;
 322}
 323
 324static int regcache_lzo_sync(struct regmap *map, unsigned int min,
 325                             unsigned int max)
 326{
 327        struct regcache_lzo_ctx **lzo_blocks;
 328        unsigned int val;
 329        int i;
 330        int ret;
 331
 332        lzo_blocks = map->cache;
 333        i = min;
 334        for_each_set_bit_from(i, lzo_blocks[0]->sync_bmp,
 335                              lzo_blocks[0]->sync_bmp_nbits) {
 336                if (i > max)
 337                        continue;
 338
 339                ret = regcache_read(map, i, &val);
 340                if (ret)
 341                        return ret;
 342
 343                /* Is this the hardware default?  If so skip. */
 344                ret = regcache_lookup_reg(map, i);
 345                if (ret > 0 && val == map->reg_defaults[ret].def)
 346                        continue;
 347
 348                map->cache_bypass = true;
 349                ret = _regmap_write(map, i, val);
 350                map->cache_bypass = false;
 351                if (ret)
 352                        return ret;
 353                dev_dbg(map->dev, "Synced register %#x, value %#x\n",
 354                        i, val);
 355        }
 356
 357        return 0;
 358}
 359
 360struct regcache_ops regcache_lzo_ops = {
 361        .type = REGCACHE_COMPRESSED,
 362        .name = "lzo",
 363        .init = regcache_lzo_init,
 364        .exit = regcache_lzo_exit,
 365        .read = regcache_lzo_read,
 366        .write = regcache_lzo_write,
 367        .sync = regcache_lzo_sync
 368};
 369