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
  14 static 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
  23 static bool sg_dwiter_is_at_end(struct sg_mapping_iter *miter)
  24 {
  25         return miter->length == miter->consumed && !sg_dwiter_next(miter);
  26 }
  27
  28 static 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
  48 static 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
  57 static 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  */
  93 uint32_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 }
 102 EXPORT_SYMBOL_GPL(cb710_sg_dwiter_read_next_block);
 103
 104 static 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  */
 133 void 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 }
 145 EXPORT_SYMBOL_GPL(cb710_sg_dwiter_write_next_block);
 146