Debian Codegraph @

src/xrt/drivers/opengloves/communication/serial/opengloves_prober_serial.c

raw
1 // Copyright 2019-2022, Collabora, Ltd.
2 // SPDX-License-Identifier: BSL-1.0
3 /*!
4 * @file
5 * @brief OpenGloves serial prober implementation.
6 * @author Daniel Willmott <[email protected]>
7 * @ingroup drv_opengloves
8 */
9
10 #include <libudev.h>
11 #include <stdlib.h>
12 #include <string.h>
13 #include <stdio.h>
14
15 #include "util/u_debug.h"
16 #include "xrt/xrt_defines.h"
17
18 #include "opengloves_prober_serial.h"
19 #include "opengloves_serial.h"
20
21 #define OPENGLOVES_PROBER_LOG_LEVEL U_LOGGING_TRACE
22
23 #define OPENGLOVES_ERROR(...) U_LOG_IFL_E(OPENGLOVES_PROBER_LOG_LEVEL, __VA_ARGS__)
24 #define OPENGLOVES_INFO(...) U_LOG_IFL_I(OPENGLOVES_PROBER_LOG_LEVEL, __VA_ARGS__)
25
26 #define OPENGLOVES_TTY_PATH_SIZE 14
27
28 static int
29 opengloves_udev_get_sysattr_u16_base16(struct udev_device *dev, const char *name, uint16_t *out_value)
30 {
31 const char *str = udev_device_get_sysattr_value(dev, name);
32 if (str == NULL) {
33 return -1;
34 }
35
36 *out_value = (uint16_t)strtol(str, NULL, 16);
37
38 return 0;
39 }
40
41 static int
42 opengloves_serial_device_found(const char *sysfs_path, struct opengloves_communication_device **ocdev)
43 {
44 // ttyUSBx comes after the last / in sysfs_path
45 const char *tty_name = strrchr(sysfs_path, '/') + 1;
46
47 char tty_path[OPENGLOVES_TTY_PATH_SIZE] = {0};
48 int cx = snprintf(tty_path, OPENGLOVES_TTY_PATH_SIZE, "/dev/%s", tty_name);
49
50 if (cx < 0) {
51 OPENGLOVES_ERROR("Failed to create tty path!");
52 return 0;
53 }
54
55 OPENGLOVES_INFO("Device discovered! Attempting connection to %s", tty_path);
56
57 int ret = opengloves_serial_open(tty_path, ocdev);
58 if (ret < 0) {
59 OPENGLOVES_ERROR("Failed to connect to serial device, %s", strerror(-ret));
60 return 0;
61 }
62
63 OPENGLOVES_INFO("Successfully connected to device");
64
65 return 1;
66 }
67
68 int
69 opengloves_get_serial_devices(uint16_t vid, uint16_t pid, struct opengloves_communication_device **out_ocd)
70 {
71 struct udev *ud = udev_new();
72
73 struct udev_enumerate *tty_enumerate = udev_enumerate_new(ud);
74
75 udev_enumerate_add_match_subsystem(tty_enumerate, "tty");
76 udev_enumerate_scan_devices(tty_enumerate);
77
78 struct udev_list_entry *tty_devices;
79 tty_devices = udev_enumerate_get_list_entry(tty_enumerate);
80
81 struct udev_list_entry *tty_dev_list_entry;
82
83 int dev_count = 0;
84 udev_list_entry_foreach(tty_dev_list_entry, tty_devices)
85 {
86 const char *sysfs_path = udev_list_entry_get_name(tty_dev_list_entry);
87 struct udev_device *raw_dev = udev_device_new_from_syspath(ud, sysfs_path);
88
89 struct udev_device *parent_dev = raw_dev;
90 while (parent_dev != NULL) {
91 uint16_t vendor_id = 0;
92 uint16_t product_id = 0;
93 opengloves_udev_get_sysattr_u16_base16(parent_dev, "idVendor", &vendor_id);
94 opengloves_udev_get_sysattr_u16_base16(parent_dev, "idProduct", &product_id);
95
96 // if vendor and product id match what was requested
97 if (vendor_id == vid && product_id == pid && *out_ocd == NULL)
98 dev_count = dev_count + opengloves_serial_device_found(sysfs_path, out_ocd);
99
100 parent_dev = udev_device_get_parent(parent_dev);
101 }
102
103 // Need to unref the raw device since we create that.
104 udev_device_unref(raw_dev);
105 }
106
107 // Both the enumerate and the udev struct needs to be unreferenced.
108 udev_enumerate_unref(tty_enumerate);
109 udev_unref(ud);
110
111 return dev_count;
112 }