summaryrefslogtreecommitdiffstats
path: root/drivers/misc/cb710/sgbuf2.c
diff options
context:
space:
mode:
Diffstat (limited to 'drivers/misc/cb710/sgbuf2.c')
-rw-r--r--drivers/misc/cb710/sgbuf2.c144
1 files changed, 144 insertions, 0 deletions
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 <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/cb710.h>
+
+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);
+