diff --git a/quectel_cm_5G/src/device.c b/quectel_cm_5G/src/device.c index f67cfc0..6e95405 100644 --- a/quectel_cm_5G/src/device.c +++ b/quectel_cm_5G/src/device.c @@ -32,21 +32,21 @@ #include "QMIThread.h" #include "ethtool-copy.h" -#define USB_CLASS_VENDOR_SPEC 0xff -#define USB_CLASS_COMM 2 -#define USB_CDC_SUBCLASS_ACM 0x02 +#define USB_CLASS_VENDOR_SPEC 0xff +#define USB_CLASS_COMM 2 +#define USB_CDC_SUBCLASS_ACM 0x02 #define USB_CDC_SUBCLASS_ETHERNET 0x06 -#define USB_CDC_SUBCLASS_NCM 0x0d -#define USB_CDC_SUBCLASS_MBIM 0x0e -#define USB_CLASS_WIRELESS_CONTROLLER 0xe0 +#define USB_CDC_SUBCLASS_NCM 0x0d +#define USB_CDC_SUBCLASS_MBIM 0x0e +#define USB_CLASS_WIRELESS_CONTROLLER 0xe0 #define CM_MAX_PATHLEN 256 #define CM_INVALID_VAL (~((int)0)) + /* get first line from file 'fname' * And convert the content into a hex number, then return this number */ -static int file_get_value(const char *fname, int base) -{ +static int file_get_value(const char *fname, int base) { FILE *fp = NULL; long num; char buff[32 + 1] = {'\0'}; @@ -58,7 +58,7 @@ static int file_get_value(const char *fname, int base) goto error; fclose(fp); - num = (int)strtol(buff, &endptr, base); + num = (int) strtol(buff, &endptr, base); if (errno == ERANGE && (num == LONG_MAX || num == LONG_MIN)) goto error; /* if there is no digit in buff */ @@ -67,9 +67,9 @@ static int file_get_value(const char *fname, int base) if (debug_qmi) dbg_time("(%s) = %lx", fname, num); - return (int)num; + return (int) num; -error: + error: if (fp) fclose(fp); return CM_INVALID_VAL; } @@ -78,8 +78,7 @@ error: * This function will search the directory 'dirname' and return the first child. * '.' and '..' is ignored by default */ -static int dir_get_child(const char *dirname, char *buff, unsigned bufsize, const char *prefix) -{ +static int dir_get_child(const char *dirname, char *buff, unsigned bufsize, const char *prefix) { struct dirent *entptr = NULL; DIR *dirptr; @@ -102,14 +101,13 @@ static int dir_get_child(const char *dirname, char *buff, unsigned bufsize, cons return 0; } -static int conf_get_val(const char *fname, const char *key) -{ +static int conf_get_val(const char *fname, const char *key) { char buff[128] = {'\0'}; FILE *fp = fopen(fname, "r"); if (!fp) return CM_INVALID_VAL; - while (fgets(buff, sizeof(buff)-1, fp)) { + while (fgets(buff, sizeof(buff) - 1, fp)) { char prefix[128] = {'\0'}; char tail[128] = {'\0'}; /* To eliminate cppcheck warnning: Assume string length is no more than 15 */ @@ -190,17 +188,16 @@ static void query_usb_interface_info(char *path, struct usb_interface_info *p) { break; n--; } - strncpy(p->driver, &driver[n+1], sizeof(p->driver) - 1); + strncpy(p->driver, &driver[n + 1], sizeof(p->driver) - 1); } path[offset] = '\0'; } -static int detect_path_cdc_wdm_or_qcqmi(char *path, char *devname, size_t bufsize) -{ +static int detect_path_cdc_wdm_or_qcqmi(char *path, char *devname, size_t bufsize) { size_t offset = strlen(path); char tmp[32]; - + devname[0] = 0; if (access(path, R_OK)) @@ -223,7 +220,7 @@ static int detect_path_cdc_wdm_or_qcqmi(char *path, char *devname, size_t bufsiz return -1; -step_1: + step_1: /* get device(qcqmiX|cdc-wdmX) */ if (debug_qmi) dbg_time("%s", path); dir_get_child(path, tmp, sizeof(tmp), NULL); @@ -232,8 +229,7 @@ step_1: /* There is a chance that, no device(qcqmiX|cdc-wdmX) is generated. We should warn user about that! */ snprintf(devname, bufsize, "/dev/%s", tmp); - if (access(devname, R_OK | F_OK) && errno == ENOENT) - { + if (access(devname, R_OK | F_OK) && errno == ENOENT) { int major, minor; dbg_time("access %s failed, errno: %d (%s)", devname, errno, strerror(errno)); @@ -243,9 +239,9 @@ step_1: major = conf_get_val(path, "MAJOR"); minor = conf_get_val(path, "MINOR"); - if(major == CM_INVALID_VAL || minor == CM_INVALID_VAL) + if (major == CM_INVALID_VAL || minor == CM_INVALID_VAL) dbg_time("get major and minor failed"); - else if (mknod(devname, S_IFCHR|0666, (((major & 0xfff) << 8) | (minor & 0xff) | ((minor & 0xfff00) << 12)))) + else if (mknod(devname, S_IFCHR | 0666, (((major & 0xfff) << 8) | (minor & 0xff) | ((minor & 0xfff00) << 12)))) dbg_time("please mknod %s c %d %d", devname, major, minor); } @@ -258,11 +254,11 @@ step_1: * TRUE -> ok */ BOOL qmidevice_detect(char *qmichannel, char *usbnet_adapter, unsigned bufsize, PROFILE_T *profile) { - struct dirent* ent = NULL; + struct dirent *ent = NULL; DIR *pDir; const char *rootdir = "/sys/bus/usb/devices"; struct { - char path[255*2]; + char path[255 * 2]; } *pl; pl = (typeof(pl)) malloc(sizeof(*pl)); memset(pl, 0x00, sizeof(*pl)); @@ -273,9 +269,9 @@ BOOL qmidevice_detect(char *qmichannel, char *usbnet_adapter, unsigned bufsize, goto error; } - while ((ent = readdir(pDir)) != NULL) { - char netcard[32+1] = {'\0'}; - char devname[32+5] = {'\0'}; //+strlen("/dev/") + while ((ent = readdir(pDir)) != NULL) { + char netcard[32 + 1] = {'\0'}; + char devname[32 + 5] = {'\0'}; //+strlen("/dev/") int netIntf; int driver_type; @@ -289,15 +285,15 @@ BOOL qmidevice_detect(char *qmichannel, char *usbnet_adapter, unsigned bufsize, if (profile->usb_dev.idVendor == 0x2c7c || profile->usb_dev.idVendor == 0x05c6) { dbg_time("Find %s/%s idVendor=0x%x idProduct=0x%x, bus=0x%03x, dev=0x%03x", - rootdir, ent->d_name, profile->usb_dev.idVendor, profile->usb_dev.idProduct, - profile->usb_dev.busnum, profile->usb_dev.devnum); + rootdir, ent->d_name, profile->usb_dev.idVendor, profile->usb_dev.idProduct, + profile->usb_dev.busnum, profile->usb_dev.devnum); } /* get network interface */ /* NOTICE: there is a case that, bNumberInterface=6, but the net interface is 8 */ /* toolchain-mips_24kc_gcc-5.4.0_musl donot support GLOB_BRACE */ /* RG500U's MBIM is at inteface 0 */ - for (netIntf = 0; netIntf < (profile->usb_dev.bNumInterfaces + 8); netIntf++) { + for (netIntf = 0; netIntf < (profile->usb_dev.bNumInterfaces + 8); netIntf++) { snprintf(pl->path, sizeof(pl->path), "%s/%s:1.%d/net", rootdir, ent->d_name, netIntf); dir_get_child(pl->path, netcard, sizeof(netcard), NULL); if (netcard[0]) @@ -305,7 +301,7 @@ BOOL qmidevice_detect(char *qmichannel, char *usbnet_adapter, unsigned bufsize, } if (netcard[0] == '\0') { //for centos 2.6.x - const char *n= "usb0"; + const char *n = "usb0"; const char *c = "qcqmi0"; snprintf(pl->path, sizeof(pl->path), "%s/%s:1.4/net:%s", rootdir, ent->d_name, n); @@ -334,29 +330,27 @@ BOOL qmidevice_detect(char *qmichannel, char *usbnet_adapter, unsigned bufsize, if (driver_type == SOFTWARE_QMI || driver_type == SOFTWARE_MBIM) { detect_path_cdc_wdm_or_qcqmi(pl->path, devname, sizeof(devname)); - } - else if (driver_type == SOFTWARE_ECM_RNDIS_NCM) - { + } else if (driver_type == SOFTWARE_ECM_RNDIS_NCM) { int atIntf = -1; if (profile->usb_dev.idVendor == 0x2c7c) { //Quectel switch (profile->usb_dev.idProduct) { //EC200U - case 0x0901: //EC200U - case 0x8101: //RG801H - atIntf = 2; - break; - case 0x0900: //RG500U - atIntf = 4; - break; - case 0x6026: //EC200T - case 0x6005: //EC200A - case 0x6002: //EC200S - case 0x6001: //EC100Y - atIntf = 3; - break; - default: - dbg_time("unknow at interface for USB idProduct:%04x\n", profile->usb_dev.idProduct); - break; + case 0x0901: //EC200U + case 0x8101: //RG801H + atIntf = 2; + break; + case 0x0900: //RG500U + atIntf = 4; + break; + case 0x6026: //EC200T + case 0x6005: //EC200A + case 0x6002: //EC200S + case 0x6001: //EC100Y + atIntf = 3; + break; + default: + dbg_time("unknow at interface for USB idProduct:%04x\n", profile->usb_dev.idProduct); + break; } } @@ -369,7 +363,7 @@ BOOL qmidevice_detect(char *qmichannel, char *usbnet_adapter, unsigned bufsize, } } } - + if (netcard[0] && devname[0]) { if (devname[0] == '/') snprintf(qmichannel, bufsize, "%s", devname); @@ -389,51 +383,76 @@ BOOL qmidevice_detect(char *qmichannel, char *usbnet_adapter, unsigned bufsize, } free(pl); return TRUE; -error: + error: free(pl); return FALSE; } + +int isContain(const char *str1, const char *str2) { + if (strstr(str1, str2) != NULL) { + return 1; + } else { + return 0; + } +} + +void replaceStr(char *str, char *orig, char *rep) { + char buffer[1024]; + char *p; + while (p = strstr(str, orig)) { + memcpy(buffer, str, p - str); + buffer[p - str] = '\0'; + sprintf(buffer + strlen(buffer), "%s%s", rep, p + strlen(orig)); + strcpy(str, buffer); + } +} + int mhidevice_detect(char *qmichannel, char *usbnet_adapter, PROFILE_T *profile) { - struct dirent* ent = NULL; + struct dirent *ent = NULL; DIR *pDir; const char *rootdir_mhi[] = {"/sys/bus/mhi_q/devices", "/sys/bus/mhi/devices", NULL}; int i = 0; char path[256]; int find = 0; - while (rootdir_mhi[i]) { const char *rootdir = rootdir_mhi[i++]; - + dbg_time("rootdir : %s ", rootdir); pDir = opendir(rootdir); if (!pDir) { - if (errno != ENOENT) - dbg_time("opendir %s failed: %s", rootdir, strerror(errno)); + if (errno != ENOENT) { + dbg_time("opendir %s failed: %s", rootdir, strerror(errno)); + } continue; } - - while ((ent = readdir(pDir)) != NULL) { + while ((ent = readdir(pDir)) != NULL) { char netcard[32] = {'\0'}; char devname[32] = {'\0'}; int software_interface = SOFTWARE_QMI; char *pNode = NULL; - pNode = strstr(ent->d_name, "_IP_HW0"); //0306_00.01.00_IP_HW0 - if (!pNode) + if (!pNode) { continue; + } + + char *d_name[32]; + strcpy(d_name, ent->d_name); + replaceStr(d_name, "_IP_HW0", ""); + dbg_time("while -> pcie_name = : %s ", d_name); snprintf(path, sizeof(path), "%s/%.32s/net", rootdir, ent->d_name); dir_get_child(path, netcard, sizeof(netcard), NULL); - if (!netcard[0]) + if (!netcard[0]) { continue; - - if (usbnet_adapter[0] && strcmp(netcard, usbnet_adapter)) //not '-i x' + } + if (usbnet_adapter[0] && strcmp(netcard, usbnet_adapter)) { //not '-i x continue; - + } if (!strcmp(rootdir, "/sys/bus/mhi/devices")) { - snprintf(path, sizeof(path), "%s/%.13s_IPCR", rootdir, ent->d_name); // 13 is sizeof(0306_00.01.00) + // snprintf(path, sizeof(path), "%s/%.13s_IPCR", rootdir, ent->d_name); // 13 is sizeof(0306_00.01.00) + snprintf(path, sizeof(path), "%s/%s_IPCR", rootdir, d_name); if (!access(path, F_OK)) { - /* we also need 'cat /dev/mhi_0306_00.01.00_pipe_14' to enable rmnet as like USB's DTR + /* we also need 'cat /dev/mhi_0306_00.01.00_pipe_14' to enable rmnet as like USB's DTR or will get error 'requestSetEthMode QMUXResult = 0x1, QMUXError = 0x46' */ sprintf(usbnet_adapter, "%s", netcard); sprintf(qmichannel, "qrtr-%d", 3); // 3 is sdx modem's node id @@ -444,42 +463,59 @@ int mhidevice_detect(char *qmichannel, char *usbnet_adapter, PROFILE_T *profile) continue; } - snprintf(path, sizeof(path), "%s/%.13s_IPCR", rootdir, ent->d_name); + //解决PCIE硬编码导致不兼容hv问题 + snprintf(path, sizeof(path), "%s/%s_IPCR", rootdir, d_name); if (access(path, F_OK)) { - snprintf(path, sizeof(path), "%s/%.13s_QMI0", rootdir, ent->d_name); + snprintf(path, sizeof(path), "%s/%s_QMI0", rootdir, d_name); if (access(path, F_OK)) { - snprintf(path, sizeof(path), "%s/%.13s_MBIM", rootdir, ent->d_name); - if (!access(path, F_OK)) + snprintf(path, sizeof(path), "%s/%s_MBIM", rootdir, d_name); + if (!access(path, F_OK)) { software_interface = SOFTWARE_MBIM; + } } } - if (access(path, F_OK)) + + //原 + // snprintf(path, sizeof(path), "%s/%.14s_IPCR", rootdir, ent->d_name); + // dbg_time("while -> path3 : %s ", path); + // if (access(path, F_OK)) { + // snprintf(path, sizeof(path), "%s/%.14s_QMI0", rootdir, ent->d_name); + // dbg_time("while -> path4 : %s ", path); + // if (access(path, F_OK)) { + // snprintf(path, sizeof(path), "%s/%.14s_MBIM", rootdir, ent->d_name); + // dbg_time("while -> path5 : %s ", path); + // if (!access(path, F_OK)) { + // software_interface = SOFTWARE_MBIM; + // } + // } + // } + + dbg_time("while -> path_final : %s ", path); + if (access(path, F_OK)) { continue; - - strncat(path, "/mhi_uci_q", sizeof(path)-1); + } + strncat(path, "/mhi_uci_q", sizeof(path) - 1); dir_get_child(path, devname, sizeof(devname), NULL); - if (!devname[0]) - continue; - + if (!devname[0]) { + continue; + } + dbg_time("while -> path_final_sub : %s ,dev_name: %s ", path, devname); sprintf(usbnet_adapter, "%s", netcard); sprintf(qmichannel, "/dev/%s", devname); profile->software_interface = software_interface; find = 1; break; } - closedir(pDir); } - return find; } int atdevice_detect(char *atchannel, char *usbnet_adapter, PROFILE_T *profile) { if (!access("/sys/class/net/sipa_dummy0", F_OK)) { - strcpy(usbnet_adapter, "sipa_dummy0"); - snprintf(profile->qmapnet_adapter, sizeof(profile->qmapnet_adapter), "%s%d", "pcie", profile->pdp - 1); - } - else { + strcpy(usbnet_adapter, "sipa_dummy0"); + snprintf(profile->qmapnet_adapter, sizeof(profile->qmapnet_adapter), "%s%d", "pcie", profile->pdp - 1); + } else { dbg_time("atdevice_detect failed"); goto error; } @@ -487,65 +523,58 @@ int atdevice_detect(char *atchannel, char *usbnet_adapter, PROFILE_T *profile) { if (!access("/dev/stty_nr31", F_OK)) { strcpy(atchannel, "/dev/stty_nr31"); profile->software_interface = SOFTWARE_ECM_RNDIS_NCM; - } - else { + } else { goto error; } return 1; -error: + error: return 0; } -int get_driver_type(PROFILE_T *profile) -{ +int get_driver_type(PROFILE_T *profile) { /* QMI_WWAN */ if (profile->usb_intf.bInterfaceClass == USB_CLASS_VENDOR_SPEC) { return SOFTWARE_QMI; - } - else if (profile->usb_intf.bInterfaceClass == USB_CLASS_COMM) { + } else if (profile->usb_intf.bInterfaceClass == USB_CLASS_COMM) { switch (profile->usb_intf.bInterfaceSubClass) { case USB_CDC_SUBCLASS_MBIM: return SOFTWARE_MBIM; - break; + break; case USB_CDC_SUBCLASS_ETHERNET: case USB_CDC_SUBCLASS_NCM: return SOFTWARE_ECM_RNDIS_NCM; - break; + break; default: - break; + break; } - } - else if (profile->usb_intf.bInterfaceClass == USB_CLASS_WIRELESS_CONTROLLER) { + } else if (profile->usb_intf.bInterfaceClass == USB_CLASS_WIRELESS_CONTROLLER) { if (profile->usb_intf.bInterfaceSubClass == 1 && profile->usb_intf.bInterfaceProtocol == 3) return SOFTWARE_ECM_RNDIS_NCM; } dbg_time("%s unknow bInterfaceClass=%d, bInterfaceSubClass=%d", __func__, - profile->usb_intf.bInterfaceClass, profile->usb_intf.bInterfaceSubClass); + profile->usb_intf.bInterfaceClass, profile->usb_intf.bInterfaceSubClass); return DRV_INVALID; } -struct usbfs_getdriver -{ +struct usbfs_getdriver { unsigned int interface; char driver[255 + 1]; }; -struct usbfs_ioctl -{ +struct usbfs_ioctl { int ifno; /* interface 0..N ; negative numbers reserved */ int ioctl_code; /* MUST encode size + direction of data so the * macros in give correct values */ void *data; /* param buffer (in, or out) */ }; -#define IOCTL_USBFS_DISCONNECT _IO('U', 22) -#define IOCTL_USBFS_CONNECT _IO('U', 23) +#define IOCTL_USBFS_DISCONNECT _IO('U', 22) +#define IOCTL_USBFS_CONNECT _IO('U', 23) -int usbfs_is_kernel_driver_alive(int fd, int ifnum) -{ +int usbfs_is_kernel_driver_alive(int fd, int ifnum) { struct usbfs_getdriver getdrv; getdrv.interface = ifnum; if (ioctl(fd, USBDEVFS_GETDRIVER, &getdrv) < 0) { @@ -556,8 +585,7 @@ int usbfs_is_kernel_driver_alive(int fd, int ifnum) return 1; } -void usbfs_detach_kernel_driver(int fd, int ifnum) -{ +void usbfs_detach_kernel_driver(int fd, int ifnum) { struct usbfs_ioctl operate; operate.data = NULL; operate.ifno = ifnum; @@ -569,8 +597,7 @@ void usbfs_detach_kernel_driver(int fd, int ifnum) } } -void usbfs_attach_kernel_driver(int fd, int ifnum) -{ +void usbfs_attach_kernel_driver(int fd, int ifnum) { struct usbfs_ioctl operate; operate.data = NULL; operate.ifno = ifnum; @@ -582,15 +609,13 @@ void usbfs_attach_kernel_driver(int fd, int ifnum) } } -int reattach_driver(PROFILE_T *profile) -{ +int reattach_driver(PROFILE_T *profile) { int ifnum = 4; int fd; char devpath[128] = {'\0'}; snprintf(devpath, sizeof(devpath), "/dev/bus/usb/%03d/%03d", profile->usb_dev.busnum, profile->usb_dev.devnum); fd = open(devpath, O_RDWR | O_NOCTTY); - if (fd < 0) - { + if (fd < 0) { dbg_time("%s fail to open %s", __func__, devpath); return -1; } @@ -601,11 +626,11 @@ int reattach_driver(PROFILE_T *profile) } #define SIOCETHTOOL 0x8946 -int ql_get_netcard_driver_info(const char *devname) -{ + +int ql_get_netcard_driver_info(const char *devname) { int fd = -1; struct ethtool_drvinfo drvinfo; - struct ifreq ifr; /* ifreq suitable for ethtool ioctl */ + struct ifreq ifr; /* ifreq suitable for ethtool ioctl */ memset(&ifr, 0, sizeof(ifr)); strcpy(ifr.ifr_name, devname); @@ -617,7 +642,7 @@ int ql_get_netcard_driver_info(const char *devname) } drvinfo.cmd = ETHTOOL_GDRVINFO; - ifr.ifr_data = (void *)&drvinfo; + ifr.ifr_data = (void *) &drvinfo; if (ioctl(fd, SIOCETHTOOL, &ifr) < 0) { dbg_time("ioctl() error: errno(%d)(%s)", errno, strerror(errno)); @@ -632,11 +657,10 @@ int ql_get_netcard_driver_info(const char *devname) return 0; } -int ql_get_netcard_carrier_state(const char *devname) -{ +int ql_get_netcard_carrier_state(const char *devname) { int fd = -1; struct ethtool_value edata; - struct ifreq ifr; /* ifreq suitable for ethtool ioctl */ + struct ifreq ifr; /* ifreq suitable for ethtool ioctl */ memset(&ifr, 0, sizeof(ifr)); strcpy(ifr.ifr_name, devname); @@ -649,7 +673,7 @@ int ql_get_netcard_carrier_state(const char *devname) edata.cmd = ETHTOOL_GLINK; edata.data = 0; - ifr.ifr_data = (void *)&edata; + ifr.ifr_data = (void *) &edata; if (ioctl(fd, SIOCETHTOOL, &ifr) < 0) { dbg_time("ioctl('%s') error: errno(%d)(%s)", devname, errno, strerror(errno)); @@ -664,17 +688,16 @@ int ql_get_netcard_carrier_state(const char *devname) return edata.data; } -static void *catch_log(void *arg) -{ - PROFILE_T *profile = (PROFILE_T *)arg; +static void *catch_log(void *arg) { + PROFILE_T *profile = (PROFILE_T *) arg; int nreads = 0; - char tbuff[256+32]; + char tbuff[256 + 32]; char filter[32]; size_t tsize = strlen(get_time()) + 1; snprintf(filter, sizeof(filter), ":%d:%03d:", profile->usb_dev.busnum, profile->usb_dev.devnum); - while(1) { + while (1) { nreads = read(profile->usbmon_fd, tbuff + tsize, sizeof(tbuff) - tsize - 1); if (nreads <= 0) { if (nreads == -1 && errno == EINTR) @@ -682,13 +705,13 @@ static void *catch_log(void *arg) break; } - tbuff[tsize+nreads] = '\0'; // printf("%s", buff); + tbuff[tsize + nreads] = '\0'; // printf("%s", buff); - if (!strstr(tbuff+tsize, filter)) + if (!strstr(tbuff + tsize, filter)) continue; snprintf(tbuff, sizeof(tbuff), "%s", get_time()); - tbuff[tsize-1] = ' '; + tbuff[tsize - 1] = ' '; fwrite(tbuff, strlen(tbuff), 1, profile->usbmon_logfile_fp); } @@ -696,8 +719,7 @@ static void *catch_log(void *arg) return NULL; } -int ql_capture_usbmon_log(PROFILE_T *profile, const char *log_path) -{ +int ql_capture_usbmon_log(PROFILE_T *profile, const char *log_path) { char usbmon_path[256]; pthread_t pt; pthread_attr_t attr; @@ -724,16 +746,16 @@ int ql_capture_usbmon_log(PROFILE_T *profile, const char *log_path) profile->usbmon_logfile_fp = fopen(log_path, "wb"); if (!profile->usbmon_logfile_fp) { - dbg_time("open %s error(%d) (%s)", log_path, errno, strerror(errno)); - close(profile->usbmon_fd); - profile->usbmon_fd = -1; - return -1; + dbg_time("open %s error(%d) (%s)", log_path, errno, strerror(errno)); + close(profile->usbmon_fd); + profile->usbmon_fd = -1; + return -1; } pthread_attr_init(&attr); pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED); - pthread_create(&pt, &attr, catch_log, (void *)profile); + pthread_create(&pt, &attr, catch_log, (void *) profile); return 0; }