Add lightweight daemon to synchronize keyboard indicators to current xkb state

Start keyboard indicator sync daemon on tdm load
This resolves Bug 427

1 files changed, 310 insertions, 50 deletions

diff --git a/tdekbdledsync/main.cpp b/tdekbdledsync/main.cpp
index f33df68c9..a22864f25 100644
--- a/tdekbdledsync/main.cpp
+++ b/tdekbdledsync/main.cpp
@@ -30,6 +30,7 @@ License along with tdekbdledsync. If not, see http://www.gnu.org/licenses/.
 #include <fcntl.h>
 #include <limits.h>
 #include <dirent.h>
+#include <linux/vt.h>
 #include <linux/input.h>
 #include <linux/uinput.h>
 #include <sys/types.h>
@@ -38,12 +39,15 @@ License along with tdekbdledsync. If not, see http://www.gnu.org/licenses/.
 #include <sys/time.h>
 #include <termios.h>
 #include <signal.h>
+#include <stdint.h>
 extern "C" {
 #include <libudev.h>
+#include "getfd.h"
 }
 #include <libgen.h>
 
 #include <X11/Xlib.h>
+#include <X11/Xatom.h>
 #include <X11/XKBlib.h>
 #include <X11/extensions/XTest.h>
 #include <X11/keysym.h>
@@ -67,9 +71,149 @@ int keyboard_fds[MAX_KEYBOARDS];
 
 Display* display = NULL;
 
+// --------------------------------------------------------------------------------------
+// Useful function from Stack Overflow
+// http://stackoverflow.com/questions/874134/find-if-string-endswith-another-string-in-c
+// --------------------------------------------------------------------------------------
+/*  returns 1 iff str ends with suffix  */
+int str_ends_with(const char * str, const char * suffix) {
+
+  if( str == NULL || suffix == NULL )
+    return 0;
+
+  size_t str_len = strlen(str);
+  size_t suffix_len = strlen(suffix);
+
+  if(suffix_len > str_len)
+    return 0;
+
+  return 0 == strncmp( str + str_len - suffix_len, suffix, suffix_len );
+}
+// --------------------------------------------------------------------------------------
+
+// --------------------------------------------------------------------------------------
+// Get the VT number X is running on
+// (code taken from GDM, daemon/getvt.c, GPLv2+)
+// --------------------------------------------------------------------------------------
+int get_x_vtnum(Display *dpy)
+{
+	Atom prop;
+	Atom actualtype;
+	int actualformat;
+	unsigned long nitems;
+	unsigned long bytes_after;
+	unsigned char *buf;
+	int num;
+	
+	prop = XInternAtom (dpy, "XFree86_VT", False);
+	if (prop == None)
+	return -1;
+	
+	if (XGetWindowProperty (dpy, DefaultRootWindow (dpy), prop, 0, 1,
+				False, AnyPropertyType, &actualtype, &actualformat,
+				&nitems, &bytes_after, &buf)) {
+		return -1;
+	}
+	
+	if (nitems != 1) {
+		XFree (buf);
+		return -1;
+	}
+	
+	switch (actualtype) {
+		case XA_CARDINAL:
+		case XA_INTEGER:
+		case XA_WINDOW:
+		switch (actualformat) {
+			case 8:
+				num = (*(uint8_t  *)(void *)buf);
+			break;
+			case 16:
+				num = (*(uint16_t *)(void *)buf);
+			break;
+			case 32:
+				num = (*(uint32_t *)(void *)buf);
+			break;
+			default:
+				XFree (buf);
+				return -1;
+			}
+		break;
+		default:
+		XFree (buf);
+		return -1;
+	}
+	
+	XFree (buf);
+	
+	return num;
+}
+// --------------------------------------------------------------------------------------
+
+// --------------------------------------------------------------------------------------
+// Get the specified xkb mask modifier
+// (code taken from numlockx)
+// --------------------------------------------------------------------------------------
+unsigned int xkb_mask_modifier(XkbDescPtr xkb, const char *name) {
+	int i;
+	if( !xkb || !xkb->names ) {
+		return 0;
+	}
+	for( i = 0; i < XkbNumVirtualMods; i++ ) {
+		char* modStr = XGetAtomName( xkb->dpy, xkb->names->vmods[i] );
+		if( modStr != NULL && strcmp(name, modStr) == 0 ) {
+			unsigned int mask;
+			XkbVirtualModsToReal( xkb, 1 << i, &mask );
+			return mask;
+		}
+	}
+	return 0;
+}
+// --------------------------------------------------------------------------------------
+
+// --------------------------------------------------------------------------------------
+// Get the capslock xkb mask modifier
+// --------------------------------------------------------------------------------------
+unsigned int xkb_capslock_mask() {
+	return LockMask;
+}
+// --------------------------------------------------------------------------------------
+
+// --------------------------------------------------------------------------------------
+// Get the numlock xkb mask modifier
+// (code taken from numlockx)
+// --------------------------------------------------------------------------------------
+unsigned int xkb_numlock_mask() {
+	XkbDescPtr xkb;
+	if(( xkb = XkbGetKeyboard(display, XkbAllComponentsMask, XkbUseCoreKbd )) != NULL ) {
+		unsigned int mask = xkb_mask_modifier( xkb, "NumLock" );
+		XkbFreeKeyboard( xkb, 0, True );
+		return mask;
+	}
+	return 0;
+}
+// --------------------------------------------------------------------------------------
+
+// --------------------------------------------------------------------------------------
+// Get the scroll lock xkb mask modifier
+// (code taken from numlockx and modified)
+// --------------------------------------------------------------------------------------
+unsigned int xkb_scrolllock_mask() {
+	XkbDescPtr xkb;
+	if(( xkb = XkbGetKeyboard(display, XkbAllComponentsMask, XkbUseCoreKbd )) != NULL ) {
+		unsigned int mask = xkb_mask_modifier( xkb, "ScrollLock" );
+		XkbFreeKeyboard( xkb, 0, True );
+		return mask;
+	}
+	return 0;
+}
+// --------------------------------------------------------------------------------------
+
+
 int find_keyboards() {
 	int i, j;
 	int fd;
+	char name[256] = "Unknown";
 
 	keyboard_fd_num = 0;
 	for (i=0; i<MAX_KEYBOARDS; i++) {
@@ -82,15 +226,19 @@ int find_keyboards() {
 		fd = open(filename, O_RDWR|O_SYNC);
 		if (fd >= 0) {
 			ioctl(fd, EVIOCGBIT(EV_KEY, sizeof(key_bitmask)), key_bitmask);
-	
-			struct input_id input_info;
-			ioctl (fd, EVIOCGID, &input_info);
-			if ((input_info.vendor != 0) && (input_info.product != 0)) {
-				/* We assume that anything that has an alphabetic key in the
-					QWERTYUIOP range in it is the main keyboard. */
-				for (j = KEY_Q; j <= KEY_P; j++) {
-					if (TestBit(j, key_bitmask)) {
-						keyboard_fds[keyboard_fd_num] = fd;
+
+			// Ensure that we do not detect tsak faked keyboards
+			ioctl (fd, EVIOCGNAME(sizeof(name)), name);
+			if (str_ends_with(name, "+tsak") == 0) {
+				struct input_id input_info;
+				ioctl (fd, EVIOCGID, &input_info);
+				if ((input_info.vendor != 0) && (input_info.product != 0)) {
+					/* We assume that anything that has an alphabetic key in the
+						QWERTYUIOP range in it is the main keyboard. */
+					for (j = KEY_Q; j <= KEY_P; j++) {
+						if (TestBit(j, key_bitmask)) {
+							keyboard_fds[keyboard_fd_num] = fd;
+						}
 					}
 				}
 			}
@@ -111,11 +259,23 @@ int main() {
 	char name[256] = "Unknown";
 	unsigned int states;
 	struct input_event ev;
+	struct vt_stat vtstat;
+	int vt_fd;
+	int x11_vt_num = -1;
+	XEvent xev;
+	XkbStateRec state;
 
 	bool num_lock_set = false;
 	bool caps_lock_set = false;
 	bool scroll_lock_set = false;
 
+	int num_lock_mask;
+	int caps_lock_mask;
+	int scroll_lock_mask;
+
+	int evBase;
+	int errBase;
+
 	// Open X11 display
 	display = XOpenDisplay(NULL);
 	if (!display) {
@@ -123,58 +283,158 @@ int main() {
 		return -1;
 	}
 
-	// Find keyboards
-	find_keyboards();
-	if (keyboard_fd_num == 0) {
-		printf ("[tdekbdledsync] Could not find any usable keyboard(s)!\n");
+	// Set up Xkb extension
+	int i1, mn, mj;
+	mj = XkbMajorVersion;
+	mn = XkbMinorVersion;
+	if (!XkbQueryExtension(display, &i1, &evBase, &errBase, &mj, &mn)) {
+		printf("[tdekbdledsync] Server doesn't support a compatible XKB\n");
 		return -2;
 	}
-	else {
-		fprintf(stderr, "[tdekbdledsync] Found %d keyboard(s)\n", keyboard_fd_num);
+	XkbSelectEvents(display, XkbUseCoreKbd, XkbStateNotifyMask, XkbStateNotifyMask);
+
+	// Get X server VT number
+	x11_vt_num = get_x_vtnum(display);
+
+	// Monitor for hotplugged keyboards
+	struct udev *udev;
+	struct udev_device *dev;
+	struct udev_monitor *mon;
+	struct timeval tv;
+
+	// Create the udev object
+	udev = udev_new();
+	if (!udev) {
+		printf("[tdekbdledsync] Cannot connect to udev interface\n");
+		return -3;
+	}
+
+	// Set up a udev monitor to monitor input devices
+	mon = udev_monitor_new_from_netlink(udev, "udev");
+	udev_monitor_filter_add_match_subsystem_devtype(mon, "input", NULL);
+	udev_monitor_enable_receiving(mon);
 
-		for (current_keyboard=0;current_keyboard<keyboard_fd_num;current_keyboard++) {
-			// Print device name
-			ioctl(keyboard_fds[current_keyboard], EVIOCGNAME (sizeof (name)), name);
-			fprintf(stderr, "[tdekbdledsync] Syncing keyboard: (%s)\n", name);
+	while (1) {
+		// Get masks
+		num_lock_mask = xkb_numlock_mask();
+		caps_lock_mask = xkb_capslock_mask();
+		scroll_lock_mask = xkb_scrolllock_mask();
+
+		// Find keyboards
+		find_keyboards();
+		if (keyboard_fd_num == 0) {
+			printf ("[tdekbdledsync] Could not find any usable keyboard(s)!\n");
+			return -4;
 		}
+		else {
+			fprintf(stderr, "[tdekbdledsync] Found %d keyboard(s)\n", keyboard_fd_num);
 
-		while (1) {
-			// Get Virtual Core keyboard status
-			if (XkbGetIndicatorState(display, XkbUseCoreKbd, &states) != Success) {
-				fprintf(stderr, "[tdekbdledsync] Unable to query X11 Virtual Core keyboard!\n");
-				return -3;
-			}
-	
-			// From "xset -q"
-			caps_lock_set = (states & 0x01);
-			num_lock_set = (states & 0x02);
-			scroll_lock_set = (states & 0x04);
-	
 			for (current_keyboard=0;current_keyboard<keyboard_fd_num;current_keyboard++) {
-				// Set LEDs
-				ev.type = EV_LED;
-				ev.code = LED_CAPSL;
-				ev.value = caps_lock_set;
-				if (write(keyboard_fds[current_keyboard], &ev, sizeof(ev)) < 0) {
-					fprintf(stderr, "[tdekbdledsync] Unable to set LED state\n");
+				// Print device name
+				ioctl(keyboard_fds[current_keyboard], EVIOCGNAME (sizeof (name)), name);
+				fprintf(stderr, "[tdekbdledsync] Syncing keyboard: (%s)\n", name);
+			}
+
+			while (1) {
+				// Get current active VT
+				vt_fd = getfd(NULL);
+				if (ioctl(vt_fd, VT_GETSTATE, &vtstat)) {
+					fprintf(stderr, "[tdekbdledsync] Unable to get current VT!\n");
+					return -5;
 				}
-	
-				ev.type = EV_LED;
-				ev.code = LED_NUML;
-				ev.value = num_lock_set;
-				if (write(keyboard_fds[current_keyboard], &ev, sizeof(ev)) < 0) {
-					fprintf(stderr, "[tdekbdledsync] Unable to set LED state\n");
+
+				if (x11_vt_num == vtstat.v_active) {
+					// Get Virtual Core keyboard status
+					if (XkbGetIndicatorState(display, XkbUseCoreKbd, &states) != Success) {
+						fprintf(stderr, "[tdekbdledsync] Unable to query X11 Virtual Core keyboard!\n");
+						return -6;
+					}
+
+					XkbGetState(display, XkbUseCoreKbd, &state);
+
+					caps_lock_set = (state.mods & caps_lock_mask);
+					num_lock_set = (state.mods & num_lock_mask);
+					scroll_lock_set = (state.mods & scroll_lock_mask);
+
+					for (current_keyboard=0;current_keyboard<keyboard_fd_num;current_keyboard++) {
+						// Set LEDs
+						ev.type = EV_LED;
+						ev.code = LED_CAPSL;
+						ev.value = caps_lock_set;
+						if (write(keyboard_fds[current_keyboard], &ev, sizeof(ev)) < 0) {
+							fprintf(stderr, "[tdekbdledsync] Unable to set LED state\n");
+						}
+
+						ev.type = EV_LED;
+						ev.code = LED_NUML;
+						ev.value = num_lock_set;
+						if (write(keyboard_fds[current_keyboard], &ev, sizeof(ev)) < 0) {
+							fprintf(stderr, "[tdekbdledsync] Unable to set LED state\n");
+						}
+
+						ev.type = EV_LED;
+						ev.code = LED_SCROLLL;
+						ev.value = scroll_lock_set;
+						if (write(keyboard_fds[current_keyboard], &ev, sizeof(ev)) < 0) {
+							fprintf(stderr, "[tdekbdledsync] Unable to set LED state\n");
+						}
+					}
 				}
-	
-				ev.type = EV_LED;
-				ev.code = LED_SCROLLL;
-				ev.value = scroll_lock_set;
-				if (write(keyboard_fds[current_keyboard], &ev, sizeof(ev)) < 0) {
-					fprintf(stderr, "[tdekbdledsync] Unable to set LED state\n");
+
+				// Check the hotplug monitoring process to see if any keyboards were added or removed
+				fd_set readfds;
+				FD_ZERO(&readfds);
+				FD_SET(udev_monitor_get_fd(mon), &readfds);
+				tv.tv_sec = 0;
+				tv.tv_usec = 0;
+				int fdcount = select(udev_monitor_get_fd(mon)+1, &readfds, NULL, NULL, &tv);
+				if (fdcount < 0) {
+					if (errno == EINTR) {
+						fprintf(stderr, "[tdekbdledsync] Signal caught in hotplug monitoring process; ignoring\n");
+					}
+					else {
+						fprintf(stderr, "[tdekbdledsync] Select failed on udev file descriptor in hotplug monitoring process\n");
+					}
 				}
+				else {
+					dev = udev_monitor_receive_device(mon);
+					if (dev) {
+						if (strcmp(udev_device_get_action(dev), "add") == 0) {
+							// Reload keyboards
+							break;
+						}
+						if (strcmp(udev_device_get_action(dev), "remove") == 0) {
+							// Reload keyboards
+							break;
+						}
+					}
+				}
+
+				// Poll
+				usleep(250*1000);
+
+// 				// Wait for an Xkb event
+// 				// FIXME
+// 				// This prevents the udev hotplug monitor from working, as XNextEvent does not stop blocking when a keyboard hotplug occurs
+// 				while (1) {
+// 					XNextEvent(display, &xev);
+// 					if (xev.type == evBase + XkbEventCode) {
+// 						XkbEvent *xkb_event = reinterpret_cast<XkbEvent*>(&xev);
+// 						if (xkb_event->any.xkb_type & XkbStateNotify) {
+// 							if ((xkb_event->state.changed & XkbModifierStateMask) || (xkb_event->state.changed & XkbModifierBaseMask)) {
+// 								// Modifier state has changed
+// 								// Synchronize keyboard indicators
+// 								break;
+// 							}
+// 						}
+// 					}
+// 				}
 			}
+		}
 
-			usleep(500*1000);
+		// Close all keyboard file descriptors
+		for (int current_keyboard=0;current_keyboard<keyboard_fd_num;current_keyboard++) {
+			close(keyboard_fds[current_keyboard]);
 		}
 	}