android13/external/wifi_driver/rtl8852be/phl/phl_sound_cmd.c

/******************************************************************************
 *
 * Copyright(c) 2019 - 2020 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.
 *
 *****************************************************************************/
#include "phl_headers.h"

#ifdef CONFIG_CMD_DISP

/* START of sounding / beamform cmd_disp module */
void
_phl_snd_cmd_set_eng_busy(struct phl_info_t *phl_info, enum snd_cmd_disp_ctrl ctrl)
{
	struct phl_sound_obj *snd = (struct phl_sound_obj *)phl_info->snd_obj;

	if (snd != NULL) {
		snd->msg_busy |= BIT(ctrl);
	}
}

void
_phl_snd_cmd_set_eng_idle(struct phl_info_t *phl_info, enum snd_cmd_disp_ctrl ctrl)
{
	struct phl_sound_obj *snd = (struct phl_sound_obj *)phl_info->snd_obj;

	if (snd != NULL) {
		snd->msg_busy &= ~(BIT(ctrl));
	}
}

enum rtw_phl_status
_phl_snd_cmd_get_eng_status(struct phl_info_t *phl_info, enum snd_cmd_disp_ctrl ctrl)
{
	struct phl_sound_obj *snd = (struct phl_sound_obj *)phl_info->snd_obj;
	enum rtw_phl_status pstatus = RTW_PHL_STATUS_FAILURE;

	if (snd != NULL) {
		if (0 != (snd->msg_busy & BIT(ctrl))) {
			pstatus = RTW_PHL_STATUS_PENDING;
		} else {
			pstatus = RTW_PHL_STATUS_SUCCESS;
		}
	} else {
		pstatus = RTW_PHL_STATUS_RESOURCE;
	}
	return pstatus;
}
enum rtw_phl_status
_phl_snd_aquire_eng(struct phl_info_t *phl_info, enum snd_cmd_disp_ctrl ctrl)
{
	struct phl_sound_obj *snd = (struct phl_sound_obj *)phl_info->snd_obj;
	void *d = phl_to_drvpriv(phl_info);
	enum rtw_phl_status pstatus = RTW_PHL_STATUS_SUCCESS;

	if (snd == NULL)
		return RTW_PHL_STATUS_FAILURE;

	/* acuired cmd_disp_eng control */
	_os_spinlock(d, &snd->cmd_lock, _bh, NULL);
	pstatus = _phl_snd_cmd_get_eng_status(phl_info, ctrl);
	if (pstatus != RTW_PHL_STATUS_SUCCESS) {
		_os_spinunlock(d, &snd->cmd_lock, _bh, NULL);
		return pstatus;
	}
	_phl_snd_cmd_set_eng_busy(phl_info, SND_CMD_DISP_CTRL_BFEE);
	_os_spinunlock(d, &snd->cmd_lock, _bh, NULL);

	return pstatus;
}

void
_phl_snd_free_eng(struct phl_info_t *phl_info, enum snd_cmd_disp_ctrl ctrl)
{
	struct phl_sound_obj *snd = (struct phl_sound_obj *)phl_info->snd_obj;
	void *d = phl_to_drvpriv(phl_info);

	if (snd != NULL) {
		_os_spinlock(d, &snd->cmd_lock, _bh, NULL);
		_phl_snd_cmd_set_eng_idle(phl_info, ctrl);
		_os_spinunlock(d, &snd->cmd_lock, _bh, NULL);
	}
}

static enum phl_mdl_ret_code
_phl_snd_cmd_module_init(void *phl, void *dispr, void **priv)
{
	enum phl_mdl_ret_code ret = MDL_RET_SUCCESS;
#ifdef CONFIG_SND_CMD
	struct phl_info_t *phl_info = (struct phl_info_t *)phl;
	void *d = phl_to_drvpriv(phl_info);
	struct phl_sound_obj *snd_obj = NULL;

	PHL_INFO("--> %s\n", __func__);
	if (NULL == phl_info->snd_obj) {
		phl_info->snd_obj = _os_kmem_alloc(d, sizeof(struct phl_sound_obj));
	}
	snd_obj = phl_info->snd_obj;
	if (snd_obj == NULL) {
		ret = MDL_RET_FAIL;
	} else {
		/* Init the snd static resources here */
		snd_obj->phl_info = (struct phl_info_t *)phl;
		_os_spinlock_init(d, &snd_obj->snd_lock);
		_os_spinlock_init(d, &snd_obj->cmd_lock);
	}
#endif
	*priv = (void *)phl;
	return ret;
}

static void _phl_snd_cmd_module_deinit(void *dispr, void *priv)
{
#ifdef CONFIG_SND_CMD
	struct phl_info_t *phl_info = (struct phl_info_t *)priv;
	struct phl_sound_obj *snd_obj = (struct phl_sound_obj *)phl_info->snd_obj;
	void *d = phl_to_drvpriv(phl_info);

	PHL_INFO("--> %s\n", __func__);
	if (NULL != snd_obj) {
		_os_spinlock_free(d, &snd_obj->snd_lock);
		_os_spinlock_free(d, &snd_obj->cmd_lock);
		_os_kmem_free(d, snd_obj, sizeof(struct phl_sound_obj));
	}
	phl_info->snd_obj = NULL;
#endif
	return;
}

static enum phl_mdl_ret_code _phl_snd_cmd_module_start(void *dispr, void *priv)
{
	return MDL_RET_SUCCESS;
}

static enum phl_mdl_ret_code _phl_snd_cmd_module_stop(void *dispr, void *priv)
{
	return MDL_RET_SUCCESS;
}

static enum phl_mdl_ret_code
_phl_snd_cmd_module_set_info(void *dispr, void *priv,
			     struct phl_module_op_info *info)
{
	enum phl_mdl_ret_code mstatus = MDL_RET_IGNORE;
	struct phl_info_t *phl = (struct phl_info_t *)priv;

	PHL_INFO("[SND_CMD], %s(): opcode %d.\n", __func__, info->op_code);

	switch (info->op_code) {
		case SND_CMD_OP_SET_AID:
		{
			struct snd_cmd_set_aid *cmdbuf = (struct snd_cmd_set_aid *)info->inbuf;

			if (NULL == cmdbuf) {
				mstatus = MDL_RET_FAIL;
				break;
			}
			if (NULL == cmdbuf->sta_info) {
				mstatus = MDL_RET_FAIL;
				cmdbuf->cmd_sts = MDL_RET_FAIL;
				break;
			}
			if (RTW_HAL_STATUS_SUCCESS != rtw_hal_beamform_set_aid(
						phl->hal, cmdbuf->sta_info, cmdbuf->aid)) {
				mstatus = MDL_RET_FAIL;
				cmdbuf->cmd_sts = MDL_RET_FAIL;
				break;
			}
#ifdef RTW_WKARD_BFEE_SET_AID
			cmdbuf->sta_info->wrole->last_set_aid = cmdbuf->aid;
#endif
			cmdbuf->cmd_sts = MDL_RET_SUCCESS;
			mstatus = MDL_RET_SUCCESS;
		}
		break;
		case SND_CMD_OP_NONE:
		case SND_CMD_OP_MAX:
		default:
		break;
	}

	return mstatus;
}

static enum phl_mdl_ret_code
_phl_snd_cmd_module_query_info(void *dispr, void *priv,
			       struct phl_module_op_info *info)
{
	return MDL_RET_IGNORE;
}

static enum phl_mdl_ret_code
_phl_snd_cmd_module_msg_msg_hdlr_pre(struct phl_info_t *phl,
				     struct phl_msg *msg)
{
	enum phl_mdl_ret_code mstatus = MDL_RET_IGNORE;
	switch (MSG_EVT_ID_FIELD(msg->msg_id)) {
	case MSG_EVT_SET_VHT_GID:
		/* do nothing in pre phase */
	break;
	case MSG_EVT_SET_BFEE_AID:
		/* do nothing in pre phase */
	break;
	default:
		break;
	}

	return mstatus;
}

static enum phl_mdl_ret_code
_phl_snd_cmd_module_msg_msg_hdlr_post(struct phl_info_t *phl,
				      struct phl_msg *msg)
{
	enum phl_mdl_ret_code mstatus = MDL_RET_IGNORE;
	switch (MSG_EVT_ID_FIELD(msg->msg_id)) {
	case MSG_EVT_SET_VHT_GID:
	{
		struct rtw_phl_gid_pos_tbl *gid_tbl = (struct rtw_phl_gid_pos_tbl *)msg->inbuf;
		if (msg->inlen != sizeof(struct rtw_phl_gid_pos_tbl)) {
			PHL_TRACE(COMP_PHL_DBG, _PHL_INFO_,
				  "%s() VHT-BFEE : Error, size mis-match \n", __func__);
			mstatus = MDL_RET_FAIL;
			break;
		}
		rtw_hal_beamform_set_vht_gid(phl->hal, msg->band_idx, gid_tbl);
		mstatus = MDL_RET_SUCCESS;
	}
	break;
	case MSG_EVT_SET_BFEE_AID:
	{
		struct snd_cmd_set_aid *cmdbuf = (struct snd_cmd_set_aid *)msg->inbuf;

		if (NULL == cmdbuf) {
			mstatus = MDL_RET_FAIL;
			break;
		}
		if (NULL == cmdbuf->sta_info) {
			mstatus = MDL_RET_FAIL;
			cmdbuf->cmd_sts = MDL_RET_FAIL;
			break;
		}
		if (RTW_HAL_STATUS_SUCCESS != rtw_hal_beamform_set_aid(
					phl->hal, cmdbuf->sta_info, cmdbuf->aid)) {
			mstatus = MDL_RET_FAIL;
			cmdbuf->cmd_sts = MDL_RET_FAIL;
			break;
		}
		cmdbuf->cmd_sts = MDL_RET_SUCCESS;
		mstatus = MDL_RET_SUCCESS;
	}
	break;
	default:
		break;
	}

	return mstatus;
}

static enum phl_mdl_ret_code
_phl_snd_cmd_module_msg_hdlr(void *dispr, void *priv,
			     struct phl_msg *msg)
{
	struct phl_info_t *phl_info = (struct phl_info_t *)priv;
	enum phl_mdl_ret_code mstatus = MDL_RET_IGNORE;

	PHL_TRACE(COMP_PHL_SOUND, _PHL_INFO_,
		  "===> %s() event id : 0x%x \n", __func__, MSG_EVT_ID_FIELD(msg->msg_id));

	if (IS_MSG_FAIL(msg->msg_id)) {
		PHL_TRACE(COMP_PHL_DBG, _PHL_WARNING_,
			  "%s: cmd dispatcher notify cmd failure: 0x%x.\n",
			  __FUNCTION__, msg->msg_id);
		mstatus = MDL_RET_FAIL;
		return mstatus;
	}

	if (IS_MSG_IN_PRE_PHASE(msg->msg_id)) {
		mstatus = _phl_snd_cmd_module_msg_msg_hdlr_pre(phl_info, msg);
	} else {
		mstatus = _phl_snd_cmd_module_msg_msg_hdlr_post(phl_info, msg);
	}

	return mstatus;
}

enum rtw_phl_status phl_snd_cmd_register_module(struct phl_info_t *phl_info)
{
	enum rtw_phl_status phl_status = RTW_PHL_STATUS_FAILURE;
	struct phl_bk_module_ops bk_ops;
	struct phl_cmd_dispatch_engine *disp_eng = &(phl_info->disp_eng);
	u8 i = 0;

	bk_ops.init = _phl_snd_cmd_module_init;
	bk_ops.deinit = _phl_snd_cmd_module_deinit;
	bk_ops.start = _phl_snd_cmd_module_start;
	bk_ops.stop = _phl_snd_cmd_module_stop;
	bk_ops.msg_hdlr = _phl_snd_cmd_module_msg_hdlr;
	bk_ops.set_info = _phl_snd_cmd_module_set_info;
	bk_ops.query_info = _phl_snd_cmd_module_query_info;


	for (i = 0; i < disp_eng->phy_num; i++) {
		phl_status = phl_disp_eng_register_module(phl_info, i,
						PHL_MDL_SOUND, &bk_ops);
		if (phl_status != RTW_PHL_STATUS_SUCCESS) {
			PHL_ERR("%s register SOUND module in cmd disp band[%d] failed\n",
				__func__, i);
			phl_status = RTW_PHL_STATUS_FAILURE;
			break;
		}
	}

	return phl_status;
}


/* Start of APIs for Core/OtherModule */

/* sub-functions */
/**
 * _phl_snd_cmd_post_set_vht_gid(...)
 * 	used by rtw_phl_snd_cmd_set_vht_gid(..)
 **/
void _phl_snd_cmd_post_set_vht_gid(void* priv, struct phl_msg* msg)
{
	struct phl_info_t *phl_info = (struct phl_info_t *)priv;

	PHL_TRACE(COMP_PHL_DBG, _PHL_INFO_, "--> %s : release memeory \n", __func__);
	if (msg->inbuf && msg->inlen){
		_os_kmem_free(phl_to_drvpriv(phl_info), msg->inbuf, msg->inlen);
	}
	/* release cmd_disp_eng control */
	_phl_snd_free_eng(phl_info, SND_CMD_DISP_CTRL_BFEE);
}
/* main-functions */
/**
 * rtw_phl_snd_cmd_set_vht_gid (...)
 * input : struct rtw_phl_gid_pos_tbl *tbl
 * 		the received VHT GID management frame's GID / Position informaion.
 **/
enum rtw_phl_status
rtw_phl_snd_cmd_set_vht_gid(void *phl,
			    struct rtw_wifi_role_t *wrole,
			    struct rtw_phl_gid_pos_tbl *tbl)
{
	enum rtw_phl_status phl_status = RTW_PHL_STATUS_FAILURE;
	struct phl_info_t *phl_info = (struct phl_info_t *)phl;
	struct phl_msg msg = {0};
	struct phl_msg_attribute attr = {0};
	void *d = phl_to_drvpriv(phl_info);
	struct rtw_phl_gid_pos_tbl *gid_tbl;

	/* acuired cmd_disp_eng control */
	if (RTW_PHL_STATUS_SUCCESS !=
		_phl_snd_aquire_eng(phl_info, SND_CMD_DISP_CTRL_BFEE)) {
			return RTW_PHL_STATUS_FAILURE;
	}

	SET_MSG_MDL_ID_FIELD(msg.msg_id, PHL_MDL_SOUND);
	SET_MSG_EVT_ID_FIELD(msg.msg_id, MSG_EVT_SET_VHT_GID);

	msg.band_idx = wrole->hw_band;

	attr.completion.completion = _phl_snd_cmd_post_set_vht_gid;
	attr.completion.priv = phl_info;
	gid_tbl = (struct rtw_phl_gid_pos_tbl *)_os_kmem_alloc(d, sizeof(struct rtw_phl_gid_pos_tbl));
	_os_mem_cpy(d, gid_tbl, tbl, sizeof(struct rtw_phl_gid_pos_tbl));

	msg.inbuf = (u8 *)gid_tbl;
	msg.inlen = sizeof(struct rtw_phl_gid_pos_tbl);

	phl_status = phl_disp_eng_send_msg(phl_info, &msg, &attr, NULL);
	if (phl_status != RTW_PHL_STATUS_SUCCESS) {
		PHL_TRACE(COMP_PHL_DBG, _PHL_ERR_, "%s: Dispr send msg fail!\n",
			  __func__);
		goto exit;
	}

	return phl_status;

exit:
	_os_kmem_free(d, gid_tbl,
		      sizeof(struct rtw_phl_gid_pos_tbl));
	/* release cmd_disp_eng control */
	_phl_snd_free_eng(phl_info, SND_CMD_DISP_CTRL_BFEE);
	return phl_status;

}

void _phl_snd_cmd_post_set_aid(void* priv, struct phl_msg* msg)
{
	struct phl_info_t *phl_info = (struct phl_info_t *)priv;
#ifdef RTW_WKARD_BFEE_SET_AID
	struct snd_cmd_set_aid *cmdbuf = NULL;
#endif

	if (msg->inbuf && msg->inlen){
#ifdef RTW_WKARD_BFEE_SET_AID
		cmdbuf = (struct snd_cmd_set_aid *)msg->inbuf;
		/* backup aid */
		if (MDL_RET_SUCCESS == cmdbuf->cmd_sts) {
			cmdbuf->sta_info->wrole->last_set_aid = cmdbuf->aid;
			PHL_TRACE(COMP_PHL_DBG, _PHL_INFO_,
				"%s : set aid (%d)\n",
				__func__, cmdbuf->sta_info->wrole->last_set_aid);
		}
#endif
		_os_kmem_free(phl_to_drvpriv(phl_info), msg->inbuf, msg->inlen);
	}
	/* release cmd_disp_eng control */
	_phl_snd_free_eng(phl_info, SND_CMD_DISP_CTRL_BFEE);
	PHL_TRACE(COMP_PHL_DBG, _PHL_INFO_, "%s : set aid complete\n", __func__);
}

enum rtw_phl_status
rtw_phl_snd_cmd_set_aid(void *phl,
			struct rtw_wifi_role_t *wrole,
			struct rtw_phl_stainfo_t *sta,
			u16 aid)
{
	enum rtw_phl_status phl_status = RTW_PHL_STATUS_FAILURE;
	struct phl_info_t *phl_info = (struct phl_info_t *)phl;
	struct phl_msg msg = {0};
	struct phl_msg_attribute attr = {0};
	void *d = phl_to_drvpriv(phl_info);
	struct snd_cmd_set_aid *cmdbuf = NULL;

	/* acuired cmd_disp_eng control */
	if (RTW_PHL_STATUS_SUCCESS !=
		_phl_snd_aquire_eng(phl_info, SND_CMD_DISP_CTRL_BFEE)) {
			return RTW_PHL_STATUS_FAILURE;
	}

	SET_MSG_MDL_ID_FIELD(msg.msg_id, PHL_MDL_SOUND);
	SET_MSG_EVT_ID_FIELD(msg.msg_id, MSG_EVT_SET_BFEE_AID);

	msg.band_idx = wrole->hw_band;

	attr.completion.completion = _phl_snd_cmd_post_set_aid;
	attr.completion.priv = phl_info;

	cmdbuf = (struct snd_cmd_set_aid *)_os_kmem_alloc(d, sizeof(struct snd_cmd_set_aid));
	cmdbuf->aid = aid;
	cmdbuf->sta_info = sta;

	msg.inbuf = (u8 *)cmdbuf;
	msg.inlen = sizeof(struct snd_cmd_set_aid);

	phl_status = phl_disp_eng_send_msg(phl_info, &msg, &attr, NULL);

	if (phl_status != RTW_PHL_STATUS_SUCCESS) {
		PHL_TRACE(COMP_PHL_DBG, _PHL_ERR_, "%s: Dispr send msg fail!\n",
			  __func__);
		goto exit;
	}

	return phl_status;

exit:
	_os_kmem_free(d, cmdbuf, sizeof(struct snd_cmd_set_aid));
	/* release cmd_disp_eng control */
	_phl_snd_free_eng(phl_info, SND_CMD_DISP_CTRL_BFEE);
	return phl_status;

}

/**
 * For other module, such as LPS, direct set aid.
 **/
enum rtw_phl_status
phl_snd_cmd_set_aid_info(struct phl_info_t *phl,
			 struct rtw_wifi_role_t *wrole,
			 struct rtw_phl_stainfo_t *sta,
			 u16 aid)
{
	enum rtw_phl_status phl_status = RTW_PHL_STATUS_FAILURE;
	struct phl_module_op_info op_info = {0};
	struct snd_cmd_set_aid cmdbuf = {0};
	PHL_TRACE(COMP_PHL_DBG, _PHL_INFO_, "--> %s\n", __func__);

	cmdbuf.aid = aid;
	cmdbuf.sta_info = sta;

	op_info.op_code = SND_CMD_OP_SET_AID;
	op_info.inbuf = (u8 *)&cmdbuf;
	op_info.inlen = sizeof(struct snd_cmd_set_aid);

	phl_status = phl_disp_eng_set_bk_module_info(phl, wrole->hw_band,
						     PHL_MDL_SOUND, &op_info);

	PHL_TRACE(COMP_PHL_DBG, _PHL_INFO_, "<-- %s\n", __func__);
	return phl_status;
}

#ifdef RTW_WKARD_BFEE_SET_AID
enum rtw_phl_status
_phl_cmd_snd_restore_aid(struct phl_info_t *phl,
			 struct rtw_wifi_role_t *wrole)
{
	enum rtw_phl_status phl_status = RTW_PHL_STATUS_SUCCESS;
	struct rtw_phl_stainfo_t *sta = NULL;

	do {
		if (MLME_NO_LINK == wrole->mstate)
			break;

		sta = rtw_phl_get_stainfo_self(phl, wrole);

		if (NULL == sta)
			break;

		if (wrole->last_set_aid == sta->aid)
			break;

		phl_status = phl_snd_cmd_set_aid_info(phl, wrole, sta, sta->aid);

	} while (0);

	return phl_status;
}
#endif


enum rtw_phl_status
_phl_snd_cmd_ntfy_ps_enter(struct phl_info_t *phl,
			   struct rtw_wifi_role_t *wrole)
{
	enum rtw_phl_status phl_status = RTW_PHL_STATUS_SUCCESS;

#ifdef RTW_WKARD_BFEE_SET_AID
	phl->phl_com->is_in_lps = 1;

	if (PHL_RTYPE_STATION == wrole->type) {
		phl_status= _phl_cmd_snd_restore_aid(phl, wrole);
	}
#endif

	/* TODO: stop BFer period sounding if in progress */
	return phl_status;
}

enum rtw_phl_status
_phl_snd_cmd_ntfy_ps_leave(struct phl_info_t *phl,
			   struct rtw_wifi_role_t *wrole)
{
	enum rtw_phl_status phl_status = RTW_PHL_STATUS_SUCCESS;

#ifdef RTW_WKARD_BFEE_SET_AID
	phl->phl_com->is_in_lps = 0;
#endif

	/* TODO: restart BFer period sounding if sounding needed */
	return phl_status;
}


enum rtw_phl_status
phl_snd_cmd_ntfy_ps(struct phl_info_t *phl,
		    struct rtw_wifi_role_t *wrole,
		    bool enter)
{
	enum rtw_phl_status phl_status = RTW_PHL_STATUS_SUCCESS;

	if (true == enter)
		phl_status = _phl_snd_cmd_ntfy_ps_enter(phl, wrole);
	else
		phl_status = _phl_snd_cmd_ntfy_ps_leave(phl, wrole);

	return phl_status;
}

#endif