misc/cb710/sgbuf2.c

0001 // SPDX-License-Identifier: GPL-2.0-only
0002 /*
0003  *  cb710/sgbuf2.c
0004  *
0005  *  Copyright by Michał Mirosław, 2008-2009
0006  */
0007 #include <linux/kernel.h>
0008 #include <linux/module.h>
0009 #include <linux/cb710.h>
0010
0011 static bool sg_dwiter_next(struct sg_mapping_iter *miter)
0012 {
0013     if (sg_miter_next(miter)) {
0014         miter->consumed = 0;
0015         return true;
0016     } else
0017         return false;
0018 }
0019
0020 static bool sg_dwiter_is_at_end(struct sg_mapping_iter *miter)
0021 {
0022     return miter->length == miter->consumed && !sg_dwiter_next(miter);
0023 }
0024
0025 static uint32_t sg_dwiter_read_buffer(struct sg_mapping_iter *miter)
0026 {
0027     size_t len, left = 4;
0028     uint32_t data;
0029     void *addr = &data;
0030
0031     do {
0032         len = min(miter->length - miter->consumed, left);
0033         memcpy(addr, miter->addr + miter->consumed, len);
0034         miter->consumed += len;
0035         left -= len;
0036         if (!left)
0037             return data;
0038         addr += len;
0039     } while (sg_dwiter_next(miter));
0040
0041     memset(addr, 0, left);
0042     return data;
0043 }
0044
0045 static inline bool needs_unaligned_copy(const void *ptr)
0046 {
0047 #ifdef CONFIG_HAVE_EFFICIENT_UNALIGNED_ACCESS
0048     return false;
0049 #else
0050     return ((uintptr_t)ptr & 3) != 0;
0051 #endif
0052 }
0053
0054 static bool sg_dwiter_get_next_block(struct sg_mapping_iter *miter, uint32_t **ptr)
0055 {
0056     size_t len;
0057
0058     if (sg_dwiter_is_at_end(miter))
0059         return true;
0060
0061     len = miter->length - miter->consumed;
0062
0063     if (likely(len >= 4 && !needs_unaligned_copy(
0064             miter->addr + miter->consumed))) {
0065         *ptr = miter->addr + miter->consumed;
0066         miter->consumed += 4;
0067         return true;
0068     }
0069
0070     return false;
0071 }
0072
0073 /**
0074  * cb710_sg_dwiter_read_next_block() - get next 32-bit word from sg buffer
0075  * @miter: sg mapping iterator used for reading
0076  *
0077  * Description:
0078  *   Returns 32-bit word starting at byte pointed to by @miter@
0079  *   handling any alignment issues.  Bytes past the buffer's end
0080  *   are not accessed (read) but are returned as zeroes.  @miter@
0081  *   is advanced by 4 bytes or to the end of buffer whichever is
0082  *   closer.
0083  *
0084  * Context:
0085  *   Same requirements as in sg_miter_next().
0086  *
0087  * Returns:
0088  *   32-bit word just read.
0089  */
0090 uint32_t cb710_sg_dwiter_read_next_block(struct sg_mapping_iter *miter)
0091 {
0092     uint32_t *ptr = NULL;
0093
0094     if (likely(sg_dwiter_get_next_block(miter, &ptr)))
0095         return ptr ? *ptr : 0;
0096
0097     return sg_dwiter_read_buffer(miter);
0098 }
0099 EXPORT_SYMBOL_GPL(cb710_sg_dwiter_read_next_block);
0100
0101 static void sg_dwiter_write_slow(struct sg_mapping_iter *miter, uint32_t data)
0102 {
0103     size_t len, left = 4;
0104     void *addr = &data;
0105
0106     do {
0107         len = min(miter->length - miter->consumed, left);
0108         memcpy(miter->addr, addr, len);
0109         miter->consumed += len;
0110         left -= len;
0111         if (!left)
0112             return;
0113         addr += len;
0114     } while (sg_dwiter_next(miter));
0115 }
0116
0117 /**
0118  * cb710_sg_dwiter_write_next_block() - write next 32-bit word to sg buffer
0119  * @miter: sg mapping iterator used for writing
0120  * @data: data to write to sg buffer
0121  *
0122  * Description:
0123  *   Writes 32-bit word starting at byte pointed to by @miter@
0124  *   handling any alignment issues.  Bytes which would be written
0125  *   past the buffer's end are silently discarded. @miter@ is
0126  *   advanced by 4 bytes or to the end of buffer whichever is closer.
0127  *
0128  * Context:
0129  *   Same requirements as in sg_miter_next().
0130  */
0131 void cb710_sg_dwiter_write_next_block(struct sg_mapping_iter *miter, uint32_t data)
0132 {
0133     uint32_t *ptr = NULL;
0134
0135     if (likely(sg_dwiter_get_next_block(miter, &ptr))) {
0136         if (ptr)
0137             *ptr = data;
0138         else
0139             return;
0140     } else
0141         sg_dwiter_write_slow(miter, data);
0142 }
0143 EXPORT_SYMBOL_GPL(cb710_sg_dwiter_write_next_block);
0144