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
  11 static 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
  20 static bool sg_dwiter_is_at_end(struct sg_mapping_iter *miter)
  21 {
  22         return miter->length == miter->consumed && !sg_dwiter_next(miter);
  23 }
  24
  25 static 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
  45 static 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
  54 static 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  */
  90 uint32_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 }
  99 EXPORT_SYMBOL_GPL(cb710_sg_dwiter_read_next_block);
 100
 101 static 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  */
 131 void 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 }
 143 EXPORT_SYMBOL_GPL(cb710_sg_dwiter_write_next_block);
 144