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

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