1 /******************************************************************************
2 @file quectel-qmi-proxy.c
3 @brief The qmi proxy.
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 #include <unistd.h>
17 #include <stdlib.h>
18 #include <stdio.h>
19 #include <string.h>
20 #include <errno.h>
21 #include <stdarg.h>
22 #include <stddef.h>
23 #include <fcntl.h>
24 #include <pthread.h>
25 #include <poll.h>
26 #include <sys/socket.h>
27 #include <sys/time.h>
28 #include <sys/ioctl.h>
29 #include <linux/un.h>
30 #include <linux/if.h>
31 #include <dirent.h>
32 #include <signal.h>
33 #include <endian.h>
34 #include <inttypes.h>
35
36 #ifndef MIN
37 #define MIN(a, b) ((a) < (b)? (a): (b))
38 #endif
39
40 #ifndef htole32
41 #if __BYTE_ORDER == __LITTLE_ENDIAN
42 #define htole16(x) (uint16_t)(x)
43 #define le16toh(x) (uint16_t)(x)
44 #define letoh16(x) (uint16_t)(x)
45 #define htole32(x) (uint32_t)(x)
46 #define le32toh(x) (uint32_t)(x)
47 #define letoh32(x) (uint32_t)(x)
48 #define htole64(x) (uint64_t)(x)
49 #define le64toh(x) (uint64_t)(x)
50 #define letoh64(x) (uint64_t)(x)
51 #else
__bswap16(uint16_t __x)52 static __inline uint16_t __bswap16(uint16_t __x) {
53 return (__x<<8) | (__x>>8);
54 }
55
__bswap32(uint32_t __x)56 static __inline uint32_t __bswap32(uint32_t __x) {
57 return (__x>>24) | (__x>>8&0xff00) | (__x<<8&0xff0000) | (__x<<24);
58 }
59
__bswap64(uint64_t __x)60 static __inline uint64_t __bswap64(uint64_t __x) {
61 return (__bswap32(__x)+0ULL<<32) | (__bswap32(__x>>32));
62 }
63
64 #define htole16(x) __bswap16(x)
65 #define le16toh(x) __bswap16(x)
66 #define letoh16(x) __bswap16(x)
67 #define htole32(x) __bswap32(x)
68 #define le32toh(x) __bswap32(x)
69 #define letoh32(x) __bswap32(x)
70 #define htole64(x) __bswap64(x)
71 #define le64toh(x) __bswap64(x)
72 #define letoh64(x) __bswap64(x)
73 #endif
74 #endif
75
get_time(void)76 const char * get_time(void) {
77 static char time_buf[128];
78 struct timeval tv;
79 time_t time;
80 suseconds_t millitm;
81 struct tm *ti;
82
83 gettimeofday (&tv, NULL);
84
85 time= tv.tv_sec;
86 millitm = (tv.tv_usec + 500) / 1000;
87
88 if (millitm == 1000) {
89 ++time;
90 millitm = 0;
91 }
92
93 ti = localtime(&time);
94 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);
95 return time_buf;
96 }
97
98 #define dprintf(fmt, args...) do { fprintf(stdout, "%s " fmt, get_time(), ##args); } while(0);
99 #define SYSCHECK(c) do{if((c)<0) {dprintf("%s %d error: '%s' (code: %d)\n", __func__, __LINE__, strerror(errno), errno); return -1;}}while(0)
100 #define cfmakenoblock(fd) do{fcntl(fd, F_SETFL, fcntl(fd,F_GETFL) | O_NONBLOCK);}while(0)
101
102 typedef struct _QCQMI_HDR
103 {
104 uint8_t IFType;
105 uint16_t Length;
106 uint8_t CtlFlags; // reserved
107 uint8_t QMIType;
108 uint8_t ClientId;
109 } __attribute__ ((packed)) QCQMI_HDR, *PQCQMI_HDR;
110
111 typedef struct _QMICTL_SYNC_REQ_MSG
112 {
113 uint8_t CtlFlags; // QMICTL_FLAG_REQUEST
114 uint8_t TransactionId;
115 uint16_t QMICTLType; // QMICTL_CTL_SYNC_REQ
116 uint16_t Length; // 0
117 } __attribute__ ((packed)) QMICTL_SYNC_REQ_MSG, *PQMICTL_SYNC_REQ_MSG;
118
119 typedef struct _QMICTL_SYNC_RESP_MSG
120 {
121 uint8_t CtlFlags; // QMICTL_FLAG_RESPONSE
122 uint8_t TransactionId;
123 uint16_t QMICTLType; // QMICTL_CTL_SYNC_RESP
124 uint16_t Length;
125 uint8_t TLVType; // QCTLV_TYPE_RESULT_CODE
126 uint16_t TLVLength; // 0x0004
127 uint16_t QMIResult;
128 uint16_t QMIError;
129 } __attribute__ ((packed)) QMICTL_SYNC_RESP_MSG, *PQMICTL_SYNC_RESP_MSG;
130
131 typedef struct _QMICTL_SYNC_IND_MSG
132 {
133 uint8_t CtlFlags; // QMICTL_FLAG_INDICATION
134 uint8_t TransactionId;
135 uint16_t QMICTLType; // QMICTL_REVOKE_CLIENT_ID_IND
136 uint16_t Length;
137 } __attribute__ ((packed)) QMICTL_SYNC_IND_MSG, *PQMICTL_SYNC_IND_MSG;
138
139 typedef struct _QMICTL_GET_CLIENT_ID_REQ_MSG
140 {
141 uint8_t CtlFlags; // QMICTL_FLAG_REQUEST
142 uint8_t TransactionId;
143 uint16_t QMICTLType; // QMICTL_GET_CLIENT_ID_REQ
144 uint16_t Length;
145 uint8_t TLVType; // QCTLV_TYPE_REQUIRED_PARAMETER
146 uint16_t TLVLength; // 1
147 uint8_t QMIType; // QMUX type
148 } __attribute__ ((packed)) QMICTL_GET_CLIENT_ID_REQ_MSG, *PQMICTL_GET_CLIENT_ID_REQ_MSG;
149
150 typedef struct _QMICTL_GET_CLIENT_ID_RESP_MSG
151 {
152 uint8_t CtlFlags; // QMICTL_FLAG_RESPONSE
153 uint8_t TransactionId;
154 uint16_t QMICTLType; // QMICTL_GET_CLIENT_ID_RESP
155 uint16_t Length;
156 uint8_t TLVType; // QCTLV_TYPE_RESULT_CODE
157 uint16_t TLVLength; // 0x0004
158 uint16_t QMIResult; // result code
159 uint16_t QMIError; // error code
160 uint8_t TLV2Type; // QCTLV_TYPE_REQUIRED_PARAMETER
161 uint16_t TLV2Length; // 2
162 uint8_t QMIType;
163 uint8_t ClientId;
164 } __attribute__ ((packed)) QMICTL_GET_CLIENT_ID_RESP_MSG, *PQMICTL_GET_CLIENT_ID_RESP_MSG;
165
166 typedef struct _QMICTL_RELEASE_CLIENT_ID_REQ_MSG
167 {
168 uint8_t CtlFlags; // QMICTL_FLAG_REQUEST
169 uint8_t TransactionId;
170 uint16_t QMICTLType; // QMICTL_RELEASE_CLIENT_ID_REQ
171 uint16_t Length;
172 uint8_t TLVType; // QCTLV_TYPE_REQUIRED_PARAMETER
173 uint16_t TLVLength; // 0x0002
174 uint8_t QMIType;
175 uint8_t ClientId;
176 } __attribute__ ((packed)) QMICTL_RELEASE_CLIENT_ID_REQ_MSG, *PQMICTL_RELEASE_CLIENT_ID_REQ_MSG;
177
178 typedef struct _QMICTL_RELEASE_CLIENT_ID_RESP_MSG
179 {
180 uint8_t CtlFlags; // QMICTL_FLAG_RESPONSE
181 uint8_t TransactionId;
182 uint16_t QMICTLType; // QMICTL_RELEASE_CLIENT_ID_RESP
183 uint16_t Length;
184 uint8_t TLVType; // QCTLV_TYPE_RESULT_CODE
185 uint16_t TLVLength; // 0x0004
186 uint16_t QMIResult; // result code
187 uint16_t QMIError; // error code
188 uint8_t TLV2Type; // QCTLV_TYPE_REQUIRED_PARAMETER
189 uint16_t TLV2Length; // 2
190 uint8_t QMIType;
191 uint8_t ClientId;
192 } __attribute__ ((packed)) QMICTL_RELEASE_CLIENT_ID_RESP_MSG, *PQMICTL_RELEASE_CLIENT_ID_RESP_MSG;
193
194 // QMICTL Control Flags
195 #define QMICTL_CTL_FLAG_CMD 0x00
196 #define QMICTL_CTL_FLAG_RSP 0x01
197 #define QMICTL_CTL_FLAG_IND 0x02
198
199 typedef struct _QCQMICTL_MSG_HDR
200 {
201 uint8_t CtlFlags; // 00-cmd, 01-rsp, 10-ind
202 uint8_t TransactionId;
203 uint16_t QMICTLType;
204 uint16_t Length;
205 } __attribute__ ((packed)) QCQMICTL_MSG_HDR, *PQCQMICTL_MSG_HDR;
206
207 #define QCQMICTL_MSG_HDR_SIZE sizeof(QCQMICTL_MSG_HDR)
208
209 typedef struct _QCQMICTL_MSG_HDR_RESP
210 {
211 uint8_t CtlFlags; // 00-cmd, 01-rsp, 10-ind
212 uint8_t TransactionId;
213 uint16_t QMICTLType;
214 uint16_t Length;
215 uint8_t TLVType; // 0x02 - result code
216 uint16_t TLVLength; // 4
217 uint16_t QMUXResult; // QMI_RESULT_SUCCESS
218 // QMI_RESULT_FAILURE
219 uint16_t QMUXError; // QMI_ERR_INVALID_ARG
220 // QMI_ERR_NO_MEMORY
221 // QMI_ERR_INTERNAL
222 // QMI_ERR_FAULT
223 } __attribute__ ((packed)) QCQMICTL_MSG_HDR_RESP, *PQCQMICTL_MSG_HDR_RESP;
224
225
226 typedef struct _QMICTL_MSG
227 {
228 union
229 {
230 // Message Header
231 QCQMICTL_MSG_HDR QMICTLMsgHdr;
232 QCQMICTL_MSG_HDR_RESP QMICTLMsgHdrRsp;
233
234 // QMICTL Message
235 //QMICTL_SET_INSTANCE_ID_REQ_MSG SetInstanceIdReq;
236 //QMICTL_SET_INSTANCE_ID_RESP_MSG SetInstanceIdRsp;
237 //QMICTL_GET_VERSION_REQ_MSG GetVersionReq;
238 //QMICTL_GET_VERSION_RESP_MSG GetVersionRsp;
239 QMICTL_GET_CLIENT_ID_REQ_MSG GetClientIdReq;
240 QMICTL_GET_CLIENT_ID_RESP_MSG GetClientIdRsp;
241 //QMICTL_RELEASE_CLIENT_ID_REQ_MSG ReleaseClientIdReq;
242 QMICTL_RELEASE_CLIENT_ID_RESP_MSG ReleaseClientIdRsp;
243 //QMICTL_REVOKE_CLIENT_ID_IND_MSG RevokeClientIdInd;
244 //QMICTL_INVALID_CLIENT_ID_IND_MSG InvalidClientIdInd;
245 //QMICTL_SET_DATA_FORMAT_REQ_MSG SetDataFormatReq;
246 //QMICTL_SET_DATA_FORMAT_RESP_MSG SetDataFormatRsp;
247 QMICTL_SYNC_REQ_MSG SyncReq;
248 QMICTL_SYNC_RESP_MSG SyncRsp;
249 QMICTL_SYNC_IND_MSG SyncInd;
250 };
251 } __attribute__ ((packed)) QMICTL_MSG, *PQMICTL_MSG;
252
253 typedef struct _QCQMUX_MSG_HDR
254 {
255 uint8_t CtlFlags; // 0: single QMUX Msg; 1:
256 uint16_t TransactionId;
257 uint16_t Type;
258 uint16_t Length;
259 uint8_t payload[0];
260 } __attribute__ ((packed)) QCQMUX_MSG_HDR, *PQCQMUX_MSG_HDR;
261
262 typedef struct _QCQMUX_MSG_HDR_RESP
263 {
264 uint8_t CtlFlags; // 0: single QMUX Msg; 1:
265 uint16_t TransactionId;
266 uint16_t Type;
267 uint16_t Length;
268 uint8_t TLVType; // 0x02 - result code
269 uint16_t TLVLength; // 4
270 uint16_t QMUXResult; // QMI_RESULT_SUCCESS
271 // QMI_RESULT_FAILURE
272 uint16_t QMUXError; // QMI_ERR_INVALID_ARG
273 // QMI_ERR_NO_MEMORY
274 // QMI_ERR_INTERNAL
275 // QMI_ERR_FAULT
276 } __attribute__ ((packed)) QCQMUX_MSG_HDR_RESP, *PQCQMUX_MSG_HDR_RESP;
277
278 //#define QUECTEL_QMI_MERGE
279 typedef uint32_t UINT;
280
281 typedef struct _QCQMUX_TLV
282 {
283 uint8_t Type;
284 uint16_t Length;
285 uint8_t Value[0];
286 } __attribute__ ((packed)) QCQMUX_TLV, *PQCQMUX_TLV;
287
288 typedef struct _QMUX_MSG
289 {
290 union
291 {
292 // Message Header
293 QCQMUX_MSG_HDR QMUXMsgHdr;
294 QCQMUX_MSG_HDR_RESP QMUXMsgHdrResp;
295 //QMIWDS_ADMIN_SET_DATA_FORMAT_REQ_MSG SetDataFormatReq;
296 };
297 } __attribute__ ((packed)) QMUX_MSG, *PQMUX_MSG;
298
299 typedef struct _QCQMIMSG {
300 QCQMI_HDR QMIHdr;
301 union {
302 QMICTL_MSG CTLMsg;
303 QMUX_MSG MUXMsg;
304 };
305 } __attribute__ ((packed)) QCQMIMSG, *PQCQMIMSG;
306
307
308 // QMUX Message Definitions -- QMI SDU
309 #define QMUX_CTL_FLAG_SINGLE_MSG 0x00
310 #define QMUX_CTL_FLAG_COMPOUND_MSG 0x01
311 #define QMUX_CTL_FLAG_TYPE_CMD 0x00
312 #define QMUX_CTL_FLAG_TYPE_RSP 0x02
313 #define QMUX_CTL_FLAG_TYPE_IND 0x04
314 #define QMUX_CTL_FLAG_MASK_COMPOUND 0x01
315 #define QMUX_CTL_FLAG_MASK_TYPE 0x06 // 00-cmd, 01-rsp, 10-ind
316
317 #define USB_CTL_MSG_TYPE_QMI 0x01
318
319 #define QMICTL_FLAG_REQUEST 0x00
320 #define QMICTL_FLAG_RESPONSE 0x01
321 #define QMICTL_FLAG_INDICATION 0x02
322
323 // QMICTL Type
324 #define QMICTL_SET_INSTANCE_ID_REQ 0x0020
325 #define QMICTL_SET_INSTANCE_ID_RESP 0x0020
326 #define QMICTL_GET_VERSION_REQ 0x0021
327 #define QMICTL_GET_VERSION_RESP 0x0021
328 #define QMICTL_GET_CLIENT_ID_REQ 0x0022
329 #define QMICTL_GET_CLIENT_ID_RESP 0x0022
330 #define QMICTL_RELEASE_CLIENT_ID_REQ 0x0023
331 #define QMICTL_RELEASE_CLIENT_ID_RESP 0x0023
332 #define QMICTL_REVOKE_CLIENT_ID_IND 0x0024
333 #define QMICTL_INVALID_CLIENT_ID_IND 0x0025
334 #define QMICTL_SET_DATA_FORMAT_REQ 0x0026
335 #define QMICTL_SET_DATA_FORMAT_RESP 0x0026
336 #define QMICTL_SYNC_REQ 0x0027
337 #define QMICTL_SYNC_RESP 0x0027
338 #define QMICTL_SYNC_IND 0x0027
339
340 #define QCTLV_TYPE_REQUIRED_PARAMETER 0x01
341
342 // Define QMI Type
343 typedef enum _QMI_SERVICE_TYPE
344 {
345 QMUX_TYPE_CTL = 0x00,
346 QMUX_TYPE_WDS = 0x01,
347 QMUX_TYPE_DMS = 0x02,
348 QMUX_TYPE_NAS = 0x03,
349 QMUX_TYPE_QOS = 0x04,
350 QMUX_TYPE_WMS = 0x05,
351 QMUX_TYPE_PDS = 0x06,
352 QMUX_TYPE_UIM = 0x0B,
353 QMUX_TYPE_WDS_IPV6 = 0x11,
354 QMUX_TYPE_WDS_ADMIN = 0x1A,
355 QMUX_TYPE_MAX = 0xFF,
356 QMUX_TYPE_ALL = 0xFF
357 } QMI_SERVICE_TYPE;
358
359 #define QMIWDS_ADMIN_SET_DATA_FORMAT_REQ 0x0020
360 #define QMIWDS_ADMIN_SET_DATA_FORMAT_RESP 0x0020
361
362 struct qlistnode
363 {
364 struct qlistnode *next;
365 struct qlistnode *prev;
366 };
367
368 #define qnode_to_item(node, container, member) \
369 (container *) (((char*) (node)) - offsetof(container, member))
370
371 #define qlist_for_each(node, list) \
372 for (node = (list)->next; node != (list); node = node->next)
373
374 #define qlist_empty(list) ((list) == (list)->next)
375 #define qlist_head(list) ((list)->next)
376 #define qlist_tail(list) ((list)->prev)
377
378 typedef struct {
379 struct qlistnode qnode;
380 int ClientFd;
381 QCQMIMSG qmi[0];
382 } QMI_PROXY_MSG;
383
384 typedef struct {
385 struct qlistnode qnode;
386 uint8_t QMIType;
387 uint8_t ClientId;
388 unsigned AccessTime;
389 } QMI_PROXY_CLINET;
390
391 typedef struct {
392 struct qlistnode qnode;
393 struct qlistnode client_qnode;
394 int ClientFd;
395 unsigned AccessTime;
396 } QMI_PROXY_CONNECTION;
397
398 #ifdef QUECTEL_QMI_MERGE
399 #define MERGE_PACKET_IDENTITY 0x2c7c
400 #define MERGE_PACKET_VERSION 0x0001
401 #define MERGE_PACKET_MAX_PAYLOAD_SIZE 56
402 typedef struct __QMI_MSG_HEADER {
403 uint16_t idenity;
404 uint16_t version;
405 uint16_t cur_len;
406 uint16_t total_len;
407 } QMI_MSG_HEADER;
408
409 typedef struct __QMI_MSG_PACKET {
410 QMI_MSG_HEADER header;
411 uint16_t len;
412 char buf[4096];
413 } QMI_MSG_PACKET;
414 #endif
415
qlist_init(struct qlistnode * node)416 static void qlist_init(struct qlistnode *node)
417 {
418 node->next = node;
419 node->prev = node;
420 }
421
qlist_add_tail(struct qlistnode * head,struct qlistnode * item)422 static void qlist_add_tail(struct qlistnode *head, struct qlistnode *item)
423 {
424 item->next = head;
425 item->prev = head->prev;
426 head->prev->next = item;
427 head->prev = item;
428 }
429
qlist_remove(struct qlistnode * item)430 static void qlist_remove(struct qlistnode *item)
431 {
432 item->next->prev = item->prev;
433 item->prev->next = item->next;
434 }
435
436 static int qmi_proxy_quit = 0;
437 static pthread_t thread_id = 0;
438 static int cdc_wdm_fd = -1;
439 static int qmi_proxy_server_fd = -1;
440 static struct qlistnode qmi_proxy_connection;
441 static struct qlistnode qmi_proxy_ctl_msg;
442 static int verbose_debug = 0;
443 static int modem_reset_flag = 0;
444 static int qmi_sync_done = 0;
445 static uint8_t qmi_buf[4096];
446
447 #ifdef QUECTEL_QMI_MERGE
merge_qmi_rsp_packet(void * buf,ssize_t * src_size)448 static int merge_qmi_rsp_packet(void *buf, ssize_t *src_size) {
449 static QMI_MSG_PACKET s_QMIPacket;
450 QMI_MSG_HEADER *header = NULL;
451 ssize_t size = *src_size;
452
453 if((uint16_t)size < sizeof(QMI_MSG_HEADER))
454 return -1;
455
456 header = (QMI_MSG_HEADER *)buf;
457 if(le16toh(header->idenity) != MERGE_PACKET_IDENTITY || le16toh(header->version) != MERGE_PACKET_VERSION || le16toh(header->cur_len) > le16toh(header->total_len))
458 return -1;
459
460 if(le16toh(header->cur_len) == le16toh(header->total_len)) {
461 *src_size = le16toh(header->total_len);
462 memcpy(buf, buf + sizeof(QMI_MSG_HEADER), *src_size);
463 s_QMIPacket.len = 0;
464 return 0;
465 }
466
467 memcpy(s_QMIPacket.buf + s_QMIPacket.len, buf + sizeof(QMI_MSG_HEADER), le16toh(header->cur_len));
468 s_QMIPacket.len += le16toh(header->cur_len);
469
470 if (le16toh(header->cur_len) < MERGE_PACKET_MAX_PAYLOAD_SIZE || s_QMIPacket.len >= le16toh(header->total_len)) {
471 memcpy(buf, s_QMIPacket.buf, s_QMIPacket.len);
472 *src_size = s_QMIPacket.len;
473 s_QMIPacket.len = 0;
474 return 0;
475 }
476
477 return -1;
478 }
479 #endif
480
create_local_server(const char * name)481 static int create_local_server(const char *name) {
482 int sockfd = -1;
483 int reuse_addr = 1;
484 struct sockaddr_un sockaddr;
485 socklen_t alen;
486
487 /*Create server socket*/
488 SYSCHECK(sockfd = socket(AF_LOCAL, SOCK_STREAM, 0));
489
490 memset(&sockaddr, 0, sizeof(sockaddr));
491 sockaddr.sun_family = AF_LOCAL;
492 sockaddr.sun_path[0] = 0;
493 memcpy(sockaddr.sun_path + 1, name, strlen(name) );
494
495 alen = strlen(name) + offsetof(struct sockaddr_un, sun_path) + 1;
496 SYSCHECK(setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR, &reuse_addr,sizeof(reuse_addr)));
497 if(bind(sockfd, (struct sockaddr *)&sockaddr, alen) < 0) {
498 close(sockfd);
499 dprintf("bind %s errno: %d (%s)\n", name, errno, strerror(errno));
500 return -1;
501 }
502
503 dprintf("local server: %s sockfd = %d\n", name, sockfd);
504 cfmakenoblock(sockfd);
505 listen(sockfd, 1);
506
507 return sockfd;
508 }
509
accept_qmi_connection(int serverfd)510 static void accept_qmi_connection(int serverfd) {
511 int clientfd = -1;
512 unsigned char addr[128];
513 socklen_t alen = sizeof(addr);
514 QMI_PROXY_CONNECTION *qmi_con;
515
516 clientfd = accept(serverfd, (struct sockaddr *)addr, &alen);
517
518 qmi_con = (QMI_PROXY_CONNECTION *)malloc(sizeof(QMI_PROXY_CONNECTION));
519 if (qmi_con) {
520 qlist_init(&qmi_con->qnode);
521 qlist_init(&qmi_con->client_qnode);
522 qmi_con->ClientFd= clientfd;
523 qmi_con->AccessTime = 0;
524 dprintf("+++ ClientFd=%d\n", qmi_con->ClientFd);
525 qlist_add_tail(&qmi_proxy_connection, &qmi_con->qnode);
526 }
527
528 cfmakenoblock(clientfd);
529 }
530
cleanup_qmi_connection(int clientfd)531 static void cleanup_qmi_connection(int clientfd) {
532 struct qlistnode *con_node, *qmi_node;
533
534 qlist_for_each(con_node, &qmi_proxy_connection) {
535 QMI_PROXY_CONNECTION *qmi_con = qnode_to_item(con_node, QMI_PROXY_CONNECTION, qnode);
536
537 if (qmi_con->ClientFd == clientfd) {
538 while (!qlist_empty(&qmi_con->client_qnode)) {
539 QMI_PROXY_CLINET *qmi_client = qnode_to_item(qlist_head(&qmi_con->client_qnode), QMI_PROXY_CLINET, qnode);
540
541 dprintf("xxx ClientFd=%d QMIType=%d ClientId=%d\n", qmi_con->ClientFd, qmi_client->QMIType, qmi_client->ClientId);
542
543 qlist_remove(&qmi_client->qnode);
544 free(qmi_client);
545 }
546
547 qlist_for_each(qmi_node, &qmi_proxy_ctl_msg) {
548 QMI_PROXY_MSG *qmi_msg = qnode_to_item(qmi_node, QMI_PROXY_MSG, qnode);
549
550 if (qmi_msg->ClientFd == qmi_con->ClientFd) {
551 qlist_remove(&qmi_msg->qnode);
552 free(qmi_msg);
553 break;
554 }
555 }
556
557 dprintf("--- ClientFd=%d\n", qmi_con->ClientFd);
558 close(qmi_con->ClientFd);
559 qlist_remove(&qmi_con->qnode);
560 free(qmi_con);
561 break;
562 }
563 }
564 }
565
get_client_id(QMI_PROXY_CONNECTION * qmi_con,PQMICTL_GET_CLIENT_ID_RESP_MSG pClient)566 static void get_client_id(QMI_PROXY_CONNECTION *qmi_con, PQMICTL_GET_CLIENT_ID_RESP_MSG pClient) {
567 if (pClient->QMIResult == 0 && pClient->QMIError == 0) {
568 QMI_PROXY_CLINET *qmi_client = (QMI_PROXY_CLINET *)malloc(sizeof(QMI_PROXY_CLINET));
569
570 qlist_init(&qmi_client->qnode);
571 qmi_client->QMIType = pClient->QMIType;
572 qmi_client->ClientId = pClient->ClientId;
573 qmi_client->AccessTime = 0;
574
575 dprintf("+++ ClientFd=%d QMIType=%d ClientId=%d\n", qmi_con->ClientFd, qmi_client->QMIType, qmi_client->ClientId);
576 qlist_add_tail(&qmi_con->client_qnode, &qmi_client->qnode);
577 }
578 }
579
release_client_id(QMI_PROXY_CONNECTION * qmi_con,PQMICTL_RELEASE_CLIENT_ID_RESP_MSG pClient)580 static void release_client_id(QMI_PROXY_CONNECTION *qmi_con, PQMICTL_RELEASE_CLIENT_ID_RESP_MSG pClient) {
581 struct qlistnode *client_node;
582
583 if (pClient->QMIResult == 0 && pClient->QMIError == 0) {
584 qlist_for_each (client_node, &qmi_con->client_qnode) {
585 QMI_PROXY_CLINET *qmi_client = qnode_to_item(client_node, QMI_PROXY_CLINET, qnode);
586
587 if (pClient->QMIType == qmi_client->QMIType && pClient->ClientId == qmi_client->ClientId) {
588 dprintf("--- ClientFd=%d QMIType=%d ClientId=%d\n", qmi_con->ClientFd, qmi_client->QMIType, qmi_client->ClientId);
589 qlist_remove(&qmi_client->qnode);
590 free(qmi_client);
591 break;
592 }
593 }
594 }
595 }
596
dump_qmi(PQCQMIMSG pQMI,int fd,const char flag)597 static void dump_qmi(PQCQMIMSG pQMI, int fd, const char flag)
598 {
599 if (verbose_debug)
600 {
601 unsigned i;
602 unsigned size = le16toh(pQMI->QMIHdr.Length) + 1;
603 printf("%c %d %u: ", flag, fd, size);
604 if (size > 16)
605 size = 16;
606 for (i = 0; i < size; i++)
607 printf("%02x ", ((uint8_t *)pQMI)[i]);
608 printf("\n");
609 }
610 }
611
send_qmi_to_cdc_wdm(PQCQMIMSG pQMI)612 static int send_qmi_to_cdc_wdm(PQCQMIMSG pQMI) {
613 struct pollfd pollfds[]= {{cdc_wdm_fd, POLLOUT, 0}};
614 ssize_t ret = 0;
615
616 do {
617 ret = poll(pollfds, sizeof(pollfds)/sizeof(pollfds[0]), 5000);
618 } while (ret == -1 && errno == EINTR && qmi_proxy_quit == 0);
619
620 if (pollfds[0].revents & POLLOUT) {
621 ssize_t size = le16toh(pQMI->QMIHdr.Length) + 1;
622 ret = write(cdc_wdm_fd, pQMI, size);
623 dump_qmi(pQMI, cdc_wdm_fd, 'w');
624 }
625
626 return ret;
627 }
628
send_qmi_to_client(PQCQMIMSG pQMI,int clientFd)629 static int send_qmi_to_client(PQCQMIMSG pQMI, int clientFd) {
630 struct pollfd pollfds[]= {{clientFd, POLLOUT, 0}};
631 ssize_t ret = 0;
632
633 do {
634 ret = poll(pollfds, sizeof(pollfds)/sizeof(pollfds[0]), 5000);
635 } while (ret == -1 && errno == EINTR && qmi_proxy_quit == 0);
636
637 if (pollfds[0].revents & POLLOUT) {
638 ssize_t size = le16toh(pQMI->QMIHdr.Length) + 1;
639 ret = write(clientFd, pQMI, size);
640 dump_qmi(pQMI, clientFd, 'w');
641 }
642
643 return ret;
644 }
645
recv_qmi_from_dev(PQCQMIMSG pQMI)646 static void recv_qmi_from_dev(PQCQMIMSG pQMI) {
647 struct qlistnode *con_node, *client_node;
648
649 if (qmi_proxy_server_fd == -1) {
650 qmi_sync_done = 1;
651 }
652 else if (pQMI->QMIHdr.QMIType == QMUX_TYPE_CTL) {
653 if (pQMI->CTLMsg.QMICTLMsgHdr.CtlFlags == QMICTL_CTL_FLAG_RSP) {
654 if (!qlist_empty(&qmi_proxy_ctl_msg)) {
655 QMI_PROXY_MSG *qmi_msg = qnode_to_item(qlist_head(&qmi_proxy_ctl_msg), QMI_PROXY_MSG, qnode);
656
657 qlist_for_each(con_node, &qmi_proxy_connection) {
658 QMI_PROXY_CONNECTION *qmi_con = qnode_to_item(con_node, QMI_PROXY_CONNECTION, qnode);
659
660 if (qmi_con->ClientFd == qmi_msg->ClientFd) {
661 send_qmi_to_client(pQMI, qmi_msg->ClientFd);
662
663 if (le16toh(pQMI->CTLMsg.QMICTLMsgHdrRsp.QMICTLType) == QMICTL_GET_CLIENT_ID_RESP)
664 get_client_id(qmi_con, &pQMI->CTLMsg.GetClientIdRsp);
665 else if ((le16toh(pQMI->CTLMsg.QMICTLMsgHdrRsp.QMICTLType) == QMICTL_RELEASE_CLIENT_ID_RESP) ||
666 (le16toh(pQMI->CTLMsg.QMICTLMsgHdrRsp.QMICTLType) == QMICTL_REVOKE_CLIENT_ID_IND)) {
667 release_client_id(qmi_con, &pQMI->CTLMsg.ReleaseClientIdRsp);
668 if (le16toh(pQMI->CTLMsg.QMICTLMsgHdrRsp.QMICTLType) == QMICTL_REVOKE_CLIENT_ID_IND)
669 modem_reset_flag = 1;
670 }
671 else {
672 }
673 }
674 }
675
676 qlist_remove(&qmi_msg->qnode);
677 free(qmi_msg);
678 }
679 }
680
681 if (!qlist_empty(&qmi_proxy_ctl_msg)) {
682 QMI_PROXY_MSG *qmi_msg = qnode_to_item(qlist_head(&qmi_proxy_ctl_msg), QMI_PROXY_MSG, qnode);
683
684 qlist_for_each(con_node, &qmi_proxy_connection) {
685 QMI_PROXY_CONNECTION *qmi_con = qnode_to_item(con_node, QMI_PROXY_CONNECTION, qnode);
686
687 if (qmi_con->ClientFd == qmi_msg->ClientFd) {
688 send_qmi_to_cdc_wdm(qmi_msg->qmi);
689 }
690 }
691 }
692 }
693 else {
694 qlist_for_each(con_node, &qmi_proxy_connection) {
695 QMI_PROXY_CONNECTION *qmi_con = qnode_to_item(con_node, QMI_PROXY_CONNECTION, qnode);
696
697 qlist_for_each(client_node, &qmi_con->client_qnode) {
698 QMI_PROXY_CLINET *qmi_client = qnode_to_item(client_node, QMI_PROXY_CLINET, qnode);
699 if (pQMI->QMIHdr.QMIType == qmi_client->QMIType) {
700 if (pQMI->QMIHdr.ClientId == 0 || pQMI->QMIHdr.ClientId == qmi_client->ClientId) {
701 send_qmi_to_client(pQMI, qmi_con->ClientFd);
702 }
703 }
704 }
705 }
706 }
707 }
708
recv_qmi_from_client(PQCQMIMSG pQMI,unsigned size,int clientfd)709 static int recv_qmi_from_client(PQCQMIMSG pQMI, unsigned size, int clientfd) {
710 if (qmi_proxy_server_fd <= 0) {
711 send_qmi_to_cdc_wdm(pQMI);
712 }
713 else if (pQMI->QMIHdr.QMIType == QMUX_TYPE_CTL) {
714 QMI_PROXY_MSG *qmi_msg;
715
716 if (pQMI->CTLMsg.QMICTLMsgHdr.QMICTLType == QMICTL_SYNC_REQ) {
717 dprintf("do not allow client send QMICTL_SYNC_REQ\n");
718 return 0;
719 }
720
721 if (qlist_empty(&qmi_proxy_ctl_msg))
722 send_qmi_to_cdc_wdm(pQMI);
723
724 qmi_msg = malloc(sizeof(QMI_PROXY_MSG) + size);
725 qlist_init(&qmi_msg->qnode);
726 qmi_msg->ClientFd = clientfd;
727 memcpy(qmi_msg->qmi, pQMI, size);
728 qlist_add_tail(&qmi_proxy_ctl_msg, &qmi_msg->qnode);
729 }
730 else {
731 send_qmi_to_cdc_wdm(pQMI);
732 }
733
734 return 0;
735 }
736
qmi_proxy_init(void)737 static int qmi_proxy_init(void) {
738 unsigned i;
739 QCQMIMSG _QMI;
740 PQCQMIMSG pQMI = &_QMI;
741
742 dprintf("%s enter\n", __func__);
743
744 pQMI->QMIHdr.IFType = USB_CTL_MSG_TYPE_QMI;
745 pQMI->QMIHdr.CtlFlags = 0x00;
746 pQMI->QMIHdr.QMIType = QMUX_TYPE_CTL;
747 pQMI->QMIHdr.ClientId= 0x00;
748
749 pQMI->CTLMsg.QMICTLMsgHdr.CtlFlags = QMICTL_FLAG_REQUEST;
750
751 qmi_sync_done = 0;
752 for (i = 0; i < 10; i++) {
753 pQMI->CTLMsg.SyncReq.TransactionId = i+1;
754 pQMI->CTLMsg.SyncReq.QMICTLType = QMICTL_SYNC_REQ;
755 pQMI->CTLMsg.SyncReq.Length = 0;
756
757 pQMI->QMIHdr.Length =
758 htole16(le16toh(pQMI->CTLMsg.QMICTLMsgHdr.Length) + sizeof(QCQMI_HDR) + sizeof(QCQMICTL_MSG_HDR) - 1);
759
760 if (send_qmi_to_cdc_wdm(pQMI) <= 0)
761 break;
762
763 sleep(1);
764 if (qmi_sync_done)
765 break;
766 }
767
768 dprintf("%s %s\n", __func__, qmi_sync_done ? "succful" : "fail");
769 return qmi_sync_done ? 0 : -1;
770 }
771
qmi_start_server(const char * servername)772 static void qmi_start_server(const char* servername) {
773 qmi_proxy_server_fd = create_local_server(servername);
774 dprintf("qmi_proxy_server_fd = %d\n", qmi_proxy_server_fd);
775 if (qmi_proxy_server_fd == -1) {
776 dprintf("Failed to create %s, errno: %d (%s)\n", servername, errno, strerror(errno));
777 }
778 }
779
qmi_close_server(const char * servername)780 static void qmi_close_server(const char* servername) {
781 if (qmi_proxy_server_fd != -1) {
782 dprintf("%s %s close server\n", __func__, servername);
783 close(qmi_proxy_server_fd);
784 qmi_proxy_server_fd = -1;
785 }
786 }
787
qmi_proxy_loop(void * param)788 static void *qmi_proxy_loop(void *param)
789 {
790 PQCQMIMSG pQMI = (PQCQMIMSG)qmi_buf;
791 struct qlistnode *con_node;
792 QMI_PROXY_CONNECTION *qmi_con;
793
794 (void)param;
795 dprintf("%s enter thread_id %p\n", __func__, (void *)pthread_self());
796
797 qlist_init(&qmi_proxy_connection);
798 qlist_init(&qmi_proxy_ctl_msg);
799
800 while (cdc_wdm_fd > 0 && qmi_proxy_quit == 0) {
801 struct pollfd pollfds[2+64];
802 int ne, ret, nevents = 0;
803 ssize_t nreads;
804
805 pollfds[nevents].fd = cdc_wdm_fd;
806 pollfds[nevents].events = POLLIN;
807 pollfds[nevents].revents= 0;
808 nevents++;
809
810 if (qmi_proxy_server_fd > 0) {
811 pollfds[nevents].fd = qmi_proxy_server_fd;
812 pollfds[nevents].events = POLLIN;
813 pollfds[nevents].revents= 0;
814 nevents++;
815 }
816
817 qlist_for_each(con_node, &qmi_proxy_connection) {
818 qmi_con = qnode_to_item(con_node, QMI_PROXY_CONNECTION, qnode);
819
820 pollfds[nevents].fd = qmi_con->ClientFd;
821 pollfds[nevents].events = POLLIN;
822 pollfds[nevents].revents= 0;
823 nevents++;
824
825 if (nevents == (sizeof(pollfds)/sizeof(pollfds[0])))
826 break;
827 }
828
829 #if 0
830 dprintf("poll ");
831 for (ne = 0; ne < nevents; ne++) {
832 dprintf("%d ", pollfds[ne].fd);
833 }
834 dprintf("\n");
835 #endif
836
837 do {
838 //ret = poll(pollfds, nevents, -1);
839 ret = poll(pollfds, nevents, (qmi_proxy_server_fd > 0) ? -1 : 200);
840 } while (ret == -1 && errno == EINTR && qmi_proxy_quit == 0);
841
842 if (ret < 0) {
843 dprintf("%s poll=%d, errno: %d (%s)\n", __func__, ret, errno, strerror(errno));
844 goto qmi_proxy_loop_exit;
845 }
846
847 for (ne = 0; ne < nevents; ne++) {
848 int fd = pollfds[ne].fd;
849 short revents = pollfds[ne].revents;
850
851 if (revents & (POLLERR | POLLHUP | POLLNVAL)) {
852 dprintf("%s poll fd = %d, revents = %04x\n", __func__, fd, revents);
853 if (fd == cdc_wdm_fd) {
854 goto qmi_proxy_loop_exit;
855 } else if(fd == qmi_proxy_server_fd) {
856
857 } else {
858 cleanup_qmi_connection(fd);
859 }
860
861 continue;
862 }
863
864 if (!(pollfds[ne].revents & POLLIN)) {
865 continue;
866 }
867
868 if (fd == qmi_proxy_server_fd) {
869 accept_qmi_connection(fd);
870 }
871 else if (fd == cdc_wdm_fd) {
872 nreads = read(fd, pQMI, sizeof(qmi_buf));
873 if (nreads <= 0) {
874 dprintf("%s read=%d errno: %d (%s)\n", __func__, (int)nreads, errno, strerror(errno));
875 goto qmi_proxy_loop_exit;
876 }
877 #ifdef QUECTEL_QMI_MERGE
878 if(merge_qmi_rsp_packet(pQMI, &nreads))
879 continue;
880 #endif
881 if (nreads != (le16toh(pQMI->QMIHdr.Length) + 1)) {
882 dprintf("%s nreads=%d, pQCQMI->QMIHdr.Length = %d\n", __func__, (int)nreads, le16toh(pQMI->QMIHdr.Length));
883 continue;
884 }
885
886 dump_qmi(pQMI, fd, 'r');
887 recv_qmi_from_dev(pQMI);
888 if (modem_reset_flag)
889 goto qmi_proxy_loop_exit;
890 }
891 else {
892 nreads = read(fd, pQMI, sizeof(qmi_buf));
893
894 if (nreads <= 0) {
895 dprintf("%s read=%d errno: %d (%s)", __func__, (int)nreads, errno, strerror(errno));
896 cleanup_qmi_connection(fd);
897 break;
898 }
899
900 if (nreads != (le16toh(pQMI->QMIHdr.Length) + 1)) {
901 dprintf("%s nreads=%d, pQCQMI->QMIHdr.Length = %d\n", __func__, (int)nreads, le16toh(pQMI->QMIHdr.Length));
902 continue;
903 }
904
905 dump_qmi(pQMI, fd, 'r');
906 recv_qmi_from_client(pQMI, nreads, fd);
907 }
908 }
909 }
910
911 qmi_proxy_loop_exit:
912 while (!qlist_empty(&qmi_proxy_connection)) {
913 QMI_PROXY_CONNECTION *qmi_con = qnode_to_item(qlist_head(&qmi_proxy_connection), QMI_PROXY_CONNECTION, qnode);
914
915 cleanup_qmi_connection(qmi_con->ClientFd);
916 }
917
918 dprintf("%s exit, thread_id %p\n", __func__, (void *)pthread_self());
919
920 return NULL;
921 }
922
usage(void)923 static void usage(void) {
924 dprintf(" -d <device_name> A valid qmi device\n"
925 " default /dev/cdc-wdm0, but cdc-wdm0 may be invalid\n"
926 " -i <netcard_name> netcard name\n"
927 " -v Will show all details\n");
928 }
929
sig_action(int sig)930 static void sig_action(int sig) {
931 if (qmi_proxy_quit == 0) {
932 qmi_proxy_quit = 1;
933 if (thread_id)
934 pthread_kill(thread_id, sig);
935 }
936 }
937
main(int argc,char * argv[])938 int main(int argc, char *argv[]) {
939 int opt;
940 char cdc_wdm[32+1] = "/dev/cdc-wdm0";
941 int retry_times = 0;
942 char servername[64] = {0};
943
944 optind = 1;
945
946 signal(SIGINT, sig_action);
947
948 while ( -1 != (opt = getopt(argc, argv, "d:i:vh"))) {
949 switch (opt) {
950 case 'd':
951 strcpy(cdc_wdm, optarg);
952 break;
953 case 'v':
954 verbose_debug = 1;
955 break;
956 default:
957 usage();
958 return 0;
959 }
960 }
961
962 if (access(cdc_wdm, R_OK | W_OK)) {
963 dprintf("Fail to access %s, errno: %d (%s). break\n", cdc_wdm, errno, strerror(errno));
964 return -1;
965 }
966
967 sprintf(servername, "quectel-qmi-proxy%c", cdc_wdm[strlen(cdc_wdm)-1]);
968 dprintf("Will use cdc-wdm='%s', proxy='%s'\n", cdc_wdm, servername);
969
970 while (qmi_proxy_quit == 0) {
971 if (access(cdc_wdm, R_OK | W_OK)) {
972 dprintf("Fail to access %s, errno: %d (%s). continue\n", cdc_wdm, errno, strerror(errno));
973 // wait device
974 sleep(3);
975 continue;
976 }
977
978 cdc_wdm_fd = open(cdc_wdm, O_RDWR | O_NONBLOCK | O_NOCTTY);
979 if (cdc_wdm_fd == -1) {
980 dprintf("Failed to open %s, errno: %d (%s). break\n", cdc_wdm, errno, strerror(errno));
981 return -1;
982 }
983 cfmakenoblock(cdc_wdm_fd);
984
985 /* no qmi_proxy_loop lives, create one */
986 pthread_create(&thread_id, NULL, qmi_proxy_loop, NULL);
987 /* try to redo init if failed, init function must be successfully */
988 while (qmi_proxy_init() != 0) {
989 if (retry_times < 5) {
990 dprintf("fail to init proxy, try again in 2 seconds.\n");
991 sleep(2);
992 retry_times++;
993 } else {
994 dprintf("has failed too much times, restart the modem and have a try...\n");
995 break;
996 }
997 /* break loop if modem is detached */
998 if (access(cdc_wdm, F_OK|R_OK|W_OK))
999 break;
1000 }
1001 retry_times = 0;
1002 qmi_start_server(servername);
1003 if (qmi_proxy_server_fd == -1)
1004 pthread_cancel(thread_id);
1005 pthread_join(thread_id, NULL);
1006
1007 /* close local server at last */
1008 qmi_close_server(servername);
1009 close(cdc_wdm_fd);
1010 /* DO RESTART IN 20s IF MODEM RESET ITSELF */
1011 if (modem_reset_flag) {
1012 unsigned int time_to_wait = 20;
1013 while (time_to_wait) {
1014 time_to_wait = sleep(time_to_wait);
1015 }
1016 modem_reset_flag = 0;
1017 }
1018 }
1019
1020 return 0;
1021 }
1022