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#include <linux/string.h>
35
36#include <rdma/ib_pack.h>
37
38static u64 value_read(int offset, int size, void *structure)
39{
40 switch (size) {
41 case 1: return *(u8 *) (structure + offset);
42 case 2: return be16_to_cpup((__be16 *) (structure + offset));
43 case 4: return be32_to_cpup((__be32 *) (structure + offset));
44 case 8: return be64_to_cpup((__be64 *) (structure + offset));
45 default:
46 printk(KERN_WARNING "Field size %d bits not handled\n", size * 8);
47 return 0;
48 }
49}
50
51
52
53
54
55
56
57
58
59
60
61void ib_pack(const struct ib_field *desc,
62 int desc_len,
63 void *structure,
64 void *buf)
65{
66 int i;
67
68 for (i = 0; i < desc_len; ++i) {
69 if (desc[i].size_bits <= 32) {
70 int shift;
71 u32 val;
72 __be32 mask;
73 __be32 *addr;
74
75 shift = 32 - desc[i].offset_bits - desc[i].size_bits;
76 if (desc[i].struct_size_bytes)
77 val = value_read(desc[i].struct_offset_bytes,
78 desc[i].struct_size_bytes,
79 structure) << shift;
80 else
81 val = 0;
82
83 mask = cpu_to_be32(((1ull << desc[i].size_bits) - 1) << shift);
84 addr = (__be32 *) buf + desc[i].offset_words;
85 *addr = (*addr & ~mask) | (cpu_to_be32(val) & mask);
86 } else if (desc[i].size_bits <= 64) {
87 int shift;
88 u64 val;
89 __be64 mask;
90 __be64 *addr;
91
92 shift = 64 - desc[i].offset_bits - desc[i].size_bits;
93 if (desc[i].struct_size_bytes)
94 val = value_read(desc[i].struct_offset_bytes,
95 desc[i].struct_size_bytes,
96 structure) << shift;
97 else
98 val = 0;
99
100 mask = cpu_to_be64((~0ull >> (64 - desc[i].size_bits)) << shift);
101 addr = (__be64 *) ((__be32 *) buf + desc[i].offset_words);
102 *addr = (*addr & ~mask) | (cpu_to_be64(val) & mask);
103 } else {
104 if (desc[i].offset_bits % 8 ||
105 desc[i].size_bits % 8) {
106 printk(KERN_WARNING "Structure field %s of size %d "
107 "bits is not byte-aligned\n",
108 desc[i].field_name, desc[i].size_bits);
109 }
110
111 if (desc[i].struct_size_bytes)
112 memcpy(buf + desc[i].offset_words * 4 +
113 desc[i].offset_bits / 8,
114 structure + desc[i].struct_offset_bytes,
115 desc[i].size_bits / 8);
116 else
117 memset(buf + desc[i].offset_words * 4 +
118 desc[i].offset_bits / 8,
119 0,
120 desc[i].size_bits / 8);
121 }
122 }
123}
124EXPORT_SYMBOL(ib_pack);
125
126static void value_write(int offset, int size, u64 val, void *structure)
127{
128 switch (size * 8) {
129 case 8: *( u8 *) (structure + offset) = val; break;
130 case 16: *(__be16 *) (structure + offset) = cpu_to_be16(val); break;
131 case 32: *(__be32 *) (structure + offset) = cpu_to_be32(val); break;
132 case 64: *(__be64 *) (structure + offset) = cpu_to_be64(val); break;
133 default:
134 printk(KERN_WARNING "Field size %d bits not handled\n", size * 8);
135 }
136}
137
138
139
140
141
142
143
144
145
146
147
148void ib_unpack(const struct ib_field *desc,
149 int desc_len,
150 void *buf,
151 void *structure)
152{
153 int i;
154
155 for (i = 0; i < desc_len; ++i) {
156 if (!desc[i].struct_size_bytes)
157 continue;
158
159 if (desc[i].size_bits <= 32) {
160 int shift;
161 u32 val;
162 u32 mask;
163 __be32 *addr;
164
165 shift = 32 - desc[i].offset_bits - desc[i].size_bits;
166 mask = ((1ull << desc[i].size_bits) - 1) << shift;
167 addr = (__be32 *) buf + desc[i].offset_words;
168 val = (be32_to_cpup(addr) & mask) >> shift;
169 value_write(desc[i].struct_offset_bytes,
170 desc[i].struct_size_bytes,
171 val,
172 structure);
173 } else if (desc[i].size_bits <= 64) {
174 int shift;
175 u64 val;
176 u64 mask;
177 __be64 *addr;
178
179 shift = 64 - desc[i].offset_bits - desc[i].size_bits;
180 mask = (~0ull >> (64 - desc[i].size_bits)) << shift;
181 addr = (__be64 *) buf + desc[i].offset_words;
182 val = (be64_to_cpup(addr) & mask) >> shift;
183 value_write(desc[i].struct_offset_bytes,
184 desc[i].struct_size_bytes,
185 val,
186 structure);
187 } else {
188 if (desc[i].offset_bits % 8 ||
189 desc[i].size_bits % 8) {
190 printk(KERN_WARNING "Structure field %s of size %d "
191 "bits is not byte-aligned\n",
192 desc[i].field_name, desc[i].size_bits);
193 }
194
195 memcpy(structure + desc[i].struct_offset_bytes,
196 buf + desc[i].offset_words * 4 +
197 desc[i].offset_bits / 8,
198 desc[i].size_bits / 8);
199 }
200 }
201}
202EXPORT_SYMBOL(ib_unpack);
203