sgbuf2.c
3.41 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
/*
* cb710/sgbuf2.c
*
* Copyright by Michał Mirosław, 2008-2009
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*/
#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/cb710.h>
static bool sg_dwiter_next(struct sg_mapping_iter *miter)
{
if (sg_miter_next(miter)) {
miter->consumed = 0;
return true;
} else
return false;
}
static bool sg_dwiter_is_at_end(struct sg_mapping_iter *miter)
{
return miter->length == miter->consumed && !sg_dwiter_next(miter);
}
static uint32_t sg_dwiter_read_buffer(struct sg_mapping_iter *miter)
{
size_t len, left = 4;
uint32_t data;
void *addr = &data;
do {
len = min(miter->length - miter->consumed, left);
memcpy(addr, miter->addr + miter->consumed, len);
miter->consumed += len;
left -= len;
if (!left)
return data;
addr += len;
} while (sg_dwiter_next(miter));
memset(addr, 0, left);
return data;
}
static inline bool needs_unaligned_copy(const void *ptr)
{
#ifdef CONFIG_HAVE_EFFICIENT_UNALIGNED_ACCESS
return false;
#else
return ((ptr - NULL) & 3) != 0;
#endif
}
static bool sg_dwiter_get_next_block(struct sg_mapping_iter *miter, uint32_t **ptr)
{
size_t len;
if (sg_dwiter_is_at_end(miter))
return true;
len = miter->length - miter->consumed;
if (likely(len >= 4 && !needs_unaligned_copy(
miter->addr + miter->consumed))) {
*ptr = miter->addr + miter->consumed;
miter->consumed += 4;
return true;
}
return false;
}
/**
* cb710_sg_dwiter_read_next_block() - get next 32-bit word from sg buffer
* @miter: sg mapping iterator used for reading
*
* Description:
* Returns 32-bit word starting at byte pointed to by @miter@
* handling any alignment issues. Bytes past the buffer's end
* are not accessed (read) but are returned as zeroes. @miter@
* is advanced by 4 bytes or to the end of buffer whichever is
* closer.
*
* Context:
* Same requirements as in sg_miter_next().
*
* Returns:
* 32-bit word just read.
*/
uint32_t cb710_sg_dwiter_read_next_block(struct sg_mapping_iter *miter)
{
uint32_t *ptr = NULL;
if (likely(sg_dwiter_get_next_block(miter, &ptr)))
return ptr ? *ptr : 0;
return sg_dwiter_read_buffer(miter);
}
EXPORT_SYMBOL_GPL(cb710_sg_dwiter_read_next_block);
static void sg_dwiter_write_slow(struct sg_mapping_iter *miter, uint32_t data)
{
size_t len, left = 4;
void *addr = &data;
do {
len = min(miter->length - miter->consumed, left);
memcpy(miter->addr, addr, len);
miter->consumed += len;
left -= len;
if (!left)
return;
addr += len;
} while (sg_dwiter_next(miter));
}
/**
* cb710_sg_dwiter_write_next_block() - write next 32-bit word to sg buffer
* @miter: sg mapping iterator used for writing
*
* Description:
* Writes 32-bit word starting at byte pointed to by @miter@
* handling any alignment issues. Bytes which would be written
* past the buffer's end are silently discarded. @miter@ is
* advanced by 4 bytes or to the end of buffer whichever is closer.
*
* Context:
* Same requirements as in sg_miter_next().
*/
void cb710_sg_dwiter_write_next_block(struct sg_mapping_iter *miter, uint32_t data)
{
uint32_t *ptr = NULL;
if (likely(sg_dwiter_get_next_block(miter, &ptr))) {
if (ptr)
*ptr = data;
else
return;
} else
sg_dwiter_write_slow(miter, data);
}
EXPORT_SYMBOL_GPL(cb710_sg_dwiter_write_next_block);