gpio-motors: deliver sub-tick delays on HZ=100 kernels

general/package/gpio-motors/src/gpio-motors.c

@@ -1,14 +1,24 @@
 #include <errno.h>
 #include <fcntl.h>
+#include <limits.h>
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
+#include <time.h>
 #include <unistd.h>
 
 int PAN_PINS[4];
 int TILT_PINS[4];
 int SELECT_PIN = -1;
 
+int PAN_FDS[4] = {-1, -1, -1, -1};
+int TILT_FDS[4] = {-1, -1, -1, -1};
+int SELECT_FD = -1;
+
+/* CLOCK_MONOTONIC resolution, read once in main(): 1ns on kernels with
+ * high-resolution timers, one jiffy (10ms at HZ=100) without them */
+long CLOCK_RES_NS = 0;
+
 int STEP_SEQUENCE[8][4] = {
 	{1, 0, 0, 0}, {1, 1, 0, 0}, {0, 1, 0, 0}, {0, 1, 1, 0},
 	{0, 0, 1, 0}, {0, 0, 1, 1}, {0, 0, 0, 1}, {1, 0, 0, 1}
@@ -39,10 +49,19 @@ void gpio_release(int pin) {
 
 void gpio_clean(int error) {
 	for (int i = 0; i < 4; i++) {
+		if (PAN_FDS[i] != -1) {
+			close(PAN_FDS[i]);
+		}
+		if (TILT_FDS[i] != -1) {
+			close(TILT_FDS[i]);
+		}
 		gpio_release(PAN_PINS[i]);
 		gpio_release(TILT_PINS[i]);
 	}
 
+	if (SELECT_FD != -1) {
+		close(SELECT_FD);
+	}
 	if (SELECT_PIN != -1) {
 		gpio_release(SELECT_PIN);
 	}
@@ -76,14 +95,22 @@ void gpio_export(int pin) {
 	}
 }
 
-void gpio_set(int pin, int value) {
+/* the value fds stay open for the whole run: a path lookup plus open/close per
+ * write costs ~0.5ms on these SoCs, which at 4 writes per micro-step dwarfs
+ * the step delay itself */
+int gpio_open(int pin) {
 	char path[64];
 	snprintf(path, sizeof(path), "/sys/class/gpio/gpio%d/value", pin);
-	FILE *file = fopen(path, "w");
-	if (file) {
-		fprintf(file, "%d", value);
-		fclose(file);
-	} else {
+	int fd = open(path, O_WRONLY);
+	if (fd == -1) {
+		printf("Unable to open value of GPIO %d: [%d] %s\n", pin, errno, strerror(errno));
+		gpio_clean(1);
+	}
+	return fd;
+}
+
+void gpio_set(int fd, int pin, int value) {
+	if (lseek(fd, 0, SEEK_SET) == -1 || write(fd, value ? "1" : "0", 1) != 1) {
 		printf("Unable to set value of GPIO %d: [%d] %s\n", pin, errno, strerror(errno));
 		gpio_clean(1);
 	}
@@ -128,33 +155,74 @@ void gpio_config() {
 	pclose(fp);
 }
 
-void axis_run(const int pins[4], int level, int steps, int delay) {
+/*
+ * On kernels built without high-resolution timers every sleep rounds up to a
+ * whole tick, so at HZ=100 usleep(1500) waits ~10ms and 8 micro-steps x 10ms
+ * put a hard ~80ms floor under every step no matter how small the requested
+ * delay is (measured on Hi3518EV200: 200 steps took 33s at delay 15 and still
+ * 18s at delay 4). Kernels with hrtimers deliver usleep(1500) in ~1.5ms, and
+ * sleeping is strictly better there. clock_getres() tells the two apart, so
+ * sleep whenever the kernel can honour the delay - or when the delay is at
+ * least a tick, where rounding no longer dominates - and spin on
+ * CLOCK_MONOTONIC only for sub-tick delays on a coarse-timer kernel. Moves
+ * are short and bounded, so burning the CPU for their duration is a fair
+ * trade in that remaining case.
+ */
+void delay_us(long us) {
+	if (us <= 0) {
+		return;
+	}
+
+	if (CLOCK_RES_NS <= 1000000 || us >= CLOCK_RES_NS / 1000) {
+		usleep(us);
+		return;
+	}
+
+	struct timespec start, now;
+	if (clock_gettime(CLOCK_MONOTONIC, &start) != 0) {
+		usleep(us);
+		return;
+	}
+
+	/* 64-bit on purpose: a 32-bit long overflows after ~2.1s of elapsed
+	 * time, which a preemption in the middle of the spin can reach */
+	long long target = (long long)us * 1000;
+	for (;;) {
+		clock_gettime(CLOCK_MONOTONIC, &now);
+		long long elapsed = (long long)(now.tv_sec - start.tv_sec) * 1000000000LL + (now.tv_nsec - start.tv_nsec);
+		if (elapsed >= target) {
+			return;
+		}
+	}
+}
+
+void axis_run(const int pins[4], const int fds[4], int level, int steps, int delay) {
 	int remaining = abs(steps);
 	if (remaining == 0) {
 		return;
 	}
 
 	const int (*seq)[4] = (steps < 0) ? REVERSE_STEP_SEQUENCE : STEP_SEQUENCE;
 	if (SELECT_PIN != -1) {
-		gpio_set(SELECT_PIN, level);
-		usleep(100);
+		gpio_set(SELECT_FD, SELECT_PIN, level);
+		delay_us(100);
 	}
 
 	int micro = 0;
 	while (remaining > 0) {
 		for (int i = 0; i < 4; i++) {
-			gpio_set(pins[i], seq[micro][i]);
+			gpio_set(fds[i], pins[i], seq[micro][i]);
 		}
 
-		usleep(delay);
+		delay_us(delay);
 		if (++micro >= 8) {
 			micro = 0;
 			--remaining;
 		}
 	}
 
 	for (int i = 0; i < 4; i++) {
-		gpio_set(pins[i], 0);
+		gpio_set(fds[i], pins[i], 0);
 	}
 }
 
@@ -166,7 +234,17 @@ int main(int argc, char *argv[]) {
 
 	int pan_steps = atoi(argv[1]);
 	int tilt_steps = atoi(argv[2]);
-	int delay = atoi(argv[3]) * 1000;
+	int delay_ms = atoi(argv[3]);
+	if (delay_ms < 0 || delay_ms > INT_MAX / 1000) {
+		fprintf(stderr, "delay must be between 0 and %d ms\n", INT_MAX / 1000);
+		return 1;
+	}
+	int delay = delay_ms * 1000;
+
+	struct timespec res;
+	if (clock_getres(CLOCK_MONOTONIC, &res) == 0) {
+		CLOCK_RES_NS = res.tv_sec ? 1000000000L : res.tv_nsec;
+	}
 
 	gpio_config();
 	for (int i = 0; i < 4; i++) {
@@ -178,8 +256,17 @@ int main(int argc, char *argv[]) {
 		gpio_export(SELECT_PIN);
 	}
 
-	axis_run(PAN_PINS, 0, pan_steps, delay);
-	axis_run(TILT_PINS, 1, tilt_steps, delay);
+	for (int i = 0; i < 4; i++) {
+		PAN_FDS[i] = gpio_open(PAN_PINS[i]);
+		TILT_FDS[i] = gpio_open(TILT_PINS[i]);
+	}
+
+	if (SELECT_PIN != -1) {
+		SELECT_FD = gpio_open(SELECT_PIN);
+	}
+
+	axis_run(PAN_PINS, PAN_FDS, 0, pan_steps, delay);
+	axis_run(TILT_PINS, TILT_FDS, 1, tilt_steps, delay);
 	gpio_clean(0);
 
 	return 0;