rooty/PiBits
1
0
Fork 0
mirror of https://github.com/richardghirst/PiBits.git synced 2024-11-28 12:24:11 +01:00
Code Issues Releases Activity

Major update to servod.c to handle up to 21 servos and to allow pulse widths between 0 and 100%

Browse Source
This commit is contained in:
Richard Hirst 2013-09-11 22:00:00 +01:00
parent 071fc182bc
commit 29ce55de75
2 changed files with 597 additions and 159 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

188
ServoBlaster/README.txt
View File

@ -6,17 +6,22 @@ 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.
By default is it configured to drive 8 servos, although you can configure it to
drive up to 21. 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.
In addition to driving servos, ServoBlaster can be configured to generate pulse
widths between 0 and 100% of the 20ms cycle time, making it suitable for
controling the brightness of up to 21 LEDs, for example.
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:
number is by default 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
@ -25,8 +30,8 @@ echo 3=120 > /dev/servoblaster
If you set a servo width to 0 it turns off the servo output, without changing
the current servo position.
The code supports 8 servos, the control signals of which should be connected
to P1 header pins as follows:
The code defaults to driving 8 servos, the control signals of which should be
connected to P1 header pins as follows:
Servo number GPIO number Pin in P1 header
0 4 P1-7
@ -40,20 +45,22 @@ to P1 header pins as follows:
P1-13 is connected to either GPIO-21 or GPIO-27, depending on board revision.
Command line options described later in this document allow you to configure
which header pins map to which servo numbers.
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
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.
cleanly and revert the GPIO pins to their original configuration, 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 takes note of how many servos you have configured and distributes
the start time for the servo pulses evenly across the 20ms cycle time. This
way, provided you are not driving more than 8 servos, the driver ensures that
only one servo pulse will be active at a time, which should help minimise total
drive current needed.
In the following description it refers to using the PWM peripheral. For the
user space implementation it can instead use the PCM peripheral, see below
@ -63,19 +70,14 @@ uses the PWM peripheral, so ServoBlaster can interfere with sound output.
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.
when a pulse width needs to be changed. For a given 10us period there are two DMA
control blocks; the first transfers a single word to the GPIO 'clear output'
register, while the second transfers some number of words to the PWM FIFO to
generate the required pulse width time. In addition, interspersed with these
control blocks is one for each configured servo which is used to set an output.
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.
transfers, so as to generate accurate delays.
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
@ -93,15 +95,15 @@ 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!
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, 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
daemon implementation is much more convenient to use and now has rather more
features than 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
@ -117,43 +119,118 @@ The user space daemon
To use this daemon grab the servod.c source and Makefile and:
$ make servod
$ ./servod --help
Usage: ./servod <options>
Options:
--pcm tells servod to use PCM rather than PWM hardware
to implement delays
--idle-timeout=N tells servod to stop sending servo pulses for a
given output N milliseconds after the last update
--min=N specifies the minimum allowed pulse width, default
50 or 500us
--max=N specifies the maximum allowed pulse width, default
250 or 2500us
--invert Inverts outputs
--p1pins=<list> tells servod which pins on the P1 header to use
--p5pins=<list> tells servod which pins on the P5 header to use
where <list> defaults to "7,11,12,13,15,16,18,22" for p1pins and
"" for p5pins. p5pins is only valid on rev 2 boards.
$ sudo ./servod
Using hardware: PWM
Number of servos: 8
Servo cycle time: 20000us
Pulse width units: 10us
Maximum width value: 249 (2490us)
$
Board revision: 1
Using hardware: PWM
Idle timeout: Disabled
Number of servos: 8
Servo cycle time: 20000us
Pulse width units: 10us
Minimum width value: 50 (500us)
Maximum width value: 250 (2500us)
Output levels: Normal
Using P1 pins: 7,11,12,13,15,16,18,22
Servo mapping:
0 on P1-7 GPIO-4
1 on P1-11 GPIO-17
2 on P1-12 GPIO-18
3 on P1-13 GPIO-21
4 on P1-15 GPIO-22
5 on P1-16 GPIO-23
6 on P1-18 GPIO-24
7 on P1-22 GPIO-25
$
The prompt will return immediately, and servod is left running in the
background. You can check it is running via the "ps ax" command.
If you want to stop servod, the easiest way is to run:
background. You can check it is running via the "ps ax" command. If you want
to stop servod, the easiest way is to run:
$ sudo killall servod
Note that use of PWM will interfere with 3.5mm jack audio output. Instead
of using the PWM hardware, you can use the PCM hardware, which is less likely
to cause a conflict. Please be aware that the PCM mode is very lightly tested
at present. To use PCM mode, invoke servod as follows:
$ sudo ./servod --pcm
Using hardware: PCM
...
at present.
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:
$ sudo ./servod --idle-timeout=2000
module load time. The value is specified in milliseconds.
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
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.
idle-timeout defaults to 0, which disables the feature.
By default ServoBlaster attempts to protect your servos by setting minimum and
maximum values on the pulse width of 50 and 250 (500us and 2.5ms). If you want
to generate pulse widths up to 100% for some other purpose, you need to specify
the minimum and maximum values you want (probably as 0 and 2000, for 0ms and
20ms).
If you are connecting some external drive circuitry you may want active low
rather than active high outputs. In that case you can specify an option to
invert the outputs.
The final options relate to which header pins you want to use to drive your
servos. On a Rev 1 board you can use up to 17 pins on the P1 header, and on a
Rev 2 board there are an additional 4 pins available on the P5 header. The
default option is the equivalent of specifying
--p1pins=7,11,12,13,15,16,18,22
As anotehr exmple, if for some reason you want only two servos but you want
them to be referenced as servos 4 and 5 (perhaps you have existing software
that uses those servo numbers), you can use '0' as a placeholder for unused
servo IDs, as follows:
$ sudo ./servod --p1pins=0,0,0,0,15,16
...
Using P1 pins: 0,0,0,0,15,16
Servo mapping:
4 on P1-15 GPIO-22
5 on P1-16 GPIO-23
If you specify both --p1pins and --p5pins, then the order in which you specify
them is relevant because servo numbers are allocated in the order the
parameters are specified on the command line.
For the full set of P1 and P5 header pins you can use, please refer to the
GPIO section of the following web page:
http://elinux.org/Rpi_Low-level_peripherals
It should also be clear from the servod.c source which header pins ServoBlaster
will allow you to select. Clearly if you tell ServoBlaster to use pins that
are normally used for some other purpose, then that other functionality will
not be available while servod is running.
If you want servod to start automatically when the system boots, then you
can install it along with a startup script as follows:
@ -161,7 +238,8 @@ 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).
specified in that script (e.g. the idle-timeout, which is set to 2 seconds in
the shipped version of that script).
@ -171,6 +249,10 @@ 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.
The kernel implementation is missing most of the command line options available
in the user space implementation, and always uses the default set of 8 pins
described above.
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:

568
ServoBlaster/user/servod.c
View File

@ -36,36 +36,110 @@
#include <sys/mman.h>
#include <getopt.h>
/* 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.
/* 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:
* http://elinux.org/Rpi_Low-level_peripherals
*
* Which P1 header pins are actually used can be overridden via command line
* parameter '--p1pins=...'.
*/
static uint8_t servo2gpio_rev1[] = {
4, // P1-7
17, // P1-11
18, // P1-12
21, // P1-13
22, // P1-15
23, // P1-16
24, // P1-18
25, // P1-22
static char *default_p1_pins = "7,11,12,13,15,16,18,22";
static char *default_p5_pins = "";
#define DMY 255 // Used to represent an invalid P1 pin, or unmapped servo
#define NUM_P1PINS 26
#define NUM_P5PINS 8
#define MAX_SERVOS 21 // Count of GPIO pins on P1 and P5
static uint8_t rev1_p1pin2gpio_map[] = {
DMY, // P1-1 3v3
DMY, // P1-2 5v
0, // P1-3 GPIO 0 (SDA)
DMY, // P1-4 5v
1, // P1-5 GPIO 1 (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)
21, // P1-13 GPIO 21
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)
};
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 rev1_p5pin2gpio_map[] = {
DMY, // (P5-1 on rev 2 boards)
DMY, // (P5-2 on rev 2 boards)
DMY, // (P5-3 on rev 2 boards)
DMY, // (P5-4 on rev 2 boards)
DMY, // (P5-5 on rev 2 boards)
DMY, // (P5-6 on rev 2 boards)
DMY, // (P5-7 on rev 2 boards)
DMY, // (P5-8 on rev 2 boards)
};
static uint8_t *servo2gpio;
static uint8_t rev2_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)
};
static uint8_t rev2_p5pin2gpio_map[] = {
DMY, // P5-1 5v0
DMY, // P5-2 3v3
28, // P5-3 GPIO 28 (I2C0_SDA)
29, // P5-4 GPIO 29 (I2C0_SCL)
30, // P5-5 GPIO 30
31, // P5-6 GPIO 31
DMY, // P5-7 Ground
DMY, // P5-8 Ground
};
static uint8_t servo2gpio[MAX_SERVOS];
static int servostart[MAX_SERVOS];
static int servowidth[MAX_SERVOS];
static int num_servos;
static uint32_t gpiomode[MAX_SERVOS];
static int restore_gpio_modes;
#define DEVFILE "/dev/servoblaster"
@ -82,14 +156,13 @@ static int num_servos;
#define CYCLE_TIME_US 20000
#define SAMPLE_US 10
#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)
#define DEFAULT_MIN (500/SAMPLE_US)
#define DEFAULT_MAX (2500/SAMPLE_US)
#define NUM_SAMPLES (CYCLE_TIME_US/SAMPLE_US)
#define NUM_CBS (NUM_SAMPLES*2)
#define NUM_CBS (NUM_SAMPLES*2+MAX_SERVOS)
#define NUM_PAGES ((NUM_CBS * 32 + NUM_SAMPLES * 4 + \
MAX_SERVOS * 4 + \
PAGE_SIZE - 1) >> PAGE_SHIFT)
#define DMA_BASE 0x20007000
@ -157,24 +230,27 @@ static int num_servos;
#define DELAY_VIA_PWM 0
#define DELAY_VIA_PCM 1
#define ROUNDUP(val, blksz) (((val)+((blksz)-1)) & ~(blksz-1))
typedef struct {
uint32_t info, src, dst, length,
stride, next, pad[2];
} dma_cb_t;
struct ctl {
uint32_t sample[NUM_SAMPLES];
uint32_t turnoff[NUM_SAMPLES];
uint32_t turnon[ROUNDUP(MAX_SERVOS,8)];
dma_cb_t cb[NUM_CBS];
};
typedef struct {
uint8_t *virtaddr;
uint32_t physaddr;
} page_map_t;
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;
@ -187,8 +263,12 @@ static int delay_hw = DELAY_VIA_PWM;
static struct timeval *servo_kill_time;
static int idle_timeout = 0;
static int invert = 0;
static int servo_min = DEFAULT_MIN;
static int servo_max = DEFAULT_MAX;
static void set_servo(int servo, int width);
static void gpio_set_mode(uint32_t gpio, uint32_t mode);
static void
udelay(int us)
@ -204,12 +284,20 @@ terminate(int dummy)
int i;
if (dma_reg && virtbase) {
for (i = 0; i < num_servos; i++)
set_servo(i, 0);
for (i = 0; i < MAX_SERVOS; i++) {
if (servo2gpio[i] != DMY)
set_servo(i, 0);
}
udelay(CYCLE_TIME_US);
dma_reg[DMA_CS] = DMA_RESET;
udelay(10);
}
if (restore_gpio_modes) {
for (i = 0; i < MAX_SERVOS; i++) {
if (servo2gpio[i] != DMY)
gpio_set_mode(servo2gpio[i], gpiomode[i]);
}
}
unlink(DEVFILE);
exit(1);
}
@ -228,7 +316,7 @@ fatal(char *fmt, ...)
static void
init_idle_timers(void)
{
servo_kill_time = calloc(num_servos, sizeof(struct timeval));
servo_kill_time = calloc(MAX_SERVOS, sizeof(struct timeval));
if (!servo_kill_time)
fatal("servod: calloc() failed\n");
}
@ -257,8 +345,8 @@ get_next_idle_timeout(struct timeval *tv)
long this_diff, min_diff;
gettimeofday(&now, NULL);
for (i = 0; i < num_servos; i++) {
if (servo_kill_time[i].tv_sec == 0)
for (i = 0; i < MAX_SERVOS; i++) {
if (servo2gpio[i] == DMY || 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 &&
@ -278,23 +366,30 @@ get_next_idle_timeout(struct timeval *tv)
*tv = min;
}
static void
gpio_set_mode(uint32_t pin, uint32_t mode)
static uint32_t gpio_get_mode(uint32_t gpio)
{
uint32_t fsel = gpio_reg[GPIO_FSEL0 + pin/10];
uint32_t fsel = gpio_reg[GPIO_FSEL0 + gpio/10];
fsel &= ~(7 << ((pin % 10) * 3));
fsel |= mode << ((pin % 10) * 3);
gpio_reg[GPIO_FSEL0 + pin/10] = fsel;
return (fsel >> ((gpio % 10) * 3)) & 7;
}
static void
gpio_set(int pin, int level)
gpio_set_mode(uint32_t gpio, uint32_t mode)
{
uint32_t fsel = gpio_reg[GPIO_FSEL0 + gpio/10];
fsel &= ~(7 << ((gpio % 10) * 3));
fsel |= mode << ((gpio % 10) * 3);
gpio_reg[GPIO_FSEL0 + gpio/10] = fsel;
}
static void
gpio_set(int gpio, int level)
{
if (level)
gpio_reg[GPIO_SET0] = 1 << pin;
gpio_reg[GPIO_SET0] = 1 << gpio;
else
gpio_reg[GPIO_CLR0] = 1 << pin;
gpio_reg[GPIO_CLR0] = 1 << gpio;
}
static uint32_t
@ -321,28 +416,51 @@ map_peripheral(uint32_t base, uint32_t len)
return vaddr;
}
/* 'servo' has already been validated as to refer to a mapped servo */
static void
set_servo(int servo, int width)
{
struct ctl *ctl = (struct ctl *)virtbase;
dma_cb_t *cbp = ctl->cb + servo * SERVO_SAMPLES * 2;
uint32_t phys_gpclr0 = 0x7e200000 + 0x28;
uint32_t phys_gpset0 = 0x7e200000 + 0x1c;
uint32_t *dp = ctl->sample + servo * SERVO_SAMPLES;
volatile uint32_t *dp;
int i;
uint32_t mask = 1 << servo2gpio[servo];
dp[width] = mask;
if (width == servowidth[servo])
return;
if (width == 0) {
cbp->dst = phys_gpclr0;
if (width == 0)
ctl->turnon[servo] = 0;
if (width > servowidth[servo]) {
dp = ctl->turnoff + servostart[servo] + width;
if (dp >= ctl->turnoff + NUM_SAMPLES)
dp -= NUM_SAMPLES;
for (i = width; i > servowidth[servo]; i--) {
dp--;
if (dp < ctl->turnoff)
dp = ctl->turnoff + NUM_SAMPLES - 1;
//printf("%5d, clearing at %p\n", dp - ctl->turnoff, dp);
*dp &= ~mask;
}
} else {
for (i = width - 1; i > 0; i--)
dp[i] = 0;
dp[0] = mask;
cbp->dst = phys_gpset0;
update_idle_time(servo);
dp = ctl->turnoff + servostart[servo] + width;
if (dp >= ctl->turnoff + NUM_SAMPLES)
dp -= NUM_SAMPLES;
for (i = width; i < servowidth[servo]; i++) {
//printf("%5d, setting at %p\n", dp - ctl->turnoff, dp);
*dp++ |= mask;
if (dp >= ctl->turnoff + NUM_SAMPLES)
dp = ctl->turnoff;
}
}
servowidth[servo] = width;
if (width)
ctl->turnon[servo] = mask;
update_idle_time(servo);
}
static void
@ -362,23 +480,28 @@ make_pagemap(void)
fd = open(pagemap_fn, O_RDONLY);
if (fd < 0)
fatal("servod: Failed to open %s: %m\n", pagemap_fn);
if (lseek(fd, (uint32_t)virtbase >> 9, SEEK_SET) !=
(uint32_t)virtbase >> 9) {
if (lseek(fd, (uint32_t)virtcached >> 9, SEEK_SET) !=
(uint32_t)virtcached >> 9) {
fatal("servod: Failed to seek on %s: %m\n", pagemap_fn);
}
for (i = 0; i < NUM_PAGES; i++) {
uint64_t pfn;
page_map[i].virtaddr = virtbase + i * PAGE_SIZE;
// Following line forces page to be allocated
page_map[i].virtaddr[0] = 0;
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
@ -402,34 +525,74 @@ init_ctrl_data(void)
{
struct ctl *ctl = (struct ctl *)virtbase;
dma_cb_t *cbp = ctl->cb;
uint32_t phys_fifo_addr;
uint32_t phys_gpclr0 = 0x7e200000 + 0x28;
int servo, i;
uint32_t phys_fifo_addr, cbinfo;
uint32_t phys_gpclr0;
uint32_t phys_gpset0;
int servo, i, numservos = 0, curstart = 0;
uint32_t maskall = 0;
if (delay_hw == DELAY_VIA_PWM)
phys_fifo_addr = (PWM_BASE | 0x7e000000) + 0x18;
else
phys_fifo_addr = (PCM_BASE | 0x7e000000) + 0x04;
memset(ctl->sample, 0, sizeof(ctl->sample));
for (servo = 0 ; servo < num_servos; servo++) {
for (i = 0; i < SERVO_SAMPLES; i++)
ctl->sample[servo * SERVO_SAMPLES + i] = 1 << servo2gpio[servo];
if (invert) {
phys_gpclr0 = 0x7e200000 + 0x1c;
phys_gpset0 = 0x7e200000 + 0x28;
} else {
phys_gpclr0 = 0x7e200000 + 0x28;
phys_gpset0 = 0x7e200000 + 0x1c;
}
if (delay_hw == DELAY_VIA_PWM) {
phys_fifo_addr = (PWM_BASE | 0x7e000000) + 0x18;
cbinfo = DMA_NO_WIDE_BURSTS | DMA_WAIT_RESP | DMA_D_DREQ | DMA_PER_MAP(5);
} else {
phys_fifo_addr = (PCM_BASE | 0x7e000000) + 0x04;
cbinfo = DMA_NO_WIDE_BURSTS | DMA_WAIT_RESP | DMA_D_DREQ | DMA_PER_MAP(2);
}
memset(ctl->turnon, 0, sizeof(ctl->turnon));
for (servo = 0 ; servo < MAX_SERVOS; servo++) {
servowidth[servo] = 0;
if (servo2gpio[servo] != DMY) {
numservos++;
maskall |= 1 << servo2gpio[servo];
}
}
for (i = 0; i < NUM_SAMPLES; i++)
ctl->turnoff[i] = maskall;
for (servo = 0; servo < MAX_SERVOS; servo++) {
if (servo2gpio[servo] != DMY) {
servostart[servo] = curstart;
curstart += NUM_SAMPLES / num_servos;
}
}
servo = 0;
while (servo < MAX_SERVOS && servo2gpio[servo] == DMY)
servo++;
for (i = 0; i < NUM_SAMPLES; i++) {
cbp->info = DMA_NO_WIDE_BURSTS | DMA_WAIT_RESP;
cbp->src = mem_virt_to_phys(ctl->sample + i);
cbp->src = mem_virt_to_phys(ctl->turnoff + i);
cbp->dst = phys_gpclr0;
cbp->length = 4;
cbp->stride = 0;
cbp->next = mem_virt_to_phys(cbp + 1);
cbp++;
if (i == servostart[servo]) {
cbp->info = DMA_NO_WIDE_BURSTS | DMA_WAIT_RESP;
cbp->src = mem_virt_to_phys(ctl->turnon + servo);
cbp->dst = phys_gpset0;
cbp->length = 4;
cbp->stride = 0;
cbp->next = mem_virt_to_phys(cbp + 1);
cbp++;
servo++;
while (servo < MAX_SERVOS && servo2gpio[servo] == DMY)
servo++;
}
// Delay
if (delay_hw == DELAY_VIA_PWM)
cbp->info = DMA_NO_WIDE_BURSTS | DMA_WAIT_RESP | DMA_D_DREQ | DMA_PER_MAP(5);
else
cbp->info = DMA_NO_WIDE_BURSTS | DMA_WAIT_RESP | DMA_D_DREQ | DMA_PER_MAP(2);
cbp->info = cbinfo;
cbp->src = mem_virt_to_phys(ctl); // Any data will do
cbp->dst = phys_fifo_addr;
cbp->length = 4;
@ -499,6 +662,30 @@ init_hardware(void)
}
}
static void
do_debug(void)
{
int i;
uint32_t mask = 0;
uint32_t last = 0xffffffff;
struct ctl *ctl = (struct ctl *)virtbase;
printf("Servo Start Width\n");
for (i = 0; i < MAX_SERVOS; i++) {
if (servo2gpio[i] != DMY) {
printf("%3d: %6d %6d\n", i, servostart[i], servowidth[i]);
mask |= 1 << servo2gpio[i];
}
}
printf("\nData:\n");
for (i = 0; i < NUM_SAMPLES; i++) {
uint32_t curr = ctl->turnoff[i] & mask;
if (curr != last)
printf("%5d: %08x\n", i, curr);
last = curr;
}
}
static void
go_go_go(void)
{
@ -525,11 +712,15 @@ go_go_go(void)
line[++nchars] = '\0';
nchars = 0;
n = sscanf(line, "%d=%d%c", &servo, &width, &nl);
if (n !=3 || nl != '\n') {
if (!strcmp(line, "debug\n")) {
do_debug();
} else if (n !=3 || nl != '\n') {
fprintf(stderr, "Bad input: %s", line);
} else if (servo < 0 || servo >= num_servos) {
} else if (servo < 0 || servo >= MAX_SERVOS) {
fprintf(stderr, "Invalid servo number %d\n", servo);
} else if (width < SERVO_MIN || width > SERVO_MAX) {
} else if (servo2gpio[servo] == DMY) {
fprintf(stderr, "Servo %d is not mapped to a GPIO pin\n", servo);
} else if (width && (width < servo_min || width > servo_max)) {
fprintf(stderr, "Invalid width %d\n", width);
} else {
set_servo(servo, width);
@ -588,10 +779,103 @@ board_rev(void)
return rev;
}
static void
parse_pin_lists(int p1first, char *p1pins, char*p5pins)
{
char *name, *pins;
int mapcnt;
uint8_t *map;
int lst, servo = 0;
memset(servo2gpio, DMY, sizeof(servo2gpio));
for (lst = 0; lst < 2; lst++) {
if (lst == 0 && p1first) {
name = "P1";
pins = p1pins;
if (board_rev() == 1) {
map = rev1_p1pin2gpio_map;
mapcnt = sizeof(rev1_p1pin2gpio_map);
} else {
map = rev2_p1pin2gpio_map;
mapcnt = sizeof(rev2_p1pin2gpio_map);
}
} else {
name = "P5";
pins = p5pins;
if (board_rev() == 1) {
map = rev1_p5pin2gpio_map;
mapcnt = sizeof(rev1_p5pin2gpio_map);
} else {
map = rev2_p5pin2gpio_map;
mapcnt = sizeof(rev2_p5pin2gpio_map);
}
}
while (*pins) {
char *end;
long pin = strtol(pins, &end, 0);
if (*end && (end == pins || *end != ','))
fatal("Invalid character '%c' in %s pin list\n", *end, name);
if (pin < 0 || pin > mapcnt)
fatal("Invalid pin number %d in %s pin list\n", pin, name);
if (servo == MAX_SERVOS)
fatal("Too many servos specified\n");
if (pin == 0) {
servo++;
} else {
if (map[pin-1] == DMY)
fatal("Pin %d on header %s cannot be used for a servo output\n", pin, name);
servo2gpio[servo++] = map[pin-1];
num_servos++;
}
pins = end;
if (*pins == ',')
pins++;
}
}
}
static uint8_t gpiosearch(uint8_t gpio, uint8_t *map, int len)
{
while (--len) {
if (map[len] == gpio)
return len+1;
}
return 0;
}
static char *
gpio2pinname(uint8_t gpio)
{
static char res[16];
uint8_t pin;
if (board_rev() == 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 {
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);
}
return res;
}
int
main(int argc, char **argv)
{
int i;
char *p1pins = default_p1_pins;
char *p5pins = default_p5_pins;
int p1first = 1, hadp1 = 0, hadp5 = 0;
while (1) {
int c;
@ -600,12 +884,16 @@ main(int argc, char **argv)
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' },
{ "p1pins", required_argument, 0, '1' },
{ "p5pins", required_argument, 0, '5' },
{ "min", required_argument, 0, 'm' },
{ "max", required_argument, 0, 'x' },
{ "invert", no_argument, 0, 'i' },
{ 0, 0, 0, 0 }
};
c = getopt_long(argc, argv, "hpt:", long_options, &option_index);
c = getopt_long(argc, argv, "mxhnt:15i", long_options, &option_index);
if (c == -1) {
break;
} else if (c == 'p') {
@ -615,36 +903,77 @@ main(int argc, char **argv)
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"
printf("\nUsage: %s <options>\n\n"
"Options:\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]);
" given output N milliseconds after the last update\n"
" --min=N specifies the minimum allowed pulse width, default\n"
" %d or %dus\n"
" --max=N specifies the maximum allowed pulse width, default\n"
" %d or %dus\n"
" --invert Inverts outputs\n"
" --p1pins=<list> tells servod which pins on the P1 header to use\n"
" --p5pins=<list> tells servod which pins on the P5 header to use\n"
"\nwhere <list> defaults to \"%s\" for p1pins and\n"
"\"%s\" for p5pins. p5pins is only valid on rev 2 boards.\n\n",
argv[0],
DEFAULT_MIN, DEFAULT_MIN * SAMPLE_US,
DEFAULT_MAX, DEFAULT_MAX * SAMPLE_US,
default_p1_pins, default_p5_pins);
exit(0);
} else if (c == '1') {
p1pins = optarg;
hadp1 = 1;
if (!hadp5)
p1first = 1;
} else if (c == '5') {
p5pins = optarg;
hadp5 = 1;
if (!hadp1)
p1first = 0;
} else if (c == 'm') {
servo_min = atoi(optarg);
} else if (c == 'x') {
servo_max = atoi(optarg);
} else if (c == 'i') {
invert = 1;
} else {
fatal("Invalid parameter\n");
}
}
if (servo_min < 0 || servo_max > NUM_SAMPLES || servo_min >= servo_max)
fatal("Invalid min and/or max values, min=%d, max=%d\n", servo_min, servo_max);
if (board_rev() == 1 && p5pins[0])
fatal("Board rev 1 does not have a P5 header\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);
parse_pin_lists(p1first, p1pins, p5pins);
printf("\nBoard revision: %5d\n", board_rev());
printf("Using hardware: %s\n", delay_hw == DELAY_VIA_PWM ? "PWM" : "PCM");
if (idle_timeout)
printf("Idle timeout: %5dms\n", idle_timeout);
else
printf("Idle timeout: Disabled\n");
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("Minimum width value: %5d (%dus)\n", servo_min,
servo_min * SAMPLE_US);
printf("Maximum width value: %5d (%dus)\n", servo_max,
servo_max * SAMPLE_US);
printf("Output levels: %s\n", invert ? "Inverted" : " Normal");
printf("\nUsing P1 pins: %s\n", p1pins);
if (board_rev() > 1)
printf("Using P5 pins: %s\n", p5pins);
printf("\nServo mapping:\n");
for (i = 0; i < MAX_SERVOS; i++) {
if (servo2gpio[i] == DMY)
continue;
printf(" %2d on %-5s GPIO-%d\n", i, gpio2pinname(servo2gpio[i]), servo2gpio[i]);
}
printf("Board revision: %5d\n", board_rev());
printf("Using hardware: %5s\n", delay_hw == DELAY_VIA_PWM ? "PWM" : "PCM");
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);
printf("\n");
init_idle_timers();
setup_sighandlers();
@ -655,20 +984,47 @@ main(int argc, char **argv)
clk_reg = map_peripheral(CLK_BASE, CLK_LEN);
gpio_reg = map_peripheral(GPIO_BASE, GPIO_LEN);
/*
* Map the pages to our vitual address space; this reserves them and
* locks them in memory. However, these are L1 & L2 non-coherent
* cached pages and we want coherent access to them so the DMA
* controller sees our changes immediately. To get that, we create a
* second mapping of the same size and immediately free it. This gives
* us an address in our virtual address space where we can map in a
* coherent view of the physical pages that were allocated by the first
* mmap(). This coherent mapping happens in make_pagemap(). All
* accesses to our memory that is shared with the DMA controller are
* via this second coherent mapping. The memset() below forces the
* pages to be allocated.
*/
virtcached = mmap(NULL, NUM_PAGES * PAGE_SIZE, PROT_READ|PROT_WRITE,
MAP_SHARED|MAP_ANONYMOUS|MAP_NORESERVE|MAP_LOCKED,
-1, 0);
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,
MAP_SHARED|MAP_ANONYMOUS|MAP_NORESERVE|MAP_LOCKED,
-1, 0);
if (virtbase == MAP_FAILED)
fatal("servod: Failed to mmap physical pages: %m\n");
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();
for (i = 0; i < num_servos; i++) {
gpio_set(servo2gpio[i], 0);
for (i = 0; i < MAX_SERVOS; i++) {
if (servo2gpio[i] == DMY)
continue;
gpiomode[i] = gpio_get_mode(servo2gpio[i]);
gpio_set(servo2gpio[i], invert ? 1 : 0);
gpio_set_mode(servo2gpio[i], GPIO_MODE_OUT);
}
restore_gpio_modes = 1;
init_ctrl_data();
init_hardware();