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