support PantechMode
[project/usbmode.git] / switch.c
1 #include <unistd.h>
2 #include "switch.h"
3
4 enum {
5         DATA_MODE,
6         DATA_MODEVAL,
7         DATA_MSG,
8         DATA_INTERFACE,
9         DATA_MSG_EP,
10         DATA_RES_EP,
11         DATA_RESPONSE,
12         DATA_RELEASE_DELAY,
13         DATA_CONFIG,
14         DATA_ALT,
15         DATA_DEV_CLASS,
16         __DATA_MAX
17 };
18
19 static void detach_driver(struct usbdev_data *data)
20 {
21         libusb_detach_kernel_driver(data->devh, data->interface);
22 }
23
24 struct msg_entry {
25         char *data;
26         int len;
27 };
28
29 static int send_msg(struct usbdev_data *data, struct msg_entry *msg)
30 {
31         int transferred;
32
33         return libusb_bulk_transfer(data->devh, data->msg_endpoint,
34                                     (void *) msg->data, msg->len,
35                                     &transferred, 3000);
36 }
37
38 static int read_response(struct usbdev_data *data, int len)
39 {
40         unsigned char *buf;
41         int ret, transferred;
42
43         if (len < 13)
44                 len = 13;
45         buf = alloca(len);
46         ret = libusb_bulk_transfer(data->devh, data->response_endpoint,
47                                    buf, len, &transferred, 3000);
48         libusb_bulk_transfer(data->devh, data->response_endpoint,
49                              buf, 13, &transferred, 100);
50         return ret;
51 }
52
53 static void send_messages(struct usbdev_data *data, struct msg_entry *msg, int n_msg)
54 {
55         int i, len;
56
57         libusb_claim_interface(data->devh, data->interface);
58         libusb_clear_halt(data->devh, data->msg_endpoint);
59
60         for (i = 0; i < n_msg; i++) {
61                 if (send_msg(data, &msg[i])) {
62                         fprintf(stderr, "Failed to send switch message\n");
63                         continue;
64                 }
65
66                 if (!data->need_response)
67                         continue;
68
69                 if (!memcmp(msg[i].data, "\x55\x53\x42\x43", 4))
70                         len = 13;
71                 else
72                         len = msg[i].len;
73
74                 if (read_response(data, len))
75                         return;
76         }
77
78         libusb_clear_halt(data->devh, data->msg_endpoint);
79         libusb_clear_halt(data->devh, data->response_endpoint);
80
81         usleep(200000);
82
83         if (data->release_delay)
84                 usleep(data->release_delay * 1000);
85
86         libusb_release_interface(data->devh, data->interface);
87         return;
88 }
89
90 static void send_config_messages(struct usbdev_data *data, struct blob_attr *attr)
91 {
92         struct blob_attr *cur;
93         int rem, n_msg = 0;
94         struct msg_entry *msg;
95
96         blobmsg_for_each_attr(cur, attr, rem)
97                 n_msg++;
98
99         msg = alloca(n_msg * sizeof(*msg));
100         n_msg = 0;
101         blobmsg_for_each_attr(cur, attr, rem) {
102                 int msg_nr;
103
104                 if (blobmsg_type(cur) != BLOBMSG_TYPE_INT32) {
105                         fprintf(stderr, "Invalid data in message list\n");
106                         return;
107                 }
108
109                 msg_nr = blobmsg_get_u32(cur);
110                 if (msg_nr >= n_messages) {
111                         fprintf(stderr, "Message index out of range!\n");
112                         return;
113                 }
114
115                 msg[n_msg].data = messages[msg_nr];
116                 msg[n_msg++].len = message_len[msg_nr];
117         }
118
119         send_messages(data, msg, n_msg);
120 }
121
122 static void handle_generic(struct usbdev_data *data, struct blob_attr **tb)
123 {
124         detach_driver(data);
125         send_config_messages(data, tb[DATA_MSG]);
126 }
127
128 static void send_control_packet(struct usbdev_data *data, uint8_t type, uint8_t req,
129                                 uint16_t val, uint16_t idx, int len)
130 {
131         unsigned char *buffer = alloca(len ? len : 1);
132
133         libusb_control_transfer(data->devh, type, req, val, idx, buffer, len, 1000);
134 }
135
136 static void handle_huawei(struct usbdev_data *data, struct blob_attr **tb)
137 {
138         int type = LIBUSB_REQUEST_TYPE_STANDARD | LIBUSB_RECIPIENT_DEVICE;
139         send_control_packet(data, type, LIBUSB_REQUEST_SET_FEATURE, 1, 0, 0);
140 }
141
142 static void handle_huaweinew(struct usbdev_data *data, struct blob_attr **tb)
143 {
144         static struct msg_entry msgs[] = {
145                 {
146                         "\x55\x53\x42\x43\x12\x34\x56\x78\x00\x00\x00\x00\x00\x00\x00\x11"
147                         "\x06\x20\x00\x00\x01\x01\x00\x01\x00\x00\x00\x00\x00\x00\x00", 31
148                 }
149         };
150
151         detach_driver(data);
152         data->need_response = false;
153         send_messages(data, msgs, ARRAY_SIZE(msgs));
154 }
155
156 static void handle_option(struct usbdev_data *data, struct blob_attr **tb)
157 {
158         static struct msg_entry msgs[] = {
159                 {
160                         "\x55\x53\x42\x43\x12\x34\x56\x78\x00\x00\x00\x00\x00\x00\x06\x01"
161                         "\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", 31
162                 }
163         };
164
165         detach_driver(data);
166         data->need_response = false;
167         send_messages(data, msgs, ARRAY_SIZE(msgs));
168 }
169
170 static void handle_standardeject(struct usbdev_data *data, struct blob_attr **tb)
171 {
172         static struct msg_entry msgs[] = {
173                 {
174                         "\x55\x53\x42\x43\x12\x34\x56\x78\x00\x00\x00\x00\x00\x00\x06\x1e"
175                         "\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", 31
176                 }, {
177                         "\x55\x53\x42\x43\x12\x34\x56\x79\x00\x00\x00\x00\x00\x00\x06\x1b"
178                         "\x00\x00\x00\x02\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", 31
179                 }, {
180                         "\x55\x53\x42\x43\x12\x34\x56\x78\x00\x00\x00\x00\x00\x01\x06\x1e"
181                         "\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", 31
182                 }, {
183                         "\x55\x53\x42\x43\x12\x34\x56\x79\x00\x00\x00\x00\x00\x01\x06\x1b"
184                         "\x00\x00\x00\x02\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", 31
185                 }
186         };
187
188         detach_driver(data);
189         data->need_response = true;
190         send_messages(data, msgs, ARRAY_SIZE(msgs));
191 }
192
193 static void handle_sierra(struct usbdev_data *data, struct blob_attr **tb)
194 {
195         int type = LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE;
196         send_control_packet(data, type, LIBUSB_REQUEST_SET_INTERFACE, 1, 0, 0);
197 }
198
199 static void handle_sony(struct usbdev_data *data, struct blob_attr **tb)
200 {
201         int type = LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_ENDPOINT_IN;
202         int i;
203
204         detach_driver(data);
205         send_control_packet(data, type, 0x11, 2, 0, 3);
206
207         libusb_close(data->devh);
208         sleep(5);
209
210         for (i = 0; i < 25; i++) {
211                 data->devh = libusb_open_device_with_vid_pid(usb,
212                         data->desc.idVendor, data->desc.idProduct);
213                 if (data->devh)
214                         break;
215         }
216
217         send_control_packet(data, type, 0x11, 2, 0, 3);
218 }
219
220 static void handle_qisda(struct usbdev_data *data, struct blob_attr **tb)
221 {
222         static unsigned char buffer[] = "\x05\x8c\x04\x08\xa0\xee\x20\x00\x5c\x01\x04\x08\x98\xcd\xea\xbf";
223         int type = LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE;
224
225         libusb_control_transfer(data->devh, type, 0x04, 0, 0, buffer, 16, 1000);
226 }
227
228 static void handle_gct(struct usbdev_data *data, struct blob_attr **tb)
229 {
230         int type = LIBUSB_REQUEST_TYPE_CLASS | LIBUSB_RECIPIENT_INTERFACE | LIBUSB_ENDPOINT_IN;
231
232         detach_driver(data);
233
234         if (libusb_claim_interface(data->devh, data->interface))
235             return;
236
237         send_control_packet(data, type, 0xa0, 0, data->interface, 1);
238         send_control_packet(data, type, 0xfe, 0, data->interface, 1);
239
240         libusb_release_interface(data->devh, data->interface);
241 }
242
243 static void handle_kobil(struct usbdev_data *data, struct blob_attr **tb)
244 {
245         int type = LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE | LIBUSB_ENDPOINT_IN;
246
247         detach_driver(data);
248         send_control_packet(data, type, 0x88, 0, 0, 8);
249 }
250
251 static void handle_sequans(struct usbdev_data *data, struct blob_attr **tb)
252 {
253         int type = LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE;
254         send_control_packet(data, type, LIBUSB_REQUEST_SET_INTERFACE, 2, 0, 0);
255 }
256
257 static void mobile_action_interrupt_msg(struct usbdev_data *data, void *msg, int n_in)
258 {
259         unsigned char *buf = alloca(8);
260         int ep_out = 0x02, ep_in = 0x81;
261         int transferred;
262         int i;
263
264         if (msg)
265                 libusb_interrupt_transfer(data->devh, ep_out, msg, 8, &transferred, 1000);
266         for (i = 0; i < n_in; i++)
267                 libusb_interrupt_transfer(data->devh, ep_in, buf, 8, &transferred, 1000);
268 }
269
270 static void handle_mobile_action(struct usbdev_data *data, struct blob_attr **tb)
271 {
272         int type = LIBUSB_REQUEST_TYPE_CLASS | LIBUSB_RECIPIENT_INTERFACE;
273         char *msg[] = {
274                 "\xb0\x04\x00\x00\x02\x90\x26\x86",
275                 "\x37\x01\xfe\xdb\xc1\x33\x1f\x83",
276                 "\x37\x0e\xb5\x9d\x3b\x8a\x91\x51",
277                 "\x34\x87\xba\x0d\xfc\x8a\x91\x51",
278                 "\x37\x01\xfe\xdb\xc1\x33\x1f\x83",
279                 "\x37\x0e\xb5\x9d\x3b\x8a\x91\x51",
280                 "\x34\x87\xba\x0d\xfc\x8a\x91\x51",
281                 "\x33\x04\xfe\x00\xf4\x6c\x1f\xf0",
282                 "\x32\x07\xfe\xf0\x29\xb9\x3a\xf0"
283         };
284         int i;
285
286         for (i = 0; i < 2; i++)
287                 libusb_control_transfer(data->devh, type, 0x09, 0x0300, 0, (void *) msg[0], 8, 1000);
288         mobile_action_interrupt_msg(data, NULL, 2);
289         mobile_action_interrupt_msg(data, msg[1], 1);
290         mobile_action_interrupt_msg(data, msg[2], 1);
291         mobile_action_interrupt_msg(data, msg[3], 63);
292         mobile_action_interrupt_msg(data, msg[4], 1);
293         mobile_action_interrupt_msg(data, msg[5], 1);
294         mobile_action_interrupt_msg(data, msg[6], 73);
295         mobile_action_interrupt_msg(data, msg[7], 1);
296         mobile_action_interrupt_msg(data, msg[8], 1);
297 }
298
299 static void handle_cisco(struct usbdev_data *data, struct blob_attr **tb)
300 {
301         static struct msg_entry msgs[] = {
302                 {
303                         "\x55\x53\x42\x43\xf8\x3b\xcd\x81\x00\x02\x00\x00\x80\x00\x0a\xfd"
304                         "\x00\x00\x00\x03\x00\x00\x00\x01\x00\x00\x00\x00\x00\x00\x00", 31
305                 }, {
306                         "\x55\x53\x42\x43\x98\x43\x00\x82\x00\x02\x00\x00\x80\x00\x0a\xfd"
307                         "\x00\x00\x00\x07\x00\x00\x00\x01\x00\x00\x00\x00\x00\x00\x00", 31
308                 }, {
309                         "\x55\x53\x42\x43\x98\x43\x00\x82\x00\x00\x00\x00\x00\x00\x0a\xfd"
310                         "\x00\x01\x00\x07\x10\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", 31
311                 }, {
312                         "\x55\x53\x42\x43\x98\x43\x00\x82\x00\x02\x00\x00\x80\x00\x0a\xfd"
313                         "\x00\x02\x00\x23\x00\x00\x00\x01\x00\x00\x00\x00\x00\x00\x00", 31
314                 }, {
315                         "\x55\x53\x42\x43\x98\x43\x00\x82\x00\x00\x00\x00\x00\x00\x0a\xfd"
316                         "\x00\x03\x00\x23\x82\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", 31
317                 }, {
318                         "\x55\x53\x42\x43\x98\x43\x00\x82\x00\x02\x00\x00\x80\x00\x0a\xfd"
319                         "\x00\x02\x00\x26\x00\x00\x00\x01\x00\x00\x00\x00\x00\x00\x00", 31
320                 }, {
321                         "\x55\x53\x42\x43\x98\x43\x00\x82\x00\x00\x00\x00\x00\x00\x0a\xfd"
322                         "\x00\x03\x00\x26\xc8\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", 31
323                 }, {
324                         "\x55\x53\x42\x43\xd8\x4c\x04\x82\x00\x02\x00\x00\x80\x00\x0a\xfd"
325                         "\x00\x00\x10\x73\x00\x00\x00\x01\x00\x00\x00\x00\x00\x00\x00", 31
326                 }, {
327                         "\x55\x53\x42\x43\xd8\x4c\x04\x82\x00\x02\x00\x00\x80\x00\x0a\xfd"
328                         "\x00\x02\x00\x24\x00\x00\x00\x01\x00\x00\x00\x00\x00\x00\x00", 31
329                 }, {
330                         "\x55\x53\x42\x43\xd8\x4c\x04\x82\x00\x00\x00\x00\x00\x00\x0a\xfd"
331                         "\x00\x03\x00\x24\x13\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", 31
332                 }, {
333                         "\x55\x53\x42\x43\xd8\x4c\x04\x82\x00\x00\x00\x00\x00\x00\x0a\xfd"
334                         "\x00\x01\x10\x73\x24\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", 31
335                 }
336
337         };
338
339         detach_driver(data);
340         data->need_response = true;
341         send_messages(data, msgs, ARRAY_SIZE(msgs));
342 }
343
344 static void handle_mbim(struct usbdev_data *data, struct blob_attr **tb)
345 {
346         int j;
347
348         if (data->desc.bNumConfigurations < 2)
349                 return;
350
351         for (j = 0; j < data->desc.bNumConfigurations; j++) {
352                 struct libusb_config_descriptor *config;
353                 int i;
354
355                 libusb_get_config_descriptor(data->dev, j, &config);
356
357                 for (i = 0; i < config->bNumInterfaces; i++) {
358                         if (config->interface[i].altsetting[0].bInterfaceClass == 2) {
359                                 if (config->interface[i].altsetting[0].bInterfaceSubClass == 0x0e) {
360                                         struct libusb_config_descriptor *active;
361                                         int count = 5;
362
363                                         libusb_get_active_config_descriptor(data->dev, &active);
364                                         if (active->bConfigurationValue == config->bConfigurationValue)
365                                                 return;
366                                         while ((libusb_set_configuration(data->devh, config->bConfigurationValue) < 0) && --count)
367                                                 libusb_detach_kernel_driver(data->devh, active->interface[0].altsetting[0].bInterfaceNumber);
368
369                                         libusb_free_config_descriptor(config);
370                                         return;
371                                 }
372                         }
373                 }
374
375                 libusb_free_config_descriptor(config);
376         }
377 }
378
379 static void handle_quanta(struct usbdev_data *data, struct blob_attr **tb)
380 {
381         int type = LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE | LIBUSB_ENDPOINT_IN;
382
383         detach_driver(data);
384         send_control_packet(data, type, 0xff, 0, 0, 8);
385 }
386
387 static void handle_blackberry(struct usbdev_data *data, struct blob_attr **tb)
388 {
389         int type = LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE | LIBUSB_ENDPOINT_IN;
390
391         detach_driver(data);
392         send_control_packet(data, type, 0xb1, 0x0000, 0, 8);
393         send_control_packet(data, type, 0xa9, 0x000e, 0, 8);
394 }
395
396 static void handle_pantech(struct usbdev_data *data, struct blob_attr **tb)
397 {
398         int type = LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE | LIBUSB_ENDPOINT_OUT;
399         int val = 1;
400
401         if (tb[DATA_MODEVAL])
402                 val = blobmsg_get_u32(tb[DATA_MODEVAL]);
403         detach_driver(data);
404         if (val > 1)
405                 send_control_packet(data, type, 0x70, val, 0, 0);
406 }
407
408 static void set_alt_setting(struct usbdev_data *data, int setting)
409 {
410         if (libusb_claim_interface(data->devh, data->interface))
411                 return;
412
413         libusb_set_interface_alt_setting(data->devh, data->interface, setting);
414         libusb_release_interface(data->devh, data->interface);
415 }
416
417 enum {
418         MODE_GENERIC,
419         MODE_HUAWEI,
420         MODE_HUAWEINEW,
421         MODE_SIERRA,
422         MODE_STDEJECT,
423         MODE_SONY,
424         MODE_QISDA,
425         MODE_GCT,
426         MODE_KOBIL,
427         MODE_SEQUANS,
428         MODE_MOBILE_ACTION,
429         MODE_CISCO,
430         MODE_MBIM,
431         MODE_OPTION,
432         MODE_QUANTA,
433         MODE_BLACKBERRY,
434         MODE_PANTECH,
435         __MODE_MAX
436 };
437
438 static const struct {
439         const char *name;
440         void (*cb)(struct usbdev_data *data, struct blob_attr **tb);
441 } modeswitch_cb[__MODE_MAX] = {
442         [MODE_GENERIC] = { "Generic", handle_generic },
443         [MODE_STDEJECT] = { "StandardEject", handle_standardeject },
444         [MODE_HUAWEI] = { "Huawei", handle_huawei },
445         [MODE_HUAWEINEW] = { "HuaweiNew", handle_huaweinew },
446         [MODE_SIERRA] = { "Sierra", handle_sierra },
447         [MODE_SONY] = { "Sony", handle_sony },
448         [MODE_QISDA] = { "Qisda", handle_qisda },
449         [MODE_GCT] = { "GCT", handle_gct },
450         [MODE_KOBIL] = { "Kobil", handle_kobil },
451         [MODE_SEQUANS] = { "Sequans", handle_sequans },
452         [MODE_MOBILE_ACTION] = { "MobileAction", handle_mobile_action },
453         [MODE_CISCO] = { "Cisco", handle_cisco },
454         [MODE_MBIM] = { "MBIM", handle_mbim },
455         [MODE_OPTION] = { "Option", handle_option },
456         [MODE_QUANTA] = { "Quanta", handle_quanta },
457         [MODE_BLACKBERRY] = { "Blackberry", handle_blackberry },
458         [MODE_PANTECH] = { "Pantech", handle_pantech },
459 };
460
461 void handle_switch(struct usbdev_data *data)
462 {
463         static const struct blobmsg_policy data_policy[__DATA_MAX] = {
464                 [DATA_MODE] = { .name = "mode", .type = BLOBMSG_TYPE_STRING },
465                 [DATA_MODEVAL] = { .name = "modeval", .type = BLOBMSG_TYPE_INT32 },
466                 [DATA_MSG] = { .name = "msg", .type = BLOBMSG_TYPE_ARRAY },
467                 [DATA_INTERFACE] = { .name = "interface", .type = BLOBMSG_TYPE_INT32 },
468                 [DATA_MSG_EP] = { .name = "msg_endpoint", .type = BLOBMSG_TYPE_INT32 },
469                 [DATA_RES_EP] = { .name = "response_endpoint", .type = BLOBMSG_TYPE_INT32 },
470                 [DATA_RESPONSE] = { .name = "response", .type = BLOBMSG_TYPE_BOOL },
471                 [DATA_CONFIG] = { .name = "config", .type = BLOBMSG_TYPE_INT32 },
472                 [DATA_ALT] = { .name = "alt", .type = BLOBMSG_TYPE_INT32 },
473                 [DATA_DEV_CLASS] = { .name = "t_class", .type = BLOBMSG_TYPE_INT32 },
474         };
475         struct blob_attr *tb[__DATA_MAX];
476         int mode = MODE_GENERIC;
477         int t_class = 0;
478
479         blobmsg_parse(data_policy, __DATA_MAX, tb, blobmsg_data(data->info), blobmsg_data_len(data->info));
480
481         if (tb[DATA_DEV_CLASS])
482                 t_class = blobmsg_get_u32(tb[DATA_DEV_CLASS]);
483
484         if (tb[DATA_INTERFACE])
485                 data->interface = blobmsg_get_u32(tb[DATA_INTERFACE]);
486
487         if (tb[DATA_MSG_EP])
488                 data->msg_endpoint = blobmsg_get_u32(tb[DATA_MSG_EP]);
489
490         if (tb[DATA_RES_EP])
491                 data->response_endpoint = blobmsg_get_u32(tb[DATA_RES_EP]);
492
493         if (tb[DATA_RELEASE_DELAY])
494                 data->release_delay = blobmsg_get_u32(tb[DATA_RELEASE_DELAY]);
495
496         if (tb[DATA_RESPONSE])
497                 data->need_response = blobmsg_get_bool(tb[DATA_RESPONSE]);
498
499         if (t_class > 0 && data->dev_class != t_class)
500                 return;
501
502         if (tb[DATA_MODE]) {
503                 const char *modestr;
504                 int i;
505
506                 modestr = blobmsg_data(tb[DATA_MODE]);
507                 for (i = 0; i < __MODE_MAX; i++) {
508                         if (strcmp(modeswitch_cb[i].name, modestr) != 0)
509                                 continue;
510
511                         mode = i;
512                         break;
513                 }
514         }
515
516         modeswitch_cb[mode].cb(data, tb);
517
518         if (tb[DATA_CONFIG]) {
519                 int config, config_new;
520
521                 config_new = blobmsg_get_u32(tb[DATA_CONFIG]);
522                 if (libusb_get_configuration(data->devh, &config) ||
523                     config != config_new)
524                         libusb_set_configuration(data->devh, config_new);
525         }
526
527         if (tb[DATA_ALT]) {
528                 int new = blobmsg_get_u32(tb[DATA_ALT]);
529                 set_alt_setting(data, new);
530         }
531 }