main.c

   1 #include <stdio.h>
   2 #include <getopt.h>
   3 #include <stdbool.h>
   4 #include <ctype.h>
   5
   6 #include <libubox/blobmsg_json.h>
   7 #include <libubox/avl.h>
   8 #include <libubox/avl-cmp.h>
   9 #include "switch.h"
  10
  11 #define DEFAULT_CONFIG "/etc/usb-mode.json"
  12
  13 struct device {
  14         struct avl_node avl;
  15         struct blob_attr *data;
  16 };
  17
  18 static int verbose = 0;
  19 static const char *config_file = DEFAULT_CONFIG;
  20 static struct blob_buf conf;
  21
  22 char **messages = NULL;
  23 int *message_len;
  24 int n_messages = 0;
  25
  26 static struct avl_tree devices;
  27
  28 struct libusb_context *usb;
  29 static struct libusb_device **usbdevs;
  30 static int n_usbdevs;
  31
  32 static int hex2num(char c)
  33 {
  34         if (c >= '0' && c <= '9')
  35                 return c - '0';
  36
  37         c = toupper(c);
  38         if (c >= 'A' && c <= 'F')
  39                 return c - 'A' + 10;
  40
  41         return -1;
  42 }
  43
  44 static int hex2byte(const char *hex)
  45 {
  46         int a, b;
  47
  48         a = hex2num(*hex++);
  49         if (a < 0)
  50                 return -1;
  51
  52         b = hex2num(*hex++);
  53         if (b < 0)
  54                 return -1;
  55
  56         return (a << 4) | b;
  57 }
  58
  59 static int hexstr2bin(const char *hex, char *buffer, int len)
  60 {
  61         const char *ipos = hex;
  62         char *opos = buffer;
  63         int i, a;
  64
  65         for (i = 0; i < len; i++) {
  66                 a = hex2byte(ipos);
  67                 if (a < 0)
  68                         return -1;
  69
  70                 *opos++ = a;
  71                 ipos += 2;
  72         }
  73
  74         return 0;
  75 }
  76
  77 static int convert_message(struct blob_attr *attr)
  78 {
  79         char *data;
  80         int len;
  81
  82         data = blobmsg_data(attr);
  83         len = strlen(data);
  84         if (len % 2)
  85                 return -1;
  86
  87         if (hexstr2bin(data, data, len / 2))
  88                 return -1;
  89
  90         return len / 2;
  91 }
  92
  93 static int parse_config(void)
  94 {
  95         enum {
  96                 CONF_MESSAGES,
  97                 CONF_DEVICES,
  98                 __CONF_MAX
  99         };
 100         static const struct blobmsg_policy policy[__CONF_MAX] = {
 101                 [CONF_MESSAGES] = { .name = "messages", .type = BLOBMSG_TYPE_ARRAY },
 102                 [CONF_DEVICES] = { .name = "devices", .type = BLOBMSG_TYPE_TABLE },
 103         };
 104         struct blob_attr *tb[__CONF_MAX];
 105         struct blob_attr *cur;
 106         struct device *dev;
 107         int rem;
 108
 109         blobmsg_parse(policy, __CONF_MAX, tb, blob_data(conf.head), blob_len(conf.head));
 110         if (!tb[CONF_MESSAGES] || !tb[CONF_DEVICES]) {
 111                 fprintf(stderr, "Configuration incomplete\n");
 112                 return -1;
 113         }
 114
 115         blobmsg_for_each_attr(cur, tb[CONF_MESSAGES], rem)
 116                 n_messages++;
 117
 118         messages = calloc(n_messages, sizeof(*messages));
 119         message_len = calloc(n_messages, sizeof(*message_len));
 120         n_messages = 0;
 121         blobmsg_for_each_attr(cur, tb[CONF_MESSAGES], rem) {
 122                 int len = convert_message(cur);
 123
 124                 if (len < 0) {
 125                         fprintf(stderr, "Invalid data in message %d\n", n_messages);
 126                         return -1;
 127                 }
 128
 129                 message_len[n_messages] = len;
 130                 messages[n_messages++] = blobmsg_data(cur);
 131         }
 132
 133         blobmsg_for_each_attr(cur, tb[CONF_DEVICES], rem) {
 134             dev = calloc(1, sizeof(*dev));
 135             dev->avl.key = blobmsg_name(cur);
 136             dev->data = cur;
 137             avl_insert(&devices, &dev->avl);
 138         }
 139
 140         return 0;
 141 }
 142
 143 static int usage(const char *prog)
 144 {
 145         fprintf(stderr, "Usage: %s <command> <options>\n"
 146                 "Commands:\n"
 147                 "       -l              List matching devices\n"
 148                 "       -s              Modeswitch matching devices\n"
 149                 "\n"
 150                 "Options:\n"
 151                 "       -v              Verbose output\n"
 152                 "       -c <file>       Set configuration file to <file> (default: %s)\n"
 153                 "\n", prog, DEFAULT_CONFIG);
 154         return 1;
 155 }
 156
 157 typedef void (*cmd_cb_t)(struct usbdev_data *data);
 158
 159 static struct blob_attr *
 160 find_dev_data(struct usbdev_data *data, struct device *dev)
 161 {
 162         struct blob_attr *cur;
 163         int rem;
 164
 165         blobmsg_for_each_attr(cur, dev->data, rem) {
 166                 const char *name = blobmsg_name(cur);
 167                 const char *next;
 168                 char *val;
 169
 170                 if (!strcmp(blobmsg_name(cur), "*"))
 171                         return cur;
 172
 173                 next = strchr(name, '=');
 174                 if (!next)
 175                         continue;
 176
 177                 next++;
 178                 if (!strncmp(name, "uMa", 3)) {
 179                         val = data->mfg;
 180                 } else if (!strncmp(name, "uPr", 3)) {
 181                         val = data->prod;
 182                 } else if (!strncmp(name, "uSe", 3)) {
 183                         val = data->serial;
 184                 } else {
 185                         /* ignore unsupported scsi attributes */
 186                         return cur;
 187                 }
 188
 189                 if (!strcmp(val, next))
 190                         return cur;
 191         }
 192
 193         return NULL;
 194 }
 195
 196 static void
 197 parse_interface_config(libusb_device *dev, struct usbdev_data *data)
 198 {
 199         struct libusb_config_descriptor *config;
 200         const struct libusb_interface *iface;
 201         const struct libusb_interface_descriptor *alt;
 202         int i;
 203
 204         data->interface = -1;
 205         if (libusb_get_config_descriptor(dev, 0, &config))
 206                 return;
 207
 208         data->config = config;
 209         if (!config->bNumInterfaces)
 210                 return;
 211
 212         iface = &config->interface[0];
 213         if (!iface->num_altsetting)
 214                 return;
 215
 216         alt = &iface->altsetting[0];
 217         data->interface = alt->bInterfaceNumber;
 218         data->dev_class = alt->bInterfaceClass;
 219
 220         for (i = 0; i < alt->bNumEndpoints; i++) {
 221                 const struct libusb_endpoint_descriptor *ep = &alt->endpoint[i];
 222                 bool out = false;
 223
 224                 if (data->msg_endpoint && data->response_endpoint)
 225                         break;
 226
 227                 if ((ep->bmAttributes & LIBUSB_TRANSFER_TYPE_MASK) !=
 228                     LIBUSB_TRANSFER_TYPE_BULK)
 229                         continue;
 230
 231                 out = (ep->bEndpointAddress & LIBUSB_ENDPOINT_DIR_MASK) ==
 232                       LIBUSB_ENDPOINT_OUT;
 233
 234                 if (!data->msg_endpoint && out)
 235                         data->msg_endpoint = ep->bEndpointAddress;
 236                 if (!data->response_endpoint && !out)
 237                         data->response_endpoint = ep->bEndpointAddress;
 238         }
 239 }
 240
 241 static void iterate_devs(cmd_cb_t cb)
 242 {
 243         struct usbdev_data data;
 244         struct device *dev;
 245         int i;
 246
 247         if (!cb)
 248                 return;
 249
 250         for (i = 0; i < n_usbdevs; i++) {
 251                 memset(&data, 0, sizeof(data));
 252
 253                 if (libusb_get_device_descriptor(usbdevs[i], &data.desc))
 254                         continue;
 255
 256                 sprintf(data.idstr, "%04x:%04x", data.desc.idVendor, data.desc.idProduct);
 257
 258                 dev = avl_find_element(&devices, data.idstr, dev, avl);
 259                 if (!dev)
 260                         continue;
 261
 262                 if (libusb_open(usbdevs[i], &data.devh))
 263                         continue;
 264
 265                 data.dev = usbdevs[i];
 266
 267                 libusb_get_string_descriptor_ascii(
 268                         data.devh, data.desc.iManufacturer,
 269                         (void *) data.mfg, sizeof(data.mfg));
 270                 libusb_get_string_descriptor_ascii(
 271                         data.devh, data.desc.iProduct,
 272                         (void *) data.prod, sizeof(data.prod));
 273                 libusb_get_string_descriptor_ascii(
 274                         data.devh, data.desc.iSerialNumber,
 275                         (void *) data.serial, sizeof(data.serial));
 276
 277                 parse_interface_config(usbdevs[i], &data);
 278
 279                 data.info = find_dev_data(&data, dev);
 280                 if (data.info)
 281                         cb(&data);
 282
 283                 if (data.config)
 284                         libusb_free_config_descriptor(data.config);
 285
 286                 if (data.devh)
 287                         libusb_close(data.devh);
 288         }
 289 }
 290
 291 static void handle_list(struct usbdev_data *data)
 292 {
 293         fprintf(stderr, "Found device: %s (Manufacturer: \"%s\", Product: \"%s\", Serial: \"%s\")\n",
 294                 data->idstr, data->mfg, data->prod, data->serial);
 295 }
 296
 297 int main(int argc, char **argv)
 298 {
 299         cmd_cb_t cb = NULL;
 300         int ret;
 301         int ch;
 302
 303         avl_init(&devices, avl_strcmp, false, NULL);
 304
 305         while ((ch = getopt(argc, argv, "lsc:v")) != -1) {
 306                 switch (ch) {
 307                 case 'l':
 308                         cb = handle_list;
 309                         break;
 310                 case 's':
 311                         cb = handle_switch;
 312                         break;
 313                 case 'c':
 314                         config_file = optarg;
 315                         break;
 316                 case 'v':
 317                         verbose++;
 318                         break;
 319                 default:
 320                         return usage(argv[0]);
 321                 }
 322         }
 323
 324         blob_buf_init(&conf, 0);
 325         if (!blobmsg_add_json_from_file(&conf, config_file) ||
 326             parse_config()) {
 327                 fprintf(stderr, "Failed to load config file\n");
 328                 return 1;
 329         }
 330
 331         ret = libusb_init(&usb);
 332         if (ret) {
 333                 fprintf(stderr, "Failed to initialize libusb: %s\n", libusb_error_name(ret));
 334                 return 1;
 335         }
 336
 337         n_usbdevs = libusb_get_device_list(usb, &usbdevs);
 338         iterate_devs(cb);
 339         libusb_free_device_list(usbdevs, 1);
 340         libusb_exit(usb);
 341
 342         return 0;
 343 }