ncd: modules: add sys.watch_usb()

blog_channels.txt

@@ -42,6 +42,7 @@ ncd_net_ipv4_dhcp 4
 ncd_net_ipv4_arp_probe 4
 ncd_net_watch_interfaces 4
 ncd_sys_watch_input 4
+ncd_sys_watch_usb 4
 ncd_sys_evdev 4
 ncd_sys_watch_directory 4
 StreamPeerIO 4

generated/blog_channel_ncd_sys_watch_usb.h

@@ -0,0 +1,4 @@
+#ifdef BLOG_CURRENT_CHANNEL
+#undef BLOG_CURRENT_CHANNEL
+#endif
+#define BLOG_CURRENT_CHANNEL BLOG_CHANNEL_ncd_sys_watch_usb

generated/blog_channels_defines.h

@@ -42,56 +42,57 @@
 #define BLOG_CHANNEL_ncd_net_ipv4_arp_probe 41
 #define BLOG_CHANNEL_ncd_net_watch_interfaces 42
 #define BLOG_CHANNEL_ncd_sys_watch_input 43
-#define BLOG_CHANNEL_ncd_sys_evdev 44
-#define BLOG_CHANNEL_ncd_sys_watch_directory 45
-#define BLOG_CHANNEL_StreamPeerIO 46
-#define BLOG_CHANNEL_DatagramPeerIO 47
-#define BLOG_CHANNEL_BReactor 48
-#define BLOG_CHANNEL_BSignal 49
-#define BLOG_CHANNEL_FragmentProtoAssembler 50
-#define BLOG_CHANNEL_BPredicate 51
-#define BLOG_CHANNEL_ServerConnection 52
-#define BLOG_CHANNEL_Listener 53
-#define BLOG_CHANNEL_DataProto 54
-#define BLOG_CHANNEL_FrameDecider 55
-#define BLOG_CHANNEL_BSocksClient 56
-#define BLOG_CHANNEL_BDHCPClientCore 57
-#define BLOG_CHANNEL_BDHCPClient 58
-#define BLOG_CHANNEL_NCDIfConfig 59
-#define BLOG_CHANNEL_BUnixSignal 60
-#define BLOG_CHANNEL_BProcess 61
-#define BLOG_CHANNEL_PRStreamSink 62
-#define BLOG_CHANNEL_PRStreamSource 63
-#define BLOG_CHANNEL_PacketProtoDecoder 64
-#define BLOG_CHANNEL_DPRelay 65
-#define BLOG_CHANNEL_BThreadWork 66
-#define BLOG_CHANNEL_DPReceive 67
-#define BLOG_CHANNEL_BInputProcess 68
-#define BLOG_CHANNEL_NCDUdevMonitorParser 69
-#define BLOG_CHANNEL_NCDUdevMonitor 70
-#define BLOG_CHANNEL_NCDUdevCache 71
-#define BLOG_CHANNEL_NCDUdevManager 72
-#define BLOG_CHANNEL_BTime 73
-#define BLOG_CHANNEL_BEncryption 74
-#define BLOG_CHANNEL_SPProtoDecoder 75
-#define BLOG_CHANNEL_LineBuffer 76
-#define BLOG_CHANNEL_BTap 77
-#define BLOG_CHANNEL_lwip 78
-#define BLOG_CHANNEL_NCDConfigTokenizer 79
-#define BLOG_CHANNEL_NCDConfigParser 80
-#define BLOG_CHANNEL_nsskey 81
-#define BLOG_CHANNEL_addr 82
-#define BLOG_CHANNEL_PasswordListener 83
-#define BLOG_CHANNEL_NCDInterfaceMonitor 84
-#define BLOG_CHANNEL_NCDRfkillMonitor 85
-#define BLOG_CHANNEL_udpgw 86
-#define BLOG_CHANNEL_UdpGwClient 87
-#define BLOG_CHANNEL_SocksUdpGwClient 88
-#define BLOG_CHANNEL_BNetwork 89
-#define BLOG_CHANNEL_BConnection 90
-#define BLOG_CHANNEL_BSSLConnection 91
-#define BLOG_CHANNEL_BDatagram 92
-#define BLOG_CHANNEL_PeerChat 93
-#define BLOG_CHANNEL_BArpProbe 94
-#define BLOG_CHANNEL_NCDModuleIndex 95
-#define BLOG_NUM_CHANNELS 96
+#define BLOG_CHANNEL_ncd_sys_watch_usb 44
+#define BLOG_CHANNEL_ncd_sys_evdev 45
+#define BLOG_CHANNEL_ncd_sys_watch_directory 46
+#define BLOG_CHANNEL_StreamPeerIO 47
+#define BLOG_CHANNEL_DatagramPeerIO 48
+#define BLOG_CHANNEL_BReactor 49
+#define BLOG_CHANNEL_BSignal 50
+#define BLOG_CHANNEL_FragmentProtoAssembler 51
+#define BLOG_CHANNEL_BPredicate 52
+#define BLOG_CHANNEL_ServerConnection 53
+#define BLOG_CHANNEL_Listener 54
+#define BLOG_CHANNEL_DataProto 55
+#define BLOG_CHANNEL_FrameDecider 56
+#define BLOG_CHANNEL_BSocksClient 57
+#define BLOG_CHANNEL_BDHCPClientCore 58
+#define BLOG_CHANNEL_BDHCPClient 59
+#define BLOG_CHANNEL_NCDIfConfig 60
+#define BLOG_CHANNEL_BUnixSignal 61
+#define BLOG_CHANNEL_BProcess 62
+#define BLOG_CHANNEL_PRStreamSink 63
+#define BLOG_CHANNEL_PRStreamSource 64
+#define BLOG_CHANNEL_PacketProtoDecoder 65
+#define BLOG_CHANNEL_DPRelay 66
+#define BLOG_CHANNEL_BThreadWork 67
+#define BLOG_CHANNEL_DPReceive 68
+#define BLOG_CHANNEL_BInputProcess 69
+#define BLOG_CHANNEL_NCDUdevMonitorParser 70
+#define BLOG_CHANNEL_NCDUdevMonitor 71
+#define BLOG_CHANNEL_NCDUdevCache 72
+#define BLOG_CHANNEL_NCDUdevManager 73
+#define BLOG_CHANNEL_BTime 74
+#define BLOG_CHANNEL_BEncryption 75
+#define BLOG_CHANNEL_SPProtoDecoder 76
+#define BLOG_CHANNEL_LineBuffer 77
+#define BLOG_CHANNEL_BTap 78
+#define BLOG_CHANNEL_lwip 79
+#define BLOG_CHANNEL_NCDConfigTokenizer 80
+#define BLOG_CHANNEL_NCDConfigParser 81
+#define BLOG_CHANNEL_nsskey 82
+#define BLOG_CHANNEL_addr 83
+#define BLOG_CHANNEL_PasswordListener 84
+#define BLOG_CHANNEL_NCDInterfaceMonitor 85
+#define BLOG_CHANNEL_NCDRfkillMonitor 86
+#define BLOG_CHANNEL_udpgw 87
+#define BLOG_CHANNEL_UdpGwClient 88
+#define BLOG_CHANNEL_SocksUdpGwClient 89
+#define BLOG_CHANNEL_BNetwork 90
+#define BLOG_CHANNEL_BConnection 91
+#define BLOG_CHANNEL_BSSLConnection 92
+#define BLOG_CHANNEL_BDatagram 93
+#define BLOG_CHANNEL_PeerChat 94
+#define BLOG_CHANNEL_BArpProbe 95
+#define BLOG_CHANNEL_NCDModuleIndex 96
+#define BLOG_NUM_CHANNELS 97

generated/blog_channels_list.h

@@ -42,6 +42,7 @@
 {.name = "ncd_net_ipv4_arp_probe", .loglevel = 4},
 {.name = "ncd_net_watch_interfaces", .loglevel = 4},
 {.name = "ncd_sys_watch_input", .loglevel = 4},
+{.name = "ncd_sys_watch_usb", .loglevel = 4},
 {.name = "ncd_sys_evdev", .loglevel = 4},
 {.name = "ncd_sys_watch_directory", .loglevel = 4},
 {.name = "StreamPeerIO", .loglevel = 4},

ncd/CMakeLists.txt

@@ -76,6 +76,7 @@ add_executable(badvpn-ncd
     modules/net_ipv4_arp_probe.c
     modules/net_watch_interfaces.c
     modules/sys_watch_input.c
+    modules/sys_watch_usb.c
     ${NCD_ADDITIONAL_SOURCES}
 )
 target_link_libraries(badvpn-ncd system flow flowextra dhcpclient arpprobe ncdconfig udevmonitor)

ncd/modules/modules.h

@@ -68,6 +68,7 @@ extern const struct NCDModuleGroup ncdmodule_net_ipv4_dhcp;
 extern const struct NCDModuleGroup ncdmodule_net_ipv4_arp_probe;
 extern const struct NCDModuleGroup ncdmodule_net_watch_interfaces;
 extern const struct NCDModuleGroup ncdmodule_sys_watch_input;
+extern const struct NCDModuleGroup ncdmodule_sys_watch_usb;
 #ifdef BADVPN_USE_LINUX_INPUT
 extern const struct NCDModuleGroup ncdmodule_sys_evdev;
 #endif
@@ -117,6 +118,7 @@ static const struct NCDModuleGroup *ncd_modules[] = {
     &ncdmodule_net_ipv4_arp_probe,
     &ncdmodule_net_watch_interfaces,
     &ncdmodule_sys_watch_input,
+    &ncdmodule_sys_watch_usb,
 #ifdef BADVPN_USE_LINUX_INPUT
     &ncdmodule_sys_evdev,
 #endif

ncd/modules/sys_watch_usb.c

@@ -0,0 +1,451 @@
+/**
+ * @file sys_watch_usb.c
+ * @author Ambroz Bizjak <ambrop7@gmail.com>
+ * 
+ * @section LICENSE
+ * 
+ * This file is part of BadVPN.
+ * 
+ * BadVPN is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.
+ * 
+ * BadVPN is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ * 
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ * 
+ * @section DESCRIPTION
+ * 
+ * USB device watcher.
+ * 
+ * Synopsis: sys.watch_usb()
+ * Description: reports USB device events. Transitions up when an event is detected, and
+ *   goes down waiting for the next event when ->nextevent() is called.
+ *   On startup, "added" events are reported for existing USB devices.
+ * 
+ * Variables:
+ *   string event_type - what happened with the USB device: "added" or "removed"
+ *   string devname - device node path, e.g. /dev/bus/usb/XXX/YYY
+ *   string vendor_id - vendor ID, e.g. 046d
+ *   string model_id - model ID, e.g. c03e
+ *   
+ * Synopsis: sys.watch_usb::nextevent()
+ * Description: makes the watch_usb module transition down in order to report the next event.
+ */
+
+#include <inttypes.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdio.h>
+
+#include <misc/debug.h>
+#include <misc/offset.h>
+#include <misc/parse_number.h>
+#include <structure/LinkedList1.h>
+#include <udevmonitor/NCDUdevManager.h>
+#include <ncd/NCDModule.h>
+#include <ncd/modules/event_template.h>
+
+#include <generated/blog_channel_ncd_sys_watch_usb.h>
+
+#define ModuleLog(i, ...) NCDModuleInst_Backend_Log((i), BLOG_CURRENT_CHANNEL, __VA_ARGS__)
+
+struct device {
+    char *devname;
+    char *devpath;
+    uint16_t vendor_id;
+    uint16_t model_id;
+    BStringMap removed_map;
+    LinkedList1Node devices_list_node;
+};
+
+struct instance {
+    NCDModuleInst *i;
+    NCDUdevClient client;
+    LinkedList1 devices_list;
+    event_template templ;
+};
+
+struct nextevent_instance {
+    NCDModuleInst *i;
+};
+
+static void templ_func_free (struct instance *o);
+
+static struct device * find_device_by_devname (struct instance *o, const char *devname)
+{
+    for (LinkedList1Node *list_node = LinkedList1_GetFirst(&o->devices_list); list_node; list_node = LinkedList1Node_Next(list_node)) {
+        struct device *device = UPPER_OBJECT(list_node, struct device, devices_list_node);
+        if (!strcmp(device->devname, devname)) {
+            return device;
+        }
+    }
+    
+    return NULL;
+}
+
+static struct device * find_device_by_devpath (struct instance *o, const char *devpath)
+{
+    for (LinkedList1Node *list_node = LinkedList1_GetFirst(&o->devices_list); list_node; list_node = LinkedList1Node_Next(list_node)) {
+        struct device *device = UPPER_OBJECT(list_node, struct device, devices_list_node);
+        if (!strcmp(device->devpath, devpath)) {
+            return device;
+        }
+    }
+    
+    return NULL;
+}
+
+static void free_device (struct instance *o, struct device *device, int have_removed_map)
+{
+    // remove from devices list
+    LinkedList1_Remove(&o->devices_list, &device->devices_list_node);
+    
+    // free removed map
+    if (have_removed_map) {
+        BStringMap_Free(&device->removed_map);
+    }
+    
+    // free devpath
+    free(device->devpath);
+    
+    // free devname
+    free(device->devname);
+    
+    // free structure
+    free(device);
+}
+
+static int make_event_map (struct instance *o, int added, const char *devname, uint16_t vendor_id, uint16_t model_id, BStringMap *out_map)
+{
+    // init map
+    BStringMap map;
+    BStringMap_Init(&map);
+    
+    // set type
+    if (!BStringMap_Set(&map, "event_type", (added ? "added" : "removed"))) {
+        ModuleLog(o->i, BLOG_ERROR, "BStringMap_Set failed");
+        goto fail1;
+    }
+    
+    // set devname
+    if (!BStringMap_Set(&map, "devname", devname)) {
+        ModuleLog(o->i, BLOG_ERROR, "BStringMap_Set failed");
+        goto fail1;
+    }
+    
+    // set vendor ID
+    char vendor_id_str[5];
+    sprintf(vendor_id_str, "%04"PRIx16, vendor_id);
+    if (!BStringMap_Set(&map, "vendor_id", vendor_id_str)) {
+        ModuleLog(o->i, BLOG_ERROR, "BStringMap_Set failed");
+        goto fail1;
+    }
+    
+    // set model ID
+    char model_id_str[5];
+    sprintf(model_id_str, "%04"PRIx16, model_id);
+    if (!BStringMap_Set(&map, "model_id", model_id_str)) {
+        ModuleLog(o->i, BLOG_ERROR, "BStringMap_Set failed");
+        goto fail1;
+    }
+    
+    *out_map = map;
+    return 1;
+    
+fail1:
+    BStringMap_Free(&map);
+    return 0;
+}
+
+static void queue_event (struct instance *o, BStringMap map)
+{
+    // pass event to template
+    int was_empty;
+    event_template_queue(&o->templ, map, &was_empty);
+    
+    // if event queue was empty, stop receiving udev events
+    if (was_empty) {
+        NCDUdevClient_Pause(&o->client);
+    }
+}
+
+static void add_device (struct instance *o, const char *devname, const char *devpath, uint16_t vendor_id, uint16_t model_id)
+{
+    ASSERT(!find_device_by_devname(o, devname))
+    ASSERT(!find_device_by_devpath(o, devpath))
+    
+    // allocate structure
+    struct device *device = malloc(sizeof(*device));
+    if (!device) {
+        ModuleLog(o->i, BLOG_ERROR, "malloc failed");
+        goto fail0;
+    }
+    
+    // init devname
+    if (!(device->devname = strdup(devname))) {
+        ModuleLog(o->i, BLOG_ERROR, "strdup failed");
+        goto fail1;
+    }
+    
+    // init devpath
+    if (!(device->devpath = strdup(devpath))) {
+        ModuleLog(o->i, BLOG_ERROR, "strdup failed");
+        goto fail2;
+    }
+    
+    // set vendor and model ID
+    device->vendor_id = vendor_id;
+    device->model_id = model_id;
+    
+    // init removed map
+    if (!make_event_map(o, 0, devname, vendor_id, model_id, &device->removed_map)) {
+        ModuleLog(o->i, BLOG_ERROR, "make_event_map failed");
+        goto fail3;
+    }
+    
+    // init added map
+    BStringMap added_map;
+    if (!make_event_map(o, 1, devname, vendor_id, model_id, &added_map)) {
+        ModuleLog(o->i, BLOG_ERROR, "make_event_map failed");
+        goto fail4;
+    }
+    
+    // insert to devices list
+    LinkedList1_Append(&o->devices_list, &device->devices_list_node);
+    
+    // queue event
+    queue_event(o, added_map);
+    return;
+    
+fail4:
+    BStringMap_Free(&device->removed_map);
+fail3:
+    free(device->devpath);
+fail2:
+    free(device->devname);
+fail1:
+    free(device);
+fail0:
+    ModuleLog(o->i, BLOG_ERROR, "failed to add device %s", devname);
+}
+
+static void remove_device (struct instance *o, struct device *device)
+{
+    queue_event(o, device->removed_map);
+    free_device(o, device, 0);
+}
+
+static void next_event (struct instance *o)
+{
+    event_template_assert_enabled(&o->templ);
+    
+    // order template to finish the current event
+    int is_empty;
+    event_template_dequeue(&o->templ, &is_empty);
+    
+    // if template has no events, continue udev events
+    if (is_empty) {
+        NCDUdevClient_Continue(&o->client);
+    }
+}
+
+static void client_handler (struct instance *o, char *devpath, int have_map, BStringMap map)
+{
+    // lookup existing device with this devpath
+    struct device *ex_device = find_device_by_devpath(o, devpath);
+    // lookup cache entry
+    const BStringMap *cache_map = NCDUdevManager_Query(o->i->umanager, devpath);
+    
+    if (!cache_map) {
+        if (ex_device) {
+            remove_device(o, ex_device);
+        }
+        goto out;
+    }
+    
+    const char *subsystem = BStringMap_Get(cache_map, "SUBSYSTEM");
+    const char *devname = BStringMap_Get(cache_map, "DEVNAME");
+    const char *devtype = BStringMap_Get(cache_map, "DEVTYPE");
+    const char *vendor_id_str = BStringMap_Get(cache_map, "ID_VENDOR_ID");
+    const char *model_id_str = BStringMap_Get(cache_map, "ID_MODEL_ID");
+    
+    uintmax_t vendor_id;
+    uintmax_t model_id;
+    
+    if (!(subsystem && !strcmp(subsystem, "usb") &&
+          devname &&
+          devtype && !strcmp(devtype, "usb_device") &&
+          vendor_id_str && parse_unsigned_hex_integer(vendor_id_str, &vendor_id) &&
+          model_id_str && parse_unsigned_hex_integer(model_id_str, &model_id)
+    )) {
+        if (ex_device) {
+            remove_device(o, ex_device);
+        }
+        goto out;
+    }
+    
+    if (ex_device && (
+        strcmp(ex_device->devname, devname) ||
+        ex_device->vendor_id != vendor_id || ex_device->model_id != model_id
+    )) {
+        remove_device(o, ex_device);
+        ex_device = NULL;
+    }
+    
+    if (!ex_device) {
+        struct device *ex_devname_device = find_device_by_devname(o, devname);
+        if (ex_devname_device) {
+            remove_device(o, ex_devname_device);
+        }
+        
+        add_device(o, devname, devpath, vendor_id, model_id);
+    }
+    
+out:
+    free(devpath);
+    if (have_map) {
+        BStringMap_Free(&map);
+    }
+}
+
+static void func_new (NCDModuleInst *i)
+{
+    // allocate instance
+    struct instance *o = malloc(sizeof(*o));
+    if (!o) {
+        ModuleLog(i, BLOG_ERROR, "failed to allocate instance");
+        goto fail0;
+    }
+    NCDModuleInst_Backend_SetUser(i, o);
+    
+    // init arguments
+    o->i = i;
+    
+    // check arguments
+    if (!NCDValue_ListRead(i->args, 0)) {
+        ModuleLog(o->i, BLOG_ERROR, "wrong arity");
+        goto fail1;
+    }
+    
+    // init client
+    NCDUdevClient_Init(&o->client, o->i->umanager, o, (NCDUdevClient_handler)client_handler);
+    
+    // init devices list
+    LinkedList1_Init(&o->devices_list);
+    
+    event_template_new(&o->templ, o->i, BLOG_CURRENT_CHANNEL, 3, o, (event_template_func_free)templ_func_free);
+    return;
+    
+fail1:
+    free(o);
+fail0:
+    NCDModuleInst_Backend_SetError(i);
+    NCDModuleInst_Backend_Dead(i);
+}
+
+static void templ_func_free (struct instance *o)
+{
+    NCDModuleInst *i = o->i;
+    
+    // free devices
+    while (!LinkedList1_IsEmpty(&o->devices_list)) {
+        struct device *device = UPPER_OBJECT(LinkedList1_GetFirst(&o->devices_list), struct device, devices_list_node);
+        free_device(o, device, 1);
+    }
+    
+    // free client
+    NCDUdevClient_Free(&o->client);
+    
+    // free instance
+    free(o);
+    
+    NCDModuleInst_Backend_Dead(i);
+}
+
+static void func_die (void *vo)
+{
+    struct instance *o = vo;
+    event_template_die(&o->templ);
+}
+
+static int func_getvar (void *vo, const char *name, NCDValue *out)
+{
+    struct instance *o = vo;
+    return event_template_getvar(&o->templ, name, out);
+}
+
+static void nextevent_func_new (NCDModuleInst *i)
+{
+    // allocate instance
+    struct nextevent_instance *o = malloc(sizeof(*o));
+    if (!o) {
+        ModuleLog(i, BLOG_ERROR, "failed to allocate instance");
+        goto fail0;
+    }
+    NCDModuleInst_Backend_SetUser(i, o);
+    
+    // init arguments
+    o->i = i;
+    
+    // check arguments
+    if (!NCDValue_ListRead(o->i->args, 0)) {
+        ModuleLog(o->i, BLOG_ERROR, "wrong arity");
+        goto fail1;
+    }
+    
+    // get method object
+    struct instance *mo = i->method_object->inst_user;
+    event_template_assert_enabled(&mo->templ);
+    
+    // signal up.
+    // Do it before finishing the event so our process does not advance any further if
+    // we would be killed the event provider going down.
+    NCDModuleInst_Backend_Up(o->i);
+    
+    // wait for next event
+    next_event(mo);
+    
+    return;
+    
+fail1:
+    free(o);
+fail0:
+    NCDModuleInst_Backend_SetError(i);
+    NCDModuleInst_Backend_Dead(i);
+}
+
+static void nextevent_func_die (void *vo)
+{
+    struct nextevent_instance *o = vo;
+    NCDModuleInst *i = o->i;
+    
+    // free instance
+    free(o);
+    
+    NCDModuleInst_Backend_Dead(i);
+}
+
+static const struct NCDModule modules[] = {
+    {
+        .type = "sys.watch_usb",
+        .func_new = func_new,
+        .func_die = func_die,
+        .func_getvar = func_getvar
+    }, {
+        .type = "sys.watch_usb::nextevent",
+        .func_new = nextevent_func_new,
+        .func_die = nextevent_func_die
+    }, {
+        .type = NULL
+    }
+};
+
+const struct NCDModuleGroup ncdmodule_sys_watch_usb = {
+    .modules = modules
+};