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
14static 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
23static bool sg_dwiter_is_at_end(struct sg_mapping_iter *miter)
24{
25	return miter->length == miter->consumed && !sg_dwiter_next(miter);
26}
27
28static 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
48static 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
57static 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 */
93uint32_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}
102EXPORT_SYMBOL_GPL(cb710_sg_dwiter_read_next_block);
103
104static 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 */
133void 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}
145EXPORT_SYMBOL_GPL(cb710_sg_dwiter_write_next_block);
146
147