/*
 *
 *    Copyright (c) 2014-2017 Nest Labs, Inc.
 *    All rights reserved.
 *
 *    Licensed under the Apache License, Version 2.0 (the "License");
 *    you may not use this file except in compliance with the License.
 *    You may obtain a copy of the License at
 *
 *        http://www.apache.org/licenses/LICENSE-2.0
 *
 *    Unless required by applicable law or agreed to in writing, software
 *    distributed under the License is distributed on an "AS IS" BASIS,
 *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *    See the License for the specific language governing permissions and
 *    limitations under the License.
 */

/**
 *    @file
 *      This file contains the implementation of
 *      WoBle Control Path and Troughput Test
 *
 */

#ifndef __STDC_LIMIT_MACROS
#define __STDC_LIMIT_MACROS
#endif
#include <stdint.h>

#include <BuildConfig.h>
#if CONFIG_NETWORK_LAYER_BLE

#include <string.h>

#include <Weave/Core/WeaveConfig.h>
#include <Weave/Core/WeaveEncoding.h>
#include <Weave/Support/logging/WeaveLogging.h>
#include <Weave/Support/CodeUtils.h>
#include <Weave/Support/FlagUtils.hpp>

#include <BleLayer/BleConfig.h>
#include <BleLayer/BLEEndPoint.h>
#include <BleLayer/BleLayer.h>
#include <BleLayer/WoBle.h>
#if WEAVE_ENABLE_WOBLE_TEST
#include "WoBleTest.h"
#endif

// Define below to enable extremely verbose, BLE end point-specific debug logging.
#undef NL_BLE_END_POINT_DEBUG_LOGGING_ENABLED

#ifdef NL_BLE_END_POINT_DEBUG_LOGGING_ENABLED
#define WeaveLogDebugBleEndPoint(MOD, MSG, ...) WeaveLogError(MOD, MSG, ##__VA_ARGS__)
#else
#define WeaveLogDebugBleEndPoint(MOD, MSG, ...)
#endif

namespace nl {
namespace Ble {

#if WEAVE_ENABLE_WOBLE_TEST

BLE_ERROR HandleCommandTest(void * ble, BLE_CONNECTION_OBJECT connObj, uint32_t packetCount, uint32_t duration, uint16_t txGap,
                            uint8_t needAck, uint16_t payloadSize, bool reverse)
{
    BLE_ERROR err          = BLE_NO_ERROR;
    BLEEndPoint * endPoint = ((nl::Ble::BleLayer *) ble)->mTestBleEndPoint;

    if (endPoint == NULL)
    {
        WeaveLogError(Ble, "no endpoint for BLE sent data");
        return BLE_ERROR_BAD_ARGS;
    }

    WeaveLogDebugBleEndPoint(Ble, "%s: Start count %u, duration %u, ack %u, size %u, reverse %u\n", __FUNCTION__, packetCount,
                             duration, needAck, payloadSize, reverse);

    endPoint->mWoBleTest.mCommandTestRequest.PacketCount = packetCount;
    endPoint->mWoBleTest.mCommandTestRequest.Duration    = duration;
    endPoint->mWoBleTest.mCommandTestRequest.TxGap       = txGap;
    endPoint->mWoBleTest.mCommandTestRequest.NeedAck     = needAck;
    if (payloadSize < COMMAND_TESTDATA_HDR_LEN)
        payloadSize = COMMAND_TESTDATA_HDR_LEN;
    // Actual payload includes the test data header
    endPoint->mWoBleTest.mCommandTestRequest.PayloadSize = payloadSize - COMMAND_TESTDATA_HDR_LEN;
    if (reverse)
    {
        err = endPoint->mWoBleTest.DoCommandTestRequest(endPoint);
    }
    else
    {
        err = endPoint->mWoBleTest.HandleCommandTest(endPoint);
    }

    return err;
}

BLE_ERROR HandleCommandTestResult(void * ble, BLE_CONNECTION_OBJECT connObj, bool local)
{
    BLE_ERROR err    = BLE_NO_ERROR;
    BLEEndPoint * ep = ((nl::Ble::BleLayer *) ble)->mTestBleEndPoint;

    if (ep == NULL)
    {
        WeaveLogError(Ble, "no endpoint for BLE sent data");
        return BLE_ERROR_BAD_ARGS;
    }

    if (!local)
        err = ep->mWoBleTest.DoCommandTestResult(kBleCommandTestResult_Request, 0);
    else
        WoBleTest::LogBleTestResult(&ep->mWoBleTest.mCommandTestResult);

    return err;
}

BLE_ERROR HandleCommandTestAbort(void * ble, BLE_CONNECTION_OBJECT connObj)
{
    BLE_ERROR err          = BLE_NO_ERROR;
    BLEEndPoint * endPoint = ((nl::Ble::BleLayer *) ble)->mTestBleEndPoint;

    if (endPoint == NULL)
    {
        WeaveLogError(Ble, "no endpoint for BLE sent ABORT");
        return err;
    }

    err = endPoint->mWoBleTest.DoCommandTestAbort(-1);

    return err;
}

BLE_ERROR HandleCommandTxTiming(void * ble, BLE_CONNECTION_OBJECT connObj, bool enabled, bool remote)
{
    BLE_ERROR err    = BLE_NO_ERROR;
    BLEEndPoint * ep = ((nl::Ble::BleLayer *) ble)->mTestBleEndPoint;

    if (ep == NULL)
    {
        WeaveLogError(Ble, "no endpoint for BLE sent data");
        return BLE_ERROR_BAD_ARGS;
    }

    if (remote)
        err = ep->mWoBleTest.DoCommandTxTiming(enabled);
    else
        err = ep->mWoBleTest.HandleCommandTxTiming(ep, enabled);

    return err;
}

// BleTransportCommandMessage implementation:

BLE_ERROR BleTransportCommandMessage::Encode(PacketBuffer * msgBuf, BleTransportCommandMessage & cmd) const
{
    uint8_t * p   = msgBuf->Start();
    BLE_ERROR err = BLE_NO_ERROR;

    // Verify we can write the fixed-length request without running into the end of the buffer.
    VerifyOrExit(msgBuf->MaxDataLength() > COMMAND_HEADER_LEN, err = BLE_ERROR_NO_MEMORY);

    nl::Weave::Encoding::LittleEndian::Write16(p, cmd.CmdHdr.PacketLength);
    nl::Weave::Encoding::Write8(p, cmd.CmdHdr.Version);

    nl::Weave::Encoding::Write8(p, cmd.CmdHdr.PacketType);

    switch (cmd.CmdHdr.PacketType)
    {
    case kBleCommandType_TestAck:
        nl::Weave::Encoding::Write8(p, cmd.Payload.MsgTestAck.Type);
        nl::Weave::Encoding::Write8(p, cmd.Payload.MsgTestAck.SequenceNumber);
        nl::Weave::Encoding::LittleEndian::Write32(p, cmd.Payload.MsgTestAck.ResultCode);
        msgBuf->SetDataLength(COMMAND_HEADER_LEN + COMMAND_ACK_HDR_LEN);
        break;
    case kBleCommandType_TestData:
        nl::Weave::Encoding::Write8(p, cmd.Payload.MsgTestData.Type);
        nl::Weave::Encoding::Write8(p, cmd.Payload.MsgTestData.NeedAck);
        nl::Weave::Encoding::LittleEndian::Write16(p, cmd.Payload.MsgTestData.Length);
        nl::Weave::Encoding::LittleEndian::Write32(p, cmd.Payload.MsgTestData.Sequence);
        VerifyOrExit(Data != NULL && cmd.Payload.MsgTestData.Length <= BLE_TEST_DATA_MAX_LEN, err = BLE_ERROR_NO_MEMORY);
        memcpy(p, Data, cmd.Payload.MsgTestData.Length);
        msgBuf->SetDataLength(COMMAND_HEADER_LEN + COMMAND_DATA_HDR_LEN + cmd.Payload.MsgTestData.Length);
        break;
    case kBleCommandType_TestRequest:
        nl::Weave::Encoding::LittleEndian::Write32(p, cmd.Payload.MsgTestRequest.PacketCount);
        nl::Weave::Encoding::LittleEndian::Write32(p, cmd.Payload.MsgTestRequest.Duration);
        nl::Weave::Encoding::LittleEndian::Write16(p, cmd.Payload.MsgTestRequest.TxGap);
        nl::Weave::Encoding::Write8(p, cmd.Payload.MsgTestRequest.NeedAck);
        nl::Weave::Encoding::LittleEndian::Write16(p, cmd.Payload.MsgTestRequest.PayloadSize);
        msgBuf->SetDataLength(COMMAND_HEADER_LEN + COMMAND_TESTREQ_HDR_LEN);
        break;
    case kBleCommandType_TestResult:
        nl::Weave::Encoding::LittleEndian::Write16(p, cmd.Payload.MsgTestResult.TestResultOp);
        nl::Weave::Encoding::LittleEndian::Write32(p, cmd.Payload.MsgTestResult.TestResult);
        nl::Weave::Encoding::LittleEndian::Write32(p, cmd.Payload.MsgTestResult.PacketCount);
        nl::Weave::Encoding::LittleEndian::Write32(p, cmd.Payload.MsgTestResult.Duration);
        nl::Weave::Encoding::LittleEndian::Write32(p, cmd.Payload.MsgTestResult.AckCount);
        nl::Weave::Encoding::LittleEndian::Write32(p, cmd.Payload.MsgTestResult.TxDrops);
        nl::Weave::Encoding::LittleEndian::Write16(p, cmd.Payload.MsgTestResult.TxGap);
        nl::Weave::Encoding::LittleEndian::Write16(p, cmd.Payload.MsgTestResult.PayloadSize);
        nl::Weave::Encoding::LittleEndian::Write16(p, cmd.Payload.MsgTestResult.TxPktCount);
        nl::Weave::Encoding::LittleEndian::Write32(p, cmd.Payload.MsgTestResult.TxTimeMs);
        nl::Weave::Encoding::LittleEndian::Write16(p, cmd.Payload.MsgTestResult.TxTimeMax);
        nl::Weave::Encoding::LittleEndian::Write16(p, cmd.Payload.MsgTestResult.TxTimeMin);
        nl::Weave::Encoding::LittleEndian::Write16(p, cmd.Payload.MsgTestResult.TxAckCount);
        nl::Weave::Encoding::LittleEndian::Write32(p, cmd.Payload.MsgTestResult.TxAckTimeMs);
        nl::Weave::Encoding::LittleEndian::Write16(p, cmd.Payload.MsgTestResult.TxAckTimeMax);
        nl::Weave::Encoding::LittleEndian::Write16(p, cmd.Payload.MsgTestResult.TxAckTimeMin);
        nl::Weave::Encoding::LittleEndian::Write16(p, cmd.Payload.MsgTestResult.TxTimeLastMs);
        nl::Weave::Encoding::LittleEndian::Write16(p, cmd.Payload.MsgTestResult.PayloadLast);
        nl::Weave::Encoding::LittleEndian::Write32(p, cmd.Payload.MsgTestResult.PayloadBytes);
        msgBuf->SetDataLength(COMMAND_HEADER_LEN + COMMAND_TESTRESULT_HDR_LEN);
        break;
    case kBleCommandType_WobleMTU:
        nl::Weave::Encoding::Write8(p, cmd.Payload.MsgWobleMTU.Op);
        nl::Weave::Encoding::LittleEndian::Write16(p, cmd.Payload.MsgWobleMTU.TxFragmentSize);
        nl::Weave::Encoding::LittleEndian::Write16(p, cmd.Payload.MsgWobleMTU.RxFragmentSize);
        msgBuf->SetDataLength(COMMAND_HEADER_LEN + COMMAND_WOBLEMTU_HDR_LEN);
        break;
    case kBleCommandType_WobleWindowSize:
        nl::Weave::Encoding::Write8(p, cmd.Payload.MsgWobleWindowSize.Op);
        nl::Weave::Encoding::Write8(p, cmd.Payload.MsgWobleWindowSize.TxWindowSize);
        nl::Weave::Encoding::Write8(p, cmd.Payload.MsgWobleWindowSize.RxWindowSize);
        msgBuf->SetDataLength(COMMAND_HEADER_LEN + COMMAND_WINDOWSIZE_HDR_LEN);
        break;
    case kBleCommandType_TxTiming:
        nl::Weave::Encoding::Write8(p, cmd.Payload.MsgTxTiming.Enable);
        msgBuf->SetDataLength(COMMAND_HEADER_LEN + COMMAND_TXTIMING_HDR_LEN);
        break;
    default:
        WeaveLogError(Ble, "%s: Not yet support", __FUNCTION__);
        break;
    }

exit:
    return err;
}

BLE_ERROR BleTransportCommandMessage::Decode(const PacketBuffer & msgBuf, BleTransportCommandMessage & cmd)
{
    const uint8_t * p = msgBuf.Start();
    BLE_ERROR err     = BLE_NO_ERROR;

    // Verify we can read the fixed-length response without running into the end of the buffer.
    VerifyOrExit(msgBuf.DataLength() >= COMMAND_HEADER_LEN, err = BLE_ERROR_MESSAGE_INCOMPLETE);

    cmd.CmdHdr.PacketLength = nl::Weave::Encoding::LittleEndian::Read16(p);
    cmd.CmdHdr.Version      = nl::Weave::Encoding::Read8(p);
    cmd.CmdHdr.PacketType   = nl::Weave::Encoding::Read8(p);

    switch (cmd.CmdHdr.PacketType)
    {
    case kBleCommandType_TestAck:
        cmd.Payload.MsgTestAck.Type           = nl::Weave::Encoding::Read8(p);
        cmd.Payload.MsgTestAck.SequenceNumber = nl::Weave::Encoding::Read8(p);
        cmd.Payload.MsgTestAck.ResultCode     = nl::Weave::Encoding::LittleEndian::Read32(p);
        break;
    case kBleCommandType_TestData:
        cmd.Payload.MsgTestData.Type     = nl::Weave::Encoding::Read8(p);
        cmd.Payload.MsgTestData.NeedAck  = nl::Weave::Encoding::Read8(p);
        cmd.Payload.MsgTestData.Length   = nl::Weave::Encoding::LittleEndian::Read16(p);
        cmd.Payload.MsgTestData.Sequence = nl::Weave::Encoding::LittleEndian::Read32(p);
        VerifyOrExit(cmd.Data != NULL && cmd.Payload.MsgTestData.Length <= BLE_TEST_DATA_MAX_LEN, err = BLE_ERROR_NO_MEMORY);
        memcpy(cmd.Data, p, cmd.Payload.MsgTestData.Length);
        break;
    case kBleCommandType_TestRequest:
        cmd.Payload.MsgTestRequest.PacketCount = nl::Weave::Encoding::LittleEndian::Read32(p);
        cmd.Payload.MsgTestRequest.Duration    = nl::Weave::Encoding::LittleEndian::Read32(p);
        cmd.Payload.MsgTestRequest.TxGap       = nl::Weave::Encoding::LittleEndian::Read16(p);
        cmd.Payload.MsgTestRequest.NeedAck     = nl::Weave::Encoding::Read8(p);
        cmd.Payload.MsgTestRequest.PayloadSize = nl::Weave::Encoding::LittleEndian::Read16(p);
        break;
    case kBleCommandType_TestResult:
        cmd.Payload.MsgTestResult.TestResultOp = nl::Weave::Encoding::LittleEndian::Read16(p);
        cmd.Payload.MsgTestResult.TestResult   = nl::Weave::Encoding::LittleEndian::Read32(p);
        cmd.Payload.MsgTestResult.PacketCount  = nl::Weave::Encoding::LittleEndian::Read32(p);
        cmd.Payload.MsgTestResult.Duration     = nl::Weave::Encoding::LittleEndian::Read32(p);
        cmd.Payload.MsgTestResult.AckCount     = nl::Weave::Encoding::LittleEndian::Read32(p);
        cmd.Payload.MsgTestResult.TxDrops      = nl::Weave::Encoding::LittleEndian::Read32(p);
        cmd.Payload.MsgTestResult.TxGap        = nl::Weave::Encoding::LittleEndian::Read16(p);
        cmd.Payload.MsgTestResult.PayloadSize  = nl::Weave::Encoding::LittleEndian::Read16(p);
        cmd.Payload.MsgTestResult.TxPktCount   = nl::Weave::Encoding::LittleEndian::Read16(p);
        cmd.Payload.MsgTestResult.TxTimeMs     = nl::Weave::Encoding::LittleEndian::Read32(p);
        cmd.Payload.MsgTestResult.TxTimeMax    = nl::Weave::Encoding::LittleEndian::Read16(p);
        cmd.Payload.MsgTestResult.TxTimeMin    = nl::Weave::Encoding::LittleEndian::Read16(p);
        cmd.Payload.MsgTestResult.TxAckCount   = nl::Weave::Encoding::LittleEndian::Read16(p);
        cmd.Payload.MsgTestResult.TxAckTimeMs  = nl::Weave::Encoding::LittleEndian::Read32(p);
        cmd.Payload.MsgTestResult.TxAckTimeMax = nl::Weave::Encoding::LittleEndian::Read16(p);
        cmd.Payload.MsgTestResult.TxAckTimeMin = nl::Weave::Encoding::LittleEndian::Read16(p);
        cmd.Payload.MsgTestResult.TxTimeLastMs = nl::Weave::Encoding::LittleEndian::Read16(p);
        cmd.Payload.MsgTestResult.PayloadLast  = nl::Weave::Encoding::LittleEndian::Read16(p);
        cmd.Payload.MsgTestResult.PayloadBytes = nl::Weave::Encoding::LittleEndian::Read32(p);
        break;
    case kBleCommandType_WobleMTU:
        cmd.Payload.MsgWobleMTU.Op             = nl::Weave::Encoding::Read8(p);
        cmd.Payload.MsgWobleMTU.TxFragmentSize = nl::Weave::Encoding::LittleEndian::Read16(p);
        cmd.Payload.MsgWobleMTU.RxFragmentSize = nl::Weave::Encoding::LittleEndian::Read16(p);
        break;
    case kBleCommandType_WobleWindowSize:
        cmd.Payload.MsgWobleWindowSize.Op           = nl::Weave::Encoding::Read8(p);
        cmd.Payload.MsgWobleWindowSize.TxWindowSize = nl::Weave::Encoding::Read8(p);
        cmd.Payload.MsgWobleWindowSize.RxWindowSize = nl::Weave::Encoding::Read8(p);
        break;
    case kBleCommandType_TxTiming:
        cmd.Payload.MsgTxTiming.Enable = nl::Weave::Encoding::Read8(p);
        break;
    default:
        WeaveLogError(Ble, "%s: Not yet support", __FUNCTION__);
        break;
    }

exit:
    if (err != BLE_NO_ERROR)
        WeaveLogError(Ble, "%s: ERROR = %d", __FUNCTION__, err);

    return err;
}

BLE_ERROR WoBleTest::Init(BLEEndPoint * ep)
{
    BLE_ERROR err = BLE_NO_ERROR;

    mCommandUnderTest    = WOBLE_TEST_NONE;
    mCommandTxTiming     = false;
    mCommandReceiveQueue = NULL;
    mCommandSendQueue    = NULL;
    mCommandAckToSend    = NULL;
    memset(&mTestTxThread, 0, sizeof(mTestTxThread));
    memset(&mTxHistogram, 0, sizeof(mTxHistogram));

    mMainThread = pthread_self();
    mEp         = ep;

    WeaveLogError(Ble, "%s: Initialize WoBleTest, ep->%#p, thread %x", __FUNCTION__, ep, mMainThread);

    // Register the command handler
    ep->SetOnCommandReceivedCB(HandleCommandReceived);

    return err;
}

void WoBleTest::Decode(uint8_t * src, uint8_t * dst, size_t size)
{
    switch (size)
    {
    case sizeof(int8_t):
        *dst = *src;
        break;
    case sizeof(int16_t):
    {
        int16_t i = nl::Weave::Encoding::LittleEndian::Read16(src);
        memcpy(dst, &i, size);
    }
    break;
    case sizeof(int32_t):
    {
        int32_t i = nl::Weave::Encoding::LittleEndian::Read32(src);
        memcpy(dst, &i, size);
    }
    break;
    case sizeof(int64_t):
    {
        int64_t i = nl::Weave::Encoding::LittleEndian::Read64(src);
        memcpy(dst, &i, size);
    }
    break;
    default:
        WeaveLogError(Ble, "%s: unsupported size %u\n", __FUNCTION__, size);
        break;
    }
}

// This is the WoBle Command Handler
void WoBleTest::HandleCommandReceived(BLEEndPoint * ep, PacketBuffer * data)
{
    BLE_ERROR err = BLE_NO_ERROR;
    Weave::System::Error timerErr;

    // Fail if any data missing
    VerifyOrExit(ep != NULL && data != NULL, err = BLE_ERROR_BAD_ARGS);

    // Add new message to send queue.
    if (ep->mWoBleTest.mCommandReceiveQueue == NULL)
    {
        WeaveLogDebugBleEndPoint(Ble, "set data as new mCommandReceiveQueue");
        ep->mWoBleTest.mCommandReceiveQueue = data;
    }
    else
    {
        WeaveLogDebugBleEndPoint(Ble, "added data to end");
        ep->mWoBleTest.mCommandReceiveQueue->AddToEnd(data);
    }
    data = NULL;

    // Handle BTP command in timer
    timerErr = ep->mBle->mSystemLayer->StartTimer(0, HandleCommandPacket, ep);
    VerifyOrExit(timerErr == BLE_NO_ERROR, err = BLE_ERROR_START_TIMER_FAILED);

exit:
    if (err != BLE_NO_ERROR)
        WeaveLogError(Ble, "%s: Error %d", __FUNCTION__, err);

    if (data != NULL)
    {
        PacketBuffer::Free(data);
    }

    return;
}

// This is the WoBle TxTiming Handler
void WoBleTest::DoTxTiming(PacketBuffer * data, int stage)
{
    if (mCommandUnderTest == WOBLE_TEST_NONE && !mCommandTxTiming)
        return; // TxTiming is not enabled

    WeaveLogDebugBleEndPoint(Ble, "%s: stage %d, mode %u:%u, data->%p, type %u, len %u", __FUNCTION__, stage, mCommandUnderTest,
                             mCommandTxTiming, data, mEp->mWoBle.TxPacketType(), data->DataLength());

    if (!mCommandTxTiming && (mCommandUnderTest && mEp->mWoBle.TxPacketType() != kType_Control))
        return; // TxTiming is not required

    switch (stage)
    {
    case WOBLE_TX_START:
        mTimeStats.mTxStartMs = nl::Weave::System::Timer::GetCurrentEpoch();
        mTimeStats.mTxPayload = data->DataLength();
        break;

    case WOBLE_TX_DONE:
        if (mTimeStats.mTxStartMs)
        {
            // Compute Tx time and collect the statistics
            int diff = nl::Weave::System::Timer::GetCurrentEpoch() - mTimeStats.mTxStartMs;

            AddTxRecord(mTimeStats.mTxStartMs, diff, mTimeStats.mTxPayload);

            // Always keep the TxTime of last WoBle packet
            mCommandTestResult.TxTimeLastMs = diff;
            mCommandTestResult.PayloadLast  = mTimeStats.mTxPayload;
            mCommandTestResult.TxTimeMs += diff;
            if (mCommandTestResult.TxTimeMax == 0)
                mCommandTestResult.TxTimeMax = mCommandTestResult.TxTimeMin = diff;
            else if (diff > mCommandTestResult.TxTimeMax)
                mCommandTestResult.TxTimeMax = diff;
            else if (diff < mCommandTestResult.TxTimeMin)
                mCommandTestResult.TxTimeMin = diff;

            mCommandTestResult.TxPktCount++;
            mCommandTestResult.PayloadBytes += mCommandTestResult.PayloadLast;

            // Check if we're done with this packet
            if (mCommandTestRequest.NeedAck)
                mTimeStats.mTxAckStartMs = mTimeStats.mTxStartMs;
            mTimeStats.mTxStartMs = 0;

            WeaveLogDebugBleEndPoint(Ble, "%s: TxTimeMs %u, TxPktCount %u, PayloadBytes %u", __FUNCTION__,
                                     mCommandTestResult.TxTimeMs, mCommandTestResult.TxPktCount, mCommandTestResult.PayloadBytes);
            WeaveLogDebugBleEndPoint(Ble, "%s: TxTimeLastMs %u, PayloadLast %u", __FUNCTION__, mCommandTestResult.TxTimeLastMs,
                                     mCommandTestResult.PayloadLast);

            // Send the next packet if it's already past due, so
            // no need to wait for the next Tx time. It's to eliminate the Tx idle gap.
            if (mCommandUnderTest == WOBLE_TEST_TX && diff >= mCommandTestRequest.TxGap)
                mEp->mBle->mSystemLayer->StartTimer(0, DoTestDataSend, (void *) mEp);
        }

        if (mCommandUnderTest == WOBLE_TEST_TX && mEp->mWoBleTest.mCommand.CommandTest_Duration <= 0 &&
            mEp->mWoBleTest.mCommand.CommandTest_PacketCount == 0)
        {
            WeaveLogError(Ble, "%s: *** Finished sending last Tx test packet", __FUNCTION__);
            // We just finished sending the last test packet
            mCommandUnderTest = WOBLE_TEST_NONE;
        }
        break;

    case WOBLE_TX_DATA_ACK:
        if (mTimeStats.mTxAckStartMs)
        {
            // Compute Tx+Ack time and collect the statistics
            int diff = nl::Weave::System::Timer::GetCurrentEpoch() - mTimeStats.mTxAckStartMs;

            mCommandTestResult.TxAckTimeMs += diff;
            if (mCommandTestResult.TxAckTimeMax == 0)
                mCommandTestResult.TxAckTimeMax = mCommandTestResult.TxAckTimeMin = diff;
            else if (diff > mCommandTestResult.TxAckTimeMax)
                mCommandTestResult.TxAckTimeMax = diff;
            else if (diff < mCommandTestResult.TxAckTimeMin)
                mCommandTestResult.TxAckTimeMin = diff;

            mCommandTestResult.TxAckCount++;

            // Done with this packet
            mTimeStats.mTxAckStartMs = 0;
            WeaveLogDebugBleEndPoint(Ble, "%s: TxAckTimeMs %u, TxAckCount %u", __FUNCTION__, mCommandTestResult.TxAckTimeMs,
                                     mCommandTestResult.TxAckCount);
        }
        break;

    default:
        break;
    }
}

/*
 * Check whether it's a null pointer or containing all 0s
 */
bool isEmptyData(char * data, size_t size)
{
    if (data == NULL)
        return true;

    while (size-- > 0)
        if (*data++ != 0)
            return false;

    return true;
}

void WoBleTest::DoTestDataSend(Weave::System::Layer * systemLayer, void * appState, Weave::System::Error err)
{
    BLEEndPoint * ep    = static_cast<BLEEndPoint *>(appState);
    PacketBuffer * data = NULL;
    uint8_t type;
    pthread_t curThread = pthread_self();

    if (ep == NULL || ep->mState == BLEEndPoint::kState_Closed || ep->mWoBleTest.mCommandUnderTest != WOBLE_TEST_TX)
    {
        WeaveLogDebugBleEndPoint(Ble, "%s: State = %d, mCommandUnderTest %d, thread %x", __FUNCTION__, ep ? ep->mState : -1,
                                 ep->mWoBleTest.mCommandUnderTest, curThread);
        ExitNow();
    }

    if (!(isEmptyData((char *) &ep->mWoBleTest.mTestTxThread, sizeof(pthread_t)) ||
          pthread_equal(curThread, ep->mWoBleTest.mTestTxThread) || pthread_equal(curThread, ep->mWoBleTest.mMainThread)))
    {
        WeaveLogDebugBleEndPoint(Ble, "%s: Keep Tx thread (id %x) and stop %x", __FUNCTION__, ep->mWoBleTest.mTestTxThread,
                                 curThread);
        pthread_exit(NULL); // Suicide is the safest way to kill a thread
    }

    if (ep->mWoBleTest.mCommandTestResult.PacketCount++ == 0)
    {
        memcpy(&ep->mWoBleTest.mTestTxThread, &curThread, sizeof(pthread_t));
        WeaveLogDebugBleEndPoint(Ble, "%s: Tx thread started (id %x)", __FUNCTION__, ep->mWoBleTest.mTestTxThread);
        ep->mWoBleTest.mCommand.CommandTest_Start = true;
        SetFlag(ep->mTimerStateFlags, BLEEndPoint::kTimerState_UnderTestTimerRunnung, true);
        ep->mWoBleTest.mCommand.CommandTest_Duration    = (int32_t) ep->mWoBleTest.mCommandTestRequest.Duration;
        ep->mWoBleTest.mCommand.CommandTest_PacketCount = (int32_t) ep->mWoBleTest.mCommandTestRequest.PacketCount;
        ep->mWoBleTest.mCommandTestResult.TxGap         = ep->mWoBleTest.mCommandTestRequest.TxGap;
        ep->mWoBleTest.mCommandTestResult.PayloadSize   = ep->mWoBleTest.mCommandTestRequest.PayloadSize;
        WeaveLogDebugBleEndPoint(Ble, "\n%s: Count %d, Duration %u, TxGap %u, Ack %d, Size %u", __FUNCTION__,
                                 ep->mWoBleTest.mCommandTestRequest.PacketCount, ep->mWoBleTest.mCommandTestRequest.Duration,
                                 ep->mWoBleTest.mCommandTestRequest.TxGap, ep->mWoBleTest.mCommandTestRequest.NeedAck,
                                 ep->mWoBleTest.mCommandTestRequest.PayloadSize);
    }
    else
        type = kDataType_CONTINUE;

    // Check if it's the last test packet
    if (ep->mWoBleTest.mCommand.CommandTest_Duration > 0)
    {
        ep->mWoBleTest.mCommand.CommandTest_Duration -= ep->mWoBleTest.mCommandTestRequest.TxGap;
        if ((int) ep->mWoBleTest.mCommand.CommandTest_Duration <= 0)
            type = kDataType_END;
    }
    if (ep->mWoBleTest.mCommand.CommandTest_PacketCount > 0)
    {
        if (--(ep->mWoBleTest.mCommand.CommandTest_PacketCount) == 0)
            type = kDataType_END;
    }

    if (ep->mSendQueue == NULL) // Allow Tx test data only when Tx queue is empty
    {
        if (ep->mWoBleTest.mCommand.CommandTest_Start)
        {
            type                                      = kDataType_START;
            ep->mWoBleTest.mCommand.CommandTest_Start = false;
        }
        data = PacketBuffer::New();
        WeaveLogDebugBleEndPoint(Ble, "%s: Tx pkt# %d, data->%p, TxGap %d, duration %d, count %d", __FUNCTION__,
                                 ep->mWoBleTest.mCommandTestResult.PacketCount, data, ep->mWoBleTest.mCommandTestRequest.TxGap,
                                 ep->mWoBleTest.mCommand.CommandTest_Duration, ep->mWoBleTest.mCommand.CommandTest_PacketCount);
        VerifyOrExit(data != NULL, err = BLE_ERROR_NO_MEMORY);
        ep->mWoBleTest.mCommand.CmdHdr.PacketLength =
            sizeof(ep->mWoBleTest.mCommand.Payload.MsgTestData) + ep->mWoBleTest.mCommandTestRequest.PayloadSize;
        ep->mWoBleTest.mCommand.CmdHdr.Version               = 0;
        ep->mWoBleTest.mCommand.CmdHdr.PacketType            = kBleCommandType_TestData;
        ep->mWoBleTest.mCommand.Payload.MsgTestData.Type     = type;
        ep->mWoBleTest.mCommand.Payload.MsgTestData.NeedAck  = ep->mWoBleTest.mCommandTestRequest.NeedAck;
        ep->mWoBleTest.mCommand.Payload.MsgTestData.Length   = ep->mWoBleTest.mCommandTestRequest.PayloadSize;
        ep->mWoBleTest.mCommand.Payload.MsgTestData.Sequence = ep->mWoBleTest.mCommandTestResult.PacketCount;

        err = ep->mWoBleTest.mCommand.Encode(data, ep->mWoBleTest.mCommand);
        SuccessOrExit(err);

        // Add new data to send queue.
        ep->QueueTx(data, kType_Control);
        data = NULL; // Buffer freed when send queue freed on close, or on completion of current message transmission.

        err = ep->DriveSending();
        SuccessOrExit(err);
    }
    else
    {
        WeaveLogDebugBleEndPoint(Ble, "%s: GATT ON, Dropping pkt %d, TxGap %d, duration %d", __FUNCTION__,
                                 ep->mWoBleTest.mCommandTestResult.PacketCount, ep->mWoBleTest.mCommandTestRequest.TxGap,
                                 ep->mWoBleTest.mCommand.CommandTest_Duration);
        ep->mWoBleTest.mCommandTestResult.TxDrops++;
    }

    ep->mWoBleTest.mCommandTestResult.Duration += ep->mWoBleTest.mCommandTestRequest.TxGap; // in ms

    // Check if it's the last packet
    if (type == kDataType_END)
    {
        SetFlag(ep->mTimerStateFlags, BLEEndPoint::kTimerState_UnderTestTimerRunnung, false);
        ep->mWoBleTest.mCommand.CommandTest_Duration = ep->mWoBleTest.mCommand.CommandTest_PacketCount = 0;
        WeaveLogDebugBleEndPoint(Ble, "%s: Tx Test Done (id %x)", __FUNCTION__, curThread);
    }

    systemLayer->StartTimer(ep->mWoBleTest.mCommandTestRequest.TxGap, DoTestDataSend, (void *) ep);

exit:
    if (err != BLE_NO_ERROR)
        WeaveLogError(Ble, "%s: err %d", __FUNCTION__, err);

    if (data != NULL)
    {
        PacketBuffer::Free(data);
    }

    return;
}

void * WoBleTest::StartTxThread(void * data)
{
    BLEEndPoint * ep = static_cast<BLEEndPoint *>(data);
    BLE_ERROR err    = BLE_NO_ERROR;
    Weave::System::Error timerErr;

    timerErr = ep->mBle->mSystemLayer->StartTimer(0, DoTestDataSend, ep);
    VerifyOrExit(timerErr == BLE_NO_ERROR, err = BLE_ERROR_START_TIMER_FAILED);

exit:
    if (err != BLE_NO_ERROR)
        WeaveLogError(Ble, "%s: Error %d", __FUNCTION__, err);

    WeaveLogDebugBleEndPoint(Ble, "%s: Thread exited (id %x)", __FUNCTION__, pthread_self());

    return NULL;
}

BLE_ERROR WoBleTest::HandleCommandTest(BLEEndPoint * ep)
{
    BLE_ERROR err = BLE_NO_ERROR;
    pthread_t newThread;

    ep->mWoBleTest.mCommandUnderTest = WOBLE_TEST_TX;
    memset(&ep->mWoBleTest.mCommandTestResult, 0, sizeof(BTCommandTypeTestResult));
    memset((char *) &ep->mWoBleTest.mTimeStats, 0, sizeof(ep->mWoBleTest.mTimeStats));

    // Start the test
    memset(ep->mWoBleTest.mCommand.Data, 0xff, BLE_TEST_DATA_MAX_LEN);
    err = (BLE_ERROR) pthread_create(&newThread, NULL, StartTxThread, (void *) ep);
    WeaveLogDebugBleEndPoint(Ble, "%s: Started thread (id %x), err %d", __FUNCTION__, newThread, err);
    SuccessOrExit(err);

exit:
    if (err != BLE_NO_ERROR)
        WeaveLogError(Ble, "%s: Error %d", __FUNCTION__, err);

    return err;
}

// This function enables/disables the Tx Timing and Histogram
BLE_ERROR WoBleTest::HandleCommandTxTiming(BLEEndPoint * ep, bool enabled)
{
    WeaveLogDebugBleEndPoint(Ble, "%s: enabled = %d", __FUNCTION__, enabled);

    ep->mWoBleTest.mCommandTxTiming = enabled;
    if (enabled)
    {
        memset(&ep->mWoBleTest.mCommandTestResult, 0, sizeof(BTCommandTypeTestResult));
        memset((char *) &ep->mWoBleTest.mTimeStats, 0, sizeof(ep->mWoBleTest.mTimeStats));
        if (ep->mWoBleTest.InitTxHistogram(WOBLE_TX_HISTOGRAM_FILE, WOBLE_TX_RECORD_COUNT, true) < 0)
            WeaveLogError(Ble, "%s: Warning - No Tx Histogram\n", __FUNCTION__);
    }
    else
        ep->mWoBleTest.DoneTxHistogram(true);

    return BLE_NO_ERROR;
}

BLE_ERROR WoBleTest::DoCommandSendAck(SequenceNumber_t seqNum, int32_t result)
{
    BLE_ERROR err = BLE_NO_ERROR;
    PacketBuffer * data;

    data                                       = PacketBuffer::New();
    mCommand.CmdHdr.PacketLength               = sizeof(mCommand.Payload.MsgTestAck);
    mCommand.CmdHdr.Version                    = 0;
    mCommand.CmdHdr.PacketType                 = kBleCommandType_TestAck;
    mCommand.Payload.MsgTestAck.Type           = result ? kAckType_OK : kAckType_NOK;
    mCommand.Payload.MsgTestAck.SequenceNumber = seqNum;
    mCommand.Payload.MsgTestAck.ResultCode     = result;

    err = mCommand.Encode(data, mCommand);
    SuccessOrExit(err);

    mEp->QueueTx(data, kType_Control);
    data = NULL; // Buffer freed when send queue freed on close, or on completion of current message transmission.

    err = mEp->DriveSending();
    SuccessOrExit(err);

exit:
    if (err != BLE_NO_ERROR)
        WeaveLogError(Ble, "%s: err %d\n", __FUNCTION__, err);

    if (data != NULL)
    {
        PacketBuffer::Free(data);
    }

    return err;
}

BLE_ERROR WoBleTest::DoCommandTestResult(int32_t op, int32_t result)
{
    BLE_ERROR err = BLE_NO_ERROR;
    PacketBuffer * data;

    data                                        = PacketBuffer::New();
    mCommand.CmdHdr.PacketLength                = sizeof(mCommand.Payload.MsgTestResult);
    mCommand.CmdHdr.Version                     = 0;
    mCommand.CmdHdr.PacketType                  = kBleCommandType_TestResult;
    mCommand.Payload.MsgTestResult.TestResultOp = op;
    if (op == kBleCommandTestResult_Reply)
    {
        mCommand.Payload.MsgTestResult            = mCommandTestResult;
        mCommand.Payload.MsgTestResult.TestResult = result;
    }

    err = mCommand.Encode(data, mCommand);
    SuccessOrExit(err);

    // Add packet to send queue head
    mEp->QueueTx(data, kType_Control);
    data = NULL; // Buffer freed when send queue freed on close, or on completion of current message transmission.

    err = mEp->DriveSending();
    SuccessOrExit(err);

exit:
    if (err != BLE_NO_ERROR)
        WeaveLogError(Ble, "%s: err %d\n", __FUNCTION__, err);

    if (data != NULL)
    {
        PacketBuffer::Free(data);
    }

    return err;
}

BLE_ERROR WoBleTest::DoCommandTxTiming(bool enable)
{
    BLE_ERROR err = BLE_NO_ERROR;
    PacketBuffer * data;

    data                                = PacketBuffer::New();
    mCommand.CmdHdr.PacketLength        = sizeof(mCommand.Payload.MsgTxTiming);
    mCommand.CmdHdr.Version             = 0;
    mCommand.CmdHdr.PacketType          = kBleCommandType_TxTiming;
    mCommand.Payload.MsgTxTiming.Enable = enable;

    err = mCommand.Encode(data, mCommand);
    SuccessOrExit(err);

    // Add packet to send queue head
    mEp->QueueTx(data, kType_Control);
    data = NULL; // Buffer freed when send queue freed on close, or on completion of current message transmission.

    err = mEp->DriveSending();
    SuccessOrExit(err);

exit:
    if (err != BLE_NO_ERROR)
        WeaveLogError(Ble, "%s: err %d\n", __FUNCTION__, err);

    if (data != NULL)
    {
        PacketBuffer::Free(data);
    }

    return err;
}

BLE_ERROR WoBleTest::DoCommandTestAbort(int32_t result)
{
    BLE_ERROR err = BLE_NO_ERROR;
    PacketBuffer * data;

    WeaveLogDebugBleEndPoint(Ble, "%s: result %d\n", __FUNCTION__, result);
    if (mCommandUnderTest == WOBLE_TEST_NONE)
        return err;

    StopTestTimer();
    data                                 = PacketBuffer::New();
    mCommand.CmdHdr.PacketLength         = sizeof(mCommand.Payload.MsgTestData);
    mCommand.CmdHdr.Version              = 0;
    mCommand.CmdHdr.PacketType           = kBleCommandType_TestData;
    mCommand.Payload.MsgTestData.Type    = kDataType_ABORT;
    mCommand.Payload.MsgTestData.NeedAck = false;
    mCommand.Payload.MsgTestData.Length  = 0;

    err = mCommand.Encode(data, mCommand);
    SuccessOrExit(err);

    mEp->QueueTx(data, kType_Control);
    data = NULL; // Buffer freed when send queue freed on close, or on completion of current message transmission.

    err = mEp->DriveSending();
    SuccessOrExit(err);

exit:
    mCommandUnderTest = WOBLE_TEST_NONE; // always terminate local test

    if (err != BLE_NO_ERROR)
        WeaveLogError(Ble, "%s: err %d\n", __FUNCTION__, err);

    if (data != NULL)
    {
        PacketBuffer::Free(data);
    }

    return err;
}

BLE_ERROR WoBleTest::DoCommandRequestTestResult()
{
    BLE_ERROR err = BLE_NO_ERROR;
    PacketBuffer * data;

    data                                        = PacketBuffer::New();
    mCommand.CmdHdr.PacketLength                = sizeof(mCommand.Payload.MsgTestResult);
    mCommand.CmdHdr.Version                     = 0;
    mCommand.CmdHdr.PacketType                  = kBleCommandType_TestResult;
    mCommand.Payload.MsgTestResult.TestResultOp = kBleCommandTestResult_Request;
    mCommand.Payload.MsgTestResult.PacketCount  = 0;

    err = mCommand.Encode(data, mCommand);
    SuccessOrExit(err);

    // Add packet to send queue head
    mEp->QueueTx(data, kType_Control);
    data = NULL; // Buffer freed when send queue freed on close, or on completion of current message transmission.

    err = mEp->DriveSending();
    SuccessOrExit(err);

exit:
    if (err != BLE_NO_ERROR)
        WeaveLogError(Ble, "%s: err %d\n", __FUNCTION__, err);

    if (data != NULL)
    {
        PacketBuffer::Free(data);
    }

    return err;
}

BLE_ERROR WoBleTest::DoCommandTestRequest(BLEEndPoint * ep)
{
    BLE_ERROR err = BLE_NO_ERROR;
    PacketBuffer * data;

    WeaveLogDebugBleEndPoint(Ble, "%s: Sending TestRequest to device...\n", __FUNCTION__);

    data                                        = PacketBuffer::New();
    mCommand.CmdHdr.PacketLength                = sizeof(mCommand.Payload.MsgTestRequest);
    mCommand.CmdHdr.Version                     = 0;
    mCommand.CmdHdr.PacketType                  = kBleCommandType_TestRequest;
    mCommand.Payload.MsgTestRequest.PacketCount = mCommandTestRequest.PacketCount;
    mCommand.Payload.MsgTestRequest.Duration    = mCommandTestRequest.Duration;
    mCommand.Payload.MsgTestRequest.TxGap       = mCommandTestRequest.TxGap;
    mCommand.Payload.MsgTestRequest.NeedAck     = mCommandTestRequest.NeedAck;
    mCommand.Payload.MsgTestRequest.PayloadSize = mCommandTestRequest.PayloadSize;

    err = mCommand.Encode(data, mCommand);
    SuccessOrExit(err);

    ep->QueueTx(data, kType_Control);
    data = NULL; // Buffer freed when send queue freed on close, or on completion of current message transmission.

    err = ep->DriveSending();
    SuccessOrExit(err);

exit:
    if (err != BLE_NO_ERROR)
        WeaveLogError(Ble, "%s: err %d\n", __FUNCTION__, err);

    if (data != NULL)
    {
        PacketBuffer::Free(data);
    }

    return err;
}

void WoBleTest::LogBleTestResult(BTCommandTypeTestResult * result)
{
    WeaveLogDebugBleEndPoint(Ble, "%s: PacketCount %u, TxPktCount %u, TxAckCount %u", __FUNCTION__, result->PacketCount,
                             result->TxPktCount, result->TxAckCount);

    if (result->PacketCount > 0)
    {
        WeaveLogError(Ble, "TestResult  : %d", result->TestResult);
        WeaveLogError(Ble, "PacketCount : %u", result->PacketCount);
        WeaveLogError(Ble, "Duration    : %u", result->Duration);
        WeaveLogError(Ble, "AckCount    : %u", result->AckCount);
        WeaveLogError(Ble, "TxDrops     : %u", result->TxDrops);
        WeaveLogError(Ble, "TxGap       : %u", result->TxGap);
        // Actual payload includes the test data header
        WeaveLogError(Ble, "PayloadSize : %u", result->PayloadSize + COMMAND_TESTDATA_HDR_LEN);
    }
    if (result->TxPktCount > 0)
    {
        WeaveLogError(Ble, "=========================");
        WeaveLogError(Ble, "Last Tx time        : %u", result->TxTimeLastMs);
        WeaveLogError(Ble, "Last Payload Bytes  : %u", result->PayloadLast);
        WeaveLogError(Ble, "=========================");
        WeaveLogError(Ble, "Tx Packet Count     : %u", result->TxPktCount);
        WeaveLogError(Ble, "Total Payload Bytes : %u", result->PayloadBytes);
        WeaveLogError(Ble, "Average Tx time/pkt : %u", result->TxTimeMs / result->TxPktCount);
        WeaveLogError(Ble, "Max Tx time         : %u", result->TxTimeMax);
        WeaveLogError(Ble, "Min Tx time         : %u", result->TxTimeMin);
        if (result->TxAckCount)
        {
            WeaveLogError(Ble, "Ack Packet Count    : %u", result->TxAckCount);
            WeaveLogError(Ble, "Average Tx+Ack time : %u", result->TxAckTimeMs / result->TxAckCount);
            WeaveLogError(Ble, "Max Tx+Ack time     : %u", result->TxAckTimeMax);
            WeaveLogError(Ble, "Min Tx+Ack time     : %u", result->TxAckTimeMin);
        }
    }
}

void WoBleTest::HandleCommandPacket(Weave::System::Layer * systemLayer, void * appState, Weave::System::Error err)
{
    BLEEndPoint * ep = static_cast<BLEEndPoint *>(appState);
    BleTransportCommandMessage cmd;
    bool needAck        = false;
    PacketBuffer * data = NULL;
    Weave::System::Error timerErr;

    if (ep == NULL)
    {
        WeaveLogError(Ble, "%s: ep->%p", __FUNCTION__, ep);
        err = BLE_ERROR_NO_ENDPOINTS;
        SuccessOrExit(err);
    }

    data                                = ep->mWoBleTest.mCommandReceiveQueue;
    ep->mWoBleTest.mCommandReceiveQueue = ep->mWoBleTest.mCommandReceiveQueue->DetachTail();

    if (data == NULL)
    {
        WeaveLogError(Ble, "%s: ep->%p, data->%p", __FUNCTION__, ep, data);
        err = BLE_ERROR_NO_ENDPOINTS;
        SuccessOrExit(err);
    }

    err = BleTransportCommandMessage::Decode(*data, cmd);
    SuccessOrExit(err);

    WeaveLogDebugBleEndPoint(Ble, "%s: packet seq# %u, type %u, len %u", __FUNCTION__, ep->mWoBle.RxPacketSeq(),
                             cmd.CmdHdr.PacketType, data->DataLength());

    switch (cmd.CmdHdr.PacketType)
    {
    case kBleCommandType_TestAck:
        ep->mWoBleTest.mCommandTestResult.AckCount++;
        if (ep->mWoBleTest.mCommandUnderTest == WOBLE_TEST_TX)
        {
            ep->mWoBleTest.DoTxTiming(data, WOBLE_TX_DATA_ACK);
            // Sender sends the next data packet
            timerErr = systemLayer->StartTimer(0, DoTestDataSend, (void *) ep);
            VerifyOrExit(timerErr == BLE_NO_ERROR, err = BLE_ERROR_START_TIMER_FAILED);
        }
        break;

    case kBleCommandType_TestData:
        switch (cmd.Payload.MsgTestData.Type)
        {
        case kDataType_START:
            // reset counters if sender restarted
            if (ep->mWoBleTest.mCommandUnderTest == WOBLE_TEST_RX)
            {
                memset(&ep->mWoBleTest.mCommandTestResult, 0, sizeof(BTCommandTypeTestResult));
                ep->mWoBleTest.mCommandTestResult.PayloadSize = cmd.Payload.MsgTestData.Length;
            }
            // falls thru
        case kDataType_CONTINUE:
            if (ep->mWoBleTest.mCommandUnderTest == WOBLE_TEST_NONE)
            {
                memset(&ep->mWoBleTest.mCommandTestResult, 0, sizeof(BTCommandTypeTestResult));
                ep->mWoBleTest.mCommandTestResult.PayloadSize = cmd.Payload.MsgTestData.Length;
                ep->mWoBleTest.mCommandUnderTest              = WOBLE_TEST_RX;
            }
            ep->mWoBleTest.mCommandTestResult.PacketCount++;
            break;
        case kDataType_END:
            ep->mWoBleTest.mCommandTestResult.PacketCount++;
            // falls thru
        case kDataType_ABORT:
        default:
            WeaveLogDebugBleEndPoint(Ble, "%s: Test Ended with test data type %d", __FUNCTION__, cmd.Payload.MsgTestData.Type);
            err = ep->mWoBleTest.DoCommandTestResult(kBleCommandTestResult_Reply, ep->mWoBleTest.mCommandTestResult.TestResult);
            ep->mWoBleTest.mCommandUnderTest = WOBLE_TEST_NONE;
            break;
        }
        needAck = cmd.Payload.MsgTestData.NeedAck;
        break;

    case kBleCommandType_TestRequest:
        memcpy(&ep->mWoBleTest.mCommandTestRequest, &cmd.Payload.MsgTestRequest, sizeof(BTCommandTypeTestRequest));
        WeaveLogError(Ble, "%s: PacketCount %lu, Duration %lu, TxGap %d, NeedAck %d, PayLoadSize %u", __FUNCTION__,
                      ep->mWoBleTest.mCommandTestRequest.PacketCount, ep->mWoBleTest.mCommandTestRequest.Duration,
                      ep->mWoBleTest.mCommandTestRequest.TxGap, ep->mWoBleTest.mCommandTestRequest.NeedAck,
                      ep->mWoBleTest.mCommandTestRequest.PayloadSize);
        // Start the test
        err = ep->mWoBleTest.HandleCommandTest(ep);
        SuccessOrExit(err);
        break;

    case kBleCommandType_TestResult:
        WeaveLogError(Ble, "\nIncoming TestResultOp : %d", cmd.Payload.MsgTestResult.TestResultOp);
        if (cmd.Payload.MsgTestResult.TestResultOp == kBleCommandTestResult_Request)
        {
            err = ep->mWoBleTest.DoCommandTestResult(kBleCommandTestResult_Reply, ep->mWoBleTest.mCommandTestResult.TestResult);
            LogBleTestResult(&ep->mWoBleTest.mCommandTestResult);
            SuccessOrExit(err);
        }
        else
            LogBleTestResult(&cmd.Payload.MsgTestResult);
        break;

    case kBleCommandType_WobleMTU:
        WeaveLogError(Ble, "\nIncoming WobleMTU : Op %u, Tx/Rx fragment size %u/%u", cmd.Payload.MsgWobleMTU.Op,
                      cmd.Payload.MsgWobleMTU.TxFragmentSize, cmd.Payload.MsgWobleMTU.RxFragmentSize);
        if (cmd.Payload.MsgWobleMTU.Op == kCmdType_Set)
        {
            if (cmd.Payload.MsgWobleMTU.TxFragmentSize > 0)
                ep->mWoBle.SetTxFragmentSize(cmd.Payload.MsgWobleMTU.TxFragmentSize);
            if (cmd.Payload.MsgWobleMTU.RxFragmentSize > 0)
                ep->mWoBle.SetRxFragmentSize(cmd.Payload.MsgWobleMTU.RxFragmentSize);
        }
        break;

    case kBleCommandType_WobleWindowSize:
        WeaveLogError(Ble, "\nIncoming WobleWindowSize : Op %u, Tx/Rx Window Sizes %u/%u", cmd.Payload.MsgWobleWindowSize.Op,
                      cmd.Payload.MsgWobleWindowSize.TxWindowSize, cmd.Payload.MsgWobleWindowSize.RxWindowSize);
        if (cmd.Payload.MsgWobleWindowSize.Op == kCmdType_Set)
        {
            if (cmd.Payload.MsgWobleWindowSize.TxWindowSize > 0)
                ep->SetTxWindowSize(cmd.Payload.MsgWobleWindowSize.TxWindowSize);
            if (cmd.Payload.MsgWobleWindowSize.RxWindowSize > 0)
                ep->SetRxWindowSize(cmd.Payload.MsgWobleWindowSize.RxWindowSize);
        }
        break;

    case kBleCommandType_TxTiming:
        err = ep->mWoBleTest.HandleCommandTxTiming(ep, cmd.Payload.MsgTxTiming.Enable);
        SuccessOrExit(err);
        break;

    default:
        WeaveLogError(Ble, "%s: Control type %d is not yet supported", __FUNCTION__, cmd.CmdHdr.PacketType);
        break;
    }

    if (needAck)
        err = ep->mWoBleTest.DoCommandSendAck(ep->mWoBle.RxPacketSeq(), err);
exit:
    if (err != BLE_NO_ERROR)
        WeaveLogError(Ble, "%s: err %d\n", __FUNCTION__, err);

    if (data != NULL)
    {
        PacketBuffer::Free(data);
    }

    return;
}

void WoBleTest::StopTestTimer()
{
    if (GetFlag(mEp->mTimerStateFlags, BLEEndPoint::kTimerState_UnderTestTimerRunnung))
    {
        // Cancel any existing test timer.
        mEp->mBle->mSystemLayer->CancelTimer(HandleTestClose, mEp);
        SetFlag(mEp->mTimerStateFlags, BLEEndPoint::kTimerState_UnderTestTimerRunnung, false);
    }
}

void WoBleTest::HandleTestClose(Weave::System::Layer * systemLayer, void * appState, Weave::System::Error err)
{
    BLEEndPoint * ep = static_cast<BLEEndPoint *>(appState);

    // Check for event-based timer race condition.
    if (GetFlag(ep->mTimerStateFlags, BLEEndPoint::kTimerState_UnderTestTimerRunnung))
    {
        WeaveLogError(Ble, "Test closed, ble ep %p", ep);
        SetFlag(ep->mTimerStateFlags, BLEEndPoint::kTimerState_UnderTestTimerRunnung, false);
    }

    ep->mWoBleTest.mCommandUnderTest = WOBLE_TEST_NONE;
    ep->mWoBleTest.mCommandTxTiming  = false;
}

// Initialize the Tx histogram table, if reset is true, the old content is discarded
int WoBleTest::InitTxHistogram(char * file, int count, bool reset)
{
    int err = 0;

    VerifyOrExit(count > 0, err = -1);

    // Check if it's called already
    if (mTxHistogram.File != NULL)
        fclose(mTxHistogram.File);

    mTxHistogram.File = fopen(file, reset ? "w" : "a");
    VerifyOrExit(mTxHistogram.File, err = -2);

    mTxHistogram.Record = (WoBleTxRecord *) malloc(count * sizeof(WoBleTxRecord));
    VerifyOrExit(mTxHistogram.Record, err = -3);

    mTxHistogram.Total = count;
    mTxHistogram.Idx   = 0;

exit:
    if (err)
    {
        WeaveLogError(Ble, "%s: err %d\n", __FUNCTION__, err);
        if (mTxHistogram.File)
        {
            fclose(mTxHistogram.File);
            mTxHistogram.File = NULL;
        }
    }

    return err;
}

// This function saves up to N records
void WoBleTest::SaveTxRecords(uint8_t N)
{
    if (N == 0 || mTxHistogram.Total == 0 || mTxHistogram.File == NULL)
    {
        WeaveLogDebugBleEndPoint(Ble, "%s: Nothing to write", __FUNCTION__);
        return;
    }

    if (N > mTxHistogram.Total)
        N = mTxHistogram.Total;

    WoBleTxRecord * record = &mTxHistogram.Record[0];

    while (N-- && record->TxStart)
    {
        // The records in histogram file: "TxStartTime  PayloadSize  TxTime"
        fprintf(mTxHistogram.File, "%u\t%u\t%u\n", record->TxStart, record->Payload, record->TxTime);
        record++;
    }

    // restart the recording
    mTxHistogram.Idx = 0;
}

void WoBleTest::AddTxRecord(uint32_t txStart, uint16_t txTime, uint16_t size)
{
    if (mTxHistogram.Total == 0)
    {
        WeaveLogDebugBleEndPoint(Ble, "%s: Tx Histogram was not enabled", __FUNCTION__);
        return;
    }

    WoBleTxRecord * record = &mTxHistogram.Record[mTxHistogram.Idx++];

    record->TxStart = txStart;
    record->TxTime  = txTime;
    record->Payload = size;

    if (mTxHistogram.Idx == 0 || mTxHistogram.Idx >= mTxHistogram.Total)
        SaveTxRecords(mTxHistogram.Total);
}

void WoBleTest::DoneTxHistogram(bool final)
{
    // Save the last records
    if (mTxHistogram.Idx > 0)
        SaveTxRecords(mTxHistogram.Idx);

    if (final)
    {
        if (mTxHistogram.File)
            fclose(mTxHistogram.File);
        if (mTxHistogram.Record)
            free((void *) mTxHistogram.Record);
        memset(&mTxHistogram, 0, sizeof(mTxHistogram));
    }
    else
        mTxHistogram.Idx = 0;
}

#endif /* WEAVE_ENABLE_WOBLE_TEST */

} /* namespace Ble */
} /* namespace nl */

#endif /* CONFIG_NETWORK_LAYER_BLE */