Add --idle-timeout and make install features for servod

ServoBlaster/README.txt

@@ -32,12 +32,14 @@ to P1 header pins as follows:
           0               4             P1-7
           1              17             P1-11
           2              18             P1-12
-          3              21             P1-13
+          3             21/27           P1-13
           4              22             P1-15
           5              23             P1-16
           6              24             P1-18
           7              25             P1-22
 
+P1-13 is connected to either GPIO-21 or GPIO-27, depending on board revision.
+
 When the driver is first loaded the GPIO pins are configure to be outputs, and
 their pulse widths are set to 0.  This is so that servos don't jump to some
 arbitrary position when you load the driver.  Once you know where you want your
@@ -95,12 +97,15 @@ enough to crash the Pi!
 
 
 
-There are two implementions of ServoBlaster; a kernel module based one, and
-a user space daemon.  The kernel module based one is the original, and is
-more mature.  The user space daemon implementation is much more convenient to
-use but is less well tested and does not have all the features of the kernel
-based one.  I would recommend you try the user space implementation first, as
-it is likely to be easier to get going.
+There are two implementions of ServoBlaster; a kernel module based one, and a
+user space daemon.  The kernel module based one is the original, but is more of
+a pain to build because you need a matching kernel build.  The user space
+daemon implementation is much more convenient to use but does not have all the
+features of the kernel based one.  I would recommend you try the user space
+implementation first, as it is likely to be easier to get going.
+
+The kernel module implementation is in the subdirectory 'kernel', while the
+user space implementation can be found in subdirectory 'user'.
 
 Details specific to each implementation are provided in separate sections
 below.
@@ -135,18 +140,37 @@ $ sudo ./servod --pcm
 Using hardware:        PCM
 ...
 
-Features not currently supported in the user space implementation:
+Some people have requested that a servo output turns off automatically if no
+new pulse width has been requested recently, and I've had two reports of
+servos overheating when driven for long periods of time.  To support this
+request, servod implements an idle timeout which can be specified at
+module load time.  The value is specified in milliseconds, so if you want
+to drive your servos for 2 seconds following each new width request you would
+do this:
 
-- does not support the timeout= option to turn off servo outputs after
-  some specified delay.  You must set a servo width to 0 to turn off an
-  output, if you want to.
-- you cannot read /dev/servoblaster to see the current servo settings
+$ sudo ./servod --idle-timeout=2000
+
+Typical small servos take a few 100 milliseconds to rotate from one extreme
+to the other, so for small values of idle_timeout you might find the control
+pulse is turned off before your servo has reached the required position.
+idle_timeout defaults to 0, which disables the feature.
+
+If you want servod to start automatically when the system boots, then you
+can install it along with a startup script as follows:
+
+$ sudo make install
+
+You may wish to edit /etc/init.d/servoblaster to change the parameters that are
+specified in that script (e.g.  the idle-timeout, which is set to 2 seconds).
 
 
 
 The kernel space implementation
 -------------------------------
 
+Please note that the user space implementation is the preferred one to use and
+the kernel implementation (servoblaster.ko) has been depreciated.
+
 Upon reading /dev/servoblaster, the device file provides feedback as to what
 position each servo is currently set.  For example, after starting the driver
 and sending the command "3=120", you would see:
@@ -176,6 +200,10 @@ on the wiki (http://elinux.org/RPi_Kernel_Compilation) to compile the kernel,
 then edit the servoblaster Makefile to point at your kernel tree, then build
 servoblaster.
 
+As the mapping of GPIO to P1 header pins changed between Rev 1 and Rev 2
+boards, you will need to modify servoblaster.c approriately for your board.
+Please uncomment the define for REV_1 or REV_2 as appropriate.
+
 It is not currently possible to make the kernel implementation use the PCM
 hardware rather than the PWM hardware, therefore it will interfere with 3.5mm
 jack audio output.

ServoBlaster/kernel/Makefile

@@ -3,20 +3,14 @@ INSTALL_PATH := /lib/modules/$(shell /bin/uname -r)/kernel/drivers/misc/servobla
 #CROSS_OPTS := CROSS_COMPILE=/usr/bin/arm-linux-gnueabi- ARCH=arm
 CROSS_OPTS :=
 
-.PHONY: all install install_autostart
-all:	servod servoblaster.ko
+.PHONY: all install install_autostart uninstall
+all:	servoblaster.ko
 
 servoblaster.ko:	servoblaster.c servoblaster.h
 	@[ -d ${KERNEL_TREE} ] || { echo "Edit Makefile to set KERNEL_TREE to point at your kernel"; exit 1; }
 	@[ -e ${KERNEL_TREE}/Module.symvers ] || { echo "KERNEL_TREE/Module.symvers does not exist, you need to configure and compile your kernel"; exit 1; }
 	make -C ${KERNEL_TREE} ${CROSS_OPTS} M=$(PWD) modules
 
-servodemo:	servodemo.c servoblaster.h
-	gcc -Wall -g -O2 -o servodemo servodemo.c -Wl,--export-dynamic `pkg-config --cflags gtk+-3.0 gmodule-export-2.0` `pkg-config --libs gtk+-3.0 gmodule-export-2.0`
-
-servod:	servod.c
-	gcc -Wall -g -O2 -o servod servod.c
-
 install: servoblaster.ko
 	@sudo cp $(PWD)/udev_scripts/servoblaster /lib/udev
 	@sudo cp $(PWD)/udev_scripts/20-servoblaster.rules /etc/udev/rules.d
@@ -34,7 +28,13 @@ install_autostart: install
 	@echo "	modprobe servoblaster"
 	@echo "	modprobe -r servoblaster"
 
+uninstall:
+	@modprobe -r servoblaster
+	@sudo rm -f /lib/udev/servoblaster
+	@sudo rm -f /etc/udev/rules.d/20-servoblaster.rules
+	@sudo rm -f $(INSTALL_PATH)/servoblaster.ko
+	@sudo depmod -a
+	
 clean:
 	make -C ${KERNEL_TREE} ${CROSS_OPTS} M=$(PWD) clean
-	rm -f servod servodemo
 

ServoBlaster/kernel/servoblaster.c

@@ -107,6 +107,12 @@ static struct file_operations fops =
 	.compat_ioctl = dev_ioctl,
 };
 
+// Define REV_1 or REV_2 depending on whcih rev of Pi you have.  Alternatively
+// just don't try to use P1-13.
+
+//#define REV_1
+//#define REV_2
+
 // Map servo channels to GPIO pins
 static uint8_t servo2gpio[] = {
 		4,	// P1-7
@@ -116,7 +122,13 @@ static uint8_t servo2gpio[] = {
 #else
 		18,	// P1-12
 #endif
+#if defined(REV_1)
 		21,	// P1-13
+#elif defined(REV_2)
+		27,	// P1-13
+#else
+  #error "You must define REV_1 or REV_2"
+#endif
 		22,	// P1-15
 		23,	// P1-16
 		24,	// P1-18

ServoBlaster/servodemo.c

@@ -1,4 +0,0 @@
-int main(int argc, char **argv)
-{
-	return 0;
-}

ServoBlaster/user/Makefile

@@ -0,0 +1,24 @@
+
+.PHONY: all install uninstall
+all:	servod
+
+servod:	servod.c
+	gcc -Wall -g -O2 -o servod servod.c
+
+install: servod
+	[ "`id -u`" = "0" ] || { echo "Must be run as root"; exit 1; }
+	cp -f servod /usr/local/sbin
+	cp -f init-script /etc/init.d/servoblaster
+	update-rc.d servoblaster defaults 92 08
+	/etc/init.d/servoblaster start
+
+uninstall:
+	[ "`id -u`" = "0" ] || { echo "Must be run as root"; exit 1; }
+	[ -e /etc/init.d/servoblaster ] && /etc/init.d/servoblaster stop || :
+	update-rc.d servoblaster remove
+	rm -f /usr/local/sbin/servod
+	rm -f /etc/init.d/servoblaster
+
+clean:
+	rm -f servod
+

ServoBlaster/user/init-script

@@ -0,0 +1,41 @@
+#!/bin/sh
+### BEGIN INIT INFO
+# Provides:          servoblaster
+# Required-Start:    hostname $local_fs
+# Required-Stop:
+# Should-Start:
+# Default-Start:     2 3 4 5
+# Default-Stop:      0 1 6
+# Short-Description: Start/stop servod.
+# Description:       This script starts/stops servod.
+### END INIT INFO
+
+PATH=/sbin:/usr/sbin:/bin:/usr/bin
+. /lib/init/vars.sh
+
+OPTS="--idle-timeout=2000"
+
+res=0
+
+case "$1" in
+  start)
+	/usr/local/sbin/servod $OPTS >/dev/null
+	;;
+  restart|reload|force-reload)
+	killall servod
+	/usr/local/sbin/servod $OPTS >/dev/null
+	;;
+  stop)
+	killall servod
+	;;
+  status)
+	[ -e /dev/servoblaster ] || res=4
+	;;
+  *)
+	echo "Usage: servoblaster [start|stop|status]" >&2
+	res=3
+	;;
+esac
+
+exit $res
+

ServoBlaster/user/servod.c

@@ -34,8 +34,15 @@
 #include <sys/stat.h>
 #include <fcntl.h>
 #include <sys/mman.h>
+#include <getopt.h>
 
-static uint8_t servo2gpio[] = {
+/* Define which GPIOs/P1-pins to use slightly differently for Rev 1 and
+ * Rev 2 boards, as P1-13 is different.  On Rev 1 GPIO 27 was used for
+ * camera control and GPIO-21 was routed to P1-13, but on Rev 2 it is the
+ * other way round.
+ */
+
+static uint8_t servo2gpio_rev1[] = {
 	4,	// P1-7
 	17,	// P1-11
 	18,	// P1-12
@@ -46,7 +53,19 @@ static uint8_t servo2gpio[] = {
 	25,	// P1-22
 };
 
-#define NUM_SERVOS		(sizeof(servo2gpio)/sizeof(servo2gpio[0]))
+static uint8_t servo2gpio_rev2[] = {
+	4,	// P1-7
+	17,	// P1-11
+	18,	// P1-12
+	27,	// P1-13
+	22,	// P1-15
+	23,	// P1-16
+	24,	// P1-18
+	25,	// P1-22
+};
+
+static uint8_t *servo2gpio;
+static int num_servos;
 
 #define DEVFILE			"/dev/servoblaster"
 
@@ -63,7 +82,7 @@ static uint8_t servo2gpio[] = {
 
 #define CYCLE_TIME_US		20000
 #define SAMPLE_US		10
-#define SERVO_TIME_US		(CYCLE_TIME_US/NUM_SERVOS)
+#define SERVO_TIME_US		(CYCLE_TIME_US/num_servos)
 #define SERVO_SAMPLES		(SERVO_TIME_US/SAMPLE_US)
 #define SERVO_MIN		0
 #define SERVO_MAX		(SERVO_SAMPLES - 1)
@@ -165,27 +184,12 @@ static volatile uint32_t *gpio_reg;
 
 static int delay_hw = DELAY_VIA_PWM;
 
+static struct timeval *servo_kill_time;
+
+static int idle_timeout = 0;
+
 static void set_servo(int servo, int width);
 
-static void
-gpio_set_mode(uint32_t pin, uint32_t mode)
-{
-	uint32_t fsel = gpio_reg[GPIO_FSEL0 + pin/10];
-
-	fsel &= ~(7 << ((pin % 10) * 3));
-	fsel |= mode << ((pin % 10) * 3);
-	gpio_reg[GPIO_FSEL0 + pin/10] = fsel;
-}
-
-static void
-gpio_set(int pin, int level)
-{
-	if (level)
-		gpio_reg[GPIO_SET0] = 1 << pin;
-	else
-		gpio_reg[GPIO_CLR0] = 1 << pin;
-}
-
 static void
 udelay(int us)
 {
@@ -200,7 +204,7 @@ terminate(int dummy)
 	int i;
 
 	if (dma_reg && virtbase) {
-		for (i = 0; i < NUM_SERVOS; i++)
+		for (i = 0; i < num_servos; i++)
 			set_servo(i, 0);
 		udelay(CYCLE_TIME_US);
 		dma_reg[DMA_CS] = DMA_RESET;
@@ -221,6 +225,78 @@ fatal(char *fmt, ...)
 	terminate(0);
 }
 
+static void
+init_idle_timers(void)
+{
+	servo_kill_time = calloc(num_servos, sizeof(struct timeval));
+	if (!servo_kill_time)
+		fatal("servod: calloc() failed\n");
+}
+
+static void
+update_idle_time(int servo)
+{
+	if (idle_timeout == 0)
+		return;
+
+	gettimeofday(servo_kill_time + servo, NULL);
+	servo_kill_time[servo].tv_sec += idle_timeout / 1000;
+	servo_kill_time[servo].tv_usec += (idle_timeout % 1000) * 1000;
+	while (servo_kill_time[servo].tv_usec >= 1000000) {
+		servo_kill_time[servo].tv_usec -= 1000000;
+		servo_kill_time[servo].tv_sec++;
+	}
+}
+
+static void
+get_next_idle_timeout(struct timeval *tv)
+{
+	int i;
+	struct timeval now;
+	struct timeval min = { 60, 0 };
+	long this_diff, min_diff;
+
+	gettimeofday(&now, NULL);
+	for (i = 0; i < num_servos; i++) {
+		if (servo_kill_time[i].tv_sec == 0)
+			continue;
+		else if (servo_kill_time[i].tv_sec < now.tv_sec ||
+			(servo_kill_time[i].tv_sec == now.tv_sec &&
+			 servo_kill_time[i].tv_usec <= now.tv_usec)) {
+			servo_kill_time[i].tv_sec = 0;
+			set_servo(i, 0);
+		} else {
+			this_diff = (servo_kill_time[i].tv_sec - now.tv_sec) * 1000000
+				+ servo_kill_time[i].tv_usec - now.tv_usec;
+			min_diff = min.tv_sec * 1000000 + min.tv_usec;
+			if (this_diff < min_diff) {
+				min.tv_sec = this_diff / 1000000;
+				min.tv_usec = this_diff % 1000000;
+			}
+		}
+	}
+	*tv = min;
+}
+
+static void
+gpio_set_mode(uint32_t pin, uint32_t mode)
+{
+	uint32_t fsel = gpio_reg[GPIO_FSEL0 + pin/10];
+
+	fsel &= ~(7 << ((pin % 10) * 3));
+	fsel |= mode << ((pin % 10) * 3);
+	gpio_reg[GPIO_FSEL0 + pin/10] = fsel;
+}
+
+static void
+gpio_set(int pin, int level)
+{
+	if (level)
+		gpio_reg[GPIO_SET0] = 1 << pin;
+	else
+		gpio_reg[GPIO_CLR0] = 1 << pin;
+}
+
 static uint32_t
 mem_virt_to_phys(void *virt)
 {
@@ -265,6 +341,7 @@ set_servo(int servo, int width)
 			dp[i] = 0;
 		dp[0] = mask;
 		cbp->dst = phys_gpset0;
+		update_idle_time(servo);
 	}
 }
 
@@ -335,7 +412,7 @@ init_ctrl_data(void)
 		phys_fifo_addr = (PCM_BASE | 0x7e000000) + 0x04;
 
 	memset(ctl->sample, 0, sizeof(ctl->sample));
-	for (servo = 0 ; servo < NUM_SERVOS; servo++) {
+	for (servo = 0 ; servo < num_servos; servo++) {
 		for (i = 0; i < SERVO_SAMPLES; i++)
 			ctl->sample[servo * SERVO_SAMPLES + i] = 1 << servo2gpio[servo];
 	}
@@ -425,49 +502,151 @@ init_hardware(void)
 static void
 go_go_go(void)
 {
-	FILE *fp;
+	int fd;
+	struct timeval tv;
+	static char line[128];
+	int nchars = 0;
+	char nl;
 
-	if ((fp = fopen(DEVFILE, "r+")) == NULL)
+	if ((fd = open(DEVFILE, O_RDWR|O_NONBLOCK)) == -1)
 		fatal("servod: Failed to open %s: %m\n", DEVFILE);
 
-	char *lineptr = NULL, nl;
-	size_t linelen;
-
 	for (;;) {
 		int n, width, servo;
+		fd_set ifds;
 
-		if ((n = getline(&lineptr, &linelen, fp)) < 0)
+		FD_ZERO(&ifds);
+		FD_SET(fd, &ifds);
+		get_next_idle_timeout(&tv);
+		if ((n = select(fd+1, &ifds, NULL, NULL, &tv)) != 1)
 			continue;
-		//fprintf(stderr, "[%d]%s", n, lineptr);
-		n = sscanf(lineptr, "%d=%d%c", &servo, &width, &nl);
+		while (read(fd, line+nchars, 1) == 1) {
+			if (line[nchars] == '\n') {
+				line[++nchars] = '\0';
+				nchars = 0;
+				n = sscanf(line, "%d=%d%c", &servo, &width, &nl);
 				if (n !=3 || nl != '\n') {
-			fprintf(stderr, "Bad input: %s", lineptr);
-		} else if (servo < 0 || servo >= NUM_SERVOS) {
+					fprintf(stderr, "Bad input: %s", line);
+				} else if (servo < 0 || servo >= num_servos) {
 					fprintf(stderr, "Invalid servo number %d\n", servo);
 				} else if (width < SERVO_MIN || width > SERVO_MAX) {
 					fprintf(stderr, "Invalid width %d\n", width);
 				} else {
 					set_servo(servo, width);
 				}
+			} else {
+				if (++nchars >= 126) {
+					fprintf(stderr, "Input too long\n");
+					nchars = 0;
 				}
 			}
+		}
+	}
+}
+
+/* Determining the board revision is a lot more complicated than it should be
+ * (see comments in wiringPi for details).  We will just look at the last two
+ * digits of the Revision string and treat '00' and '01' as errors, '02' and
+ * '03' as rev 1, and any other hex value as rev 2.
+ */
+static int
+board_rev(void)
+{
+	char buf[128];
+	char *ptr, *end, *res;
+	static int rev = 0;
+	FILE *fp;
+
+	if (rev)
+		return rev;
+
+	fp = fopen("/proc/cpuinfo", "r");
+
+	if (!fp)
+		fatal("Unable to open /proc/cpuinfo: %m\n");
+
+	while ((res = fgets(buf, 128, fp))) {
+		if (!strncmp(buf, "Revision", 8))
+			break;
+	}
+	fclose(fp);
+
+	if (!res)
+		fatal("servod: No 'Revision' record in /proc/cpuinfo\n");
+
+	ptr = buf + strlen(buf) - 3;
+	rev = strtol(ptr, &end, 16);
+	if (end != ptr + 2)
+		fatal("servod: Failed to parse Revision string\n");
+	if (rev < 1)
+		fatal("servod: Invalid board Revision\n");
+	else if (rev < 4)
+		rev = 1;
+	else
+		rev = 2;
+
+	return rev;
+}
 
 int
 main(int argc, char **argv)
 {
 	int i;
 
-	// Very crude...
-	if (argc == 2 && !strcmp(argv[1], "--pcm"))
-		delay_hw = DELAY_VIA_PCM;
+	while (1) {
+		int c;
+		int option_index;
 
+		static struct option long_options[] = {
+			{ "pcm",          no_argument,       0, 'p' },
+			{ "idle-timeout", required_argument, 0, 't' },
+			{ "idle_timeout", required_argument, 0, 't' },
+			{ "help",         no_argument,       0, 'h' },
+			{ 0,              0,                 0, 0   }
+		};
+
+		c = getopt_long(argc, argv, "hpt:", long_options, &option_index);
+		if (c == -1) {
+			break;
+		} else if (c == 'p') {
+			delay_hw = DELAY_VIA_PCM;
+		} else if (c == 't') {
+			idle_timeout = atoi(optarg);
+			if (idle_timeout < 10 || idle_timeout > 3600000)
+				fatal("Invalid idle_timeout specified\n");
+		} else if (c == 'h') {
+			printf("\nUsage: %s [--pcm] [--idle-timeout=N]\n\n"
+				"Where:\n"
+                                "  --pcm             tells servod to use PCM rather than PWM hardware\n"
+                                "                    to implement delays\n"
+				"  --idle-timeout=N  tells servod to stop sending servo pulses for a\n"
+				"                    given output N milliseconds after the last update\n\n",
+				argv[0]);
+			exit(0);
+		} else {
+			fatal("Invalid parameter\n");
+		}
+	}
+
+	/* Select the correct pin mapping based on board rev */
+	if (board_rev() == 1) {
+		servo2gpio = servo2gpio_rev1;
+		num_servos = sizeof(servo2gpio_rev1);
+	} else {
+		servo2gpio = servo2gpio_rev2;
+		num_servos = sizeof(servo2gpio_rev2);
+	}
+
+	printf("Board revision:      %5d\n", board_rev());
 	printf("Using hardware:      %5s\n", delay_hw == DELAY_VIA_PWM ? "PWM" : "PCM");
-	printf("Number of servos:    %5d\n", NUM_SERVOS);
+	printf("Idle timeout (ms):   %5d\n", idle_timeout);
+	printf("Number of servos:    %5d\n", num_servos);
 	printf("Servo cycle time:    %5dus\n", CYCLE_TIME_US);
 	printf("Pulse width units:   %5dus\n", SAMPLE_US);
 	printf("Maximum width value: %5d (%dus)\n", SERVO_MAX,
 						SERVO_MAX * SAMPLE_US);
 
+	init_idle_timers();
 	setup_sighandlers();
 
 	dma_reg = map_peripheral(DMA_BASE, DMA_LEN);
@@ -486,7 +665,7 @@ main(int argc, char **argv)
 
 	make_pagemap();
 
-	for (i = 0; i < NUM_SERVOS; i++) {
+	for (i = 0; i < num_servos; i++) {
 		gpio_set(servo2gpio[i], 0);
 		gpio_set_mode(servo2gpio[i], GPIO_MODE_OUT);
 	}