rooty/PiBits
1
0
Fork 0
mirror of https://github.com/richardghirst/PiBits.git synced 2025-02-26 19:54:16 +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

186
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 use. The driver maintains that pulse width until you send a new command
requesting some other width. requesting some other width.
Currently is it configured to drive 8 servos. Servos typically need an active By default is it configured to drive 8 servos, although you can configure it to
high pulse of somewhere between 0.5ms and 2.5ms, where the pulse width controls drive up to 21. Servos typically need an active high pulse of somewhere
the position of the servo. The pulse should be repeated approximately every between 0.5ms and 2.5ms, where the pulse width controls the position of the
20ms, although pulse frequency is not critical. The pulse width is critical, servo. The pulse should be repeated approximately every 20ms, although pulse
as that translates directly to the servo position. 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 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 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 number is by default a number from 0 to 7 inclusive, and servo position is the
you want in units of 10us. So, if you want to set servo 3 to a pulse width of pulse width you want in units of 10us. So, if you want to set servo 3 to a
1.2ms you could do this at the shell prompt: pulse width of 1.2ms you could do this at the shell prompt:
echo 3=120 > /dev/servoblaster 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 If you set a servo width to 0 it turns off the servo output, without changing
the current servo position. the current servo position.
The code supports 8 servos, the control signals of which should be connected The code defaults to driving 8 servos, the control signals of which should be
to P1 header pins as follows: connected to P1 header pins as follows:
Servo number GPIO number Pin in P1 header Servo number GPIO number Pin in P1 header
0 4 P1-7 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. 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 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 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 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 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 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 cleanly and revert the GPIO pins to their original configuration, rather than
jump. 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 The driver takes note of how many servos you have configured and distributes
a cycle time of 20ms). A servo output is set high at the start of its 2.5ms the start time for the servo pulses evenly across the 20ms cycle time. This
timeslot, and set low after the appropriate delay. There is then a further way, provided you are not driving more than 8 servos, the driver ensures that
delay to take us to the end of that timeslot before the next servo output is only one servo pulse will be active at a time, which should help minimise total
set high. This way there is only ever one servo output active at a time, which drive current needed.
helps keep the code simple.
In the following description it refers to using the PWM peripheral. For the 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 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 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 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 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 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 'set output' control blocks; the first transfers a single word to the GPIO 'clear output'
register, the second transfers some number of words to the PWM FIFO to generate register, while the second transfers some number of words to the PWM FIFO to
the required pulse width time, the third transfers a single word to the GPIO generate the required pulse width time. In addition, interspersed with these
'clear output' register, and the fourth transfers a number of words to the PWM control blocks is one for each configured servo which is used to set an output.
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 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 transfers, so as to generate accurate delays.
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 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 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 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 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 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 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 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 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 daemon implementation is much more convenient to use and now has rather more
features of the kernel based one. I would recommend you try the user space 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. implementation first, as it is likely to be easier to get going.
The kernel module implementation is in the subdirectory 'kernel', while the 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: To use this daemon grab the servod.c source and Makefile and:
$ make servod $ 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 $ sudo ./servod
Using hardware: PWM
Number of servos: 8 Board revision: 1
Servo cycle time: 20000us Using hardware: PWM
Pulse width units: 10us Idle timeout: Disabled
Maximum width value: 249 (2490us) 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 The prompt will return immediately, and servod is left running in the
background. You can check it is running via the "ps ax" command. background. You can check it is running via the "ps ax" command. If you want
If you want to stop servod, the easiest way is to run: to stop servod, the easiest way is to run:
$ sudo killall servod $ sudo killall servod
Note that use of PWM will interfere with 3.5mm jack audio output. Instead 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 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 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: at present.
$ sudo ./servod --pcm
Using hardware: PCM
...
Some people have requested that a servo output turns off automatically if no 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 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 servos overheating when driven for long periods of time. To support this
request, servod implements an idle timeout which can be specified at request, servod implements an idle timeout which can be specified at
module load time. The value is specified in milliseconds, so if you want module load time. The value is specified in milliseconds.
to drive your servos for 2 seconds following each new width request you would
do this:
$ sudo ./servod --idle-timeout=2000
Typical small servos take a few 100 milliseconds to rotate from one extreme 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. 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 If you want servod to start automatically when the system boots, then you
can install it along with a startup script as follows: 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 $ sudo make install
You may wish to edit /etc/init.d/servoblaster to change the parameters that are 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 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 (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 Upon reading /dev/servoblaster, the device file provides feedback as to what
position each servo is currently set. For example, after starting the driver position each servo is currently set. For example, after starting the driver
and sending the command "3=120", you would see: 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 <sys/mman.h>
#include <getopt.h> #include <getopt.h>
/* Define which GPIOs/P1-pins to use slightly differently for Rev 1 and /* Define which P1 header pins to use by default. These are the eight standard
* Rev 2 boards, as P1-13 is different. On Rev 1 GPIO 27 was used for * GPIO pins (those coloured green in the diagram on this page:
* camera control and GPIO-21 was routed to P1-13, but on Rev 2 it is the * http://elinux.org/Rpi_Low-level_peripherals
* other way round. *
* Which P1 header pins are actually used can be overridden via command line
* parameter '--p1pins=...'.
*/ */
static uint8_t servo2gpio_rev1[] = { static char *default_p1_pins = "7,11,12,13,15,16,18,22";
4, // P1-7 static char *default_p5_pins = "";
17, // P1-11
18, // P1-12 #define DMY 255 // Used to represent an invalid P1 pin, or unmapped servo
21, // P1-13
22, // P1-15 #define NUM_P1PINS 26
23, // P1-16 #define NUM_P5PINS 8
24, // P1-18
25, // P1-22 #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[] = { static uint8_t rev1_p5pin2gpio_map[] = {
4, // P1-7 DMY, // (P5-1 on rev 2 boards)
17, // P1-11 DMY, // (P5-2 on rev 2 boards)
18, // P1-12 DMY, // (P5-3 on rev 2 boards)
27, // P1-13 DMY, // (P5-4 on rev 2 boards)
22, // P1-15 DMY, // (P5-5 on rev 2 boards)
23, // P1-16 DMY, // (P5-6 on rev 2 boards)
24, // P1-18 DMY, // (P5-7 on rev 2 boards)
25, // P1-22 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 int num_servos;
static uint32_t gpiomode[MAX_SERVOS];
static int restore_gpio_modes;
#define DEVFILE "/dev/servoblaster" #define DEVFILE "/dev/servoblaster"
@ -82,14 +156,13 @@ static int num_servos;
#define CYCLE_TIME_US 20000 #define CYCLE_TIME_US 20000
#define SAMPLE_US 10 #define SAMPLE_US 10
#define SERVO_TIME_US (CYCLE_TIME_US/num_servos) #define DEFAULT_MIN (500/SAMPLE_US)
#define SERVO_SAMPLES (SERVO_TIME_US/SAMPLE_US) #define DEFAULT_MAX (2500/SAMPLE_US)
#define SERVO_MIN 0
#define SERVO_MAX (SERVO_SAMPLES - 1)
#define NUM_SAMPLES (CYCLE_TIME_US/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 + \ #define NUM_PAGES ((NUM_CBS * 32 + NUM_SAMPLES * 4 + \
MAX_SERVOS * 4 + \
PAGE_SIZE - 1) >> PAGE_SHIFT) PAGE_SIZE - 1) >> PAGE_SHIFT)
#define DMA_BASE 0x20007000 #define DMA_BASE 0x20007000
@ -157,24 +230,27 @@ static int num_servos;
#define DELAY_VIA_PWM 0 #define DELAY_VIA_PWM 0
#define DELAY_VIA_PCM 1 #define DELAY_VIA_PCM 1
#define ROUNDUP(val, blksz) (((val)+((blksz)-1)) & ~(blksz-1))
typedef struct { typedef struct {
uint32_t info, src, dst, length, uint32_t info, src, dst, length,
stride, next, pad[2]; stride, next, pad[2];
} dma_cb_t; } dma_cb_t;
struct ctl { 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]; dma_cb_t cb[NUM_CBS];
}; };
typedef struct { typedef struct {
uint8_t *virtaddr;
uint32_t physaddr; uint32_t physaddr;
} page_map_t; } page_map_t;
page_map_t *page_map; page_map_t *page_map;
static uint8_t *virtbase; static uint8_t *virtbase;
static uint8_t *virtcached;
static volatile uint32_t *pwm_reg; static volatile uint32_t *pwm_reg;
static volatile uint32_t *pcm_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 struct timeval *servo_kill_time;
static int idle_timeout = 0; 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 set_servo(int servo, int width);
static void gpio_set_mode(uint32_t gpio, uint32_t mode);
static void static void
udelay(int us) udelay(int us)
@ -204,12 +284,20 @@ terminate(int dummy)
int i; int i;
if (dma_reg && virtbase) { if (dma_reg && virtbase) {
for (i = 0; i < num_servos; i++) for (i = 0; i < MAX_SERVOS; i++) {
set_servo(i, 0); if (servo2gpio[i] != DMY)
set_servo(i, 0);
}
udelay(CYCLE_TIME_US); udelay(CYCLE_TIME_US);
dma_reg[DMA_CS] = DMA_RESET; dma_reg[DMA_CS] = DMA_RESET;
udelay(10); 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); unlink(DEVFILE);
exit(1); exit(1);
} }
@ -228,7 +316,7 @@ fatal(char *fmt, ...)
static void static void
init_idle_timers(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) if (!servo_kill_time)
fatal("servod: calloc() failed\n"); fatal("servod: calloc() failed\n");
} }
@ -257,8 +345,8 @@ get_next_idle_timeout(struct timeval *tv)
long this_diff, min_diff; long this_diff, min_diff;
gettimeofday(&now, NULL); gettimeofday(&now, NULL);
for (i = 0; i < num_servos; i++) { for (i = 0; i < MAX_SERVOS; i++) {
if (servo_kill_time[i].tv_sec == 0) if (servo2gpio[i] == DMY || servo_kill_time[i].tv_sec == 0)
continue; continue;
else if (servo_kill_time[i].tv_sec < now.tv_sec || 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_sec == now.tv_sec &&
@ -278,23 +366,30 @@ get_next_idle_timeout(struct timeval *tv)
*tv = min; *tv = min;
} }
static void static uint32_t gpio_get_mode(uint32_t gpio)
gpio_set_mode(uint32_t pin, uint32_t mode)
{ {
uint32_t fsel = gpio_reg[GPIO_FSEL0 + pin/10]; uint32_t fsel = gpio_reg[GPIO_FSEL0 + gpio/10];
fsel &= ~(7 << ((pin % 10) * 3)); return (fsel >> ((gpio % 10) * 3)) & 7;
fsel |= mode << ((pin % 10) * 3);
gpio_reg[GPIO_FSEL0 + pin/10] = fsel;
} }
static void 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) if (level)
gpio_reg[GPIO_SET0] = 1 << pin; gpio_reg[GPIO_SET0] = 1 << gpio;
else else
gpio_reg[GPIO_CLR0] = 1 << pin; gpio_reg[GPIO_CLR0] = 1 << gpio;
} }
static uint32_t static uint32_t
@ -321,28 +416,51 @@ map_peripheral(uint32_t base, uint32_t len)
return vaddr; return vaddr;
} }
/* 'servo' has already been validated as to refer to a mapped servo */
static void static void
set_servo(int servo, int width) set_servo(int servo, int width)
{ {
struct ctl *ctl = (struct ctl *)virtbase; struct ctl *ctl = (struct ctl *)virtbase;
dma_cb_t *cbp = ctl->cb + servo * SERVO_SAMPLES * 2; volatile uint32_t *dp;
uint32_t phys_gpclr0 = 0x7e200000 + 0x28;
uint32_t phys_gpset0 = 0x7e200000 + 0x1c;
uint32_t *dp = ctl->sample + servo * SERVO_SAMPLES;
int i; int i;
uint32_t mask = 1 << servo2gpio[servo]; uint32_t mask = 1 << servo2gpio[servo];
dp[width] = mask; if (width == servowidth[servo])
return;
if (width == 0) { if (width == 0)
cbp->dst = phys_gpclr0; 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 { } else {
for (i = width - 1; i > 0; i--) dp = ctl->turnoff + servostart[servo] + width;
dp[i] = 0; if (dp >= ctl->turnoff + NUM_SAMPLES)
dp[0] = mask; dp -= NUM_SAMPLES;
cbp->dst = phys_gpset0;
update_idle_time(servo); 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 static void
@ -362,23 +480,28 @@ make_pagemap(void)
fd = open(pagemap_fn, O_RDONLY); fd = open(pagemap_fn, O_RDONLY);
if (fd < 0) if (fd < 0)
fatal("servod: Failed to open %s: %m\n", pagemap_fn); fatal("servod: Failed to open %s: %m\n", pagemap_fn);
if (lseek(fd, (uint32_t)virtbase >> 9, SEEK_SET) != if (lseek(fd, (uint32_t)virtcached >> 9, SEEK_SET) !=
(uint32_t)virtbase >> 9) { (uint32_t)virtcached >> 9) {
fatal("servod: Failed to seek on %s: %m\n", pagemap_fn); fatal("servod: Failed to seek on %s: %m\n", pagemap_fn);
} }
for (i = 0; i < NUM_PAGES; i++) { for (i = 0; i < NUM_PAGES; i++) {
uint64_t pfn; 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)) if (read(fd, &pfn, sizeof(pfn)) != sizeof(pfn))
fatal("servod: Failed to read %s: %m\n", pagemap_fn); fatal("servod: Failed to read %s: %m\n", pagemap_fn);
if (((pfn >> 55) & 0x1bf) != 0x10c) if (((pfn >> 55) & 0x1bf) != 0x10c)
fatal("servod: Page %d not present (pfn 0x%016llx)\n", i, pfn); fatal("servod: Page %d not present (pfn 0x%016llx)\n", i, pfn);
page_map[i].physaddr = (uint32_t)pfn << PAGE_SHIFT | 0x40000000; 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(fd);
close(memfd); close(memfd);
memset(virtbase, 0, NUM_PAGES * PAGE_SIZE);
} }
static void static void
@ -402,34 +525,74 @@ init_ctrl_data(void)
{ {
struct ctl *ctl = (struct ctl *)virtbase; struct ctl *ctl = (struct ctl *)virtbase;
dma_cb_t *cbp = ctl->cb; dma_cb_t *cbp = ctl->cb;
uint32_t phys_fifo_addr; uint32_t phys_fifo_addr, cbinfo;
uint32_t phys_gpclr0 = 0x7e200000 + 0x28; uint32_t phys_gpclr0;
int servo, i; uint32_t phys_gpset0;
int servo, i, numservos = 0, curstart = 0;
uint32_t maskall = 0;
if (delay_hw == DELAY_VIA_PWM) if (invert) {
phys_fifo_addr = (PWM_BASE | 0x7e000000) + 0x18; phys_gpclr0 = 0x7e200000 + 0x1c;
else phys_gpset0 = 0x7e200000 + 0x28;
phys_fifo_addr = (PCM_BASE | 0x7e000000) + 0x04; } else {
phys_gpclr0 = 0x7e200000 + 0x28;
memset(ctl->sample, 0, sizeof(ctl->sample)); phys_gpset0 = 0x7e200000 + 0x1c;
for (servo = 0 ; servo < num_servos; servo++) {
for (i = 0; i < SERVO_SAMPLES; i++)
ctl->sample[servo * SERVO_SAMPLES + i] = 1 << servo2gpio[servo];
} }
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++) { for (i = 0; i < NUM_SAMPLES; i++) {
cbp->info = DMA_NO_WIDE_BURSTS | DMA_WAIT_RESP; 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->dst = phys_gpclr0;
cbp->length = 4; cbp->length = 4;
cbp->stride = 0; cbp->stride = 0;
cbp->next = mem_virt_to_phys(cbp + 1); cbp->next = mem_virt_to_phys(cbp + 1);
cbp++; 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 // Delay
if (delay_hw == DELAY_VIA_PWM) cbp->info = cbinfo;
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->src = mem_virt_to_phys(ctl); // Any data will do cbp->src = mem_virt_to_phys(ctl); // Any data will do
cbp->dst = phys_fifo_addr; cbp->dst = phys_fifo_addr;
cbp->length = 4; 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 static void
go_go_go(void) go_go_go(void)
{ {
@ -525,11 +712,15 @@ go_go_go(void)
line[++nchars] = '\0'; line[++nchars] = '\0';
nchars = 0; nchars = 0;
n = sscanf(line, "%d=%d%c", &servo, &width, &nl); 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); 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); 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); fprintf(stderr, "Invalid width %d\n", width);
} else { } else {
set_servo(servo, width); set_servo(servo, width);
@ -588,10 +779,103 @@ board_rev(void)
return rev; 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 int
main(int argc, char **argv) main(int argc, char **argv)
{ {
int i; int i;
char *p1pins = default_p1_pins;
char *p5pins = default_p5_pins;
int p1first = 1, hadp1 = 0, hadp5 = 0;
while (1) { while (1) {
int c; int c;
@ -600,12 +884,16 @@ main(int argc, char **argv)
static struct option long_options[] = { static struct option long_options[] = {
{ "pcm", no_argument, 0, 'p' }, { "pcm", no_argument, 0, 'p' },
{ "idle-timeout", required_argument, 0, 't' }, { "idle-timeout", required_argument, 0, 't' },
{ "idle_timeout", required_argument, 0, 't' },
{ "help", no_argument, 0, 'h' }, { "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 } { 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) { if (c == -1) {
break; break;
} else if (c == 'p') { } else if (c == 'p') {
@ -615,36 +903,77 @@ main(int argc, char **argv)
if (idle_timeout < 10 || idle_timeout > 3600000) if (idle_timeout < 10 || idle_timeout > 3600000)
fatal("Invalid idle_timeout specified\n"); fatal("Invalid idle_timeout specified\n");
} else if (c == 'h') { } else if (c == 'h') {
printf("\nUsage: %s [--pcm] [--idle-timeout=N]\n\n" printf("\nUsage: %s <options>\n\n"
"Where:\n" "Options:\n"
" --pcm tells servod to use PCM rather than PWM hardware\n" " --pcm tells servod to use PCM rather than PWM hardware\n"
" to implement delays\n" " to implement delays\n"
" --idle-timeout=N tells servod to stop sending servo pulses for a\n" " --idle-timeout=N tells servod to stop sending servo pulses for a\n"
" given output N milliseconds after the last update\n\n", " given output N milliseconds after the last update\n"
argv[0]); " --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); 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 { } else {
fatal("Invalid parameter\n"); 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 */ parse_pin_lists(p1first, p1pins, p5pins);
if (board_rev() == 1) {
servo2gpio = servo2gpio_rev1; printf("\nBoard revision: %5d\n", board_rev());
num_servos = sizeof(servo2gpio_rev1); printf("Using hardware: %s\n", delay_hw == DELAY_VIA_PWM ? "PWM" : "PCM");
} else { if (idle_timeout)
servo2gpio = servo2gpio_rev2; printf("Idle timeout: %5dms\n", idle_timeout);
num_servos = sizeof(servo2gpio_rev2); 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("\n");
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);
init_idle_timers(); init_idle_timers();
setup_sighandlers(); setup_sighandlers();
@ -655,20 +984,47 @@ main(int argc, char **argv)
clk_reg = map_peripheral(CLK_BASE, CLK_LEN); clk_reg = map_peripheral(CLK_BASE, CLK_LEN);
gpio_reg = map_peripheral(GPIO_BASE, GPIO_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, virtbase = mmap(NULL, NUM_PAGES * PAGE_SIZE, PROT_READ|PROT_WRITE,
MAP_SHARED|MAP_ANONYMOUS|MAP_NORESERVE|MAP_LOCKED, MAP_SHARED|MAP_ANONYMOUS|MAP_NORESERVE|MAP_LOCKED,
-1, 0); -1, 0);
if (virtbase == MAP_FAILED) 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)) if ((unsigned long)virtbase & (PAGE_SIZE-1))
fatal("servod: Virtual address is not page aligned\n"); fatal("servod: Virtual address is not page aligned\n");
munmap(virtbase, NUM_PAGES * PAGE_SIZE);
make_pagemap(); make_pagemap();
for (i = 0; i < num_servos; i++) { for (i = 0; i < MAX_SERVOS; i++) {
gpio_set(servo2gpio[i], 0); 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); gpio_set_mode(servo2gpio[i], GPIO_MODE_OUT);
} }
restore_gpio_modes = 1;
init_ctrl_data(); init_ctrl_data();
init_hardware(); init_hardware();