1 /******************************************************************************
2 @file util.c
3 @brief some utils for this QCM tool.
4
5 DESCRIPTION
6 Connectivity Management Tool for USB network adapter of Quectel wireless cellular modules.
7
8 INITIALIZATION AND SEQUENCING REQUIREMENTS
9 None.
10
11 ---------------------------------------------------------------------------
12 Copyright (c) 2016 - 2020 Quectel Wireless Solution, Co., Ltd. All Rights Reserved.
13 Quectel Wireless Solution Proprietary and Confidential.
14 ---------------------------------------------------------------------------
15 ******************************************************************************/
16
17 #include <sys/time.h>
18 #include <net/if.h>
19 typedef unsigned short sa_family_t;
20 #include <linux/un.h>
21
22 #if defined(__STDC__)
23 #include <stdarg.h>
24 #define __V(x) x
25 #else
26 #include <varargs.h>
27 #define __V(x) (va_alist) va_dcl
28 #define const
29 #define volatile
30 #endif
31
32 #include <syslog.h>
33
34 #include "QMIThread.h"
35
36 pthread_mutex_t cm_command_mutex = PTHREAD_MUTEX_INITIALIZER;
37 pthread_cond_t cm_command_cond = PTHREAD_COND_INITIALIZER;
38 unsigned int cm_recv_buf[1024];
39
cm_open_dev(const char * dev)40 int cm_open_dev(const char *dev) {
41 int fd;
42
43 fd = open(dev, O_RDWR | O_NONBLOCK | O_NOCTTY);
44 if (fd != -1) {
45 fcntl(fd, F_SETFL, fcntl(fd,F_GETFL) | O_NONBLOCK);
46 fcntl(fd, F_SETFD, FD_CLOEXEC);
47
48 if (!strncmp(dev, "/dev/tty", strlen("/dev/tty")))
49 {
50 //disable echo on serial ports
51 struct termios ios;
52
53 memset(&ios, 0, sizeof(ios));
54 tcgetattr( fd, &ios );
55 cfmakeraw(&ios);
56 cfsetispeed(&ios, B115200);
57 cfsetospeed(&ios, B115200);
58 tcsetattr( fd, TCSANOW, &ios );
59 tcflush(fd, TCIOFLUSH);
60 }
61 } else {
62 dbg_time("Failed to open %s, errno: %d (%s)", dev, errno, strerror(errno));
63 }
64
65 return fd;
66 }
67
cm_open_proxy(const char * name)68 int cm_open_proxy(const char *name) {
69 int sockfd = -1;
70 int reuse_addr = 1;
71 struct sockaddr_un sockaddr;
72 socklen_t alen;
73
74 /*Create server socket*/
75 sockfd = socket(AF_LOCAL, SOCK_STREAM, 0);
76 if (sockfd < 0)
77 return sockfd;
78
79 memset(&sockaddr, 0, sizeof(sockaddr));
80 sockaddr.sun_family = AF_LOCAL;
81 sockaddr.sun_path[0] = 0;
82 memcpy(sockaddr.sun_path + 1, name, strlen(name) );
83
84 alen = strlen(name) + offsetof(struct sockaddr_un, sun_path) + 1;
85 if(connect(sockfd, (struct sockaddr *)&sockaddr, alen) < 0) {
86 close(sockfd);
87 dbg_time("connect %s errno: %d (%s)", name, errno, strerror(errno));
88 return -1;
89 }
90 setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR, &reuse_addr,sizeof(reuse_addr));
91 fcntl(sockfd, F_SETFL, fcntl(sockfd,F_GETFL) | O_NONBLOCK);
92 fcntl(sockfd, F_SETFD, FD_CLOEXEC);
93
94 dbg_time("connect to %s sockfd = %d", name, sockfd);
95
96 return sockfd;
97 }
98
setTimespecRelative(struct timespec * p_ts,long long msec)99 static void setTimespecRelative(struct timespec *p_ts, long long msec)
100 {
101 struct timeval tv;
102
103 gettimeofday(&tv, (struct timezone *) NULL);
104
105 /* what's really funny about this is that I know
106 pthread_cond_timedwait just turns around and makes this
107 a relative time again */
108 p_ts->tv_sec = tv.tv_sec + (msec / 1000);
109 p_ts->tv_nsec = (tv.tv_usec + (msec % 1000) * 1000L ) * 1000L;
110 if ((unsigned long)p_ts->tv_nsec >= 1000000000UL) {
111 p_ts->tv_sec += 1;
112 p_ts->tv_nsec -= 1000000000UL;
113 }
114 }
115
pthread_cond_timeout_np(pthread_cond_t * cond,pthread_mutex_t * mutex,unsigned msecs)116 int pthread_cond_timeout_np(pthread_cond_t *cond, pthread_mutex_t * mutex, unsigned msecs) {
117 if (msecs != 0) {
118 unsigned i;
119 unsigned t = msecs/4;
120 int ret = 0;
121
122 if (t == 0)
123 t = 1;
124
125 for (i = 0; i < msecs; i += t) {
126 struct timespec ts;
127 setTimespecRelative(&ts, t);
128 //very old uclibc do not support pthread_condattr_setclock(CLOCK_MONOTONIC)
129 ret = pthread_cond_timedwait(cond, mutex, &ts); //to advoid system time change
130 if (ret != ETIMEDOUT) {
131 if(ret) dbg_time("ret=%d, msecs=%u, t=%u", ret, msecs, t);
132 break;
133 }
134 }
135
136 return ret;
137 } else {
138 return pthread_cond_wait(cond, mutex);
139 }
140 }
141
get_time(void)142 const char * get_time(void) {
143 static char time_buf[128];
144 struct timeval tv;
145 time_t time;
146 suseconds_t millitm;
147 struct tm *ti;
148
149 gettimeofday (&tv, NULL);
150
151 time= tv.tv_sec;
152 millitm = (tv.tv_usec + 500) / 1000;
153
154 if (millitm == 1000) {
155 ++time;
156 millitm = 0;
157 }
158
159 ti = localtime(&time);
160 sprintf(time_buf, "%02d-%02d_%02d:%02d:%02d:%03d", ti->tm_mon+1, ti->tm_mday, ti->tm_hour, ti->tm_min, ti->tm_sec, (int)millitm);
161 return time_buf;
162 }
163
clock_msec(void)164 unsigned long clock_msec(void)
165 {
166 struct timespec tm;
167 clock_gettime( CLOCK_MONOTONIC, &tm);
168 return (unsigned long)(tm.tv_sec*1000 + (tm.tv_nsec/1000000));
169 }
170
171 FILE *logfilefp = NULL;
172
173 const int i = 1;
174 #define is_bigendian() ( (*(char*)&i) == 0 )
175
le16_to_cpu(USHORT v16)176 USHORT le16_to_cpu(USHORT v16) {
177 USHORT tmp = v16;
178 if (is_bigendian()) {
179 unsigned char *s = (unsigned char *)(&v16);
180 unsigned char *d = (unsigned char *)(&tmp);
181 d[0] = s[1];
182 d[1] = s[0];
183 }
184 return tmp;
185 }
186
le32_to_cpu(UINT v32)187 UINT le32_to_cpu (UINT v32) {
188 UINT tmp = v32;
189 if (is_bigendian()) {
190 unsigned char *s = (unsigned char *)(&v32);
191 unsigned char *d = (unsigned char *)(&tmp);
192 d[0] = s[3];
193 d[1] = s[2];
194 d[2] = s[1];
195 d[3] = s[0];
196 }
197 return tmp;
198 }
199
ql_swap32(UINT v32)200 UINT ql_swap32(UINT v32) {
201 UINT tmp = v32;
202 {
203 unsigned char *s = (unsigned char *)(&v32);
204 unsigned char *d = (unsigned char *)(&tmp);
205 d[0] = s[3];
206 d[1] = s[2];
207 d[2] = s[1];
208 d[3] = s[0];
209 }
210 return tmp;
211 }
212
cpu_to_le16(USHORT v16)213 USHORT cpu_to_le16(USHORT v16) {
214 USHORT tmp = v16;
215 if (is_bigendian()) {
216 unsigned char *s = (unsigned char *)(&v16);
217 unsigned char *d = (unsigned char *)(&tmp);
218 d[0] = s[1];
219 d[1] = s[0];
220 }
221 return tmp;
222 }
223
cpu_to_le32(UINT v32)224 UINT cpu_to_le32 (UINT v32) {
225 UINT tmp = v32;
226 if (is_bigendian()) {
227 unsigned char *s = (unsigned char *)(&v32);
228 unsigned char *d = (unsigned char *)(&tmp);
229 d[0] = s[3];
230 d[1] = s[2];
231 d[2] = s[1];
232 d[3] = s[0];
233 }
234 return tmp;
235 }
236
update_resolv_conf(int iptype,const char * ifname,const char * dns1,const char * dns2)237 void update_resolv_conf(int iptype, const char *ifname, const char *dns1, const char *dns2) {
238 const char *dns_file = "/etc/resolv.conf";
239 FILE *dns_fp;
240 char dns_line[256];
241 #define MAX_DNS 16
242 char *dns_info[MAX_DNS];
243 char dns_tag[64];
244 int dns_match = 0;
245 int i;
246
247 snprintf(dns_tag, sizeof(dns_tag), "# IPV%d %s", iptype, ifname);
248
249 for (i = 0; i < MAX_DNS; i++)
250 dns_info[i] = NULL;
251
252 dns_fp = fopen(dns_file, "r");
253 if (dns_fp) {
254 i = 0;
255 dns_line[sizeof(dns_line)-1] = '\0';
256
257 while((fgets(dns_line, sizeof(dns_line)-1, dns_fp)) != NULL) {
258 if ((strlen(dns_line) > 1) && (dns_line[strlen(dns_line) - 1] == '\n'))
259 dns_line[strlen(dns_line) - 1] = '\0';
260 //dbg_time("%s", dns_line);
261 if (strstr(dns_line, dns_tag)) {
262 dns_match++;
263 continue;
264 }
265 dns_info[i++] = strdup(dns_line);
266 if (i == MAX_DNS)
267 break;
268 }
269
270 fclose(dns_fp);
271 }
272 else if (errno != ENOENT) {
273 dbg_time("fopen %s fail, errno:%d (%s)", dns_file, errno, strerror(errno));
274 return;
275 }
276
277 if (dns1 == NULL && dns_match == 0)
278 return;
279
280 dns_fp = fopen(dns_file, "w");
281 if (dns_fp) {
282 if (dns1)
283 fprintf(dns_fp, "nameserver %s %s\n", dns1, dns_tag);
284 if (dns2)
285 fprintf(dns_fp, "nameserver %s %s\n", dns2, dns_tag);
286
287 for (i = 0; i < MAX_DNS && dns_info[i]; i++)
288 fprintf(dns_fp, "%s\n", dns_info[i]);
289 fclose(dns_fp);
290 }
291 else {
292 dbg_time("fopen %s fail, errno:%d (%s)", dns_file, errno, strerror(errno));
293 }
294
295 for (i = 0; i < MAX_DNS && dns_info[i]; i++)
296 free(dns_info[i]);
297 }
298
getpid_by_pdp(int pdp,const char * program_name)299 pid_t getpid_by_pdp(int pdp, const char* program_name)
300 {
301 glob_t gt;
302 int ret;
303 char filter[16];
304 pid_t pid;
305
306 snprintf(filter, sizeof(filter), "-n %d", pdp);
307 ret = glob("/proc/*/cmdline", GLOB_NOSORT, NULL, >);
308 if (ret != 0) {
309 dbg_time("glob error, errno = %d(%s)", errno, strerror(errno));
310 return -1;
311 } else {
312 int i = 0, fd = -1;
313 ssize_t nreads;
314 char cmdline[512] = {0};
315
316 for (i = 0; i < (int)gt.gl_pathc; i++) {
317 fd = open(gt.gl_pathv[i], O_RDONLY);
318 if (fd == -1) {
319 dbg_time("open %s failed, errno = %d(%s)", gt.gl_pathv[i], errno, strerror(errno));
320 globfree(>);
321 return -1;
322 }
323
324 nreads = read(fd, cmdline, sizeof(cmdline));
325 if (nreads > 0) {
326 int pos = 0;
327 while (pos < nreads-1) {
328 if (cmdline[pos] == '\0')
329 cmdline[pos] = ' '; // space
330 pos++;
331 }
332 // printf("%s\n", cmdline);
333 }
334
335 if (strstr(cmdline, program_name) && strstr(cmdline, filter)) {
336 char path[64] = {0};
337 char pidstr[64] = {0};
338 char *p;
339
340 dbg_time("%s: %s", gt.gl_pathv[i], cmdline);
341 strcpy(path, gt.gl_pathv[i]);
342 p = strstr(gt.gl_pathv[i], "/cmdline");
343 *p = '\0';
344 while (*(--p) != '/') ;
345
346 strcpy(pidstr, p+1);
347 pid = atoi(pidstr);
348 globfree(>);
349
350 return pid;
351 }
352 }
353 }
354
355 globfree(>);
356 return -1;
357 }
358
ql_get_driver_rmnet_info(PROFILE_T * profile,RMNET_INFO * rmnet_info)359 void ql_get_driver_rmnet_info(PROFILE_T *profile, RMNET_INFO *rmnet_info) {
360 int ifc_ctl_sock;
361 struct ifreq ifr;
362 int rc;
363 int request = 0x89F3;
364 unsigned char data[512];
365
366 memset(rmnet_info, 0x00, sizeof(*rmnet_info));
367
368 ifc_ctl_sock = socket(AF_INET, SOCK_DGRAM, 0);
369 if (ifc_ctl_sock <= 0) {
370 dbg_time("socket() failed: %s\n", strerror(errno));
371 return;
372 }
373
374 memset(&ifr, 0, sizeof(struct ifreq));
375 strncpy(ifr.ifr_name, profile->usbnet_adapter, IFNAMSIZ);
376 ifr.ifr_name[IFNAMSIZ - 1] = 0;
377 ifr.ifr_ifru.ifru_data = (void *)data;
378
379 rc = ioctl(ifc_ctl_sock, request, &ifr);
380 if (rc < 0) {
381 if (errno != ENOTSUP)
382 dbg_time("ioctl(0x%x, qmap_settings) errno:%d (%s), rc=%d", request, errno, strerror(errno), rc);
383 }
384 else {
385 memcpy(rmnet_info, data, sizeof(*rmnet_info));
386 }
387
388 close(ifc_ctl_sock);
389 }
390
ql_set_driver_qmap_setting(PROFILE_T * profile,QMAP_SETTING * qmap_settings)391 void ql_set_driver_qmap_setting(PROFILE_T *profile, QMAP_SETTING *qmap_settings) {
392 int ifc_ctl_sock;
393 struct ifreq ifr;
394 int rc;
395 int request = 0x89F2;
396
397 ifc_ctl_sock = socket(AF_INET, SOCK_DGRAM, 0);
398 if (ifc_ctl_sock <= 0) {
399 dbg_time("socket() failed: %s\n", strerror(errno));
400 return;
401 }
402
403 memset(&ifr, 0, sizeof(struct ifreq));
404 strncpy(ifr.ifr_name, profile->usbnet_adapter, IFNAMSIZ);
405 ifr.ifr_name[IFNAMSIZ - 1] = 0;
406 ifr.ifr_ifru.ifru_data = (void *)qmap_settings;
407
408 rc = ioctl(ifc_ctl_sock, request, &ifr);
409 if (rc < 0) {
410 dbg_time("ioctl(0x%x, qmap_settings) failed: %s, rc=%d", request, strerror(errno), rc);
411 }
412
413 close(ifc_ctl_sock);
414 }
415