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