// SPDX-License-Identifier: GPL-2.0
/******************************************************************************
 *
 * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
 *
 ******************************************************************************/

#include "odm_precomp.h"

static void odm_SetCrystalCap(void *pDM_VOID, u8 CrystalCap)
{
        struct dm_odm_t *pDM_Odm = (struct dm_odm_t *)pDM_VOID;
        struct cfo_tracking *pCfoTrack = &pDM_Odm->DM_CfoTrack;

        if (pCfoTrack->CrystalCap == CrystalCap)
                return;

        pCfoTrack->CrystalCap = CrystalCap;

        /*  0x2C[23:18] = 0x2C[17:12] = CrystalCap */
        CrystalCap = CrystalCap & 0x3F;
        PHY_SetBBReg(
                pDM_Odm->Adapter,
                REG_MAC_PHY_CTRL,
                0x00FFF000,
                (CrystalCap | (CrystalCap << 6))
        );
}

static u8 odm_GetDefaultCrytaltalCap(void *pDM_VOID)
{
        struct dm_odm_t *pDM_Odm = (struct dm_odm_t *)pDM_VOID;

        struct adapter *Adapter = pDM_Odm->Adapter;
        struct hal_com_data *pHalData = GET_HAL_DATA(Adapter);

        return pHalData->CrystalCap & 0x3f;
}

static void odm_SetATCStatus(void *pDM_VOID, bool ATCStatus)
{
        struct dm_odm_t *pDM_Odm = (struct dm_odm_t *)pDM_VOID;
        struct cfo_tracking *pCfoTrack = &pDM_Odm->DM_CfoTrack;

        if (pCfoTrack->bATCStatus == ATCStatus)
                return;

        PHY_SetBBReg(
                pDM_Odm->Adapter,
                ODM_REG(BB_ATC, pDM_Odm),
                ODM_BIT(BB_ATC, pDM_Odm),
                ATCStatus
        );
        pCfoTrack->bATCStatus = ATCStatus;
}

static bool odm_GetATCStatus(void *pDM_VOID)
{
        bool ATCStatus;
        struct dm_odm_t *pDM_Odm = (struct dm_odm_t *)pDM_VOID;

        ATCStatus = (bool)PHY_QueryBBReg(
                pDM_Odm->Adapter,
                ODM_REG(BB_ATC, pDM_Odm),
                ODM_BIT(BB_ATC, pDM_Odm)
        );
        return ATCStatus;
}

void ODM_CfoTrackingReset(void *pDM_VOID)
{
        struct dm_odm_t *pDM_Odm = (struct dm_odm_t *)pDM_VOID;
        struct cfo_tracking *pCfoTrack = &pDM_Odm->DM_CfoTrack;

        pCfoTrack->DefXCap = odm_GetDefaultCrytaltalCap(pDM_Odm);
        pCfoTrack->bAdjust = true;

        odm_SetCrystalCap(pDM_Odm, pCfoTrack->DefXCap);
        odm_SetATCStatus(pDM_Odm, true);
}

void ODM_CfoTrackingInit(void *pDM_VOID)
{
        struct dm_odm_t *pDM_Odm = (struct dm_odm_t *)pDM_VOID;
        struct cfo_tracking *pCfoTrack = &pDM_Odm->DM_CfoTrack;

        pCfoTrack->DefXCap =
                pCfoTrack->CrystalCap = odm_GetDefaultCrytaltalCap(pDM_Odm);
        pCfoTrack->bATCStatus = odm_GetATCStatus(pDM_Odm);
        pCfoTrack->bAdjust = true;
}

void ODM_CfoTracking(void *pDM_VOID)
{
        struct dm_odm_t *pDM_Odm = (struct dm_odm_t *)pDM_VOID;
        struct cfo_tracking *pCfoTrack = &pDM_Odm->DM_CfoTrack;
        int CFO_kHz_A, CFO_ave = 0;
        int CFO_ave_diff;
        int CrystalCap = (int)pCfoTrack->CrystalCap;
        u8 Adjust_Xtal = 1;

        /* 4 Support ability */
        if (!(pDM_Odm->SupportAbility & ODM_BB_CFO_TRACKING)) {
                return;
        }

        if (!pDM_Odm->bLinked || !pDM_Odm->bOneEntryOnly) {
                /* 4 No link or more than one entry */
                ODM_CfoTrackingReset(pDM_Odm);
        } else {
                /* 3 1. CFO Tracking */
                /* 4 1.1 No new packet */
                if (pCfoTrack->packetCount == pCfoTrack->packetCount_pre) {
                        return;
                }
                pCfoTrack->packetCount_pre = pCfoTrack->packetCount;

                /* 4 1.2 Calculate CFO */
                CFO_kHz_A =  (int)(pCfoTrack->CFO_tail[0] * 3125)  / 1280;

                CFO_ave = CFO_kHz_A;

                /* 4 1.3 Avoid abnormal large CFO */
                CFO_ave_diff =
                        (pCfoTrack->CFO_ave_pre >= CFO_ave) ?
                        (pCfoTrack->CFO_ave_pre-CFO_ave) :
                        (CFO_ave-pCfoTrack->CFO_ave_pre);

                if (
                        CFO_ave_diff > 20 &&
                        pCfoTrack->largeCFOHit == 0 &&
                        !pCfoTrack->bAdjust
                ) {
                        pCfoTrack->largeCFOHit = 1;
                        return;
                } else
                        pCfoTrack->largeCFOHit = 0;
                pCfoTrack->CFO_ave_pre = CFO_ave;

                /* 4 1.4 Dynamic Xtal threshold */
                if (pCfoTrack->bAdjust == false) {
                        if (CFO_ave > CFO_TH_XTAL_HIGH || CFO_ave < (-CFO_TH_XTAL_HIGH))
                                pCfoTrack->bAdjust = true;
                } else {
                        if (CFO_ave < CFO_TH_XTAL_LOW && CFO_ave > (-CFO_TH_XTAL_LOW))
                                pCfoTrack->bAdjust = false;
                }

                /* 4 1.5 BT case: Disable CFO tracking */
                if (pDM_Odm->bBtEnabled) {
                        pCfoTrack->bAdjust = false;
                        odm_SetCrystalCap(pDM_Odm, pCfoTrack->DefXCap);
                }

                /* 4 1.6 Big jump */
                if (pCfoTrack->bAdjust) {
                        if (CFO_ave > CFO_TH_XTAL_LOW)
                                Adjust_Xtal = Adjust_Xtal+((CFO_ave-CFO_TH_XTAL_LOW)>>2);
                        else if (CFO_ave < (-CFO_TH_XTAL_LOW))
                                Adjust_Xtal = Adjust_Xtal+((CFO_TH_XTAL_LOW-CFO_ave)>>2);
                }

                /* 4 1.7 Adjust Crystal Cap. */
                if (pCfoTrack->bAdjust) {
                        if (CFO_ave > CFO_TH_XTAL_LOW)
                                CrystalCap = CrystalCap + Adjust_Xtal;
                        else if (CFO_ave < (-CFO_TH_XTAL_LOW))
                                CrystalCap = CrystalCap - Adjust_Xtal;

                        if (CrystalCap > 0x3f)
                                CrystalCap = 0x3f;
                        else if (CrystalCap < 0)
                                CrystalCap = 0;

                        odm_SetCrystalCap(pDM_Odm, (u8)CrystalCap);
                }

                /* 3 2. Dynamic ATC switch */
                if (CFO_ave < CFO_TH_ATC && CFO_ave > -CFO_TH_ATC) {
                        odm_SetATCStatus(pDM_Odm, false);
                } else {
                        odm_SetATCStatus(pDM_Odm, true);
                }
        }
}

void odm_parsing_cfo(void *dm_void, void *pkt_info_void, s8 *cfotail)
{
        struct dm_odm_t *dm_odm = (struct dm_odm_t *)dm_void;
        struct odm_packet_info *pkt_info = pkt_info_void;
        struct cfo_tracking *cfo_track = &dm_odm->DM_CfoTrack;
        u8 i;

        if (!(dm_odm->SupportAbility & ODM_BB_CFO_TRACKING))
                return;

        if (pkt_info->station_id != 0) {
                /*
                 * 3 Update CFO report for path-A & path-B
                 * Only paht-A and path-B have CFO tail and short CFO
                 */
                for (i = RF_PATH_A; i <= RF_PATH_B; i++)
                        cfo_track->CFO_tail[i] = (int)cfotail[i];

                /* 3 Update packet counter */
                if (cfo_track->packetCount == 0xffffffff)
                        cfo_track->packetCount = 0;
                else
                        cfo_track->packetCount++;
        }
}