switch.c

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