main.c

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