Added ServoBlaster

README.md

@@ -1,2 +1,11 @@
 PiBits
-======
+======
+
+Here you'll find various RaspberryPi related projcts:
+
+ServoBlaster:  A kernel driver that lets you control 8 (or more) servos from your RaspberryPi.
+
+Panalyzer:     A Pi based Logic Analyzer.  This can be found in a separate repository alongside this one, but is mentioned here for completeness.
+
+
+

ServoBlaster/Kbuild

@@ -0,0 +1,10 @@
+obj-m = servoblaster.o
+
+KERNEL_TREE := /home/richard/Pi/git/linux
+
+module:
+	make -C ${KERNEL_TREE} ARCH=arm CROSS_COMPILE=/usr/bin/arm-linux-gnueabi- M=$(PWD) modules
+
+clean:
+	make -C ${KERNEL_TREE} ARCH=arm CROSS_COMPILE=/usr/bin/arm-linux-gnueabi- M=$(PWD) clean
+

ServoBlaster/Makefile

@@ -0,0 +1,14 @@
+KERNEL_TREE := /home/richard/Pi/git/linux
+
+all:	servoblaster.ko servodemo
+
+servoblaster.ko:	servoblaster.c servoblaster.h
+	make -C ${KERNEL_TREE} ARCH=arm CROSS_COMPILE=/usr/bin/arm-linux-gnueabi- 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`
+
+clean:
+	make -C ${KERNEL_TREE} ARCH=arm CROSS_COMPILE=/usr/bin/arm-linux-gnueabi- M=$(PWD) clean
+	rm -f servodemo
+

ServoBlaster/README.txt

@@ -0,0 +1,82 @@
+                                ServoBlaster
+
+This is a Linux kernel driver for the RaspberryPi, which provides an interface
+to drive multiple servos via the GPIO pins.   You control the servo postions by
+sending commands to the driver saying what pulse width a particular servo
+output should use.  The driver maintains that pulse width until you send a new
+command requesting some other width.
+
+Currently is it configured to drive 8 servos.  Servos typically need an active
+high pulse of somewhere between 0.5ms and 2.5ms, where the pulse width controls
+the position of the servo.  The pulse should be repeated approximately every
+20ms, although pulse frequency is not critical.  The pulse width is critical,
+as that translates directly to the servo position.
+
+The driver creates a device file, /dev/servoblaster, in to which you can send
+commands.  The command format is "<servo-number>=<sero-position>", where servo
+number is a number from 0 to 7 inclusive, and servo position is the pulse width
+you want in units of 10us.  So, if you want to set servo 3 to a pulse width of
+1.2ms you could do this at the shell prompt:
+
+echo 3=120 > /dev/servoblaster
+
+120 is in units os 10us, so that is 1200us, or 1.2ms.
+
+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 postion when you load the driver.  Once you know where you want your
+servos positioned, write a value to /dev/servoblaster to enable the respective
+output.  When the driver is unloaded it attempts to shut down the outputs
+cleanly, rather than cutting some pulse short and causing a servo position to
+jump.
+
+The driver allocates a timeslot of 2.5ms to each output (8 servos resulting in
+a cycle time of 20ms).  A servo output is set high at the start of its 2.5ms
+timeslot, and set low after the appropriate delay.  There is then a further
+delay to take us to the end of that timeslot before the next servo output is
+set high.  This way there is only ever one servo output active at a time, which
+helps keep the code simple.
+
+The driver works by setting up a linked list of DMA control blocks with the
+last one linked back to the first, so once initialized the DMA controller
+cycles round continuously and the driver does not need to get involved except
+when a pulse width needs to be changed.  For a given servo there are four DMA
+control blocks; the first transfers a single word to the GPIO 'set output'
+register, the second transfers some number of words to the PWM FIFO to generate
+the required pulse width time, the third transfers a single word to the GPIO
+'clear output' register, and the fourth transfers a number of words to the PWM
+FIFO to generate a delay up to the end of the 2.5ms timeslot.
+
+While the driver does use the PWM peripheral, it only uses it to pace the DMA
+transfers, so as to generate accurate delays.  The PWM is set up such that it
+consumes one word from the FIFO every 10us, so to generate a delay of 1.2ms the
+driver sets the DMA transfer count to 480 (1200/10*4, as the FIFO is 32 bits
+wide).  The PWM is set to request data as soon as there is a single word free
+in the FIFO, so there should be no burst transfers to upset the timing.
+
+I used Panalyzer running on one Pi to mointor the servo outputs from a second
+Pi.  The pulse widths and frequencies seem very stable, even under heavy SD
+card use.  This is expected, because the pulse generation is effectively
+handled in hardware and not influenced by interrupt latency or scheduling
+effects.
+
+Please read the driver source for more details, such as which GPIO pin maps to
+which servo number.  The comments at the top of servoblaster.c also explain how
+to make your system create the /dev/servoblaster device node automatically when
+the driver is loaded.
+
+The driver uses DMA channel 0, and PWM channel 1.  It makes no attempt to
+protect against other code using those peripherals.  It sets the relevant GPIO
+pins to be outputs when the driver is loaded, so please ensure that you are not
+driving those pins externally.
+
+I would of course recommend some buffering between the GPIO outputs and the
+servo controls, to protect the Pi.  That said, I'm living dangerously and doing
+without :-)  If you just want to experiment with a small servo you can probably
+take the 5 volts for it from the header pins on the Pi, but I find that doing
+anything non-trivial with four servos connected pulls the 5 volts down far
+enough to crash the Pi!
+
+
+Richard Hirst <richardghirst@gmail.com>  August 2012
+

ServoBlaster/rock.sh

@@ -0,0 +1,37 @@
+#!/bin/bash
+
+# I have four servos driving four 'legs' on a robot.  This script makes
+# the legs go though something like a walking motion.  It actually just
+# skids back and fore on the spot at the moment, becuase it needs knees
+# and lower legs before it can really lift up its feet and take a real
+# step forward.  This is just a quick demo though..
+
+ss()
+{
+  echo $1 > /dev/servoblaster
+}
+
+p[0]=132
+p[1]=164
+p[2]=150
+p[3]=170
+
+step=2
+cnt=0
+while true; do
+  for i in 0 1 2 3; do
+    ss $i=${p[$i]}
+    p[$i]=$[ p[$i] + step ]
+    sleep 0.05
+  done
+  cnt=$[ cnt + step ]
+  if [ $step -gt 0 ]; then
+    if [ $cnt -ge 20 ]; then
+      step=-2
+    fi
+  else
+    if [ $cnt -lt -20 ]; then
+      step=2
+    fi
+  fi
+done

ServoBlaster/servoblaster.c

@@ -0,0 +1,380 @@
+/*
+ * servoblaster.c Multiple Servo Driver for the RaspberryPi
+ * Copyright (c) 2012 Richard Hirst <richardghirst@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.	 See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+/*
+ * If you want the device node created automatically create these two
+ * files, and make /lib/udev/servoblaster executable (chmod +x):
+ *
+ * ============= /etc/udev/rules.d/20-servoblaster.rules =============
+ * SUBSYSTEM=="module", DEVPATH=="/module/servoblaster", RUN+="/lib/udev/servoblaster"
+ * ===================================================================
+ *
+ * ===================== /lib/udev/servoblaster ======================
+ * #!/bin/bash
+ *
+ * if [ "$ACTION" = "remove" ]; then
+ *         rm -f /dev/servoblaster
+ * elif [ "$ACTION" = "add" ]; then
+ *          major=$( sed -n 's/ servoblaster//p' /proc/devices )
+ *        [ "$major" ] && mknod -m 0666 /dev/servoblaster c $major 0
+ * fi
+ *
+ * exit 0
+ * ===================================================================
+ */
+
+#include <linux/module.h>
+#include <linux/string.h>
+#include <linux/fs.h>
+#include <linux/io.h>
+#include <linux/vmalloc.h>
+#include <linux/cdev.h>
+#include <linux/scatterlist.h>
+#include <linux/delay.h>
+#include <linux/sched.h>
+#include <mach/platform.h>
+#include <asm/uaccess.h>
+#include <mach/dma.h>
+#include "servoblaster.h"
+
+#define GPIO_LEN		0xb4
+#define DMA_LEN			0x24
+#define PWM_BASE		(BCM2708_PERI_BASE + 0x20C000)
+#define PWM_LEN			0x28
+#define CLK_BASE        (BCM2708_PERI_BASE + 0x101000)
+#define CLK_LEN			0xA8
+
+#define GPFSEL0			(0x00/4)
+#define GPFSEL1			(0x04/4)
+#define GPSET0			(0x1c/4)
+#define GPCLR0			(0x28/4)
+
+#define PWM_CTL			(0x00/4)
+#define PWM_DMAC		(0x08/4)
+#define PWM_RNG1		(0x10/4)
+#define PWM_FIFO		(0x18/4)
+
+#define PWMCLK_CNTL		40
+#define PWMCLK_DIV		41
+
+#define PWMCTL_MODE1	(1<<1)
+#define PWMCTL_PWEN1	(1<<0)
+#define PWMCTL_CLRF		(1<<6)
+#define PWMCTL_USEF1	(1<<5)
+
+#define PWMDMAC_ENAB	(1<<31)
+// I think this means it requests as soon as there is one free slot in the FIFO
+// which is what we want as burst DMA would mess up our timing..
+#define PWMDMAC_THRSHLD	((15<<8)|(15<<0))
+
+#define DMA_CS			(BCM2708_DMA_CS/4)
+#define DMA_CONBLK_AD	(BCM2708_DMA_ADDR/4)
+#define DMA_DEBUG		(BCM2708_DMA_DEBUG/4)
+
+#define BCM2708_DMA_END				(1<<1)	// Why is this not in mach/dma.h ?
+#define BCM2708_DMA_NO_WIDE_BURSTS	(1<<26)
+
+static int dev_open(struct inode *, struct file *);
+static int dev_close(struct inode *, struct file *);
+static ssize_t dev_read(struct file *, char *, size_t, loff_t *);
+static ssize_t dev_write(struct file *, const char *, size_t, loff_t *);
+static long dev_ioctl(struct file *, unsigned int, unsigned long);
+
+static struct file_operations fops = 
+{
+	.open = dev_open,
+	.read = dev_read,
+	.write = dev_write,
+	.release = dev_close,
+	.unlocked_ioctl = dev_ioctl,
+	.compat_ioctl = dev_ioctl,
+};
+
+// Map servo channels to GPIO pins
+static uint8_t servo2gpio[] = {
+		4,	// P1-7
+		17,	// P1-11
+#ifdef PWM0_ON_GPIO18
+		1,	// P1-5 (GPIO-18, P1-12 is currently PWM0, for debug)
+#else
+		18,	// P1-12
+#endif
+		21,	// P1-13
+		22,	// P1-15
+		23,	// P1-16
+		24,	// P1-18
+		25,	// P1-22
+};
+#define NUM_SERVOS	(sizeof(servo2gpio)/sizeof(servo2gpio[0]))
+
+// Structure of our control data, stored in a 4K page, and accessed by dma controller
+struct ctldata_s {
+	struct bcm2708_dma_cb cb[NUM_SERVOS * 4];	// gpio-hi, delay, gpio-lo, delay, for each servo output
+	uint32_t gpiodata[NUM_SERVOS];				// set-pin, clear-pin values, per servo output
+	uint32_t pwmdata;							// the word we write to the pwm fifo
+};
+
+static struct ctldata_s *ctl;
+static unsigned long ctldatabase;
+static volatile uint32_t *gpio_reg;
+static volatile uint32_t *dma_reg;
+static volatile uint32_t *clk_reg;
+static volatile uint32_t *pwm_reg;
+
+static dev_t devno;
+static struct cdev my_cdev;
+static int my_major;
+
+// Wait until we're not processing the given servo (actually wait until
+// we are not processing the low period of the previous servo, or the
+// high period of this one).
+static int wait_for_servo(int servo)
+{
+	local_irq_disable();
+	for (;;) {
+		int cb = (dma_reg[DMA_CONBLK_AD] - ((uint32_t)ctl->cb & 0x7fffffff)) / sizeof(ctl->cb[0]);
+
+		if (servo > 0) {
+			if (cb < servo*4-2 || cb > servo*4+2)
+				break;
+		} else {
+			if (cb > 2 && cb < NUM_SERVOS*4-2)
+				break;
+		}
+		local_irq_enable();
+		set_current_state(TASK_INTERRUPTIBLE);
+		if (schedule_timeout(1))
+			return -EINTR;
+		local_irq_disable();
+	}
+	// Return with IRQs disabled!!!
+	return 0;
+}
+
+int init_module(void)
+{
+	int res, i, s;
+	
+	res = alloc_chrdev_region(&devno, 0, 1, "servoblaster");
+	if (res < 0) {
+		printk(KERN_WARNING "ServoBlaster: Can't allocated device number\n");
+		return res;
+	}
+	my_major = MAJOR(devno);
+	cdev_init(&my_cdev, &fops);
+	my_cdev.owner = THIS_MODULE;
+	my_cdev.ops = &fops;
+	res = cdev_add(&my_cdev, MKDEV(my_major, 0), 1);
+	if (res) {
+		printk(KERN_WARNING "ServoBlaster: Error %d adding device\n", res);
+		unregister_chrdev_region(devno, 1);
+		return res;
+	}
+
+	ctldatabase = __get_free_pages(GFP_KERNEL, 0);
+	printk(KERN_INFO "ServoBlaster: Control page is at 0x%lx\n", ctldatabase);
+	if (ctldatabase == 0) {
+		printk(KERN_WARNING "ServoBlaster: alloc_pages failed\n");
+		cdev_del(&my_cdev);
+		unregister_chrdev_region(devno, 1);
+		return -EFAULT;
+	}
+	ctl = (struct ctldata_s *)ctldatabase;
+	gpio_reg = (uint32_t *)ioremap(GPIO_BASE, GPIO_LEN);
+	dma_reg  = (uint32_t *)ioremap(DMA_BASE,  DMA_LEN);
+	clk_reg  = (uint32_t *)ioremap(CLK_BASE,  CLK_LEN);
+	pwm_reg  = (uint32_t *)ioremap(PWM_BASE,  PWM_LEN);
+
+	memset(ctl, 0, sizeof(*ctl));
+
+	// Set all servo control pins to be outputs
+	for (i = 0; i < NUM_SERVOS; i++) {
+		int gpio = servo2gpio[i];
+		int fnreg = gpio/10 + GPFSEL0;
+		int fnshft = (gpio %10) * 3;
+		gpio_reg[GPCLR0] = 1 << gpio;
+		gpio_reg[fnreg] = (gpio_reg[fnreg] & ~(7 << fnshft)) | (1 << fnshft);
+	}
+#ifdef PWM0_ON_GPIO18
+	// Set pwm0 output on gpio18
+	gpio_reg[GPCLR0] = 1 << 18;
+	gpio_reg[GPFSEL1] = (gpio_reg[GPFSEL1] & ~(7 << 8*3)) | ( 2 << 8*3);
+#endif
+
+	// Build the DMA CB chain
+	for (s = 0; s < NUM_SERVOS; s++) {
+		int i = s*4;
+		// Set gpio high
+		ctl->gpiodata[s] = 1 << servo2gpio[s];
+		ctl->cb[i].info   = BCM2708_DMA_NO_WIDE_BURSTS | BCM2708_DMA_WAIT_RESP;
+		ctl->cb[i].src    = (uint32_t)(&ctl->gpiodata[s]) & 0x7fffffff;
+		// We clear the GPIO here initially, so outputs go to 0 on startup
+		// Once someone writes to /dev/servoblaster we'll patch it to a 'set'
+		ctl->cb[i].dst    = ((GPIO_BASE + GPCLR0*4) & 0x00ffffff) | 0x7e000000;
+		ctl->cb[i].length = sizeof(uint32_t);
+		ctl->cb[i].stride = 0;
+		ctl->cb[i].next = (uint32_t)(ctl->cb + i + 1) & 0x7fffffff;
+		// delay
+		i++;
+		ctl->cb[i].info   = BCM2708_DMA_NO_WIDE_BURSTS | BCM2708_DMA_WAIT_RESP | BCM2708_DMA_D_DREQ | BCM2708_DMA_PER_MAP(5);
+		ctl->cb[i].src    = (uint32_t)(&ctl->pwmdata) & 0x7fffffff;
+		ctl->cb[i].dst    = ((PWM_BASE + PWM_FIFO*4) & 0x00ffffff) | 0x7e000000;
+		ctl->cb[i].length = sizeof(uint32_t) * 100;	// default 1000us high
+		ctl->cb[i].stride = 0;
+		ctl->cb[i].next = (uint32_t)(ctl->cb + i + 1) & 0x7fffffff;
+		// Set gpio lo
+		i++;
+		ctl->cb[i].info   = BCM2708_DMA_NO_WIDE_BURSTS | BCM2708_DMA_WAIT_RESP;
+		ctl->cb[i].src    = (uint32_t)(&ctl->gpiodata[s]) & 0x7fffffff;
+		ctl->cb[i].dst    = ((GPIO_BASE + GPCLR0*4) & 0x00ffffff) | 0x7e000000;
+		ctl->cb[i].length = sizeof(uint32_t);
+		ctl->cb[i].stride = 0;
+		ctl->cb[i].next = (uint32_t)(ctl->cb + i + 1) & 0x7fffffff;
+		// delay
+		i++;
+		ctl->cb[i].info   = BCM2708_DMA_NO_WIDE_BURSTS | BCM2708_DMA_WAIT_RESP | BCM2708_DMA_D_DREQ | BCM2708_DMA_PER_MAP(5);
+		ctl->cb[i].src    = (uint32_t)(&ctl->pwmdata) & 0x7fffffff;
+		ctl->cb[i].dst    = ((PWM_BASE + PWM_FIFO*4) & 0x00ffffff) | 0x7e000000;
+		ctl->cb[i].length = sizeof(uint32_t) * 150;	// default 1500us, to give 2.5ms per servo
+		ctl->cb[i].stride = 0;
+		ctl->cb[i].next = (uint32_t)(ctl->cb + i + 1) & 0x7fffffff;
+	}
+	// Point last cb back to first one so it loops continuously
+	ctl->cb[NUM_SERVOS*4-1].next = (uint32_t)(ctl->cb) & 0x7fffffff;
+
+	// Initialise PWM
+	pwm_reg[PWM_CTL] = 0;
+	udelay(10);
+	clk_reg[PWMCLK_DIV] = 0x5A000000 | (32<<12);    // set pwm div to 32 (19.2MHz/32 = 600KHz)
+	clk_reg[PWMCLK_CNTL] = 0x5A000011;              // Source=osc and enable
+	pwm_reg[PWM_RNG1] = 6;							// 600KHz/6 = 10us per FIFO write
+	udelay(10);
+	ctl->pwmdata = 1;								// Give a pulse of one clock width for each fifo write
+	pwm_reg[PWM_DMAC] = PWMDMAC_ENAB | PWMDMAC_THRSHLD;
+	udelay(10);
+	pwm_reg[PWM_CTL] = PWMCTL_CLRF;
+	udelay(10);
+	pwm_reg[PWM_CTL] = PWMCTL_USEF1 | PWMCTL_PWEN1;
+	udelay(10);
+
+	// Initialise the DMA
+	dma_reg[DMA_CS] = BCM2708_DMA_RESET;
+	udelay(10);
+	dma_reg[DMA_CS] = BCM2708_DMA_INT | BCM2708_DMA_END;
+	dma_reg[DMA_CONBLK_AD] = (uint32_t)(ctl->cb) & 0x7fffffff;
+	dma_reg[DMA_DEBUG] = 7; // clear debug error flags
+	dma_reg[DMA_CS] = 0x10880001;	// go, mid priority, wait for outstanding writes
+
+	return 0;
+}
+
+
+void cleanup_module(void)
+{
+	int servo;
+
+	// Take care to stop servos with outputs low, so we don't get
+	// spurious movement on module unload
+	for (servo = 0; servo < NUM_SERVOS; servo++) {
+		// Wait until we're not driving this servo
+		if (wait_for_servo(servo))
+			break;
+		// Patch the control block so it stays low
+		ctl->cb[servo*4+0].dst = ((GPIO_BASE + GPCLR0*4) & 0x00ffffff) | 0x7e000000;
+		local_irq_enable();
+	}
+	// Wait 20ms to be sure it has finished it's cycle an all outputs are low
+	msleep(20);
+	// Now we can kill everything
+	dma_reg[DMA_CS] = BCM2708_DMA_RESET;
+	pwm_reg[PWM_CTL] = 0;
+	udelay(10);
+	free_pages(ctldatabase, 0);
+	iounmap(gpio_reg);
+	iounmap(dma_reg);
+	iounmap(clk_reg);
+	iounmap(pwm_reg);
+	cdev_del(&my_cdev);
+	unregister_chrdev_region(devno, 1);
+}
+
+
+static int dev_open(struct inode *inod,struct file *fil)
+{
+	return 0;
+}
+
+static ssize_t dev_read(struct file *filp,char *buf,size_t count,loff_t *f_pos)
+{
+	return 0;
+}
+
+static ssize_t dev_write(struct file *filp,const char *buf,size_t count,loff_t *f_pos)
+{
+	int servo;
+	int cnt;
+	int n;
+	char str[32];
+	char dummy;
+
+	cnt = count < 32 ? count : 31;
+	if (copy_from_user(str, buf, cnt)) {
+		return -EFAULT;
+	}
+	str[cnt] = '\0';
+	n = sscanf(str, "%d=%d\n%c", &servo, &cnt, &dummy);
+	if (n != 2) {
+		printk(KERN_WARNING "ServoBlaster: Failed to parse command (n=%d)\n", n);
+		return -EINVAL;
+	}
+	if (servo < 0 || servo >= NUM_SERVOS) {
+		printk(KERN_WARNING "ServoBlaster: Bad servo number %d\n", servo);
+		return -EINVAL;
+	}
+	if (cnt < 0 || cnt > 249) {
+		printk(KERN_WARNING "ServoBlaster: Bad value %d\n", cnt);
+		return -EINVAL;
+	}
+	if (wait_for_servo(servo))
+		return -EINTR;
+	if (cnt == 0) {
+		ctl->cb[servo*4+0].dst = ((GPIO_BASE + GPCLR0*4) & 0x00ffffff) | 0x7e000000;
+	} else {
+		ctl->cb[servo*4+0].dst = ((GPIO_BASE + GPSET0*4) & 0x00ffffff) | 0x7e000000;
+		ctl->cb[servo*4+1].length = cnt * sizeof(uint32_t);
+		ctl->cb[servo*4+3].length = (250 - cnt) * sizeof(uint32_t);
+	}
+	local_irq_enable();
+
+	return count;
+}
+
+static int dev_close(struct inode *inod,struct file *fil)
+{
+	return 0;
+}
+
+static long dev_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+	return -EINVAL;
+}
+
+MODULE_DESCRIPTION("ServoBlaster, Multiple Servo Driver for the RaspberryPi");
+MODULE_AUTHOR("Richard Hirst <richardghirst@gmail.com>");
+MODULE_LICENSE("GPL v2");
+

ServoBlaster/servodemo.c

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