release/src/router/hotplug2/hotplug2-dnode.c

   1 /*****************************************************************************\
   2 *  _  _       _          _              ___                                   *
   3 * | || | ___ | |_  _ __ | | _  _  __ _ |_  )                                  *
   4 * | __ |/ _ \|  _|| '_ \| || || |/ _` | / /                                   *
   5 * |_||_|\___/ \__|| .__/|_| \_,_|\__, |/___|                                  *
   6 *                 |_|            |___/                                        *
   7 \*****************************************************************************/
   8
   9 #define _GNU_SOURCE
  10
  11 #include <fcntl.h>
  12 #include <stdio.h>
  13 #include <unistd.h>
  14 #include <stdlib.h>
  15 #include <string.h>
  16 #include <errno.h>
  17 #include <ctype.h>
  18 #include <sys/socket.h>
  19 #include <sys/types.h>
  20 #include <sys/un.h>
  21 #include <sys/wait.h>
  22 #include <sys/stat.h>
  23 #include <sys/mman.h>
  24 #include <linux/types.h>
  25 #include <linux/netlink.h>
  26 #include <linux/input.h>
  27
  28 #include "mem_utils.h"
  29 #include "hotplug2.h"
  30 #include "hotplug2_utils.h"
  31 #include "parser_utils.h"
  32
  33 #define MODALIAS_MAX_LEN                1024
  34
  35 #ifndef KEY_MIN_INTERESTING
  36 #define KEY_MIN_INTERESTING             KEY_MUTE
  37 #endif
  38
  39 /* Some kernel headers appear to define it even without __KERNEL__ */
  40 #ifndef BITS_PER_LONG
  41 #define BITS_PER_LONG                   (sizeof(long) * 8)
  42 #endif
  43 #ifndef NBITS
  44 #define NBITS(x)                        ((x / BITS_PER_LONG) + 1)
  45 #endif
  46
  47 #define TEST_INPUT_BIT(i,bm)    (bm[i / BITS_PER_LONG] & (((unsigned long)1) << (i%BITS_PER_LONG)))
  48
  49 /**
  50  * Parses a bitmap; output is a list of offsets of bits of a bitmap
  51  * of arbitrary size that are set to 1.
  52  *
  53  * @1 Name of the bitmap parsed
  54  * @2 The actual bitmap pointer
  55  * @3 Lower boundary of the bitmap
  56  * @4 Upper boundary of the bitmap
  57  *
  58  * Returns: Newly allocated string containing the offsets
  59  */
  60 char *bitmap_to_bitstring(char name, unsigned long *bm, unsigned int min_bit, unsigned int max_bit)
  61 {
  62         char *rv;
  63         unsigned int i, len = 0, size = 16, srv;
  64
  65         rv = xmalloc(size);
  66
  67         len += snprintf(rv + len, size - len, "%c", name);
  68
  69         for (i = min_bit; i < max_bit; i++) {
  70                 if (TEST_INPUT_BIT(i, bm)) {
  71                         while ((srv = snprintf(rv + len, size - len, "%X,", i)) >= (size - len)) {
  72                                 size = size * 2;
  73                                 rv = xrealloc(rv, size);
  74                         }
  75                         len += srv;
  76                 }
  77         }
  78
  79         return rv;
  80 }
  81
  82 /**
  83  * Reverses the bitmap_to_bitstring function.
  84  *
  85  * @1 Bitstring to be converted
  86  * @2 Output bitmap
  87  * @3 Size of the whole bitmap
  88  *
  89  * Returns: void
  90  */
  91 void string_to_bitmap(char *input, unsigned long *bitmap, int bm_len) {
  92         char *token, *ptr;
  93         int i = 0;
  94
  95         ptr = input + strlen(input);
  96
  97         while ((token = dup_token_r(ptr, input, &ptr, isspace)) != NULL) {
  98                 bitmap[i] = strtoul(token, NULL, 16);
  99                 free(token);
 100                 i++;
 101         }
 102
 103         while (i < bm_len)
 104                 bitmap[i++] = 0;
 105 }
 106
 107 #define GET_BITMAP(mapkey, bitmap, name, min) \
 108         if (TEST_INPUT_BIT(EV_ ## mapkey, ev_bits)) { \
 109                 token = getenv(#mapkey); \
 110                 if (token == NULL) \
 111                         return -1; \
 112                 \
 113                 string_to_bitmap(token, bitmap ## _bits, NBITS(mapkey ## _MAX)); \
 114         } \
 115         bitmap = bitmap_to_bitstring(name, bitmap ## _bits, min, mapkey ## _MAX);
 116
 117 /**
 118  * Creates an input modalias out of preset environmental variables.
 119  *
 120  * @1 Pointer to where modalias will be created
 121  * @2 Maximum size of the modalias
 122  *
 123  * Returns: 0 if success, -1 otherwise
 124  */
 125 int get_input_modalias(char *modalias, int modalias_len) {
 126         char *product_env;
 127         char *ptr;
 128         char *token;
 129         unsigned int bustype, vendor, product, version;
 130
 131         char *ev, *key, *rel, *abs, *sw, *msc, *led, *snd, *ff;
 132
 133         unsigned long ev_bits[NBITS(EV_MAX)];
 134         unsigned long key_bits[NBITS(KEY_MAX)];
 135         unsigned long rel_bits[NBITS(REL_MAX)];
 136         unsigned long abs_bits[NBITS(ABS_MAX)];
 137         unsigned long msc_bits[NBITS(MSC_MAX)];
 138         unsigned long led_bits[NBITS(LED_MAX)];
 139         unsigned long snd_bits[NBITS(SND_MAX)];
 140         unsigned long ff_bits[NBITS(FF_MAX)];
 141
 142         #if defined(SW_MAX) && defined(EV_SW)
 143         unsigned long sw_bits[NBITS(SW_MAX)];
 144         #endif
 145
 146         memset(ev_bits, 0, NBITS(EV_MAX) * sizeof(long));
 147         memset(key_bits, 0, NBITS(KEY_MAX) * sizeof(long));
 148         memset(rel_bits, 0, NBITS(REL_MAX) * sizeof(long));
 149         memset(abs_bits, 0, NBITS(ABS_MAX) * sizeof(long));
 150         memset(msc_bits, 0, NBITS(MSC_MAX) * sizeof(long));
 151         memset(led_bits, 0, NBITS(LED_MAX) * sizeof(long));
 152         memset(snd_bits, 0, NBITS(SND_MAX) * sizeof(long));
 153         memset(ff_bits, 0, NBITS(FF_MAX) * sizeof(long));
 154         #if defined(SW_MAX) && defined(EV_SW)
 155         memset(sw_bits, 0, NBITS(SW_MAX) * sizeof(long));
 156         #endif
 157
 158         product_env = getenv("PRODUCT");
 159
 160         if (product_env == NULL)
 161                 return -1;
 162
 163         /* PRODUCT */
 164         ptr = strchr(product_env, '/');
 165         if (ptr == NULL || ptr[1] == '\0')
 166                 return -1;
 167
 168         bustype = strtoul(product_env, NULL, 16);
 169         vendor = strtoul(ptr+1, NULL, 16);
 170         ptr = strchr(ptr+1, '/');
 171         if (ptr == NULL || ptr[1] == '\0')
 172                 return -1;
 173
 174         product = strtoul(ptr+1, NULL, 16);
 175         ptr = strchr(ptr+1, '/');
 176         if (ptr == NULL || ptr[1] == '\0')
 177                 return -1;
 178
 179         version = strtoul(ptr+1, NULL, 16);
 180
 181         /* EV */
 182         token = getenv("EV");
 183         if (token == NULL)
 184                 return -1;
 185
 186         string_to_bitmap(token, ev_bits, NBITS(EV_MAX));
 187
 188         ev = bitmap_to_bitstring('e', ev_bits, 0, EV_MAX);
 189         GET_BITMAP(KEY, key, 'k', KEY_MIN_INTERESTING);
 190         GET_BITMAP(REL, rel, 'r', 0);
 191         GET_BITMAP(ABS, abs, 'a', 0);
 192         GET_BITMAP(MSC, msc, 'm', 0);
 193         GET_BITMAP(LED, led, 'l', 0);
 194         GET_BITMAP(SND, snd, 's', 0);
 195         GET_BITMAP(FF, ff, 'f', 0);
 196         #if defined(SW_MAX) && defined(EV_SW)
 197         GET_BITMAP(SW, sw, 'w', 0);
 198         #else
 199         sw=strdup("");
 200         #endif
 201
 202         snprintf(modalias, modalias_len,
 203                 "MODALIAS=input:b%04Xv%04Xp%04Xe%04X-"
 204                 "%s%s%s%s%s%s%s%s%s",
 205                 bustype, vendor, product, version,
 206                 ev, key, rel, abs, msc, led, snd, ff, sw);
 207
 208         /* Ugly but straightforward*/
 209         free(ev);
 210         free(key);
 211         free(rel);
 212         free(abs);
 213         free(msc);
 214         free(led);
 215         free(snd);
 216         free(ff);
 217         free(sw);
 218
 219         return 0;
 220 }
 221 #undef NBITS
 222 #undef TEST_INPUT_BIT
 223
 224 /**
 225  * Creates a PCI modalias out of preset environmental variables.
 226  *
 227  * @1 Pointer to where modalias will be created
 228  * @2 Maximum size of the modalias
 229  *
 230  * Returns: 0 if success, -1 otherwise
 231  */
 232 int get_pci_modalias(char *modalias, int modalias_len) {
 233         char *class_env, *id_env, *subsys_env;
 234         char *ptr;
 235         unsigned long vendor, device, sub_vendor, sub_device, class_type;
 236         unsigned char baseclass, subclass, interface;
 237
 238         id_env = getenv("PCI_ID");
 239         subsys_env = getenv("PCI_SUBSYS_ID");
 240         class_env = getenv("PCI_CLASS");
 241         if (id_env == NULL || subsys_env == NULL || class_env == NULL)
 242                 return -1;
 243
 244         if (strlen(id_env) < 9 || strlen(subsys_env) < 9)
 245                 return -1;
 246
 247         /* PCI_ID */
 248         ptr = strchr(id_env, ':');
 249         if (ptr == NULL || ptr[1] == '\0')
 250                 return -1;
 251
 252         vendor = strtoul(id_env, NULL, 16);
 253         device = strtoul(ptr+1, NULL, 16);
 254
 255         /* PCI_SUBSYS_ID */
 256         ptr = strchr(subsys_env, ':');
 257         if (ptr == NULL || ptr[1] == '\0')
 258                 return -1;
 259
 260         sub_vendor = strtoul(id_env, NULL, 16);
 261         sub_device = strtoul(ptr+1, NULL, 16);
 262
 263         /* PCI_CLASS */
 264         class_type = strtoul(class_env, NULL, 16);
 265         baseclass = (unsigned char)(class_type >> 16);
 266         subclass = (unsigned char)(class_type >> 8);
 267         interface = (unsigned char)class_type;
 268
 269         snprintf(modalias, modalias_len,
 270                 "MODALIAS=pci:v%08lXd%08lXsv%08lXsd%08lXbc%02Xsc%02Xi%02X",
 271                 vendor, device, sub_vendor, sub_device,
 272                 baseclass, subclass, interface);
 273
 274         return 0;
 275 }
 276
 277 /**
 278  * Creates an IEEE1394 (FireWire) modalias out of preset environmental
 279  * variables.
 280  *
 281  * @1 Pointer to where modalias will be created
 282  * @2 Maximum size of the modalias
 283  *
 284  * Returns: 0 if success, -1 otherwise
 285  */
 286 int get_ieee1394_modalias(char *modalias, int modalias_len) {
 287         char *vendor_env, *model_env;
 288         char *specifier_env, *version_env;
 289         unsigned long vendor, model;
 290         unsigned long specifier, version;
 291
 292         vendor_env = getenv("VENDOR_ID");
 293         model_env = getenv("MODEL_ID");
 294         specifier_env = getenv("SPECIFIER_ID");
 295         version_env = getenv("VERSION");
 296
 297         if (vendor_env == NULL || model_env == NULL  ||
 298                 specifier_env == NULL || version_env == NULL)
 299                 return -1;
 300
 301         vendor = strtoul(vendor_env, NULL, 16);
 302         model = strtoul(model_env, NULL, 16);
 303         specifier = strtoul(specifier_env, NULL, 16);
 304         version = strtoul(version_env, NULL, 16);
 305
 306         snprintf(modalias, modalias_len,
 307                 "MODALIAS=ieee1394:ven%08lXmo%08lXsp%08lXver%08lX",
 308                 vendor, model, specifier, version);
 309
 310         return 0;
 311 }
 312
 313 /**
 314  * Creates a serio modalias out of preset environmental variables.
 315  *
 316  * @1 Pointer to where modalias will be created
 317  * @2 Maximum size of the modalias
 318  *
 319  * Returns: 0 if success, -1 otherwise
 320  */
 321 int get_serio_modalias(char *modalias, int modalias_len) {
 322         char *serio_type_env, *serio_proto_env;
 323         char *serio_id_env, *serio_extra_env;
 324         unsigned int serio_type, serio_proto;
 325         unsigned int serio_specifier, serio_version;
 326
 327         serio_type_env = getenv("SERIO_TYPE");
 328         serio_proto_env = getenv("SERIO_PROTO");
 329         serio_id_env = getenv("SERIO_ID");
 330         serio_extra_env = getenv("SERIO_EXTRA");
 331
 332         if (serio_type_env == NULL || serio_proto_env == NULL  ||
 333                 serio_id_env == NULL || serio_extra_env == NULL)
 334                 return -1;
 335
 336         serio_type = strtoul(serio_type_env, NULL, 16);
 337         serio_proto = strtoul(serio_proto_env, NULL, 16);
 338         serio_specifier = strtoul(serio_id_env, NULL, 16);
 339         serio_version = strtoul(serio_extra_env, NULL, 16);
 340
 341         snprintf(modalias, modalias_len,
 342                 "MODALIAS=serio:ty%02Xpr%02Xid%02Xex%02X",
 343                 serio_type, serio_proto, serio_specifier, serio_version);
 344
 345         return 0;
 346 }
 347
 348 /**
 349  * Creates an USB modalias out of preset environmental variables.
 350  *
 351  * @1 Pointer to where modalias will be created
 352  * @2 Maximum size of the modalias
 353  *
 354  * Returns: 0 if success, -1 otherwise
 355  */
 356 int get_usb_modalias(char *modalias, int modalias_len) {
 357         char *product_env, *type_env, *interface_env;
 358         char *ptr;
 359         unsigned int idVendor, idProduct, bcdDevice;
 360         unsigned int device_class, device_subclass, device_protocol;
 361         unsigned int interface_class, interface_subclass, interface_protocol;
 362
 363         product_env = getenv("PRODUCT");
 364         type_env = getenv("TYPE");
 365         interface_env = getenv("INTERFACE");
 366
 367         if (product_env == NULL || type_env == NULL)
 368                 return -1;
 369
 370         /* PRODUCT */
 371         ptr = strchr(product_env, '/');
 372         if (ptr == NULL || ptr[1] == '\0')
 373                 return -1;
 374         idVendor = strtoul(product_env, NULL, 16);
 375         idProduct = strtoul(ptr+1, NULL, 16);
 376         ptr = strchr(ptr+1, '/');
 377         if (ptr == NULL || ptr[1] == '\0')
 378                 return -1;
 379         bcdDevice = strtoul(ptr+1, NULL, 16);
 380
 381         /* TYPE */
 382         ptr = strchr(type_env, '/');
 383         if (ptr == NULL || ptr[1] == '\0')
 384                 return -1;
 385         device_class = strtoul(type_env, NULL, 10);
 386         device_subclass = strtoul(ptr+1, NULL, 10);
 387         ptr = strchr(ptr+1, '/');
 388         if (ptr == NULL || ptr[1] == '\0')
 389                 return -1;
 390         device_protocol = strtoul(ptr+1, NULL, 10);
 391
 392         /* INTERFACE */
 393         if (interface_env != NULL) {
 394                 ptr = strchr(interface_env, '/');
 395                 if (ptr == NULL || ptr[1] == '\0')
 396                         return -1;
 397                 interface_class = strtoul(interface_env, NULL, 10);
 398                 interface_subclass = strtoul(ptr+1, NULL, 10);
 399                 ptr = strchr(ptr+1, '/');
 400                 if (ptr == NULL || ptr[1] == '\0')
 401                         return -1;
 402                 interface_protocol = strtoul(ptr+1, NULL, 10);
 403
 404                 snprintf(modalias, modalias_len,
 405                         "MODALIAS=usb:v%04Xp%04Xd%04X"
 406                         "dc%02Xdsc%02Xdp%02Xic%02Xisc%02Xip%02X",
 407                         idVendor, idProduct, bcdDevice,
 408                         device_class, device_subclass, device_protocol,
 409                         interface_class, interface_subclass, interface_protocol);
 410         } else {
 411                 snprintf(modalias, modalias_len,
 412                         "MODALIAS=usb:v%04Xp%04Xd%04X"
 413                         "dc%02Xdsc%02Xdp%02Xic*isc*ip*",
 414                         idVendor, idProduct, bcdDevice,
 415                         device_class, device_subclass, device_protocol);
 416         }
 417
 418         return 0;
 419 }
 420
 421 /**
 422  * Distributes modalias generating according to the bus name.
 423  *
 424  * @1 Bus name
 425  * @2 Pointer to where modalias will be created
 426  * @3 Maximum size of the modalias
 427  *
 428  * Returns: The return value of the subsystem modalias function, or -1 if
 429  * no match.
 430  */
 431 int get_modalias(char *bus, char *modalias, int modalias_len) {
 432         memset(modalias, 0, modalias_len);
 433
 434         if (!strcmp(bus, "pci"))
 435                 return get_pci_modalias(modalias, modalias_len);
 436
 437         if (!strcmp(bus, "usb"))
 438                 return get_usb_modalias(modalias, modalias_len);
 439
 440         if (!strcmp(bus, "ieee1394"))
 441                 return get_ieee1394_modalias(modalias, modalias_len);
 442
 443         if (!strcmp(bus, "serio"))
 444                 return get_serio_modalias(modalias, modalias_len);
 445
 446         if (!strcmp(bus, "input"))
 447                 return get_input_modalias(modalias, modalias_len);
 448
 449         /* 'ccw' devices do not generate events, we do not need to handle them */
 450         /* 'of' devices do not generate events either */
 451         /* 'pnp' devices do generate events, but they lack any device */
 452         /* description whatsoever. */
 453
 454         return -1;
 455 }
 456
 457 /**
 458  * Turns all environmental variables as set when invoked by /proc/sys/hotplug
 459  * into an uevent formatted (thus not null-terminated) string.
 460  *
 461  * @1 All environmental variables
 462  * @2 Bus of the event (as read from argv)
 463  * @3 Pointer to size of the returned uevent string
 464  *
 465  * Returns: Not null terminated uevent string.
 466  */
 467 inline char *get_uevent_string(char **environ, char *bus, unsigned long *uevent_string_len) {
 468         char *uevent_string;
 469         char *tmp;
 470         char modalias[MODALIAS_MAX_LEN];
 471         unsigned long offset;
 472
 473         tmp = getenv("ACTION");
 474         if (tmp == NULL)
 475                 return NULL;
 476
 477         *uevent_string_len = strlen(tmp) + 1;
 478         offset = *uevent_string_len - 1;
 479         uevent_string = xmalloc(*uevent_string_len);
 480         strcpy(uevent_string, tmp);
 481         uevent_string[offset] = '@';
 482         offset++;
 483
 484         for (; *environ != NULL; environ++) {
 485                 *uevent_string_len += strlen(*environ) + 1;
 486                 uevent_string = xrealloc(uevent_string, *uevent_string_len);
 487                 strcpy(&uevent_string[offset], *environ);
 488                 offset = *uevent_string_len;
 489         }
 490
 491         if (getenv("SEQNUM") == NULL) {
 492                 /* 64 + 7 ('SEQNUM=') + 1 ('\0') */
 493                 tmp = xmalloc(72);
 494                 memset(tmp, 0, 72);
 495                 snprintf(tmp, 72, "SEQNUM=%llu", get_kernel_seqnum());
 496
 497                 *uevent_string_len += strlen(tmp) + 1;
 498                 uevent_string = xrealloc(uevent_string, *uevent_string_len);
 499                 strcpy(&uevent_string[offset], tmp);
 500                 offset = *uevent_string_len;
 501
 502                 free(tmp);
 503         }
 504
 505         if (getenv("SUBSYSTEM") == NULL) {
 506                 /* 10 ('SUBSYSTEM=') + 1 ('\0') */
 507                 tmp = xmalloc(11 + strlen(bus));
 508                 strcpy(tmp, "SUBSYSTEM=");
 509                 strcat(tmp+10, bus);
 510
 511                 *uevent_string_len += strlen(tmp) + 1;
 512                 uevent_string = xrealloc(uevent_string, *uevent_string_len);
 513                 strcpy(&uevent_string[offset], tmp);
 514                 offset = *uevent_string_len;
 515
 516                 free(tmp);
 517         }
 518
 519         /* Only create our own MODALIAS if we do not have one set... */
 520         if (getenv("MODALIAS") == NULL) {
 521                 if (!get_modalias(bus, modalias, MODALIAS_MAX_LEN)) {
 522                         *uevent_string_len += strlen(modalias) + 1;
 523                         uevent_string = xrealloc(uevent_string, *uevent_string_len);
 524                         strcpy(&uevent_string[offset], modalias);
 525                         offset = *uevent_string_len;
 526                 }
 527         }
 528
 529         return uevent_string;
 530 }
 531
 532 int main(int argc, char *argv[], char **environ) {
 533         char *uevent_string;
 534         unsigned long uevent_string_len;
 535         int netlink_socket;
 536
 537         if (argv[1] == NULL) {
 538                 ERROR("parsing arguments", "Malformed event arguments (bus missing).");
 539                 return 1;
 540         }
 541
 542         uevent_string = get_uevent_string(environ, argv[1], &uevent_string_len);
 543         if (uevent_string == NULL) {
 544                 ERROR("parsing env vars", "Malformed event environmental variables.");
 545                 return 1;
 546         }
 547
 548         netlink_socket = init_netlink_socket(NETLINK_CONNECT);
 549         if (netlink_socket == -1) {
 550                 ERROR("netlink init","Unable to open netlink socket.");
 551                 goto exit;
 552         }
 553
 554         if (send(netlink_socket, uevent_string, uevent_string_len, 0) == -1) {
 555                 ERROR("sending data","send failed: %s.", strerror(errno));
 556         }
 557         close(netlink_socket);
 558
 559 exit:
 560         free(uevent_string);
 561
 562         return 0;
 563 }