1219820Sjeff/* 2219820Sjeff * Copyright (c) 2004 Topspin Corporation. All rights reserved. 3219820Sjeff * Copyright (c) 2005 Sun Microsystems, Inc. All rights reserved. 4219820Sjeff * 5219820Sjeff * This software is available to you under a choice of one of two 6219820Sjeff * licenses. You may choose to be licensed under the terms of the GNU 7219820Sjeff * General Public License (GPL) Version 2, available from the file 8219820Sjeff * COPYING in the main directory of this source tree, or the 9219820Sjeff * OpenIB.org BSD license below: 10219820Sjeff * 11219820Sjeff * Redistribution and use in source and binary forms, with or 12219820Sjeff * without modification, are permitted provided that the following 13219820Sjeff * conditions are met: 14219820Sjeff * 15219820Sjeff * - Redistributions of source code must retain the above 16219820Sjeff * copyright notice, this list of conditions and the following 17219820Sjeff * disclaimer. 18219820Sjeff * 19219820Sjeff * - Redistributions in binary form must reproduce the above 20219820Sjeff * copyright notice, this list of conditions and the following 21219820Sjeff * disclaimer in the documentation and/or other materials 22219820Sjeff * provided with the distribution. 23219820Sjeff * 24219820Sjeff * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 25219820Sjeff * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 26219820Sjeff * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 27219820Sjeff * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS 28219820Sjeff * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN 29219820Sjeff * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 30219820Sjeff * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 31219820Sjeff * SOFTWARE. 32219820Sjeff */ 33219820Sjeff 34219820Sjeff#include <linux/string.h> 35219820Sjeff 36219820Sjeff#include <rdma/ib_pack.h> 37219820Sjeff 38219820Sjeffstatic u64 value_read(int offset, int size, void *structure) 39219820Sjeff{ 40219820Sjeff switch (size) { 41219820Sjeff case 1: return *(u8 *) (structure + offset); 42219820Sjeff case 2: return be16_to_cpup((__be16 *) (structure + offset)); 43219820Sjeff case 4: return be32_to_cpup((__be32 *) (structure + offset)); 44219820Sjeff case 8: return be64_to_cpup((__be64 *) (structure + offset)); 45219820Sjeff default: 46219820Sjeff printk(KERN_WARNING "Field size %d bits not handled\n", size * 8); 47219820Sjeff return 0; 48219820Sjeff } 49219820Sjeff} 50219820Sjeff 51219820Sjeff/** 52219820Sjeff * ib_pack - Pack a structure into a buffer 53219820Sjeff * @desc:Array of structure field descriptions 54219820Sjeff * @desc_len:Number of entries in @desc 55219820Sjeff * @structure:Structure to pack from 56219820Sjeff * @buf:Buffer to pack into 57219820Sjeff * 58219820Sjeff * ib_pack() packs a list of structure fields into a buffer, 59219820Sjeff * controlled by the array of fields in @desc. 60219820Sjeff */ 61219820Sjeffvoid ib_pack(const struct ib_field *desc, 62219820Sjeff int desc_len, 63219820Sjeff void *structure, 64219820Sjeff void *buf) 65219820Sjeff{ 66219820Sjeff int i; 67219820Sjeff 68219820Sjeff for (i = 0; i < desc_len; ++i) { 69219820Sjeff if (desc[i].size_bits <= 32) { 70219820Sjeff int shift; 71219820Sjeff u32 val; 72219820Sjeff __be32 mask; 73219820Sjeff __be32 *addr; 74219820Sjeff 75219820Sjeff shift = 32 - desc[i].offset_bits - desc[i].size_bits; 76219820Sjeff if (desc[i].struct_size_bytes) 77219820Sjeff val = value_read(desc[i].struct_offset_bytes, 78219820Sjeff desc[i].struct_size_bytes, 79219820Sjeff structure) << shift; 80219820Sjeff else 81219820Sjeff val = 0; 82219820Sjeff 83219820Sjeff mask = cpu_to_be32(((1ull << desc[i].size_bits) - 1) << shift); 84219820Sjeff addr = (__be32 *) buf + desc[i].offset_words; 85219820Sjeff *addr = (*addr & ~mask) | (cpu_to_be32(val) & mask); 86219820Sjeff } else if (desc[i].size_bits <= 64) { 87219820Sjeff int shift; 88219820Sjeff u64 val; 89219820Sjeff __be64 mask; 90219820Sjeff __be64 *addr; 91219820Sjeff 92219820Sjeff shift = 64 - desc[i].offset_bits - desc[i].size_bits; 93219820Sjeff if (desc[i].struct_size_bytes) 94219820Sjeff val = value_read(desc[i].struct_offset_bytes, 95219820Sjeff desc[i].struct_size_bytes, 96219820Sjeff structure) << shift; 97219820Sjeff else 98219820Sjeff val = 0; 99219820Sjeff 100219820Sjeff mask = cpu_to_be64((~0ull >> (64 - desc[i].size_bits)) << shift); 101219820Sjeff addr = (__be64 *) ((__be32 *) buf + desc[i].offset_words); 102219820Sjeff *addr = (*addr & ~mask) | (cpu_to_be64(val) & mask); 103219820Sjeff } else { 104219820Sjeff if (desc[i].offset_bits % 8 || 105219820Sjeff desc[i].size_bits % 8) { 106219820Sjeff printk(KERN_WARNING "Structure field %s of size %d " 107219820Sjeff "bits is not byte-aligned\n", 108219820Sjeff desc[i].field_name, desc[i].size_bits); 109219820Sjeff } 110219820Sjeff 111219820Sjeff if (desc[i].struct_size_bytes) 112219820Sjeff memcpy(buf + desc[i].offset_words * 4 + 113219820Sjeff desc[i].offset_bits / 8, 114219820Sjeff structure + desc[i].struct_offset_bytes, 115219820Sjeff desc[i].size_bits / 8); 116219820Sjeff else 117219820Sjeff memset(buf + desc[i].offset_words * 4 + 118219820Sjeff desc[i].offset_bits / 8, 119219820Sjeff 0, 120219820Sjeff desc[i].size_bits / 8); 121219820Sjeff } 122219820Sjeff } 123219820Sjeff} 124219820SjeffEXPORT_SYMBOL(ib_pack); 125219820Sjeff 126219820Sjeffstatic void value_write(int offset, int size, u64 val, void *structure) 127219820Sjeff{ 128219820Sjeff switch (size * 8) { 129219820Sjeff case 8: *( u8 *) (structure + offset) = val; break; 130219820Sjeff case 16: *(__be16 *) (structure + offset) = cpu_to_be16(val); break; 131219820Sjeff case 32: *(__be32 *) (structure + offset) = cpu_to_be32(val); break; 132219820Sjeff case 64: *(__be64 *) (structure + offset) = cpu_to_be64(val); break; 133219820Sjeff default: 134219820Sjeff printk(KERN_WARNING "Field size %d bits not handled\n", size * 8); 135219820Sjeff } 136219820Sjeff} 137219820Sjeff 138219820Sjeff/** 139219820Sjeff * ib_unpack - Unpack a buffer into a structure 140219820Sjeff * @desc:Array of structure field descriptions 141219820Sjeff * @desc_len:Number of entries in @desc 142219820Sjeff * @buf:Buffer to unpack from 143219820Sjeff * @structure:Structure to unpack into 144219820Sjeff * 145219820Sjeff * ib_pack() unpacks a list of structure fields from a buffer, 146219820Sjeff * controlled by the array of fields in @desc. 147219820Sjeff */ 148219820Sjeffvoid ib_unpack(const struct ib_field *desc, 149219820Sjeff int desc_len, 150219820Sjeff void *buf, 151219820Sjeff void *structure) 152219820Sjeff{ 153219820Sjeff int i; 154219820Sjeff 155219820Sjeff for (i = 0; i < desc_len; ++i) { 156219820Sjeff if (!desc[i].struct_size_bytes) 157219820Sjeff continue; 158219820Sjeff 159219820Sjeff if (desc[i].size_bits <= 32) { 160219820Sjeff int shift; 161219820Sjeff u32 val; 162219820Sjeff u32 mask; 163219820Sjeff __be32 *addr; 164219820Sjeff 165219820Sjeff shift = 32 - desc[i].offset_bits - desc[i].size_bits; 166219820Sjeff mask = ((1ull << desc[i].size_bits) - 1) << shift; 167219820Sjeff addr = (__be32 *) buf + desc[i].offset_words; 168219820Sjeff val = (be32_to_cpup(addr) & mask) >> shift; 169219820Sjeff value_write(desc[i].struct_offset_bytes, 170219820Sjeff desc[i].struct_size_bytes, 171219820Sjeff val, 172219820Sjeff structure); 173219820Sjeff } else if (desc[i].size_bits <= 64) { 174219820Sjeff int shift; 175219820Sjeff u64 val; 176219820Sjeff u64 mask; 177219820Sjeff __be64 *addr; 178219820Sjeff 179219820Sjeff shift = 64 - desc[i].offset_bits - desc[i].size_bits; 180219820Sjeff mask = (~0ull >> (64 - desc[i].size_bits)) << shift; 181219820Sjeff addr = (__be64 *) buf + desc[i].offset_words; 182219820Sjeff val = (be64_to_cpup(addr) & mask) >> shift; 183219820Sjeff value_write(desc[i].struct_offset_bytes, 184219820Sjeff desc[i].struct_size_bytes, 185219820Sjeff val, 186219820Sjeff structure); 187219820Sjeff } else { 188219820Sjeff if (desc[i].offset_bits % 8 || 189219820Sjeff desc[i].size_bits % 8) { 190219820Sjeff printk(KERN_WARNING "Structure field %s of size %d " 191219820Sjeff "bits is not byte-aligned\n", 192219820Sjeff desc[i].field_name, desc[i].size_bits); 193219820Sjeff } 194219820Sjeff 195219820Sjeff memcpy(structure + desc[i].struct_offset_bytes, 196219820Sjeff buf + desc[i].offset_words * 4 + 197219820Sjeff desc[i].offset_bits / 8, 198219820Sjeff desc[i].size_bits / 8); 199219820Sjeff } 200219820Sjeff } 201219820Sjeff} 202219820SjeffEXPORT_SYMBOL(ib_unpack); 203