Pastebin launched a little side project called VERYVIRAL.com, check it out ;-) Want more features on Pastebin? Sign Up, it's FREE!
Guest

qmi_scanner.c

By: a guest on May 14th, 2012  |  syntax: C  |  size: 22.64 KB  |  views: 855  |  expires: Never
download  |  raw  |  embed  |  report abuse  |  print
Text below is selected. Please press Ctrl+C to copy to your clipboard. (⌘+C on Mac)
  1. /*  
  2.  *    ~ Simple 3GPP Network Scanner using the Qualcomm MSM Interface (QMI) ~
  3.  *
  4.  *                              by plastinka
  5.  *  
  6.  *  WARNING: This is Research Code! For testing purposes only!
  7.  *
  8.  *  This program is free software; you can redistribute it and/or modify
  9.  *  it under the terms of the GNU General Public License version 2 as
  10.  *  published by the Free Software Foundation.
  11.  *    
  12.  *  This program is distributed in the hope that it will be useful,
  13.  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  14.  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15.  *  GNU General Public License for more details.
  16.  *
  17.  *  You should have received a copy of the GNU General Public License
  18.  *  along with this program; if not, write to the Free Software
  19.  *  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA
  20.  *
  21.  *
  22.  *  Build command: gcc -o qmi_scanner qmi_scanner.c
  23.  */
  24.  
  25. #include <sys/types.h>
  26. #include <sys/select.h>
  27. #include <sys/stat.h>
  28. #include <sys/time.h>
  29. #include <sys/ioctl.h>
  30. #include <sys/socket.h>
  31.  
  32. #include <asm/types.h>
  33.  
  34. #include <termio.h>
  35. #include <syslog.h>
  36. #include <time.h>
  37. #include <getopt.h>
  38. #include <termios.h>
  39. #include <err.h>
  40. #include <errno.h>
  41. #include <fcntl.h>
  42. #include <stdarg.h>
  43. #include <stdio.h>
  44. #include <stdlib.h>
  45. #include <stdint.h>
  46. #include <string.h>
  47. #include <unistd.h>
  48.  
  49. #ifndef TRUE
  50. #define TRUE 1
  51. #endif
  52. #ifndef FALSE
  53. #define FALSE 0
  54. #endif
  55.  
  56. #define   QMI_MSG_REQUEST      0x00
  57. #define   QMI_MSG_RESPONSE     0x02
  58. #define   QMI_MSG_INDICATION   0x04
  59.  
  60.  
  61. #define   QMI_CTL   0x00      //Control Service
  62. #define   QMI_WDS   0x01      //Wireless Data Service
  63. #define   QMI_DMS   0x02      //Device Managment Service
  64. #define   QMI_NAS   0x03      //Network Access Service
  65.  
  66. #define   QMI_CTL_SET_INSTANCE_ID       0x0020       //Set the unique link instance ID
  67. #define   QMI_CTL_GET_VERSION_INFO      0x0021       //Get supported service version info
  68. #define   QMI_CTL_GET_CLIENT_ID         0x0022       //Get a unique client ID
  69. #define   QMI_CTL_RELEASE_CLIENT_ID     0x0023       //Release the unique client ID
  70. #define   QMI_CTL_REVOKE_CLIENT_ID_IND  0x0024       //Indication of client ID revocation
  71. #define   QMI_CTL_INVALID_CLIENT_ID     0x0025       //Indication of invalid client ID
  72. #define   QMI_CTL_SET_DATA_FORMAT       0x0026       //Set host driver data format
  73. #define   QMI_CTL_SYNC                  0x0027       //Synchronize client/server
  74. #define   QMI_CTL_SYNC_IND              0x0027       //Synchronize indication
  75. #define   QMI_CTL_SET_EVENT             0x0028       //Set event report conditions
  76. #define   QMI_CTL_EVENT_IND             0x0028       //Event report indication
  77.  
  78. #define   QMI_DMS_GET_BAND_CAPS         0x0045       //This message requests the band capability of the device
  79.  
  80. #define   QMI_NAS_RESET                 0x0000       //Reset NAS service state variables
  81. #define   QMI_NAS_ABORT                 0x0001       //Abort previously issued NAS command
  82. #define   QMI_NAS_SET_EVENT             0x0002       //Set NAS state report conditions
  83. #define   QMI_NAS_EVENT_IND             0x0002       //Connection state report indication
  84. #define   QMI_NAS_SET_REG_EVENT         0x0003       //Set NAS registration report conditions
  85. #define   QMI_NAS_SCAN_NETS             0x0021       //Scan for visible network
  86. #define   QMI_NAS_GET_RF_BAND_INFO      0x0031       //Queries radio band/channel information regarding the system currently providing service
  87. #define   QMI_NAS_SET_SYS_SELECTION     0x0033       //Sets the different system selection preferences of the device
  88. #define   QMI_NAS_GET_SYS_SELECTION     0x0034       //Queries the different system selection preferences of the device
  89.  
  90. #define   QMI_RESULT_SUCCESS            0x0000
  91. #define   QMI_RESULT_FAILURE            0x0001
  92.  
  93. #define   QMI_ERR_NONE                  0x0000
  94. #define   QMI_ERR_MALFORMED_MSG         0x0001
  95. #define   QMI_ERR_NO_MEMORY             0x0002
  96. #define   QMI_ERR_INTERNAL              0x0003
  97. #define   QMI_ERR_ABORTED               0x0004
  98. #define   QMI_ERR_CLIENT_IDS_EXHAUSTED  0x0005
  99. #define   QMI_ERR_INVALID_CLIENT_ID     0x0007
  100.  
  101.  
  102. #define QMI_TLV_TYPE_RESULT_CODE        0x02
  103.  
  104. //CTL is always client zero
  105. #define QMI_CTL_CLIENT 0x0000
  106.  
  107. #define RAT_GSM   0x04
  108. #define RAT_UMTS  0x08
  109. #define RAT_LTE   0x10
  110.  
  111. #define LTE800    0x00080000  //Band 20
  112. #define LTE1800   0x00000004  //Band 3
  113. #define LTE2600   0x00000040  //Band 7
  114. #define LTE2100   0x00000001  //Band 1
  115. #define GSM1800   0x00000080
  116. #define GSM900E   0x00000100
  117. #define GSM900P   0x00000200
  118. #define GSM850    0x00080000
  119. #define GSM1900   0x00200000
  120. #define UMTS2100  0x00400000
  121.  
  122. //terminal control
  123. #define SET_RED      fprintf(stderr,"\033[1;31;40\155");
  124. #define SET_GREEN    fprintf(stderr,"\033[0;32;40\155");
  125. #define SET_WHITE    fprintf(stderr,"\033[0;37;40\155");
  126. #define SET_LWHITE   fprintf(stderr,"\033[1;37;40\155");
  127. #define SET_DEBUG    fprintf(stderr,"\033[1;30;40\155");
  128. #define SET_DBLUE    fprintf(stderr,"\033[0;34;40\155");
  129. #define SET_YELLOW   fprintf(stderr,"\033[1;33;40\155");
  130. #define SET_CYAN     fprintf(stderr,"\033[0;35;40\155");
  131. #define SET_BLUE     fprintf(stderr,"\033[0;34;40\155");
  132. #define CURSOR_ON    fprintf(stderr,"\033[?25h");
  133. #define CURSOR_OFF   fprintf(stderr,"\033[?25l");
  134. #define CLS          fprintf(stderr,"\033[2J");
  135.  
  136. typedef struct qmux
  137. {
  138.    uint8_t  tf;
  139.    uint16_t len;
  140.    uint8_t  cflags;
  141.    uint8_t  stype;
  142.    uint8_t  cid;
  143. }__attribute__((__packed__)) qmx_t;
  144.  
  145. typedef struct qtrans
  146. {
  147.    struct qmux qmh;
  148.    uint8_t          cflags;
  149.    uint16_t         tid;
  150.    uint16_t         msgid;
  151.    uint16_t         msgsize;
  152. } __attribute__((__packed__)) qmi_t;
  153.  
  154. typedef struct qtrans_ctl
  155. {
  156.    struct qmux qmh;
  157.    uint8_t          cflags;
  158.    uint8_t          tid;
  159.    uint16_t         msgid;
  160.    uint16_t         msgsize;
  161. } __attribute__((__packed__)) qmictl_t;
  162.  
  163.  
  164. struct result_code
  165. {
  166.     uint16_t qmi_result;
  167.     uint16_t qmi_error;
  168. } __attribute__((__packed__));
  169.  
  170.  
  171. int debug = 1;
  172.  
  173. // default qmi device
  174. char qmi_dev[32];// = "/dev/cdc-wdm0";
  175.  
  176. // service client id's
  177. uint16_t nas_cid = 0;
  178.  
  179. // service transaction id's
  180. uint8_t ctl_tid = 1;
  181. uint8_t nas_tid = 1;
  182.  
  183. uint8_t rbuf[2048];
  184. uint8_t wbuf[512];
  185.  
  186. int qmi_fd;
  187.  
  188. struct result_code rc;
  189.  
  190. static void print_buf(const char* detail,const char* buf,size_t len)
  191. {
  192.     int i = 0, z;
  193.     int newline = FALSE, indent = FALSE;
  194.     char f[512];
  195.     unsigned int flen;
  196.  
  197.     snprintf(f,500,"%s (%zu)  ",detail,len);
  198.     flen = strlen(f);
  199.     printf("%s",f);
  200.  
  201.     for (i = 0; i < len; i++)
  202.     {
  203.         if(indent)
  204.         {
  205.             z = flen;
  206.             while (z--)
  207.                 printf(" ");
  208.             indent = FALSE;
  209.         }
  210.         printf("%02x ", buf[i]&0xFF);
  211.         if(((i + 1) % 32) == 0)
  212.         {
  213.             printf("\n");
  214.             newline = TRUE;
  215.             indent = TRUE;
  216.         }else{
  217.             newline = FALSE;
  218.         }
  219.     }
  220.  
  221.     if (!newline)
  222.         printf("\n");
  223.  
  224. }
  225.  
  226. static int tlv_get2(void* msg,uint16_t msgsize,uint8_t tlv_type,void* buf,uint16_t bufsize)
  227. {
  228.    int pos;
  229.    uint16_t tlv_size = 0;
  230.  
  231.    memset(buf,0x00,bufsize);
  232.  
  233.    for(pos = 0;pos + 3 < msgsize; pos += tlv_size + 3)
  234.    {
  235.       tlv_size = *(uint16_t*)(msg+pos+1);
  236.       if(*(uint8_t*)(msg + pos) == tlv_type)
  237.       {
  238.          if(bufsize < tlv_size)
  239.             return -1;
  240.  
  241.          memcpy(buf,msg+pos+3,tlv_size);
  242.          return 0;
  243.       }
  244.    }
  245.  
  246.    return -1;
  247. }
  248.  
  249.  
  250. static int get_result_code(void* msg,uint16_t msgsize)
  251. {
  252.    int      pos;
  253.    uint16_t tlv_size = 0;
  254.    uint16_t r[2];
  255.  
  256.    for(pos = 0;pos + 3 < msgsize;pos += tlv_size + 3)
  257.    {
  258.       tlv_size = *(uint16_t*)(msg+pos+1);
  259.       if(*(uint8_t*)(msg+pos) == QMI_TLV_TYPE_RESULT_CODE)
  260.       {
  261.          memcpy(&r,msg+pos+3,4);
  262.          rc.qmi_result = le16toh(r[0]);
  263.          rc.qmi_error  = le16toh(r[1]);
  264.          if(debug > 3)
  265.             printf("Result: 0x%04x Error: 0x%04x\n",rc.qmi_result,rc.qmi_error);
  266.          return rc.qmi_result;
  267.       }
  268.    }
  269. }
  270.  
  271.  
  272. static int send_msg(void* msg,size_t msglen)
  273. {
  274.    ssize_t ret;
  275.  
  276.    if(debug > 2)
  277.    {
  278.       SET_DBLUE;
  279.       print_buf("W ",msg,msglen);
  280.       SET_WHITE;
  281.    }
  282.  
  283.    ret = write(qmi_fd,msg,msglen);
  284.  
  285.    free(msg);
  286.    msg = (void*)NULL;
  287.  
  288.    if(ret != msglen)
  289.    {
  290.       fprintf(stderr,"Failed to write: wrote %zd err %d\n", ret, errno);
  291.       return 0;
  292.    }
  293.  
  294.    return ret;
  295. }
  296.  
  297.  
  298. static size_t read_msg(int skip_ind,int timewait)
  299. {
  300.     ssize_t num;
  301.     fd_set in;
  302.     int result,i,n,len;
  303.     unsigned char c;
  304.     qmi_t* resp;
  305.     struct stat sb;
  306.     struct timeval timeout;
  307.  
  308.     for(;;)
  309.     {
  310.        timeout.tv_sec  = timewait;
  311.        timeout.tv_usec = 0;
  312.  
  313.        if(stat(qmi_dev,&sb) == -1)
  314.        {
  315.           perror("stat");
  316.           SET_LWHITE;
  317.           exit(EXIT_FAILURE);
  318.        }
  319.        
  320.        FD_ZERO(&in);
  321.        FD_SET(qmi_fd,&in);
  322.  
  323.        result = select(qmi_fd+1, &in, NULL, NULL, &timeout);
  324.  
  325.        if(result < 1)
  326.            return result;
  327.  
  328.        memset(rbuf,2048,0x00);
  329.  
  330.        errno = 0;
  331.  
  332.        for(i = 0,n = 1,len = 2;i <= len && n == 1;i++)
  333.        {
  334.           n = read(qmi_fd,&c,1);
  335.           if(i == 1) len = c;
  336.           rbuf[i] = c;
  337.        }
  338.  
  339.        //FIXME: How to deal with these server syncs???
  340.        if(rbuf[4] == 0x00 && rbuf[8] == 0x27)
  341.        {
  342.           if(debug > 1)
  343.           {
  344.              printf("Got QMI CTL Server Sync Indication Message:\n");
  345.              if(debug > 2)
  346.              {
  347.                 SET_DEBUG;
  348.                 print_buf("R ",rbuf,len+1);
  349.                 SET_WHITE;
  350.              }
  351.           }
  352.           continue;
  353.        }
  354.  
  355.  
  356.        if(skip_ind && rbuf[6] == QMI_MSG_INDICATION)
  357.        {
  358.           if(debug > 1)
  359.           {
  360.              resp = (qmi_t*)rbuf;
  361.              printf("Got QMI Service Indication Message:\n");
  362.              if(debug > 2)
  363.              {
  364.                 SET_DEBUG;
  365.                 print_buf("R ",rbuf,len+1);
  366.                 SET_WHITE;
  367.              }
  368.           }
  369.           continue;
  370.        }
  371.  
  372.        break;
  373.     }
  374.  
  375.     if(debug > 2)
  376.     {
  377.        SET_DEBUG;
  378.        print_buf("R ",rbuf,len+1);
  379.        SET_WHITE;
  380.     }
  381.  
  382.     return len+1;
  383. }
  384.  
  385.  
  386.  
  387. static int get_cid(uint16_t* cid,uint8_t service_type)
  388. {
  389.    size_t  rlen;
  390.    int     err,i;
  391.  
  392.    struct getcid_req
  393.    {
  394.       struct qtrans_ctl qth;
  395.       uint8_t     tlv_type;
  396.       uint16_t    tlv_len;
  397.       uint8_t     tlv_value;
  398.    } __attribute__((__packed__)) *req;
  399.  
  400.    struct  qtrans_ctl* resp = (struct  qtrans_ctl*)rbuf;
  401.  
  402.    req = (struct getcid_req*)calloc(sizeof(*req),1);
  403.  
  404.    req->qth.qmh.tf      = 1;
  405.    req->qth.qmh.len     = sizeof(*req) - 1;
  406.    req->qth.qmh.cflags  = 0;
  407.    req->qth.qmh.stype   = QMI_CTL;
  408.    req->qth.qmh.cid     = 0x00;
  409.  
  410.    req->qth.cflags      = 0x00;
  411.    req->qth.tid         = ctl_tid++;
  412.    req->qth.msgid       = QMI_CTL_GET_CLIENT_ID;
  413.    req->qth.msgsize     = 4;
  414.  
  415.    req->tlv_type        = 0x01;
  416.    req->tlv_len         = 1;
  417.    req->tlv_value       = service_type;
  418.  
  419.    if(debug > 1)
  420.       printf("Running %s...\n",__func__);
  421.  
  422.    send_msg(req,sizeof(*req));
  423.  
  424.    for(i = 0;i < 8;i++)
  425.    {
  426.       if((rlen = read_msg(TRUE,10)) < 12)
  427.         goto errout;
  428.  
  429.       if(resp->msgid != QMI_CTL_GET_CLIENT_ID)
  430.          continue;
  431.  
  432.       if(!get_result_code(rbuf+sizeof(struct qtrans_ctl),resp->msgsize))
  433.       {
  434.           tlv_get2(rbuf+sizeof(struct qtrans_ctl),resp->msgsize,0x01,cid,2);
  435.       }else{
  436.          SET_RED;
  437.          fprintf(stderr,"%s: Request failed! Error Code: 0x%04X\n",__func__,rc.qmi_error);
  438.          goto errout;
  439.       }
  440.       break;
  441.    }
  442.    
  443.    if(debug > 2)
  444.       printf("CID 0x%04X\n", *cid);
  445.  
  446.    return 0;
  447.  
  448. errout:
  449.    fprintf(stderr,"ERROR: %s failed!\n",__func__);
  450.    SET_WHITE;
  451.    return -1;
  452. }
  453.  
  454.  
  455. static int release_cid(uint16_t cid)
  456. {
  457.    size_t rlen;
  458.    int err,i;
  459.  
  460.    struct releasecid_req
  461.    {
  462.       struct qtrans_ctl qth;
  463.       uint8_t     tlv_type;
  464.       uint16_t    tlv_len;
  465.       uint16_t    tlv_value;
  466.    } __attribute__((__packed__)) *req;
  467.  
  468.    struct qtrans_ctl*     resp = (struct qtrans_ctl*)rbuf;
  469.  
  470.    req = (struct releasecid_req*)calloc(sizeof(*req),1);
  471.  
  472.    req->qth.qmh.tf      = 1;
  473.    req->qth.qmh.len     = sizeof(*req) - 1;
  474.    req->qth.qmh.cflags  = 0;
  475.    req->qth.qmh.stype   = QMI_CTL;
  476.    req->qth.qmh.cid     = 0x00;
  477.  
  478.    req->qth.cflags      = 0x00;
  479.    req->qth.tid         = ctl_tid++;
  480.    req->qth.msgid       = QMI_CTL_RELEASE_CLIENT_ID;
  481.    req->qth.msgsize     = 5;
  482.  
  483.    req->tlv_type        = 0x01;
  484.    req->tlv_len         = 2;
  485.    req->tlv_value       = cid;
  486.  
  487.    if(debug > 1)
  488.       printf("Running %s...\n",__func__);
  489.  
  490.    send_msg(req,sizeof(*req));
  491.  
  492.    for(i = 0;i < 4;i++)
  493.    {
  494.       if((rlen = read_msg(TRUE,10)) < 12)
  495.         goto errout;
  496.  
  497.       if(resp->msgid != QMI_CTL_RELEASE_CLIENT_ID)
  498.          continue;
  499.  
  500.       if(get_result_code(rbuf+sizeof(struct qtrans_ctl),resp->msgsize))
  501.       {
  502.          SET_RED;
  503.          fprintf(stderr,"Request failed! Error Code: 0x%04X\n",rc.qmi_error);
  504.          goto errout;
  505.       }
  506.  
  507.       break;
  508.    }
  509.  
  510.    return 0;
  511.  
  512. errout:
  513.    fprintf(stderr,"ERROR: release_cid() failed!\n");
  514.    SET_WHITE;
  515.    return -1;
  516. }
  517.  
  518.  
  519. static int abort_nas_req(uint16_t tid)
  520. {
  521.    int i,rlen;
  522.  
  523.    struct abort_req
  524.    {
  525.       qmi_t       qth;
  526.       uint8_t     tlv_type;
  527.       uint16_t    tlv_len;
  528.       uint16_t    tlv_value;
  529.    }__attribute__((__packed__)) *req;
  530.  
  531.    qmi_t* resp = (qmi_t*)rbuf;
  532.  
  533.    req = (struct abort_req*)calloc(sizeof(*req),1);
  534.  
  535.    req->qth.qmh.tf      = 1;
  536.    req->qth.qmh.len     = sizeof(*req) - 1;
  537.    req->qth.qmh.cflags  = 0;
  538.    req->qth.qmh.stype   = QMI_NAS;
  539.    req->qth.qmh.cid     = nas_cid>>8;
  540.  
  541.    req->qth.cflags      = 0x00;
  542.    req->qth.tid         = nas_tid++;
  543.    req->qth.msgid       = QMI_NAS_ABORT;
  544.    req->qth.msgsize     = 0x0005;
  545.  
  546.    req->tlv_type        = 0x01;
  547.    req->tlv_len         = 2;
  548.    req->tlv_value       = tid;
  549.    
  550.    if(debug > 1)
  551.       printf("Running %s...\n",__func__);
  552.    
  553.    send_msg(req,sizeof(*req));
  554.  
  555.    for(i = 0;i < 4;i++)
  556.    {
  557.       if((rlen = read_msg(TRUE,5)) < 12)
  558.         goto errout;
  559.  
  560.       if(resp->msgid != QMI_NAS_ABORT)
  561.          continue;
  562.  
  563.       if(get_result_code(rbuf+sizeof(qmi_t),resp->msgsize))
  564.       {
  565.           fprintf(stderr,"Request failed! Error Code: 0x%04X\n",rc.qmi_error);
  566.           goto errout;
  567.       }
  568.  
  569.       break;
  570.    }
  571.  
  572.    return 0;
  573.  
  574. errout:
  575.    fprintf(stderr,"ERROR: %s failed!\n",__func__);
  576.    return -1;
  577.  
  578. }
  579.  
  580.  
  581.  
  582. static int set_sys_mode(uint8_t rat,uint32_t band,uint32_t lte_band)
  583. {
  584.    int i,rlen;
  585.    struct sys_select_pref_req
  586.    {
  587.       qmi_t     qth;
  588.       uint8_t   rat;
  589.       uint16_t  rat_len;
  590.       uint8_t   rat_mode[2];
  591.       uint8_t   band;
  592.       uint16_t  band_len;
  593.       uint8_t   rf_band[8];
  594.       uint8_t   lte_band;
  595.       uint16_t  lte_band_len;
  596.       uint8_t   lte_rf_band[8];
  597.       uint8_t   duration;
  598.       uint16_t  duration_len;
  599.       uint8_t   duration_mode;
  600.    }__attribute__((__packed__)) *req;
  601.  
  602.  
  603.    qmi_t* resp = (qmi_t*)rbuf;
  604.  
  605.    req = (struct sys_select_pref_req*)calloc(sizeof(*req),1);
  606.  
  607.    req->qth.qmh.tf      = 1;
  608.    req->qth.qmh.len     = sizeof(*req) - 1;
  609.    req->qth.qmh.cflags  = 0;
  610.    req->qth.qmh.stype   = QMI_NAS;
  611.    req->qth.qmh.cid     = nas_cid>>8;
  612.  
  613.    req->qth.cflags      = 0x00;
  614.    req->qth.tid         = nas_tid++;
  615.    req->qth.msgid       = QMI_NAS_SET_SYS_SELECTION;
  616.    req->qth.msgsize     = 31;
  617.  
  618.    req->rat             = 0x11;
  619.    req->rat_len         = 0x0002;
  620.    req->rat_mode[0]     = rat;
  621.    req->rat_mode[1]     = 0x00;
  622.    
  623.    req->band            = 0x12;
  624.    req->band_len        = 0x0008;
  625.    req->rf_band[0]      = band&0xFF;
  626.    req->rf_band[1]      = (band>>8)&0xFF;
  627.    req->rf_band[2]      = (band>>16)&0xFF;
  628.  
  629.    req->lte_band        = 0x15;
  630.    req->lte_band_len    = 0x0008;
  631.    req->lte_rf_band[0]  = lte_band&0xFF;
  632.    req->lte_rf_band[2]  = (lte_band>>16)&0xFF;
  633.    
  634.    req->duration        = 0x17;
  635.    req->duration_len    = 0x0001;
  636.    req->duration_mode   = 0x00;
  637.  
  638.    if(debug > 1)
  639.       printf("Running %s...\n",__func__);
  640.    
  641.    send_msg(req,sizeof(*req));
  642.  
  643.    for(i = 0;i < 4;i++)
  644.    {
  645.       if((rlen = read_msg(TRUE,5)) < 12)
  646.         goto errout;
  647.  
  648.       if(resp->msgid != QMI_NAS_SET_SYS_SELECTION)
  649.          continue;
  650.  
  651.       if(get_result_code(rbuf+sizeof(qmi_t),resp->msgsize))
  652.       {
  653.          SET_RED;
  654.          fprintf(stderr,"Request failed! Error Code: 0x%04X\n",rc.qmi_error);
  655.          goto errout;
  656.       }
  657.  
  658.       break;
  659.    }
  660.  
  661.    return 0;
  662.  
  663. errout:
  664.    fprintf(stderr,"ERROR: set_sys_mode() failed!\n");
  665.    SET_WHITE;
  666.    return -1;
  667.  
  668. }
  669.  
  670.  
  671. static int set_nas_indication(uint8_t ind,uint8_t on_off)
  672. {
  673.    int i,rlen;
  674.  
  675.    struct set_ind_req
  676.    {
  677.       qmi_t  qth;
  678.       uint8_t     tlv_type;
  679.       uint16_t    tlv_len;
  680.       uint8_t     tlv_value;
  681.    }__attribute__((__packed__)) *req;
  682.  
  683.    qmi_t* resp = (qmi_t*)rbuf;
  684.  
  685.    req = (struct set_ind_req*)calloc(sizeof(*req),1);
  686.  
  687.    req->qth.qmh.tf      = 1;
  688.    req->qth.qmh.len     = sizeof(*req) - 1;
  689.    req->qth.qmh.cflags  = 0;
  690.    req->qth.qmh.stype   = QMI_NAS;
  691.    req->qth.qmh.cid     = nas_cid>>8;
  692.  
  693.    req->qth.cflags      = 0x00;
  694.    req->qth.tid         = nas_tid++;
  695.    req->qth.msgid       = QMI_NAS_SET_REG_EVENT;
  696.    req->qth.msgsize     = 0x0004;
  697.  
  698.    req->tlv_type        = ind;
  699.    req->tlv_len         = 1;
  700.    req->tlv_value       = on_off;
  701.    
  702.    if(debug > 1)
  703.       printf("Running %s...\n",__func__);
  704.    
  705.    send_msg(req,sizeof(*req));
  706.  
  707.    for(i = 0;i < 4;i++)
  708.    {
  709.       if((rlen = read_msg(TRUE,5)) < 12)
  710.         goto errout;
  711.  
  712.       if(resp->msgid != QMI_NAS_SET_REG_EVENT)
  713.          continue;
  714.  
  715.       if(get_result_code(rbuf+sizeof(qmi_t),resp->msgsize))
  716.       {
  717.           fprintf(stderr,"Request failed! Error Code: 0x%04X\n",rc.qmi_error);
  718.           goto errout;
  719.       }
  720.  
  721.       break;
  722.    }
  723.  
  724.    return 0;
  725.  
  726. errout:
  727.    fprintf(stderr,"ERROR: set_nas_indication() failed!\n");
  728.    return -1;
  729. }
  730.  
  731.  
  732. static int scan_network(uint8_t rat,const char* act)
  733. {
  734.    int i,j,k,rlen;
  735.    char desc[128];
  736.    uint8_t tlv_buf[256];
  737.  
  738.    memset(tlv_buf,0x00,256);
  739.  
  740.    struct scan_req
  741.    {
  742.       qmi_t    qth;
  743.       uint8_t  type;
  744.       uint16_t type_len;
  745.       uint8_t  type_value;
  746.    }__attribute__((__packed__)) *req;
  747.  
  748.    qmi_t* resp = (qmi_t*)rbuf;
  749.  
  750.    req = (struct scan_req*)calloc(sizeof(*req),1);
  751.  
  752.    req->qth.qmh.tf      = 1;
  753.    req->qth.qmh.len     = sizeof(*req) - 1;
  754.    req->qth.qmh.cflags  = 0;
  755.    req->qth.qmh.stype   = QMI_NAS;
  756.    req->qth.qmh.cid     = nas_cid>>8;
  757.  
  758.    req->qth.cflags      = 0x00;
  759.    req->qth.tid         = nas_tid++;
  760.    req->qth.msgid       = QMI_NAS_SCAN_NETS;
  761.    req->qth.msgsize     = 0x0004;
  762.  
  763.    req->type            = 0x10;
  764.    req->type_len        = 0x0001;
  765.    req->type_value      = rat;
  766.  
  767.    if(debug > 1)
  768.       printf("Running %s...\n",__func__);
  769.    //print_buf(">>>",req,req_len);
  770.    send_msg(req,sizeof(*req));
  771.  
  772.    for(i = 0;i < 4;i++)
  773.    {
  774.       if((rlen = read_msg(TRUE,60)) < 12)
  775.       {
  776.         if(rlen == 0)
  777.         {
  778.            fprintf(stderr,"Scan timed out! Abort scan request!\n");
  779.            abort_nas_req(req->qth.tid);
  780.            goto out;
  781.         }
  782.         goto errout;
  783.       }
  784.  
  785.       if(resp->msgid != QMI_NAS_SCAN_NETS)
  786.          continue;
  787.  
  788.       if(!get_result_code(rbuf+sizeof(struct qtrans),resp->msgsize))
  789.       {
  790.           tlv_get2(rbuf+sizeof(struct qtrans),resp->msgsize,0x10,&tlv_buf,256);
  791.  
  792.           printf("%i network(s) found!\n",tlv_buf[0]);
  793.          
  794.           switch(rat)
  795.           {
  796.              case 1: SET_GREEN; break;
  797.              case 2: SET_BLUE; break;
  798.              case 4: SET_CYAN; break;
  799.              default: SET_YELLOW; break;
  800.           }
  801.  
  802.           for(j = 0,k = 2;j < tlv_buf[0];j++)
  803.           {
  804.              if(j == 0) printf("\n");
  805.  
  806.              memset(desc,0x00,128);
  807.              memcpy(desc,tlv_buf+k+6,tlv_buf[k+5]);
  808.  
  809.              printf("\t%i. -- [%s]  MCC:%d MNC:%02d (%s)\n",j+1,act,*(uint16_t*)(tlv_buf+k),tlv_buf[k+2],desc);
  810.  
  811.              k += 6 + tlv_buf[k+5];
  812.              if(j == tlv_buf[0]-1) printf("\n");
  813.           }
  814.  
  815.           SET_WHITE;
  816.       }else{
  817.           SET_RED;
  818.           fprintf(stderr,"Request failed! Error Code: 0x%04X\n",rc.qmi_error);
  819.           goto errout;
  820.       }
  821.  
  822.       break;
  823.    }
  824.  
  825. out:
  826.    return 0;
  827.  
  828. errout:
  829.    fprintf(stderr,"ERROR: scan_network() failed!\n");
  830.    SET_WHITE;
  831.    return -1;
  832. }
  833.  
  834.  
  835. static void scan_network_bands()
  836. {
  837.    fprintf(stderr,"\n\t-----  QMI Network Scanner v0.1  -----\n\n");
  838.    set_nas_indication(0x13,0); //disable Serving System Events
  839.    fprintf(stderr,"Scanning for GSM1800...\t\t ");
  840.    set_sys_mode(RAT_GSM,GSM1800,0);
  841.    scan_network(0x01,"GSM1800");
  842.    fprintf(stderr,"Scanning for GSM900 Extended...\t ");
  843.    set_sys_mode(RAT_GSM,GSM900E,0);
  844.    scan_network(0x01,"E-GSM900");
  845.    fprintf(stderr,"Scanning for GSM900 Primary...\t ");
  846.    set_sys_mode(RAT_GSM,GSM900P,0);
  847.    scan_network(0x01,"P-GSM900");
  848.    fprintf(stderr,"Scanning for UMTS2100...\t ");
  849.    set_sys_mode(RAT_UMTS,UMTS2100,0);
  850.    scan_network(0x02,"UMTS2100");
  851.    fprintf(stderr,"Scanning for LTE800...\t\t ");
  852.    set_sys_mode(RAT_LTE,0,LTE800);
  853.    scan_network(0x04,"LTE800");
  854.    fprintf(stderr,"Scanning for LTE1800...\t\t ");
  855.    set_sys_mode(RAT_LTE,0,LTE1800);
  856.    scan_network(0x04,"LTE1800");
  857.    fprintf(stderr,"Scanning for LTE2600...\t\t ");
  858.    set_sys_mode(RAT_LTE,0,LTE2600);
  859.    scan_network(0x04,"LTE 2600");
  860.    set_sys_mode(RAT_GSM,GSM850,0);
  861. }
  862.  
  863. static void usage()
  864. {
  865.    printf("\nusage: qmi_scanner [-d /dev/cdc-wdmX] [-D] [-h]\n\n");
  866.    printf("\t\t-d: qmi control device (defaults to /dev/cdc-wdm0)\n");
  867.    printf("\t\t-D: debug level\n");
  868.    printf("\t\t-h: show usage\n\n");
  869.    exit(0);
  870. }
  871.  
  872.  
  873. int main(int argc, char *argv[])
  874. {
  875.    int opt;
  876.  
  877.    SET_WHITE;
  878.  
  879.    strncpy(qmi_dev,"/dev/cdc-wdm0",30);
  880.  
  881.    for(;;)
  882.    {
  883.       opt = getopt(argc, argv, "d:Dh");
  884.       if(opt == -1) break;
  885.  
  886.       switch(opt)
  887.       {
  888.          case 'd': strncpy(qmi_dev,optarg,30); break;
  889.          case 'D': debug++; break;
  890.          case 'h': usage(); break;
  891.          default: break;
  892.       }
  893.    }
  894.  
  895.    errno = 0;
  896.    qmi_fd = open(qmi_dev, O_RDWR | O_EXCL | /*O_NONBLOCK |*/ O_NOCTTY);
  897.  
  898.     if(qmi_fd < 0)
  899.     {
  900.        SET_RED;
  901.        fprintf(stderr,"Failed to open control device %s : %s\n",qmi_dev,strerror(errno));
  902.        fprintf(stderr,"Make sure the qmi_wwan module is loaded and your modem has been modeswitched!\n");
  903.        SET_LWHITE;
  904.        exit(1);
  905.     }
  906.  
  907.    while(get_cid(&nas_cid,QMI_NAS) == -1)
  908.       sleep(1);
  909.    
  910.    scan_network_bands();
  911.  
  912.    if(nas_cid)
  913.        release_cid(nas_cid);
  914.  
  915.    close(qmi_fd);
  916.  
  917.    SET_LWHITE;
  918.    return 0;
  919. }
clone this paste RAW Paste Data