qmi_parser_qlinkdatanode.c

#include <stdlib.h>
#include <pthread.h>
#include <errno.h>
#include <unistd.h>
#include <fcntl.h>
#include <string.h>
#include <sys/select.h>

#include <qmi.h>
#include <qmi_client.h>
#include <device_management_service_v01.h>

#include "queue_qlinkdatanode.h"
#include "common_qlinkdatanode.h"
#include "qmi_common_qlinkdatanode.h"
#include "qmi_parser_qlinkdatanode.h"
#include "qmi_sender_qlinkdatanode.h"
#include "protocol_qlinkdatanode.h"
#include "feature_macro_qlinkdatanode.h"
#include "at_qlinkdatanode.h"
#include "socket_qlinkdatanode.h"

#ifndef FEATURE_ENABLE_QMI_PARSER_qlinkdatanode_C_LOG
#define LOG print_to_null
#endif

/******************************External Functions*******************************/
extern unsigned char get_byte_of_void_mem(void *ptr, unsigned int idx);
extern void go_to_next_state_after_removing_USIM_files(void);
extern void print_fmt_data(char *str);
extern void notify_EvtLoop(const char msg);
extern int generate_manual_APDU_and_resp_to_modem(void *cache, int cache_type);
extern int proto_update_flow_state_ext(rsmp_control_block_type * fi, rsmp_state_s_type state);
extern void power_up_virtual_usim(void);
extern void msq_send_imsi_ind(char opt);


/******************************External Variables*******************************/
extern At_Config at_config;
extern rsmp_transmit_buffer_s_type g_rsmp_transmit_buffer[THD_IDX_MAX+1];
extern rsmp_recv_buffer_s_type g_rsmp_recv_buffer;
extern QMI_Msg_Config qmi_msg_config;
extern APDU_Setting APDU_setting;

extern bool isQueued;
extern bool isProcOfInitApp;
extern bool MdmEmodemIsRcvrWhenSetPd;
extern bool g_qlinkdatanode_run_status_03;

extern qmi_client_type dms_client_handle;

extern rsmp_control_block_type flow_info;
extern Dev_Info sim5360_dev_info;
extern Net_Info rsmp_net_info;
extern int ChannelNum;
#ifdef FEATURE_ENABLE_RWLOCK_CHAN_LCK
extern pthread_rwlock_t ChannelLock;
#else
extern pthread_mutex_t ChannelLock;
#endif

extern pthread_mutex_t evtloop_mtx;
extern pthread_mutex_t log_file_mtx;

extern bool isUimActed;
extern pthread_mutex_t act_uim_mtx;
extern pthread_cond_t act_uim_cond;

extern int dms_inf_acqed;
extern pthread_mutex_t acq_dms_inf_mtx;
extern pthread_cond_t acq_dms_inf_cnd;

extern bool isUimRstOperCplt;
extern pthread_mutex_t rst_uim_mtx;
extern pthread_cond_t rst_uim_cond;

extern bool isUimPwrDwn;

extern unsigned long thread_id_array[THD_IDX_MAX];

extern int nas_inf_acqed;
extern pthread_mutex_t acq_nas_inf_mtx;
extern pthread_cond_t acq_nas_inf_cnd;
extern bool isUMTSOnlySet;

extern bool isApduSenderMtxLocked;
extern pthread_mutex_t apdu_sender_mtx;
extern bool isEnableApduSenderMtx;
extern bool isApduSentToMdm;
extern pthread_cond_t apdu_sender_cond;
extern pthread_mutex_t apdu_mtx; // 20160304 jackli

extern unsigned char  g_is7100modemstartfail;
extern bool b_enable_softwarecard_auth;
extern bool b_enable_softwarecard_ok;

extern char server_des_ki[16];
extern char server_des_opc[16];
//extern char server_des_ki_backup[16];

#ifdef FEATURE_ENABLE_SFOTCARD_DEBUG

extern unsigned int  c_softwarecard_auth_len;

#endif

  
/******************************Local Variables*******************************/
static pthread_t qmi_parser_tid;
static pthread_mutex_t parser_mutex = PTHREAD_MUTEX_INITIALIZER;
static pthread_cond_t parser_cond = PTHREAD_COND_INITIALIZER;

static int parserStarted;
static int DMS_rd_fd = -1;

/******************************Global Variables*******************************/
int DMS_wr_fd = -1;
//Description:
// If this bool var is set true, app will assembly APDU accoring to recved APDU cmd,
// and send it back to modem as incoming APDU rsp.
bool isEnableManualApdu = false;

//Function:
//Remove QMI message header.
//Return Values:
//-1: Wrong param input.
static int qmi_msg_decode(qmi_client_type user_handle,
                                void *raw_data_buf,
                                int raw_data_len,
                                void **p_decoded_data_buf,
                                int *p_decoded_data_len)
{
  int data_len;
  char tmp_buf[5];

  if(user_handle == NULL){
    LOG("QP: QMI client handle is NULL.\n");
    return -1;
  }
  
  if(raw_data_buf==NULL){
    LOG("Input buffer is empty.\n");
    return -1;
  }

  memcpy(tmp_buf, raw_data_buf, 5);

  data_len = tmp_buf[3] | (tmp_buf[4]<<8);
  LOG("data_len=%d.\n", data_len);
  *p_decoded_data_len = data_len;
  *p_decoded_data_buf = malloc(data_len);
  memset(*p_decoded_data_buf, 0, data_len);
  memcpy(*p_decoded_data_buf, raw_data_buf+5, data_len);

  return 1;
}

#define CHECK_PROACTIVE_MODE_STATE_ERROR \
{ \
  if(isProcOfInitApp == true){ \
  LOG("ERROR: isProcOfInitApp=1. No request should be received now!\n"); \
  RLS_CHANNEL_LOCK; \
  return; \
  } \
}

static void notify_transmit_apdu_to_server(void)
{
  pthread_mutex_lock(&evtloop_mtx); 
  notify_EvtLoop(0x00);
  pthread_mutex_unlock(&evtloop_mtx); 
}

static void parse_and_process_qmi_msg(void)
{
  int ret;
  rsmp_transmit_buffer_s_type *t_buf = &g_rsmp_transmit_buffer[THD_IDX_PARSER]; 
  rsmp_control_block_type *fi = &flow_info;
  APDU_Setting *as = &APDU_setting;

 #ifdef FEATURE_ENABLE_SFOTCARD_DEBUG
  BYTE Debug_rand[16];
  BYTE Debug_autn[16];

  //int i_temp = 0;

  memset(Debug_rand, 0 ,16);
  memset(Debug_autn, 0 ,16);

 #endif

  ret = qmi_msg_decode(dms_client_handle,
                        qmi_msg_config.raw_data_buf,
                        qmi_msg_config.raw_data_len,
                        &qmi_msg_config.data_buf,
                        &qmi_msg_config.data_len);
  if(ret != 1){
    LOG("qmi_msg_decode() failed. ret=%d.\n", ret);
    return;
  }



#ifdef FEATURE_ENABLE_PARSE_QMI_IND_DECODED_CONTENT_LOG
	{ //log: output decoded message
		#ifndef FEATURE_DEBUG_QXDM
			#ifndef FEATURE_DEBUG_LOG_FILE
				int i = 0;
				LOG2("**********After qmi_msg_decode**********\n");
		    	for(; i<qmi_msg_config.data_len; i++){
					LOG2("%02x ", get_byte_of_void_mem(qmi_msg_config.data_buf, i) );
				}
				LOG2("\n****************************************\n");
			#else
				int i = 1;
				
				pthread_mutex_lock(&log_file_mtx);
				LOG5("**********After qmi_msg_decode**********\n");
				for(; i<=qmi_msg_config.data_len; i++){
					if(i%20 == 1)
						LOG4(" %02x", get_byte_of_void_mem(qmi_msg_config.data_buf, i-1) );
					else
						LOG3(" %02x", get_byte_of_void_mem(qmi_msg_config.data_buf, i-1) );
					
					if(i%20 == 0) //Default val of i is 1
						LOG3("\n");
				}
				if((i-1)%20 != 0) // Avoid print redundant line break
					LOG3("\n");
				LOG4("****************************************\n");
				pthread_mutex_unlock(&log_file_mtx);
			#endif /*FEATURE_DEBUG_LOG_FILE*/
		#else
			int i = 0;
			int data_len = qmi_msg_config.data_len;
			int buf_len = 3*data_len+1;
			char buf[buf_len];
			
			memset(buf, 0, buf_len);
			for(; i<data_len; i++){
				sprintf(buf+i*3, "%02x ", get_byte_of_void_mem(qmi_msg_config.data_buf, i) );
			}
			LOG2("**********After qmi_msg_decode**********\n");
			#ifdef FEATURE_ENABLE_FMT_PRINT_DATA_LOG
				print_fmt_data(buf);
			#else
				LOG2("  %s", buf);
			#endif
			LOG2("****************************************\n");
		#endif
	}
#endif // FEATURE_ENABLE_PARSE_QMI_IND_DECODED_CONTENT_LOG

  ACQ_CHANNEL_LOCK_RD;

  switch(qmi_msg_config.msg_id)
  {
  case QMI_DMS_VSM_CTPDU_CASE1_IND_V01://97
  {
    break;
  } // case 97
  case QMI_DMS_VSM_CTPDU_CASE2_IND_V01://98
  {
    CHECK_PROACTIVE_MODE_STATE_ERROR;

    as->head_byte = get_byte_of_void_mem(qmi_msg_config.data_buf, 1);
    LOG("Save 2nd byte of APDU data (case %lu): %02x.\n", qmi_msg_config.msg_id, as->head_byte);
    if(!isEnableManualApdu)
    {
#ifdef FEATURE_NEW_APDU_COMM
      if(2 == as->proc_step)
      {
        as->proc_step = 0; //reset proc_step
        #if 1 //xiaobin modify 2016.01.08
	       //20160304 jackli
	  pthread_mutex_lock(&apdu_mtx);
	  pthread_mutex_lock(&apdu_sender_mtx);
	  notify_Sender(QMISENDER_RESPONSE_TPDU);
	  pthread_mutex_unlock(&apdu_mtx);
	  while(!isApduSentToMdm)
	  {
	    pthread_cond_wait(&apdu_sender_cond, &apdu_sender_mtx);
	  }
	  isApduSentToMdm = false;
	  pthread_mutex_unlock(&apdu_sender_mtx);
        #else
        notify_Sender(QMISENDER_RESPONSE_TPDU);
        #endif
      }
#else
      int length = qmi_msg_config.data_len;
      reset_rsmp_transmit_buffer(t_buf);
      t_buf->data = proto_encode_raw_data(qmi_msg_config.data_buf, &length, REQ_TYPE_APDU, NULL, NULL);
      t_buf->size = length;
      
      notify_transmit_apdu_to_server();
#endif // FEATURE_NEW_APDU_COMM
    }else{
      int ret = -99;

      LOG("isEnableManualApdu = 1.\n");
      ret = generate_manual_APDU_and_resp_to_modem(qmi_msg_config.data_buf, 1);
      if(ret != 1){
        LOG("ERROR: generate_manual_APDU_and_resp_to_modem() failed. ret=%d.\n", ret);
        break;
      }
    } // true == isEnableAssemblyApdu

    break;
  } // case 98
  case QMI_DMS_VSM_CTPDU_CASE3_IND_V01://99
  {
    break;
  } // case 99
  case QMI_DMS_VSM_CTPDU_CASE4_IND_V01://100
  {
    if(!isEnableManualApdu)
    {
      if(as->proc_step == 0)
      {
        as->proc_step = 1;
#ifdef  FEATURE_ENABLE_CANCEL_LOG_OUTPUT  //20160708
	LOG("Test: the status var value 01 as->proc_step=%d.\n", as->proc_step);
#endif
	 
      
        unsigned char rsp;
        rsp = get_byte_of_void_mem(qmi_msg_config.data_buf, 1); 
        reset_rsmp_recv_buffer(); 
        g_rsmp_recv_buffer.size = 1; 
        g_rsmp_recv_buffer.data = malloc(g_rsmp_recv_buffer.size); 
        memcpy(g_rsmp_recv_buffer.data, &rsp, g_rsmp_recv_buffer.size);

        memset(as->data, 0, 5);
        memcpy(as->data, qmi_msg_config.data_buf, 5);
        if(as->data[0] == 0xA0)
    	{
        	as->is2GCard = true;
		}
         //20160304 jackli
	  pthread_mutex_lock(&apdu_mtx);
	  pthread_mutex_lock(&apdu_sender_mtx);
	  notify_Sender(QMISENDER_RESPONSE_TPDU);
	  pthread_mutex_unlock(&apdu_mtx);
	  while(!isApduSentToMdm)
	  {
	    pthread_cond_wait(&apdu_sender_cond, &apdu_sender_mtx);
	  }
	  isApduSentToMdm = false;
	  pthread_mutex_unlock(&apdu_sender_mtx);

	  
        as->proc_step++;// 2

#ifdef  FEATURE_ENABLE_CANCEL_LOG_OUTPUT  //20160708
	 LOG("Test: the status var value 02 isApduSentToMdm=%d.\n", isApduSentToMdm);
        LOG("Test: the status var value 03 as->proc_step=%d.\n", as->proc_step);
#endif
        
      }
      else if(as->proc_step == 2)
      {
        // do not reset the proc_step
        unsigned char buf[qmi_msg_config.data_len+5]; 
        int length = qmi_msg_config.data_len+5; 

        reset_rsmp_transmit_buffer(t_buf); 

        memset((void *)buf, 0, qmi_msg_config.data_len+5);
        memcpy((void *)buf, as->data, 5); 
        as->head_byte = buf[1];
        LOG("the authenticate request data comes\n");
	 msq_send_qlinkdatanode_run_stat_ind(20); // 20161011 by jack li.
        memcpy((void *)(buf+5), qmi_msg_config.data_buf, qmi_msg_config.data_len); 
        t_buf->data = proto_encode_raw_data(buf, &length, REQ_TYPE_APDU, NULL, NULL); 
        t_buf->size = length;
	 g_qlinkdatanode_run_status_03 = true;
	 LOG("Debug: g_qlinkdatanode_run_status_03 = %d\n",g_qlinkdatanode_run_status_03);
	 
#ifdef FEATURE_ENABLE_SFOTCARD_DEBUG
	LOG(" Debug: b_enable_softwarecard_ok = %d \n",b_enable_softwarecard_ok);
	if (b_enable_softwarecard_ok){

		b_enable_softwarecard_auth = true;
		memcpy(Debug_rand, (char *)&qmi_msg_config.data_buf[1],16);
		memcpy(Debug_autn, (char *)&qmi_msg_config.data_buf[18],16);
		vsm_qmi_softsim_authentication(Debug_rand,Debug_autn,server_des_ki,server_des_opc);
		
	}

	 LOG("Debug: c_softwarecard_auth_len = %d\n", c_softwarecard_auth_len);

#endif

        notify_transmit_apdu_to_server();


      }else
      {
        LOG("ERROR: Wrong procedure step found! as->proc_step=%d.\n", as->proc_step);
        RLS_CHANNEL_LOCK;
        return 0;
      }
    }
    else 
    {
      int ret = -99;
      unsigned char rsp;
      
     
 #ifdef  FEATURE_ENABLE_CANCEL_LOG_OUTPUT  //20160708
	 LOG("isEnableManualApdu = 1.\n");
#endif

      rsp = get_byte_of_void_mem(qmi_msg_config.data_buf, 1); 
      if( qmi_msg_config.data_len == 5 && rsp == 0x88)
      {
        ret = generate_manual_APDU_and_resp_to_modem(qmi_msg_config.data_buf, 2);
      }else
      {
        as->head_byte = get_byte_of_void_mem(as->data, 1);
        LOG("Save 2nd byte of APDU data (case %lu): %02x.\n", qmi_msg_config.msg_id, as->head_byte);
        ret = generate_manual_APDU_and_resp_to_modem(as->data, 1);
      }
        
      if(ret != 1)
      {
        LOG("ERROR: generate_manual_APDU_and_resp_to_modem() failed. ret=%d.\n", ret);
        break;
      }
    }
    
    break;
  } // case 100
  case QMI_DMS_VSM_PKT_DATA_IND_V01:
  {
    if(*((unsigned char *)(qmi_msg_config.data_buf)) != 1)
    {
      //Basically, not possible to reach here.
      LOG("ERROR: USIM file setup failed!\n");
      break;
    }

    LOG("USIM file setup succeed.\n");

    msq_send_imsi_ind(0x01);

    //Note:
    //ID auth msg was being prepared to send when USIM had been powered down already. So no ID auth msg was being 
    //processed when USIM was being powered down.
    if(!isUimPwrDwn)
    {
      if(MdmEmodemIsRcvrWhenSetPd)
      {
        LOG("ERROR: isUimPwrDwn=0 and MdmEmodemIsRcvrWhenSetPd=1! ID auth msg should be sent when Usim has been powered down.\n");
        break; // Not release lock
      }else
      {
#ifdef FEATURE_SET_3G_ONLY_ON_REJ_15
        pthread_mutex_lock(&acq_dms_inf_mtx);
        notify_DmsSender(0x02);
        while(0 == dms_inf_acqed){
          pthread_cond_wait(&acq_dms_inf_cnd, &acq_dms_inf_mtx);
        }
        dms_inf_acqed = 0;
        pthread_mutex_unlock(&acq_dms_inf_mtx);

        isUMTSOnlySet = (14 == rsmp_net_info.cnmp_val);
              
        if(isUMTSOnlySet)
        {
          pthread_mutex_lock(&acq_nas_inf_mtx);
          notify_NasSender(0x03);
          while(0 == nas_inf_acqed){
            pthread_cond_wait(&acq_nas_inf_cnd, &acq_nas_inf_mtx);
          }
          nas_inf_acqed = 0; // Reset it for using it again
          pthread_mutex_unlock(&acq_nas_inf_mtx);
          isUMTSOnlySet = false;
        }
#endif 
        proto_update_flow_state_ext(fi, FLOW_STATE_SET_OPER_MODE);
        notify_Sender(QMISENDER_SET_ONLINE);
      }
    }else
    {
      if(MdmEmodemIsRcvrWhenSetPd)
      { //Add by rjf at 20151027
        MdmEmodemIsRcvrWhenSetPd = false; // Reset it
      }

      //FIXME: Update flow_info.flow_state but not care the pointer "fi".
      power_up_virtual_usim();
    }

    break;
  } // case 101
  case QMI_DMS_DEL_VIRT_USIM_DATA_IND_V01:
  {
    if(*((unsigned char *)(qmi_msg_config.data_buf)) != 1)
    {
      //Basically, not possible to reach here.
      LOG("ERROR: USIM file del failed!\n");
      break;
    }
    go_to_next_state_after_removing_USIM_files();
    break;
  }

  default:
  {
    LOG("ERROR: Wrong QMI msg ID found! ID=%d.\n", qmi_msg_config.msg_id);
    RLS_CHANNEL_LOCK;
    return;
  }
  }

  RLS_CHANNEL_LOCK;

  return;
}

void ParserLoop(void)
{
  int n;
  int fdp1;
  fd_set rfd;
  char buf;

  FD_ZERO(&rfd);
  FD_SET(DMS_rd_fd, &rfd);
  fdp1 = DMS_rd_fd+1;

  for(;;){
    n = select(fdp1, &rfd, NULL, NULL, NULL);
    if (n < 0) {
      if (errno == EINTR) continue;

      LOG("select() error. errno=%s(%d).\n", strerror(errno), errno);
      continue;
    }else if(n == 0)
      continue;

    do{
      n = read(DMS_rd_fd, &buf, 1);
    }while ((n<0 && errno==EINTR) || n>0);

    //NOTE: Any error will be ignored
    if(0 == buf)
    	{
    parse_and_process_qmi_msg();
    	}else
	{
    		 pthread_mutex_lock(&apdu_mtx);
	  pthread_mutex_lock(&apdu_sender_mtx);
	  notify_Sender(QMISENDER_RESPONSE_TPDU);
	  pthread_mutex_unlock(&apdu_mtx);
	  while(!isApduSentToMdm)
	  {
	    pthread_cond_wait(&apdu_sender_cond, &apdu_sender_mtx);
	  }
	  isApduSentToMdm = false;
	  pthread_mutex_unlock(&apdu_sender_mtx);
    		}
  }

  LOG("Debug: Parser loop ended.\n");
  return;
}

static void *initParser(void *user_data)
{
  int ret;
  int DMS_pipe[2];

  ret = pipe(DMS_pipe);
  *((int *)user_data) = ret;

  pthread_mutex_lock(&parser_mutex);
  parserStarted = 1;
  pthread_cond_broadcast(&parser_cond);
  pthread_mutex_unlock(&parser_mutex);

  if (ret < 0) {
    LOG("ERROR: pipe() error.\n");
    return NULL;
  }
  DMS_wr_fd = DMS_pipe[1];
  DMS_rd_fd = DMS_pipe[0];
  fcntl(DMS_rd_fd, F_SETFL, O_NONBLOCK);

  ParserLoop();

  LOG("ERROR: ParserLoop() ended unexpectedly!\n");
  
  while(1){
    sleep(0x00ffffff);
  }
  return NULL;
}

//return value
// 0: error
// 1: success
int startQMIParserThread(void)
{
  int ret;
  pthread_attr_t attr;
  int result;

  parserStarted = 0;
  pthread_mutex_lock(&parser_mutex);

  pthread_attr_init (&attr);
  pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);

#ifdef FEATURE_ENABLE_SYSTEM_RESTORATION
  pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL);
  pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, NULL);
#endif

__QMI_PARSER_PTHREAD_CREATE_AGAIN__:
  ret = pthread_create(&qmi_parser_tid, &attr, initParser, (void *)(&result));
  if(ret != 0){
    if(EAGAIN == errno){
      sleep(1);
      goto __QMI_PARSER_PTHREAD_CREATE_AGAIN__;
    }

    LOG("ERROR: pthread_create() failed. errno=%d.\n", errno);
    return 0;
  }

  thread_id_array[THD_IDX_PARSER] = (unsigned long)qmi_parser_tid;

  while(parserStarted == 0){
    pthread_cond_wait(&parser_cond, &parser_mutex);
  }

  pthread_mutex_unlock(&parser_mutex);

  if(result < 0){
    LOG("ERROR: initParser() error.\n.");
    return 0;
  }

  return 1;
}