1 // SPDX-License-Identifier: GPL-2.0-only
2 /*
3  *  cb710/sgbuf2.c
4  *
5  *  Copyright by Michał Mirosław, 2008-2009
6  */
7 #include <linux/kernel.h>
8 #include <linux/module.h>
9 #include <linux/cb710.h>
10 
sg_dwiter_next(struct sg_mapping_iter * miter)11 static bool sg_dwiter_next(struct sg_mapping_iter *miter)
12 {
13 	if (sg_miter_next(miter)) {
14 		miter->consumed = 0;
15 		return true;
16 	} else
17 		return false;
18 }
19 
sg_dwiter_is_at_end(struct sg_mapping_iter * miter)20 static bool sg_dwiter_is_at_end(struct sg_mapping_iter *miter)
21 {
22 	return miter->length == miter->consumed && !sg_dwiter_next(miter);
23 }
24 
sg_dwiter_read_buffer(struct sg_mapping_iter * miter)25 static 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 
needs_unaligned_copy(const void * ptr)45 static inline bool needs_unaligned_copy(const void *ptr)
46 {
47 #ifdef CONFIG_HAVE_EFFICIENT_UNALIGNED_ACCESS
48 	return false;
49 #else
50 	return ((uintptr_t)ptr & 3) != 0;
51 #endif
52 }
53 
sg_dwiter_get_next_block(struct sg_mapping_iter * miter,uint32_t ** ptr)54 static 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 buffer
75  * @miter: sg mapping iterator used for reading
76  *
77  * Description:
78  *   Returns 32-bit word starting at byte pointed to by @miter@
79  *   handling any alignment issues.  Bytes past the buffer's end
80  *   are not accessed (read) but are returned as zeroes.  @miter@
81  *   is advanced by 4 bytes or to the end of buffer whichever is
82  *   closer.
83  *
84  * Context:
85  *   Same requirements as in sg_miter_next().
86  *
87  * Returns:
88  *   32-bit word just read.
89  */
cb710_sg_dwiter_read_next_block(struct sg_mapping_iter * miter)90 uint32_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 }
99 EXPORT_SYMBOL_GPL(cb710_sg_dwiter_read_next_block);
100 
sg_dwiter_write_slow(struct sg_mapping_iter * miter,uint32_t data)101 static 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 buffer
119  * @miter: sg mapping iterator used for writing
120  * @data: data to write to sg buffer
121  *
122  * Description:
123  *   Writes 32-bit word starting at byte pointed to by @miter@
124  *   handling any alignment issues.  Bytes which would be written
125  *   past the buffer's end are silently discarded. @miter@ is
126  *   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  */
cb710_sg_dwiter_write_next_block(struct sg_mapping_iter * miter,uint32_t data)131 void 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 		else
139 			return;
140 	} else
141 		sg_dwiter_write_slow(miter, data);
142 }
143 EXPORT_SYMBOL_GPL(cb710_sg_dwiter_write_next_block);
144 
145