1 /*- 2 * SPDX-License-Identifier: BSD-2-Clause OR GPL-2.0 3 * 4 * Copyright (c) 2004 Topspin Corporation. All rights reserved. 5 * Copyright (c) 2005 Sun Microsystems, Inc. All rights reserved. 6 * 7 * This software is available to you under a choice of one of two 8 * licenses. You may choose to be licensed under the terms of the GNU 9 * General Public License (GPL) Version 2, available from the file 10 * COPYING in the main directory of this source tree, or the 11 * OpenIB.org BSD license below: 12 * 13 * Redistribution and use in source and binary forms, with or 14 * without modification, are permitted provided that the following 15 * conditions are met: 16 * 17 * - Redistributions of source code must retain the above 18 * copyright notice, this list of conditions and the following 19 * disclaimer. 20 * 21 * - Redistributions in binary form must reproduce the above 22 * copyright notice, this list of conditions and the following 23 * disclaimer in the documentation and/or other materials 24 * provided with the distribution. 25 * 26 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 27 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 28 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 29 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS 30 * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN 31 * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 32 * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 33 * SOFTWARE. 34 */ 35 36 #include <sys/cdefs.h> 37 __FBSDID("$FreeBSD$"); 38 39 #include <linux/string.h> 40 41 #include <rdma/ib_pack.h> 42 43 static u64 value_read(int offset, int size, void *structure) 44 { 45 switch (size) { 46 case 1: return *(u8 *) ((char *)structure + offset); 47 case 2: return be16_to_cpup((__be16 *) ((char *)structure + offset)); 48 case 4: return be32_to_cpup((__be32 *) ((char *)structure + offset)); 49 case 8: return be64_to_cpup((__be64 *) ((char *)structure + offset)); 50 default: 51 pr_warn("Field size %d bits not handled\n", size * 8); 52 return 0; 53 } 54 } 55 56 /** 57 * ib_pack - Pack a structure into a buffer 58 * @desc:Array of structure field descriptions 59 * @desc_len:Number of entries in @desc 60 * @structure:Structure to pack from 61 * @buf:Buffer to pack into 62 * 63 * ib_pack() packs a list of structure fields into a buffer, 64 * controlled by the array of fields in @desc. 65 */ 66 void ib_pack(const struct ib_field *desc, 67 int desc_len, 68 void *structure, 69 void *buf) 70 { 71 int i; 72 73 for (i = 0; i < desc_len; ++i) { 74 if (desc[i].size_bits <= 32) { 75 int shift; 76 u32 val; 77 __be32 mask; 78 __be32 *addr; 79 80 shift = 32 - desc[i].offset_bits - desc[i].size_bits; 81 if (desc[i].struct_size_bytes) 82 val = value_read(desc[i].struct_offset_bytes, 83 desc[i].struct_size_bytes, 84 structure) << shift; 85 else 86 val = 0; 87 88 mask = cpu_to_be32(((1ull << desc[i].size_bits) - 1) << shift); 89 addr = (__be32 *) buf + desc[i].offset_words; 90 *addr = (*addr & ~mask) | (cpu_to_be32(val) & mask); 91 } else if (desc[i].size_bits <= 64) { 92 int shift; 93 u64 val; 94 __be64 mask; 95 __be64 *addr; 96 97 shift = 64 - desc[i].offset_bits - desc[i].size_bits; 98 if (desc[i].struct_size_bytes) 99 val = value_read(desc[i].struct_offset_bytes, 100 desc[i].struct_size_bytes, 101 structure) << shift; 102 else 103 val = 0; 104 105 mask = cpu_to_be64((~0ull >> (64 - desc[i].size_bits)) << shift); 106 addr = (__be64 *) ((__be32 *) buf + desc[i].offset_words); 107 *addr = (*addr & ~mask) | (cpu_to_be64(val) & mask); 108 } else { 109 if (desc[i].offset_bits % 8 || 110 desc[i].size_bits % 8) { 111 pr_warn("Structure field %s of size %d bits is not byte-aligned\n", 112 desc[i].field_name, desc[i].size_bits); 113 } 114 115 if (desc[i].struct_size_bytes) 116 memcpy((char *)buf + desc[i].offset_words * 4 + 117 desc[i].offset_bits / 8, 118 (char *)structure + desc[i].struct_offset_bytes, 119 desc[i].size_bits / 8); 120 else 121 memset((char *)buf + desc[i].offset_words * 4 + 122 desc[i].offset_bits / 8, 123 0, 124 desc[i].size_bits / 8); 125 } 126 } 127 } 128 EXPORT_SYMBOL(ib_pack); 129 130 static void value_write(int offset, int size, u64 val, void *structure) 131 { 132 switch (size * 8) { 133 case 8: *( u8 *) ((char *)structure + offset) = val; break; 134 case 16: *(__be16 *) ((char *)structure + offset) = cpu_to_be16(val); break; 135 case 32: *(__be32 *) ((char *)structure + offset) = cpu_to_be32(val); break; 136 case 64: *(__be64 *) ((char *)structure + offset) = cpu_to_be64(val); break; 137 default: 138 pr_warn("Field size %d bits not handled\n", size * 8); 139 } 140 } 141 142 /** 143 * ib_unpack - Unpack a buffer into a structure 144 * @desc:Array of structure field descriptions 145 * @desc_len:Number of entries in @desc 146 * @buf:Buffer to unpack from 147 * @structure:Structure to unpack into 148 * 149 * ib_pack() unpacks a list of structure fields from a buffer, 150 * controlled by the array of fields in @desc. 151 */ 152 void ib_unpack(const struct ib_field *desc, 153 int desc_len, 154 void *buf, 155 void *structure) 156 { 157 int i; 158 159 for (i = 0; i < desc_len; ++i) { 160 if (!desc[i].struct_size_bytes) 161 continue; 162 163 if (desc[i].size_bits <= 32) { 164 int shift; 165 u32 val; 166 u32 mask; 167 __be32 *addr; 168 169 shift = 32 - desc[i].offset_bits - desc[i].size_bits; 170 mask = ((1ull << desc[i].size_bits) - 1) << shift; 171 addr = (__be32 *) buf + desc[i].offset_words; 172 val = (be32_to_cpup(addr) & mask) >> shift; 173 value_write(desc[i].struct_offset_bytes, 174 desc[i].struct_size_bytes, 175 val, 176 structure); 177 } else if (desc[i].size_bits <= 64) { 178 int shift; 179 u64 val; 180 u64 mask; 181 __be64 *addr; 182 183 shift = 64 - desc[i].offset_bits - desc[i].size_bits; 184 mask = (~0ull >> (64 - desc[i].size_bits)) << shift; 185 addr = (__be64 *) buf + desc[i].offset_words; 186 val = (be64_to_cpup(addr) & mask) >> shift; 187 value_write(desc[i].struct_offset_bytes, 188 desc[i].struct_size_bytes, 189 val, 190 structure); 191 } else { 192 if (desc[i].offset_bits % 8 || 193 desc[i].size_bits % 8) { 194 pr_warn("Structure field %s of size %d bits is not byte-aligned\n", 195 desc[i].field_name, desc[i].size_bits); 196 } 197 198 memcpy((char *)structure + desc[i].struct_offset_bytes, 199 (char *)buf + desc[i].offset_words * 4 + 200 desc[i].offset_bits / 8, 201 desc[i].size_bits / 8); 202 } 203 } 204 } 205 EXPORT_SYMBOL(ib_unpack); 206