/*
 * Copyright (c) 2013-2021 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
 *
 * This software is available to you under a choice of one of two
 * licenses.  You may choose to be licensed under the terms of the GNU
 * General Public License (GPL) Version 2, available from the file
 * COPYING in the main directory of this source tree, or the
 * OpenIB.org BSD license below:
 *
 *     Redistribution and use in source and binary forms, with or
 *     without modification, are permitted provided that the following
 *     conditions are met:
 *
 *      - Redistributions of source code must retain the above
 *        copyright notice, this list of conditions and the following
 *        disclaimer.
 *
 *      - Redistributions in binary form must reproduce the above
 *        copyright notice, this list of conditions and the following
 *        disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
 * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
 * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
 * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 *
 */

/********************************************************
**
*   ibvsmad.c
*
*   the ibvsmad library.
*   this library provides vendor specific mad capabilities.
*
*   mread64, mwrite64 - IS3 I2C bus access MAD - ExtPortAccess -> I2C compliant port #1
*   mread4 , mwrite4  - IS3 CR-Space access MAD - ConfigSpaceAccess
*
*
*   author: yoniy@mellanox.co.il , Apr. 16th 2007
*/

#if HAVE_CONFIG_H
#include <config.h>
#endif
#include <ctype.h>
#include <stdio.h>
#include <stdlib.h>
#include <stdarg.h>
#include <time.h>
#include <string.h>
#include <signal.h>

#ifdef MST_UL
#include <errno.h>
#include "mtcr_int_defs.h"
#endif

#include "mtcr_ib.h"

#ifndef __WIN__
#include "mtcr_mf.h"
#include "mtcr_com_defs.h"
#endif

#include <infiniband/mad.h>

#ifdef __WIN__
#include <windows.h>
#include "com_def.h"

#define BAD_RET_VAL 1
#define LIB_IBMAD   "libibmad"

#define MY_GetPointerAddress(ivm, func_name, f_type)                                 \
    do                                                                               \
    {                                                                                \
        ivm->func_name = (f_type)GetProcAddress(ivm->dl_handle, #func_name);         \
        if (ivm->func_name == NULL)                                                  \
        {                                                                            \
            IBERROR(("Failed to find %s function on ibal.dll file.\n", #func_name)); \
            errno = ENOSYS;                                                          \
            return -1;                                                               \
        }                                                                            \
    } while (0)

#define MY_GetProcAddress(ivm, func_name) MY_GetPointerAddress(ivm, func_name, f_ ## func_name)
#define MY_GetProcAddressIgnoreFail(ivm, func_name) \
    ivm->func_name = (f_ ## func_name)GetProcAddress(ivm->dl_handle, #func_name)
#define MY_GetVarAddress(ivm, var_name) MY_GetPointerAddress(ivm, var_name, void*)

#define DLL_HANDLE       HINSTANCE
#define OP_NOT_SUPPORTED EINVAL
#define IBMAD_CALL_CONV  __stdcall

#else
#include <getopt.h>
#include <unistd.h>
#include <dlfcn.h>
#include "common/compatibility.h"

#define DLL_HANDLE  void*
#define BAD_RET_VAL ~0llu
#define MY_DLSYM(mf, func_name)                           \
    do                                                    \
    {                                                     \
        const char* dl_error;                             \
        mf->func_name = dlsym(mf->dl_handle, #func_name); \
        if ((dl_error = dlerror()) != NULL)               \
        {                                                 \
            IBERROR(("%s", dl_error));                    \
            return -1;                                    \
        }                                                 \
    } while (0)

#define MY_DLSYM_IGNORE_FAIL(mf, func_name) mf->func_name = dlsym(mf->dl_handle, #func_name)

#define OP_NOT_SUPPORTED EOPNOTSUPP
#define IBMAD_CALL_CONV

#endif /* ifdef __WIN__ */

#define __BUILD_VERSION_TAG__ 1.1
/*#include "ibvsmad.h" */

#undef DEBUG
/* TODO: Ask Mohammad why was the iberror changed ? */

/* #define DEBUG(fmt, args...)  if (verbose) iberror(0, __FUNCTION__, fmt, ## args) */
/* #define IBERROR(fmt, args...)   iberror(1, __FUNCTION__, fmt, ## args) */

#define IBERROR(args)             \
    do                            \
    {                             \
        printf("-E- ibvsmad : "); \
        printf args;              \
        printf("\n");             \
        errno = EINVAL;           \
    } while (0)
#define DEBUG(args)                   \
    do                                \
    {                                 \
        if (verbose)                  \
        {                             \
            printf("-D- ibvsmad : "); \
            printf args;              \
            printf("\n");             \
            errno = EINVAL;           \
        }                             \
    } while (0)

/* BIT Slicing macros */
#define ONES32(size)         ((size) ? (0xffffffff >> (32 - (size))) : 0)
#define MASK32(offset, size) (ONES32(size) << (offset))

#define EXTRACT_C(source, offset, size) ((((unsigned)(source)) >> (offset)) & ONES32(size))
#define EXTRACT(src, start, len)        (((len) == 32) ? (src) : EXTRACT_C(src, start, len))

#define MERGE_C(rsrc1, rsrc2, start, len) \
    ((((rsrc2) << (start)) & (MASK32((start), (len)))) | ((rsrc1) & (~MASK32((start), (len)))))
#define MERGE(rsrc1, rsrc2, start, len) (((len) == 32) ? (rsrc2) : MERGE_C(rsrc1, rsrc2, start, len))

#define SWAPL(l) bswap_32(l)

#define IB_VENDOR_SPECIFIC_CLASS_0xA 0x0A

#define NUMBER_OF_MGMT_ELEMENTS 3

#define IB_VS_ATTR_SW_RESET            0x12
#define IB_VS_ATTR_EXT_PORT_INFO       0x13
#define IB_VS_ATTR_CR_ACCESS           0x50
#define IB_VS_ATTR_GENERAL_INFO        0x17
#define IB_VS_ATTR_ACCESS_REGISTER_GMP 0x51

#define IB_VS_ATTRMOD_CR            0x00008000
#define IB_VS_ATTR_EPI_ATTRMOD_I2C1 0x02

#define IB_VS_CR_NUM_OF_RECORDS  1
#define IB_VS_CR_ONE_RECORD_SIZE 4

#define I2C_CHUNK_SIZE  64
#define I2C_DEVICE_ID   0x56
#define I2C_MEMORY_ADDR 0

#define KEY_GUID_SIZE 32

#define UNSUPP_DEVS_NUM 15
#define DEVID_ADDRESS   0xf0014

#define CLASS_VS_SIZE_BYTES  224
#define CLASS_VS_DATA_OFFSET 0x20

/* ConfigSpaceAccess Mode 0 supports address of up to 24 bits. */
#define MODE_0_MAX_ADDRESS_RANGE 0x7FFFFF
#define MODE_2_MAX_RECORDS_NUM   18
#define MODE_2_MAX_DATA_SIZE     (MODE_2_MAX_RECORDS_NUM * 4)

/* Convert BYTES - DWORDS with MEMCPY BE */
#define BYTES_TO_DWORD_BE(dw_dest, byte_src) \
    do                                       \
    {                                        \
        u_int32_t tmp;                       \
        memcpy(&tmp, byte_src, 4);           \
        *(dw_dest) = __be32_to_cpu(tmp);     \
    } while (0)

#define DWORD_TO_BYTES_BE(bytes_dest, dw_src) \
    do                                        \
    {                                         \
        u_int32_t tmp;                        \
        tmp = __cpu_to_be32(*(dw_src));       \
        memcpy(bytes_dest, &tmp, 4);          \
    } while (0)

#define CHECK_NULL(pointer) \
    if (pointer == NULL)    \
    {                       \
        return -1;          \
    }

typedef struct ibmad_port*
    IBMAD_CALL_CONV (*f_mad_rpc_open_port)(char* dev_name, int dev_port, int* mgmt_classes, int num_classes);
typedef void IBMAD_CALL_CONV (*f_mad_rpc_close_port)(struct ibmad_port* srcport);
typedef int IBMAD_CALL_CONV (*f_ib_resolve_portid_str_via)(ib_portid_t            * portid,
                                                           char                   * addr_str,
                                                           int                      dest,
                                                           ib_portid_t            * sm_id,
                                                           const struct ibmad_port* srcport);
typedef uint8_t* IBMAD_CALL_CONV (*f_ib_vendor_call_via)(void                   * data,
                                                         ib_portid_t            * portid,
                                                         ib_vendor_call_t       * call,
                                                         const struct ibmad_port* srcport);
typedef uint8_t* IBMAD_CALL_CONV (*f_smp_query_via)(void                   * buf,
                                                    ib_portid_t            * id,
                                                    unsigned                 attrid,
                                                    unsigned                 mod,
                                                    unsigned                 timeout,
                                                    const struct ibmad_port* srcport);
typedef uint8_t* IBMAD_CALL_CONV (*f_smp_query_status_via)(void                   * buf,
                                                           ib_portid_t            * id,
                                                           unsigned                 attrid,
                                                           unsigned                 mod,
                                                           unsigned                 timeout,
                                                           int                    * rstatus,
                                                           const struct ibmad_port* srcport);
typedef uint8_t* IBMAD_CALL_CONV (*f_smp_set_via)(void                   * buf,
                                                  ib_portid_t            * id,
                                                  unsigned                 attrid,
                                                  unsigned                 mod,
                                                  unsigned                 timeout,
                                                  const struct ibmad_port* srcport);
typedef uint8_t* IBMAD_CALL_CONV (*f_smp_set_status_via)(void                   * buf,
                                                         ib_portid_t            * id,
                                                         unsigned                 attrid,
                                                         unsigned                 mod,
                                                         unsigned                 timeout,
                                                         int                    * rstatus,
                                                         const struct ibmad_port* srcport);
typedef void IBMAD_CALL_CONV (*f_smp_mkey_set)(struct ibmad_port* srcport, uint64_t mkey);

typedef void IBMAD_CALL_CONV (*f_mad_rpc_set_retries)(struct ibmad_port* port, int retries);
typedef void IBMAD_CALL_CONV (*f_mad_rpc_set_timeout)(struct ibmad_port* port, int timeout);
typedef uint32_t IBMAD_CALL_CONV (*f_mad_get_field)(void* buf, int base_offs, enum MAD_FIELDS field);

typedef char* IBMAD_CALL_CONV (*f_portid2str)(ib_portid_t* portid);

typedef void* IBMAD_CALL_CONV (*f_mad_rpc_rmpp)(const struct ibmad_port* srcport,
                                                ib_rpc_t               * rpc,
                                                ib_portid_t            * dport,
                                                ib_rmpp_hdr_t          * rmpp,
                                                void                   * data);

typedef int IBMAD_CALL_CONV (
*f_mad_send_via)(ib_rpc_t* rpc, ib_portid_t* dport, ib_rmpp_hdr_t* rmpp, void* data, struct ibmad_port* srcport);

typedef void* IBMAD_CALL_CONV (
*f_mad_rpc)(const struct ibmad_port* srcport, ib_rpc_t* rpc, ib_portid_t* dport, void* payload, void* rcvdata);

struct __ibvsmad_hndl_t {
    struct ibmad_port         * srcport;
    ib_portid_t                 portid;
    int                         sock;
    int                         tp;
    int                         i2c_secondary;
    int                         use_smp;
    int                         use_class_a;
    u_int64_t                   mkey;
    int                         mkey_is_supported;
    int                         timeout;
    int                         retries_num;
    u_int64_t                   vskey;
    int                         vskey_is_supported;
    enum MAD_DEST               dest_type;
    DLL_HANDLE                  dl_handle;
    f_mad_rpc_open_port         mad_rpc_open_port;
    f_mad_rpc_close_port        mad_rpc_close_port;
    f_ib_vendor_call_via        ib_vendor_call_via;
    f_ib_resolve_portid_str_via ib_resolve_portid_str_via;
    f_smp_query_via             smp_query_via;
    f_smp_query_status_via      smp_query_status_via;
    f_smp_set_via               smp_set_via;
    f_smp_set_status_via        smp_set_status_via;
    f_mad_rpc_set_retries       mad_rpc_set_retries;
    f_mad_rpc_set_timeout       mad_rpc_set_timeout;
    f_mad_rpc_rmpp              mad_rpc_rmpp;
    f_mad_get_field             mad_get_field;
    f_portid2str                portid2str;
    f_smp_mkey_set              smp_mkey_set;
    f_mad_send_via              mad_send_via;
    f_mad_rpc                   mad_rpc;
    void                      * ibdebug;
};

typedef struct __ibvsmad_hndl_t ibvs_mad;

static int verbose = 0;

/********************************************************
**
*    Management class :     0x09 VendorSpecific
*    Management attribute : 0x50 ConfigSpaceAccess
*
*/

#define IB_SMP_ATTR_CR_ACCESS  0xff50
#define IB_DATA_INDEX          8
#define MAX_IB_SMP_DATA_SIZE   (IB_SMP_DATA_SIZE - IB_DATA_INDEX)
#define MAX_IB_SMP_DATA_DW_NUM MAX_IB_SMP_DATA_SIZE / 4

#define CONFIG_ACCESS_MODE_2_DATA_OFFSET    4
#define CONFIG_ACCESS_MODE_2_BITMASK_OFFSET 8

void set_mkey_for_smp_mad(ibvs_mad* vsmad)
{
#ifdef __linux__
    if (vsmad->mkey) {
        vsmad->smp_mkey_set(vsmad->srcport, vsmad->mkey);
    } else {
        vsmad->smp_mkey_set(vsmad->srcport, 0);
    }
#else
    (void)vsmad;
#endif
}

/* Attribute modifier for mode 0: */
/* Bits 24-31: Address MSB */
/* Bits 22-23: Mode = 0 */
/* Bits 16-21: Number of dwords */
/* Bits 0-15: Address LSB */
static u_int32_t create_attribute_mode_0(u_int32_t memory_address, u_int8_t num_of_dwords)
{
    return ((EXTRACT(memory_address, 0, 16) << 00) & 0x0000ffff) | ((num_of_dwords << 16) & 0x00ff0000) |
           ((EXTRACT(memory_address, 16, 8) << 24) & 0xff000000);
}

/* Attribute modifier for mode 2: */
/* Bits 22-23: Mode = 2 */
/* Bits 16-21: Number of records (= number of dwords) */
static u_int32_t create_attribute_mode_2(u_int8_t num_of_dwords)
{
    return (((2 << 22) & 0x800000) | ((num_of_dwords << 16) & 0x00ff0000));
}

/* For ConfigSpaceAccess Mad we use mode 2 */
/* only for accessing addresses larger than 24 bits. */
static unsigned int should_use_mode_2(u_int32_t memory_address, u_int8_t num_of_dwords)
{
    return memory_address + (num_of_dwords * 4) > MODE_0_MAX_ADDRESS_RANGE ? 1 : 0;
}

static void set_mad_data_for_mode_2(u_int32_t     memory_address,
                                    u_int8_t      num_of_dwords,
                                    u_int8_t    * mad_data,
                                    u_int32_t   * attribute_mod,
                                    u_int32_t   * mask,
                                    unsigned int* data_offset)
{
    int i = 0;

    *attribute_mod = create_attribute_mode_2(num_of_dwords);

    /* First dword of each record of ConfigSpaceAccess mode 2 */
    /* contains the current address. */
    for (i = 0; i < num_of_dwords; i++) {
        u_int32_t record_offset = memory_address + i;
        DWORD_TO_BYTES_BE(mad_data + IB_DATA_INDEX + (i * 4), &record_offset);
    }

    /* Second dword contains the data. */
    *data_offset = CONFIG_ACCESS_MODE_2_DATA_OFFSET;

    /* Third dword contains the bitmask. */
    *mask = 0xFFFFFFFF;
}

static uint64_t ibvsmad_craccess_rw_smp(ibvs_mad * h,
                                        u_int32_t  memory_address,
                                        int        method,
                                        u_int8_t   num_of_dwords,
                                        u_int32_t* data)
{
    u_int8_t     mad_data[2 * IB_SMP_DATA_SIZE] = {0};
    int          i;
    u_int8_t   * p;
    u_int32_t    attribute_mod = 0;
    u_int64_t    mkey = 0;
    u_int32_t    mask = 0;
    unsigned int data_offset = 0;
    unsigned int use_mode_2 = should_use_mode_2(memory_address, num_of_dwords);

    if (num_of_dwords > MAX_IB_SMP_DATA_DW_NUM) {
        IBERROR(("size is too big, maximum number of dwords is %d", MAX_IB_SMP_DATA_DW_NUM));
        return BAD_RET_VAL;
    }

    /* Set the mkey */
    mkey = __cpu_to_be64(h->mkey);
    memcpy(&mad_data[0], &mkey, 8);
    set_mkey_for_smp_mad(h);

    if (use_mode_2) {
        set_mad_data_for_mode_2(memory_address, num_of_dwords, mad_data, &attribute_mod, &mask, &data_offset);
    } else {
        attribute_mod = create_attribute_mode_0(memory_address, num_of_dwords);
    }

    DEBUG(("Sending ConfigSpaceAccess SMP MAD...\n"));
    if (h->portid.lid) {
        DEBUG(("Management Class: 0x1\n"));
    } else {
        DEBUG(("Management Class: 0x81\n"));
    }
    DEBUG(("Attribute Modifier = 0x%08x\n", attribute_mod));
    if (use_mode_2) {
        DEBUG(("Mode: 2\n"));
    } else {
        DEBUG(("Mode: 0\n"));
    }
    DEBUG(("Attribute ID = 0x%08x\n", IB_SMP_ATTR_CR_ACCESS));
    DEBUG(("Memory Address = 0x%08x\n", memory_address));
    DEBUG(("Number of dwords = %d\n", num_of_dwords));

    if (method == IB_MAD_METHOD_GET) {
        p = h->smp_query_via(mad_data, &h->portid, IB_SMP_ATTR_CR_ACCESS, attribute_mod, 0, h->srcport);

        if (!p) {
            return BAD_RET_VAL;
        }

        for (i = 0; i < num_of_dwords; i++) {
            BYTES_TO_DWORD_BE(data + i, mad_data + IB_DATA_INDEX + data_offset + (i * 4));
        }
    } else {
        for (i = 0; i < num_of_dwords; i++) {
            DWORD_TO_BYTES_BE(mad_data + IB_DATA_INDEX + data_offset + (i * 4), data + i);
            DWORD_TO_BYTES_BE(mad_data + IB_DATA_INDEX + CONFIG_ACCESS_MODE_2_BITMASK_OFFSET + (i * 4), &mask);
        }
        p = h->smp_set_via(mad_data, &h->portid, IB_SMP_ATTR_CR_ACCESS, attribute_mod, 0, h->srcport);
        if (!p) {
            return BAD_RET_VAL;
        }
    }

    return 0;
}

/* TODO: Correct this value. */
#define MAX_VS_DATA_SIZE      (IB_VENDOR_RANGE1_DATA_SIZE - IB_DATA_INDEX)
#define MAX_IB_VS_DATA_DW_NUM MAX_VS_DATA_SIZE / 4

static uint64_t ibvsmad_craccess_rw_vs(ibvs_mad * h,
                                       u_int32_t  memory_address,
                                       int        method,
                                       u_int8_t   num_of_dwords,
                                       u_int32_t* data,
                                       int        class)
{
    u_int8_t         vsmad_data[2 * IB_VENDOR_RANGE1_DATA_SIZE] = {0};
    ib_vendor_call_t call;
    int              i;
    u_int8_t       * p;
    u_int32_t        attribute_mod = 0;
    u_int32_t        mask = 0;
    u_int64_t        vskey = 0;
    unsigned int     data_offset = 0;
    int              use_mode_2 = should_use_mode_2(memory_address, num_of_dwords);

    if ((h == NULL) || (data == NULL)) {
        return BAD_RET_VAL;
    }

    if (num_of_dwords > MAX_IB_VS_DATA_DW_NUM) {
        IBERROR(("size (%d) is too big, maximum num of dwords is %d", num_of_dwords, MAX_IB_VS_DATA_DW_NUM));
        return BAD_RET_VAL;
    }

    if (use_mode_2) {
        set_mad_data_for_mode_2(memory_address, num_of_dwords, vsmad_data, &attribute_mod, &mask, &data_offset);
    } else {
        attribute_mod = create_attribute_mode_0(memory_address, num_of_dwords);
    }

    call.method = method;
    call.mgmt_class = class;
    call.attrid = IB_VS_ATTR_CR_ACCESS;
    call.mod = attribute_mod;
    call.oui = IB_OPENIB_OUI;
    call.timeout = 0;
    memset(&call.rmpp, 0, sizeof(call.rmpp));

    DEBUG(("Sending ConfigSpaceAccess VS MAD...\n"));
    DEBUG(("Management Class = 0x%02x\n", call.mgmt_class));
    DEBUG(("Attribute Modifier = 0x%08x\n", call.mod));
    if (use_mode_2) {
        DEBUG(("Mode: 2\n"));
    } else {
        DEBUG(("Mode: 0\n"));
    }
    DEBUG(("Attribute ID = 0x%08x\n", call.attrid));
    DEBUG(("Memory Address = 0x%08x\n", memory_address));
    DEBUG(("Number of dwords = %d\n", num_of_dwords));

    /* Set the VSkey. */
    vskey = __cpu_to_be64(h->vskey);
    memcpy(vsmad_data, &vskey, 8);

    for (i = 0; i < num_of_dwords; i++) {
        if (method == IB_MAD_METHOD_SET) {
            DWORD_TO_BYTES_BE(vsmad_data + IB_DATA_INDEX + data_offset + (i * 4), data + i);
            DWORD_TO_BYTES_BE(vsmad_data + IB_DATA_INDEX + CONFIG_ACCESS_MODE_2_BITMASK_OFFSET + (i * 4), &mask);
        }
    }

    p = h->ib_vendor_call_via(vsmad_data, &h->portid, &call, h->srcport);
    if (!p) {
        return BAD_RET_VAL;
    }

    for (i = 0; i < num_of_dwords; i++) {
        BYTES_TO_DWORD_BE(data + i, vsmad_data + IB_DATA_INDEX + data_offset + (i * 4));
    }

    return 0;
}

static uint64_t ibvsmad_craccess_rw(ibvs_mad * h,
                                    u_int32_t  memory_address,
                                    int        method,
                                    u_int8_t   num_of_dwords,
                                    u_int32_t* data)
{
    if (h->use_smp) {
        return ibvsmad_craccess_rw_smp(h, memory_address, method, num_of_dwords, data);
    } else {
        if (h->use_class_a) {
            return ibvsmad_craccess_rw_vs(h, memory_address, method, num_of_dwords, data,
                                          IB_VENDOR_SPECIFIC_CLASS_0xA);
        } else {
            return 1;
        }
    }
    return 0;
}

#ifdef __WIN__

int process_dynamic_linking(ibvs_mad* ivm, int mad_init)
{
    ivm->dl_handle = LoadLibrary(TEXT(LIB_IBMAD));

    if (ivm->dl_handle != NULL) {
        MY_GetProcAddress(ivm, mad_rpc_open_port);
        MY_GetProcAddress(ivm, mad_rpc_close_port);
        MY_GetProcAddress(ivm, ib_vendor_call_via);
        MY_GetProcAddress(ivm, ib_resolve_portid_str_via);
        MY_GetProcAddress(ivm, smp_query_via);
        MY_GetProcAddressIgnoreFail(ivm, smp_query_status_via);
        MY_GetProcAddress(ivm, smp_set_via);
        MY_GetProcAddressIgnoreFail(ivm, smp_set_status_via);
        MY_GetProcAddress(ivm, mad_rpc_set_retries);
        MY_GetProcAddress(ivm, mad_rpc_set_timeout);
        MY_GetProcAddress(ivm, mad_rpc_rmpp);
        MY_GetProcAddress(ivm, mad_rpc);
        MY_GetProcAddress(ivm, portid2str);
        MY_GetProcAddress(ivm, mad_get_field);
        MY_GetProcAddress(ivm, mad_send_via);
        MY_GetVarAddress(ivm, ibdebug);
    } else {
        IBERROR(("Failed to load " LIB_IBMAD ".dll library %#x", (unsigned int)GetLastError()));
        errno = ENOENT;
        return -1;
    }
    return 0;
}

int free_dll_handle(mfile* mf)
{
    return FreeLibrary(((ibvs_mad*)(mf->ctx))->dl_handle);
}

#else

int process_dynamic_linking(ibvs_mad* ivm, int mad_init)
{
    char    * libs[] = {"libibmad.so.5", "libibmad.so.12"};
    u_int32_t i;

    (void)mad_init;
    for (i = 0; i < sizeof(libs) / sizeof(libs[0]); i++) {
        ivm->dl_handle = dlopen(libs[i], RTLD_LAZY);
        if (ivm->dl_handle) {
            break;
        }
    }
    if (!ivm->dl_handle) {
        const char* errstr = dlerror();
        IBERROR(("%s", errstr));
        /* free(ivm); */
        return -1;
    }

    dlerror();

    MY_DLSYM(ivm, mad_rpc_open_port);
    MY_DLSYM(ivm, mad_rpc_close_port);
    MY_DLSYM(ivm, ib_vendor_call_via);
    MY_DLSYM(ivm, ib_resolve_portid_str_via);
    MY_DLSYM(ivm, smp_query_via);
    MY_DLSYM_IGNORE_FAIL(ivm, smp_query_status_via);
    MY_DLSYM(ivm, smp_set_via);
    MY_DLSYM_IGNORE_FAIL(ivm, smp_set_status_via);
    MY_DLSYM(ivm, mad_rpc_set_retries);
    MY_DLSYM(ivm, mad_rpc_set_timeout);
    MY_DLSYM(ivm, mad_rpc_rmpp);
    MY_DLSYM(ivm, mad_get_field);
    MY_DLSYM(ivm, portid2str);
    MY_DLSYM(ivm, smp_mkey_set);
    MY_DLSYM(ivm, mad_send_via);
    MY_DLSYM(ivm, mad_rpc);
    MY_DLSYM(ivm, ibdebug);
    return 0;
}

int free_dll_handle(mfile* mf)
{
    return dlclose(((ibvs_mad*)(mf->ctx))->dl_handle);
}

#endif /* ifdef __WIN__ */

char* my_strdup(const char* name)
{
    int   len;
    char* buf;

    if (name == NULL) {
        return NULL;
    }
    len = strlen(name);

    buf = malloc((len + 1) * sizeof(char));
    if (buf == NULL) {
        return NULL;
    }
    strcpy(buf, name);

    return buf;
}
#define MTCR_IB_TIMEOUT  "MTCR_IB_TIMEOUT"
#define MTCR_IB_RETRIES  "MTCR_IB_RETRIES"
#define MTCR_IB_VKEY     "MTCR_IB_VKEY"
#define MTCR_IBMAD_DEBUG "MTCR_IBMAD_DEBUG"

int get_env_var(char* env_name, int* env_var)
{
    char* env_val;

    env_val = getenv(env_name);
    if (env_val) {
        *env_var = atol(env_val);
    }
    return 0;
}
int get_64_env_var(char* env_name, u_int64_t* env_var)
{
    char *env_val, *endp = NULL;

    env_val = getenv(env_name);
    if (env_val) {
        *env_var = strtoull(env_val, &endp, 0);
    }
    return 0;
}

int get_env_vars(ibvs_mad* ivm)
{
    get_env_var(MTCR_IB_TIMEOUT, &(ivm->timeout));
    get_env_var(MTCR_IB_RETRIES, &(ivm->retries_num));
    get_64_env_var(MTCR_IB_VKEY, &((ivm->vskey)));
    return 0;
}

int is_vs_crspace_supported(ibvs_mad* h)
{
    u_int8_t* p;
    u_int8_t  mad_data[IB_SMP_DATA_SIZE] = {0};
    u_int32_t dev_id;
    u_int32_t data = 0;
    int       i = 0;
    u_int32_t unsupported_devs[UNSUPP_DEVS_NUM] = {
        0x6746, /* MT26438 [ConnectX-2 VPI w/ Virtualization+]"*/
        0x6764, /* MT26468 [Mountain top]"                     */
        0xbd34, /* IS4 IB SDR"                                 */
        0xbd35, /* IS4 IB DDR"                                 */
        0xbd36, /* IS4 IB QDR"                                 */
        0xfa66, /* BridgeX VPI up, E/FC down"                  */
        0xfa7a, /* BridgeX EN up, E/FC down"                   */
        0x1001, /* ConnectX-2 VF"                              */
        0x1003, /* MT27500 [ConnectX-3]"                       */
        0x1005, /* MT27510 Family"                             */
        0x1007, /* MT27520 ConnectX-3 Pro Family"              */
    };
    uint64_t  ret =
        ibvsmad_craccess_rw_vs(h, DEVID_ADDRESS, IB_MAD_METHOD_GET, 1, &data, IB_VENDOR_SPECIFIC_CLASS_0xA);

    if (!ret) {
        h->use_class_a = 1;
        return 1;
    }

    DEBUG(("Vendor Specific is not supported, checking SMP .. "));
    set_mkey_for_smp_mad(h);
    p = h->smp_query_via(mad_data, &h->portid, IB_ATTR_NODE_INFO, 0, 0, h->srcport);

    if (NULL == p) {
        return 1;
    }

    dev_id = h->mad_get_field(mad_data, 0, IB_NODE_DEVID_F);

    for (i = 0; i < UNSUPP_DEVS_NUM; i++) {
        if (dev_id == unsupported_devs[i]) {
            return 1;
        }
    }

    return 0;
}

typedef enum key_type_t {
    MKEY,
    VSKEY
} key_type;

char* trim(char* string)
{
    char* back;

    /* Left trim. */
    while (isspace(*string)) {
        string++;
    }

    int len = strlen(string);

    if (len == 0) {
        return (string);
    }

    /* Right trim. */
    back = string + len;

    while (isspace(*--back)) {
    }

    *(back + 1) = '\0';

    return string;
}

int get_mft_conf_field_value(char* line, char* field_name, char* value, int* is_empty)
{
    char* delimiter = "=";
    char* tmp_value;

    if (strstr(line, field_name) != NULL) {
        /* Get the value. */
        tmp_value = strtok(line, delimiter);
        tmp_value = strtok(NULL, delimiter);

        /* Remove spaces. */
        tmp_value = trim(tmp_value);

        if (strlen(tmp_value)) {
            memcpy(value, tmp_value, sizeof(strlen(tmp_value)));
        } else {
            *is_empty = 1;
        }

        return 0;
    }

    return -1;
}

int load_file(FILE** file_descriptor, const char* file_name_path)
{
    /* Open the file. */
    *file_descriptor = fopen(file_name_path, "r");

    if (!(*file_descriptor)) {
        /* Failed to open the file. */
        return -1;
    }

    return 0;
}

int parse_mft_cfg_file(char* sm_config_path, key_type key)
{
    const char* sm_config_default_path = "/var/cache/opensm/";
    const char* mft_conf_file_path = "/etc/mft/mft.conf";
    int         is_key_enabled = 0;
    int         is_empty = 0;
    int         ret_value = -1;
    char        line[1024] = {0};
    char        value[256] = {0};
    char      * key_enabled_field;

    if (key == MKEY) {
        key_enabled_field = "mkey_enable";
    } else {
        key_enabled_field = "vskey_enable";
    }

    FILE* mft_conf_file_descriptor = NULL;

    /* Load the mft configuration file. */
    if (load_file(&mft_conf_file_descriptor, mft_conf_file_path)) {
        /* Failed to open the file. */
        return -1;
    }

    /* Go over the content file. */
    while ((fgets(line, 1024, mft_conf_file_descriptor))) {
        /* Check if the mkey feature is enable. */
        if (!get_mft_conf_field_value(line, key_enabled_field, value, &is_empty)) {
            /* key feature is supported ? */
            if (strcmp(value, "yes")) {
                /* Mkey is unsuppurted. */
                break;
            }

            is_key_enabled = 1;
        }
        /* Check if the user changed the default mkey directory. */
        else if (!get_mft_conf_field_value(line, "sm_config_dir", value, &is_empty)) {
            if (!is_key_enabled) {
                /* key feature is disabled. */
                break;
            }

            /* Empty value ? */
            if (is_empty) {
                /* Use the default sm path. */
                memcpy(sm_config_path, sm_config_default_path, strlen(sm_config_default_path));
            } else {
                memcpy(sm_config_path, value, strlen(value));
            }

            /* sm_config_path found. */
            ret_value = 0;
        }
    }

    /* Close the file. */
    fclose(mft_conf_file_descriptor);

    return ret_value;
}

void get_lid_integer(char* lid, int* lid_integer)
{
    int base = 10;

    /* Check if we have '0x' in the beginning of the string. */
    if ((strlen(lid) > 1) && (lid[0] == '0') && ((lid[1] == 'x') || (lid[1] == 'X'))) {
        base = 16;
    }

    *lid_integer = strtol(lid, NULL, base);
}

int find_guid(char* lid, char* guid, char* line)
{
    char* delimiter = " ";
    char* tmp_value;
    char* tmp_guid;
    int   ret_value = -1;
    int   lid_lower_bound;
    int   lid_upper_bound;
    int   lid_from_device;

    CHECK_NULL(guid)

    /* Get the lid ranges from device, */
    /* remove spaces and convert to integer */
    get_lid_integer(trim(lid), &lid_from_device);

    tmp_guid = strtok(line, delimiter);
    CHECK_NULL(tmp_guid)

    tmp_value = strtok(NULL, delimiter);
    CHECK_NULL(tmp_value)

    get_lid_integer(trim(tmp_value), &lid_lower_bound);

    tmp_value = strtok(NULL, delimiter);
    CHECK_NULL(tmp_value)

    get_lid_integer(trim(tmp_value), &lid_upper_bound);

    if ((lid_from_device >= lid_lower_bound) && (lid_from_device <= lid_upper_bound)) {
        strcpy(guid, tmp_guid);
        ret_value = 0;
    }

    return ret_value;
}

int parse_lid2guid_file(char* sm_config_path, char* lid, char* guid)
{
    const char* guid2lid = "guid2lid";
    FILE      * file_descriptor = NULL;
    char        line[1024] = {0};
    char        conf_path[256];
    int         ret_value = -1;

    /* Parse the guid2lid file. */
    strcpy(conf_path, sm_config_path);
    strcat(conf_path, guid2lid);

    if (load_file(&file_descriptor, conf_path)) {
        /* Failed to open file. */
        return -1;
    }

    /* Go over the content of the guid2lid file. */
    while ((fgets(line, 1024, file_descriptor))) {
        if (!find_guid(lid, guid, line)) {
            ret_value = 0;
            break;
        }
    }

    fclose(file_descriptor);

    /* No guid found. */
    return ret_value;
}

int parse_guid2key_file(ibvs_mad* ivm, char* sm_config_path, char* guid, key_type key)
{
    char* guid2key_filename;
    char* delimiter = " ";
    char* tmp_value;
    FILE* file_descriptor = NULL;
    char  line[1024] = {0};
    char  conf_path[256];
    int   ret_value = -1;

    if (key == MKEY) {
        guid2key_filename = "guid2mkey";
    } else {
        guid2key_filename = "guid2vskey";
    }

    /* Parse the guid2lid file. */
    strcpy(conf_path, sm_config_path);
    strcat(conf_path, guid2key_filename);

    if (load_file(&file_descriptor, conf_path)) {
        /* Failed to open file. */
        return -1;
    }

    /* Go over the content of the guid2key file. */
    while ((fgets(line, 1024, file_descriptor))) {
        /* Get the key. */
        tmp_value = strtok(line, delimiter);

        if (!strcmp(tmp_value, guid)) {
            tmp_value = strtok(NULL, delimiter);

            if (key == MKEY) {
                ivm->mkey = strtoull(tmp_value, NULL, 0);
            } else {
                ivm->vskey = strtoull(tmp_value, NULL, 0);
            }

            ret_value = 0;
            break;
        }
    }

    fclose(file_descriptor);

    /* Failed to get the mkey. */
    return ret_value;
}

int extract_key(ibvs_mad* ivm, char* sm_config_path, char* lid, key_type key)
{
    char guid[KEY_GUID_SIZE];

    /* Parse the lid2guid file. */
    if (parse_lid2guid_file(sm_config_path, lid, guid)) {
        return -1;
    }

    /* Parse the guid2key file. */
    if (parse_guid2key_file(ivm, sm_config_path, guid, key)) {
        return -1;
    }

    return 0;
}

int get_key(ibvs_mad* ivm, char* lid, key_type key)
{
    char sm_config_path[256] = {0};

    /* Parameters validation. */
    if (!ivm || !lid) {
        return -1;
    }

    /* Parse the configuration file in order to extract the key info. */
    if (parse_mft_cfg_file(sm_config_path, key)) {
        /* Failed to parse the key fields from */
        /*   the mft configuration file. */
        return -1;
    }

    /* Extract the key. */
    if (extract_key(ivm, sm_config_path, lid, key)) {
        /* Failed to extract the key. */
        return -1;
    }

    return 0;
}

/********************************************************
**
*    Initialize madrpc; Register the vendor specific management classes under
*    the libibmad library; Resolve destination lid from input.
*
*/
int mib_open(const char* name, mfile* mf, int mad_init)
{
    char* ca = 0;
    int   mgmt_classes[NUMBER_OF_MGMT_ELEMENTS] =
    {IB_SMI_CLASS, IB_SMI_DIRECT_CLASS,
     IB_VENDOR_SPECIFIC_CLASS_0xA};
    ib_portid_t* sm_id = 0;
    int          ca_port = 0;
    ibvs_mad   * ivm = NULL;
    char       * nbuf = NULL;
    char        *path_str, *p;
    int          rc = -1;
    int          lid_provided = 0;
    char       * sl_str;
    char       * first_comma;
    char       * second_comma;

    /* ibdebug = 1; */
    if (!mf || !name) {
        IBERROR(("Bad(null) device argument for inband access"));
        errno = EINVAL;
        goto end;
    }
    if (!(ivm = (ibvs_mad*)malloc(sizeof(ibvs_mad)))) {
        IBERROR(("can't allocate ibvsmad_mfile"));
        errno = ENOMEM;
        goto end;
    }
    memset(ivm, 0, sizeof(ibvs_mad));

    nbuf = my_strdup(name);
    if (nbuf == NULL) {
        errno = ENOMEM;
        goto end;
    }

    ivm->mkey_is_supported = 0;
    ivm->vskey_is_supported = 0;

    if (strncmp("ibdr-", nbuf, 5) == 0) {
        ivm->use_smp = 1;
        ivm->dest_type = IB_DEST_DRPATH;
        path_str = nbuf + 5;
    } else if (strncmp("iblid-", nbuf, 6) == 0) {
        ivm->use_smp = 1;
        ivm->dest_type = IB_DEST_LID;
        path_str = nbuf + 6;
    } else if ((p = strstr(nbuf, "lid-")) != 0) {
#ifdef __linux__
        ivm->mkey_is_supported = 1;
        ivm->vskey_is_supported = 1;
#endif
        lid_provided = 1;
        ivm->dest_type = IB_DEST_LID;
        path_str = p + 4;
    } else if ((p = strstr(nbuf, "lid_noinit-")) != 0) {
#ifdef __linux__
        ivm->mkey_is_supported = 1;
        ivm->vskey_is_supported = 1;
#endif
        lid_provided = 1;
        ivm->dest_type = IB_DEST_LID;
        path_str = p + 11;
        mad_init = 0;
    } else {
        IBERROR(("Bad device argument for inband access"));
        errno = EINVAL;
        goto end;
    }

    first_comma = strstr(nbuf, ",");
    if (first_comma) {
        *first_comma = '\0';
        ca = first_comma + 1;

        second_comma = strstr(ca, ",");
        if (second_comma) {
            char* ep;
            *second_comma = '\0';

            ca_port = strtol(second_comma + 1, &ep, 0);

            if (*ep != '\0') {
                IBERROR(("Bad port number format (%s)", second_comma + 1));
                errno = EINVAL;
                goto end;
            }
        }
    }

    ivm->retries_num = MAD_DEF_RETRIES;
    ivm->timeout = MAD_DEF_TIMEOUT_MS;
    /* Get env variables. */
    get_env_vars(ivm);
    /* The DR in the device name is a '.' separated list. */
    /* Change it to a ',' separated list like ibmad likes. */
    if (ivm->dest_type == IB_DEST_DRPATH) {
        char* c;
        for (c = path_str; *c; c++) {
            if (*c == '.') {
                *c = ',';
            }
        }
    }

#ifdef IBVSMAD_DLOPEN

    ivm->mad_rpc_open_port = mad_rpc_open_port;
    ivm->mad_rpc_close_port = mad_rpc_close_port;
    ivm->ib_vendor_call_via = ib_vendor_call_via;
    ivm->ib_resolve_portid_str_via = ib_resolve_portid_str_via;
    ivm->smp_query_via = smp_query_via;
    ivm->smp_set_via = smp_set_via;
    ivm->mad_rpc_set_retries = mad_rpc_set_retries;
    ivm->mad_rpc_set_timeout = mad_rpc_set_timeout;
    ivm->mad_get_field = mad_get_field;
    ivm->portid2str = portid2str;

#ifdef __linux__
    ivm->smp_mkey_set = smp_mkey_set;
#endif

#else
    if (process_dynamic_linking(ivm, mad_init) == -1) {
        goto end;
    }
#endif
    get_env_var(MTCR_IBMAD_DEBUG, ivm->ibdebug);

    ivm->srcport = ivm->mad_rpc_open_port(ca, ca_port, mgmt_classes, NUMBER_OF_MGMT_ELEMENTS);
    if (ivm->srcport == NULL) {
        int vendor_specific_class[1] = {IB_VENDOR_SPECIFIC_CLASS_0xA};
        ivm->srcport = ivm->mad_rpc_open_port(ca, ca_port, vendor_specific_class, 1);
        ivm->use_smp = 0;
        ivm->use_class_a = 1;
        if (ivm->srcport == NULL) {
            goto end;
        }
    }

    ivm->mad_rpc_set_retries(ivm->srcport, ivm->retries_num);
    ivm->mad_rpc_set_timeout(ivm->srcport, ivm->timeout);

    /* Initialize the mkey & vskey. */
    ivm->mkey = 0;
    ivm->vskey = 0;

    if (ivm->mkey_is_supported) {
        rc = get_key(ivm, path_str, MKEY);
        /* Do we have an mkey? */
        if (!rc) {
            /* Set the mkey. */
            ivm->smp_mkey_set(ivm->srcport, ivm->mkey);
        }
    }

    if (ivm->vskey_is_supported) {
        get_key(ivm, path_str, VSKEY);
    }

    rc = ivm->ib_resolve_portid_str_via(&ivm->portid, (char*)path_str, ivm->dest_type, sm_id, ivm->srcport);
    if (rc != 0) {
        IBERROR(("can't resolve destination port %s", path_str));
        goto end;
    }

    /* TODO: Remove the SL setting if DR method is mainly used. */

    /* For ConnectIB: */
    /* If the target device is connectib or SX and we use LID rout, use the SMP LID route and not the GPM */
    if (lid_provided) {
        if (is_vs_crspace_supported(ivm)) {
            DEBUG(("For this LID using VS MAD"));
            ivm->use_smp = 0;
        } else {
            DEBUG(("For this LID using SMP MAD"));
            ivm->use_smp = 1;
        }
    }

#define MTCR_IBSL_ENV "MTCR_IB_SL"

    if (!ivm->use_smp) {
        sl_str = getenv(MTCR_IBSL_ENV);
        if (sl_str) {
            char    * ep;
            u_int32_t sl = strtoul(sl_str, &ep, 0);
            if (*ep) {
                fprintf(stderr, "-E- Failed to parse env var %s (%s). Ignoring\n", MTCR_IBSL_ENV, sl_str);
            } else {
                ivm->portid.sl = (u_int8_t)sl;
            }
        }
    }

    mf->ctx = ivm;

    if (nbuf) {
        free(nbuf);
    }

    return 0;
end:
    if (ivm) {
#ifndef __WIN__
        if (ivm->dl_handle) {
            dlclose(ivm->dl_handle);
        }
#endif
        free(ivm);
    }
    if (nbuf) {
        free(nbuf);
    }
    return -1;
}

/********************************************************
**
*    Free resources
*
*/
int mib_close(mfile* mf)
{
    if (mf) {
        if (mf->ctx) {
            /* TODO: free the ddl handlers */
            ibvs_mad* h = (ibvs_mad*)(mf->ctx);
            h->mad_rpc_close_port(h->srcport);
#ifndef IBVSMAD_DLOPEN
            free_dll_handle(mf);
#endif
            free(mf->ctx);
            mf->ctx = NULL;
        }
    }
    return 0;
}

#define CHECK_ALIGN(length)                                      \
    {                                                            \
        if (length % 4)                                          \
        {                                                        \
            IBERROR(("Size must be 4 aligned, got %d", length)); \
            return -1;                                           \
        }                                                        \
    }

int mib_get_chunk_size(mfile* mf)
{
    if (!mf || !mf->ctx) {
        IBERROR(("get chunk size failed. Null Param."));
        return -1;
    }
    ibvs_mad* h = (ibvs_mad*)(mf->ctx);

    if (h->use_smp) {
        return MAX_IB_SMP_DATA_SIZE;
    }
    return MAX_VS_DATA_SIZE;
}

/********************************************************
**
*    Read an IS3 CR-Space 32 bit register
*
*/
int mib_read4(mfile* mf, u_int32_t memory_address, u_int32_t* data)
{
    if (!mf || !mf->ctx || !data) {
        IBERROR(("cr access read failed. Null Param."));
        return -1;
    }
    ibvs_mad* h = (ibvs_mad*)(mf->ctx);

    DEBUG(("mread4 of 0x%08x", memory_address));
    if (ibvsmad_craccess_rw(h, memory_address, IB_MAD_METHOD_GET, 1, data) == ~0ull) {
        IBERROR(("cr access read to %s failed", h->portid2str(&h->portid)));
        return -1;
    }
    return 4;
}

/********************************************************
**
*    Write an IS3 CR-Space 32 bit register
*
*/
int mib_write4(mfile* mf, u_int32_t memory_address, u_int32_t _data)
{
    if (!mf || !mf->ctx) {
        IBERROR(("cr access write failed. Null Param."));
        return -1;
    }
    ibvs_mad* h = (ibvs_mad*)(mf->ctx);
    u_int32_t data = _data;

    DEBUG(("mwrite4 of 0x%08x\n", memory_address));

    if (ibvsmad_craccess_rw(h, memory_address, IB_MAD_METHOD_SET, 1, &data) == ~0ull) {
        IBERROR(("cr access write to %s failed", h->portid2str(&h->portid)));
        return -1;
    }
    return 4;
}

enum {
    BLOCKOP_READ,
    BLOCKOP_WRITE
};

int mib_block_op(mfile* mf, unsigned int offset, u_int32_t* data, int length, int op)
{
    if (!mf || !mf->ctx || !data) {
        IBERROR(("cr access read failed. Null Param."));
        return -1;
    }
    ibvs_mad* h = (ibvs_mad*)(mf->ctx);
    int       method = IB_MAD_METHOD_GET;

    if (op == BLOCKOP_WRITE) {
        method = IB_MAD_METHOD_SET;
    }
    CHECK_ALIGN(length);
    int chunk_size = mib_get_chunk_size(mf);

    /* for addresses > 24 bits we use ConfigSpaceAccess mode 2 */
    /* which is limited to 72 bytes. */
    if (offset + MAX_VS_DATA_SIZE > MODE_0_MAX_ADDRESS_RANGE) {
        chunk_size = MODE_2_MAX_DATA_SIZE;
    }
    int t_offset = 0;

    while (t_offset < length) {
        int          left_size = length - t_offset;
        int          to_op = left_size > chunk_size ? chunk_size : left_size;
        unsigned int curret_offset = offset + t_offset;
        if (ibvsmad_craccess_rw(h, curret_offset, method, (to_op / 4), data + t_offset / 4) == ~0ull) {
            IBERROR(("cr access %s to %s failed", op == BLOCKOP_READ ? "read" : "write", h->portid2str(&h->portid)));
            return -1;
        }
        /* If approaching addresses > 24 bits we will switch to mode 2. */
        if (curret_offset + chunk_size > MODE_0_MAX_ADDRESS_RANGE) {
            chunk_size = MODE_2_MAX_DATA_SIZE;
        }
        t_offset += chunk_size;
    }
    return length;
}

MTCR_API int mib_readblock(mfile* mf, unsigned int offset, u_int32_t* data, int length)
{
    return mib_block_op(mf, offset, data, length, BLOCKOP_READ);
}
MTCR_API int mib_writeblock(mfile* mf, unsigned int offset, u_int32_t* data, int length)
{
    return mib_block_op(mf, offset, data, length, BLOCKOP_WRITE);
}

int is_managed_node_supports_swreset(mfile* mf)
{
    ib_vendor_call_t call;
    ibvs_mad       * h = (ibvs_mad*)(mf->ctx);
    u_int8_t       * p;
    u_int32_t        vsmad_data[IB_VENDOR_RANGE1_DATA_SIZE / 4] = {0};

    call.method = IB_MAD_METHOD_GET;
    call.mgmt_class = IB_VENDOR_SPECIFIC_CLASS_0xA;
    call.attrid = IB_VS_ATTR_GENERAL_INFO;
    call.mod = 0;
    call.oui = IB_OPENIB_OUI;
    call.timeout = 0;
    memset(&call.rmpp, 0, sizeof(call.rmpp));

    p = h->ib_vendor_call_via(vsmad_data, &h->portid, &call, h->srcport);
    if (!p) {
        fprintf(stderr, "-E- ib mad method call failed.\n");
        return 0;
    }

    int i;

    for (i = 0; i < IB_VENDOR_RANGE1_DATA_SIZE / 4; i++) {
        vsmad_data[i] = __be32_to_cpu(vsmad_data[i]);
    }

    /* vend_key 0x8 bytes, which present in any vendor specific MAD return in vsmad_data. */
    /* We want to skip them when relating GeneralInfo MAD fields in specific. */
    /* The capability_mask offset which is 0x80, is calculated from the beginning of GeneralInfo MAD fields. */
    /* So overall we have an offset of 0x8+0x80 bytes. */
    const int capability_mask_byte_offset = 0x8 + 0x80;
    const int is_enh_port0_device_reset_supported_bit_offset = 25;

    return EXTRACT(vsmad_data[capability_mask_byte_offset / 4], is_enh_port0_device_reset_supported_bit_offset, 1);
}

int is_node_managed(ibvs_mad* h)
{
    u_int8_t* p;
    u_int8_t  mad_data[IB_SMP_DATA_SIZE] = {0};
    u_int8_t  enhanced_port;

    /* Set the mkey. */
    u_int64_t mkey = __cpu_to_be64(h->mkey);

    memcpy(&mad_data[0], &mkey, 8);
    set_mkey_for_smp_mad(h);

    p = h->smp_query_via(mad_data, &h->portid, IB_ATTR_SWITCH_INFO, 0, 0, h->srcport);

    if (NULL == p) {
        return 0;
    }

    enhanced_port = EXTRACT(mad_data[16], /* Big endian */ 3, 1);
    return enhanced_port;
}
/********************************************************
**
*    Management class :     0x0A VendorSpecific
*    Management attribute : 0x12 SwReset
*
*
*/
#define MTCR_SWRESET_ENV "MTCR_SWRESET_TIMER"
int mib_swreset(mfile* mf)
{
    if (!mf || !mf->ctx) {
        errno = EINVAL;
        IBERROR(("swreset write failed. Null Param."));
        return -1;
    }

    u_int32_t        swreset_timer = 15;
    char           * ep;
    char           * swreset_env;
    ibvs_mad       * h = (ibvs_mad*)(mf->ctx);
    u_int8_t       * p;
    u_int8_t         vsmad_data[IB_VENDOR_RANGE1_DATA_SIZE] = {0};
    ib_vendor_call_t call;

    swreset_env = getenv(MTCR_SWRESET_ENV);
    if (swreset_env) {
        u_int32_t new_timer = strtol(swreset_env, &ep, 0);
        if (*ep != '\0') {
            fprintf(stderr, "-W- Bad %s env var format. Ignoring\n", MTCR_SWRESET_ENV);
        } else if (new_timer > 255) {
            fprintf(stderr, "-W- Bad %s env var value ( > 255). Ignoring\n", MTCR_SWRESET_ENV);
        } else {
            /* All OK */
            fprintf(stderr, "-I- Set reset timer to %d seconds\n", new_timer);
            swreset_timer = new_timer;
        }
    }

    if (is_node_managed(h) && !is_managed_node_supports_swreset(mf)) {
        errno = OP_NOT_SUPPORTED;
        return -1;
    }

    call.method = IB_MAD_METHOD_SET;
    call.mgmt_class = IB_VENDOR_SPECIFIC_CLASS_0xA;
    call.attrid = IB_VS_ATTR_SW_RESET;
    call.mod = swreset_timer;
    call.oui = IB_OPENIB_OUI;
    call.timeout = 0;
    memset(&call.rmpp, 0, sizeof call.rmpp);

    p = h->ib_vendor_call_via(vsmad_data, &h->portid, &call, h->srcport);
    if (!p) {
        return -1;
    }

    return 0;
}

#define IB_SMP_ATTR_REG_ACCESS 0xff52

static int mib_status_translate(int status)
{
    switch ((status >> 2) & 0x7) {
    case 1:
        return ME_MAD_BAD_VER;

    case 2:
        return ME_MAD_METHOD_NOT_SUPP;

    case 3:
        return ME_MAD_METHOD_ATTR_COMB_NOT_SUPP;

    case 7:
        return ME_MAD_BAD_DATA;
    }

    if (status & 0x1) {
        return ME_MAD_BUSY;
    } else if ((status >> 1) & 0x1) {
        return ME_MAD_REDIRECT;
    }

    return ME_MAD_GENERAL_ERR;
}

uint8_t* cls_a_query_via(void                   * rcvbuf,
                         ibvs_mad               * vsmad,
                         ib_portid_t            * dest,
                         const unsigned           timeout,
                         const unsigned           attribute_id,
                         const struct ibmad_port* srcport,
                         int                    * return_status)
{
    ib_rpc_v1_t rpc;

    rpc.rstatus = 0;
    ib_rpc_t* rpcold = (ib_rpc_t*)(void*)&rpc;
    int       lid = dest->lid;
    void    * p_ret;

    if (lid == -1) {
        IBWARN("only lid routed is supported");
        return NULL;
    }

    rpc.mgtclass = IB_VENDOR_SPECIFIC_CLASS_0xA | IB_MAD_RPC_VERSION1;
    rpc.method = MACCESS_REG_METHOD_SET;
    rpc.attr.id = attribute_id;
    rpc.attr.mod = 0;
    rpc.timeout = timeout;
    rpc.datasz = CLASS_VS_SIZE_BYTES;
    rpc.dataoffs = CLASS_VS_DATA_OFFSET;
    /* There is no member for VSKey in rpc struct, */
    /* but the offset for VSKey & MKey in the mad is the same. */
    rpc.mkey = vsmad->vskey;

    if (!dest->qp) {
        dest->qp = 1;
    }
    if (!dest->qkey) {
        dest->qkey = IB_DEFAULT_QP1_QKEY;
    }

    p_ret = vsmad->mad_rpc(srcport, rpcold, dest, rcvbuf, rcvbuf);
    errno = rpc.error;
    *return_status = rpc.rstatus;
    return p_ret;
}

int mib_send_cls_a_access_reg_mad(mfile* mf, u_int8_t* data)
{
    if (!mf || !mf->ctx || !data) {
        IBERROR(("mib_send_cls_a_access_reg_mad failed. Null Param."));
        return ME_BAD_PARAMS;
    }
    ibvs_mad* vsmad = (ibvs_mad*)(mf->ctx);
    u_int8_t* call_result;
    int       return_status = -1;

    /* Set the VSKey. */
    if (vsmad->vskey) {
        vsmad->smp_mkey_set(vsmad->srcport, vsmad->vskey);
    } else {
        vsmad->smp_mkey_set(vsmad->srcport, 0);
    }

    call_result =
        cls_a_query_via(data, vsmad, &vsmad->portid, 0, IB_SMP_ATTR_REG_ACCESS, vsmad->srcport, &return_status);

    if (!call_result || (return_status > 0)) {
        if (return_status > 0) {
            return mib_status_translate(return_status);
        }
        return -1;
    }

    return ME_OK;
}

int response_expected(int method)
{
    return method == IB_MAD_METHOD_GET || method == IB_MAD_METHOD_SET || method == IB_MAD_METHOD_TRAP;
}

uint8_t* ib_vendor_call_status_via(ibvs_mad         * vsmad,
                                   void             * data,
                                   ib_portid_t      * portid,
                                   ib_vendor_call_t * call,
                                   struct ibmad_port* srcport,
                                   int              * return_status)
{
    ib_rpc_v1_t rpc;

    rpc.rstatus = 0;
    ib_rpc_t* rpcold = (ib_rpc_t*)(void*)&rpc;
    int       range1 = 0, resp_expected;
    void    * p_ret;

    if (portid->lid <= 0) {
        return NULL; /* no direct SMI */
    }

    if (!(range1 = mad_is_vendor_range1(call->mgmt_class)) && !(mad_is_vendor_range2(call->mgmt_class))) {
        return NULL;
    }

    resp_expected = response_expected(call->method);

    rpc.mgtclass = call->mgmt_class | IB_MAD_RPC_VERSION1;
    rpc.method = call->method;
    rpc.attr.id = call->attrid;
    rpc.attr.mod = call->mod;
    rpc.timeout = resp_expected ? call->timeout : 0;
    rpc.datasz = range1 ? IB_VENDOR_RANGE1_DATA_SIZE : IB_VENDOR_RANGE2_DATA_SIZE;
    rpc.dataoffs = range1 ? IB_VENDOR_RANGE1_DATA_OFFS : IB_VENDOR_RANGE2_DATA_OFFS;

    if (!range1) {
        rpc.oui = call->oui;
    }

    portid->qp = 1;
    if (!portid->qkey) {
        portid->qkey = IB_DEFAULT_QP1_QKEY;
    }

    if (resp_expected) {
        p_ret = vsmad->mad_rpc_rmpp(srcport, rpcold, portid, NULL, data);
        errno = rpc.error;
        *return_status = rpc.rstatus;
        return p_ret;
    }

    return vsmad->mad_send_via(rpcold, portid, NULL, data, srcport) < 0 ? NULL : data;
}

/* AccessRegisterGMP does not use OperationTLV struct */
/* and encodes the register operation status in the */
/* returned status field of the MAD. */
/* The translation table is available in the MADs PRM. */
int translate_mad_status_to_reg_status_gmp(int mad_status)
{
    switch (mad_status) {
    case 0:
        return ME_OK;

    case 1:
        return ME_REG_ACCESS_DEV_BUSY;

    case 4:
        return ME_REG_ACCESS_CLASS_NOT_SUPP;

    case 20:
        return ME_REG_ACCESS_REG_NOT_SUPP;

    case 12:
        return ME_REG_ACCESS_METHOD_NOT_SUPP;

    case 28:
        return ME_REG_ACCESS_BAD_PARAM;

    default:
        return ME_REG_ACCESS_UNKNOWN_ERR;
    }
}

int mib_send_gmp_access_reg_mad(mfile              * mf,
                                u_int32_t          * data,
                                u_int32_t            reg_size,
                                u_int32_t            reg_id,
                                maccess_reg_method_t reg_method,
                                int                * reg_status)
{
    /* The buffer should contain: */
    /* 2 dwords for the vskey, */
    /* 1 dword for a configuration dword, */
    /* 55 dwords for data. */
    /* All sizes and offsets are in dword unit. */
    const int vskey_offset = 0;
    const int vskey_size = 2;
    const int config_offset = 2;
    const int config_size = 1;
    const int data_offset = 3;
    const int data_size = 55;
    const int buffer_size = vskey_size + config_size + data_size; /* == 58 */
    u_int32_t vsmad_data[buffer_size];
    u_int32_t orig_vsmad_data[buffer_size];

    if (!mf || !mf->ctx || !data) {
        IBERROR(("mib_send_gmp_access_reg_mad failed. Null Param."));
        return ME_BAD_PARAMS;
    }
    if (!mib_supports_reg_access_gmp(mf, reg_method)) {
        return ME_GMP_MAD_UNSUPPORTED_OPERATION;
    }

    memset(vsmad_data, 0x0, buffer_size);
    memset(orig_vsmad_data, 0x0, buffer_size);

    ibvs_mad* vsmad = (ibvs_mad*)(mf->ctx);
    u_int64_t vskey = __cpu_to_be64(vsmad->vskey);

    memcpy(&vsmad_data[vskey_offset], &vskey, vskey_size * 4);
    memcpy(&vsmad_data[data_offset], data, data_size * 4);
    memcpy(&orig_vsmad_data[0], vsmad_data, buffer_size * 4);

    ib_vendor_call_t call;
    u_int8_t       * call_result;

    call.method = reg_method;
    call.mgmt_class = IB_VENDOR_SPECIFIC_CLASS_0xA;
    call.attrid = IB_VS_ATTR_ACCESS_REGISTER_GMP;
    call.mod = reg_id;
    call.oui = IB_OPENIB_OUI;
    call.timeout = 0;
    memset(&call.rmpp, 0, sizeof(call.rmpp));

    int       num_of_blocks = (reg_size / (data_size * 4)) + ((reg_size % (data_size * 4)) ? 1 : 0);
    int       block_num;
    u_int32_t block_dword;
    int       return_status = -1;

    for (block_num = 0; block_num < num_of_blocks; block_num++) {
        block_dword =
            __cpu_to_be32(((u_int32_t)block_num) << 16); /* block offset in the config dword, see PRM for details */
        memcpy(&vsmad_data[config_offset], &block_dword, config_size * 4);
        call_result =
            ib_vendor_call_status_via(vsmad, vsmad_data, &vsmad->portid, &call, vsmad->srcport, &return_status);
        if (!call_result) {
            return -1;
        }

        if (return_status > 0) {
            *reg_status = translate_mad_status_to_reg_status_gmp(return_status);
        }
        int num_of_bytes = (block_num == num_of_blocks - 1) ? ((int)reg_size % (data_size * 4)) : (data_size * 4);
        memcpy(&data[(block_num * data_size)], &vsmad_data[data_offset], num_of_bytes);

        memcpy(vsmad_data, orig_vsmad_data, buffer_size * 4);
    }
    return ME_OK;
}

int mib_get_gmp(mfile* mf, unsigned attr_id, unsigned mod, u_int32_t* vsmad_data, size_t vsmad_data_len)
{
    if (!mf || !mf->ctx || !vsmad_data || (vsmad_data_len != IB_VENDOR_RANGE1_DATA_SIZE / 4)) {
        return ME_BAD_PARAMS;
    }

    ib_vendor_call_t call;
    ibvs_mad       * vs_mad = (ibvs_mad*)(mf->ctx);
    u_int8_t       * call_result;

    call.method = IB_MAD_METHOD_GET;
    call.mgmt_class = IB_VENDOR_SPECIFIC_CLASS_0xA;
    call.attrid = attr_id;
    call.mod = mod;
    call.oui = IB_OPENIB_OUI;
    call.timeout = 0;
    memset(&call.rmpp, 0, sizeof(call.rmpp));

    call_result = vs_mad->ib_vendor_call_via(vsmad_data, &vs_mad->portid, &call, vs_mad->srcport);
    if (!call_result) {
        return -1;
    }

    int i;

    for (i = 0; i < IB_VENDOR_RANGE1_DATA_SIZE / 4; i++) {
        vsmad_data[i] = __be32_to_cpu(vsmad_data[i]);
    }
    return ME_OK;
}

int mib_get_general_info_gmp(mfile* mf, u_int32_t* vsmad_data, size_t vsmad_data_len)
{
    return mib_get_gmp(mf, IB_VS_ATTR_GENERAL_INFO, 0, vsmad_data, vsmad_data_len);
}

int mib_supports_reg_access_gmp(mfile* mf, maccess_reg_method_t reg_method)
{
    if (!mf || !mf->ctx) {
        return 0;
    }

    if (!((mf->flags & MDEVS_IB) && (((ibvs_mad*)(mf->ctx))->dest_type == IB_DEST_LID) &&
          (reg_method == MACCESS_REG_METHOD_GET))) {
        return 0;
    }


    u_int32_t vsmad_data[IB_VENDOR_RANGE1_DATA_SIZE / 4] = {0};

    if (mib_get_general_info_gmp(mf, vsmad_data, IB_VENDOR_RANGE1_DATA_SIZE / 4)) {
        return 0;
    }

    /* vend_key 0x8 bytes, which present in any vendor specific MAD return in vsmad_data. * /
     *  / * We want to skip them when relating GeneralInfo MAD fields in specific. * /
     *  / * The capability_mask offset which is 0x80, is calculated from the beginning of GeneralInfo MAD fields. * /
     *  / * So overall we have an offset of 0x8+0x80 bytes. */
    const int capability_mask_byte_offset = 0x8 + 0x80;
    const int is_access_register_supported_bit_offset = 20;

    return EXTRACT(vsmad_data[capability_mask_byte_offset / 4], is_access_register_supported_bit_offset, 1);
}

int mib_acces_reg_mad(mfile* mf, u_int8_t* data)
{
    if (!mf || !mf->ctx || !data) {
        IBERROR(("mib_acces_reg_mad failed. Null Param."));
        return ME_BAD_PARAMS;
    }
    u_int8_t* p;
    ibvs_mad* h = (ibvs_mad*)(mf->ctx);
    int       status = -1;

    /* Set the MKey. */
    set_mkey_for_smp_mad(h);

    /* Call smp set function */
    if (h->smp_set_status_via) {
        p = h->smp_set_status_via(data, &(h->portid), IB_SMP_ATTR_REG_ACCESS, 0, 0, &status, h->srcport);
    } else {
        p = h->smp_set_via(data, &(h->portid), IB_SMP_ATTR_REG_ACCESS, 0, 0, h->srcport);
    }
    if (!p || (status > 0)) {
        if (status != -1) {
            return mib_status_translate(status);
        } else {
            return -1;
        }
    }

    return ME_OK;
}

int mib_smp_set(mfile* mf, u_int8_t* data, u_int16_t attr_id, u_int32_t attr_mod)
{
    if (!mf || !mf->ctx || !data) {
        IBERROR(("mib_smp_set failed. Null Param."));
        return ME_BAD_PARAMS;
    }
    u_int8_t* p;
    int       status = -1;
    ibvs_mad* h = (ibvs_mad*)(mf->ctx);

    if (!h->use_smp) {
        return ME_UNSUPPORTED_ACCESS_TYPE;
    }

    /* Set the MKey. */
    set_mkey_for_smp_mad(h);

    /* Call smp set function */
    if (h->smp_set_status_via) {
        p = h->smp_set_status_via(data, &(h->portid), attr_id, attr_mod, 0, &status, h->srcport);
    } else {
        p = h->smp_set_via(data, &(h->portid), attr_id, attr_mod, 0, h->srcport);
    }
    if (!p || (status > 0)) {
        if (status != -1) {
            return mib_status_translate(status);
        } else {
            return -1;
        }
    }

    return ME_OK;
}

int mib_smp_get(mfile* mf, u_int8_t* data, u_int16_t attr_id, u_int32_t attr_mod)
{
    if (!mf || !mf->ctx || !data) {
        IBERROR(("mib_smp_get failed. Null Param."));
        return ME_BAD_PARAMS;
    }
    u_int8_t* p;
    int       status = -1;
    ibvs_mad* h = (ibvs_mad*)(mf->ctx);

    if (!h->use_smp) {
        return ME_UNSUPPORTED_ACCESS_TYPE;
    }

    /* Set the MKey. */
    set_mkey_for_smp_mad(h);

    /* Call smp set function */
    if (h->smp_query_status_via) {
        p = h->smp_query_status_via(data, &(h->portid), attr_id, attr_mod, 0, &status, h->srcport);
    } else {
        p = h->smp_query_via(data, &(h->portid), attr_id, attr_mod, 0, h->srcport);
    }

    if (!p || (status > 0)) {
        if (status != -1) {
            return mib_status_translate(status);
        } else {
            return -1;
        }
    }

    return ME_OK;
}

int mib_semaphore_lock_smp(mfile* mf, u_int8_t* data, sem_lock_method_t method)
{
    if (!mf || !mf->ctx || !data) {
        IBERROR(("mib_semaphore_lock_smp failed. Null Param."));
        return ME_BAD_PARAMS;
    }
    u_int8_t* smp_result;
    int       status = -1;
    ibvs_mad* vs_mad = (ibvs_mad*)(mf->ctx);

    if (!vs_mad->use_smp) {
        return ME_UNSUPPORTED_ACCESS_TYPE;
    }

    /* Setting MKey for this attribute caused MAD failures */
    /* and was disabled. */

    if (method == SEM_LOCK_SET) {
        if (vs_mad->smp_set_status_via) {
            smp_result = vs_mad->smp_set_status_via(data, &(vs_mad->portid), SMP_SEMAPHOE_LOCK_CMD, 0, 0, &status,
                                                    vs_mad->srcport);
        } else {
            smp_result = vs_mad->smp_set_via(data, &(vs_mad->portid), SMP_SEMAPHOE_LOCK_CMD, 0, 0, vs_mad->srcport);
        }
    } else {
        if (vs_mad->smp_query_status_via) {
            smp_result = vs_mad->smp_query_status_via(data, &(vs_mad->portid), SMP_SEMAPHOE_LOCK_CMD, 0, 0, &status,
                                                      vs_mad->srcport);
        } else {
            smp_result = vs_mad->smp_query_via(data, &(vs_mad->portid), SMP_SEMAPHOE_LOCK_CMD, 0, 0, vs_mad->srcport);
        }
    }

    if (!smp_result || (status > 0)) {
        if (status != -1) {
            return mib_status_translate(status);
        } else {
            return -1;
        }
    }

    return ME_OK;
}

int mib_supports_reg_access_cls_a(mfile* mf, maccess_reg_method_t reg_method)
{
    if (!mf || !mf->ctx) {
        return 0;
    }

    if (!((mf->flags & MDEVS_IB) && (((ibvs_mad*)(mf->ctx))->dest_type == IB_DEST_LID) &&
          ((reg_method == MACCESS_REG_METHOD_SET) || (reg_method == MACCESS_REG_METHOD_GET)))) {
        return 0;
    }

    return 1;

    u_int32_t vsmad_data[IB_VENDOR_RANGE1_DATA_SIZE / 4] = {0};

    if (mib_get_general_info_gmp(mf, vsmad_data, IB_VENDOR_RANGE1_DATA_SIZE / 4)) {
        return 0;
    }

    /* vend_key 0x8 bytes, which present in any vendor specific MAD return in vsmad_data. * /
     *  / * We want to skip them when relating GeneralInfo MAD fields in specific. * /
     *  / * The capability_mask offset which is 0x80, is calculated from the beginning of GeneralInfo MAD fields. * /
     *  / * So overall we have an offset of 0x8+0x80 bytes. */
    const int capability_mask_byte_offset = 0x8 + 0x80;
    const int is_access_register_supported_bit_offset = 20;
    const int is_supported =
        EXTRACT(vsmad_data[capability_mask_byte_offset / 4], is_access_register_supported_bit_offset, 1);

    return is_supported;
}


int mib_supports_reg_access_smp(mfile* mf)
{
    return ((mf->tp == MST_IB && ((ibvs_mad*)mf->ctx)->use_smp) && (mf->flags & (MDEVS_IB | MDEVS_FWCTX))) ||
           (!(mf->flags & MDEVS_IB));
}
