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
11static 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
20static bool sg_dwiter_is_at_end(struct sg_mapping_iter *miter)
21{
22	return miter->length == miter->consumed && !sg_dwiter_next(miter);
23}
24
25static 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
45static 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
54static 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 */
90uint32_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}
99EXPORT_SYMBOL_GPL(cb710_sg_dwiter_read_next_block);
100
101static 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 */
131void 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}
143EXPORT_SYMBOL_GPL(cb710_sg_dwiter_write_next_block);
144
145