From 2c3c1048746a4622d8c89a29670120dc8fab93c4 Mon Sep 17 00:00:00 2001 From: Daniel Baumann Date: Sun, 7 Apr 2024 20:49:45 +0200 Subject: Adding upstream version 6.1.76. Signed-off-by: Daniel Baumann --- drivers/misc/cb710/sgbuf2.c | 144 ++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 144 insertions(+) create mode 100644 drivers/misc/cb710/sgbuf2.c (limited to 'drivers/misc/cb710/sgbuf2.c') diff --git a/drivers/misc/cb710/sgbuf2.c b/drivers/misc/cb710/sgbuf2.c new file mode 100644 index 000000000..a798fad5f --- /dev/null +++ b/drivers/misc/cb710/sgbuf2.c @@ -0,0 +1,144 @@ +// SPDX-License-Identifier: GPL-2.0-only +/* + * cb710/sgbuf2.c + * + * Copyright by Michał Mirosław, 2008-2009 + */ +#include +#include +#include + +static bool sg_dwiter_next(struct sg_mapping_iter *miter) +{ + if (sg_miter_next(miter)) { + miter->consumed = 0; + return true; + } else + return false; +} + +static bool sg_dwiter_is_at_end(struct sg_mapping_iter *miter) +{ + return miter->length == miter->consumed && !sg_dwiter_next(miter); +} + +static uint32_t sg_dwiter_read_buffer(struct sg_mapping_iter *miter) +{ + size_t len, left = 4; + uint32_t data; + void *addr = &data; + + do { + len = min(miter->length - miter->consumed, left); + memcpy(addr, miter->addr + miter->consumed, len); + miter->consumed += len; + left -= len; + if (!left) + return data; + addr += len; + } while (sg_dwiter_next(miter)); + + memset(addr, 0, left); + return data; +} + +static inline bool needs_unaligned_copy(const void *ptr) +{ +#ifdef CONFIG_HAVE_EFFICIENT_UNALIGNED_ACCESS + return false; +#else + return ((uintptr_t)ptr & 3) != 0; +#endif +} + +static bool sg_dwiter_get_next_block(struct sg_mapping_iter *miter, uint32_t **ptr) +{ + size_t len; + + if (sg_dwiter_is_at_end(miter)) + return true; + + len = miter->length - miter->consumed; + + if (likely(len >= 4 && !needs_unaligned_copy( + miter->addr + miter->consumed))) { + *ptr = miter->addr + miter->consumed; + miter->consumed += 4; + return true; + } + + return false; +} + +/** + * cb710_sg_dwiter_read_next_block() - get next 32-bit word from sg buffer + * @miter: sg mapping iterator used for reading + * + * Description: + * Returns 32-bit word starting at byte pointed to by @miter@ + * handling any alignment issues. Bytes past the buffer's end + * are not accessed (read) but are returned as zeroes. @miter@ + * is advanced by 4 bytes or to the end of buffer whichever is + * closer. + * + * Context: + * Same requirements as in sg_miter_next(). + * + * Returns: + * 32-bit word just read. + */ +uint32_t cb710_sg_dwiter_read_next_block(struct sg_mapping_iter *miter) +{ + uint32_t *ptr = NULL; + + if (likely(sg_dwiter_get_next_block(miter, &ptr))) + return ptr ? *ptr : 0; + + return sg_dwiter_read_buffer(miter); +} +EXPORT_SYMBOL_GPL(cb710_sg_dwiter_read_next_block); + +static void sg_dwiter_write_slow(struct sg_mapping_iter *miter, uint32_t data) +{ + size_t len, left = 4; + void *addr = &data; + + do { + len = min(miter->length - miter->consumed, left); + memcpy(miter->addr, addr, len); + miter->consumed += len; + left -= len; + if (!left) + return; + addr += len; + } while (sg_dwiter_next(miter)); +} + +/** + * cb710_sg_dwiter_write_next_block() - write next 32-bit word to sg buffer + * @miter: sg mapping iterator used for writing + * @data: data to write to sg buffer + * + * Description: + * Writes 32-bit word starting at byte pointed to by @miter@ + * handling any alignment issues. Bytes which would be written + * past the buffer's end are silently discarded. @miter@ is + * advanced by 4 bytes or to the end of buffer whichever is closer. + * + * Context: + * Same requirements as in sg_miter_next(). + */ +void cb710_sg_dwiter_write_next_block(struct sg_mapping_iter *miter, uint32_t data) +{ + uint32_t *ptr = NULL; + + if (likely(sg_dwiter_get_next_block(miter, &ptr))) { + if (ptr) + *ptr = data; + else + return; + } else + sg_dwiter_write_slow(miter, data); +} +EXPORT_SYMBOL_GPL(cb710_sg_dwiter_write_next_block); + -- cgit v1.2.3