release/src/router/hotplug2/linux24_compat/hotplug2-coldplug-2.4.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 <errno.h>
  16 #include <dirent.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
  27 #include "../hotplug2.h"
  28 #include "../mem_utils.h"
  29 #include "../parser_utils.h"
  30 #include "../filemap_utils.h"
  31 #include "../hotplug2_utils.h"
  32
  33 inline char *get_uevent_string(char **environ, unsigned long *uevent_string_len) {
  34         char *uevent_string;
  35         char *tmp;
  36         unsigned long offset;
  37
  38         *uevent_string_len = 4;
  39         offset = *uevent_string_len - 1;
  40         uevent_string = xmalloc(*uevent_string_len);
  41         strcpy(uevent_string, "add");
  42         uevent_string[offset] = '@';
  43         offset++;
  44
  45         for (; *environ != NULL; environ++) {
  46                 *uevent_string_len += strlen(*environ) + 1;
  47                 uevent_string = xrealloc(uevent_string, *uevent_string_len);
  48                 strcpy(&uevent_string[offset], *environ);
  49                 offset = *uevent_string_len;
  50         }
  51
  52         /* 64 + 7 ('SEQNUM=') + 1 ('\0') */
  53         tmp = xmalloc(72);
  54         memset(tmp, 0, 72);
  55         snprintf(tmp, 72, "SEQNUM=%llu", get_kernel_seqnum());
  56
  57         *uevent_string_len += strlen(tmp) + 1;
  58         uevent_string = xrealloc(uevent_string, *uevent_string_len);
  59         strcpy(&uevent_string[offset], tmp);
  60         offset = *uevent_string_len;
  61
  62         free(tmp);
  63
  64         return uevent_string;
  65 }
  66
  67 int dispatch_event(int netlink_socket, char **environ) {
  68         char *uevent_string;
  69         unsigned long uevent_string_len;
  70
  71         uevent_string = get_uevent_string(environ, &uevent_string_len);
  72         if (uevent_string == NULL) {
  73                 ERROR("dispatch_event", "Unable to create uevent string.");
  74                 return 1;
  75         }
  76
  77         if (send(netlink_socket, uevent_string, uevent_string_len, 0) == -1) {
  78                 ERROR("dispatch_event","send failed: %s.", strerror(errno));
  79                 free(uevent_string);
  80                 return 1;
  81         }
  82
  83         free(uevent_string);
  84
  85         return 0;
  86 }
  87
  88 char *join_to_fourhex(unsigned char hex0, unsigned char hex1) {
  89         char *tmp;
  90
  91         tmp = xmalloc(5);
  92         if (hex0 != 0) {
  93                 snprintf(tmp, 4, "%x%2x", hex0, hex1);
  94         } else {
  95                 snprintf(tmp, 4, "%x", hex1);
  96         }
  97
  98         return tmp;
  99 }
 100
 101 void generate_pci_events(int netlink_socket) {
 102         unsigned char pci_desc_info[256];
 103         char pci_dir_name[256];
 104         char pci_dev_name[512];
 105
 106         char **environ;
 107         int i;
 108
 109         DIR *procdir, *pcidir;
 110         FILE *item;
 111         struct dirent *cur_bus, *cur_device;
 112
 113         procdir = opendir("/proc/bus/pci");
 114         if (procdir == NULL) {
 115                 ERROR("pci coldplug", "Unable to open procfs PCI interface.");
 116                 return;
 117         }
 118
 119         while ((cur_bus = readdir(procdir)) != NULL) {
 120                 if (cur_bus->d_type != DT_DIR)
 121                         continue;
 122
 123                 if (cur_bus->d_name[0] == '.')
 124                         continue;
 125
 126                 memset(pci_dir_name, 0, 256);
 127                 snprintf(pci_dir_name, 255, "/proc/bus/pci/%s", cur_bus->d_name);
 128                 pcidir = opendir(pci_dir_name);
 129
 130                 while ((cur_device = readdir(pcidir)) != NULL) {
 131                         if (cur_device->d_type != DT_REG)
 132                                 continue;
 133
 134                         if (cur_device->d_name[0] == '.')
 135                                 continue;
 136
 137                         memset(pci_dev_name, 0, 256);
 138                         snprintf(pci_dev_name, 511, "/proc/bus/pci/%s/%s", cur_bus->d_name, cur_device->d_name);
 139                         item = fopen(pci_dev_name, "rb");
 140
 141                         if (item == NULL) {
 142                                 ERROR("pci coldplug", "Missing PCI entry.");
 143                                 continue;
 144                         }
 145
 146                         fread(pci_desc_info, 256, 1, item);
 147                         fclose(item);
 148
 149
 150                         environ = xmalloc(sizeof(char*) * 8);
 151                         environ[0] = NULL;
 152                         environ[1] = NULL;
 153                         environ[2] = NULL;
 154                         environ[3] = NULL;
 155                         environ[4] = NULL;
 156                         environ[5] = NULL;
 157                         environ[6] = NULL;
 158                         environ[7] = NULL;
 159
 160                         environ[0] = strdup("ACTION=add");
 161                         environ[1] = strdup("SUBSYSTEM=pci");
 162
 163                         environ[2] = xmalloc(21 + strlen(cur_bus->d_name) + strlen(cur_device->d_name));
 164                         strcpy(environ[2], "PCI_SLOT_NAME=0000:");
 165                         strcat(environ[2], cur_bus->d_name);
 166                         strcat(environ[2], ":");
 167                         strcat(environ[2], cur_device->d_name);
 168
 169                         environ[3] = xmalloc(17);
 170                         snprintf(environ[3], 17, "PCI_ID=%02X%02X:%02X%02X", pci_desc_info[1], pci_desc_info[0], pci_desc_info[3], pci_desc_info[2]);
 171
 172                         environ[4] = xmalloc(24);
 173                         snprintf(environ[4], 24, "PCI_SUBSYS_ID=%02X%02X:%02X%02X", pci_desc_info[45], pci_desc_info[44], pci_desc_info[47], pci_desc_info[46]);
 174
 175                         environ[5] = xmalloc(17);
 176                         snprintf(environ[5], 17, "PCI_CLASS=%x%02X%02X", pci_desc_info[11], pci_desc_info[10], pci_desc_info[9]);
 177
 178                         environ[6] = xmalloc(64);
 179                         snprintf(environ[6], 64, "MODALIAS=pci:v0000%02X%02Xd0000%02X%02Xsv0000%02X%02Xsd0000%02X%02Xbc%02Xsc%02Xi%02X",
 180                         pci_desc_info[1], pci_desc_info[0], pci_desc_info[3], pci_desc_info[2],
 181                         pci_desc_info[45], pci_desc_info[44], pci_desc_info[47], pci_desc_info[46],
 182                         pci_desc_info[11], pci_desc_info[10], pci_desc_info[9]);
 183
 184                         dispatch_event(netlink_socket, environ);
 185
 186                         for (i = 0; environ[i] != NULL; i++)
 187                                 free(environ[i]);
 188                         free(environ);
 189                 }
 190
 191                 closedir(pcidir);
 192         }
 193
 194         closedir(procdir);
 195 }
 196
 197 void generate_usb_events(int netlink_socket) {
 198         unsigned char usb_desc_info[52];
 199         char usb_dir_name[256];
 200         char usb_dev_name[512];
 201
 202         char **environ;
 203         char *t1, *t2, *t3;
 204         int i;
 205
 206         DIR *procdir, *usbdir;
 207         FILE *item;
 208         struct dirent *cur_bus, *cur_device;
 209
 210         procdir = opendir("/proc/bus/usb");
 211         if (procdir == NULL) {
 212                 ERROR("usb coldplug", "Unable to open procfs usb interface.");
 213                 return;
 214         }
 215
 216         while ((cur_bus = readdir(procdir)) != NULL) {
 217                 if (cur_bus->d_type != DT_DIR)
 218                         continue;
 219
 220                 if (cur_bus->d_name[0] == '.')
 221                         continue;
 222
 223                 memset(usb_dir_name, 0, 256);
 224                 snprintf(usb_dir_name, 255, "/proc/bus/usb/%s", cur_bus->d_name);
 225                 usbdir = opendir(usb_dir_name);
 226
 227                 while ((cur_device = readdir(usbdir)) != NULL) {
 228                         if (cur_device->d_type != DT_REG)
 229                                 continue;
 230
 231                         if (cur_device->d_name[0] == '.')
 232                                 continue;
 233
 234                         memset(usb_dev_name, 0, 256);
 235                         snprintf(usb_dev_name, 511, "/proc/bus/usb/%s/%s", cur_bus->d_name, cur_device->d_name);
 236                         item = fopen(usb_dev_name, "rb");
 237
 238                         if (item == NULL) {
 239                                 ERROR("usb coldplug", "Missing usb entry.");
 240                                 continue;
 241                         }
 242
 243                         fread(usb_desc_info, 52, 1, item);
 244                         fclose(item);
 245
 246                         environ = xmalloc(sizeof(char*) * 8);
 247                         environ[7] = NULL;
 248
 249                         environ[0] = strdup("ACTION=add");
 250                         environ[1] = strdup("SUBSYSTEM=usb");
 251
 252                         environ[2] = xmalloc(8 + strlen(usb_dev_name));
 253                         strcpy(environ[2], "DEVICE=");
 254                         strcat(environ[2], usb_dev_name);
 255
 256                         environ[3] = xmalloc(24);
 257                         memset(environ[3], 0, 24);
 258                         t1 = join_to_fourhex(usb_desc_info[9], usb_desc_info[8]);
 259                         t2 = join_to_fourhex(usb_desc_info[11], usb_desc_info[10]);
 260                         t3 = join_to_fourhex(usb_desc_info[13], usb_desc_info[12]);
 261
 262                         snprintf(environ[3], 23, "PRODUCT=%s/%s/%s", t1, t2, t3);
 263
 264                         free(t1); free(t2); free(t3);
 265
 266                         environ[4] = xmalloc(14);
 267                         memset(environ[4], 0, 14);
 268                         snprintf(environ[4], 13, "TYPE=%x/%x/%x", usb_desc_info[4], usb_desc_info[5], usb_desc_info[6]);
 269
 270                         environ[5] = xmalloc(55);
 271                         memset(environ[5], 0, 55);
 272
 273                         if (usb_desc_info[32] != 0x0 && usb_desc_info[33] != 0x0 && usb_desc_info[34] != 0x0) {
 274                                 snprintf(environ[5], 55, "MODALIAS=usb:v%02X%02Xp%02X%02Xd%02X%02Xdc%02Xdsc%02Xdp%02Xic%02Xisc%02Xip%02X",
 275                                 usb_desc_info[9], usb_desc_info[8], usb_desc_info[11], usb_desc_info[10], usb_desc_info[13], usb_desc_info[12],
 276                                 usb_desc_info[4], usb_desc_info[5], usb_desc_info[6],
 277                                 usb_desc_info[32], usb_desc_info[33], usb_desc_info[34]);
 278
 279                                 environ[6] = xmalloc(24);
 280                                 memset(environ[6], 0, 24);
 281                                 snprintf(environ[6], 24, "INTERFACE=%x/%x/%x", usb_desc_info[32], usb_desc_info[33], usb_desc_info[34]);
 282                         } else {
 283                                 snprintf(environ[5], 55, "MODALIAS=usb:v%02X%02Xp%02X%02Xd%02X%02Xdc%02Xdsc%02Xdp%02Xic*isc*ip*",
 284                                 usb_desc_info[9], usb_desc_info[8], usb_desc_info[11], usb_desc_info[10], usb_desc_info[13], usb_desc_info[12],
 285                                 usb_desc_info[4], usb_desc_info[5], usb_desc_info[6]);
 286
 287                                 environ[6] = NULL;
 288                         }
 289
 290                         dispatch_event(netlink_socket, environ);
 291
 292                         for (i = 0; environ[i] != NULL; i++)
 293                                 free(environ[i]);
 294                         free(environ);
 295                 }
 296
 297                 closedir(usbdir);
 298         }
 299
 300         closedir(procdir);
 301 }
 302
 303 void generate_isapnp_events(int netlink_socket) {
 304         struct filemap_t isapnpmap;
 305         char **environ;
 306         char *line, *nline, *nptr;
 307         char *token;
 308         char *device;
 309         int i;
 310
 311         if (map_file("/proc/bus/isapnp/devices", &isapnpmap)) {
 312                 ERROR("isapnp coldplug", "Unable to open procfs isapnp interface.");
 313                 return;
 314         }
 315
 316         nptr = isapnpmap.map;
 317
 318         while ((line = dup_line(nptr, &nptr)) != NULL) {
 319                 nline = line;
 320                 token = dup_token(nline, &nline, isspace);
 321                 if (!token)
 322                         continue;
 323                 free(token);
 324
 325                 token = dup_token(nline, &nline, isspace);
 326                 if (!token)
 327                         continue;
 328
 329                 if (strlen(token) < 14) {
 330                         free(token);
 331                         continue;
 332                 }
 333
 334                 environ = xmalloc(sizeof(char*) * 4);
 335                 environ[3] = NULL;
 336
 337                 environ[0] = strdup("ACTION=add");
 338                 environ[1] = strdup("SUBSYSTEM=pnp");
 339
 340                 environ[2] = xmalloc(14);
 341                 strcpy(environ[2], "MODALIAS=pnp:");
 342                 for (device = &token[7]; *device != '\0'; device += 7) {
 343                         environ[2] = xrealloc(environ[2], 14 + device - token);
 344                         memcpy(environ[2] + (device - token) + 6, device, 7);
 345                         environ[2][13 + device - token] = '\0';
 346                 }
 347
 348                 dispatch_event(netlink_socket, environ);
 349
 350                 for (i = 0; environ[i] != NULL; i++)
 351                         free(environ[i]);
 352                 free(environ);
 353
 354                 free(token);
 355                 free(line);
 356         }
 357
 358         unmap_file(&isapnpmap);
 359 }
 360
 361 int main(int argc, char *argv[], char **environ) {
 362         int netlink_socket;
 363
 364         netlink_socket = init_netlink_socket(NETLINK_CONNECT);
 365         if (netlink_socket == -1) {
 366                 ERROR("netlink init","Unable to open netlink socket.");
 367                 return 1;
 368         }
 369
 370         generate_pci_events(netlink_socket);
 371         generate_usb_events(netlink_socket);
 372         generate_isapnp_events(netlink_socket);
 373
 374         close(netlink_socket);
 375         return 0;
 376 }