1 files changed, 150 insertions, 0 deletions

diff --git a/drivers/misc/cb710/sgbuf2.c b/drivers/misc/cb710/sgbuf2.c
new file mode 100644
index 000000000000..d38a7acdb6ec
--- /dev/null
+++ b/drivers/misc/cb710/sgbuf2.c
@@ -0,0 +1,150 @@
+/*
+ *  cb710/sgbuf2.c
+ *
+ *  Copyright by Michał Mirosław, 2008-2009
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#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 HAVE_EFFICIENT_UNALIGNED_ACCESS
+        return false;
+#else
+        return ((ptr - NULL) & 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;
+                flush_kernel_dcache_page(miter->page);
+        } 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
+ *
+ * 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);
+
+        if (miter->length == miter->consumed)
+                flush_kernel_dcache_page(miter->page);
+}
+EXPORT_SYMBOL_GPL(cb710_sg_dwiter_write_next_block);
+