/*
 * Copyright (c) 2017-2019, Texas Instruments Incorporated
 * All rights reserved.
 *
 * IMPORTANT: Your use of this Software is limited to those specific rights
 * granted under the terms of a software license agreement between the user
 * who downloaded the software, his/her employer (which must be your employer)
 * and Texas Instruments Incorporated (the "License"). You may not use this
 * Software unless you agree to abide by the terms of the License. The License
 * limits your use, and you acknowledge, that the Software may not be modified,
 * copied or distributed unless embedded on a Texas Instruments microcontroller
 * or used solely and exclusively in conjunction with a Texas Instruments radio
 * frequency transceiver, which is integrated into your product. Other than for
 * the foregoing purpose, you may not use, reproduce, copy, prepare derivative
 * works of, modify, distribute, perform, display or sell this Software and/or
 * its documentation for any purpose.
 *
 * YOU FURTHER ACKNOWLEDGE AND AGREE THAT THE SOFTWARE AND DOCUMENTATION ARE
 * PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESS OR IMPLIED,
 * INCLUDING WITHOUT LIMITATION, ANY WARRANTY OF MERCHANTABILITY, TITLE,
 * NON-INFRINGEMENT AND FITNESS FOR A PARTICULAR PURPOSE. IN NO EVENT SHALL
 * TEXAS INSTRUMENTS OR ITS LICENSORS BE LIABLE OR OBLIGATED UNDER CONTRACT,
 * NEGLIGENCE, STRICT LIABILITY, CONTRIBUTION, BREACH OF WARRANTY, OR OTHER
 * LEGAL EQUITABLE THEORY ANY DIRECT OR INDIRECT DAMAGES OR EXPENSES
 * INCLUDING BUT NOT LIMITED TO ANY INCIDENTAL, SPECIAL, INDIRECT, PUNITIVE
 * OR CONSEQUENTIAL DAMAGES, LOST PROFITS OR LOST DATA, COST OF PROCUREMENT
 * OF SUBSTITUTE GOODS, TECHNOLOGY, SERVICES, OR ANY CLAIMS BY THIRD PARTIES
 * (INCLUDING BUT NOT LIMITED TO ANY DEFENSE THEREOF), OR OTHER SIMILAR COSTS.
 *
 * Should you have any questions regarding your right to use this Software,
 * contact Texas Instruments Incorporated at www.TI.com.
 *
 */

#include <TOF.h>

// Tof Patches
#include <ti/devices/DeviceFamily.h>
#include DeviceFamily_constructPath(rf_patches/rf_patch_mce_tof.h)
#include DeviceFamily_constructPath(rf_patches/rf_patch_rfe_tof.h)

#if defined(CC26X2)
#include DeviceFamily_constructPath(rf_patches/rf_patch_cpe_multi_protocol_rtls.h)
#elif defined(CC26XX_R2)
#include DeviceFamily_constructPath(rf_patches/rf_patch_cpe_tof.h)
#endif

#include DeviceFamily_constructPath(inc/hw_rfc_dbell.h)

#include "rfc_cmd_tof.h"
#include <ti/drivers/rf/RF.h>
#include <ti/sysbios/BIOS.h>
#include <ti/sysbios/hal/Hwi.h>
#include <ti/sysbios/knl/Swi.h>
#include "rtls_ctrl.h"

#include <stdint.h>
#include <stdlib.h>
#include <stdbool.h>

#define USE_TRACER

// Polynomial used for CRC-16 calculation
#define CRC16_POLYNOMIAL        0x1021

// Amount of constant syncwords planted in the syncword list that allow for recovery from sync loss
#define NUM_PASSIVE_SYNC_POINTS 1

// DC balanced constant syncword
#define TOF_CONSTANT_SW 0x5AF881CCF070AAA4

// TI-RTOS RF Mode Object
RF_Mode RF_Mode_tof =
{
  .rfMode = RF_MODE_PROPRIETARY,
#if defined(CC26X2)
  .cpePatchFxn = &rf_patch_cpe_multi_protocol_rtls,
#elif defined(CC26XX_R2)
  .cpePatchFxn = &rf_patch_cpe_tof,
#endif
  .mcePatchFxn = &rf_patch_mce_tof,
  .rfePatchFxn = &rf_patch_rfe_tof,
};

static uint32_t shape[6] = {
    ((19  << 12) | ( 410 )), // 500 ns pulse
    0,
    0,
    0,
    0,
    0,
};

#if defined(CC26XX)

#if defined(CC26XX_R2)
// Override list for the fast synth settings:
// Overrides for CMD_RADIO_SETUP
uint32_t pOverrides_2M[] =
{
  MCE_RFE_OVERRIDE(1,0,0,1,0,0),
  ADI_HALFREG_OVERRIDE(0,61,0xF,0x0),
  // Force gain setting
  HW_REG_OVERRIDE(0x6084,0x3588),
  // Rx: Set LNA IB trim value based on the selected defaultPhy.mainMode setting. (NOTE: The value 0x8 is a placeholder. The value to use should be set during run-time by radio driver function.)
  ADI_HALFREG_OVERRIDE(0,4,0xF,0x8),
  // Shaper settings:
  // Enable shaper, gain = 0, use shaped 15.4 modulation
  (uint32_t)0x00810083,
  // Force mdmTxIf = 0 on synth calibration
  (uint32_t)0x00000343,
  // Force mdmRxIf = 0 on synth calibration
  (uint32_t)0x00000323,
  // Set TX IF to zero and no dynamic TX IF calculation
  (uint32_t)0x000000a3,
  // Place the pointer at correct place in command
  (uint32_t)0xC0040031,
  // Pointer to the shape
  (uint32_t)shape,

  // Fast synth settings:
  // Synth: Use 24 MHz crystal, enable extra PLL filtering
  (uint32_t)0x02010403,
  // Synth: Set fine top and bottom code to 127 and 0
  HW_REG_OVERRIDE(0x4020, 0x7F00),
  // Synth: Configure faster calibration
  HW32_ARRAY_OVERRIDE(0x4004, 1),
  // Synth: Configure faster calibration
  (uint32_t)0x1C0C0618,
  // Synth: Configure faster calibration
  (uint32_t)0xC00401A1,
  // Synth: Configure faster calibration
  (uint32_t)0x21010101,
  // Synth: Configure faster calibration
  (uint32_t)0xC0040141,
  // Synth: Configure faster calibration
  (uint32_t)0x00214AD3,
  // Synth: Decrease synth programming time-out by 90 us (0x0298 RAT ticks = 166 us)
  (uint32_t)0x02980243,
  // End of fast synth settings

  // Set the correlator threshold to 0x68, default is 0x5A
  HW_REG_OVERRIDE(0x5108, 0x0068),
  // Set correlator for pre-correlation to 0x2A, default is 0x3737
  HW_REG_OVERRIDE(0x5104, 0x2A2A),
  // Elongate the slave RX/TX time by ~4 us
  HW_REG_OVERRIDE(0x51F8, 0x01F4),

#ifdef CACHE_AS_RAM
  0x00018063,
#endif //CACHE_AS_RAM

  // End of overrides
  (uint32_t)0xFFFFFFFF,
};

#elif defined(CC26X2)
uint32_t pOverrides_2M[] =
{
  MCE_RFE_OVERRIDE(1,0,0,1,0,0),
  HW_REG_OVERRIDE(0x6098,0x3588), // Force gain setting
  (uint32_t)0x00000343, // Force mdmTxIf = 0
  (uint32_t)0x00000323, // Force mdmRxIf = 0, will cause the synth to take into account when programming
  (uint32_t)0x000000a3, // Set TX IF to zero and no dynamic TX IF calculation
  (uint32_t)0x00810083, // Enable shaper, gain = 0, use shaped 15.4 modulation
  (uint32_t)0xC0040031, // Place the pointer at correct place in command
  (uint32_t)shape,      // Pointer to the shape
  HW_REG_OVERRIDE(0x5118, 0x0068), // default = 0x005A
  HW_REG_OVERRIDE(0x5114, 0x2A2A), // default = 0x3737
  //If we want to shorten the time for slave turnaround, we can mess with this override, writing to BRMACC0. turnaround is this value at 24 MHz
  HW_REG_OVERRIDE(0x5264, 0x0340), // DEFAULT: 0x190

  (uint32_t)0xFFFFFFFF,
};
#endif// <board>

#endif //CC26XX

rfc_CMD_SYNC_START_RAT_t RF_cmdStartRAT =
{
 .commandNo = 0x080A,
 .status = 0x0000,
 .pNextOp = 0,                        // INSERT APPLICABLE POINTER: (uint8_t*)&xxx
 .startTime = 0x00000000,
 .startTrigger.triggerType = 0x0,
 .startTrigger.bEnaCmd = 0x0,
 .startTrigger.triggerNo = 0x0,
 .startTrigger.pastTrig = 0x0,
 .condition.rule = 0x1,
 .condition.nSkip = 0x0,

 .__dummy0 = 0x00,
 .rat0 = 0x0000,
};

// CMD_RADIO_SETUP
rfc_CMD_RADIO_SETUP_t RF_cmdRadioSetup =
{
  .commandNo = 0x0802,
  .status = 0x0000,
  .pNextOp = (rfc_radioOp_t*)&RF_cmdStartRAT,
  .startTime = 0x00000000,
  .startTrigger.triggerType = 0x0,
  .startTrigger.bEnaCmd = 0x0,
  .startTrigger.triggerNo = 0x0,
  .startTrigger.pastTrig = 0x0,
  .condition.rule = 0x1,
  .condition.nSkip = 0x0,
  .mode = 0x02,
#if defined(CC26XX_R2)
  .__dummy0 = 0x00,
#endif
  .config.frontEndMode = 0x0,
  .config.biasMode = 0x0,
  .config.analogCfgMode = 0x0,
  .config.bNoFsPowerUp = 0x0,
  .txPower = 0x9330,
  .pRegOverride = 0,
};

// CMD_TOF
rfc_CMD_TOF_t RF_cmdTof =
{
  .commandNo = 0x0811,
  .status = 0x0000,
  .pNextOp = 0,
  .startTime = 0x00000000,
  .startTrigger.triggerType = 0x0,
  .startTrigger.bEnaCmd = 0x0,
  .startTrigger.triggerNo = 0x0,
  .startTrigger.pastTrig = 0x0,
  .condition.rule = 0x1,
  .condition.nSkip = 0x0,
};

// CMD_FS
rfc_CMD_FS_t RF_cmdFs =
{
  .commandNo = 0x0803,
  .status = 0x0000,
  .pNextOp = 0,
  .startTime = 0x00000000,
  .startTrigger.triggerType = 0x0,
  .startTrigger.bEnaCmd = 0x0,
  .startTrigger.triggerNo = 0x0,
  .startTrigger.pastTrig = 0x0,
  .condition.rule = 0x1,
  .condition.nSkip = 0x0,
  .frequency = 2432,
  .fractFreq = 0x0000,
  .synthConf.bTxMode = 0x1,
  .synthConf.refFreq = 0x0,
  .__dummy0 = 0x00,
  .__dummy1 = 0x00,
  .__dummy2 = 0x00,
  .__dummy3 = 0x0000,
};

#define CMD_READ_FS_CAL 0x000D                //!< Read frequency calibration command

/// @brief RF Read frequency calibration command Structure
struct __RFC_STRUCT rfc_CMD_READ_FS_CAL_s
{
  uint16_t commandNo;   //!< The command ID number 0x0601
  uint8_t __dummy0;     //!< Reserved
  uint8_t coarseCal;    //!< Coarse calibration result
  uint8_t midCal;       //!< Mid calibration result
  uint8_t ktCal;        //!< KT calibration result
  uint16_t tdcCal;      //!< TDC calibration result
};
typedef struct __RFC_STRUCT rfc_CMD_READ_FS_CAL_s rfc_CMD_READ_FS_CAL_t; //!< RF Read frequency calibration command Structure

// Immediate command to read calibration settings
rfc_CMD_READ_FS_CAL_t RF_cmdReadFsCal =
{
  .commandNo = 0x000D,
  .__dummy0 = 0,
  .coarseCal = 0,
  .midCal = 0,
  .ktCal = 0,
  .tdcCal = 0
};

// Last RF event mask
uint32_t g_lastRfHwiEventMask;

// ToF application callback - will be called when a ToF run is complete (all syncwords exhausted)
TOF_ApplicationCB tofApplicationCB = NULL;

// RF Callback - RF Driver will call this function
RF_Callback pRfCallbackFxn = NULL;

// Security handle for tof_security module
tofSecHandle_t gTofSecHandle;

// Holds ToF synchronization control block
ToF_Sync gTofSyncCB = {0};

#ifdef RTLS_TOF_DEBUG
ToF_InternalStatistics gTofStatistics = {0};
#endif

void TOF_calibrateSynth(ToF_Handle handle, uint16_t *freqList, uint8_t numFreq);
void TOF_tofCallback(RF_Handle h, RF_CmdHandle ch, RF_EventMask e);
void TOF_runImmediateWrapperFs(RF_Handle h, RF_CmdHandle ch, RF_EventMask e);
void TOF_handleOutOfSync(void);
void TOF_genSyncwordBatches(uint16_t numBatches);
uint8_t TOF_genSyncWords(uint8_t bufferToFill);
TofSyncState_e TOF_getSyncStatus(void);
uint16_t TOF_crc16(uint16_t crc, uint8_t *pAddr, uint32_t len);
TofHandshakeState_e TOF_handleTofHandshake(TofHandshakeState_e currentState);

/*********************************************************************
 * @fn      TOF_tofCallback
 *
 * @brief   Callback for RF
 *
 * @param   h  - RF Handle.
 * @param   ch - RF Command Handle.
 * @param   e  - RF Event mast.
 *
 * @return  none
 */
void TOF_tofCallback(RF_Handle h, RF_CmdHandle ch, RF_EventMask e)
{
  g_lastRfHwiEventMask = e;

  if (e & RF_EventRxOk)
  {    
    if ((tofApplicationCB != NULL))
    {
      // Every time we generate a batch of syncwords we increment internal counter (all nodes)
      gTofSyncCB.internalCtr++;

      // Master needs to broadcast his current internal counter
      if (RF_cmdTof.bMaster == ToF_ROLE_MASTER)
      {
        uint16_t crc;
        uint32_t tempCtr = gTofSyncCB.internalCtr;

        // Calculate CRC
        crc = TOF_crc16(0, (uint8_t *)&tempCtr, sizeof(uint16_t));
        gTofSyncCB.tofBroadcast.msg = (uint32_t)((uint32_t)crc + (uint32_t)(gTofSyncCB.internalCtr << 16));
      }

      // Notify the upper layers that the ToF run is complete
      tofApplicationCB();

      // If we got here then we're in sync
      gTofSyncCB.currentSyncState = TOF_SYNC_SYNCHRONIZED;

      // ToF run is completed successfully mark handshake state for the next run
      gTofSyncCB.handshakeState = TOF_HANDSHAKE_STARTED;

      // Calculate prior to the ToF run, how many RF_EventDoubleSyncWordBufferSwitch are going to be produced by RF
      gTofSyncCB.numBuffSwitchRemaining = RF_cmdTof.numIterations/(gTofSecHandle.numOfSyncWordsPerBuffer);

      // This run was successful, generate initial words for next run
      TOF_genSyncwordBatches(1);

#ifdef RTLS_TOF_DEBUG
      gTofStatistics.numGoodTofRuns++;
#endif
    }
  }
  else if (e & RF_EventDoubleSyncWordBufferSwitch)
  { 
    // Try to get sync status - processed in RxNOk or RxOk
    gTofSyncCB.currentSyncState = TOF_getSyncStatus();

    if (RF_cmdTof.bMaster != ToF_ROLE_MASTER && gTofSyncCB.handshakeState != TOF_HANDSHAKE_COMPLETE)
    {
      gTofSyncCB.handshakeState = TOF_handleTofHandshake(gTofSyncCB.handshakeState);
    }

    if ((RF_cmdTof.bMaster != ToF_ROLE_MASTER && gTofSyncCB.handshakeState != TOF_HANDSHAKE_RECOVERING) ||
        RF_cmdTof.bMaster == ToF_ROLE_MASTER)
    {
      if (gTofSyncCB.numBuffSwitchRemaining != 0)
      {
        gTofSyncCB.numBuffSwitchRemaining--;
      }

      TOF_genSyncwordBatches(1);
    }
  }
  else if (e & RF_EventRxNOk)
  {
    // Handle Passive going out of sync with Master/Slave
    if (RF_cmdTof.bMaster == ToF_ROLE_PASSIVE && gTofSyncCB.currentSyncState == TOF_SYNC_OUT_OF_SYNC)
    {
      TOF_handleOutOfSync();
    }

    // Once we enter this section there are two options:
    // 1. ToF handshake could not be completed -> mark that it failed and recover in the next run
    // 2. We couldn't finish the ToF run (but we could finish the handshake), generate batches until all nodes are aligned for the next run
    if (gTofSyncCB.numBuffSwitchRemaining > 0 &&
       (gTofSyncCB.numBuffSwitchRemaining != RF_cmdTof.numIterations/(gTofSecHandle.numOfSyncWordsPerBuffer)))
    {
      // Check if Master and Slave managed to complete a handshake
      // If we got here only after 1 buffer switch then the handshake certainly failed
      if (RF_cmdTof.bMaster != ToF_ROLE_MASTER && gTofSyncCB.numBuffSwitchRemaining == (RF_cmdTof.numIterations/(gTofSecHandle.numOfSyncWordsPerBuffer)) - 1)
      {
        // After this pointer switching we will have the buffers #1,#2 loaded to the RF and #3 stored in a temp buffer
        RF_cmdTof.pSyncWord = (uint64_t *)gTofSecHandle.pTempSyncWordBuffer;

        // Handshake failed
        gTofSyncCB.handshakeState = TOF_HANDSHAKE_FAILED;

#ifdef RTLS_TOF_DEBUG
        gTofStatistics.failedHandshakes++;
#endif
      }
      else
      {
        // We did not complete a full run (numBuffSwitchRemaining > 0), generate the rest so all nodes are aligned for the next run
        TOF_genSyncwordBatches(gTofSyncCB.numBuffSwitchRemaining);

        // A full run was generated
        gTofSyncCB.internalCtr++;

        // ToF run is complete, a new handshake will begin in the next run
        gTofSyncCB.handshakeState = TOF_HANDSHAKE_STARTED;

        // Calculate prior to the ToF run, how many RF_EventDoubleSyncWordBufferSwitch are going to be produced by RF
        gTofSyncCB.numBuffSwitchRemaining = RF_cmdTof.numIterations/(gTofSecHandle.numOfSyncWordsPerBuffer);
      }
    }

    // The whole run failed
#ifdef RTLS_TOF_DEBUG
    gTofStatistics.numBadTofRuns++;
#endif
  }
}


/*********************************************************************
 * @fn      TOF_open
 *
 * @brief   Initiate RF ToF params and open RF driver
 *
 * @param   tof    - ToF structure initiated by user application
 * @param   params - User provided params
 *
 * @return  ToF Handle - Pointer to a ToF_Object
 */
ToF_Handle TOF_open(ToF_Struct *tof, ToF_Params *params)
{
  // Default values
  params->syncTimeout         = 2500*4;
  params->freqChangePeriod    = 2;      // Switch frequency for each 2 packets
  params->pRxBuf              = NULL;
  params->pTxBuf              = NULL;

  tof->freqChangePeriod       = params->freqChangePeriod;
  tof->pFrequencies           = params->pFrequencies;
  tof->numSyncwordsPerBurst   = params->numSyncwordsPerBurst;
  tof->numFreqs               = params->numFreq;
  tof->pT1RSSIBuf             = params->pT1RSSIBuf;
  tof->tofRole                = params->tofRole;
  tof->pTxBuf                 = params->pRxBuf;
  tof->pRxBuf                 = params->pTxBuf;
  tof->slaveLqiFilter         = params->slaveLqiFilter;
  tof->postProcessLqiThresh   = params->postProcessLqiThresh;
  tof->postProcessMagnRatio   = params->postProcessMagnRatio;

  // ToF Security
  if (TOF_SEC_SUCCESS_E != TOFSecurity_paramsInit(&gTofSecHandle, &(params->tofSecurityParams)))
  {
    return NULL;
  }

  if (params->tofRole == ToF_ROLE_MASTER)
  {
    if (TOF_SEC_SUCCESS_E != TOFSecurity_open(&gTofSecHandle, NULL))
    {
      return NULL;
    }
  }

  // Initialize
  RF_Params rfParams;
  RF_Params_init(&rfParams);
  RF_cmdTof = (rfc_CMD_TOF_t){0};                                                              // Clear all values

  rfParams.nPowerUpDuration      = 4500;
  rfParams.nInactivityTimeout    = BIOS_WAIT_FOREVER;

  // User configurable
  RF_cmdTof.syncTimeout          = params->syncTimeout;                                        // Timeout 0.250us ticks for first sync word. In order for RF core to stay in RX at first for longer
  RF_cmdTof.pT1RSSIBuf           = params->pT1RSSIBuf;                                         // Pointer to result buffer
  RF_cmdTof.numIterations        = params->numSyncwordsPerBurst;                               // Number of iterations of the sync word buffer. Since sync word is changed between ping/ack, it needs to be double the amount of packets.
  RF_cmdTof.missedPktLimit       = TOF_SEC_DBL_BUFF_SIZE - 2;                                  // Limit for missed packets. Reached limit will cause RF core to return
  RF_cmdTof.pBufTx               = params->pTxBuf;                                             // Pointer to Tx buffer. If NULL then the buffer will be ignored by the RF core.
  RF_cmdTof.pBufRx               = params->pRxBuf;                                             // Pointer to Rx buffer. If NULL then the buffer will be ignored by the RF core.
  RF_cmdTof.pSyncWord            = (uint64_t *)gTofSecHandle.pSyncWordBuffer1;                 // Pointer to sync words
  RF_cmdTof.pSyncWord2           = (uint64_t *)gTofSecHandle.pSyncWordBuffer2;                 // Pointer to the double buffer
  RF_cmdTof.bMaster              = params->tofRole;                                            // 2 -> Passive ,1 -> Master, 0 -> Slave
  RF_cmdTof.period               = params->freqChangePeriod;                                   // Period of frequency change in packets. Should be at least 2.
  RF_cmdTof.missedSW0Limit       = TOF_SEC_DBL_BUFF_SIZE - 2;                                  // Limit for number of missed first packets
  RF_cmdTof.resultReadyFrequency = 0xFFFF;                                                     // Partial read out not used here, set the partial readout to occur very(!) seldom -> no partial readout
  RF_cmdTof.useCompensation      = TOF_COMPENSATION_CORRELATOR_COMP;
  RF_cmdTof.txRxBufferLen        = 0;                                                          // Set buffer length to max. Needs to be set in accordance to pTxBuf and pRxBuf length.

  // Constant configuration
  RF_cmdTof.commandNo            = 0x6801;
  RF_cmdTof.condition.rule       = COND_NEVER;                                                 // When to run next command
  RF_cmdTof.endTime              = 0;                                                          // Timeout per transmission set individually for slave and master below!
  RF_cmdTof.txPower              = 0x3f;                                                       // MAX output power, not temperature compensated!
  RF_cmdTof.timeslotTimeout      = 400*4;                                                      // Timeslot delay. should be enough to also include timeouts.
  RF_cmdTof.seed                 = 0;                                                          // Seed for LFSR random frequency hopping 0 = don't use LFSR, iterate linearly instead
  RF_cmdTof.mask                 = 0x59;                                                       // Mask for LFSR random frequency hopping.
  RF_cmdTof.freqCalibTime        = 105*4;                                                      // Calibration time for the fast frequency change. Will be different depending on freq calib settings. Incluing LFSR calculation time
  RF_cmdTof.payloadLen           = 32;                                                         // Payload length for
  RF_cmdTof.lqiThreshold         = params->slaveLqiFilter;                                     // LQI threshold for ToF Slave
  RF_cmdTof.bExportLQI_MagnDiff  = TRUE;                                                       // If true, the RFcore will export LQI and magnitude difference as well

  // Double buffer used - Get the length of the double buffer
  RF_cmdTof.doubleBufferLen = gTofSecHandle.numOfSyncWordsPerBuffer;

  // Conf 2M override
  RF_cmdRadioSetup.pRegOverride = pOverrides_2M;
  RF_cmdRadioSetup.mode = 2;

  // Set up the RF commands depending on master/slave/passive
  if (RF_cmdTof.bMaster == ToF_ROLE_MASTER)
  {
    // How long to wait for sync, in ticks of 0.250us
    #if defined(CC26X2)
        RF_cmdTof.endTime = RF_cmdTof.timeslotTimeout - 70*4;
    #elif defined(CC26XX_R2)
        RF_cmdTof.endTime = RF_cmdTof.timeslotTimeout - 50*4;
    #endif
  }
  else if (RF_cmdTof.bMaster == ToF_ROLE_PASSIVE)
  {
    // How long to wait for sync, in ticks of 0.250us. Could be trimmed to be less!
    RF_cmdTof.endTime = RF_cmdTof.timeslotTimeout - 70*4;
    RF_cmdTof.syncTimeout = 2500*4;
  }
  else if (RF_cmdTof.bMaster == ToF_ROLE_SLAVE)
  {
    // How long to wait for sync, in ticks of 0.250us.
    RF_cmdTof.endTime = RF_cmdTof.timeslotTimeout - 70*4;
    RF_cmdTof.syncTimeout = 2500*4;
  }

  /* Open radio driver */
  tof->rfHandle = RF_open(&tof->rfObject, &RF_Mode_tof, (RF_RadioSetup*)&RF_cmdRadioSetup, &rfParams);

  if (tof->rfHandle)
  {
    if (params->pfnTofApplicationCB != NULL)
    {
      tofApplicationCB = params->pfnTofApplicationCB;
    }

    pRfCallbackFxn = TOF_tofCallback;

    // Zeroize sync control block
    memset(&gTofSyncCB, 0, sizeof(gTofSyncCB));

    // Configure ToF synchronization
    if (RF_cmdTof.bMaster == ToF_ROLE_MASTER || RF_cmdTof.bMaster == ToF_ROLE_PASSIVE)
    {
      if (RF_cmdTof.bMaster == ToF_ROLE_MASTER)
      {
        // Initialize buffers, the rest is handled in the callback
        gTofSyncCB.lastBuffFilled = 2;
        TOF_genSyncwordBatches(2);
      }

      // How many chances the passive has to read
      gTofSyncCB.tofBroadcast.maximumNumberOfSyncs = NUM_PASSIVE_SYNC_POINTS;

      // Send broadcast every batch
      gTofSyncCB.tofBroadcast.period = gTofSecHandle.numOfSyncWordsPerBuffer;

      // This is the constant word that will be transmitted by the Master to allow the Passive a chance to hear the shared counter
      gTofSyncCB.tofBroadcast.syncWord = TOF_CONSTANT_SW;

      // Initial counter is 0
      gTofSyncCB.tofBroadcast.msg = 0;

      // Handshake starts at started
      gTofSyncCB.handshakeState = TOF_HANDSHAKE_STARTED;

      // Set broadcast structure for RF
      RF_cmdTof.BC = &gTofSyncCB.tofBroadcast;
    }

    return (ToF_Handle)tof;
  }
  else
  {
    return NULL;
  }
}

/*********************************************************************
 * @fn      TOF_run
 *
 * @brief   Start a ToF run
 *
 * @param   handle - ToF Handle.
 * @param   nextTaskTime - Time remaining until next Stack event.
 *
 * @return  ToF Status
 */
ToF_Status TOF_run(ToF_Handle handle, uint32_t nextTaskTime)
{
  RF_ScheduleCmdParams cmdParams =
  {
    0,
    RF_StartNotSpecified,
    RF_AllowDelayAny,
    0,
    RF_EndNotSpecified,
    0,
    0
  };

  ToF_Status ret = {0};

  // Set number of syncwords per burst
  RF_cmdTof.numIterations = handle->numSyncwordsPerBurst;

  // Both buffers should be marked as ready at this point
  RF_cmdTof.syncwordBufferReady = syncWordBufferOneReady | syncWordBufferTwoReady;

  // Set number of frequencies
  RF_cmdTof.numFreq = handle->numFreqs;

  // Run frequency calibration if needed
  for (int i = 0; i < handle->numFreqs; ++i)
  {
    if (handle->synthCal[i].freq != handle->pFrequencies[i])
    {
      TOF_calibrateSynth(handle, handle->pFrequencies, handle->numFreqs);
      break;
    }
  }

  // Schedule the ToF run
  RF_cmdTof.startTime = RF_getCurrentTime() + RF_convertUsToRatTicks(1500);
  RF_cmdTof.startTrigger.triggerType = TRIG_ABSTIME;
  RF_cmdTof.startTrigger.pastTrig = 0;

  if (RF_cmdTof.bMaster == ToF_ROLE_PASSIVE)
  {
    RF_cmdTof.startTime = RF_getCurrentTime() + RF_convertUsToRatTicks(1000);
  }

  if (RF_cmdTof.bMaster == ToF_ROLE_MASTER)
  {
    RF_cmdTof.startTime = RF_cmdTof.startTime + RF_convertUsToRatTicks(1500);
  }

  cmdParams.endTime = RF_cmdTof.startTime + RF_convertUsToRatTicks(nextTaskTime - 5500);

  // Will return immediately
  RF_scheduleCmd(handle->rfHandle, (RF_Op*)&RF_cmdTof, &cmdParams, pRfCallbackFxn, RF_EventRxOk | RF_EventRxNOk | RF_EventDoubleSyncWordBufferSwitch | RF_EventLastCmdDone);

  ret.rfEvent = g_lastRfHwiEventMask,
  ret.ratSyncTime = RF_cmdTof.SW0Timestamp,

  // Reset sync time before next run
  RF_cmdTof.SW0Timestamp = 0;

  return ret;
}

/*********************************************************************
 * @fn      TOF_getBurstStat
 *
 * @brief   Get the result for the last ToF burst (run)
 *
 * @param   handle - ToF Handle.
 * @param   stats  - The ToF stats for the last run.
 *
 * @return  none
 */
void TOF_getBurstStat(ToF_Handle handle, ToF_BurstStat **resultBuffer)
{
  // Get the average
  ToF_Sample *pSample = (ToF_Sample *)handle->pT1RSSIBuf;
  ToF_BurstStat *stats = *resultBuffer;
  double magnRatio = (double)(handle->postProcessMagnRatio)/(100.0);
  uint32_t samplesPerBurst = handle->numSyncwordsPerBurst / 2;

  // Clean results buffer
  memset(stats, 0, handle->numFreqs * sizeof(ToF_BurstStat));

  for (uint32_t i = 0; i < samplesPerBurst; i++, pSample++)
  {
    uint32_t t1 = pSample->T1;

    double magn1DivMagn2 = (double)(pSample->rssiVal1)/(double)(pSample->rssiVal2);
    double magn2DivMagn1 = (double)(pSample->rssiVal2)/(double)(pSample->rssiVal1);

    if (t1 != TOF_TIMEOUTVAL && t1 != 0)
    {
      if (handle->tofRole == ToF_ROLE_MASTER)
      {
        if ((pSample->LQI_from_slave <= handle->postProcessLqiThresh) &&
            (magn1DivMagn2 < magnRatio) && (magn2DivMagn1 < magnRatio))
        {
          stats[pSample->freqIndex].tick += pSample->T1 / 256;
          stats[pSample->freqIndex].numOk++;
        }
      }
      else if (handle->tofRole == ToF_ROLE_PASSIVE)
      {
        if ((pSample->LQI_from_slave <= handle->postProcessLqiThresh) && (pSample->LQI_from_master <= handle->postProcessLqiThresh) &&
            (magn1DivMagn2 < magnRatio) && (magn2DivMagn1 < magnRatio))
        {
          stats[pSample->freqIndex].tick += pSample->T1 / 256;
          stats[pSample->freqIndex].numOk++;
        }
      }
    }
  }

  // This calculates the basic average per frequency (over only good samples)
  for (uint32_t i = 0; i < handle->numFreqs; ++i)
  {
    stats[i].freq = handle->synthCal[i].freq;
    stats[i].tick /= stats[i].numOk;
  }

  // Find the variance
  pSample = (ToF_Sample *)handle->pT1RSSIBuf;
  for (uint32_t i = 0; i < handle->numSyncwordsPerBurst / 2; i++, pSample++)
  {
    uint32_t t1 = pSample->T1;
    if (t1 != TOF_TIMEOUTVAL && t1 != 0)
    {
      double dev = pSample->T1 / 256 - stats[pSample->freqIndex].tick;
      stats[pSample->freqIndex].tickVariance += dev * dev;
    }
  }

  for (uint32_t i = 0; i < handle->numFreqs; ++i)
  {
    stats[i].tickVariance /= stats[i].numOk;
  }
}

/*********************************************************************
 * @fn      TOF_close
 *
 * @brief   Close the ToF and RF drivers
 *
 * @param   handle - ToF Handle.
 *
 * @return  none
 */
void TOF_close(ToF_Handle handle)
{
  volatile uint32_t keyHwi;

  // Delete synth calibrations
  handle->synthCal[0].freq = 0;

  // Close RF Handle
  RF_close(handle->rfHandle);

  // Close ToF Security
  TOFSecurity_close(&gTofSecHandle);

  // Free Tof Handle
  if (handle->pFrequencies)
  {
    RTLSUTIL_FREE(handle->pFrequencies);
  }

  if (handle->pT1RSSIBuf)
  {
    RTLSUTIL_FREE(handle->pT1RSSIBuf);
  }
}

/*********************************************************************
 * @fn      TOF_clearBuffers
 *
 * @brief   Clean buffers for the specified ToF Handle
 *
 * @param   handle - ToF Handle.
 *
 * @return  none
 */
void TOF_clearBuffers(ToF_Handle handle)
{
  ToF_Sample *pSamples = (ToF_Sample *)handle->pT1RSSIBuf;

  for (int i = 0 ; i < handle->numSyncwordsPerBurst / 2 ; i++)
  {
    //Reset T1SampleBuf to T1 = timeout value =0xffff | RSSI = 0
    pSamples[i].T1 = TOF_TIMEOUTVAL;
    pSamples[i].RSSI = 0xA;
  }
}

/*********************************************************************
 * @fn      TOF_calibrateSynth
 *
 * @brief   This function is used to pre-calibrate the synth for fast frequency changes.
 *          In order for the fast changing to work, ToF_run must start at the "middle" frequency
 *          so that the minimal offset between outer frequencies and start frequency if obtained.
 *
 * @param   handle   - ToF Handle.
 * @param   freqList - List of frequencies that will be used for a ToF run.
 * @param   numFreq  - Amount of frequencies.
 *
 * @return  none
 */
void TOF_calibrateSynth(ToF_Handle handle, uint16_t *freqList, uint8_t numFreq)
{
  RF_ScheduleCmdParams cmdParams =
  {
    0,
    RF_StartNotSpecified,
    RF_AllowDelayAny,
    0,
    RF_EndNotSpecified,
    0,
    0
  };

  // Allow max TOF_MAX_NUM_FREQ frequencies
  numFreq = numFreq < TOF_MAX_NUM_FREQ ? numFreq : TOF_MAX_NUM_FREQ;

  // Calibrate for all frequencies:
  for (int i = 0 ; i < numFreq ; i++)
  {
    // Set frequency in struct:
    RF_cmdFs.frequency = freqList[i];
    // Run cmd_Fs and wait for done:

    RF_cmdFs.startTime = RF_getCurrentTime();
    RF_cmdFs.startTrigger.triggerType = TRIG_ABSTIME;
    RF_cmdFs.startTrigger.pastTrig = 1;
    cmdParams.endTime = RF_cmdFs.startTime + RF_convertUsToRatTicks(300);

    RF_runScheduleCmd(handle->rfHandle, (RF_Op*)&RF_cmdFs, &cmdParams, &TOF_runImmediateWrapperFs, 0);

    // Copy the results over to struct which is passed to RF core:
    handle->synthCal[i].freq = freqList[i]; // Save the current frequency
    handle->synthCal[i].mid = RF_cmdReadFsCal.midCal;
    handle->synthCal[i].coarse = RF_cmdReadFsCal.coarseCal;
    handle->synthCal[i].kt = RF_cmdReadFsCal.ktCal;
    handle->synthCal[i].tdc = RF_cmdReadFsCal.tdcCal;
  }

  // Provide RF_cmdToF with pointer to calibration data:
  RF_cmdTof.freqCalibList = handle->synthCal;
}

/*********************************************************************
 * @fn      TOF_runImmediateWrapperFs
 *
 * @brief   A wrapper for runImmediateCmd. This routine is used as a callback
 *          to RF_scheduleCmd. Once RF_scheduleCmd is finished, this routine will
 *          be immediately called and RF_runImmediateCmd will be called subsequently.
 *
 * @param   h  - RF Handle.
 * @param   ch - RF Command Handle.
 * @param   e  - RF Event mast.
 *
 * @return  none
 */
void TOF_runImmediateWrapperFs(RF_Handle h, RF_CmdHandle ch, RF_EventMask e)
{
  RF_runImmediateCmd(h, (uint32_t*)&RF_cmdReadFsCal);
}

/*********************************************************************
* @fn       TOF_GenSyncWords
*
* @brief    Will fill Sync Word Buffers with <numOfSyncWordsPerBuffer> Sync Words
*
* @param    handle - ToF Handle.
*
* @return  None
*/
uint8_t TOF_genSyncWords(uint8_t bufferToFill)
{
  uint8_t res;

  res = TOFSecurity_genSyncWords(&gTofSecHandle, bufferToFill);

  if (TOF_SEC_SUCCESS_E == res)
  {
    switch(bufferToFill)
    {
      case TOF_SEC_FILL_1ST_BUFF:
      {
        RF_cmdTof.syncwordBufferReady |= syncWordBufferOneReady;
      }
      case TOF_SEC_FILL_2ND_BUFF:
      {
        RF_cmdTof.syncwordBufferReady |= syncWordBufferTwoReady;
      }
      case TOF_SEC_FILL_BOTH_BUFFS:
      {
        RF_cmdTof.syncwordBufferReady = syncWordBufferOneReady | syncWordBufferTwoReady;
      }
    }
  }
  return res;
}

/*********************************************************************
* @fn      TOF_setSeed
*
* @brief   call ToF Security module to set the AESCTRDRBG with a
*          new 128-bit random seed
*
*          Note: This function can be used only by RTLS Passive and
*                 RTLS Slave. It cannot be used by RTLS Master.
*
* @param   handle - ToF Handle.
*
* @return  int - TOF_SEC_SUCCESS_E if succeeded, other if failed
*/
int TOF_setSeed(uint8_t* newSeed)
{
  int result;

  result = TOFSecurity_open(&gTofSecHandle, newSeed);
  if (result == TOF_SEC_SUCCESS_E)
  {
    gTofSyncCB.lastBuffFilled = 2;
    TOF_genSyncwordBatches(2);
  }

  return result;
}

/*********************************************************************
* @fn      TOF_getSeed
*
* @brief   copy 32 byte seed into dst
*
* @param   dst - destination to copy seed into.
*
* @return  None
*/
uint8_t TOF_getSeed(uint8_t *dst)
{
  if (NULL != dst)
  {
    memcpy(dst, &gTofSecHandle.seed, AESCTRDRBG_SEED_LENGTH_AES_128);
  }
  else
  {
    return FAILURE;
  }
  return SUCCESS;
}

/*********************************************************************
* @fn      TOF_genSyncwordBatches
*
* @brief   Skip some syncwords into the future (for synchronization purposes)
*
* @param   numBatches - how many batches to generate
*
* @return  None
*/
void TOF_genSyncwordBatches(uint16_t numBatches)
{
  for (int i = 0; i < numBatches; i++)
  {
    // If the RF core is not currently using buffer 2, OK to fill it
    if (gTofSyncCB.lastBuffFilled == 1)
    {
      // Using sync word buffer 1 currently, fill the second buffer
      TOF_genSyncWords(TOF_SEC_FILL_2ND_BUFF);

      gTofSyncCB.lastBuffFilled = 2;
    }
    // Else if the RF core is not currently using buffer 1, OK to fill it
    else if (gTofSyncCB.lastBuffFilled == 2)
    {
      // Using sync word buffer 2 currently, fill the first buffer
      TOF_genSyncWords(TOF_SEC_FILL_1ST_BUFF);

      gTofSyncCB.lastBuffFilled = 1;
    }
  }
}

/*********************************************************************
* @fn      TOF_getSyncStatus
*
* @brief   Check if we are in sync or not (read the shared counter)
*
* @param   none
*
* @return  TOF_SYNC_SYNCHRONIZED - we are in sync, TOF_OUT_OF_SYNC - we are out of sync, TOF_SYNC_WAIT_FOR_SYNC - we are not sure
*/
TofSyncState_e TOF_getSyncStatus(void)
{
  uint16_t sharedCrc;
  uint32_t sharedCtr;
  uint16_t crcRes;

  // Master and Slave are always in sync
  if (RF_cmdTof.bMaster == ToF_ROLE_MASTER || RF_cmdTof.bMaster == ToF_ROLE_SLAVE)
  {
    return TOF_SYNC_SYNCHRONIZED;
  }

  // Get the shared counter
  if (RF_cmdTof.BC->rxMsg != NULL && RF_cmdTof.BC->rxMsg != 0xFFFFFFFF)
  {
    // Get shared CRC
    sharedCrc = (uint16_t)gTofSyncCB.tofBroadcast.rxMsg & 0x0000FFFF;

    // Calculate CRC on shared Ctr
    sharedCtr = gTofSyncCB.tofBroadcast.rxMsg >> 16;
    crcRes = TOF_crc16(0, (uint8_t *)&sharedCtr, sizeof(uint16_t));

    if (sharedCrc != crcRes)
    {
      // CRC is incorrect, we don't know what is happening
      return TOF_SYNC_WAIT_FOR_SYNC;
    }

    // Save the last counter that we were able to read
    gTofSyncCB.lastSharedCtr = sharedCtr;

    // If shared counter is greater than our internal then we are out of sync
    if (gTofSyncCB.lastSharedCtr > gTofSyncCB.internalCtr)
    {
      return TOF_SYNC_OUT_OF_SYNC;
    }
    else if (gTofSyncCB.internalCtr > gTofSyncCB.lastSharedCtr)
    {
      // This state should not be reached
      // If our internal counter is greater than shared, then we need to wait (no generation of new words)
      return TOF_SYNC_WAIT_FOR_SYNC;
    }

    // Counters are equal
    return TOF_SYNC_SYNCHRONIZED;
  }

  return TOF_SYNC_WAIT_FOR_SYNC;
}

/*********************************************************************
* @fn      TOF_handleOutOfSync
*
* @brief   Handle a situation where ToF Passive goes out of sync
*          This is done by comparing the shared counter derived from ToF Master with
*          our own internal counter and then skipping the right amount of syncwords
*          to regain sync
*
* @param   none
*
* @return  none
*/
void TOF_handleOutOfSync(void)
{
  int16_t skipAmount;

  // Calculate the amount of batches we need to generate
  skipAmount = (int16_t)(gTofSyncCB.lastSharedCtr - gTofSyncCB.internalCtr);

  // Handle wraparound
  if (skipAmount < 0)
  {
    skipAmount = abs(skipAmount);
  }

  gTofSyncCB.internalCtr += skipAmount;

  // Start skipping
  // Calculating how many bursts/runs we need to skip is comprised of NumberOfRuns*(NumberOfBatchesPerRun)
  TOF_genSyncwordBatches(skipAmount*(RF_cmdTof.numIterations/(gTofSecHandle.numOfSyncWordsPerBuffer)));

  // We tried to sync, we are not out of sync but can't tell for sure
  // Wait for next buffer switch to be certain
  gTofSyncCB.currentSyncState = TOF_SYNC_WAIT_FOR_SYNC;

#ifdef RTLS_TOF_DEBUG
  gTofStatistics.numRecoveries++;
  gTofStatistics.numSkippedRuns += skipAmount;
#endif
}

/*********************************************************************
* @fn      TOF_crc16
*
* @brief   A simple implementation of the crc-16 checksum algorithm,
*          courtesy of IAR (circa 2007)
*
*          Note:  If you use this function, you need to call it with an
*                     extra number of zero bytes equal to the size of the
*                     checksum (in this case 2), to "rotate out the answer"
*
* @param   crc   - The CRC value to start with
* @param   pAddr - Pointer to an array of bytes to run the CRC over
* @param   len   - The number of bytes to process
*
* @return  CRC-16 result
*/
uint16_t TOF_crc16(uint16_t crc, uint8_t *pAddr, uint32_t len)
{
  while (len--)
  {
    uint8_t i;
    uint8_t byte = *(pAddr++);

    for (i = 0; i < 8; ++i)
    {
      uint32_t temp = crc;

      crc <<= 1;

      if (byte & 0x80)
      {
        crc |= 1;
      }

      if (temp & 0x8000)
      {
        crc ^= CRC16_POLYNOMIAL;
      }

      byte <<= 1;
    }
  }

  return crc;
}

/*********************************************************************
* @fn      TOF_roleSwitch
*
* @brief   Switch ToF role. ToF Master <-> ToF Passive
*
* @param   newRole - new ToF role for RF patch
*
* @return  None
*/
void TOF_roleSwitch(ToF_Role newRole)
{
  RF_cmdTof.bMaster = newRole;
}

/*********************************************************************
* @fn      TOF_handleTofHandshake
*
* @brief   Handle ToF handshake state
*
* @param   currentState - current state of the handshake
*
* @return  returns the next state of the handshake
*/
TofHandshakeState_e TOF_handleTofHandshake(TofHandshakeState_e currentState)
{
  switch (currentState)
  {
    case TOF_HANDSHAKE_STARTED:
    {
      // Save buffer #1 in case Master<->Slave handshake fails in the future
      if (gTofSecHandle.pTempSyncWordBuffer)
      {
        memcpy(gTofSecHandle.pTempSyncWordBuffer, gTofSecHandle.pSyncWordBuffer1, sizeof(uint64_t) * gTofSecHandle.numOfSyncWordsPerBuffer);
      }

      return TOF_HANDSHAKE_IN_PROGRESS;
    }
    break;

    case TOF_HANDSHAKE_FAILED:
    {
      // We know that in the previous run the handshake failed (we did generate a new batch of syncwords)
      // Prepare for a potentially successful handshake, buffers #3,#2 are loaded to RF at this point #1 is kept in temp
      RF_cmdTof.pSyncWord = (uint64_t *)gTofSecHandle.pSyncWordBuffer1;

      gTofSyncCB.lastBuffFilled = 1;
      RF_cmdTof.syncwordBufferReady |= syncWordBufferOneReady;

      return TOF_HANDSHAKE_RECOVERING;
    }
    break;

    case TOF_HANDSHAKE_RECOVERING:
    case TOF_HANDSHAKE_IN_PROGRESS:
    {
      // Two options to get here:
      // 1. We were in progress in the previous state
      //    Now we got the second buffer switch which means that slave/passive heard the master for the second time
      //    ToF handshake is now complete
      // 2. We were recovering from a failed handshake (we heard the master but it either did not hear us or the entire run failed immediately after)
      //    Now we got the second buffer switch which means that we heard the other side for the second time
      // In either case the handshake is complete
      return TOF_HANDSHAKE_COMPLETE;
    }
    break;
  }

  // We should never reach here
  return TOF_HANDSHAKE_FAILED;
}

/*
 * Copyright (c) 2017-2018, Texas Instruments Incorporated
 * All rights reserved.
 *
 * IMPORTANT: Your use of this Software is limited to those specific rights
 * granted under the terms of a software license agreement between the user
 * who downloaded the software, his/her employer (which must be your employer)
 * and Texas Instruments Incorporated (the "License"). You may not use this
 * Software unless you agree to abide by the terms of the License. The License
 * limits your use, and you acknowledge, that the Software may not be modified,
 * copied or distributed unless embedded on a Texas Instruments microcontroller
 * or used solely and exclusively in conjunction with a Texas Instruments radio
 * frequency transceiver, which is integrated into your product. Other than for
 * the foregoing purpose, you may not use, reproduce, copy, prepare derivative
 * works of, modify, distribute, perform, display or sell this Software and/or
 * its documentation for any purpose.
 *
 * YOU FURTHER ACKNOWLEDGE AND AGREE THAT THE SOFTWARE AND DOCUMENTATION ARE
 * PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESS OR IMPLIED,
 * INCLUDING WITHOUT LIMITATION, ANY WARRANTY OF MERCHANTABILITY, TITLE,
 * NON-INFRINGEMENT AND FITNESS FOR A PARTICULAR PURPOSE. IN NO EVENT SHALL
 * TEXAS INSTRUMENTS OR ITS LICENSORS BE LIABLE OR OBLIGATED UNDER CONTRACT,
 * NEGLIGENCE, STRICT LIABILITY, CONTRIBUTION, BREACH OF WARRANTY, OR OTHER
 * LEGAL EQUITABLE THEORY ANY DIRECT OR INDIRECT DAMAGES OR EXPENSES
 * INCLUDING BUT NOT LIMITED TO ANY INCIDENTAL, SPECIAL, INDIRECT, PUNITIVE
 * OR CONSEQUENTIAL DAMAGES, LOST PROFITS OR LOST DATA, COST OF PROCUREMENT
 * OF SUBSTITUTE GOODS, TECHNOLOGY, SERVICES, OR ANY CLAIMS BY THIRD PARTIES
 * (INCLUDING BUT NOT LIMITED TO ANY DEFENSE THEREOF), OR OTHER SIMILAR COSTS.
 *
 * Should you have any questions regarding your right to use this Software,
 * contact Texas Instruments Incorporated at www.TI.com.
 *
 */

#include "tof_security.h"

#include "rtls_ctrl.h"
#include <ti/drivers/cryptoutils/cryptokey/CryptoKeyPlaintext.h>
#include <ti/drivers/TRNG.h>

#include <stdlib.h>
#include <stdbool.h>
#include <string.h>
#include <ti/sysbios/hal/Hwi.h>
#include <ti/sysbios/knl/Swi.h>

/*********************************************************************
 * LOCAL FUNCTIONS
 */
void TOFSecurity_freeBuffers(tofSecHandle_t *tofSecHandle);

/*******************************************************************************
 * @fn          TOFSecurity_paramsInit API
 *
 * @brief       This function to initialize the tof security parameters.
 *
 * input parameters
 *
 * @param       tofSecCfgPrms  - A pointer to the user-defined tof security preset values.
 *
 * @param       tofSecHandle  - A pointer to an empty tof security handle.
 *
 * output parameters
 *
 * @param       tofSecHandle  - A pointer to the tof security state.
 *
 * @return      int - NULL if failed, other if succeeded
 */
int TOFSecurity_paramsInit(tofSecHandle_t *tofSecHandle, tofSecCfgPrms_t *tofSecCfgPrms)
{
  uint16_t            sizeOfBuffToAlloc;

  // Sanity check
  if (tofSecHandle == NULL || tofSecCfgPrms == NULL)
  {
    return TOF_SEC_INVALID_PRMS_FAIL_E;
  }

  //  Verify params valid
  if ((0 == tofSecCfgPrms->syncWordSize) || (0 == tofSecCfgPrms->totalNumOfSyncWords))
  {
    return TOF_SEC_INVALID_PRMS_FAIL_E;
  }
  
  // Copy configuration parameters to handle
  memcpy(&(tofSecHandle->tofSecCfgParams),tofSecCfgPrms, sizeof(tofSecCfgPrms_t));

  // Init buffer/s sizes
  if (TOF_MODE_DBL_BUF == tofSecHandle->tofSecCfgParams.bUseDoubleBuffer)
  {
    tofSecHandle->numOfSyncWordsPerBuffer = TOF_SEC_DBL_BUFF_SIZE;
  }
  else
  {
    tofSecHandle->numOfSyncWordsPerBuffer = tofSecCfgPrms->totalNumOfSyncWords;
  }

  // Calculate size of buffer/s
  sizeOfBuffToAlloc = sizeof(uint8_t) * tofSecCfgPrms->syncWordSize * tofSecHandle->numOfSyncWordsPerBuffer;

  // Allocate first buffer
  RTLSUTIL_MALLOC(tofSecHandle->pSyncWordBuffer1, sizeOfBuffToAlloc);
  if (NULL == (tofSecHandle->pSyncWordBuffer1))
  {
    return TOF_SEC_ALLOC_FAIL_E;
  }

  // Allocate 2nd buffer?
  if (TOF_MODE_DBL_BUF == tofSecCfgPrms->bUseDoubleBuffer)
  {  
    RTLSUTIL_MALLOC(tofSecHandle->pSyncWordBuffer2, sizeOfBuffToAlloc);
    if (NULL == (tofSecHandle->pSyncWordBuffer2))
    {
      RTLSUTIL_FREE(tofSecHandle->pSyncWordBuffer1);
      return TOF_SEC_ALLOC_FAIL_E;
    }
  }

  // Allocate temp buffer
  RTLSUTIL_MALLOC(tofSecHandle->pTempSyncWordBuffer, sizeOfBuffToAlloc);
  if (NULL == (tofSecHandle->pTempSyncWordBuffer))
  {
    RTLSUTIL_FREE(tofSecHandle->pSyncWordBuffer2);
    RTLSUTIL_FREE(tofSecHandle->pSyncWordBuffer1);

    return TOF_SEC_ALLOC_FAIL_E;
  }

  return TOF_SEC_SUCCESS_E;
}

/*******************************************************************************
 * @fn          TOFSecurity_open API
 *
 * @brief       This function is used to open AESCTRDRBG driver and, when needed,
 *              to generate 128-bit random seed.
 *
 * input parameters
 *
 * @param       tofSecHandle  - A pointer to an empty tof security handle.
 *
 * @param       seed - NULL if need to generate seed, otherwise, a pointer to 128-bit random seed.
 *
 * output parameters
 *
 * @param       tofSecHandle  - A pointer to the tof security state.
 *
 * @return      int - NULL if failed, other if succeeded.
 */
int TOFSecurity_open(tofSecHandle_t *tofSecHandle, uint8_t *seed)
{
  AESCTRDRBG_Params   drbgParams;
  int_fast16_t        result;

  // If ToF master, generate seed
  if (seed == NULL)
  {
    TRNG_Handle         trngHandle;
    TRNG_Params         trngParams;
    CryptoKey           seedKey;

    // Init TRNG driver
    TRNG_init();

    TRNG_Params_init(&trngParams);
    trngParams.returnBehavior = TRNG_RETURN_BEHAVIOR_POLLING;

    // Open TRNG driver to get the handle
    trngHandle = TRNG_open(1, &trngParams);
    if (trngHandle == NULL)
    {
      TOFSecurity_freeBuffers(tofSecHandle);
      return TOF_SEC_GEN_FAIL_E;
    }

    // Init CryptoKey
    CryptoKeyPlaintext_initKey(&seedKey, tofSecHandle->seed, AESCTRDRBG_SEED_LENGTH_AES_128);

    // Generate seed
    result = TRNG_generateEntropy(trngHandle, &seedKey);
    if (result != TRNG_STATUS_SUCCESS)
    {
      TOFSecurity_freeBuffers(tofSecHandle);
      TRNG_close(trngHandle);
      return TOF_SEC_GEN_FAIL_E;
    }

    // Close TRNG driver
    TRNG_close(trngHandle);
  }
  else
  {
    memcpy(tofSecHandle->seed, seed, AESCTRDRBG_SEED_LENGTH_AES_128);
  }

  // Init DRBG driver
  AESCTRDRBG_init();

  AESCTRDRBG_Params_init(&drbgParams);
  drbgParams.keyLength = AESCTRDRBG_AES_KEY_LENGTH_128;
  drbgParams.reseedInterval = 0xFFFFFFFF;
  drbgParams.seed = tofSecHandle->seed;

  // Open DRBG driver
  tofSecHandle->drbgHandle = AESCTRDRBG_open(1, &drbgParams);

  if (tofSecHandle->drbgHandle == NULL)
  {
    TOFSecurity_freeBuffers(tofSecHandle);
    return TOF_SEC_GEN_FAIL_E;
  }

  return TOF_SEC_SUCCESS_E;
}

/*******************************************************************************
 * @fn          TOFSecurity_close API
 *
 * @brief       This function is used to close ToF Security and free buffers
 *
 * input parameters
 *
 * @param       tofSecHandle  - A pointer to an empty tof security handle.
 *
 * output parameters
 *
 * @return      int - NULL if failed, other if succeeded
 */
int TOFSecurity_close(tofSecHandle_t *tofSecHandle)
{
  // Free first buffer
  RTLSUTIL_FREE(tofSecHandle->pSyncWordBuffer1);

  // If double buffer is enabled, free the other one as well
  if (tofSecHandle->tofSecCfgParams.bUseDoubleBuffer == TOF_MODE_DBL_BUF)
  {
    RTLSUTIL_FREE(tofSecHandle->pSyncWordBuffer2);
  }

  // Close DRBG driver
  AESCTRDRBG_close(tofSecHandle->drbgHandle);

  return TOF_SEC_SUCCESS_E;
}

/*********************************************************************
* @fn       TOFSecurity_genSyncWords
*
* @brief    Will fill each Sync Word Buffer with <numOfSyncWordsPerBuffer> Sync Words
*
* input parameters
*
* @param   bufferToFill  - An enum describing which buffers to fill
*
* @param   tofSecHandle  - A pointer to the tof security state.
*
* output parameters
*
* @param   tofSecHandle  - A pointer to the tof security state.
*
* @return  NULL if failed, other if succeeded
*/
int TOFSecurity_genSyncWords(tofSecHandle_t *tofSecHandle, uint8_t bufferToFill)
{
  CryptoKey syncWordKey;
  int_fast16_t result;

  if ((TOF_SEC_FILL_BOTH_BUFFS == bufferToFill) || (TOF_SEC_FILL_1ST_BUFF == bufferToFill))
  {
    if (NULL != tofSecHandle->pSyncWordBuffer1)
    {
      // Initialise the CryptoKey
      CryptoKeyPlaintext_initKey(&syncWordKey,
                                 tofSecHandle->pSyncWordBuffer1,
                                 tofSecHandle->tofSecCfgParams.syncWordSize * tofSecHandle->numOfSyncWordsPerBuffer);
      // Generate syncwords
      result =  AESCTRDRBG_getBytes(tofSecHandle->drbgHandle, &syncWordKey);
      if (result != AESCTRDRBG_STATUS_SUCCESS)
      {
        return TOF_SEC_GEN_FAIL_E;
      }
    }
    else
    {
      return TOF_SEC_GEN_FAIL_E;
    }
  }
  
  if ((TOF_SEC_FILL_BOTH_BUFFS == bufferToFill) || (TOF_SEC_FILL_2ND_BUFF == bufferToFill))
  {
    if (tofSecHandle->tofSecCfgParams.bUseDoubleBuffer)
    {
      if (NULL != tofSecHandle->pSyncWordBuffer2)
      {
        // Initialise the CryptoKey
        CryptoKeyPlaintext_initKey(&syncWordKey,
                                   tofSecHandle->pSyncWordBuffer2,
                                   tofSecHandle->tofSecCfgParams.syncWordSize * tofSecHandle->numOfSyncWordsPerBuffer);
        // Generate syncwords
        result =  AESCTRDRBG_getBytes(tofSecHandle->drbgHandle, &syncWordKey);
        if (result != AESCTRDRBG_STATUS_SUCCESS)
        {
          return TOF_SEC_GEN_FAIL_E;
        }
      }
      else
      {
        return TOF_SEC_GEN_FAIL_E;
      }
    }
  }
  return TOF_SEC_SUCCESS_E;
}

/*********************************************************************
* @fn       TOFSecurity_freeBuffers
*
* @brief   This function free Sync Word Buffers
*
* @param   tofSecHandle  - A pointer to the tof security state.
*
* @return  none
*/
void TOFSecurity_freeBuffers(tofSecHandle_t *tofSecHandle)
{
  if (tofSecHandle->pSyncWordBuffer1 != NULL)
  {
    RTLSUTIL_FREE(tofSecHandle->pSyncWordBuffer1);
  }

  if (TOF_MODE_DBL_BUF == tofSecHandle->tofSecCfgParams.bUseDoubleBuffer &&
      tofSecHandle->pSyncWordBuffer2 != NULL)
  {
    RTLSUTIL_FREE(tofSecHandle->pSyncWordBuffer2);
  }

  if (tofSecHandle->pTempSyncWordBuffer != NULL)
  {
    RTLSUTIL_FREE(tofSecHandle->pTempSyncWordBuffer);
  }
}