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 | |
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 | |
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 | |
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 | |
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 | |
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 | */ |
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 | |
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 | */ |
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 | |