mirror of
https://github.com/richardghirst/PiBits.git
synced 2024-11-28 12:24:11 +01:00
Add support for B+, Pi2, and 4.1 kernels
This commit is contained in:
Richard Hirst
parent
bf455ee13b
commit
96014c804d
@ -44,15 +44,15 @@
|
||||
#define DMA_CONBLK_AD (0x04/4)
|
||||
#define DMA_DEBUG (0x20/4)
|
||||
|
||||
#define GPIO_BASE 0x20200000
|
||||
#define GPIO_BASE 0x3f200000
|
||||
#define GPIO_LEN 0xB4
|
||||
#define DMA_BASE 0x20007000
|
||||
#define DMA_BASE 0x3f007000
|
||||
#define DMA_LEN DMA_CHAN_SIZE * (DMA_CHAN_MAX+1)
|
||||
#define PWM_BASE 0x2020C000
|
||||
#define PWM_BASE 0x3f20C000
|
||||
#define PWM_LEN 0x28
|
||||
#define CLK_BASE 0x20101000
|
||||
#define CLK_BASE 0x3f101000
|
||||
#define CLK_LEN 0xA8
|
||||
#define TICK_BASE 0x20003000
|
||||
#define TICK_BASE 0x3f003000
|
||||
#define TICK_LEN 0x08
|
||||
|
||||
#define PWM_CTL (0x00/4)
|
||||
|
||||
@ -2,8 +2,8 @@
|
||||
.PHONY: all install uninstall
|
||||
all: servod
|
||||
|
||||
servod: servod.c
|
||||
gcc -Wall -g -O2 -o servod servod.c -lm
|
||||
servod: servod.c mailbox.c
|
||||
gcc -Wall -g -O2 -o servod servod.c mailbox.c -lm
|
||||
|
||||
install: servod
|
||||
[ "`id -u`" = "0" ] || { echo "Must be run as root"; exit 1; }
|
||||
|
||||
@ -15,7 +15,12 @@ PATH=/sbin:/usr/sbin:/bin:/usr/bin
|
||||
|
||||
OPTS="--idle-timeout=2000"
|
||||
|
||||
res=0
|
||||
STATUSFILE="/tmp/servoblaster-status"
|
||||
|
||||
if [ $( id -u ) != 0 ]; then
|
||||
echo "ERROR: Must be run as root"
|
||||
exit 1
|
||||
fi
|
||||
|
||||
case "$1" in
|
||||
start)
|
||||
@ -29,13 +34,31 @@ case "$1" in
|
||||
killall servod
|
||||
;;
|
||||
status)
|
||||
[ -e /dev/servoblaster ] || res=4
|
||||
if [ ! -e /dev/servoblaster ]; then
|
||||
echo "ERROR: /dev/servoblaster does not exist"
|
||||
exit 2
|
||||
fi
|
||||
rm -f $STATUSFILE
|
||||
echo "status $STATUSFILE" > /dev/servoblaster
|
||||
sleep 0.2
|
||||
if [ ! -e $STATUSFILE ]; then
|
||||
echo "ERROR: servod not responding"
|
||||
exit 3
|
||||
elif grep -q "^OK" $STATUSFILE; then
|
||||
echo "OK"
|
||||
exit 0
|
||||
elif grep "^ERROR:" $STATUSFILE; then
|
||||
exit 4
|
||||
else
|
||||
echo "ERROR: No status from servod"
|
||||
exit 5
|
||||
fi
|
||||
;;
|
||||
*)
|
||||
echo "Usage: servoblaster [start|stop|status]" >&2
|
||||
res=3
|
||||
exit 6
|
||||
;;
|
||||
esac
|
||||
|
||||
exit $res
|
||||
exit 0
|
||||
|
||||
|
||||
298
ServoBlaster/user/mailbox.c
Normal file
298
ServoBlaster/user/mailbox.c
Normal file
@ -0,0 +1,298 @@
|
||||
/*
|
||||
Copyright (c) 2012, Broadcom Europe Ltd.
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of the copyright holder nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
#include <fcntl.h>
|
||||
#include <unistd.h>
|
||||
#include <assert.h>
|
||||
#include <stdint.h>
|
||||
#include <errno.h>
|
||||
#include <sys/mman.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <sys/types.h>
|
||||
#include <sys/stat.h>
|
||||
|
||||
#include "mailbox.h"
|
||||
|
||||
//#define DEBUG
|
||||
|
||||
#define PAGE_SIZE (4*1024)
|
||||
|
||||
void *mapmem(unsigned base, unsigned size)
|
||||
{
|
||||
int mem_fd;
|
||||
unsigned offset = base % PAGE_SIZE;
|
||||
base = base - offset;
|
||||
/* open /dev/mem */
|
||||
if ((mem_fd = open("/dev/mem", O_RDWR|O_SYNC) ) < 0) {
|
||||
printf("can't open /dev/mem\nThis program should be run as root. Try prefixing command with: sudo\n");
|
||||
exit (-1);
|
||||
}
|
||||
void *mem = mmap(
|
||||
0,
|
||||
size,
|
||||
PROT_READ|PROT_WRITE,
|
||||
MAP_SHARED/*|MAP_FIXED*/,
|
||||
mem_fd,
|
||||
base);
|
||||
#ifdef DEBUG
|
||||
printf("base=0x%x, mem=%p\n", base, mem);
|
||||
#endif
|
||||
if (mem == MAP_FAILED) {
|
||||
printf("mmap error %d\n", (int)mem);
|
||||
exit (-1);
|
||||
}
|
||||
close(mem_fd);
|
||||
return (char *)mem + offset;
|
||||
}
|
||||
|
||||
void *unmapmem(void *addr, unsigned size)
|
||||
{
|
||||
int s = munmap(addr, size);
|
||||
if (s != 0) {
|
||||
printf("munmap error %d\n", s);
|
||||
exit (-1);
|
||||
}
|
||||
|
||||
return NULL;
|
||||
}
|
||||
|
||||
/*
|
||||
* use ioctl to send mbox property message
|
||||
*/
|
||||
|
||||
static int mbox_property(int file_desc, void *buf)
|
||||
{
|
||||
int fd = file_desc;
|
||||
int ret_val = -1;
|
||||
|
||||
if (fd < 0) {
|
||||
fd = mbox_open();
|
||||
}
|
||||
if (fd >= 0) {
|
||||
ret_val = ioctl(fd, IOCTL_MBOX_PROPERTY, buf);
|
||||
|
||||
if (ret_val < 0) {
|
||||
printf("ioctl_set_msg failed, errno %d: %m\n", errno);
|
||||
}
|
||||
}
|
||||
#ifdef DEBUG
|
||||
unsigned *p = buf; int i; unsigned size = *(unsigned *)buf;
|
||||
for (i=0; i<size/4; i++)
|
||||
printf("%04x: 0x%08x\n", i*sizeof *p, p[i]);
|
||||
#endif
|
||||
if (file_desc < 0)
|
||||
mbox_close(fd);
|
||||
|
||||
return ret_val;
|
||||
}
|
||||
|
||||
unsigned mem_alloc(int file_desc, unsigned size, unsigned align, unsigned flags)
|
||||
{
|
||||
int i=0;
|
||||
unsigned p[32];
|
||||
|
||||
#ifdef DEBUG
|
||||
printf("Requesting %d bytes\n", size);
|
||||
#endif
|
||||
p[i++] = 0; // size
|
||||
p[i++] = 0x00000000; // process request
|
||||
|
||||
p[i++] = 0x3000c; // (the tag id)
|
||||
p[i++] = 12; // (size of the buffer)
|
||||
p[i++] = 12; // (size of the data)
|
||||
p[i++] = size; // (num bytes? or pages?)
|
||||
p[i++] = align; // (alignment)
|
||||
p[i++] = flags; // (MEM_FLAG_L1_NONALLOCATING)
|
||||
|
||||
p[i++] = 0x00000000; // end tag
|
||||
p[0] = i*sizeof *p; // actual size
|
||||
|
||||
if (mbox_property(file_desc, p) < 0)
|
||||
return -1;
|
||||
else
|
||||
return p[5];
|
||||
}
|
||||
|
||||
unsigned mem_free(int file_desc, unsigned handle)
|
||||
{
|
||||
int i=0;
|
||||
unsigned p[32];
|
||||
p[i++] = 0; // size
|
||||
p[i++] = 0x00000000; // process request
|
||||
|
||||
p[i++] = 0x3000f; // (the tag id)
|
||||
p[i++] = 4; // (size of the buffer)
|
||||
p[i++] = 4; // (size of the data)
|
||||
p[i++] = handle;
|
||||
|
||||
p[i++] = 0x00000000; // end tag
|
||||
p[0] = i*sizeof *p; // actual size
|
||||
|
||||
mbox_property(file_desc, p);
|
||||
return p[5];
|
||||
}
|
||||
|
||||
unsigned mem_lock(int file_desc, unsigned handle)
|
||||
{
|
||||
int i=0;
|
||||
unsigned p[32];
|
||||
p[i++] = 0; // size
|
||||
p[i++] = 0x00000000; // process request
|
||||
|
||||
p[i++] = 0x3000d; // (the tag id)
|
||||
p[i++] = 4; // (size of the buffer)
|
||||
p[i++] = 4; // (size of the data)
|
||||
p[i++] = handle;
|
||||
|
||||
p[i++] = 0x00000000; // end tag
|
||||
p[0] = i*sizeof *p; // actual size
|
||||
|
||||
if (mbox_property(file_desc, p) < 0)
|
||||
return ~0;
|
||||
else
|
||||
return p[5];
|
||||
}
|
||||
|
||||
unsigned mem_unlock(int file_desc, unsigned handle)
|
||||
{
|
||||
int i=0;
|
||||
unsigned p[32];
|
||||
p[i++] = 0; // size
|
||||
p[i++] = 0x00000000; // process request
|
||||
|
||||
p[i++] = 0x3000e; // (the tag id)
|
||||
p[i++] = 4; // (size of the buffer)
|
||||
p[i++] = 4; // (size of the data)
|
||||
p[i++] = handle;
|
||||
|
||||
p[i++] = 0x00000000; // end tag
|
||||
p[0] = i*sizeof *p; // actual size
|
||||
|
||||
mbox_property(file_desc, p);
|
||||
return p[5];
|
||||
}
|
||||
|
||||
unsigned execute_code(int file_desc, unsigned code, unsigned r0, unsigned r1, unsigned r2, unsigned r3, unsigned r4, unsigned r5)
|
||||
{
|
||||
int i=0;
|
||||
unsigned p[32];
|
||||
p[i++] = 0; // size
|
||||
p[i++] = 0x00000000; // process request
|
||||
|
||||
p[i++] = 0x30010; // (the tag id)
|
||||
p[i++] = 28; // (size of the buffer)
|
||||
p[i++] = 28; // (size of the data)
|
||||
p[i++] = code;
|
||||
p[i++] = r0;
|
||||
p[i++] = r1;
|
||||
p[i++] = r2;
|
||||
p[i++] = r3;
|
||||
p[i++] = r4;
|
||||
p[i++] = r5;
|
||||
|
||||
p[i++] = 0x00000000; // end tag
|
||||
p[0] = i*sizeof *p; // actual size
|
||||
|
||||
mbox_property(file_desc, p);
|
||||
return p[5];
|
||||
}
|
||||
|
||||
unsigned qpu_enable(int file_desc, unsigned enable)
|
||||
{
|
||||
int i=0;
|
||||
unsigned p[32];
|
||||
|
||||
p[i++] = 0; // size
|
||||
p[i++] = 0x00000000; // process request
|
||||
|
||||
p[i++] = 0x30012; // (the tag id)
|
||||
p[i++] = 4; // (size of the buffer)
|
||||
p[i++] = 4; // (size of the data)
|
||||
p[i++] = enable;
|
||||
|
||||
p[i++] = 0x00000000; // end tag
|
||||
p[0] = i*sizeof *p; // actual size
|
||||
|
||||
mbox_property(file_desc, p);
|
||||
return p[5];
|
||||
}
|
||||
|
||||
unsigned execute_qpu(int file_desc, unsigned num_qpus, unsigned control, unsigned noflush, unsigned timeout) {
|
||||
int i=0;
|
||||
unsigned p[32];
|
||||
|
||||
p[i++] = 0; // size
|
||||
p[i++] = 0x00000000; // process request
|
||||
p[i++] = 0x30011; // (the tag id)
|
||||
p[i++] = 16; // (size of the buffer)
|
||||
p[i++] = 16; // (size of the data)
|
||||
p[i++] = num_qpus;
|
||||
p[i++] = control;
|
||||
p[i++] = noflush;
|
||||
p[i++] = timeout; // ms
|
||||
|
||||
p[i++] = 0x00000000; // end tag
|
||||
p[0] = i*sizeof *p; // actual size
|
||||
|
||||
mbox_property(file_desc, p);
|
||||
return p[5];
|
||||
}
|
||||
|
||||
int mbox_open(void) {
|
||||
int file_desc;
|
||||
char filename[64];
|
||||
|
||||
// open a char device file used for communicating with kernel mbox driver
|
||||
if ((file_desc = open("/dev/vcio", 0)) >= 0) {
|
||||
/* New kernel, we use /dev/vcio, major 249, since kernel 4.1 */
|
||||
return file_desc;
|
||||
}
|
||||
|
||||
/* Most likely an old kernel, so drop back to the old major=100 device */
|
||||
sprintf(filename, "/tmp/mailbox-%d", getpid());
|
||||
unlink(filename);
|
||||
if (mknod(filename, S_IFCHR|0600, makedev(100, 0)) < 0) {
|
||||
printf("Failed to create mailbox device %s: %m\n", filename);
|
||||
return -1;
|
||||
}
|
||||
file_desc = open(filename, 0);
|
||||
if (file_desc < 0) {
|
||||
printf("Can't open device file %s: %m\n", filename);
|
||||
unlink(filename);
|
||||
return -1;
|
||||
}
|
||||
unlink(filename);
|
||||
|
||||
return file_desc;
|
||||
}
|
||||
|
||||
void mbox_close(int file_desc) {
|
||||
close(file_desc);
|
||||
}
|
||||
46
ServoBlaster/user/mailbox.h
Normal file
46
ServoBlaster/user/mailbox.h
Normal file
@ -0,0 +1,46 @@
|
||||
/*
|
||||
Copyright (c) 2012, Broadcom Europe Ltd.
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of the copyright holder nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include <linux/ioctl.h>
|
||||
|
||||
#define MAJOR_NUM 100
|
||||
#define IOCTL_MBOX_PROPERTY _IOWR(MAJOR_NUM, 0, char *)
|
||||
|
||||
int mbox_open(void);
|
||||
void mbox_close(int file_desc);
|
||||
|
||||
unsigned get_version(int file_desc);
|
||||
unsigned mem_alloc(int file_desc, unsigned size, unsigned align, unsigned flags);
|
||||
unsigned mem_free(int file_desc, unsigned handle);
|
||||
unsigned mem_lock(int file_desc, unsigned handle);
|
||||
unsigned mem_unlock(int file_desc, unsigned handle);
|
||||
void *mapmem(unsigned base, unsigned size);
|
||||
void *unmapmem(void *addr, unsigned size);
|
||||
|
||||
unsigned execute_code(int file_desc, unsigned code, unsigned r0, unsigned r1, unsigned r2, unsigned r3, unsigned r4, unsigned r5);
|
||||
unsigned execute_qpu(int file_desc, unsigned num_qpus, unsigned control, unsigned noflush, unsigned timeout);
|
||||
unsigned qpu_enable(int file_desc, unsigned enable);
|
||||
@ -42,9 +42,11 @@
|
||||
#include <getopt.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "mailbox.h"
|
||||
|
||||
#define DMY 255 // Used to represent an invalid P1 pin, or unmapped servo
|
||||
|
||||
#define NUM_P1PINS 26
|
||||
#define NUM_P1PINS 40
|
||||
#define NUM_P5PINS 8
|
||||
|
||||
#define MAX_SERVOS 32 /* Only 21 really, but this lets you map servo IDs
|
||||
@ -68,17 +70,27 @@
|
||||
#define DMA_CHAN_MAX 14
|
||||
#define DMA_CHAN_DEFAULT 14
|
||||
|
||||
#define DMA_BASE 0x20007000
|
||||
#define DMA_BASE_OFFSET 0x00007000
|
||||
#define DMA_LEN DMA_CHAN_SIZE * (DMA_CHAN_MAX+1)
|
||||
#define PWM_BASE 0x2020C000
|
||||
#define PWM_BASE_OFFSET 0x0020C000
|
||||
#define PWM_LEN 0x28
|
||||
#define CLK_BASE 0x20101000
|
||||
#define CLK_BASE_OFFSET 0x00101000
|
||||
#define CLK_LEN 0xA8
|
||||
#define GPIO_BASE 0x20200000
|
||||
#define GPIO_BASE_OFFSET 0x00200000
|
||||
#define GPIO_LEN 0x100
|
||||
#define PCM_BASE 0x20203000
|
||||
#define PCM_BASE_OFFSET 0x00203000
|
||||
#define PCM_LEN 0x24
|
||||
|
||||
#define DMA_VIRT_BASE (periph_virt_base + DMA_BASE_OFFSET)
|
||||
#define PWM_VIRT_BASE (periph_virt_base + PWM_BASE_OFFSET)
|
||||
#define CLK_VIRT_BASE (periph_virt_base + CLK_BASE_OFFSET)
|
||||
#define GPIO_VIRT_BASE (periph_virt_base + GPIO_BASE_OFFSET)
|
||||
#define PCM_VIRT_BASE (periph_virt_base + PCM_BASE_OFFSET)
|
||||
|
||||
#define PWM_PHYS_BASE (periph_phys_base + PWM_BASE_OFFSET)
|
||||
#define PCM_PHYS_BASE (periph_phys_base + PCM_BASE_OFFSET)
|
||||
#define GPIO_PHYS_BASE (periph_phys_base + GPIO_BASE_OFFSET)
|
||||
|
||||
#define DMA_NO_WIDE_BURSTS (1<<26)
|
||||
#define DMA_WAIT_RESP (1<<3)
|
||||
#define DMA_D_DREQ (1<<6)
|
||||
@ -89,6 +101,7 @@
|
||||
|
||||
#define DMA_CS (0x00/4)
|
||||
#define DMA_CONBLK_AD (0x04/4)
|
||||
#define DMA_SOURCE_AD (0x0c/4)
|
||||
#define DMA_DEBUG (0x20/4)
|
||||
|
||||
#define GPIO_FSEL0 (0x00/4)
|
||||
@ -140,9 +153,7 @@ typedef struct {
|
||||
stride, next, pad[2];
|
||||
} dma_cb_t;
|
||||
|
||||
typedef struct {
|
||||
uint32_t physaddr;
|
||||
} page_map_t;
|
||||
#define BUS_TO_PHYS(x) ((x)&~0xC0000000)
|
||||
|
||||
/* Define which P1 header pins to use by default. These are the eight standard
|
||||
* GPIO pins (those coloured green in the diagram on this page:
|
||||
@ -235,6 +246,49 @@ static uint8_t rev2_p5pin2gpio_map[] = {
|
||||
DMY, // P5-8 Ground
|
||||
};
|
||||
|
||||
static uint8_t bplus_p1pin2gpio_map[] = {
|
||||
DMY, // P1-1 3v3
|
||||
DMY, // P1-2 5v
|
||||
2, // P1-3 GPIO 2 (SDA)
|
||||
DMY, // P1-4 5v
|
||||
3, // P1-5 GPIO 3 (SCL)
|
||||
DMY, // P1-6 Ground
|
||||
4, // P1-7 GPIO 4 (GPCLK0)
|
||||
14, // P1-8 GPIO 14 (TXD)
|
||||
DMY, // P1-9 Ground
|
||||
15, // P1-10 GPIO 15 (RXD)
|
||||
17, // P1-11 GPIO 17
|
||||
18, // P1-12 GPIO 18 (PCM_CLK)
|
||||
27, // P1-13 GPIO 27
|
||||
DMY, // P1-14 Ground
|
||||
22, // P1-15 GPIO 22
|
||||
23, // P1-16 GPIO 23
|
||||
DMY, // P1-17 3v3
|
||||
24, // P1-18 GPIO 24
|
||||
10, // P1-19 GPIO 10 (MOSI)
|
||||
DMY, // P1-20 Ground
|
||||
9, // P1-21 GPIO 9 (MISO)
|
||||
25, // P1-22 GPIO 25
|
||||
11, // P1-23 GPIO 11 (SCLK)
|
||||
8, // P1-24 GPIO 8 (CE0)
|
||||
DMY, // P1-25 Ground
|
||||
7, // P1-26 GPIO 7 (CE1)
|
||||
DMY, // P1-27 ID_SD
|
||||
DMY, // P1-28 ID_SC
|
||||
5, // P1-29 GPIO 5
|
||||
DMY, // P1-30 Ground
|
||||
6, // P1-31 GPIO 5
|
||||
12, // P1-32 GPIO 12
|
||||
13, // P1-33 GPIO 13
|
||||
DMY, // P1-34 Ground
|
||||
19, // P1-35 GPIO 19
|
||||
16, // P1-36 GPIO 16
|
||||
26, // P1-37 GPIO 26
|
||||
20, // P1-38 GPIO 20
|
||||
DMY, // P1-39 Ground
|
||||
21, // P1-40 GPIO 21
|
||||
};
|
||||
|
||||
// cycle_time_us is the pulse cycle time per servo, in microseconds.
|
||||
// Typically it should be 20ms, or 20000us.
|
||||
|
||||
@ -255,11 +309,6 @@ static int num_servos;
|
||||
static uint32_t gpiomode[MAX_SERVOS];
|
||||
static int restore_gpio_modes;
|
||||
|
||||
page_map_t *page_map;
|
||||
|
||||
static uint8_t *virtbase;
|
||||
static uint8_t *virtcached;
|
||||
|
||||
static volatile uint32_t *pwm_reg;
|
||||
static volatile uint32_t *pcm_reg;
|
||||
static volatile uint32_t *clk_reg;
|
||||
@ -282,6 +331,29 @@ static uint32_t *turnoff_mask;
|
||||
static uint32_t *turnon_mask;
|
||||
static dma_cb_t *cb_base;
|
||||
|
||||
static int board_model;
|
||||
static int gpio_cfg;
|
||||
|
||||
static uint32_t periph_phys_base;
|
||||
static uint32_t periph_virt_base;
|
||||
static uint32_t dram_phys_base;
|
||||
static uint32_t mem_flag;
|
||||
|
||||
static char *gpio_desc[] = {
|
||||
"Unknown",
|
||||
"P1 (26 pins)",
|
||||
"P1 (26 pins), P5 (8 pins)",
|
||||
"P1 (40 pins)"
|
||||
};
|
||||
|
||||
static struct {
|
||||
int handle; /* From mbox_open() */
|
||||
uint32_t size; /* Required size */
|
||||
unsigned mem_ref; /* From mem_alloc() */
|
||||
unsigned bus_addr; /* From mem_lock() */
|
||||
uint8_t *virt_addr; /* From mapmem() */
|
||||
} mbox;
|
||||
|
||||
static void set_servo(int servo, int width);
|
||||
static void set_servo_idle(int servo);
|
||||
static void gpio_set_mode(uint32_t gpio, uint32_t mode);
|
||||
@ -300,7 +372,7 @@ terminate(int dummy)
|
||||
{
|
||||
int i;
|
||||
|
||||
if (dma_reg && virtbase) {
|
||||
if (dma_reg && mbox.virt_addr) {
|
||||
for (i = 0; i < MAX_SERVOS; i++) {
|
||||
if (servo2gpio[i] != DMY)
|
||||
set_servo(i, 0);
|
||||
@ -315,6 +387,14 @@ terminate(int dummy)
|
||||
gpio_set_mode(servo2gpio[i], gpiomode[i]);
|
||||
}
|
||||
}
|
||||
if (mbox.virt_addr != NULL) {
|
||||
unmapmem(mbox.virt_addr, mbox.size);
|
||||
mem_unlock(mbox.handle, mbox.mem_ref);
|
||||
mem_free(mbox.handle, mbox.mem_ref);
|
||||
if (mbox.handle >= 0)
|
||||
mbox_close(mbox.handle);
|
||||
}
|
||||
|
||||
unlink(DEVFILE);
|
||||
unlink(CFGFILE);
|
||||
exit(1);
|
||||
@ -413,15 +493,15 @@ gpio_set(int gpio, int level)
|
||||
static uint32_t
|
||||
mem_virt_to_phys(void *virt)
|
||||
{
|
||||
uint32_t offset = (uint8_t *)virt - virtbase;
|
||||
uint32_t offset = (uint8_t *)virt - mbox.virt_addr;
|
||||
|
||||
return page_map[offset >> PAGE_SHIFT].physaddr + (offset % PAGE_SIZE);
|
||||
return mbox.bus_addr + offset;
|
||||
}
|
||||
|
||||
static void *
|
||||
map_peripheral(uint32_t base, uint32_t len)
|
||||
{
|
||||
int fd = open("/dev/mem", O_RDWR);
|
||||
int fd = open("/dev/mem", O_RDWR|O_SYNC);
|
||||
void * vaddr;
|
||||
|
||||
if (fd < 0)
|
||||
@ -500,47 +580,6 @@ set_servo(int servo, int width)
|
||||
update_idle_time(servo);
|
||||
}
|
||||
|
||||
static void
|
||||
make_pagemap(void)
|
||||
{
|
||||
int i, fd, memfd, pid;
|
||||
char pagemap_fn[64];
|
||||
|
||||
page_map = malloc(num_pages * sizeof(*page_map));
|
||||
if (page_map == 0)
|
||||
fatal("servod: Failed to malloc page_map: %m\n");
|
||||
memfd = open("/dev/mem", O_RDWR);
|
||||
if (memfd < 0)
|
||||
fatal("servod: Failed to open /dev/mem: %m\n");
|
||||
pid = getpid();
|
||||
sprintf(pagemap_fn, "/proc/%d/pagemap", pid);
|
||||
fd = open(pagemap_fn, O_RDONLY);
|
||||
if (fd < 0)
|
||||
fatal("servod: Failed to open %s: %m\n", pagemap_fn);
|
||||
if (lseek(fd, (uint32_t)(size_t)virtcached >> 9, SEEK_SET) !=
|
||||
(uint32_t)(size_t)virtcached >> 9) {
|
||||
fatal("servod: Failed to seek on %s: %m\n", pagemap_fn);
|
||||
}
|
||||
for (i = 0; i < num_pages; i++) {
|
||||
uint64_t pfn;
|
||||
if (read(fd, &pfn, sizeof(pfn)) != sizeof(pfn))
|
||||
fatal("servod: Failed to read %s: %m\n", pagemap_fn);
|
||||
if (((pfn >> 55) & 0x1bf) != 0x10c)
|
||||
fatal("servod: Page %d not present (pfn 0x%016llx)\n", i, pfn);
|
||||
page_map[i].physaddr = (uint32_t)pfn << PAGE_SHIFT | 0x40000000;
|
||||
if (mmap(virtbase + i * PAGE_SIZE, PAGE_SIZE, PROT_READ|PROT_WRITE,
|
||||
MAP_SHARED|MAP_FIXED|MAP_LOCKED|MAP_NORESERVE,
|
||||
memfd, (uint32_t)pfn << PAGE_SHIFT | 0x40000000) !=
|
||||
virtbase + i * PAGE_SIZE) {
|
||||
fatal("Failed to create uncached map of page %d at %p\n",
|
||||
i, virtbase + i * PAGE_SIZE);
|
||||
}
|
||||
}
|
||||
close(fd);
|
||||
close(memfd);
|
||||
memset(virtbase, 0, num_pages * PAGE_SIZE);
|
||||
}
|
||||
|
||||
static void
|
||||
setup_sighandlers(void)
|
||||
{
|
||||
@ -568,18 +607,18 @@ init_ctrl_data(void)
|
||||
uint32_t maskall = 0;
|
||||
|
||||
if (invert) {
|
||||
phys_gpclr0 = 0x7e200000 + 0x1c;
|
||||
phys_gpset0 = 0x7e200000 + 0x28;
|
||||
phys_gpclr0 = GPIO_PHYS_BASE + 0x1c;
|
||||
phys_gpset0 = GPIO_PHYS_BASE + 0x28;
|
||||
} else {
|
||||
phys_gpclr0 = 0x7e200000 + 0x28;
|
||||
phys_gpset0 = 0x7e200000 + 0x1c;
|
||||
phys_gpclr0 = GPIO_PHYS_BASE + 0x28;
|
||||
phys_gpset0 = GPIO_PHYS_BASE + 0x1c;
|
||||
}
|
||||
|
||||
if (delay_hw == DELAY_VIA_PWM) {
|
||||
phys_fifo_addr = (PWM_BASE | 0x7e000000) + 0x18;
|
||||
phys_fifo_addr = PWM_PHYS_BASE + 0x18;
|
||||
cbinfo = DMA_NO_WIDE_BURSTS | DMA_WAIT_RESP | DMA_D_DREQ | DMA_PER_MAP(5);
|
||||
} else {
|
||||
phys_fifo_addr = (PCM_BASE | 0x7e000000) + 0x04;
|
||||
phys_fifo_addr = PCM_PHYS_BASE + 0x04;
|
||||
cbinfo = DMA_NO_WIDE_BURSTS | DMA_WAIT_RESP | DMA_D_DREQ | DMA_PER_MAP(2);
|
||||
}
|
||||
|
||||
@ -696,12 +735,46 @@ init_hardware(void)
|
||||
}
|
||||
}
|
||||
|
||||
static void
|
||||
do_status(char *filename)
|
||||
{
|
||||
uint32_t last;
|
||||
int status = -1;
|
||||
char *p;
|
||||
int fd;
|
||||
const char *dma_dead = "ERROR: DMA not running\n";
|
||||
|
||||
while (*filename == ' ')
|
||||
filename++;
|
||||
p = filename + strlen(filename) - 1;
|
||||
while (p > filename && (*p == '\n' || *p == '\r' || *p == ' '))
|
||||
*p-- = '\0';
|
||||
|
||||
last = dma_reg[DMA_CONBLK_AD];
|
||||
udelay(step_time_us*2);
|
||||
if (dma_reg[DMA_CONBLK_AD] != last)
|
||||
status = 0;
|
||||
if ((fd = open(filename, O_WRONLY|O_CREAT, 0666)) >= 0) {
|
||||
if (status == 0)
|
||||
write(fd, "OK\n", 3);
|
||||
else
|
||||
write(fd, dma_dead, strlen(dma_dead));
|
||||
close(fd);
|
||||
} else {
|
||||
printf("Failed to open %s for writing: %m\n", filename);
|
||||
}
|
||||
}
|
||||
|
||||
static void
|
||||
do_debug(void)
|
||||
{
|
||||
int i;
|
||||
uint32_t mask = 0;
|
||||
uint32_t last = 0xffffffff;
|
||||
uint32_t last;
|
||||
|
||||
last = dma_reg[DMA_CONBLK_AD];
|
||||
udelay(step_time_us*2);
|
||||
printf("%08x %08x\n", last, dma_reg[DMA_CONBLK_AD]);
|
||||
|
||||
printf("---------------------------\n");
|
||||
printf("Servo Start Width TurnOn\n");
|
||||
@ -713,6 +786,7 @@ do_debug(void)
|
||||
}
|
||||
}
|
||||
printf("\nData:\n");
|
||||
last = 0xffffffff;
|
||||
for (i = 0; i < num_samples; i++) {
|
||||
uint32_t curr = turnoff_mask[i] & mask;
|
||||
if (curr != last)
|
||||
@ -823,6 +897,8 @@ go_go_go(void)
|
||||
n = sscanf(line, "%d=%s", &servo, width_arg);
|
||||
if (!strcmp(line, "debug\n")) {
|
||||
do_debug();
|
||||
} else if (!strncmp(line, "status ", 7)) {
|
||||
do_status(line + 7);
|
||||
} else if (n != 2) {
|
||||
fprintf(stderr, "Bad input: %s", line);
|
||||
} else if (servo < 0 || servo >= MAX_SERVOS) {
|
||||
@ -848,18 +924,18 @@ go_go_go(void)
|
||||
/* Determining the board revision is a lot more complicated than it should be
|
||||
* (see comments in wiringPi for details). We will just look at the last two
|
||||
* digits of the Revision string and treat '00' and '01' as errors, '02' and
|
||||
* '03' as rev 1, and any other hex value as rev 2.
|
||||
* '03' as rev 1, and any other hex value as rev 2. 'Pi1 and Pi2 are
|
||||
* differentiated by the Hardware being BCM2708 or BCM2709.
|
||||
*/
|
||||
static int
|
||||
board_rev(void)
|
||||
static void
|
||||
get_model_and_revision(void)
|
||||
{
|
||||
char buf[128];
|
||||
char buf[128], revstr[128], modelstr[128];
|
||||
char *ptr, *end, *res;
|
||||
static int rev = 0;
|
||||
int board_revision;
|
||||
FILE *fp;
|
||||
|
||||
if (rev)
|
||||
return rev;
|
||||
revstr[0] = modelstr[0] = '\0';
|
||||
|
||||
fp = fopen("/proc/cpuinfo", "r");
|
||||
|
||||
@ -867,26 +943,49 @@ board_rev(void)
|
||||
fatal("Unable to open /proc/cpuinfo: %m\n");
|
||||
|
||||
while ((res = fgets(buf, 128, fp))) {
|
||||
if (!strncmp(buf, "Revision", 8))
|
||||
break;
|
||||
if (!strncasecmp("hardware", buf, 8))
|
||||
memcpy(modelstr, buf, 128);
|
||||
else if (!strncasecmp(buf, "revision", 8))
|
||||
memcpy(revstr, buf, 128);
|
||||
}
|
||||
fclose(fp);
|
||||
|
||||
if (!res)
|
||||
if (modelstr[0] == '\0')
|
||||
fatal("servod: No 'Hardware' record in /proc/cpuinfo\n");
|
||||
if (revstr[0] == '\0')
|
||||
fatal("servod: No 'Revision' record in /proc/cpuinfo\n");
|
||||
|
||||
ptr = buf + strlen(buf) - 3;
|
||||
rev = strtol(ptr, &end, 16);
|
||||
if (strstr(modelstr, "BCM2708"))
|
||||
board_model = 1;
|
||||
else if (strstr(modelstr, "BCM2709"))
|
||||
board_model = 2;
|
||||
else
|
||||
fatal("servod: Cannot parse the hardware name string\n");
|
||||
|
||||
ptr = revstr + strlen(revstr) - 3;
|
||||
board_revision = strtol(ptr, &end, 16);
|
||||
if (end != ptr + 2)
|
||||
fatal("servod: Failed to parse Revision string\n");
|
||||
if (rev < 1)
|
||||
if (board_revision < 1)
|
||||
fatal("servod: Invalid board Revision\n");
|
||||
else if (rev < 4)
|
||||
rev = 1;
|
||||
else if (board_revision < 4)
|
||||
gpio_cfg = 1;
|
||||
else if (board_revision < 16)
|
||||
gpio_cfg = 2;
|
||||
else
|
||||
rev = 2;
|
||||
gpio_cfg = 3;
|
||||
|
||||
return rev;
|
||||
if (board_model == 1) {
|
||||
periph_virt_base = 0x20000000;
|
||||
periph_phys_base = 0x7e000000;
|
||||
dram_phys_base = 0x40000000;
|
||||
mem_flag = 0x0c;
|
||||
} else {
|
||||
periph_virt_base = 0x3f000000;
|
||||
periph_phys_base = 0x7e000000;
|
||||
dram_phys_base = 0xc0000000;
|
||||
mem_flag = 0x04;
|
||||
}
|
||||
}
|
||||
|
||||
static void
|
||||
@ -905,23 +1004,29 @@ parse_pin_lists(int p1first, char *p1pins, char*p5pins)
|
||||
if (lst == 0 && p1first) {
|
||||
name = "P1";
|
||||
pins = p1pins;
|
||||
if (board_rev() == 1) {
|
||||
if (board_model == 1 && gpio_cfg == 1) {
|
||||
map = rev1_p1pin2gpio_map;
|
||||
mapcnt = sizeof(rev1_p1pin2gpio_map);
|
||||
} else {
|
||||
} else if (board_model == 1 && gpio_cfg == 2) {
|
||||
map = rev2_p1pin2gpio_map;
|
||||
mapcnt = sizeof(rev2_p1pin2gpio_map);
|
||||
} else {
|
||||
map = bplus_p1pin2gpio_map;
|
||||
mapcnt = sizeof(bplus_p1pin2gpio_map);
|
||||
}
|
||||
pNpin2servo = p1pin2servo;
|
||||
} else {
|
||||
name = "P5";
|
||||
pins = p5pins;
|
||||
if (board_rev() == 1) {
|
||||
if (board_model == 1 && gpio_cfg == 1) {
|
||||
map = rev1_p5pin2gpio_map;
|
||||
mapcnt = sizeof(rev1_p5pin2gpio_map);
|
||||
} else {
|
||||
} else if (board_model == 1 && gpio_cfg == 2) {
|
||||
map = rev2_p5pin2gpio_map;
|
||||
mapcnt = sizeof(rev2_p5pin2gpio_map);
|
||||
} else {
|
||||
map = NULL;
|
||||
mapcnt = 0;
|
||||
}
|
||||
pNpin2servo = p5pin2servo;
|
||||
}
|
||||
@ -982,20 +1087,25 @@ gpio2pinname(uint8_t gpio)
|
||||
static char res[16];
|
||||
uint8_t pin;
|
||||
|
||||
if (board_rev() == 1) {
|
||||
if (board_model == 1 && gpio_cfg == 1) {
|
||||
if ((pin = gpiosearch(gpio, rev1_p1pin2gpio_map, sizeof(rev1_p1pin2gpio_map))))
|
||||
sprintf(res, "P1-%d", pin);
|
||||
else if ((pin = gpiosearch(gpio, rev1_p5pin2gpio_map, sizeof(rev1_p5pin2gpio_map))))
|
||||
sprintf(res, "P5-%d", pin);
|
||||
else
|
||||
fatal("Cannot map GPIO %d to a header pin\n", gpio);
|
||||
} else {
|
||||
} else if (board_model == 1 && gpio_cfg == 2) {
|
||||
if ((pin = gpiosearch(gpio, rev2_p1pin2gpio_map, sizeof(rev2_p1pin2gpio_map))))
|
||||
sprintf(res, "P1-%d", pin);
|
||||
else if ((pin = gpiosearch(gpio, rev2_p5pin2gpio_map, sizeof(rev2_p5pin2gpio_map))))
|
||||
sprintf(res, "P5-%d", pin);
|
||||
else
|
||||
fatal("Cannot map GPIO %d to a header pin\n", gpio);
|
||||
} else {
|
||||
if ((pin = gpiosearch(gpio, bplus_p1pin2gpio_map, sizeof(bplus_p1pin2gpio_map))))
|
||||
sprintf(res, "P1-%d", pin);
|
||||
else
|
||||
fatal("Cannot map GPIO %d to a header pin\n", gpio);
|
||||
}
|
||||
|
||||
return res;
|
||||
@ -1151,8 +1261,9 @@ main(int argc, char **argv)
|
||||
fatal("Invalid parameter\n");
|
||||
}
|
||||
}
|
||||
if (board_rev() == 1 && p5pins[0])
|
||||
fatal("Board rev 1 does not have a P5 header\n");
|
||||
get_model_and_revision();
|
||||
if (board_model == 1 && gpio_cfg == 1 && p5pins[0])
|
||||
fatal("Board model 1 revision 1 does not have a P5 header\n");
|
||||
|
||||
parse_pin_lists(p1first, p1pins, p5pins);
|
||||
|
||||
@ -1232,7 +1343,8 @@ main(int argc, char **argv)
|
||||
fatal("min value is >= max value\n");
|
||||
}
|
||||
|
||||
printf("\nBoard revision: %7d\n", board_rev());
|
||||
printf("\nBoard model: %7d\n", board_model);
|
||||
printf("GPIO configuration: %s\n", gpio_desc[gpio_cfg]);
|
||||
printf("Using hardware: %s\n", delay_hw == DELAY_VIA_PWM ? "PWM" : "PCM");
|
||||
printf("Using DMA channel: %7d\n", dma_chan);
|
||||
if (idle_timeout)
|
||||
@ -1248,7 +1360,7 @@ main(int argc, char **argv)
|
||||
servo_max_ticks * step_time_us);
|
||||
printf("Output levels: %s\n", invert ? "Inverted" : " Normal");
|
||||
printf("\nUsing P1 pins: %s\n", p1pins);
|
||||
if (board_rev() > 1)
|
||||
if (board_model == 1 && gpio_cfg == 2)
|
||||
printf("Using P5 pins: %s\n", p5pins);
|
||||
printf("\nServo mapping:\n");
|
||||
for (i = 0; i < MAX_SERVOS; i++) {
|
||||
@ -1261,55 +1373,33 @@ main(int argc, char **argv)
|
||||
init_idle_timers();
|
||||
setup_sighandlers();
|
||||
|
||||
dma_reg = map_peripheral(DMA_BASE, DMA_LEN);
|
||||
dma_reg = map_peripheral(DMA_VIRT_BASE, DMA_LEN);
|
||||
dma_reg += dma_chan * DMA_CHAN_SIZE / sizeof(uint32_t);
|
||||
pwm_reg = map_peripheral(PWM_BASE, PWM_LEN);
|
||||
pcm_reg = map_peripheral(PCM_BASE, PCM_LEN);
|
||||
clk_reg = map_peripheral(CLK_BASE, CLK_LEN);
|
||||
gpio_reg = map_peripheral(GPIO_BASE, GPIO_LEN);
|
||||
pwm_reg = map_peripheral(PWM_VIRT_BASE, PWM_LEN);
|
||||
pcm_reg = map_peripheral(PCM_VIRT_BASE, PCM_LEN);
|
||||
clk_reg = map_peripheral(CLK_VIRT_BASE, CLK_LEN);
|
||||
gpio_reg = map_peripheral(GPIO_VIRT_BASE, GPIO_LEN);
|
||||
|
||||
/*
|
||||
* Map the pages to our virtual 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);
|
||||
/* Use the mailbox interface to the VC to ask for physical memory */
|
||||
// Use the mailbox interface to request memory from the VideoCore
|
||||
// We specifiy (-1) for the handle rather than calling mbox_open()
|
||||
// so multiple users can share the resource.
|
||||
mbox.handle = -1; // mbox_open();
|
||||
mbox.size = num_pages * 4096;
|
||||
mbox.mem_ref = mem_alloc(mbox.handle, mbox.size, 4096, mem_flag);
|
||||
if (mbox.mem_ref < 0) {
|
||||
fatal("Failed to alloc memory from VideoCore\n");
|
||||
}
|
||||
mbox.bus_addr = mem_lock(mbox.handle, mbox.mem_ref);
|
||||
if (mbox.bus_addr == ~0) {
|
||||
mem_free(mbox.handle, mbox.size);
|
||||
fatal("Failed to lock memory\n");
|
||||
}
|
||||
mbox.virt_addr = mapmem(BUS_TO_PHYS(mbox.bus_addr), mbox.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 uncached pages: %m\n");
|
||||
if ((unsigned long)virtbase & (PAGE_SIZE-1))
|
||||
fatal("servod: Virtual address is not page aligned\n");
|
||||
munmap(virtbase, num_pages * PAGE_SIZE);
|
||||
|
||||
make_pagemap();
|
||||
|
||||
/*
|
||||
* Now the memory is all mapped, we can set up the pointers to the
|
||||
* bit masks used to turn outputs on and off, and to the DMA control
|
||||
* blocks. The control blocks must be 32 byte aligned (so round up
|
||||
* to multiple of 8, as we're then multiplying by 4).
|
||||
*/
|
||||
turnoff_mask = (uint32_t *)virtbase;
|
||||
turnon_mask = (uint32_t *)(virtbase + num_samples * sizeof(uint32_t));
|
||||
cb_base = (dma_cb_t *)(virtbase +
|
||||
turnoff_mask = (uint32_t *)mbox.virt_addr;
|
||||
turnon_mask = (uint32_t *)(mbox.virt_addr + num_samples * sizeof(uint32_t));
|
||||
cb_base = (dma_cb_t *)(mbox.virt_addr +
|
||||
ROUNDUP(num_samples + MAX_SERVOS, 8) * sizeof(uint32_t));
|
||||
|
||||
for (i = 0; i < MAX_SERVOS; i++) {
|
||||
|
||||
Loading…
Reference in New Issue
Block a user