1 files changed, 83 insertions, 0 deletions
diff --git a/rust/qmk-hid-com/src_c/src/hid/hid.c b/rust/qmk-hid-com/src_c/src/hid/hid.c
new file mode 100644
index 0000000..d468ba8
--- /dev/null
+++ b/rust/qmk-hid-com/src_c/src/hid/hid.c
@@ -0,0 +1,83 @@
+#include <assert.h>
+#include <hidapi/hidapi.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <unistd.h>
+
+#include "../error.h"
+#include "hid.h"
+
+static hid_device *find_device_by_usage(uint32_t expected_usage,
+                                        uint32_t expected_usage_page) {
+  struct hid_device_info *devs;
+  devs = hid_enumerate(0x0, 0x0);
+
+  hid_device *found_device = NULL;
+  bool found = false;
+
+  struct hid_device_info *cur_dev = devs;
+  for (; cur_dev; cur_dev = cur_dev->next) {
+    // printf("Checking device: %ls, %d (%d:%d) at %s ..\n",
+    //        cur_dev->product_string, cur_dev->product_id, cur_dev->usage,
+    //        cur_dev->usage_page, cur_dev->path);
+
+    if (cur_dev->usage == expected_usage &&
+        cur_dev->usage_page == expected_usage_page) {
+      found_device = hid_open_path(cur_dev->path);
+      found = true;
+
+      goto output;
+    }
+  }
+
+output:
+  hid_free_enumeration(devs);
+  if (!found_device) {
+    if (found) {
+      fail("Failed to open device usage (%d) and usage page (%d): %ls",
+           expected_usage, expected_usage_page, hid_error(found_device));
+    } else {
+      fail("Can't find device with usage (%d) and usage page (%d): %ls",
+           expected_usage, expected_usage_page, hid_error(found_device));
+    }
+    hid_exit();
+
+    return NULL;
+  } else {
+    return found_device;
+  }
+}
+
+uint32_t read_next(uint32_t usage, uint32_t usage_page) {
+  hid_device *handle;
+
+  if (hid_init()) {
+    fail("Failed to init hid");
+  }
+
+  handle = find_device_by_usage(usage, usage_page);
+
+  uint8_t buf[4];
+  int res = 0;
+  res = hid_read(handle, buf, sizeof(buf));
+  if (res < 0) {
+    fail("Failed to read 4 bytes from hid device: %ls", hid_error(handle));
+  } else if (res == 0) {
+    fail("Device did not send anything!");
+  }
+
+  hid_close(handle);
+  if (hid_exit()) {
+    fail("Failed to exit hid");
+  }
+
+  // Buf should only contain positive values.
+  assert(buf[0] >= 0 && buf[1] >= 0 && buf[2] >= 0 && buf[3] >= 0);
+
+  // Only lower Endian supported, TODO add the other one
+  uint32_t key_code =
+      (uint32_t)(buf[0] | (buf[1] << 8) | (buf[2] << 16) | (buf[3] << 24));
+
+  return key_code;
+}