Add support for B+, Pi2, and 4.1 kernels

ServoBlaster/servodebug.c

@@ -44,15 +44,15 @@
 #define DMA_CONBLK_AD		(0x04/4)
 #define DMA_DEBUG		(0x20/4)
 
-#define GPIO_BASE		0x20200000
+#define GPIO_BASE		0x3f200000
 #define GPIO_LEN		0xB4
-#define DMA_BASE		0x20007000
+#define DMA_BASE		0x3f007000
 #define DMA_LEN			DMA_CHAN_SIZE * (DMA_CHAN_MAX+1)
-#define PWM_BASE		0x2020C000
+#define PWM_BASE		0x3f20C000
 #define PWM_LEN			0x28
-#define CLK_BASE	        0x20101000
+#define CLK_BASE	        0x3f101000
 #define CLK_LEN			0xA8
-#define TICK_BASE		0x20003000
+#define TICK_BASE		0x3f003000
 #define TICK_LEN		0x08
 
 #define PWM_CTL			(0x00/4)

ServoBlaster/user/Makefile

@@ -2,8 +2,8 @@
 .PHONY: all install uninstall
 all:	servod
 
-servod:	servod.c
+servod:	servod.c mailbox.c
-	gcc -Wall -g -O2 -o servod servod.c -lm
+	gcc -Wall -g -O2 -o servod servod.c mailbox.c -lm
 
 install: servod
 	[ "`id -u`" = "0" ] || { echo "Must be run as root"; exit 1; }

ServoBlaster/user/init-script

@@ -15,7 +15,12 @@ PATH=/sbin:/usr/sbin:/bin:/usr/bin
 
 OPTS="--idle-timeout=2000"
 
-res=0
+STATUSFILE="/tmp/servoblaster-status"
+
+if [ $( id -u ) != 0 ]; then
+	echo "ERROR: Must be run as root"
+	exit 1
+fi
 
 case "$1" in
   start)
@@ -29,13 +34,31 @@ case "$1" in
 	killall servod
 	;;
   status)
-	[ -e /dev/servoblaster ] || res=4
+	if [ ! -e /dev/servoblaster ]; then
+		echo "ERROR: /dev/servoblaster does not exist"
+		exit 2
+	fi
+	rm -f $STATUSFILE
+	echo "status $STATUSFILE" > /dev/servoblaster
+	sleep 0.2
+	if [ ! -e $STATUSFILE ]; then
+		echo "ERROR: servod not responding"
+		exit 3
+	elif grep -q "^OK" $STATUSFILE; then
+		echo "OK"
+		exit 0
+	elif grep "^ERROR:" $STATUSFILE; then
+		exit 4
+	else
+		echo "ERROR: No status from servod"
+		exit 5
+	fi
 	;;
   *)
 	echo "Usage: servoblaster [start|stop|status]" >&2
-	res=3
+	exit 6
 	;;
 esac
 
-exit $res
+exit 0
 

ServoBlaster/user/mailbox.c

@@ -0,0 +1,298 @@
+/*
+Copyright (c) 2012, Broadcom Europe Ltd.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the copyright holder nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <assert.h>
+#include <stdint.h>
+#include <errno.h>
+#include <sys/mman.h>
+#include <sys/ioctl.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+
+#include "mailbox.h"
+
+//#define DEBUG
+
+#define PAGE_SIZE (4*1024)
+
+void *mapmem(unsigned base, unsigned size)
+{
+   int mem_fd;
+   unsigned offset = base % PAGE_SIZE;
+   base = base - offset;
+   /* open /dev/mem */
+   if ((mem_fd = open("/dev/mem", O_RDWR|O_SYNC) ) < 0) {
+      printf("can't open /dev/mem\nThis program should be run as root. Try prefixing command with: sudo\n");
+      exit (-1);
+   }
+   void *mem = mmap(
+      0,
+      size,
+      PROT_READ|PROT_WRITE,
+      MAP_SHARED/*|MAP_FIXED*/,
+      mem_fd,
+      base);
+#ifdef DEBUG
+   printf("base=0x%x, mem=%p\n", base, mem);
+#endif
+   if (mem == MAP_FAILED) {
+      printf("mmap error %d\n", (int)mem);
+      exit (-1);
+   }
+   close(mem_fd);
+   return (char *)mem + offset;
+}
+
+void *unmapmem(void *addr, unsigned size)
+{
+   int s = munmap(addr, size);
+   if (s != 0) {
+      printf("munmap error %d\n", s);
+      exit (-1);
+   }
+
+   return NULL;
+}
+
+/*
+ * use ioctl to send mbox property message
+ */
+
+static int mbox_property(int file_desc, void *buf)
+{
+   int fd = file_desc;
+   int ret_val = -1;
+
+   if (fd < 0) {
+      fd = mbox_open();
+   }
+   if (fd >= 0) {
+      ret_val = ioctl(fd, IOCTL_MBOX_PROPERTY, buf);
+
+      if (ret_val < 0) {
+         printf("ioctl_set_msg failed, errno %d: %m\n", errno);
+      }
+   }
+#ifdef DEBUG
+   unsigned *p = buf; int i; unsigned size = *(unsigned *)buf;
+   for (i=0; i<size/4; i++)
+      printf("%04x: 0x%08x\n", i*sizeof *p, p[i]);
+#endif
+   if (file_desc < 0)
+      mbox_close(fd);
+
+   return ret_val;
+}
+
+unsigned mem_alloc(int file_desc, unsigned size, unsigned align, unsigned flags)
+{
+   int i=0;
+   unsigned p[32];
+
+#ifdef DEBUG
+   printf("Requesting %d bytes\n", size);
+#endif
+   p[i++] = 0; // size
+   p[i++] = 0x00000000; // process request
+
+   p[i++] = 0x3000c; // (the tag id)
+   p[i++] = 12; // (size of the buffer)
+   p[i++] = 12; // (size of the data)
+   p[i++] = size; // (num bytes? or pages?)
+   p[i++] = align; // (alignment)
+   p[i++] = flags; // (MEM_FLAG_L1_NONALLOCATING)
+
+   p[i++] = 0x00000000; // end tag
+   p[0] = i*sizeof *p; // actual size
+
+   if (mbox_property(file_desc, p) < 0)
+      return -1;
+   else
+      return p[5];
+}
+
+unsigned mem_free(int file_desc, unsigned handle)
+{
+   int i=0;
+   unsigned p[32];
+   p[i++] = 0; // size
+   p[i++] = 0x00000000; // process request
+
+   p[i++] = 0x3000f; // (the tag id)
+   p[i++] = 4; // (size of the buffer)
+   p[i++] = 4; // (size of the data)
+   p[i++] = handle;
+
+   p[i++] = 0x00000000; // end tag
+   p[0] = i*sizeof *p; // actual size
+
+   mbox_property(file_desc, p);
+   return p[5];
+}
+
+unsigned mem_lock(int file_desc, unsigned handle)
+{
+   int i=0;
+   unsigned p[32];
+   p[i++] = 0; // size
+   p[i++] = 0x00000000; // process request
+
+   p[i++] = 0x3000d; // (the tag id)
+   p[i++] = 4; // (size of the buffer)
+   p[i++] = 4; // (size of the data)
+   p[i++] = handle;
+
+   p[i++] = 0x00000000; // end tag
+   p[0] = i*sizeof *p; // actual size
+
+   if (mbox_property(file_desc, p) < 0)
+      return ~0;
+   else
+      return p[5];
+}
+
+unsigned mem_unlock(int file_desc, unsigned handle)
+{
+   int i=0;
+   unsigned p[32];
+   p[i++] = 0; // size
+   p[i++] = 0x00000000; // process request
+
+   p[i++] = 0x3000e; // (the tag id)
+   p[i++] = 4; // (size of the buffer)
+   p[i++] = 4; // (size of the data)
+   p[i++] = handle;
+
+   p[i++] = 0x00000000; // end tag
+   p[0] = i*sizeof *p; // actual size
+
+   mbox_property(file_desc, p);
+   return p[5];
+}
+
+unsigned execute_code(int file_desc, unsigned code, unsigned r0, unsigned r1, unsigned r2, unsigned r3, unsigned r4, unsigned r5)
+{
+   int i=0;
+   unsigned p[32];
+   p[i++] = 0; // size
+   p[i++] = 0x00000000; // process request
+
+   p[i++] = 0x30010; // (the tag id)
+   p[i++] = 28; // (size of the buffer)
+   p[i++] = 28; // (size of the data)
+   p[i++] = code;
+   p[i++] = r0;
+   p[i++] = r1;
+   p[i++] = r2;
+   p[i++] = r3;
+   p[i++] = r4;
+   p[i++] = r5;
+
+   p[i++] = 0x00000000; // end tag
+   p[0] = i*sizeof *p; // actual size
+
+   mbox_property(file_desc, p);
+   return p[5];
+}
+
+unsigned qpu_enable(int file_desc, unsigned enable)
+{
+   int i=0;
+   unsigned p[32];
+
+   p[i++] = 0; // size
+   p[i++] = 0x00000000; // process request
+
+   p[i++] = 0x30012; // (the tag id)
+   p[i++] = 4; // (size of the buffer)
+   p[i++] = 4; // (size of the data)
+   p[i++] = enable;
+
+   p[i++] = 0x00000000; // end tag
+   p[0] = i*sizeof *p; // actual size
+
+   mbox_property(file_desc, p);
+   return p[5];
+}
+
+unsigned execute_qpu(int file_desc, unsigned num_qpus, unsigned control, unsigned noflush, unsigned timeout) {
+   int i=0;
+   unsigned p[32];
+
+   p[i++] = 0; // size
+   p[i++] = 0x00000000; // process request
+   p[i++] = 0x30011; // (the tag id)
+   p[i++] = 16; // (size of the buffer)
+   p[i++] = 16; // (size of the data)
+   p[i++] = num_qpus;
+   p[i++] = control;
+   p[i++] = noflush;
+   p[i++] = timeout; // ms
+
+   p[i++] = 0x00000000; // end tag
+   p[0] = i*sizeof *p; // actual size
+
+   mbox_property(file_desc, p);
+   return p[5];
+}
+
+int mbox_open(void) {
+   int file_desc;
+   char filename[64];
+
+   // open a char device file used for communicating with kernel mbox driver
+   if ((file_desc = open("/dev/vcio", 0)) >= 0) {
+      /* New kernel, we use /dev/vcio, major 249, since kernel 4.1 */
+      return file_desc;
+   }
+
+   /* Most likely an old kernel, so drop back to the old major=100 device */
+   sprintf(filename, "/tmp/mailbox-%d", getpid());
+   unlink(filename);
+   if (mknod(filename, S_IFCHR|0600, makedev(100, 0)) < 0) {
+      printf("Failed to create mailbox device %s: %m\n", filename);
+      return -1;
+   }
+   file_desc = open(filename, 0);
+   if (file_desc < 0) {
+      printf("Can't open device file %s: %m\n", filename);
+      unlink(filename);
+      return -1;
+   }
+   unlink(filename);
+
+   return file_desc;
+}
+
+void mbox_close(int file_desc) {
+   close(file_desc);
+}

ServoBlaster/user/mailbox.h

@@ -0,0 +1,46 @@
+/*
+Copyright (c) 2012, Broadcom Europe Ltd.
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the copyright holder nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <linux/ioctl.h>
+
+#define MAJOR_NUM 100
+#define IOCTL_MBOX_PROPERTY _IOWR(MAJOR_NUM, 0, char *)
+
+int mbox_open(void);
+void mbox_close(int file_desc);
+
+unsigned get_version(int file_desc);
+unsigned mem_alloc(int file_desc, unsigned size, unsigned align, unsigned flags);
+unsigned mem_free(int file_desc, unsigned handle);
+unsigned mem_lock(int file_desc, unsigned handle);
+unsigned mem_unlock(int file_desc, unsigned handle);
+void *mapmem(unsigned base, unsigned size);
+void *unmapmem(void *addr, unsigned size);
+
+unsigned execute_code(int file_desc, unsigned code, unsigned r0, unsigned r1, unsigned r2, unsigned r3, unsigned r4, unsigned r5);
+unsigned execute_qpu(int file_desc, unsigned num_qpus, unsigned control, unsigned noflush, unsigned timeout);
+unsigned qpu_enable(int file_desc, unsigned enable);

ServoBlaster/user/servod.c

@@ -42,9 +42,11 @@
 #include <getopt.h>
 #include <math.h>
 
+#include "mailbox.h"
+
 #define DMY	255	// Used to represent an invalid P1 pin, or unmapped servo
 
-#define NUM_P1PINS	26
+#define NUM_P1PINS	40
 #define NUM_P5PINS	8
 
 #define MAX_SERVOS	32	/* Only 21 really, but this lets you map servo IDs
@@ -68,17 +70,27 @@
 #define DMA_CHAN_MAX		14
 #define DMA_CHAN_DEFAULT	14
 
-#define DMA_BASE		0x20007000
+#define DMA_BASE_OFFSET		0x00007000
 #define DMA_LEN			DMA_CHAN_SIZE * (DMA_CHAN_MAX+1)
-#define PWM_BASE		0x2020C000
+#define PWM_BASE_OFFSET		0x0020C000
 #define PWM_LEN			0x28
-#define CLK_BASE	        0x20101000
+#define CLK_BASE_OFFSET	        0x00101000
 #define CLK_LEN			0xA8
-#define GPIO_BASE		0x20200000
+#define GPIO_BASE_OFFSET	0x00200000
 #define GPIO_LEN		0x100
-#define PCM_BASE		0x20203000
+#define PCM_BASE_OFFSET		0x00203000
 #define PCM_LEN			0x24
 
+#define DMA_VIRT_BASE		(periph_virt_base + DMA_BASE_OFFSET)
+#define PWM_VIRT_BASE		(periph_virt_base + PWM_BASE_OFFSET)
+#define CLK_VIRT_BASE		(periph_virt_base + CLK_BASE_OFFSET)
+#define GPIO_VIRT_BASE		(periph_virt_base + GPIO_BASE_OFFSET)
+#define PCM_VIRT_BASE		(periph_virt_base + PCM_BASE_OFFSET)
+
+#define PWM_PHYS_BASE		(periph_phys_base + PWM_BASE_OFFSET)
+#define PCM_PHYS_BASE		(periph_phys_base + PCM_BASE_OFFSET)
+#define GPIO_PHYS_BASE		(periph_phys_base + GPIO_BASE_OFFSET)
+
 #define DMA_NO_WIDE_BURSTS	(1<<26)
 #define DMA_WAIT_RESP		(1<<3)
 #define DMA_D_DREQ		(1<<6)
@@ -89,6 +101,7 @@
 
 #define DMA_CS			(0x00/4)
 #define DMA_CONBLK_AD		(0x04/4)
+#define DMA_SOURCE_AD		(0x0c/4)
 #define DMA_DEBUG		(0x20/4)
 
 #define GPIO_FSEL0		(0x00/4)
@@ -140,9 +153,7 @@ typedef struct {
 		 stride, next, pad[2];
 } dma_cb_t;
 
-typedef struct {
+#define BUS_TO_PHYS(x) ((x)&~0xC0000000)
-	uint32_t physaddr;
-} page_map_t;
 
 /* Define which P1 header pins to use by default.  These are the eight standard
  * GPIO pins (those coloured green in the diagram on this page:
@@ -235,6 +246,49 @@ static uint8_t rev2_p5pin2gpio_map[] = {
 	DMY,	// P5-8   Ground
 };
 
+static uint8_t bplus_p1pin2gpio_map[] = {
+	DMY,	// P1-1   3v3
+	DMY,	// P1-2   5v
+	2,	// P1-3   GPIO 2 (SDA)
+	DMY,	// P1-4   5v
+	3,	// P1-5   GPIO 3 (SCL)
+	DMY,	// P1-6   Ground
+	4,	// P1-7   GPIO 4 (GPCLK0)
+	14,	// P1-8   GPIO 14 (TXD)
+	DMY,	// P1-9   Ground
+	15,	// P1-10  GPIO 15 (RXD)
+	17,	// P1-11  GPIO 17
+	18,	// P1-12  GPIO 18 (PCM_CLK)
+	27,	// P1-13  GPIO 27
+	DMY,	// P1-14  Ground
+	22,	// P1-15  GPIO 22
+	23,	// P1-16  GPIO 23
+	DMY,	// P1-17  3v3
+	24,	// P1-18  GPIO 24
+	10,	// P1-19  GPIO 10 (MOSI)
+	DMY,	// P1-20  Ground
+	9,	// P1-21  GPIO 9 (MISO)
+	25,	// P1-22  GPIO 25
+	11,	// P1-23  GPIO 11 (SCLK)
+	8,	// P1-24  GPIO 8 (CE0)
+	DMY,	// P1-25  Ground
+	7,	// P1-26  GPIO 7 (CE1)
+	DMY,	// P1-27  ID_SD
+	DMY,	// P1-28  ID_SC
+	5,	// P1-29  GPIO 5
+	DMY,	// P1-30  Ground
+	6,	// P1-31  GPIO 5
+	12,	// P1-32  GPIO 12
+	13,	// P1-33  GPIO 13
+	DMY,	// P1-34  Ground
+	19,	// P1-35  GPIO 19
+	16,	// P1-36  GPIO 16
+	26,	// P1-37  GPIO 26
+	20,	// P1-38  GPIO 20
+	DMY,	// P1-39  Ground
+	21,	// P1-40  GPIO 21
+};
+
 // cycle_time_us is the pulse cycle time per servo, in microseconds.
 // Typically it should be 20ms, or 20000us.
 
@@ -255,11 +309,6 @@ static int num_servos;
 static uint32_t gpiomode[MAX_SERVOS];
 static int restore_gpio_modes;
 
-page_map_t *page_map;
-
-static uint8_t *virtbase;
-static uint8_t *virtcached;
-
 static volatile uint32_t *pwm_reg;
 static volatile uint32_t *pcm_reg;
 static volatile uint32_t *clk_reg;
@@ -282,6 +331,29 @@ static uint32_t *turnoff_mask;
 static uint32_t *turnon_mask;
 static dma_cb_t *cb_base;
 
+static int board_model;
+static int gpio_cfg;
+
+static uint32_t periph_phys_base;
+static uint32_t periph_virt_base;
+static uint32_t dram_phys_base;
+static uint32_t mem_flag;
+
+static char *gpio_desc[] = {
+	"Unknown",
+	"P1 (26 pins)",
+	"P1 (26 pins), P5 (8 pins)",
+	"P1 (40 pins)"
+};
+
+static struct {
+	int handle;		/* From mbox_open() */
+	uint32_t size;		/* Required size */
+	unsigned mem_ref;	/* From mem_alloc() */
+	unsigned bus_addr;	/* From mem_lock() */
+	uint8_t *virt_addr;	/* From mapmem() */
+} mbox;
+	
 static void set_servo(int servo, int width);
 static void set_servo_idle(int servo);
 static void gpio_set_mode(uint32_t gpio, uint32_t mode);
@@ -300,7 +372,7 @@ terminate(int dummy)
 {
 	int i;
 
-	if (dma_reg && virtbase) {
+	if (dma_reg && mbox.virt_addr) {
 		for (i = 0; i < MAX_SERVOS; i++) {
 			if (servo2gpio[i] != DMY)
 				set_servo(i, 0);
@@ -315,6 +387,14 @@ terminate(int dummy)
 				gpio_set_mode(servo2gpio[i], gpiomode[i]);
 		}
 	}
+	if (mbox.virt_addr != NULL) {
+		unmapmem(mbox.virt_addr, mbox.size);
+		mem_unlock(mbox.handle, mbox.mem_ref);
+		mem_free(mbox.handle, mbox.mem_ref);
+		if (mbox.handle >= 0)
+			mbox_close(mbox.handle);
+	}
+
 	unlink(DEVFILE);
 	unlink(CFGFILE);
 	exit(1);
@@ -413,15 +493,15 @@ gpio_set(int gpio, int level)
 static uint32_t
 mem_virt_to_phys(void *virt)
 {
-	uint32_t offset = (uint8_t *)virt - virtbase;
+	uint32_t offset = (uint8_t *)virt - mbox.virt_addr;
 
-	return page_map[offset >> PAGE_SHIFT].physaddr + (offset % PAGE_SIZE);
+	return mbox.bus_addr + offset;
 }
 
 static void *
 map_peripheral(uint32_t base, uint32_t len)
 {
-	int fd = open("/dev/mem", O_RDWR);
+	int fd = open("/dev/mem", O_RDWR|O_SYNC);
 	void * vaddr;
 
 	if (fd < 0)
@@ -500,47 +580,6 @@ set_servo(int servo, int width)
 	update_idle_time(servo);
 }
 
-static void
-make_pagemap(void)
-{
-	int i, fd, memfd, pid;
-	char pagemap_fn[64];
-
-	page_map = malloc(num_pages * sizeof(*page_map));
-	if (page_map == 0)
-		fatal("servod: Failed to malloc page_map: %m\n");
-	memfd = open("/dev/mem", O_RDWR);
-	if (memfd < 0)
-		fatal("servod: Failed to open /dev/mem: %m\n");
-	pid = getpid();
-	sprintf(pagemap_fn, "/proc/%d/pagemap", pid);
-	fd = open(pagemap_fn, O_RDONLY);
-	if (fd < 0)
-		fatal("servod: Failed to open %s: %m\n", pagemap_fn);
-	if (lseek(fd, (uint32_t)(size_t)virtcached >> 9, SEEK_SET) !=
-						(uint32_t)(size_t)virtcached >> 9) {
-		fatal("servod: Failed to seek on %s: %m\n", pagemap_fn);
-	}
-	for (i = 0; i < num_pages; i++) {
-		uint64_t pfn;
-		if (read(fd, &pfn, sizeof(pfn)) != sizeof(pfn))
-			fatal("servod: Failed to read %s: %m\n", pagemap_fn);
-		if (((pfn >> 55) & 0x1bf) != 0x10c)
-			fatal("servod: Page %d not present (pfn 0x%016llx)\n", i, pfn);
-		page_map[i].physaddr = (uint32_t)pfn << PAGE_SHIFT | 0x40000000;
-		if (mmap(virtbase + i * PAGE_SIZE, PAGE_SIZE, PROT_READ|PROT_WRITE,
-			MAP_SHARED|MAP_FIXED|MAP_LOCKED|MAP_NORESERVE,
-			memfd, (uint32_t)pfn << PAGE_SHIFT | 0x40000000) !=
-				virtbase + i * PAGE_SIZE) {
-			fatal("Failed to create uncached map of page %d at %p\n",
-				i,  virtbase + i * PAGE_SIZE);
-		}
-	}
-	close(fd);
-	close(memfd);
-	memset(virtbase, 0, num_pages * PAGE_SIZE);
-}
-
 static void
 setup_sighandlers(void)
 {
@@ -568,18 +607,18 @@ init_ctrl_data(void)
 	uint32_t maskall = 0;
 
 	if (invert) {
-		phys_gpclr0 = 0x7e200000 + 0x1c;
+		phys_gpclr0 = GPIO_PHYS_BASE + 0x1c;
-		phys_gpset0 = 0x7e200000 + 0x28;
+		phys_gpset0 = GPIO_PHYS_BASE + 0x28;
 	} else {
-		phys_gpclr0 = 0x7e200000 + 0x28;
+		phys_gpclr0 = GPIO_PHYS_BASE + 0x28;
-		phys_gpset0 = 0x7e200000 + 0x1c;
+		phys_gpset0 = GPIO_PHYS_BASE + 0x1c;
 	}
 
 	if (delay_hw == DELAY_VIA_PWM) {
-		phys_fifo_addr = (PWM_BASE | 0x7e000000) + 0x18;
+		phys_fifo_addr = PWM_PHYS_BASE + 0x18;
 		cbinfo = DMA_NO_WIDE_BURSTS | DMA_WAIT_RESP | DMA_D_DREQ | DMA_PER_MAP(5);
 	} else {
-		phys_fifo_addr = (PCM_BASE | 0x7e000000) + 0x04;
+		phys_fifo_addr = PCM_PHYS_BASE + 0x04;
 		cbinfo = DMA_NO_WIDE_BURSTS | DMA_WAIT_RESP | DMA_D_DREQ | DMA_PER_MAP(2);
 	}
 
@@ -696,12 +735,46 @@ init_hardware(void)
 	}
 }
 
+static void
+do_status(char *filename)
+{
+	uint32_t last;
+	int status = -1;
+	char *p;
+	int fd;
+	const char *dma_dead = "ERROR: DMA not running\n";
+
+	while (*filename == ' ')
+		filename++;
+	p = filename + strlen(filename) - 1;
+	while (p > filename && (*p == '\n' || *p == '\r' || *p == ' '))
+		*p-- = '\0';
+
+	last = dma_reg[DMA_CONBLK_AD];
+	udelay(step_time_us*2);
+	if (dma_reg[DMA_CONBLK_AD] != last)
+		status = 0;
+	if ((fd = open(filename, O_WRONLY|O_CREAT, 0666)) >= 0) {
+		if (status == 0)
+			write(fd, "OK\n", 3);
+		else
+			write(fd, dma_dead, strlen(dma_dead));
+		close(fd);
+	} else {
+		printf("Failed to open %s for writing: %m\n", filename);
+	}
+}
+
 static void
 do_debug(void)
 {
 	int i;
 	uint32_t mask = 0;
-	uint32_t last = 0xffffffff;
+	uint32_t last;
+
+	last = dma_reg[DMA_CONBLK_AD];
+	udelay(step_time_us*2);
+	printf("%08x %08x\n", last, dma_reg[DMA_CONBLK_AD]);
 
 	printf("---------------------------\n");
 	printf("Servo  Start  Width  TurnOn\n");
@@ -713,6 +786,7 @@ do_debug(void)
 		}
 	}
 	printf("\nData:\n");
+	last = 0xffffffff;
 	for (i = 0; i < num_samples; i++) {
 		uint32_t curr = turnoff_mask[i] & mask;
 		if (curr != last)
@@ -823,6 +897,8 @@ go_go_go(void)
 					n = sscanf(line, "%d=%s", &servo, width_arg);
 					if (!strcmp(line, "debug\n")) {
 						do_debug();
+					} else if (!strncmp(line, "status ", 7)) {
+						do_status(line + 7);
 					} else if (n != 2) {
 						fprintf(stderr, "Bad input: %s", line);
 					} else if (servo < 0 || servo >= MAX_SERVOS) {
@@ -848,18 +924,18 @@ go_go_go(void)
 /* 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.
+ * '03' as rev 1, and any other hex value as rev 2.  'Pi1 and Pi2 are
+ * differentiated by the Hardware being BCM2708 or BCM2709.
  */
-static int
+static void
-board_rev(void)
+get_model_and_revision(void)
 {
-	char buf[128];
+	char buf[128], revstr[128], modelstr[128];
 	char *ptr, *end, *res;
-	static int rev = 0;
+	int board_revision;
 	FILE *fp;
 
-	if (rev)
+	revstr[0] = modelstr[0] = '\0';
-		return rev;
 
 	fp = fopen("/proc/cpuinfo", "r");
 
@@ -867,26 +943,49 @@ board_rev(void)
 		fatal("Unable to open /proc/cpuinfo: %m\n");
 
 	while ((res = fgets(buf, 128, fp))) {
-		if (!strncmp(buf, "Revision", 8))
+		if (!strncasecmp("hardware", buf, 8))
-			break;
+			memcpy(modelstr, buf, 128);
+		else if (!strncasecmp(buf, "revision", 8))
+			memcpy(revstr, buf, 128);
 	}
 	fclose(fp);
 
-	if (!res)
+	if (modelstr[0] == '\0')
+		fatal("servod: No 'Hardware' record in /proc/cpuinfo\n");
+	if (revstr[0] == '\0')
 		fatal("servod: No 'Revision' record in /proc/cpuinfo\n");
 
-	ptr = buf + strlen(buf) - 3;
+	if (strstr(modelstr, "BCM2708"))
-	rev = strtol(ptr, &end, 16);
+		board_model = 1;
+	else if (strstr(modelstr, "BCM2709"))
+		board_model = 2;
+	else
+		fatal("servod: Cannot parse the hardware name string\n");
+
+	ptr = revstr + strlen(revstr) - 3;
+	board_revision = strtol(ptr, &end, 16);
 	if (end != ptr + 2)
 		fatal("servod: Failed to parse Revision string\n");
-	if (rev < 1)
+	if (board_revision < 1)
 		fatal("servod: Invalid board Revision\n");
-	else if (rev < 4)
+	else if (board_revision < 4)
-		rev = 1;
+		gpio_cfg = 1;
+	else if (board_revision < 16)
+		gpio_cfg = 2;
 	else
-		rev = 2;
+		gpio_cfg = 3;
 
-	return rev;
+	if (board_model == 1) {
+		periph_virt_base = 0x20000000;
+		periph_phys_base = 0x7e000000;
+		dram_phys_base   = 0x40000000;
+		mem_flag         = 0x0c;
+	} else {
+		periph_virt_base = 0x3f000000;
+		periph_phys_base = 0x7e000000;
+		dram_phys_base   = 0xc0000000;
+		mem_flag         = 0x04;
+	}
 }
 
 static void
@@ -905,23 +1004,29 @@ parse_pin_lists(int p1first, char *p1pins, char*p5pins)
 		if (lst == 0 && p1first) {
 			name = "P1";
 			pins = p1pins;
-			if (board_rev() == 1) {
+			if (board_model == 1 && gpio_cfg == 1) {
 				map = rev1_p1pin2gpio_map;
 				mapcnt = sizeof(rev1_p1pin2gpio_map);
-			} else {
+			} else if (board_model == 1 && gpio_cfg == 2) {
 				map = rev2_p1pin2gpio_map;
 				mapcnt = sizeof(rev2_p1pin2gpio_map);
+			} else {
+				map = bplus_p1pin2gpio_map;
+				mapcnt = sizeof(bplus_p1pin2gpio_map);
 			}
 			pNpin2servo = p1pin2servo;
 		} else {
 			name = "P5";
 			pins = p5pins;
-			if (board_rev() == 1) {
+			if (board_model == 1 && gpio_cfg == 1) {
 				map = rev1_p5pin2gpio_map;
 				mapcnt = sizeof(rev1_p5pin2gpio_map);
-			} else {
+			} else if (board_model == 1 && gpio_cfg == 2) {
 				map = rev2_p5pin2gpio_map;
 				mapcnt = sizeof(rev2_p5pin2gpio_map);
+			} else {
+				map = NULL;
+				mapcnt = 0;
 			}
 			pNpin2servo = p5pin2servo;
 		}
@@ -982,20 +1087,25 @@ gpio2pinname(uint8_t gpio)
 	static char res[16];
 	uint8_t pin;
 
-	if (board_rev() == 1) {
+	if (board_model == 1 && gpio_cfg == 1) {
 		if ((pin = gpiosearch(gpio, rev1_p1pin2gpio_map, sizeof(rev1_p1pin2gpio_map))))
 			sprintf(res, "P1-%d", pin);
 		else if ((pin = gpiosearch(gpio, rev1_p5pin2gpio_map, sizeof(rev1_p5pin2gpio_map))))
 			sprintf(res, "P5-%d", pin);
 		else
 			fatal("Cannot map GPIO %d to a header pin\n", gpio);
-	} else {
+	} else if (board_model == 1 && gpio_cfg == 2) {
 		if ((pin = gpiosearch(gpio, rev2_p1pin2gpio_map, sizeof(rev2_p1pin2gpio_map))))
 			sprintf(res, "P1-%d", pin);
 		else if ((pin = gpiosearch(gpio, rev2_p5pin2gpio_map, sizeof(rev2_p5pin2gpio_map))))
 			sprintf(res, "P5-%d", pin);
 		else
 			fatal("Cannot map GPIO %d to a header pin\n", gpio);
+	} else {
+		if ((pin = gpiosearch(gpio, bplus_p1pin2gpio_map, sizeof(bplus_p1pin2gpio_map))))
+			sprintf(res, "P1-%d", pin);
+		else
+			fatal("Cannot map GPIO %d to a header pin\n", gpio);
 	}
 
 	return res;
@@ -1151,8 +1261,9 @@ main(int argc, char **argv)
 			fatal("Invalid parameter\n");
 		}
 	}
-	if (board_rev() == 1 && p5pins[0])
+	get_model_and_revision();
-		fatal("Board rev 1 does not have a P5 header\n");
+	if (board_model == 1 && gpio_cfg == 1 && p5pins[0])
+		fatal("Board model 1 revision 1 does not have a P5 header\n");
 
 	parse_pin_lists(p1first, p1pins, p5pins);
 
@@ -1232,7 +1343,8 @@ main(int argc, char **argv)
 		fatal("min value is >= max value\n");
 	}
 
-	printf("\nBoard revision:            %7d\n", board_rev());
+	printf("\nBoard model:               %7d\n", board_model);
+	printf("GPIO configuration:            %s\n", gpio_desc[gpio_cfg]);
 	printf("Using hardware:                %s\n", delay_hw == DELAY_VIA_PWM ? "PWM" : "PCM");
 	printf("Using DMA channel:         %7d\n", dma_chan);
 	if (idle_timeout)
@@ -1248,7 +1360,7 @@ main(int argc, char **argv)
 						servo_max_ticks * step_time_us);
 	printf("Output levels:            %s\n", invert ? "Inverted" : "  Normal");
 	printf("\nUsing P1 pins:               %s\n", p1pins);
-	if (board_rev() > 1)
+	if (board_model == 1 && gpio_cfg == 2)
 		printf("Using P5 pins:               %s\n", p5pins);
 	printf("\nServo mapping:\n");
 	for (i = 0; i < MAX_SERVOS; i++) {
@@ -1261,55 +1373,33 @@ main(int argc, char **argv)
 	init_idle_timers();
 	setup_sighandlers();
 
-	dma_reg = map_peripheral(DMA_BASE, DMA_LEN);
+	dma_reg = map_peripheral(DMA_VIRT_BASE, DMA_LEN);
 	dma_reg += dma_chan * DMA_CHAN_SIZE / sizeof(uint32_t);
-	pwm_reg = map_peripheral(PWM_BASE, PWM_LEN);
+	pwm_reg = map_peripheral(PWM_VIRT_BASE, PWM_LEN);
-	pcm_reg = map_peripheral(PCM_BASE, PCM_LEN);
+	pcm_reg = map_peripheral(PCM_VIRT_BASE, PCM_LEN);
-	clk_reg = map_peripheral(CLK_BASE, CLK_LEN);
+	clk_reg = map_peripheral(CLK_VIRT_BASE, CLK_LEN);
-	gpio_reg = map_peripheral(GPIO_BASE, GPIO_LEN);
+	gpio_reg = map_peripheral(GPIO_VIRT_BASE, GPIO_LEN);
 
-	/*
+	/* Use the mailbox interface to the VC to ask for physical memory */
-	 * Map the pages to our virtual address space; this reserves them and
+	// Use the mailbox interface to request memory from the VideoCore
-	 * locks them in memory.  However, these are L1 & L2 non-coherent
+	// We specifiy (-1) for the handle rather than calling mbox_open()
-	 * cached pages and we want coherent access to them so the DMA
+	// so multiple users can share the resource.
-	 * controller sees our changes immediately.  To get that, we create a
+	mbox.handle = -1; // mbox_open();
-	 * second mapping of the same size and immediately free it.  This gives
+	mbox.size = num_pages * 4096;
-	 * us an address in our virtual address space where we can map in a
+	mbox.mem_ref = mem_alloc(mbox.handle, mbox.size, 4096, mem_flag);
-	 * coherent view of the physical pages that were allocated by the first
+	if (mbox.mem_ref < 0) {
-	 * mmap(). This coherent mapping happens in make_pagemap().  All
+		fatal("Failed to alloc memory from VideoCore\n");
-	 * accesses to our memory that is shared with the DMA controller are
+	}
-	 * via this second coherent mapping.  The memset() below forces the
+	mbox.bus_addr = mem_lock(mbox.handle, mbox.mem_ref);
-	 * pages to be allocated.
+	if (mbox.bus_addr == ~0) {
-	 */
+		mem_free(mbox.handle, mbox.size);
-	virtcached = mmap(NULL, num_pages * PAGE_SIZE, PROT_READ|PROT_WRITE,
+		fatal("Failed to lock memory\n");
-			MAP_SHARED|MAP_ANONYMOUS|MAP_NORESERVE|MAP_LOCKED,
+	}
-			-1, 0);
+	mbox.virt_addr = mapmem(BUS_TO_PHYS(mbox.bus_addr), mbox.size);
-	if (virtcached == MAP_FAILED)
-		fatal("servod: Failed to mmap for cached pages: %m\n");
-	if ((unsigned long)virtcached & (PAGE_SIZE-1))
-		fatal("servod: Virtual address is not page aligned\n");
-	memset(virtcached, 0, num_pages * PAGE_SIZE);
 
-	virtbase = mmap(NULL, num_pages * PAGE_SIZE, PROT_READ|PROT_WRITE,
+	turnoff_mask = (uint32_t *)mbox.virt_addr;
-			MAP_SHARED|MAP_ANONYMOUS|MAP_NORESERVE|MAP_LOCKED,
+	turnon_mask = (uint32_t *)(mbox.virt_addr + num_samples * sizeof(uint32_t));
-			-1, 0);
+	cb_base = (dma_cb_t *)(mbox.virt_addr +
-	if (virtbase == MAP_FAILED)
-		fatal("servod: Failed to mmap uncached pages: %m\n");
-	if ((unsigned long)virtbase & (PAGE_SIZE-1))
-		fatal("servod: Virtual address is not page aligned\n");
-	munmap(virtbase, num_pages * PAGE_SIZE);
-
-	make_pagemap();
-
-	/*
-	 * Now the memory is all mapped, we can set up the pointers to the
-	 * bit masks used to turn outputs on and off, and to the DMA control
-	 * blocks.  The control blocks must be 32 byte aligned (so round up
-	 * to multiple of 8, as we're then multiplying by 4).
-	 */
-	turnoff_mask = (uint32_t *)virtbase;
-	turnon_mask = (uint32_t *)(virtbase + num_samples * sizeof(uint32_t));
-	cb_base = (dma_cb_t *)(virtbase +
 		ROUNDUP(num_samples + MAX_SERVOS, 8) * sizeof(uint32_t));
 
 	for (i = 0; i < MAX_SERVOS; i++) {