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 147