1 #include <unistd.h>
2 #include <stdlib.h>
3 #include <stdio.h>
4 #include <string.h>
5 #include <errno.h>
6 #include <stdarg.h>
7 #include <stddef.h>
8 #include <fcntl.h>
9 #include <pthread.h>
10 #include <poll.h>
11 #include <sys/socket.h>
12 #include <sys/time.h>
13 #include <sys/ioctl.h>
14 #include <linux/un.h>
15 #include <linux/in.h>
16 #include <linux/if.h>
17 #include <dirent.h>
18 #include <signal.h>
19 #include <endian.h>
20 #include <inttypes.h>
21 #include <getopt.h>
22
23 #define QUECTEL_MBIM_PROXY "quectel-mbim-proxy"
24 #define safe_close(_fd) do { if (_fd > 0) { close(_fd); _fd = -1; } } while(0)
25
26 #define CM_MAX_CLIENT 32
27 #define TID_MASK (0xFFFFFF)
28 #define TID_SHIFT (24)
29
30 typedef enum {
31 MBIM_OPEN_MSG = 1,
32 MBIM_CLOSE_MSG = 2,
33 MBIM_OPEN_DONE = 0x80000001,
34 MBIM_CLOSE_DONE = 0x80000002,
35 } MBIM_MSG;
36
37 typedef struct {
38 unsigned int MessageType;
39 unsigned int MessageLength;
40 unsigned int TransactionId;
41 } MBIM_MESSAGE_HEADER;
42
43 typedef struct {
44 MBIM_MESSAGE_HEADER MessageHeader;
45 unsigned int MaxControlTransfer;
46 } MBIM_OPEN_MSG_T;
47
48 typedef struct {
49 MBIM_MESSAGE_HEADER MessageHeader;
50 unsigned int Status;
51 } MBIM_OPEN_DONE_T;
52
53 typedef struct {
54 int client_fd;
55 int client_idx;
56 } CM_CLIENT_T;
57
58 static unsigned char cm_recv_buffer[4096];
59 static CM_CLIENT_T cm_clients[CM_MAX_CLIENT];
60 static int verbose = 0;
61
get_time(void)62 const char * get_time(void) {
63 static char time_buf[128];
64 struct timeval tv;
65 time_t time;
66 suseconds_t millitm;
67 struct tm *ti;
68
69 gettimeofday (&tv, NULL);
70
71 time= tv.tv_sec;
72 millitm = (tv.tv_usec + 500) / 1000;
73
74 if (millitm == 1000) {
75 ++time;
76 millitm = 0;
77 }
78
79 ti = localtime(&time);
80 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);
81 return time_buf;
82 }
83
84 #define mbim_debug(fmt, args...) do { fprintf(stdout, "%s " fmt, get_time(), ##args); } while(0);
85
non_block_write(int fd,void * data,int len)86 static int non_block_write(int fd, void *data, int len)
87 {
88 int ret;
89 struct pollfd pollfd = {fd, POLLOUT, 0};
90 ret = poll(&pollfd, 1, 3000);
91
92 if (ret <= 0) {
93 mbim_debug("%s poll ret=%d, errno: %d(%s)\n", __func__, ret, errno, strerror(errno));
94 }
95
96 ret = write (fd, data, len);
97 if (ret != len)
98 mbim_debug("%s write ret=%d, errno: %d(%s)\n", __func__, ret, errno, strerror(errno));
99
100 return len;
101 }
102
mbim_send_open_msg(int mbim_dev_fd,uint32_t MaxControlTransfer)103 static int mbim_send_open_msg(int mbim_dev_fd, uint32_t MaxControlTransfer) {
104 MBIM_OPEN_MSG_T open_msg;
105 MBIM_OPEN_MSG_T *pRequest = &open_msg;
106
107 pRequest->MessageHeader.MessageType = (MBIM_OPEN_MSG);
108 pRequest->MessageHeader.MessageLength = (sizeof(MBIM_OPEN_MSG_T));
109 pRequest->MessageHeader.TransactionId = (1);
110 pRequest->MaxControlTransfer = (MaxControlTransfer);
111
112 mbim_debug("%s()\n", __func__);
113 return non_block_write(mbim_dev_fd, pRequest, sizeof(MBIM_OPEN_MSG_T));
114 }
115
116 /*
117 * parameter: proxy name
118 * return: local proxy server fd or -1
119 */
proxy_make_server(const char * proxy_name)120 static int proxy_make_server(const char *proxy_name)
121 {
122 int len, flag;
123 struct sockaddr_un sockaddr;
124 int mbim_server_fd;
125
126 mbim_server_fd = socket(AF_LOCAL, SOCK_STREAM, 0);
127 if (mbim_server_fd < 0) {
128 mbim_debug("socket failed: %s\n", strerror(errno));
129 return -1;
130 }
131 if (fcntl(mbim_server_fd, F_SETFL, fcntl(mbim_server_fd, F_GETFL) | O_NONBLOCK) < 0)
132 mbim_debug("fcntl set server(%d) NONBLOCK attribute failed: %s\n", mbim_server_fd, strerror(errno));
133
134 memset(&sockaddr, 0, sizeof(sockaddr));
135 sockaddr.sun_family = AF_LOCAL;
136 sockaddr.sun_path[0] = 0;
137 snprintf(sockaddr.sun_path, UNIX_PATH_MAX, "0%s", proxy_name);
138 sockaddr.sun_path[0] = '\0'; // string starts with leading '\0'
139 flag = 1;
140 if (setsockopt(mbim_server_fd, SOL_SOCKET, SO_REUSEADDR, &flag, sizeof(flag)) < 0) {
141 safe_close(mbim_server_fd);
142 mbim_debug("setsockopt failed\n");
143 }
144
145 len = strlen(proxy_name) + offsetof(struct sockaddr_un, sun_path) + 1;
146 if (bind(mbim_server_fd, (struct sockaddr*)&sockaddr, len) < 0) {
147 safe_close(mbim_server_fd);
148 mbim_debug("bind failed: %s\n", strerror(errno));
149 return -1;
150 }
151
152 listen(mbim_server_fd, 4);
153 return mbim_server_fd;
154 }
155
handle_client_connect(int server_fd)156 static int handle_client_connect(int server_fd)
157 {
158 int i, client_fd;
159 struct sockaddr_in cli_addr;
160 socklen_t len = sizeof(cli_addr);
161
162 client_fd = accept(server_fd, (struct sockaddr *)&cli_addr, &len);
163 if (client_fd < 0) {
164 mbim_debug("proxy accept failed: %s\n", strerror(errno));
165 return -1;
166 }
167
168 if (fcntl(client_fd, F_SETFL, fcntl(client_fd, F_GETFL) | O_NONBLOCK) < 0)
169 mbim_debug("fcntl set client(%d) NONBLOCK attribute failed: %s\n", client_fd, strerror(errno));
170
171 for (i = 0; i < CM_MAX_CLIENT; i++) {
172 if (cm_clients[i].client_fd <= 0) {
173 cm_clients[i].client_fd = client_fd;
174 cm_clients[i].client_idx= i+1;
175 mbim_debug("%s client_fd=%d, client_idx=%d\n", __func__, cm_clients[i].client_fd, cm_clients[i].client_idx);
176 return 0;
177 }
178 }
179
180 close(client_fd);
181 return -1;
182 }
183
handle_client_disconnect(int client_fd)184 static void handle_client_disconnect(int client_fd)
185 {
186 int i;
187
188 for (i = 0; i < CM_MAX_CLIENT; i++) {
189 if (cm_clients[i].client_fd == client_fd) {
190 mbim_debug("%s client_fd=%d, client_idx=%d\n", __func__, cm_clients[i].client_fd, cm_clients[i].client_idx);
191 safe_close(cm_clients[i].client_fd);
192 return;
193 }
194 }
195 }
196
handle_client_request(int mbim_dev_fd,int client_fd,void * pdata,int len)197 static int handle_client_request(int mbim_dev_fd, int client_fd, void *pdata, int len)
198 {
199 int i;
200 int client_idx = -1;
201 int ret;
202 MBIM_MESSAGE_HEADER *pRequest = (MBIM_MESSAGE_HEADER *)pdata;
203
204 for (i = 0; i < CM_MAX_CLIENT; i++) {
205 if (cm_clients[i].client_fd == client_fd) {
206 client_idx = cm_clients[i].client_idx;
207 break;
208 }
209 }
210
211 if (client_idx == -1) {
212 goto error;
213 }
214
215 /* transfer TransicationID to proxy transicationID and record in sender list */
216 pRequest->TransactionId = (pRequest->TransactionId & TID_MASK) + (client_idx << TID_SHIFT);
217 if (verbose) mbim_debug("REQ client_fd=%d, client_idx=%d, tid=%u\n", cm_clients[i].client_fd, cm_clients[i].client_idx, (pRequest->TransactionId & TID_MASK));
218 ret = non_block_write (mbim_dev_fd, pRequest, len);
219 if (ret == len)
220 return 0;
221
222 error:
223 return -1;
224 }
225
226 /*
227 * Will read message from device and transfer it to clients/client
228 * Notice:
229 * unsocial message will be send to all clients
230 */
handle_device_response(void * pdata,int len)231 static int handle_device_response(void *pdata, int len)
232 {
233 int i;
234 MBIM_MESSAGE_HEADER *pResponse = (MBIM_MESSAGE_HEADER *)pdata;
235
236 /* unsocial/function error message */
237 if (pResponse->TransactionId == 0) {
238 for (i = 0; i < CM_MAX_CLIENT; i++) {
239 if (cm_clients[i].client_fd > 0) {
240 non_block_write(cm_clients[i].client_fd, pResponse, len);
241 }
242 }
243 }
244 else {
245 /* try to find the sender */
246 int client_idx = (pResponse->TransactionId >> TID_SHIFT);
247
248 for (i = 0; i < CM_MAX_CLIENT; i++) {
249 if (cm_clients[i].client_idx == client_idx && cm_clients[i].client_fd > 0) {
250 pResponse->TransactionId &= TID_MASK;
251 if (verbose) mbim_debug("RSP client_fd=%d, client_idx=%d, tid=%u\n", cm_clients[i].client_fd, cm_clients[i].client_idx, (pResponse->TransactionId & TID_MASK));
252 non_block_write(cm_clients[i].client_fd, pResponse, len);
253 break;
254 }
255 }
256
257 if ( i == CM_MAX_CLIENT) {
258 mbim_debug("%s nobody care tid=%u\n", __func__, pResponse->TransactionId);
259 }
260 }
261
262 return 0;
263 }
264
proxy_loop(int mbim_dev_fd)265 static int proxy_loop(int mbim_dev_fd)
266 {
267 int i;
268 int mbim_server_fd = -1;
269
270 while (mbim_dev_fd > 0) {
271 struct pollfd pollfds[2+CM_MAX_CLIENT];
272 int ne, ret, nevents = 0;
273
274 pollfds[nevents].fd = mbim_dev_fd;
275 pollfds[nevents].events = POLLIN;
276 pollfds[nevents].revents= 0;
277 nevents++;
278
279 if (mbim_server_fd > 0) {
280 pollfds[nevents].fd = mbim_server_fd;
281 pollfds[nevents].events = POLLIN;
282 pollfds[nevents].revents= 0;
283 nevents++;
284
285 for (i = 0; i < CM_MAX_CLIENT; i++) {
286 if (cm_clients[i].client_fd > 0) {
287 pollfds[nevents].fd = cm_clients[i].client_fd;
288 pollfds[nevents].events = POLLIN;
289 pollfds[nevents].revents= 0;
290 nevents++;
291 }
292 }
293 }
294
295 ret = poll(pollfds, nevents, (mbim_server_fd > 0) ? -1 : (10*1000));
296 if (ret <= 0) {
297 goto error;
298 }
299
300 for (ne = 0; ne < nevents; ne++) {
301 int fd = pollfds[ne].fd;
302 short revents = pollfds[ne].revents;
303
304 if (revents & (POLLERR | POLLHUP | POLLNVAL)) {
305 mbim_debug("%s poll fd = %d, revents = %04x\n", __func__, fd, revents);
306 if (fd == mbim_dev_fd) {
307 goto error;
308 } else if(fd == mbim_server_fd) {
309
310 } else {
311 handle_client_disconnect(fd);
312 }
313 continue;
314 }
315
316 if (!(pollfds[ne].revents & POLLIN)) {
317 continue;
318 }
319
320 if (fd == mbim_server_fd) {
321 handle_client_connect(fd);
322 }
323 else {
324 int len = read(fd, cm_recv_buffer, sizeof(cm_recv_buffer));
325
326 if (len <= 0) {
327 mbim_debug("%s read fd=%d, len=%d, errno: %d(%s)\n", __func__, fd, len, errno, strerror(errno));
328 if (fd == mbim_dev_fd)
329 goto error;
330 else
331 handle_client_disconnect(fd);
332
333 return len;
334 }
335
336 if (fd == mbim_dev_fd) {
337 if (mbim_server_fd == -1) {
338 MBIM_OPEN_DONE_T *pOpenDone = (MBIM_OPEN_DONE_T *)cm_recv_buffer;
339
340 if (pOpenDone->MessageHeader.MessageType == MBIM_OPEN_DONE) {
341 mbim_debug("receive MBIM_OPEN_DONE, status=%d\n", pOpenDone->Status);
342 if (pOpenDone->Status)
343 goto error;
344 mbim_server_fd = proxy_make_server(QUECTEL_MBIM_PROXY);
345 mbim_debug("mbim_server_fd=%d\n", mbim_server_fd);
346 }
347 }
348 else {
349 handle_device_response(cm_recv_buffer, len);
350 }
351 }
352 else {
353 handle_client_request(mbim_dev_fd, fd, cm_recv_buffer, len);
354 }
355 }
356 }
357 }
358
359 error:
360 safe_close(mbim_server_fd);
361 for (i = 0; i < CM_MAX_CLIENT; i++) {
362 safe_close(cm_clients[i].client_fd);
363 }
364
365 mbim_debug("%s exit\n", __func__);
366 return 0;
367 }
368
369 /*
370 * How to use this proxy?
371 * 1. modprobe -a 8021q
372 * 2. Create network interface for channels:
373 * ip link add link wwan0 name wwan0.1 type vlan id 1
374 * ip link add link wwan0 name wwan0.2 type vlan id 2
375 * 3. Start './mbim-proxy' with -d 'device'
376 * 4. Start Clients: ./quectel-CM -n id1
377 * 5. Start Clients: ./quectel-CM -n id2
378 * ...
379 * Notice:
380 * mbim-proxy can work in backgroud as a daemon
381 * '-n' sessionID
382 * The modem may not support multi-PDN mode or how many PDN it supports is undefined. It depends!!!
383 * Besides, some modem also may not support some sessionID. For instance EC20 doesn't support SessionId 1...
384 */
main(int argc,char ** argv)385 int main(int argc, char **argv)
386 {
387 int optidx = 0;
388 int opt;
389 char *optstr = "d:vh";
390 const char *device = "/dev/cdc-wdm0";
391
392 struct option options[] = {
393 {"verbose", no_argument, NULL, 'v'},
394 {"device", required_argument, NULL, 'd'},
395 {0, 0, 0, 0},
396 };
397 while ((opt = getopt_long(argc, argv, optstr, options, &optidx)) != -1) {
398 switch (opt) {
399 case 'v':
400 verbose = 1;
401 break;
402 case 'd':
403 device = optarg;
404 break;
405 case 'h':
406 mbim_debug("-h Show this message\n");
407 mbim_debug("-v Verbose\n");
408 mbim_debug("-d [device] MBIM device\n");
409 return 0;
410 default:
411 mbim_debug("illegal argument\n");
412 return -1;
413 }
414 }
415
416 if (!device) {
417 mbim_debug("Missing parameter: device\n");
418 return -1;
419 }
420
421 while (1) {
422 int mbim_dev_fd = open(device, O_RDWR | O_NONBLOCK | O_NOCTTY);
423 if (mbim_dev_fd < 0) {
424 mbim_debug("cannot open mbim_device %s: %s\n", device, strerror(errno));
425 sleep(2);
426 continue;
427 }
428 mbim_debug ("mbim_dev_fd=%d\n", mbim_dev_fd);
429
430 memset(cm_clients, 0, sizeof(cm_clients));
431 mbim_send_open_msg(mbim_dev_fd, sizeof(cm_recv_buffer));
432 proxy_loop(mbim_dev_fd);
433 safe_close(mbim_dev_fd);
434 }
435
436 return -1;
437 }
438