/**
* Copyright (C) Mellanox Technologies Ltd. 2001-2011.  ALL RIGHTS RESERVED.
* This software product is a proprietary product of Mellanox Technologies Ltd.
* (the "Company") and all right, title, and interest and to the software product,
* including all associated intellectual property rights, are and shall
* remain exclusively with the Company.
*
* This software product is governed by the End User License Agreement
* provided with the software product.
* $COPYRIGHT$
* $HEADER$
*/

#include "libperf.h"

#include <malloc.h>
#include <unistd.h>

#define TIMING_QUEUE_SIZE   2048
#define MXM_PERF_TEST_SEED  42
#define MXM_PERF_TEST_TAG   1

/* Branch prediction */
#define mxm_likely(x)              __builtin_expect(x, 1)
#define mxm_unlikely(x)            __builtin_expect(x, 0)

enum {
    MXM_TEST_HID_QUIT,
    MXM_TEST_HID_PINGPONG,
    MXM_TEST_HID_NOP
};

typedef struct mxm_perf_context {
    mxm_perf_test_t              *test;
    mxm_h                        mxmh;
    mxm_ep_h                     ep;
    mxm_mq_h                     mq;
    mxm_conn_h                   *conns;
    unsigned                     conn_cnt;
    mxm_send_req_t               *sreq_window;
    unsigned                     sreq_window_size;
    unsigned                     sreq_head, sreq_outstanding;
    mxm_perf_report_cb_t         report_cb;
    void                         *user_data;
    mxm_time_t                   report_interval;
    void                         *data_buffer;
    mxm_mem_key_t                mkey;
    int                          quit;
    union {
        unsigned                 cycle_rr_ctx;
        unsigned                 rand_ctx;
    };

    mxm_time_t                   start_time;
    mxm_time_t                   end_time;
    mxm_time_t                   prev_time;
    mxm_perf_counter_t           max_iters;

    struct {
        mxm_perf_counter_t       msgs;
        mxm_perf_counter_t       bytes;
        mxm_perf_counter_t       iters;
        mxm_time_t               time;
    } current, prev;

    mxm_time_t                   timing_queue[TIMING_QUEUE_SIZE];
    unsigned                     timing_queue_head;
    mxm_send_req_t               sreq;
} mxm_perf_context_t;



#define MXM_PERF_TEST_FOREACH(perf) \
    while (!mxm_perf_context_done(perf))


/*
 *  This Quickselect routine is based on the algorithm described in
 *  "Numerical recipes in C", Second Edition,
 *  Cambridge University Press, 1992, Section 8.5, ISBN 0-521-43108-5
 *  This code by Nicolas Devillard - 1998. Public domain.
 */

#define ELEM_SWAP(a,b) { register mxm_time_t t=(a);(a)=(b);(b)=t; }

static mxm_time_t __find_median_quick_select(mxm_time_t arr[], int n)
{
    int low, high ;
    int median;
    int middle, ll, hh;

    low = 0 ; high = n-1 ; median = (low + high) / 2;
    for (;;) {
        if (high <= low) /* One element only */
            return arr[median] ;

        if (high == low + 1) {  /* Two elements only */
            if (arr[low] > arr[high])
                ELEM_SWAP(arr[low], arr[high]) ;
            return arr[median] ;
        }

        /* Find median of low, middle and high items; swap into position low */
        middle = (low + high) / 2;
        if (arr[middle] > arr[high])    ELEM_SWAP(arr[middle], arr[high]) ;
        if (arr[low] > arr[high])       ELEM_SWAP(arr[low], arr[high]) ;
        if (arr[middle] > arr[low])     ELEM_SWAP(arr[middle], arr[low]) ;

        /* Swap low item (now in position middle) into position (low+1) */
        ELEM_SWAP(arr[middle], arr[low+1]) ;

        /* Nibble from each end towards middle, swapping items when stuck */
        ll = low + 1;
        hh = high;
        for (;;) {
            do ll++; while (arr[low] > arr[ll]) ;
            do hh--; while (arr[hh]  > arr[low]) ;

            if (hh < ll)
            break;

            ELEM_SWAP(arr[ll], arr[hh]) ;
        }

        /* Swap middle item (in position low) back into correct position */
        ELEM_SWAP(arr[low], arr[hh]) ;

        /* Re-set active partition */
        if (hh <= median) {
            low = ll;
        }

        if (hh >= median) {
            high = hh - 1;
        }
    }
}

static void mxm_perf_context_init(mxm_perf_context_t *perf)
{
    unsigned i;

    perf->start_time = mxm_get_time();
    perf->prev_time = perf->start_time;
    perf->end_time = (perf->test->max_time == 0.0) ? UINT64_MAX :
                      mxm_time_from_sec(perf->test->max_time) + perf->start_time;
    perf->max_iters = (perf->test->max_iters == 0) ? UINT64_MAX :
                      perf->test->max_iters;

    perf->quit = 0;
    perf->current.time = 0;
    perf->current.msgs = 0;
    perf->current.bytes = 0;
    perf->current.iters = 0;
    perf->prev.time = perf->start_time;
    perf->prev.msgs = 0;
    perf->prev.bytes = 0;
    perf->prev.iters = 0;
    perf->timing_queue_head = 0;
    for (i = 0; i < TIMING_QUEUE_SIZE; ++i) {
        perf->timing_queue[i] = 0;
    }

    switch (perf->test->cycle)
    {
    case MXM_PERF_TEST_CYCLE_RR:
        perf->cycle_rr_ctx = 0;
        break;
    case MXM_PERF_TEST_CYCLE_UNIFORM:
    case MXM_PERF_TEST_CYCLE_NONUNIFORM:
        perf->rand_ctx = MXM_PERF_TEST_SEED;
        break;
    }
}

static inline int mxm_perf_context_done(mxm_perf_context_t *perf) {
    return
        mxm_unlikely((perf->current.iters > perf->max_iters) ||
                     (perf->current.time  > perf->end_time));
}

static void mxm_perf_calc_result(mxm_perf_context_t *perf, mxm_perf_result_t *result)
{
    double mxm_sec_value;

    mxm_sec_value = mxm_time_from_sec(1.0);

    result->iters = perf->current.iters;
    result->bytes = perf->current.bytes;
    result->elapsed_time = perf->current.time - perf->start_time;

    /* Latency */

    result->latency.typical =
        __find_median_quick_select(perf->timing_queue, TIMING_QUEUE_SIZE) / mxm_sec_value;

    result->latency.moment_average =
        (double)(perf->current.time - perf->prev.time)
        / (perf->current.iters - perf->prev.iters)
        / mxm_sec_value;

    result->latency.total_average =
        (double)(perf->current.time - perf->start_time)
        / perf->current.iters
        / mxm_sec_value;


    /* Bandwidth */

    result->bandwidth.typical = 0.0; // Undefined

    result->bandwidth.moment_average =
        (perf->current.bytes - perf->prev.bytes) * mxm_sec_value
        / (double)(perf->current.time - perf->prev.time);

    result->bandwidth.total_average =
        perf->current.bytes * mxm_sec_value
        / (double)(perf->current.time - perf->start_time);


    /* Packet rate */

    result->msgrate.typical = 0.0; // Undefined

    result->msgrate.moment_average =
        (perf->current.msgs - perf->prev.msgs) * mxm_sec_value
        / (double)(perf->current.time - perf->prev.time);

    result->msgrate.total_average =
        perf->current.msgs * mxm_sec_value
        / (double)(perf->current.time - perf->start_time);

}

static inline void mxm_perf_update(mxm_perf_context_t *perf, mxm_perf_counter_t iters,
                                   size_t bytes)
{
    mxm_perf_result_t result;

    perf->current.time   = mxm_get_time();
    perf->current.iters += iters;
    perf->current.bytes += bytes;
    perf->current.msgs  += 1;

    perf->timing_queue[perf->timing_queue_head++] = perf->current.time - perf->prev_time;
    perf->timing_queue_head %= TIMING_QUEUE_SIZE;
    perf->prev_time = perf->current.time;

    if ((perf->current.time - perf->prev.time >= perf->report_interval) &&
        (perf->report_cb != NULL))
    {
        mxm_perf_calc_result(perf, &result);
        perf->report_cb(&result, perf->user_data);
        perf->prev = perf->current;
    }
}

static inline void req_wait_and_print_error(mxm_req_base_t *req, const char *desc)
{
    mxm_req_wait(req);
    if (req->error != MXM_OK) {
        fprintf(stderr, "%s failed: %s\n", desc, mxm_error_string(req->error));
        exit(-1);
    }
}

static size_t __stream_recv(void *buffer, size_t length, size_t offset, void *context)
{
    return length;
}

static size_t __stream_send(void *buffer, size_t length, size_t offset, void *context)
{
    mxm_perf_context_t *perf = context;
    memcpy(buffer, (char*)perf->data_buffer + offset, length);
    return length;
}

static mxm_conn_h get_next_conn(mxm_perf_context_t *perf)
{
    unsigned index;
    double r;

    switch (perf->test->cycle)
    {
    case MXM_PERF_TEST_CYCLE_RR:
        if (++perf->cycle_rr_ctx == perf->conn_cnt) {
            perf->cycle_rr_ctx = 0;
        }
        index = perf->cycle_rr_ctx;
        break;
    case MXM_PERF_TEST_CYCLE_UNIFORM:
        index = rand_r(&perf->rand_ctx) % perf->conn_cnt;
        break;
    case MXM_PERF_TEST_CYCLE_NONUNIFORM:
        r = rand_r(&perf->rand_ctx) / ((double)RAND_MAX);
        index = (int)( r * r * r * perf->conn_cnt );
        break;
    default:
        return NULL;
    }
    return perf->conns[index];
}

static void mxm_perf_init_req(mxm_perf_context_t *perf, mxm_req_base_t *req, int recv)
{
    size_t offset, iovsize, remainder;
    unsigned i;

    /* Initialize request fields */
    req->state        = MXM_REQ_NEW;
    req->mq           = perf->mq;
    req->conn         = NULL; // Later replaced where relevant
    req->completed_cb = NULL;
    req->data_type    = perf->test->data_type;
    req->error        = MXM_OK;

    /* Initialize data */
    switch (req->data_type) {
    case MXM_REQ_DATA_BUFFER:
        req->data.buffer.ptr    = perf->data_buffer;
        req->data.buffer.length = perf->test->message_size;
        break;
    case MXM_REQ_DATA_IOV:
        req->data.iov.count = perf->test->iov_count;
        req->data.iov.vector = malloc(perf->test->iov_count * sizeof(mxm_req_buffer_t));
        iovsize = perf->test->message_size / perf->test->iov_count;
        remainder = perf->test->message_size % perf->test->iov_count;
        offset = 0;
        for (i = 0; i < req->data.iov.count; ++i) {
            req->data.iov.vector[i].ptr = (char*)perf->data_buffer + offset;
            req->data.iov.vector[i].length = iovsize;
            if (remainder > 0) {
                --remainder;
                ++req->data.iov.vector[i].length;
            }
            offset += req->data.iov.vector[i].length;
        }
        assert(offset == perf->test->message_size);
        break;

    case MXM_REQ_DATA_STREAM:
        req->context = perf;
        if (recv) {
            req->data.stream.cb = __stream_recv;
        } else {
            req->data.stream.cb = __stream_send;
        }
        req->data.stream.length = perf->test->message_size;
        break;
    }
}

static void mxm_perf_init_recv_req(mxm_perf_context_t *perf, mxm_recv_req_t *rreq)
{
    mxm_perf_init_req(perf, &rreq->base, 1);
    rreq->tag      = MXM_PERF_TEST_TAG;
    rreq->tag_mask = -1;
}

static mxm_error_t mxm_perf_init_send_req(mxm_perf_context_t *perf, mxm_send_req_t *sreq)
{
    mxm_perf_init_req(perf, &sreq->base, 0);
    sreq->flags   = 0;

    if (perf->test->flags & MXM_PERF_TEST_FLAG_REQ_WAIT) {
        sreq->flags |= MXM_REQ_SEND_FLAG_BLOCKING;
    }
    if (perf->test->flags & MXM_PERF_TEST_FLAG_REQ_LAZY) {
        sreq->flags |= MXM_REQ_SEND_FLAG_LAZY;
    }

    switch (perf->test->command) {
    case MXM_PERF_CMD_SEND:
    case MXM_PERF_CMD_SEND_RECV:
    case MXM_PERF_CMD_SEND_PINGPONG:
        sreq->opcode = (perf->test->flags & MXM_PERF_TEST_FLAG_REQ_SYNC) ?
                        MXM_REQ_OP_SEND_SYNC :
                        MXM_REQ_OP_SEND;
        sreq->op.send.tag = MXM_PERF_TEST_TAG;
        break;
    case MXM_PERF_CMD_PUT:
    case MXM_PERF_CMD_PUT_PINGPONG:
        sreq->opcode = (perf->test->flags & MXM_PERF_TEST_FLAG_REQ_SYNC) ?
                        MXM_REQ_OP_PUT_SYNC :
                        MXM_REQ_OP_PUT;
        sreq->op.mem.remote_vaddr = perf->test->remote_addr;
        sreq->op.mem.remote_mkey  = &perf->test->remote_mkey;
        break;
    case MXM_PERF_CMD_GET:
        sreq->opcode = MXM_REQ_OP_GET;
        sreq->op.mem.remote_vaddr = perf->test->remote_addr;
        sreq->op.mem.remote_mkey  = &perf->test->remote_mkey;
        break;
    case MXM_PERF_CMD_AM:
    case MXM_PERF_CMD_AM_PINGPONG:
        sreq->opcode = MXM_REQ_OP_AM;
        sreq->op.am.hid      = MXM_TEST_HID_NOP;
        sreq->op.am.imm_data = 0;
        break;
    default:
        return MXM_ERR_INVALID_PARAM;
    }

    return MXM_OK;
}



/*
 * Wait until "count" sreqs are free
 */
static mxm_error_t mxm_perf_wait_for_sreqs(mxm_perf_context_t *perf, unsigned count)
{
    unsigned window_size = perf->sreq_window_size;
    mxm_send_req_t *sreq;
    unsigned tail;

    while (window_size - perf->sreq_outstanding < count) {
        tail = perf->sreq_head + window_size - perf->sreq_outstanding;
        if (tail >= window_size) {
            tail -= window_size;
        }
        sreq = &perf->sreq_window[tail];
        mxm_req_wait(&sreq->base);
        if (sreq->base.error != MXM_OK) {
            return sreq->base.error;
        }
        --perf->sreq_outstanding;
    }
    return MXM_OK;
}

static mxm_error_t mxm_perf_alloc_sreq(mxm_perf_context_t *perf, mxm_send_req_t **sreqp)
{
    mxm_send_req_t *sreq;
    mxm_error_t error;

    error = mxm_perf_wait_for_sreqs(perf, 1);
    if (error != MXM_OK) {
        return error;
    }

    sreq = &perf->sreq_window[perf->sreq_head];
    perf->sreq_head = (perf->sreq_head + 1) % perf->sreq_window_size;
    ++perf->sreq_outstanding;
    assert(perf->sreq_outstanding <= perf->sreq_window_size);
    *sreqp = sreq;
    return MXM_OK;
}

static mxm_error_t mxm_perf_wait_all_sreqs(mxm_perf_context_t *perf)
{
    mxm_send_req_t fence_req;
    unsigned index;

    fence_req.base.mq                  = perf->mq;
    fence_req.base.completed_cb        = NULL;
    fence_req.base.data_type           = MXM_REQ_DATA_BUFFER;
    fence_req.base.data.buffer.ptr     = NULL;
    fence_req.base.data.buffer.length  = 0;
    fence_req.opcode                   = MXM_REQ_OP_PUT_SYNC;
    fence_req.flags                    = MXM_REQ_SEND_FLAG_FENCE;
    fence_req.op.mem.remote_mkey       = &mxm_empty_mem_key;

    for (index = 0; index < perf->conn_cnt; index++) {
        fence_req.base.state = MXM_REQ_NEW;
        fence_req.base.conn  = perf->conns[index];

        mxm_req_send(&fence_req);
        mxm_req_wait(&fence_req.base);
        if (fence_req.base.error != MXM_OK) {
            return fence_req.base.error;
        }
    }

    return mxm_perf_wait_for_sreqs(perf, perf->sreq_window_size);
}

static void mxm_perf_am_handler_quit(mxm_conn_h conn, mxm_imm_t imm, void *data, size_t len, size_t offset, int is_lf)
{
    mxm_perf_context_t *perf = mxm_conn_ctx_get(conn);

    perf->quit = 1;
}

static mxm_error_t mxm_perf_send_quit_am(mxm_perf_context_t *perf)
{
    mxm_send_req_t sreq;

    sreq.base.mq                  = perf->mq;
    sreq.base.completed_cb        = NULL;
    sreq.base.data_type           = MXM_REQ_DATA_BUFFER;
    sreq.base.data.buffer.ptr     = NULL;
    sreq.base.data.buffer.length  = 0;
    sreq.opcode                   = MXM_REQ_OP_AM;
    sreq.flags                    = 0;
    sreq.op.am.hid                = MXM_TEST_HID_QUIT;
    sreq.op.am.imm_data           = 0;
    sreq.base.state               = MXM_REQ_NEW;
    sreq.base.conn                = perf->conns[0];

    mxm_req_send(&sreq);
    mxm_req_wait(&sreq.base);
    return sreq.base.error;
}

static mxm_error_t mxm_perf_init_send_reqs(mxm_perf_context_t *perf)
{
    mxm_send_req_t *sreq;
    mxm_error_t error;

    for (sreq = perf->sreq_window; sreq < perf->sreq_window + perf->sreq_window_size; ++sreq) {
        error = mxm_perf_init_send_req(perf, sreq);
        if (error != MXM_OK) {
            return error;
        }
    }

    return MXM_OK;
}

static void __post_recv(mxm_perf_context_t *perf, mxm_recv_req_t *rreq)
{
    if (perf->test->flags & MXM_PERF_TEST_FLAG_RECV_CONN) {
        rreq->base.conn = get_next_conn(perf);
    }
    rreq->base.state = MXM_REQ_NEW;
    mxm_req_recv(rreq);
}

static void __post_send(mxm_perf_context_t *perf, mxm_send_req_t *sreq)
{
    sreq->base.state = MXM_REQ_NEW;
    sreq->base.conn  = get_next_conn(perf);
    mxm_req_send(sreq);
}

static mxm_error_t mxm_perf_run_send_loop(mxm_perf_context_t *perf)
{
    mxm_send_req_t *sreq;
    mxm_error_t error;

    error = mxm_perf_init_send_reqs(perf);
    if (error != MXM_OK) {
        return error;
    }

    MXM_PERF_TEST_FOREACH(perf) {
        error = mxm_perf_alloc_sreq(perf, &sreq);
        if (error != MXM_OK) {
            return error;
        }

        __post_send(perf, sreq);
        mxm_perf_update(perf, 1, perf->test->message_size);
    }

    error = mxm_perf_wait_all_sreqs(perf);
    if (error != MXM_OK) {
        return error;
    }

    return mxm_perf_send_quit_am(perf);
}

static mxm_error_t mxm_perf_run_pingpong(mxm_perf_context_t *perf)
{
    int send_first = (perf->test->index % 2 == 0);
    mxm_send_req_t sreq;
    mxm_recv_req_t rreq;

    mxm_perf_init_send_req(perf, &sreq);
    mxm_perf_init_recv_req(perf, &rreq);

    MXM_PERF_TEST_FOREACH(perf) {
        rreq.base.state = MXM_REQ_NEW;
        sreq.base.state = MXM_REQ_NEW;

        sreq.base.conn = get_next_conn(perf);
        if (perf->test->flags & MXM_PERF_TEST_FLAG_RECV_CONN) {
            rreq.base.conn = sreq.base.conn;
        }

        if (send_first) {
            mxm_req_send(&sreq);
            mxm_req_wait(&sreq.base);
            if (sreq.base.error != MXM_OK) {
                return sreq.base.error;
            }

            mxm_req_recv(&rreq);
            mxm_req_wait(&rreq.base);
            if (rreq.base.error != MXM_OK) {
                return rreq.base.error;
            }
        } else {
            mxm_req_recv(&rreq);
            mxm_req_wait(&rreq.base);
            if (rreq.base.error != MXM_OK) {
                return rreq.base.error;
            }

            mxm_req_send(&sreq);
            mxm_req_wait(&sreq.base);
            if (sreq.base.error != MXM_OK) {
                return sreq.base.error;
            }
       }

        mxm_perf_update(perf, 1, perf->test->message_size * 2);
    }

    return MXM_OK;
}

static mxm_error_t mxm_perf_run_recv(mxm_perf_context_t *perf)
{
    mxm_recv_req_t rreq;

    mxm_perf_init_recv_req(perf, &rreq);

    MXM_PERF_TEST_FOREACH(perf) {
        __post_recv(perf, &rreq);
        mxm_req_wait(&rreq.base);
        if (rreq.base.error != MXM_OK) {
            return rreq.base.error;
        }
        mxm_perf_update(perf, 1, rreq.completion.actual_len);
    }

    while (!perf->quit) {
        mxm_progress(perf->mxmh);
    }

    return MXM_OK;
}

mxm_error_t mxm_perf_run_send_recv(mxm_perf_context_t *perf)
{
    const long max_imbalance = 8192;
    mxm_send_req_t *sreq_head, *sreq_tail;
    mxm_recv_req_t rreq;
    mxm_error_t error;
    unsigned window_size;
    unsigned head, tail, outstanding;
    mxm_message_h msg;
    long imbalance;

    error = mxm_perf_init_send_reqs(perf);
    if (error != MXM_OK) {
        return error;
    }

    mxm_perf_init_recv_req(perf, &rreq);

    sreq_head   = &perf->sreq_window[0];
    sreq_tail   = &perf->sreq_window[0];
    head        = 0;
    tail        = 0;
    outstanding = 0;
    imbalance   = 0;
    window_size = perf->sreq_window_size;

    MXM_PERF_TEST_FOREACH(perf) {
        if (rreq.base.state & (MXM_REQ_COMPLETED|MXM_REQ_NEW)) {
            if (rreq.base.state & MXM_REQ_COMPLETED) {
                mxm_perf_update(perf, 1, rreq.completion.actual_len);
            }
            --imbalance;
            __post_recv(perf, &rreq);
        } else if ((outstanding < window_size) && (imbalance < max_imbalance)) {
            __post_send(perf, sreq_head);
            ++imbalance;
            mxm_perf_update(perf, 0, perf->test->message_size);

            if (++head == window_size) {
                head = 0;
            }
            sreq_head = &perf->sreq_window[head];
            ++outstanding;
        } else if (sreq_tail->base.state == MXM_REQ_COMPLETED && (outstanding > 0)) {
            if (sreq_tail->base.error != MXM_OK) {
                return sreq_tail->base.error;
            }

            if (++tail == window_size) {
                tail = 0;
            }
            sreq_tail = &perf->sreq_window[tail];
            --outstanding;
        } else {
            mxm_progress(perf->mxmh);
            if (perf->quit) {
                break;
            }
        }
    }

    mxm_req_cancel_recv(&rreq);
    mxm_perf_send_quit_am(perf);

    perf->sreq_outstanding = outstanding;
    perf->sreq_head        = head;

    mxm_perf_wait_all_sreqs(perf);
    mxm_req_wait(&rreq.base);

    while (!perf->quit) {
        rreq.base.conn  = NULL;
        rreq.base.state = MXM_REQ_NEW;
        if (mxm_req_mprobe(&rreq, &msg) == MXM_OK) {
            mxm_message_release(msg);
        }
        mxm_progress(perf->mxmh);
    }
    return MXM_OK;
}

static mxm_error_t mxm_perf_run_put_lat(mxm_perf_context_t *perf)
{
    int type = perf->test->index % 2;
    mxm_send_req_t sreq;
    uint32_t *send_id, *recv_id, prev_id;
    mxm_error_t error;

    error = mxm_perf_init_send_req(perf, &sreq);
    if (error != MXM_OK) {
        return error;
    }

    // Point id to the end of memory buffer
    recv_id = (uint32_t*)((char*)perf->test->local_addr + perf->test->message_size - sizeof(uint32_t));
    send_id = (uint32_t*)((char*)perf->data_buffer + perf->test->message_size - sizeof(uint32_t));
    *send_id = *recv_id = prev_id = 0;

    MXM_PERF_TEST_FOREACH(perf) {
        if (type == 0) {
            while (*recv_id == prev_id) {
                mxm_progress(perf->mxmh);
            }
        }
        sreq.base.state = MXM_REQ_NEW;
        sreq.base.conn = get_next_conn(perf);
        (*send_id) ++;
        prev_id = *recv_id;
        mxm_req_send(&sreq);
        mxm_req_wait(&sreq.base);
        if (sreq.base.error != MXM_OK) {
            return sreq.base.error;
        }

        if (type == 1) {
            while (*recv_id == prev_id) {
                mxm_progress(perf->mxmh);
            }
        }
        mxm_perf_update(perf, 1, perf->test->message_size * 2);
    }

    return MXM_OK;
}

static void mxm_perf_empty_am_handler(mxm_conn_h conn, mxm_imm_t imm, void *data, size_t len, size_t off, int is_lf)
{
    /* Do nothing */
}


static void __am_lat_handler(mxm_conn_h conn, mxm_imm_t imm, void *data, size_t len, size_t off, int is_lf)
{
    mxm_perf_context_t *perf = (mxm_perf_context_t *)mxm_conn_ctx_get(conn);

    if (mxm_perf_context_done(perf)) {
        return;
    }

    if (!is_lf)
        return;

    perf->sreq.base.state = MXM_REQ_NEW;
    perf->sreq.base.conn = get_next_conn(perf);
    mxm_req_send(&perf->sreq);
    mxm_req_wait(&perf->sreq.base);
    mxm_perf_update(perf, 1, perf->test->message_size);
}

static mxm_error_t mxm_perf_run_am_lat(mxm_perf_context_t *perf)
{
    mxm_perf_init_req(perf, &perf->sreq.base, 0);
    perf->sreq.base.error = MXM_OK;
    perf->sreq.base.conn = get_next_conn(perf);
    perf->sreq.opcode = MXM_REQ_OP_AM;
    perf->sreq.op.am.hid = MXM_TEST_HID_PINGPONG;
    perf->sreq.op.am.imm_data = 0;
    perf->current.iters += 1 - perf->test->index;

    mxm_set_am_handler(perf->mxmh, MXM_TEST_HID_PINGPONG, __am_lat_handler, 0);

    if (perf->test->index > 0) {
        mxm_req_send(&perf->sreq);
        mxm_req_wait(&perf->sreq.base);
        mxm_perf_update(perf, 1, perf->test->message_size);
    }

    while (!mxm_perf_context_done(perf) && perf->sreq.base.error == MXM_OK) {
        mxm_progress(perf->mxmh);
    }

    mxm_set_am_handler(perf->mxmh, MXM_TEST_HID_PINGPONG, NULL, 0);

    return perf->sreq.base.error;
}

static mxm_error_t mxm_perf_run_progress(mxm_perf_context_t *perf)
{
    mxm_set_am_handler(perf->mxmh, MXM_TEST_HID_NOP, mxm_perf_empty_am_handler, MXM_AM_FLAG_THREAD_SAFE);

    if (perf->test->local_addr) {
        *((char*)perf->test->local_addr) = 0xAE;
    }

    while (!perf->quit) {
        mxm_progress(perf->mxmh);
    }

    mxm_set_am_handler(perf->mxmh, MXM_TEST_HID_NOP, NULL, 0);
    return MXM_OK;
}

mxm_error_t run_test(mxm_perf_context_t *perf)
{
    mxm_error_t error;
    unsigned index;

    for (index = 0; index < perf->conn_cnt; index++) {
        mxm_conn_ctx_set(perf->conns[index], perf);
    }
    mxm_set_am_handler(perf->mxmh, MXM_TEST_HID_QUIT, mxm_perf_am_handler_quit, 0);

    switch (perf->test->command) {
    case MXM_PERF_CMD_SEND:
    case MXM_PERF_CMD_PUT:
    case MXM_PERF_CMD_GET:
    case MXM_PERF_CMD_AM:
        error = mxm_perf_run_send_loop(perf);
        break;
    case MXM_PERF_CMD_RECV:
        error = mxm_perf_run_recv(perf);
        break;
    case MXM_PERF_CMD_SEND_RECV:
        error = mxm_perf_run_send_recv(perf);
        break;
    case MXM_PERF_CMD_SEND_PINGPONG:
        error = mxm_perf_run_pingpong(perf);
        break;
    case MXM_PERF_CMD_PUT_PINGPONG:
        if (perf->test->message_size < sizeof(uint32_t)) {
            perf->test->message_size = sizeof(uint32_t);
        }
        error = mxm_perf_run_put_lat(perf);
        break;
    case MXM_PERF_CMD_AM_PINGPONG:
        error = mxm_perf_run_am_lat(perf);
        break;
    case MXM_PERF_CMD_PROGRESS:
        error = mxm_perf_run_progress(perf);
        break;
    default:
        error = MXM_ERR_INVALID_PARAM;
        break;
    }

    mxm_set_am_handler(perf->mxmh, MXM_TEST_HID_QUIT, NULL, 0);
    return error;
}

mxm_error_t
mxm_perf_test_run(mxm_conn_h *conns, unsigned conn_cnt, mxm_mq_h mq,
                  mxm_ep_h ep, mxm_h mxmh, mxm_perf_test_t *test,
                  mxm_perf_report_cb_t report_cb, void *user_data,
                  double report_interval, mxm_perf_result_t *result)
{
    mxm_perf_context_t perf;
    mxm_error_t error;
    long page_size;

    perf.mxmh     = mxmh;
    perf.ep       = ep;
    perf.mq       = mq;
    perf.conns    = conns;
    perf.conn_cnt = conn_cnt;

    /* Allocate and register data buffer */
    page_size = sysconf(_SC_PAGESIZE);
    if (page_size < 0) {
        fprintf(stderr, "Failed to get system page size: %m\n");
        return MXM_ERR_INVALID_PARAM;
    }

    perf.data_buffer = memalign(page_size, test->message_size);
    if (perf.data_buffer == NULL) {
        fprintf(stderr, "Failed to allocate test buffer of size %Zu alignment %ld\n",
                test->message_size, page_size);
        return MXM_ERR_NO_MEMORY;
    }

    error = MXM_OK;
    if (test->flags & MXM_PERF_TEST_FLAG_REG_MEM) {
        error = mxm_mem_map(perf.mxmh, &perf.data_buffer, &test->message_size,
                            0, NULL, 0);
        if (error != MXM_OK) {
            return error;
        }

        error = mxm_mem_get_key(perf.mxmh, perf.data_buffer, &perf.mkey);
        if (error != MXM_OK) {
            return error;
        }
    } else {
        // TODO
    }

    /* Allocate send request window */
    perf.sreq_window_size = test->window;
    perf.sreq_window = calloc(perf.sreq_window_size, sizeof(mxm_send_req_t));
    perf.sreq_head = 0;
    perf.sreq_outstanding = 0;

    /* Touch memory */
    if (test->local_addr) {
        *((char*)test->local_addr) = 0xFC;
    }

    /* Run warm-up cycles */
    if (test->flags & MXM_PERF_TEST_FLAG_WARMUP) {
        mxm_perf_test_t warmup_test;

        perf.test = &warmup_test;
        warmup_test = *test;
        warmup_test.max_iters = 100;
        mxm_perf_context_init(&perf);
        perf.report_cb = NULL;
        perf.report_interval = mxm_time_from_sec(1);
        run_test(&perf);
    }

    perf.test = test;
    perf.report_cb = report_cb;
    perf.user_data = user_data;
    perf.report_interval = mxm_time_from_sec(report_interval);
    mxm_perf_context_init(&perf);

    /* Run actual test */
    error = run_test(&perf);
    if (error == MXM_OK) {
        mxm_perf_calc_result(&perf, result);
    }

    if (test->flags & MXM_PERF_TEST_FLAG_REG_MEM) {
        mxm_mem_unmap(perf.mxmh, perf.data_buffer, test->message_size, 0);
    }

    /* Release data buffer */
    free(perf.sreq_window);
    free(perf.data_buffer);

    return error;
}

double mxm_perf_get_cpu_freq()
{
    double mhz = 0.0;
    double m;
    int rc;
    FILE* f;
    char buf[256];
    int warn;

    f = fopen("/proc/cpuinfo","r");
    if (!f) {
        return 0.0;
    }

    warn = 0;
    while (fgets(buf, sizeof(buf), f)) {

#if defined (__ia64__)
        /* Use the ITC frequency on IA64 */
        rc = sscanf(buf, "itc MHz : %lf", &m);
#elif defined (__PPC__) || defined (__PPC64__)
        /* PPC has a different format as well */
        rc = sscanf(buf, "timebase : %lf", &m);
#else
        rc = sscanf(buf, "cpu MHz : %lf", &m);
#endif
        if (rc != 1) {
            continue;
        }

        if (mhz == 0.0) {
            mhz = m;
            continue;
        }

        if (mhz != m) {
            mhz = mxm_max(mhz,m);
            warn = 1;
        }
    }
    fclose(f);

    if (warn) {
        fprintf(stderr, "Conflicting CPU frequencies detected, using: %.2f\n", mhz);
    }
    return mhz * 1e6;
}
