API tools faq
paste
Advertisement
Guest User

qmi_scanner.c

a guest
May 14th, 2012
1,313
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 22.64 KB | None | 0 0
raw download clone embed print report
  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. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!