]>
Commit | Line | Data |
---|---|---|
5f5bac82 MM |
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 | ||
14 | static 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 | ||
23 | static bool sg_dwiter_is_at_end(struct sg_mapping_iter *miter) | |
24 | { | |
25 | return miter->length == miter->consumed && !sg_dwiter_next(miter); | |
26 | } | |
27 | ||
28 | static 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 | ||
48 | static inline bool needs_unaligned_copy(const void *ptr) | |
49 | { | |
e9e8bcb8 | 50 | #ifdef CONFIG_HAVE_EFFICIENT_UNALIGNED_ACCESS |
5f5bac82 MM |
51 | return false; |
52 | #else | |
53 | return ((ptr - NULL) & 3) != 0; | |
54 | #endif | |
55 | } | |
56 | ||
57 | static 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 | */ | |
93 | uint32_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 | } | |
102 | EXPORT_SYMBOL_GPL(cb710_sg_dwiter_read_next_block); | |
103 | ||
104 | static 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; | |
5f5bac82 MM |
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 | */ | |
133 | void 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); | |
5f5bac82 MM |
144 | } |
145 | EXPORT_SYMBOL_GPL(cb710_sg_dwiter_write_next_block); | |
146 |