From 29ce55de755a41bfee26b1b555f39729ac1313ac Mon Sep 17 00:00:00 2001 From: Richard Hirst Date: Wed, 11 Sep 2013 22:00:00 +0100 Subject: [PATCH] Major update to servod.c to handle up to 21 servos and to allow pulse widths between 0 and 100% --- ServoBlaster/README.txt | 188 ++++++++---- ServoBlaster/user/servod.c | 568 ++++++++++++++++++++++++++++++------- 2 files changed, 597 insertions(+), 159 deletions(-) diff --git a/ServoBlaster/README.txt b/ServoBlaster/README.txt index f4865d2..61c6106 100644 --- a/ServoBlaster/README.txt +++ b/ServoBlaster/README.txt @@ -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 "=", 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: + --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= tells servod which pins on the P1 header to use + --p5pins= tells servod which pins on the P5 header to use + +where 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: diff --git a/ServoBlaster/user/servod.c b/ServoBlaster/user/servod.c index 6645e74..6cb87f3 100644 --- a/ServoBlaster/user/servod.c +++ b/ServoBlaster/user/servod.c @@ -36,36 +36,110 @@ #include #include -/* 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 \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= tells servod which pins on the P1 header to use\n" + " --p5pins= tells servod which pins on the P5 header to use\n" + "\nwhere 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();