qmi_nas_sender_qlinkdatanode.c

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

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

#include "common_qlinkdatanode.h"
#include "feature_macro_qlinkdatanode.h"
#include "qmi_nas_sender_qlinkdatanode.h"


/******************************External Functions*******************************/
extern int one_digit_hex_to_dec(const char digit);


/******************************External Variables*******************************/
extern int qmi_handle;
extern qmi_client_type nas_client_handle;

extern Dev_Info rsmp_dev_info;

extern unsigned long thread_id_array[THD_IDX_MAX];
bool isUimPwrDwnMssage;

/******************************Local Variables*******************************/
static pthread_t qmi_nas_sender_tid;
static pthread_mutex_t nas_sender_mutex = PTHREAD_MUTEX_INITIALIZER;
static pthread_cond_t nas_sender_cond = PTHREAD_COND_INITIALIZER;
static int nassenderStarted = 0;

static int nas_sender_pipe_rd_fd = -1;


/******************************Global Variables*******************************/
int nas_sender_pipe_wr_fd = -1;

pthread_mutex_t acq_nas_inf_mtx = PTHREAD_MUTEX_INITIALIZER;
pthread_cond_t acq_nas_inf_cnd = PTHREAD_COND_INITIALIZER;
int nas_inf_acqed = 0;

static int convert_rssi(unsigned char rssi)
{
  unsigned char siglvl = RSSI_UNKNOWN_CODE, scale = DSAT_CSQ_MAX_SIGNAL;

  if((RSSI_MIN < rssi) && (RSSI_MAX > rssi)){
    /* Round to nearest integer on scale */
    siglvl = (unsigned char)((unsigned int)(((rssi * RSSI_SLOPE + RSSI_OFFSET) * scale)/100 + 0.5));
  }else if ((RSSI_MAX <= rssi) && (RSSI_NO_SIGNAL != rssi)){
    siglvl = RSSI_TOOLO_CODE;
  }else if (RSSI_MIN >= rssi){
    siglvl = RSSI_TOOHI_CODE;  /* Capped maximum */
  }else{
    siglvl = RSSI_UNKNOWN_CODE;
  }

  return (int)siglvl;
}

//Description:
//	Convert QMI NAS PLMN value to youyou private protocol PLMN data format.
//Note:
//		mobile_country_code and mobile_network_code are all uint16_t and stored by hexadecimal data.
//	You can get the normal format of PLMN by translating them into decimal data.
static Proto_Pack_Plmn_S_Type convert_plmn(nas_plmn_type_v01 raw_plmn)
{
	char plmn_str[6] = {0};
	Proto_Pack_Plmn_S_Type conv_plmn;
	int mobile_country_code_dec = -1, mobile_network_code_dec = -1;
	
	//Convert to hex string
	sprintf(plmn_str, "%03x", raw_plmn.mobile_country_code);
	sprintf(plmn_str+3, "%03x", raw_plmn.mobile_network_code);
	//Convert to dec val
	mobile_country_code_dec = one_digit_hex_to_dec(plmn_str[0])*256+
												 one_digit_hex_to_dec(plmn_str[1])*16+
												 one_digit_hex_to_dec(plmn_str[2]);
	mobile_network_code_dec = one_digit_hex_to_dec(plmn_str[3])*256+
												 one_digit_hex_to_dec(plmn_str[4])*16+
												 one_digit_hex_to_dec(plmn_str[5]);
	//Convert to dec string
	sprintf(plmn_str, "%03d", mobile_country_code_dec);
	sprintf(plmn_str+3, "%03d", mobile_network_code_dec);
	//Convert to priv proto data type
	conv_plmn.plmn[0] = (plmn_str[0]-'0')+16*(plmn_str[1]-'0');
	conv_plmn.plmn[1] = (plmn_str[2]-'0')+16*(plmn_str[3]-'0');
	conv_plmn.plmn[2] = (plmn_str[4]-'0')+16*(plmn_str[5]-'0'); 
	
	return conv_plmn;
}

//Description:
//	Convert QMI NAS LAC value to youyou private protocol LAC data format.
static unsigned short convert_lac(unsigned short raw_lac)
{
  char lac_str[4] = {0};
  unsigned short conv_lac = 0;
  unsigned char tmp_lac[2] = {0};

  sprintf(lac_str, "%04x", raw_lac);
  tmp_lac[0] = 16*one_digit_hex_to_dec(lac_str[0])+one_digit_hex_to_dec(lac_str[1]);
  tmp_lac[1] = 16*one_digit_hex_to_dec(lac_str[2])+one_digit_hex_to_dec(lac_str[3]);
  conv_lac = tmp_lac[0] + tmp_lac[1]*256;

  return conv_lac;
}

static void nas_sender_get_signal_strength(void)
{
  qmi_client_error_type rc;
  nas_get_signal_strength_req_msg_v01 req_msg;
  nas_get_signal_strength_resp_msg_v01 resp_msg;

  req_msg.request_mask_valid = 1;
  req_msg.request_mask = QMI_NAS_REQUEST_SIG_INFO_RSSI_MASK_V01;
  LOG("QMINASSENDER (Get sig strength)\n");
  rc = qmi_client_send_msg_sync(nas_client_handle,
                                QMI_NAS_GET_SIGNAL_STRENGTH_REQ_MSG_V01,
                                &req_msg,
                                sizeof(req_msg),
                                &resp_msg,
                                sizeof(resp_msg),
                                15000);
  if(rc != QMI_NO_ERR || resp_msg.resp.result != QMI_RESULT_SUCCESS_V01){
    LOG("ERROR: qmi_client_send_msg_sync() failed. rc=%d, resp_msg.resp.error=%d.\n", rc, resp_msg.resp.error);
  }else{
    int rssi_idx = 0;

    //Check if LTE exist
    for(; rssi_idx<NAS_RSSI_LIST_MAX_V01&&rssi_idx<resp_msg.rssi_len; rssi_idx++){
      if(resp_msg.rssi[rssi_idx].radio_if == RADIO_IF_LTE){
        rsmp_dev_info.sig_strength = convert_rssi(resp_msg.rssi[rssi_idx].rssi);
        break;
      }else if(resp_msg.rssi[rssi_idx].radio_if == RADIO_IF_UMTS){
        rsmp_dev_info.sig_strength = convert_rssi(resp_msg.rssi[rssi_idx].rssi);
      }
    }

    if(rssi_idx == resp_msg.rssi_len){
      LOG("Get UMTS rssi: %d.\n", rsmp_dev_info.sig_strength);
    }else{
      LOG("Get LTE rssi: %d.\n", rsmp_dev_info.sig_strength);
    }
  }
}

static void nas_sender_get_serving_sys_inf(void)
{
  qmi_client_error_type rc;
  nas_get_serving_system_resp_msg_v01 resp_msg;

  LOG("QMINASSENDER (Get serving sys)\n");
  rc = qmi_client_send_msg_sync(nas_client_handle,
                                  QMI_NAS_GET_SERVING_SYSTEM_REQ_MSG_V01,
                                  NULL,
                                  0,
                                  &resp_msg,
                                  sizeof(resp_msg),
                                  15000);
  if(rc != QMI_NO_ERR || resp_msg.resp.result != QMI_RESULT_SUCCESS_V01){
    LOG("ERROR: qmi_client_send_msg_sync() failed. rc=%d, resp_msg.resp.error=%d.\n", rc, resp_msg.resp.error);
  }else{
    if(resp_msg.current_plmn_valid){
      Proto_Pack_Plmn_S_Type plmn_tmp_s = convert_plmn(resp_msg.current_plmn);
      rsmp_dev_info.PLMN[0] = plmn_tmp_s.plmn[0];
      rsmp_dev_info.PLMN[1] = plmn_tmp_s.plmn[1];
      rsmp_dev_info.PLMN[2] = plmn_tmp_s.plmn[2];
    }else{
      rsmp_dev_info.PLMN[0] = 0xff;
      rsmp_dev_info.PLMN[1] = 0xff;
      rsmp_dev_info.PLMN[2] = 0xff;
    }
    LOG("PLMN[0]: %02x, PLMN[1]: %02x, PLMN[2]: %02x.\n",
                rsmp_dev_info.PLMN[0], rsmp_dev_info.PLMN[1], rsmp_dev_info.PLMN[2]);

    if(resp_msg.tac_valid){
      rsmp_dev_info.LAC[0] = convert_lac(resp_msg.tac) & 0x00ff;
      rsmp_dev_info.LAC[1] = (convert_lac(resp_msg.tac) >> 8) & 0x00ff;
    }else if(resp_msg.lac_valid){
      rsmp_dev_info.LAC[0] = convert_lac(resp_msg.lac) & 0x00ff;
      rsmp_dev_info.LAC[1] = (convert_lac(resp_msg.lac) >> 8) & 0x00ff;
    }else{
      rsmp_dev_info.LAC[0] = 0xff;
      rsmp_dev_info.LAC[1] = 0xff;
    }
    LOG("LAC[0]: %02x, LAC[1]: %02x.\n",
        rsmp_dev_info.LAC[0], rsmp_dev_info.LAC[1]);
  }
}

static void nas_sender_get_sig_n_serv_sys_inf(void)
{
  qmi_client_error_type rc;
  nas_get_signal_strength_req_msg_v01 req_msg_1;
  nas_get_signal_strength_resp_msg_v01 resp_msg_1;
  nas_get_serving_system_resp_msg_v01 resp_msg_2;

//  LOG("QMINASSENDER (Get sig and serving sys)\n");

  req_msg_1.request_mask_valid = 1;
  req_msg_1.request_mask = QMI_NAS_REQUEST_SIG_INFO_RSSI_MASK_V01;
  rc = qmi_client_send_msg_sync(nas_client_handle,
                                QMI_NAS_GET_SIGNAL_STRENGTH_REQ_MSG_V01,
                                &req_msg_1,
                                sizeof(req_msg_1),
                                &resp_msg_1,
                                sizeof(resp_msg_1),
                              15000);
  if(rc != QMI_NO_ERR || resp_msg_1.resp.result != QMI_RESULT_SUCCESS_V01){
    LOG("ERROR: qmi_client_send_msg_sync() failed. rc=%d, resp_msg_1.resp.error=%d.\n", rc, resp_msg_1.resp.error);
	// isUimPwrDwnMssage = true; // add by jack 20160427 start
	// notify_RstMonitor(0x00);
#ifdef FEATURE_ENABLE_REBOOT_DEBUG
	LOG("ERROR: QMI error system reboot.\n");
	//system("reboot");
	msq_send_dev_rst_req(1);
#endif
	// add by jack 20160427 end
  }else{
    int rssi_idx = 0;

    //Check if LTE exist
    for(; rssi_idx<NAS_RSSI_LIST_MAX_V01&&rssi_idx<resp_msg_1.rssi_len; rssi_idx++){
      if(resp_msg_1.rssi[rssi_idx].radio_if == RADIO_IF_LTE){
        rsmp_dev_info.sig_strength = convert_rssi(resp_msg_1.rssi[rssi_idx].rssi);
        break;
      }else if(resp_msg_1.rssi[rssi_idx].radio_if == RADIO_IF_UMTS){
      rsmp_dev_info.sig_strength = convert_rssi(resp_msg_1.rssi[rssi_idx].rssi);
      }
    }

    if(rssi_idx == resp_msg_1.rssi_len){
      LOG("Get UMTS rssi: %d.\n",rsmp_dev_info.sig_strength);
    }else{
      LOG("Get LTE rssi: %d.\n",rsmp_dev_info.sig_strength);
    }
  }

  rc = qmi_client_send_msg_sync(nas_client_handle,
                                QMI_NAS_GET_SERVING_SYSTEM_REQ_MSG_V01,
                                NULL,
                                0,
                                &resp_msg_2,
                                sizeof(resp_msg_2),
                                15000);
  if(rc != QMI_NO_ERR || resp_msg_2.resp.result != QMI_RESULT_SUCCESS_V01){
    LOG("ERROR: qmi_client_send_msg_sync() failed. rc=%d, resp_msg_2.resp.error=%d.\n", rc, resp_msg_2.resp.error);
  }else{
    if(resp_msg_2.current_plmn_valid){
      Proto_Pack_Plmn_S_Type plmn_tmp_s = convert_plmn(resp_msg_2.current_plmn);
      rsmp_dev_info.PLMN[0] = plmn_tmp_s.plmn[0];
      rsmp_dev_info.PLMN[1] = plmn_tmp_s.plmn[1];
      rsmp_dev_info.PLMN[2] = plmn_tmp_s.plmn[2];
    }else{
      rsmp_dev_info.PLMN[0] = 0xff;
      rsmp_dev_info.PLMN[1] = 0xff;
      rsmp_dev_info.PLMN[2] = 0xff;
    }
   // LOG("PLMN[0]: %02x, PLMN[1]: %02x, PLMN[2]: %02x.\n",
     // rsmp_dev_info.PLMN[0], rsmp_dev_info.PLMN[1], rsmp_dev_info.PLMN[2]);

    if(resp_msg_2.tac_valid){
      rsmp_dev_info.LAC[0] = convert_lac(resp_msg_2.tac) & 0x00ff;
      rsmp_dev_info.LAC[1] = (convert_lac(resp_msg_2.tac) >> 8) & 0x00ff;
    }else if(resp_msg_2.lac_valid){
      rsmp_dev_info.LAC[0] = convert_lac(resp_msg_2.lac) & 0x00ff;
      rsmp_dev_info.LAC[1] = (convert_lac(resp_msg_2.lac) >> 8) & 0x00ff;
    }else{
      rsmp_dev_info.LAC[0] = 0xff;
      rsmp_dev_info.LAC[1] = 0xff;
    }
  //  LOG("LAC[0]: %02x, LAC[1]: %02x.\n",
   // rsmp_dev_info.LAC[0], rsmp_dev_info.LAC[1]);
  }
}

static void nas_sender_set_sys_sel_pref(char opt)
{
  qmi_client_error_type rc;
  nas_set_system_selection_preference_req_msg_v01 req_msg;
  nas_set_system_selection_preference_resp_msg_v01 resp_msg;

  memset(&req_msg, 0, sizeof(nas_set_system_selection_preference_req_msg_v01));
  req_msg.mode_pref_valid = 1;
  if(opt == 1)
  {
    req_msg.mode_pref = 0x08;//UMTS only
  }else{
    req_msg.mode_pref = 0x18;//UMTS + LTE
  }

  LOG("QMINASSENDER (Set sys sel pref) %d\n", opt);
  rc = qmi_client_send_msg_sync(nas_client_handle,
                                QMI_NAS_SET_SYSTEM_SELECTION_PREFERENCE_REQ_MSG_V01,
                                &req_msg,
                                sizeof(req_msg),
                                &resp_msg,
                                sizeof(resp_msg),
                                15000);
  if(rc != QMI_NO_ERR || resp_msg.resp.result != QMI_RESULT_SUCCESS_V01){
    LOG("ERROR: qmi_client_send_msg_sync() failed. rc=%d, resp_msg.resp.error=%d.\n", rc, resp_msg.resp.error);
  }else{
    LOG("qmi set sys sel pref ok!\n");
  }
}

//Param:
//	opt:
//		0x00 : Get signal info.
//		0x01 : Get serving system info.
//		0x02 : Get both signal info and serving sys info.
//		0x03 : set lte and umts
//		0x04 : set umts only
//Notice:
//	The app must acquire acq_nas_inf_mtx and wait for acq_nas_inf_cnd before calling this.
void notify_NasSender(char opt)
{
  int n = -1;

  LOG6("~~~~ notify_NasSender start (%02x) ~~~~\n", opt);
  if(opt < 0x00 || opt > 0x04){
    LOG("ERROR: Wrong param input. opt=%02x.\n", opt);
    goto __EXIT_OF_NOTIFY_NASSENDER__;
  }

  do{
    if(n == 0)
      LOG("write() failed. n=%d.\n", n);
    n = write(nas_sender_pipe_wr_fd, (void *)(&opt), 1);
  }while ((n<0 && errno==EINTR) || n==0);
  
  if(n<0){
    LOG("ERROR: write() failed. n=%d. errno=%d.\n", n, errno);
  }

__EXIT_OF_NOTIFY_NASSENDER__:
  LOG6("~~~~ notify_NasSender end ~~~~\n");
  return;
}

void NasSenderLoop(void)
{
  int n;
  int fdp1;
  fd_set rfd;
  char buf = 0xff;

  FD_ZERO(&rfd);
  FD_SET(nas_sender_pipe_rd_fd, &rfd);
  fdp1 = nas_sender_pipe_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(nas_sender_pipe_rd_fd, (void *)(&buf), sizeof(buf));
    }while((n<0 && errno==EINTR) || (n == 0));

    if(n > 0)
    {
      if(buf == 0x00){
        nas_sender_get_signal_strength();
      }else if(buf == 0x01){
        nas_sender_get_serving_sys_inf();
      }else if(buf == 0x02){
        nas_sender_get_sig_n_serv_sys_inf();
      }else if(buf == 0x03){
        nas_sender_set_sys_sel_pref(0);
      }else if(buf == 0x04){
        nas_sender_set_sys_sel_pref(1);//umts only
      }else{
        LOG("ERROR: Wrong buf (%02x) found!\n", buf);
      }

      pthread_mutex_lock(&acq_nas_inf_mtx);
      nas_inf_acqed = 1;
      pthread_cond_broadcast(&acq_nas_inf_cnd);
      pthread_mutex_unlock(&acq_nas_inf_mtx);

      LOG("QMI NAS sender completes. buf=%02x.\n", buf);
    }
  }

  LOG("ERROR: Unexpected error. NasSenderLoop() ended.\n");
  return;
}

static void *initNasSender(void *user_data)
{
  int ret;
  int nas_sender_pipe[2];

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

  pthread_mutex_lock(&nas_sender_mutex);
  nassenderStarted = 1;
  pthread_cond_broadcast(&nas_sender_cond);
  pthread_mutex_unlock(&nas_sender_mutex);

  if (ret < 0) {
    LOG("ERROR: pipe() error. errno=%d.\n", errno);
    return NULL;
  }
  
  nas_sender_pipe_wr_fd = nas_sender_pipe[1];
  nas_sender_pipe_rd_fd = nas_sender_pipe[0];
  fcntl(nas_sender_pipe_rd_fd, F_SETFL, O_NONBLOCK);

  NasSenderLoop();

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

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

  if(qmi_handle == QMI_INVALID_CLIENT_HANDLE){
    LOG("ERROR: qmi_handle doesn't exist.\n");
    return 0;
  }
  
  if(nas_client_handle == NULL){
    LOG("ERROR: nas_client_handle doesn't exist.\n");
    return 0;
  }

  nassenderStarted = 0;
  pthread_mutex_lock(&nas_sender_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_NAS_SENDER_PTHREAD_CREATE_AGAIN__:
  ret = pthread_create(&qmi_nas_sender_tid, &attr, initNasSender, (void *)(&result));
  if(ret != 0){
    if(EAGAIN == errno){
      sleep(1);
      goto __QMI_NAS_SENDER_PTHREAD_CREATE_AGAIN__;
    }

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

  thread_id_array[THD_IDX_NAS_SENDER] = (unsigned long)qmi_nas_sender_tid;

  while(nassenderStarted == 0){
    pthread_cond_wait(&nas_sender_cond, &nas_sender_mutex);
  }

  pthread_mutex_unlock(&nas_sender_mutex);

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

  return 1;
}