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 145