LXR linux/drivers/misc/cb710/sgbuf2.c

   1// SPDX-License-Identifier: GPL-2.0-only
   2/*
   3 *  cb710/sgbuf2.c
   4 *
   5 *  Copyright by Michał Mirosław, 2008-2009
   6 */
   7#include <linux/kernel.h>
   8#include <linux/module.h>
   9#include <linux/cb710.h>
  10
  11static bool sg_dwiter_next(struct sg_mapping_iter *miter)
  12{
  13        if (sg_miter_next(miter)) {
  14                miter->consumed = 0;
  15                return true;
  16        } else
  17                return false;
  18}
  19
  20static bool sg_dwiter_is_at_end(struct sg_mapping_iter *miter)
  21{
  22        return miter->length == miter->consumed && !sg_dwiter_next(miter);
  23}
  24
  25static uint32_t sg_dwiter_read_buffer(struct sg_mapping_iter *miter)
  26{
  27        size_t len, left = 4;
  28        uint32_t data;
  29        void *addr = &data;
  30
  31        do {
  32                len = min(miter->length - miter->consumed, left);
  33                memcpy(addr, miter->addr + miter->consumed, len);
  34                miter->consumed += len;
  35                left -= len;
  36                if (!left)
  37                        return data;
  38                addr += len;
  39        } while (sg_dwiter_next(miter));
  40
  41        memset(addr, 0, left);
  42        return data;
  43}
  44
  45static inline bool needs_unaligned_copy(const void *ptr)
  46{
  47#ifdef CONFIG_HAVE_EFFICIENT_UNALIGNED_ACCESS
  48        return false;
  49#else
  50        return ((ptr - NULL) & 3) != 0;
  51#endif
  52}
  53
  54static bool sg_dwiter_get_next_block(struct sg_mapping_iter *miter, uint32_t **ptr)
  55{
  56        size_t len;
  57
  58        if (sg_dwiter_is_at_end(miter))
  59                return true;
  60
  61        len = miter->length - miter->consumed;
  62
  63        if (likely(len >= 4 && !needs_unaligned_copy(
  64                        miter->addr + miter->consumed))) {
  65                *ptr = miter->addr + miter->consumed;
  66                miter->consumed += 4;
  67                return true;
  68        }
  69
  70        return false;
  71}
  72
  73/**
  74 * cb710_sg_dwiter_read_next_block() - get next 32-bit word from sg buffer
  75 * @miter: sg mapping iterator used for reading
  76 *
  77 * Description:
  78 *   Returns 32-bit word starting at byte pointed to by @miter@
  79 *   handling any alignment issues.  Bytes past the buffer's end
  80 *   are not accessed (read) but are returned as zeroes.  @miter@
  81 *   is advanced by 4 bytes or to the end of buffer whichever is
  82 *   closer.
  83 *
  84 * Context:
  85 *   Same requirements as in sg_miter_next().
  86 *
  87 * Returns:
  88 *   32-bit word just read.
  89 */
  90uint32_t cb710_sg_dwiter_read_next_block(struct sg_mapping_iter *miter)
  91{
  92        uint32_t *ptr = NULL;
  93
  94        if (likely(sg_dwiter_get_next_block(miter, &ptr)))
  95                return ptr ? *ptr : 0;
  96
  97        return sg_dwiter_read_buffer(miter);
  98}
  99EXPORT_SYMBOL_GPL(cb710_sg_dwiter_read_next_block);
 100
 101static void sg_dwiter_write_slow(struct sg_mapping_iter *miter, uint32_t data)
 102{
 103        size_t len, left = 4;
 104        void *addr = &data;
 105
 106        do {
 107                len = min(miter->length - miter->consumed, left);
 108                memcpy(miter->addr, addr, len);
 109                miter->consumed += len;
 110                left -= len;
 111                if (!left)
 112                        return;
 113                addr += len;
 114        } while (sg_dwiter_next(miter));
 115}
 116
 117/**
 118 * cb710_sg_dwiter_write_next_block() - write next 32-bit word to sg buffer
 119 * @miter: sg mapping iterator used for writing
 120 * @data: data to write to sg buffer
 121 *
 122 * Description:
 123 *   Writes 32-bit word starting at byte pointed to by @miter@
 124 *   handling any alignment issues.  Bytes which would be written
 125 *   past the buffer's end are silently discarded. @miter@ is
 126 *   advanced by 4 bytes or to the end of buffer whichever is closer.
 127 *
 128 * Context:
 129 *   Same requirements as in sg_miter_next().
 130 */
 131void cb710_sg_dwiter_write_next_block(struct sg_mapping_iter *miter, uint32_t data)
 132{
 133        uint32_t *ptr = NULL;
 134
 135        if (likely(sg_dwiter_get_next_block(miter, &ptr))) {
 136                if (ptr)
 137                        *ptr = data;
 138                else
 139                        return;
 140        } else
 141                sg_dwiter_write_slow(miter, data);
 142}
 143EXPORT_SYMBOL_GPL(cb710_sg_dwiter_write_next_block);
 144
 145