/******************************************************************************
 *
 * Copyright(c) 2007 - 2017  Realtek Corporation.
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms of version 2 of the GNU General Public License as
 * published by the Free Software Foundation.
 *
 * This program is distributed in the hope that it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
 * more details.
 *
 * The full GNU General Public License is included in this distribution in the
 * file called LICENSE.
 *
 * Contact Information:
 * wlanfae <wlanfae@realtek.com>
 * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
 * Hsinchu 300, Taiwan.
 *
 * Larry Finger <Larry.Finger@lwfinger.net>
 *
 *****************************************************************************/
#include "mp_precomp.h"
#include "phydm_precomp.h"

void phydm_set_crystal_cap(void *p_dm_void, u8 crystal_cap)
{
	struct PHY_DM_STRUCT *p_dm = (struct PHY_DM_STRUCT *)p_dm_void;
	struct phydm_cfo_track_struct *p_cfo_track =
		(struct phydm_cfo_track_struct *)phydm_get_structure(
			p_dm, PHYDM_CFOTRACK);

	if (p_cfo_track->crystal_cap == crystal_cap)
		return;

	crystal_cap = crystal_cap & 0x3F;
	p_cfo_track->crystal_cap = crystal_cap;

	if (p_dm->support_ic_type & (ODM_RTL8188E | ODM_RTL8188F)) {
		#if (RTL8188E_SUPPORT == 1) || (RTL8188F_SUPPORT == 1)
		/* write 0x24[22:17] = 0x24[16:11] = crystal_cap */
		odm_set_bb_reg(p_dm, REG_AFE_XTAL_CTRL, 0x007ff800, (crystal_cap | (crystal_cap << 6)));
		#endif
	}
	#if (RTL8812A_SUPPORT == 1)
	else if (p_dm->support_ic_type & ODM_RTL8812) {
	
		/* write 0x2C[30:25] = 0x2C[24:19] = crystal_cap */
		odm_set_bb_reg(p_dm, REG_MAC_PHY_CTRL, 0x7FF80000, (crystal_cap | (crystal_cap << 6)));
		
	} 
	#endif
	#if (RTL8703B_SUPPORT == 1) || (RTL8723B_SUPPORT == 1) || (RTL8192E_SUPPORT == 1) || (RTL8821A_SUPPORT == 1) || (RTL8723D_SUPPORT == 1)
	else if ((p_dm->support_ic_type & (ODM_RTL8703B | ODM_RTL8723B | ODM_RTL8192E | ODM_RTL8821 | ODM_RTL8723D))) {
	
		/* 0x2C[23:18] = 0x2C[17:12] = crystal_cap */
		odm_set_bb_reg(p_dm, REG_MAC_PHY_CTRL, 0x00FFF000, (crystal_cap | (crystal_cap << 6)));
		
	}
	#endif
	#if (RTL8814A_SUPPORT == 1)	
	else if (p_dm->support_ic_type & ODM_RTL8814A) {
	
		/* write 0x2C[26:21] = 0x2C[20:15] = crystal_cap */
		odm_set_bb_reg(p_dm, REG_MAC_PHY_CTRL, 0x07FF8000, (crystal_cap | (crystal_cap << 6)));
		
	}
	#endif
	#if (RTL8822B_SUPPORT == 1) || (RTL8821C_SUPPORT == 1) || (RTL8197F_SUPPORT == 1)
	else if (p_dm->support_ic_type &
		 (ODM_RTL8822B | ODM_RTL8821C | ODM_RTL8197F)) {

		/* write 0x24[30:25] = 0x28[6:1] = crystal_cap */
		odm_set_bb_reg(p_dm, REG_AFE_XTAL_CTRL, 0x7e000000,
			       crystal_cap);
		odm_set_bb_reg(p_dm, REG_AFE_PLL_CTRL, 0x7e, crystal_cap);
	}
	#endif
	#if (RTL8710B_SUPPORT == 1)
	else if (p_dm->support_ic_type & (ODM_RTL8710B)) {
	
		#if (DM_ODM_SUPPORT_TYPE & ODM_WIN)		
		/* write 0x60[29:24] = 0x60[23:18] = crystal_cap */
		HAL_SetSYSOnReg(p_dm->adapter, REG_SYS_XTAL_CTRL0, 0x3FFC0000, (crystal_cap | (crystal_cap << 6)));
		#endif
	}
	#endif
	PHYDM_DBG(p_dm, DBG_CFO_TRK,
		  ("Set rystal_cap = 0x%x\n", p_cfo_track->crystal_cap));
}

u8 phydm_get_default_crytaltal_cap(void *p_dm_void)
{
	struct PHY_DM_STRUCT *p_dm = (struct PHY_DM_STRUCT *)p_dm_void;
	u8 crystal_cap = 0x20;

#if (DM_ODM_SUPPORT_TYPE & ODM_CE) && defined(DM_ODM_CE_MAC80211)
	struct rtl_priv *rtlpriv = (struct rtl_priv *)p_dm->adapter;
	struct rtl_efuse *rtlefuse = rtl_efuse(rtlpriv);

	crystal_cap = rtlefuse->crystalcap;
#elif (DM_ODM_SUPPORT_TYPE & (ODM_WIN | ODM_CE))
	struct _ADAPTER	*adapter = p_dm->adapter;
	HAL_DATA_TYPE	*p_hal_data = GET_HAL_DATA(adapter);

	crystal_cap = p_hal_data->crystal_cap;
#else
	struct rtl8192cd_priv	*priv	= p_dm->priv;

	if (priv->pmib->dot11RFEntry.xcap > 0)
		crystal_cap = priv->pmib->dot11RFEntry.xcap;
#endif

	crystal_cap = crystal_cap & 0x3f;

	return crystal_cap;
}

void phydm_set_atc_status(void *p_dm_void, boolean atc_status)
{
	struct PHY_DM_STRUCT *p_dm = (struct PHY_DM_STRUCT *)p_dm_void;
	struct phydm_cfo_track_struct *p_cfo_track =
		(struct phydm_cfo_track_struct *)phydm_get_structure(
			p_dm, PHYDM_CFOTRACK);

	if (p_cfo_track->is_atc_status == atc_status)
		return;

	odm_set_bb_reg(p_dm, ODM_REG(BB_ATC, p_dm), ODM_BIT(BB_ATC, p_dm),
		       atc_status);
	p_cfo_track->is_atc_status = atc_status;
}

boolean phydm_get_atc_status(void *p_dm_void)
{
	boolean atc_status;
	struct PHY_DM_STRUCT *p_dm = (struct PHY_DM_STRUCT *)p_dm_void;

	atc_status = (boolean)odm_get_bb_reg(p_dm, ODM_REG(BB_ATC, p_dm),
					     ODM_BIT(BB_ATC, p_dm));
	return atc_status;
}

void phydm_cfo_tracking_reset(void *p_dm_void)
{
	struct PHY_DM_STRUCT *p_dm = (struct PHY_DM_STRUCT *)p_dm_void;
	struct phydm_cfo_track_struct *p_cfo_track =
		(struct phydm_cfo_track_struct *)phydm_get_structure(
			p_dm, PHYDM_CFOTRACK);

	PHYDM_DBG(p_dm, DBG_CFO_TRK, ("%s ======>\n", __func__));

	p_cfo_track->def_x_cap = phydm_get_default_crytaltal_cap(p_dm);
	p_cfo_track->is_adjust = true;

	if (p_cfo_track->crystal_cap > p_cfo_track->def_x_cap) {

		phydm_set_crystal_cap(p_dm, p_cfo_track->crystal_cap - 1);
		PHYDM_DBG(p_dm, DBG_CFO_TRK, ("approch to Init-val (0x%x)\n",
					      p_cfo_track->crystal_cap));

	} else if (p_cfo_track->crystal_cap < p_cfo_track->def_x_cap) {

		phydm_set_crystal_cap(p_dm, p_cfo_track->crystal_cap + 1);
		PHYDM_DBG(p_dm, DBG_CFO_TRK, ("approch to init-val 0x%x\n",
					      p_cfo_track->crystal_cap));
	}

#if (DM_ODM_SUPPORT_TYPE & (ODM_WIN | ODM_CE))
	phydm_set_atc_status(p_dm, true);
#endif
}

void phydm_cfo_tracking_init(void *p_dm_void)
{
	struct PHY_DM_STRUCT *p_dm = (struct PHY_DM_STRUCT *)p_dm_void;
	struct phydm_cfo_track_struct *p_cfo_track =
		(struct phydm_cfo_track_struct *)phydm_get_structure(
			p_dm, PHYDM_CFOTRACK);

	p_cfo_track->def_x_cap = p_cfo_track->crystal_cap =
		phydm_get_default_crytaltal_cap(p_dm);
	p_cfo_track->is_atc_status = phydm_get_atc_status(p_dm);
	p_cfo_track->is_adjust = true;
	PHYDM_DBG(p_dm, DBG_CFO_TRK, ("ODM_CfoTracking_init()=========>\n"));
	PHYDM_DBG(
		p_dm, DBG_CFO_TRK,
		("ODM_CfoTracking_init(): is_atc_status = %d, crystal_cap = 0x%x\n",
		 p_cfo_track->is_atc_status, p_cfo_track->def_x_cap));

#if RTL8822B_SUPPORT
	/* Crystal cap. control by WiFi */
	if (p_dm->support_ic_type & ODM_RTL8822B)
		odm_set_bb_reg(p_dm, 0x10, 0x40, 0x1);
#endif

#if RTL8821C_SUPPORT
	/* Crystal cap. control by WiFi */
	if (p_dm->support_ic_type & ODM_RTL8821C)
		odm_set_bb_reg(p_dm, 0x10, 0x40, 0x1);
#endif
}

void phydm_cfo_tracking(void *p_dm_void)
{
	struct PHY_DM_STRUCT *p_dm = (struct PHY_DM_STRUCT *)p_dm_void;
	struct phydm_cfo_track_struct *p_cfo_track =
		(struct phydm_cfo_track_struct *)phydm_get_structure(
			p_dm, PHYDM_CFOTRACK);
	s32 cfo_avg = 0, cfo_path_sum = 0; /*avg among each path*/
	u32 cfo_rpt_sum, cfo_khz_avg[4] = {0};
	s8 crystal_cap = p_cfo_track->crystal_cap;
	u8 i, valid_path_cnt = 0;

	if (!(p_dm->support_ability & ODM_BB_CFO_TRACKING)) {
		return;
	}

	PHYDM_DBG(p_dm, DBG_CFO_TRK, ("%s ======>\n", __func__));

	if (!p_dm->is_linked || !p_dm->is_one_entry_only) {
		phydm_cfo_tracking_reset(p_dm);
		PHYDM_DBG(p_dm, DBG_CFO_TRK,
			  ("is_linked = %d, one_entry_only = %d\n",
			   p_dm->is_linked, p_dm->is_one_entry_only));

	} else {

		/* No new packet */
		if (p_cfo_track->packet_count ==
		    p_cfo_track->packet_count_pre) {
			PHYDM_DBG(p_dm, DBG_CFO_TRK,
				  ("Pkt cnt doesn't change\n"));
			return;
		}
		p_cfo_track->packet_count_pre = p_cfo_track->packet_count;

		/*Calculate CFO */
		for (i = 0; i < p_dm->num_rf_path; i++) {
			if (p_cfo_track->CFO_cnt[i] == 0)
				continue;

			valid_path_cnt++;
			cfo_rpt_sum = (u32)CFO_HW_RPT_2_KHZ(
				((p_cfo_track->CFO_tail[i] < 0) ?
					 (0 - p_cfo_track->CFO_tail[i]) :
					 p_cfo_track->CFO_tail[i]));
			cfo_khz_avg[i] = cfo_rpt_sum / p_cfo_track->CFO_cnt[i];

			PHYDM_DBG(
				p_dm, DBG_CFO_TRK,
				("[Path-%d] CFO_sum = (( %d )), cnt = (( %d )) , CFO_avg= (( %s%d )) kHz\n",
				 i, cfo_rpt_sum, p_cfo_track->CFO_cnt[i],
				 ((p_cfo_track->CFO_tail[i] < 0) ? "-" : " "),
				 cfo_khz_avg[i]));
		}

		for (i = 0; i < valid_path_cnt; i++) {
			if (p_cfo_track->CFO_tail[i] < 0) {
				cfo_path_sum += (0 - (s32)cfo_khz_avg[i]);
			} else
				cfo_path_sum += (s32)cfo_khz_avg[i];
		}

		if (valid_path_cnt >= 2)
			cfo_avg = cfo_path_sum / valid_path_cnt;

		p_cfo_track->CFO_ave_pre = cfo_avg;

		PHYDM_DBG(p_dm, DBG_CFO_TRK,
			  ("path_cnt = ((%d)), CFO_avg_path=((%d kHz))\n",
			   valid_path_cnt, cfo_avg));

		/*reset counter*/
		for (i = 0; i < p_dm->num_rf_path; i++) {
			p_cfo_track->CFO_tail[i] = 0;
			p_cfo_track->CFO_cnt[i] = 0;
		}

		/* To adjust crystal cap or not */
		if (p_cfo_track->is_adjust == false) {
			if (cfo_avg > CFO_TRK_ENABLE_TH ||
			    cfo_avg < (-CFO_TRK_ENABLE_TH))
				p_cfo_track->is_adjust = true;
		} else {
			if (cfo_avg < CFO_TRK_STOP_TH &&
			    cfo_avg > (-CFO_TRK_STOP_TH))
				p_cfo_track->is_adjust = false;
		}

		#ifdef ODM_CONFIG_BT_COEXIST
		/*BT case: Disable CFO tracking */
		if (p_dm->bt_info_table.is_bt_enabled) {
			p_cfo_track->is_adjust = false;
			phydm_set_crystal_cap(p_dm, p_cfo_track->def_x_cap);
			PHYDM_DBG(p_dm, DBG_CFO_TRK,
				  ("Disable CFO tracking for BT\n"));
		}
		#endif

		/*Adjust Crystal Cap. */
		if (p_cfo_track->is_adjust) {
			if (cfo_avg > CFO_TRK_STOP_TH)
				crystal_cap += 1;
			else if (cfo_avg < (-CFO_TRK_STOP_TH))
				crystal_cap -= 1;

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

			phydm_set_crystal_cap(p_dm, (u8)crystal_cap);
		}

		PHYDM_DBG(p_dm, DBG_CFO_TRK,
			  ("Crystal cap{Current, Default}={0x%x, 0x%x}\n\n",
			   p_cfo_track->crystal_cap, p_cfo_track->def_x_cap));

/* Dynamic ATC switch */
		#if (DM_ODM_SUPPORT_TYPE & (ODM_WIN | ODM_CE))
		if (p_dm->support_ic_type & ODM_IC_11N_SERIES) {
			if (cfo_avg < CFO_TH_ATC && cfo_avg > -CFO_TH_ATC) {
				phydm_set_atc_status(p_dm, false);
				PHYDM_DBG(p_dm, DBG_CFO_TRK, ("Disable ATC\n"));
			} else {
				phydm_set_atc_status(p_dm, true);
				PHYDM_DBG(p_dm, DBG_CFO_TRK, ("Enable ATC\n"));
			}
		}
		#endif
	}
}

void phydm_parsing_cfo(void *p_dm_void, void *p_pktinfo_void, s8 *pcfotail,
		       u8 num_ss)
{
	struct PHY_DM_STRUCT *p_dm = (struct PHY_DM_STRUCT *)p_dm_void;
	struct phydm_perpkt_info_struct *p_pktinfo =
		(struct phydm_perpkt_info_struct *)p_pktinfo_void;
	struct phydm_cfo_track_struct *p_cfo_track =
		(struct phydm_cfo_track_struct *)phydm_get_structure(
			p_dm, PHYDM_CFOTRACK);
	u8 i;

	if (!(p_dm->support_ability & ODM_BB_CFO_TRACKING))
		return;

#if (DM_ODM_SUPPORT_TYPE & (ODM_WIN | ODM_CE))
	if (p_pktinfo->is_packet_match_bssid)
#else
	if (p_pktinfo->station_id != 0)
#endif
	{
		if (num_ss > p_dm->num_rf_path) /*For fool proof*/
			num_ss = p_dm->num_rf_path;

		/*PHYDM_DBG(p_dm, DBG_CFO_TRK, ("num_ss = ((%d)),  p_dm->num_rf_path = ((%d))\n", num_ss,  p_dm->num_rf_path));*/

		/* 3 Update CFO report for path-A & path-B */
		/* Only paht-A and path-B have CFO tail and short CFO */
		for (i = 0; i < num_ss; i++) {
			p_cfo_track->CFO_tail[i] += pcfotail[i];
			p_cfo_track->CFO_cnt[i]++;
			/*PHYDM_DBG(p_dm, DBG_CFO_TRK, ("[ID %d][path %d][rate 0x%x] CFO_tail = ((%d)), CFO_tail_sum = ((%d)), CFO_cnt = ((%d))\n",
				p_pktinfo->station_id, i, p_pktinfo->data_rate, pcfotail[i], p_cfo_track->CFO_tail[i], p_cfo_track->CFO_cnt[i]));
			*/
		}

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