/*===========================================================================
FUNCTION ftm_nfc_dispatch
DESCRIPTION
This is the function which will be called by the NFC FTM layer callback function
registered with the DIAG service./
DEPENDENCIES
RETURN VALUE
RETURN rsp pointer(containing the NFCC rsp packets) to the callback function
(subsequently for DIAG service)
SIDE EFFECTS
None
===========================================================================*/
void* ftm_nfc_dispatch(ftm_nfc_pkt_type *nfc_ftm_pkt, uint16 pkt_len)
{
ftm_nfc_i2c_write_rsp_pkt_type *i2c_write_rsp = NULL;
ftm_nfc_i2c_read_rsp_pkt_type *i2c_read_rsp = NULL;
ftm_nfc_pkt_type *rsp = NULL;
ftm_nfc_data_rsp_pkt_type *nfc_data_rsp = NULL;
struct timespec time_sec;
int sem_status;
printf("NFC FTM : nfc ftm mode requested \n");
if(nfc_ftm_pkt == NULL)
{
printf("Error : NULL packet recieved from DIAG \n");
goto error_case;
}
/* Start nfc_ftm_thread which will process all requests as per
state machine flow. By Default First state will be NCI_HAL_INIT*/
if(!nfc_ftmthread)
{
if(sem_init(&semaphore_halcmd_complete, 0, 1) != 0)
{
printf("NFC FTM :semaphore_halcmd_complete creation failed \n");
goto error_case;
}
if(sem_init(&semaphore_nfcftmcmd_complete, 0, 0) != 0)
{
printf("NFC FTM :semaphore_nfcftmcmd_complete creation failed \n");
goto error_case;
}
printf("NFC FTM : nfc ftm thread is being started \n");
pthread_create(&nfc_thread_handle, NULL, nfc_ftm_thread, NULL);
nfc_ftmthread = TRUE;
}
/* parse the diag packet to identify the NFC FTM command which needs to be sent
to QCA 1990*/
if(nfc_ftm_pkt->ftm_nfc_hdr.nfc_cmd_len > 2)
{
len = nfc_ftm_pkt->ftm_nfc_hdr.nfc_cmd_len-2;
}
else
{
/*Wrong nfc ftm packet*/
goto error_case;
}
switch(nfc_ftm_pkt->ftm_nfc_hdr.nfc_cmd_id)
{
case FTM_NFC_I2C_SLAVE_WRITE:
i2c_req_write = TRUE;
break;
case FTM_NFC_I2C_SLAVE_READ:
i2c_num_of_reg_to_read = len;
i2c_req_read = TRUE;
break;
case FTM_NFC_NFCC_COMMAND:
case FTM_NFC_SEND_DATA:
break;
default :
goto error_case;
break;
}
/*copy command to send it further to QCA1990*/
nfc_cmd_buff = (uint8 *)malloc(len+1);
if(nfc_cmd_buff)
{
memcpy(nfc_cmd_buff, nfc_ftm_pkt->nci_data, len);
}
else
{
printf("Mem allocation failed for cmd storage");
goto error_case;
}
/*send the command */
sem_post(&semaphore_halcmd_complete);
printf("\nwaiting for nfc ftm response \n");
if (clock_gettime(CLOCK_REALTIME, &time_sec) == -1)
{
printf("get clock_gettime error");
}
time_sec.tv_sec += FTM_NFC_CMD_CMPL_TIMEOUT;
sem_status = sem_timedwait(&semaphore_nfcftmcmd_complete,&time_sec);
if(sem_status == -1)
{
printf("nfc ftm command timed out\n");
goto error_case;
}
if(!hal_opened)
{
/*Hal open is failed */
free(nfc_cmd_buff);
hal_state = NCI_HAL_INIT;
goto error_case;
}
printf("\n\n *****Framing the response to send back to Diag service******** \n\n");
/* Frame the response as per the cmd request*/
switch(nfc_ftm_pkt->ftm_nfc_hdr.nfc_cmd_id)
{
case FTM_NFC_I2C_SLAVE_WRITE:
printf("Framing the response for FTM_NFC_I2C_SLAVE_WRITE cmd \n");
i2c_write_rsp = (ftm_nfc_i2c_write_rsp_pkt_type*)diagpkt_subsys_alloc(DIAG_SUBSYS_FTM,
FTM_NFC_CMD_CODE,
sizeof(ftm_nfc_i2c_write_rsp_pkt_type));
if(i2c_write_rsp)
{
i2c_write_rsp->nfc_i2c_slave_status = i2c_status;
i2c_status = 0;
i2c_cmd_cnt = 0;
i2c_req_write = FALSE;
}
break;
case FTM_NFC_I2C_SLAVE_READ:
printf("Framing the response for FTM_NFC_I2C_SLAVE_READ cmd \n");
i2c_read_rsp = (ftm_nfc_i2c_read_rsp_pkt_type*)diagpkt_subsys_alloc(DIAG_SUBSYS_FTM,
FTM_NFC_CMD_CODE,
sizeof(ftm_nfc_i2c_read_rsp_pkt_type));
if(i2c_read_rsp)
{
i2c_read_rsp->ftm_nfc_hdr.nfc_cmd_id = FTM_NFC_I2C_SLAVE_READ;
i2c_read_rsp->ftm_nfc_hdr.nfc_cmd_len = 2+(2*i2c_num_of_reg_to_read);
i2c_read_rsp->nfc_i2c_slave_status = i2c_status;
if(i2c_status == 0x00)
{
i2c_read_rsp->nfc_nb_reg_reads = i2c_num_of_reg_to_read;
}
else
{
i2c_read_rsp->nfc_nb_reg_reads = 0x00; // error case so return num of read as 0x00.
}
memcpy(i2c_read_rsp->i2c_reg_read_rsp, i2c_reg_read_data, (i2c_num_of_reg_to_read*2));
i2c_cmd_cnt = 0;
}
break;
case FTM_NFC_NFCC_COMMAND:
printf("Framing the response for FTM_NFC_NFCC_COMMAND cmd \n");
if(response_buff && res_len)
{
rsp = (ftm_nfc_pkt_type*)diagpkt_subsys_alloc(DIAG_SUBSYS_FTM,
FTM_NFC_CMD_CODE,
sizeof(ftm_nfc_pkt_type));
if(rsp)
{
rsp->ftm_nfc_hdr.nfc_cmd_id = FTM_NFC_NFCC_COMMAND;
rsp->ftm_nfc_hdr.nfc_cmd_len = 2+res_len;
rsp->nfc_nci_pkt_len = res_len;
memcpy(rsp->nci_data, response_buff, res_len);
free(response_buff);
response_buff = 0;
res_len = 0;
}
}
else
printf("ftm_nfc_dispatch : response_buff = 0x%x, res_len = %d", response_buff, res_len);
break;
case FTM_NFC_SEND_DATA:
printf("Framing the response for FTM_NFC_SEND_DATA cmd \n");
nfc_data_rsp = (ftm_nfc_data_rsp_pkt_type*)diagpkt_subsys_alloc(DIAG_SUBSYS_FTM,
FTM_NFC_CMD_CODE,
sizeof(ftm_nfc_data_rsp_pkt_type));
if(nfc_data_rsp)
{
nfc_data_rsp->ftm_nfc_hdr.nfc_cmd_id = FTM_NFC_SEND_DATA;
nfc_data_rsp->ftm_nfc_hdr.nfc_cmd_len = 0;/*Rsp as per the NFC FTM data rsp req*/
}
break;
default:
goto error_case;
break;
}
free(nfc_cmd_buff);
hal_state = NCI_HAL_WRITE;
if(async_msg_available)
{
printf(" Some async message available.. committing now.\n");
hal_state = NCI_HAL_ASYNC_LOG;
sem_post(&semaphore_halcmd_complete);
}
wait_rsp = FALSE;
if(nfc_ftm_pkt->ftm_nfc_hdr.nfc_cmd_id == FTM_NFC_I2C_SLAVE_WRITE)
{
return(void*)i2c_write_rsp;
}
else if(nfc_ftm_pkt->ftm_nfc_hdr.nfc_cmd_id == FTM_NFC_I2C_SLAVE_READ)
{
i2c_req_read = FALSE;
return(void*)i2c_read_rsp;
}
else if(nfc_ftm_pkt->ftm_nfc_hdr.nfc_cmd_id == FTM_NFC_NFCC_COMMAND)
{
return(void*)rsp;
}
else
{
return(void*)nfc_data_rsp;
}
error_case:
return NULL;
}
1 - CMD type
FC00 - VS Command OpCode
*/
PACK(void *) cnss_wlan_handle(PACK(void *)req_pkt, uint16_t pkt_len)
{
PACK(void *)rsp = NULL;
uint8_t *pkt_ptr = (uint8_t *)req_pkt + 4;
uint16_t p_len, p_opcode;
int32_t ret = 0, i = 0;
char cmd[BUF_SIZ] = {0};
/* Allocate the same length as the request
*/
rsp = diagpkt_subsys_alloc( DIAG_SUBSYS_WLAN, CNSS_WLAN_DIAG, pkt_len);
if (rsp != NULL && pkt_len > 3)
{
p_len = *(pkt_ptr+3); /* VS Command packet length */
p_opcode = (*(pkt_ptr+2) << 8) | *(pkt_ptr+1);
debug_printf(
"%s : p_len: %d, pkt_len -8: %d, p_opcode:%.04x cmd = %d\n",
__func__, p_len, pkt_len -8, p_opcode, *pkt_ptr
);
if (p_len !=(pkt_len - 8) || ( p_opcode != 0xFD00)) {
debug_printf("%s:Error in p_len or p_opcode ", __func__ );
return rsp;
}
memcpy(rsp, req_pkt, pkt_len);
if (*pkt_ptr == CNSS_WLAN_SSR_TYPE && p_len > 1) {
/* get ID */
