add per-device configuration

adds input and [in, out] cap for classic mode
adds input cap for power mode
change wrapper/input, now gets useful device names
change (now dev specific) dpi to adjust sensitivity
change y sensitivity to y/x ratio
remove spaced LUTs

grapher and convert do not build

1 files changed, 250 insertions, 49 deletions

diff --git a/driver/driver.cpp b/driver/driver.cpp
index feace77..82a56c0 100644
--- a/driver/driver.cpp
+++ b/driver/driver.cpp
@@ -7,22 +7,37 @@
 
 #ifdef ALLOC_PRAGMA
 #pragma alloc_text (INIT, DriverEntry)
+#pragma alloc_text (INIT, RawaccelInit)
+#pragma alloc_text (INIT, CreateControlDevice)
 #pragma alloc_text (PAGE, EvtDeviceAdd)
 #pragma alloc_text (PAGE, EvtIoInternalDeviceControl)
 #pragma alloc_text (PAGE, RawaccelControl)
+#pragma alloc_text (PAGE, DeviceCleanup)
+#pragma alloc_text (PAGE, DeviceSetup)
+#pragma alloc_text (PAGE, WriteDelay)
 #endif
 
 using milliseconds = double;
 
 struct {
-    ra::settings args;
+    WDFCOLLECTION device_collection;
+    WDFWAITLOCK collection_lock;
+    ra::device_config default_dev_cfg;
+    unsigned device_data_size;
+    unsigned driver_data_size;
+    ra::device_settings* device_data;
+    ra::driver_settings* driver_data;
     milliseconds tick_interval;
-    vec2<ra::accel_invoker> invokers;
-    ra::mouse_modifier modifier;
+    ra::modifier modifier_data[ra::DRIVER_CAPACITY];
 } global = {};
 
 extern "C" PULONG InitSafeBootMode;
 
+bool init_failed() 
+{ 
+    return global.driver_data_size == 0; 
+};
+
 __declspec(guard(ignore))
 VOID
 RawaccelCallback(
@@ -58,26 +73,39 @@ Arguments:
     auto num_packets = InputDataEnd - InputDataStart;
 
     if (num_packets > 0 &&
-        !(InputDataStart->Flags & MOUSE_MOVE_ABSOLUTE) && 
-            (global.args.device_id[0] == 0 ||
-                bool(wcsncmp(devExt->dev_id, global.args.device_id, ra::MAX_DEV_ID_LEN)) == 
-                    global.args.ignore)) {
-        counter_t now = KeQueryPerformanceCounter(NULL).QuadPart;
-        counter_t ticks = now - devExt->counter;
-        devExt->counter = now;
-        milliseconds raw_elapsed = ticks * global.tick_interval;
-        milliseconds time = ra::clampsd(raw_elapsed / num_packets, 
-                                        global.args.time_min, 
-                                        global.args.time_max);
+        !(InputDataStart->Flags & MOUSE_MOVE_ABSOLUTE) &&
+        devExt->enable) {
+        
+        milliseconds time;
+        if (devExt->keep_time) {
+            counter_t now = KeQueryPerformanceCounter(NULL).QuadPart;
+            counter_t ticks = now - devExt->counter;
+            devExt->counter = now;
+            milliseconds raw = ticks * global.tick_interval / num_packets;
+            time = ra::clampsd(raw, devExt->clamp.min, devExt->clamp.max);
+        }
+        else {
+            time = devExt->clamp.min;
+        }
+
         auto it = InputDataStart;
         do {
+            if (devExt->set_extra_info) {
+                union {
+                    short input[2];
+                    ULONG data;
+                } u = { short(it->LastX), short(it->LastY) };
+
+                it->ExtraInformation = u.data;
+            }
+
             if (it->LastX || it->LastY) {
                 vec2d input = {
                     static_cast<double>(it->LastX),
                     static_cast<double>(it->LastY)
                 };
 
-                global.modifier.modify(input, global.invokers, time);
+                devExt->mod_ptr->modify(input, *devExt->drv_ptr, devExt->dpi_factor, time);
 
                 double carried_result_x = input.x + devExt->carry.x;
                 double carried_result_y = input.y + devExt->carry.y;
@@ -89,8 +117,8 @@ Arguments:
                 double carry_y = carried_result_y - out_y;
 
                 if (!ra::infnan(carry_x + carry_y)) {
-                    devExt->carry.x = carried_result_x - out_x;
-                    devExt->carry.y = carried_result_y - out_y;
+                    devExt->carry.x = carry_x;
+                    devExt->carry.y = carry_y;
                     it->LastX = out_x;
                     it->LastY = out_y;
                 }
@@ -149,6 +177,11 @@ Return Value:
 
     DebugPrint(("Ioctl received into filter control object.\n"));
 
+    if (init_failed()) {
+        WdfRequestCompleteWithInformation(Request, STATUS_CANCELLED, 0);
+        return;
+    }
+
     switch (IoControlCode) {
     case RA_READ:
         status = WdfRequestRetrieveOutputBuffer(
@@ -161,10 +194,15 @@ Return Value:
             DebugPrint(("RetrieveOutputBuffer failed: 0x%x\n", status));
         }
         else {
-            ra::io_t& output = *reinterpret_cast<ra::io_t*>(buffer);
+            ra::io_t& output = *static_cast<ra::io_t*>(buffer);
 
-            output.args = global.args;
-            output.mod = global.modifier;
+            output.default_dev_cfg = global.default_dev_cfg;
+
+            output.device_data_size = global.device_data_size;
+            output.driver_data_size = global.driver_data_size;
+
+            RtlCopyMemory(output.device_data, global.device_data, sizeof(output.device_data));
+            RtlCopyMemory(output.driver_data, global.driver_data, sizeof(output.driver_data));
 
             bytes_out = sizeof(ra::io_t);
         }
@@ -180,16 +218,38 @@ Return Value:
             DebugPrint(("RetrieveInputBuffer failed: 0x%x\n", status));
         }
         else {
-            LARGE_INTEGER interval;
-            interval.QuadPart = static_cast<LONGLONG>(ra::WRITE_DELAY) * -10000;
-            KeDelayExecutionThread(KernelMode, FALSE, &interval);
+            WriteDelay();
 
-            ra::io_t& input = *reinterpret_cast<ra::io_t*>(buffer);
+            ra::io_t& input = *static_cast<ra::io_t*>(buffer);
 
-            global.args = input.args;
-            global.invokers = ra::invokers(input.args);
-            global.modifier = input.mod;
+            if (input.driver_data_size == 0) {
+                status = STATUS_CANCELLED;
+                break;
+            }
+
+            WdfWaitLockAcquire(global.collection_lock, NULL);
+
+            global.default_dev_cfg = input.default_dev_cfg;
+
+            global.device_data_size = ra::min(input.device_data_size, ra::DEVICE_CAPACITY);
+            global.driver_data_size = ra::min(input.driver_data_size, ra::DRIVER_CAPACITY);
+
+            RtlCopyMemory(global.device_data, input.device_data, sizeof(input.device_data));
+            RtlCopyMemory(global.driver_data, input.driver_data, sizeof(input.driver_data));
+
+            for (auto i = 0u; i < global.driver_data_size; i++) {
+                global.modifier_data[i] = { global.driver_data[i] };
+            }
+
+            auto count = WdfCollectionGetCount(global.device_collection);
+
+            for (auto i = 0u; i < count; i++) {
+                DeviceSetup(WdfCollectionGetItem(global.device_collection, i));
+            }
+
+            WdfWaitLockRelease(global.collection_lock);
         }
+
         break;
     case RA_GET_VERSION:
         status = WdfRequestRetrieveOutputBuffer(
@@ -202,7 +262,7 @@ Return Value:
             DebugPrint(("RetrieveOutputBuffer failed: 0x%x\n", status));
         }
         else {
-            *reinterpret_cast<ra::version_t*>(buffer) = ra::version;
+            *static_cast<ra::version_t*>(buffer) = ra::version;
             bytes_out = sizeof(ra::version_t);
         }
         break;
@@ -216,6 +276,135 @@ Return Value:
 }
 #pragma warning(pop) // enable 28118 again
 
+VOID
+RawaccelInit(WDFDRIVER driver)
+{
+    NTSTATUS status;
+
+    if (*InitSafeBootMode > 0) return;
+
+    status = CreateControlDevice(driver);
+
+    if (!NT_SUCCESS(status)) {
+        DebugPrint(("CreateControlDevice failed with status 0x%x\n", status));
+        return;
+    }
+
+    status = WdfCollectionCreate(
+        WDF_NO_OBJECT_ATTRIBUTES,
+        &global.device_collection
+    );
+
+    if (!NT_SUCCESS(status)) {
+        DebugPrint(("WdfCollectionCreate failed with status 0x%x\n", status));
+        return;
+    }
+
+    status = WdfWaitLockCreate(
+        WDF_NO_OBJECT_ATTRIBUTES,
+        &global.collection_lock
+    );
+
+    if (!NT_SUCCESS(status)) {
+        DebugPrint(("WdfWaitLockCreate failed with status 0x%x\n", status));
+        return;
+    }
+
+    void* paged_p = ExAllocatePoolWithTag(PagedPool, ra::POOL_SIZE, 'g');
+    if (paged_p) {
+        RtlZeroMemory(paged_p, ra::POOL_SIZE);
+        global.device_data = static_cast<ra::device_settings*>(paged_p);
+    }
+    else {
+        DebugPrint(("ExAllocatePoolWithTag (PagedPool) failed"));
+        return;
+    }
+
+    void* nonpaged_p = ExAllocatePoolWithTag(NonPagedPool, ra::POOL_SIZE, 'g');
+    if (nonpaged_p) {
+        RtlZeroMemory(nonpaged_p, ra::POOL_SIZE);
+        global.driver_data = static_cast<ra::driver_settings*>(nonpaged_p);
+        *global.driver_data = {};
+        *global.modifier_data = { *global.driver_data };
+        global.driver_data_size = 1;
+    }
+    else {
+        DebugPrint(("ExAllocatePoolWithTag (NonPagedPool) failed"));
+        return;
+    }
+
+    LARGE_INTEGER freq;
+    KeQueryPerformanceCounter(&freq);
+    global.tick_interval = 1e3 / freq.QuadPart;
+}
+
+VOID
+DeviceSetup(WDFOBJECT hDevice) 
+{
+    auto* devExt = FilterGetData(hDevice);
+
+    auto set_ext_from_cfg = [devExt](const ra::device_config& cfg) {
+        devExt->enable = !cfg.disable;
+        devExt->set_extra_info = cfg.set_extra_info;
+        devExt->keep_time = cfg.polling_rate <= 0;
+        devExt->dpi_factor = (cfg.dpi > 0) ? (1000.0 / cfg.dpi) : 1;
+
+        if (devExt->keep_time) {
+            devExt->clamp = cfg.clamp;
+        }
+        else {
+            milliseconds interval = 1000.0 / cfg.polling_rate;
+            devExt->clamp = { interval, interval };
+        }
+    };
+
+    set_ext_from_cfg(global.default_dev_cfg);
+    devExt->counter = 0;
+    devExt->carry = {};
+    devExt->drv_ptr = global.driver_data;
+    devExt->mod_ptr = global.modifier_data;
+
+    for (auto i = 0u; i < global.device_data_size; i++) {
+        auto& dev_settings = global.device_data[i];
+
+        if (wcsncmp(devExt->dev_id, dev_settings.id, ra::MAX_DEV_ID_LEN) == 0) {
+            set_ext_from_cfg(dev_settings.config);
+
+            if (dev_settings.profile[0] != L'\0') {
+                for (auto j = 0u; j < global.driver_data_size; j++) {
+                    auto& profile = global.driver_data[j].prof;
+
+                    if (wcsncmp(dev_settings.profile, profile.name, ra::MAX_NAME_LEN) == 0) {
+                        devExt->drv_ptr = &global.driver_data[j];
+                        devExt->mod_ptr = &global.modifier_data[j];
+                        return;
+                    }
+                }
+            }
+
+            return;
+        }
+    }
+}
+
+VOID
+DeviceCleanup(WDFOBJECT hDevice)
+{
+    PAGED_CODE();
+    DebugPrint(("Removing device from collection\n"));
+
+    WdfWaitLockAcquire(global.collection_lock, NULL);
+    WdfCollectionRemove(global.device_collection, hDevice);
+    WdfWaitLockRelease(global.collection_lock);
+}
+
+VOID 
+WriteDelay()
+{
+    LARGE_INTEGER interval;
+    interval.QuadPart = static_cast<LONGLONG>(ra::WRITE_DELAY) * -10000;
+    KeDelayExecutionThread(KernelMode, FALSE, &interval);
+}
 
 NTSTATUS
 DriverEntry(
@@ -259,14 +448,10 @@ Routine Description:
         WDF_NO_OBJECT_ATTRIBUTES,
         &config,
         &driver);
- 
 
-    if (NT_SUCCESS(status)) {
-        LARGE_INTEGER freq;
-        KeQueryPerformanceCounter(&freq);
-        global.tick_interval = 1e3 / freq.QuadPart;
 
-        CreateControlDevice(driver);
+    if (NT_SUCCESS(status)) {
+        RawaccelInit(driver);
     }
     else {
         DebugPrint(("WdfDriverCreate failed with status 0x%x\n", status));
@@ -276,8 +461,7 @@ Routine Description:
 }
 
 
-inline
-VOID
+NTSTATUS
 CreateControlDevice(WDFDRIVER Driver)
 /*++
 Routine Description:
@@ -378,7 +562,7 @@ Return Value:
     //
     WdfControlFinishInitializing(controlDevice);
 
-    return;
+    return STATUS_SUCCESS;
 
 Error:
 
@@ -393,8 +577,9 @@ Error:
     }
 
     DebugPrint(("CreateControlDevice failed with status code 0x%x\n", status));
-}
 
+    return status;
+}
 
 NTSTATUS
 EvtDeviceAdd(
@@ -437,7 +622,7 @@ Return Value:
 
     DebugPrint(("Enter FilterEvtDeviceAdd \n"));
 
-    if (*InitSafeBootMode > 0) {
+    if (init_failed()) {
         return STATUS_SUCCESS;
     }
 
@@ -453,6 +638,7 @@ Return Value:
     WDF_OBJECT_ATTRIBUTES_INIT_CONTEXT_TYPE(&deviceAttributes,
         DEVICE_EXTENSION);
 
+    deviceAttributes.EvtCleanupCallback = DeviceCleanup;
 
     //
     // Create a framework device object.  This call will in turn create
@@ -469,7 +655,7 @@ Return Value:
     // get device id from bus driver
     //
     DEVICE_OBJECT* pdo = WdfDeviceWdmGetPhysicalDevice(hDevice);
-
+    
     KEVENT ke;
     KeInitializeEvent(&ke, NotificationEvent, FALSE);
     IO_STATUS_BLOCK iosb = {};
@@ -480,18 +666,36 @@ Return Value:
     stack->MinorFunction = IRP_MN_QUERY_ID;
     stack->Parameters.QueryId.IdType = BusQueryDeviceID;
 
-    NTSTATUS nts = IoCallDriver(pdo, Irp);
+    NTSTATUS tmp = IoCallDriver(pdo, Irp);
 
-    if (nts == STATUS_PENDING) {
+    if (tmp == STATUS_PENDING) {
         KeWaitForSingleObject(&ke, Executive, KernelMode, FALSE, NULL);
+        tmp = iosb.Status;
     }
 
-    if (NT_SUCCESS(nts)) {
-        auto* id_ptr = reinterpret_cast<WCHAR*>(iosb.Information); 
-        wcsncpy(FilterGetData(hDevice)->dev_id, id_ptr, ra::MAX_DEV_ID_LEN);
-        DebugPrint(("Device ID = %ws\n", id_ptr));
+    auto* devExt = FilterGetData(hDevice);
+
+    if (NT_SUCCESS(tmp)) {
+        auto* id_ptr = reinterpret_cast<WCHAR*>(iosb.Information);
+        wcsncpy(devExt->dev_id, id_ptr, ra::MAX_DEV_ID_LEN);
         ExFreePool(id_ptr);
     }
+    else {
+        DebugPrint(("IoCallDriver failed with status 0x%x\n", tmp));
+        *devExt->dev_id = L'\0';
+    }
+
+    WdfWaitLockAcquire(global.collection_lock, NULL);
+
+    DeviceSetup(hDevice);
+
+    tmp = WdfCollectionAdd(global.device_collection, hDevice);
+
+    if (!NT_SUCCESS(tmp)) {
+        DebugPrint(("WdfCollectionAdd failed with status 0x%x\n", tmp));
+    }
+
+    WdfWaitLockRelease(global.collection_lock);
 
     //
     // Configure the default queue to be Parallel. Do not use sequential queue
@@ -597,8 +801,6 @@ Routine Description:
             break;
         }
 
-        devExt->counter = 0;
-        devExt->carry = {};
         devExt->UpperConnectData = *connectData;
 
         //
@@ -636,7 +838,6 @@ Routine Description:
 }
 
 
-inline
 VOID
 DispatchPassThrough(
     _In_ WDFREQUEST Request,