145 lines · c
1// SPDX-License-Identifier: GPL-2.0-only2/*3 * cb710/sgbuf2.c4 *5 * Copyright by Michał Mirosław, 2008-20096 */7#include <linux/kernel.h>8#include <linux/module.h>9#include <linux/cb710.h>10 11static bool sg_dwiter_next(struct sg_mapping_iter *miter)12{13 if (sg_miter_next(miter)) {14 miter->consumed = 0;15 return true;16 } else17 return false;18}19 20static bool sg_dwiter_is_at_end(struct sg_mapping_iter *miter)21{22 return miter->length == miter->consumed && !sg_dwiter_next(miter);23}24 25static 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 45static inline bool needs_unaligned_copy(const void *ptr)46{47#ifdef CONFIG_HAVE_EFFICIENT_UNALIGNED_ACCESS48 return false;49#else50 return ((uintptr_t)ptr & 3) != 0;51#endif52}53 54static 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 buffer75 * @miter: sg mapping iterator used for reading76 *77 * Description:78 * Returns 32-bit word starting at byte pointed to by @miter@79 * handling any alignment issues. Bytes past the buffer's end80 * are not accessed (read) but are returned as zeroes. @miter@81 * is advanced by 4 bytes or to the end of buffer whichever is82 * closer.83 *84 * Context:85 * Same requirements as in sg_miter_next().86 *87 * Returns:88 * 32-bit word just read.89 */90uint32_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}99EXPORT_SYMBOL_GPL(cb710_sg_dwiter_read_next_block);100 101static 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 buffer119 * @miter: sg mapping iterator used for writing120 * @data: data to write to sg buffer121 *122 * Description:123 * Writes 32-bit word starting at byte pointed to by @miter@124 * handling any alignment issues. Bytes which would be written125 * past the buffer's end are silently discarded. @miter@ is126 * 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 */131void 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 else139 return;140 } else141 sg_dwiter_write_slow(miter, data);142}143EXPORT_SYMBOL_GPL(cb710_sg_dwiter_write_next_block);144 145