11da177e4SLinus Torvalds /* 21da177e4SLinus Torvalds * Copyright (c) 2004 Topspin Corporation. All rights reserved. 32a1d9b7fSRoland Dreier * Copyright (c) 2005 Sun Microsystems, Inc. All rights reserved. 41da177e4SLinus Torvalds * 51da177e4SLinus Torvalds * This software is available to you under a choice of one of two 61da177e4SLinus Torvalds * licenses. You may choose to be licensed under the terms of the GNU 71da177e4SLinus Torvalds * General Public License (GPL) Version 2, available from the file 81da177e4SLinus Torvalds * COPYING in the main directory of this source tree, or the 91da177e4SLinus Torvalds * OpenIB.org BSD license below: 101da177e4SLinus Torvalds * 111da177e4SLinus Torvalds * Redistribution and use in source and binary forms, with or 121da177e4SLinus Torvalds * without modification, are permitted provided that the following 131da177e4SLinus Torvalds * conditions are met: 141da177e4SLinus Torvalds * 151da177e4SLinus Torvalds * - Redistributions of source code must retain the above 161da177e4SLinus Torvalds * copyright notice, this list of conditions and the following 171da177e4SLinus Torvalds * disclaimer. 181da177e4SLinus Torvalds * 191da177e4SLinus Torvalds * - Redistributions in binary form must reproduce the above 201da177e4SLinus Torvalds * copyright notice, this list of conditions and the following 211da177e4SLinus Torvalds * disclaimer in the documentation and/or other materials 221da177e4SLinus Torvalds * provided with the distribution. 231da177e4SLinus Torvalds * 241da177e4SLinus Torvalds * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 251da177e4SLinus Torvalds * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 261da177e4SLinus Torvalds * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 271da177e4SLinus Torvalds * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS 281da177e4SLinus Torvalds * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN 291da177e4SLinus Torvalds * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 301da177e4SLinus Torvalds * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 311da177e4SLinus Torvalds * SOFTWARE. 321da177e4SLinus Torvalds * 331da177e4SLinus Torvalds * $Id: packer.c 1349 2004-12-16 21:09:43Z roland $ 341da177e4SLinus Torvalds */ 351da177e4SLinus Torvalds 368c65b4a6STim Schmielau #include <linux/string.h> 378c65b4a6STim Schmielau 38a4d61e84SRoland Dreier #include <rdma/ib_pack.h> 391da177e4SLinus Torvalds 401da177e4SLinus Torvalds static u64 value_read(int offset, int size, void *structure) 411da177e4SLinus Torvalds { 421da177e4SLinus Torvalds switch (size) { 431da177e4SLinus Torvalds case 1: return *(u8 *) (structure + offset); 441da177e4SLinus Torvalds case 2: return be16_to_cpup((__be16 *) (structure + offset)); 451da177e4SLinus Torvalds case 4: return be32_to_cpup((__be32 *) (structure + offset)); 461da177e4SLinus Torvalds case 8: return be64_to_cpup((__be64 *) (structure + offset)); 471da177e4SLinus Torvalds default: 481da177e4SLinus Torvalds printk(KERN_WARNING "Field size %d bits not handled\n", size * 8); 491da177e4SLinus Torvalds return 0; 501da177e4SLinus Torvalds } 511da177e4SLinus Torvalds } 521da177e4SLinus Torvalds 531da177e4SLinus Torvalds /** 541da177e4SLinus Torvalds * ib_pack - Pack a structure into a buffer 551da177e4SLinus Torvalds * @desc:Array of structure field descriptions 561da177e4SLinus Torvalds * @desc_len:Number of entries in @desc 571da177e4SLinus Torvalds * @structure:Structure to pack from 581da177e4SLinus Torvalds * @buf:Buffer to pack into 591da177e4SLinus Torvalds * 601da177e4SLinus Torvalds * ib_pack() packs a list of structure fields into a buffer, 611da177e4SLinus Torvalds * controlled by the array of fields in @desc. 621da177e4SLinus Torvalds */ 631da177e4SLinus Torvalds void ib_pack(const struct ib_field *desc, 641da177e4SLinus Torvalds int desc_len, 651da177e4SLinus Torvalds void *structure, 661da177e4SLinus Torvalds void *buf) 671da177e4SLinus Torvalds { 681da177e4SLinus Torvalds int i; 691da177e4SLinus Torvalds 701da177e4SLinus Torvalds for (i = 0; i < desc_len; ++i) { 711da177e4SLinus Torvalds if (desc[i].size_bits <= 32) { 721da177e4SLinus Torvalds int shift; 731da177e4SLinus Torvalds u32 val; 741da177e4SLinus Torvalds __be32 mask; 751da177e4SLinus Torvalds __be32 *addr; 761da177e4SLinus Torvalds 771da177e4SLinus Torvalds shift = 32 - desc[i].offset_bits - desc[i].size_bits; 781da177e4SLinus Torvalds if (desc[i].struct_size_bytes) 791da177e4SLinus Torvalds val = value_read(desc[i].struct_offset_bytes, 801da177e4SLinus Torvalds desc[i].struct_size_bytes, 811da177e4SLinus Torvalds structure) << shift; 821da177e4SLinus Torvalds else 831da177e4SLinus Torvalds val = 0; 841da177e4SLinus Torvalds 851da177e4SLinus Torvalds mask = cpu_to_be32(((1ull << desc[i].size_bits) - 1) << shift); 861da177e4SLinus Torvalds addr = (__be32 *) buf + desc[i].offset_words; 871da177e4SLinus Torvalds *addr = (*addr & ~mask) | (cpu_to_be32(val) & mask); 881da177e4SLinus Torvalds } else if (desc[i].size_bits <= 64) { 891da177e4SLinus Torvalds int shift; 901da177e4SLinus Torvalds u64 val; 911da177e4SLinus Torvalds __be64 mask; 921da177e4SLinus Torvalds __be64 *addr; 931da177e4SLinus Torvalds 941da177e4SLinus Torvalds shift = 64 - desc[i].offset_bits - desc[i].size_bits; 951da177e4SLinus Torvalds if (desc[i].struct_size_bytes) 961da177e4SLinus Torvalds val = value_read(desc[i].struct_offset_bytes, 971da177e4SLinus Torvalds desc[i].struct_size_bytes, 981da177e4SLinus Torvalds structure) << shift; 991da177e4SLinus Torvalds else 1001da177e4SLinus Torvalds val = 0; 1011da177e4SLinus Torvalds 10248442962SRoland Dreier mask = cpu_to_be64((~0ull >> (64 - desc[i].size_bits)) << shift); 1031da177e4SLinus Torvalds addr = (__be64 *) ((__be32 *) buf + desc[i].offset_words); 1041da177e4SLinus Torvalds *addr = (*addr & ~mask) | (cpu_to_be64(val) & mask); 1051da177e4SLinus Torvalds } else { 1061da177e4SLinus Torvalds if (desc[i].offset_bits % 8 || 1071da177e4SLinus Torvalds desc[i].size_bits % 8) { 1081da177e4SLinus Torvalds printk(KERN_WARNING "Structure field %s of size %d " 1091da177e4SLinus Torvalds "bits is not byte-aligned\n", 1101da177e4SLinus Torvalds desc[i].field_name, desc[i].size_bits); 1111da177e4SLinus Torvalds } 1121da177e4SLinus Torvalds 1131da177e4SLinus Torvalds if (desc[i].struct_size_bytes) 1141da177e4SLinus Torvalds memcpy(buf + desc[i].offset_words * 4 + 1151da177e4SLinus Torvalds desc[i].offset_bits / 8, 1161da177e4SLinus Torvalds structure + desc[i].struct_offset_bytes, 1171da177e4SLinus Torvalds desc[i].size_bits / 8); 1181da177e4SLinus Torvalds else 1191da177e4SLinus Torvalds memset(buf + desc[i].offset_words * 4 + 1201da177e4SLinus Torvalds desc[i].offset_bits / 8, 1211da177e4SLinus Torvalds 0, 1221da177e4SLinus Torvalds desc[i].size_bits / 8); 1231da177e4SLinus Torvalds } 1241da177e4SLinus Torvalds } 1251da177e4SLinus Torvalds } 1261da177e4SLinus Torvalds EXPORT_SYMBOL(ib_pack); 1271da177e4SLinus Torvalds 1281da177e4SLinus Torvalds static void value_write(int offset, int size, u64 val, void *structure) 1291da177e4SLinus Torvalds { 1301da177e4SLinus Torvalds switch (size * 8) { 1311da177e4SLinus Torvalds case 8: *( u8 *) (structure + offset) = val; break; 1321da177e4SLinus Torvalds case 16: *(__be16 *) (structure + offset) = cpu_to_be16(val); break; 1331da177e4SLinus Torvalds case 32: *(__be32 *) (structure + offset) = cpu_to_be32(val); break; 1341da177e4SLinus Torvalds case 64: *(__be64 *) (structure + offset) = cpu_to_be64(val); break; 1351da177e4SLinus Torvalds default: 1361da177e4SLinus Torvalds printk(KERN_WARNING "Field size %d bits not handled\n", size * 8); 1371da177e4SLinus Torvalds } 1381da177e4SLinus Torvalds } 1391da177e4SLinus Torvalds 1401da177e4SLinus Torvalds /** 1411da177e4SLinus Torvalds * ib_unpack - Unpack a buffer into a structure 1421da177e4SLinus Torvalds * @desc:Array of structure field descriptions 1431da177e4SLinus Torvalds * @desc_len:Number of entries in @desc 1441da177e4SLinus Torvalds * @buf:Buffer to unpack from 1451da177e4SLinus Torvalds * @structure:Structure to unpack into 1461da177e4SLinus Torvalds * 1471da177e4SLinus Torvalds * ib_pack() unpacks a list of structure fields from a buffer, 1481da177e4SLinus Torvalds * controlled by the array of fields in @desc. 1491da177e4SLinus Torvalds */ 1501da177e4SLinus Torvalds void ib_unpack(const struct ib_field *desc, 1511da177e4SLinus Torvalds int desc_len, 1521da177e4SLinus Torvalds void *buf, 1531da177e4SLinus Torvalds void *structure) 1541da177e4SLinus Torvalds { 1551da177e4SLinus Torvalds int i; 1561da177e4SLinus Torvalds 1571da177e4SLinus Torvalds for (i = 0; i < desc_len; ++i) { 1581da177e4SLinus Torvalds if (!desc[i].struct_size_bytes) 1591da177e4SLinus Torvalds continue; 1601da177e4SLinus Torvalds 1611da177e4SLinus Torvalds if (desc[i].size_bits <= 32) { 1621da177e4SLinus Torvalds int shift; 1631da177e4SLinus Torvalds u32 val; 1641da177e4SLinus Torvalds u32 mask; 1651da177e4SLinus Torvalds __be32 *addr; 1661da177e4SLinus Torvalds 1671da177e4SLinus Torvalds shift = 32 - desc[i].offset_bits - desc[i].size_bits; 1681da177e4SLinus Torvalds mask = ((1ull << desc[i].size_bits) - 1) << shift; 1691da177e4SLinus Torvalds addr = (__be32 *) buf + desc[i].offset_words; 1701da177e4SLinus Torvalds val = (be32_to_cpup(addr) & mask) >> shift; 1711da177e4SLinus Torvalds value_write(desc[i].struct_offset_bytes, 1721da177e4SLinus Torvalds desc[i].struct_size_bytes, 1731da177e4SLinus Torvalds val, 1741da177e4SLinus Torvalds structure); 1751da177e4SLinus Torvalds } else if (desc[i].size_bits <= 64) { 1761da177e4SLinus Torvalds int shift; 1771da177e4SLinus Torvalds u64 val; 1781da177e4SLinus Torvalds u64 mask; 1791da177e4SLinus Torvalds __be64 *addr; 1801da177e4SLinus Torvalds 1811da177e4SLinus Torvalds shift = 64 - desc[i].offset_bits - desc[i].size_bits; 18248442962SRoland Dreier mask = (~0ull >> (64 - desc[i].size_bits)) << shift; 1831da177e4SLinus Torvalds addr = (__be64 *) buf + desc[i].offset_words; 1841da177e4SLinus Torvalds val = (be64_to_cpup(addr) & mask) >> shift; 1851da177e4SLinus Torvalds value_write(desc[i].struct_offset_bytes, 1861da177e4SLinus Torvalds desc[i].struct_size_bytes, 1871da177e4SLinus Torvalds val, 1881da177e4SLinus Torvalds structure); 1891da177e4SLinus Torvalds } else { 1901da177e4SLinus Torvalds if (desc[i].offset_bits % 8 || 1911da177e4SLinus Torvalds desc[i].size_bits % 8) { 1921da177e4SLinus Torvalds printk(KERN_WARNING "Structure field %s of size %d " 1931da177e4SLinus Torvalds "bits is not byte-aligned\n", 1941da177e4SLinus Torvalds desc[i].field_name, desc[i].size_bits); 1951da177e4SLinus Torvalds } 1961da177e4SLinus Torvalds 1971da177e4SLinus Torvalds memcpy(structure + desc[i].struct_offset_bytes, 1981da177e4SLinus Torvalds buf + desc[i].offset_words * 4 + 1991da177e4SLinus Torvalds desc[i].offset_bits / 8, 2001da177e4SLinus Torvalds desc[i].size_bits / 8); 2011da177e4SLinus Torvalds } 2021da177e4SLinus Torvalds } 2031da177e4SLinus Torvalds } 2041da177e4SLinus Torvalds EXPORT_SYMBOL(ib_unpack); 205