Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 22:48:29 -04:00
Code Issues Packages Projects Releases Wiki Activity
50765229ca
ardupilot/libraries/AP_HAL_Linux/RCInput_Navio.cpp
Staroselskii Georgii d7ac725a64 AP_HAL_Linux: fixed signal handler in RCInput_Navio 
DMA is getting stopped in the separate method now. This is the best we
can get at the current time. It does yield slightly better experience
and works in the majority of cases.

The patch is a no bulletproof solution, though.

There's a possibility of corruption in case of e.g. a SIGKILL. There's
no signal framework at the time and the commit doesn't add one. That's
why all signals are handled in the same erroneous way. This is not a
good nor a final solution to the issue.

For the issue at hand a better fix might be porting the code to kernel
space but it's a rather tediuos task that we cannot undertake in the
couple of weeks.
2015-07-23 08:51:38 +10:00

527 lines
19 KiB
C++
Raw Blame History

#include <AP_HAL.h>
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO
#include "GPIO.h"
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <stdint.h>
#include <sys/ioctl.h>
#include <pthread.h>
#include <string.h>
#include <errno.h>
#include <stdint.h>
#include <time.h>
#include <sys/time.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/mman.h>
#include <assert.h>
#include "RCInput_Navio.h"
//Parametres
#define RCIN_NAVIO_BUFFER_LENGTH 8
#define RCIN_NAVIO_SAMPLE_FREQ 500
#define RCIN_NAVIO_DMA_CHANNEL 0
#define RCIN_NAVIO_MAX_COUNTER 1300
#define PPM_INPUT_NAVIO RPI_GPIO_4
#define RCIN_NAVIO_MAX_SIZE_LINE 50
//Memory Addresses
#define RCIN_NAVIO_RPI1_DMA_BASE 0x20007000
#define RCIN_NAVIO_RPI1_CLK_BASE 0x20101000
#define RCIN_NAVIO_RPI1_PCM_BASE 0x20203000
#define RCIN_NAVIO_RPI2_DMA_BASE 0x3F007000
#define RCIN_NAVIO_RPI2_CLK_BASE 0x3F101000
#define RCIN_NAVIO_RPI2_PCM_BASE 0x3F203000
#define RCIN_NAVIO_GPIO_LEV0_ADDR 0x7e200034
#define RCIN_NAVIO_DMA_LEN 0x1000
#define RCIN_NAVIO_CLK_LEN 0xA8
#define RCIN_NAVIO_PCM_LEN 0x24
#define RCIN_NAVIO_TIMER_BASE 0x7e003004
#define RCIN_NAVIO_DMA_SRC_INC (1<<8)
#define RCIN_NAVIO_DMA_DEST_INC (1<<4)
#define RCIN_NAVIO_DMA_NO_WIDE_BURSTS (1<<26)
#define RCIN_NAVIO_DMA_WAIT_RESP (1<<3)
#define RCIN_NAVIO_DMA_D_DREQ (1<<6)
#define RCIN_NAVIO_DMA_PER_MAP(x) ((x)<<16)
#define RCIN_NAVIO_DMA_END (1<<1)
#define RCIN_NAVIO_DMA_RESET (1<<31)
#define RCIN_NAVIO_DMA_INT (1<<2)
#define RCIN_NAVIO_DMA_CS (0x00/4)
#define RCIN_NAVIO_DMA_CONBLK_AD (0x04/4)
#define RCIN_NAVIO_DMA_DEBUG (0x20/4)
#define RCIN_NAVIO_PCM_CS_A (0x00/4)
#define RCIN_NAVIO_PCM_FIFO_A (0x04/4)
#define RCIN_NAVIO_PCM_MODE_A (0x08/4)
#define RCIN_NAVIO_PCM_RXC_A (0x0c/4)
#define RCIN_NAVIO_PCM_TXC_A (0x10/4)
#define RCIN_NAVIO_PCM_DREQ_A (0x14/4)
#define RCIN_NAVIO_PCM_INTEN_A (0x18/4)
#define RCIN_NAVIO_PCM_INT_STC_A (0x1c/4)
#define RCIN_NAVIO_PCM_GRAY (0x20/4)
#define RCIN_NAVIO_PCMCLK_CNTL 38
#define RCIN_NAVIO_PCMCLK_DIV 39
extern const AP_HAL::HAL& hal;
using namespace Linux;
volatile uint32_t *LinuxRCInput_Navio::pcm_reg;
volatile uint32_t *LinuxRCInput_Navio::clk_reg;
volatile uint32_t *LinuxRCInput_Navio::dma_reg;
Memory_table::Memory_table()
{
_page_count = 0;
}
//Init Memory table
Memory_table::Memory_table(uint32_t page_count, int version)
{
uint32_t i;
int fdMem, file;
//Cache coherent adresses depends on RPI's version
uint32_t bus = version == 1 ? 0x40000000 : 0xC0000000;
uint64_t pageInfo;
void* offset;
_virt_pages = (void**)malloc(page_count * sizeof(void*));
_phys_pages = (void**)malloc(page_count * sizeof(void*));
_page_count = page_count;
if ((fdMem = open("/dev/mem", O_RDWR | O_SYNC)) < 0) {
fprintf(stderr,"Failed to open /dev/mem\n");
exit(-1);
}
if ((file = open("/proc/self/pagemap", O_RDWR | O_SYNC)) < 0) {
fprintf(stderr,"Failed to open /proc/self/pagemap\n");
exit(-1);
}
//Magic to determine the physical address for this page:
offset = mmap(0, _page_count*PAGE_SIZE, PROT_READ|PROT_WRITE, MAP_SHARED|MAP_ANONYMOUS|MAP_NORESERVE|MAP_LOCKED,-1,0);
lseek(file, ((uint32_t)offset)/PAGE_SIZE*8, SEEK_SET);
//Get list of available cache coherent physical addresses
for (i = 0; i < _page_count; i++) {
_virt_pages[i] = mmap(0, PAGE_SIZE, PROT_READ|PROT_WRITE, MAP_SHARED|MAP_ANONYMOUS|MAP_NORESERVE|MAP_LOCKED,-1,0);
::read(file, &pageInfo, 8);
_phys_pages[i] = (void*)((uint32_t)(pageInfo*PAGE_SIZE) | bus);
}
//Map physical addresses to virtual memory
for (i = 0; i < _page_count; i++) {
munmap(_virt_pages[i], PAGE_SIZE);
_virt_pages[i] = mmap(_virt_pages[i], PAGE_SIZE, PROT_READ|PROT_WRITE, MAP_SHARED|MAP_FIXED|MAP_NORESERVE|MAP_LOCKED, fdMem, ((uint32_t)_phys_pages[i] & (version == 1 ? 0xFFFFFFFF : ~bus)));
memset(_virt_pages[i], 0xee, PAGE_SIZE);
}
close(file);
close(fdMem);
}
Memory_table::~Memory_table()
{
free(_virt_pages);
free(_phys_pages);
}
// This function returns physical address with help of pointer, which is offset from the beginning of the buffer.
void* Memory_table::get_page(void** const pages, uint32_t addr) const
{
if (addr >= PAGE_SIZE * _page_count) {
return NULL;
}
return (uint8_t*)pages[(uint32_t) addr / 4096] + addr % 4096;
}
//Get virtual address from the corresponding physical address from memory_table.
void* Memory_table::get_virt_addr(const uint32_t phys_addr) const
{
// FIXME: Can't the address be calculated directly?
// FIXME: if the address room in _phys_pages is not fragmented one may avoid a complete loop ..
uint32_t i = 0;
for (; i < _page_count; i++) {
if ((uint32_t) _phys_pages[i] == (((uint32_t) phys_addr) & 0xFFFFF000)) {
return (void*) ((uint32_t) _virt_pages[i] + (phys_addr & 0xFFF));
}
}
return NULL;
}
// FIXME: in-congruent function style see above
// This function returns offset from the beginning of the buffer using virtual address and memory_table.
uint32_t Memory_table::get_offset(void ** const pages, const uint32_t addr) const
{
uint32_t i = 0;
for (; i < _page_count; i++) {
if ((uint32_t) pages[i] == (addr & 0xFFFFF000) ) {
return (i*PAGE_SIZE + (addr & 0xFFF));
}
}
return -1;
}
//How many bytes are available for reading in circle buffer?
uint32_t Memory_table::bytes_available(const uint32_t read_addr, const uint32_t write_addr) const
{
if (write_addr > read_addr) {
return (write_addr - read_addr);
}
else {
return _page_count * PAGE_SIZE - (read_addr - write_addr);
}
}
uint32_t Memory_table::get_page_count() const
{
return _page_count;
}
// More memory mapping
int get_raspberry_pi_version()
{
char buffer[RCIN_NAVIO_MAX_SIZE_LINE];
const char* hardware_description_entry = "Hardware";
const char* v1 = "BCM2708";
const char* v2 = "BCM2709";
char* flag;
FILE* fd;
fd = fopen("/proc/cpuinfo", "r");
while (fgets(buffer, RCIN_NAVIO_MAX_SIZE_LINE, fd) != NULL) {
flag = strstr(buffer, hardware_description_entry);
if (flag != NULL) {
if (strstr(buffer, v2) != NULL) {
printf("Raspberry Pi 2 with BCM2709!\n");
fclose(fd);
return 2;
}
else if (strstr(buffer, v1) != NULL) {
printf("Raspberry Pi 1 with BCM2708!\n");
fclose(fd);
return 1;
}
}
}
return 0;
}
//Physical addresses of peripheral depends on Raspberry Pi's version
void LinuxRCInput_Navio::set_physical_addresses(int version)
{
if (version == 1) {
dma_base = RCIN_NAVIO_RPI1_DMA_BASE;
clk_base = RCIN_NAVIO_RPI1_CLK_BASE;
pcm_base = RCIN_NAVIO_RPI1_PCM_BASE;
}
else if (version == 2) {
dma_base = RCIN_NAVIO_RPI2_DMA_BASE;
clk_base = RCIN_NAVIO_RPI2_CLK_BASE;
pcm_base = RCIN_NAVIO_RPI2_PCM_BASE;
}
}
//Map peripheral to virtual memory
void* LinuxRCInput_Navio::map_peripheral(uint32_t base, uint32_t len)
{
int fd = open("/dev/mem", O_RDWR);
void * vaddr;
if (fd < 0) {
printf("Failed to open /dev/mem: %m\n");
return NULL;
}
vaddr = mmap(NULL, len, PROT_READ|PROT_WRITE, MAP_SHARED, fd, base);
if (vaddr == MAP_FAILED) {
printf("rpio-pwm: Failed to map peripheral at 0x%08x: %m\n", base);
}
close(fd);
return vaddr;
}
//Method to init DMA control block
void LinuxRCInput_Navio::init_dma_cb(dma_cb_t** cbp, uint32_t mode, uint32_t source, uint32_t dest, uint32_t length, uint32_t stride, uint32_t next_cb)
{
(*cbp)->info = mode;
(*cbp)->src = source;
(*cbp)->dst = dest;
(*cbp)->length = length;
(*cbp)->next = next_cb;
(*cbp)->stride = stride;
}
void LinuxRCInput_Navio::stop_dma()
{
dma_reg[RCIN_NAVIO_DMA_CS | RCIN_NAVIO_DMA_CHANNEL << 8] = 0;
}
/* We need to be sure that the DMA is stopped upon termination */
void LinuxRCInput_Navio::termination_handler(int signum)
{
stop_dma();
hal.scheduler->panic("Interrupted");
}
//This function is used to init DMA control blocks (setting sampling GPIO register, destination adresses, synchronization)
void LinuxRCInput_Navio::init_ctrl_data()
{
uint32_t phys_fifo_addr;
uint32_t dest = 0;
uint32_t cbp = 0;
dma_cb_t* cbp_curr;
//Set fifo addr (for delay)
phys_fifo_addr = ((pcm_base + 0x04) & 0x00FFFFFF) | 0x7e000000;
//Init dma control blocks.
/*We are transferring 1 byte of GPIO register. Every 56th iteration we are
sampling TIMER register, which length is 8 bytes. So, for every 56 samples of GPIO we need
56 * 1 + 8 = 64 bytes of buffer. Value 56 was selected specially to have a 64-byte "block"
TIMER - GPIO. So, we have integer count of such "blocks" at one virtual page. (4096 / 64 = 64
"blocks" per page. As minimum, we must have 2 virtual pages of buffer (to have integer count of
vitual pages for control blocks): for every 56 iterations (64 bytes of buffer) we need 56 control blocks for GPIO
sampling, 56 control blocks for setting frequency and 1 control block for sampling timer, so,
we need 56 + 56 + 1 = 113 control blocks. For integer value, we need 113 pages of control blocks.
Each control block length is 32 bytes. In 113 pages we will have (113 * 4096 / 32) = 113 * 128 control
blocks. 113 * 128 control blocks = 64 * 128 bytes of buffer = 2 pages of buffer.
So, for 56 * 64 * 2 iteration we init DMA for sampling GPIO
and timer to (64 * 64 * 2) = 8192 bytes = 2 pages of buffer.
*/
// fprintf(stderr, "ERROR SEARCH1\n");
uint32_t i = 0;
for (i = 0; i < 56 * 128 * RCIN_NAVIO_BUFFER_LENGTH; i++) // 8 * 56 * 128 == 57344
{
//Transfer timer every 56th sample
if(i % 56 == 0) {
cbp_curr = (dma_cb_t*)con_blocks->get_page(con_blocks->_virt_pages, cbp);
init_dma_cb(&cbp_curr, RCIN_NAVIO_DMA_NO_WIDE_BURSTS | RCIN_NAVIO_DMA_WAIT_RESP | RCIN_NAVIO_DMA_DEST_INC | RCIN_NAVIO_DMA_SRC_INC, RCIN_NAVIO_TIMER_BASE, (uint32_t) circle_buffer->get_page(circle_buffer->_phys_pages, dest), 8, 0, (uint32_t) con_blocks->get_page(con_blocks->_phys_pages, cbp + sizeof(dma_cb_t)));
dest += 8;
cbp += sizeof(dma_cb_t);
}
// Transfer GPIO (1 byte)
cbp_curr = (dma_cb_t*)con_blocks->get_page(con_blocks->_virt_pages, cbp);
init_dma_cb(&cbp_curr, RCIN_NAVIO_DMA_NO_WIDE_BURSTS | RCIN_NAVIO_DMA_WAIT_RESP, RCIN_NAVIO_GPIO_LEV0_ADDR, (uint32_t) circle_buffer->get_page(circle_buffer->_phys_pages, dest), 1, 0, (uint32_t) con_blocks->get_page(con_blocks->_phys_pages, cbp + sizeof(dma_cb_t)));
dest += 1;
cbp += sizeof(dma_cb_t);
// Delay (for setting sampling frequency)
/* DMA is waiting data request signal (DREQ) from PCM. PCM is set for 1 MhZ freqency, so,
each sample of GPIO is limited by writing to PCA queue.
*/
cbp_curr = (dma_cb_t*)con_blocks->get_page(con_blocks->_virt_pages, cbp);
init_dma_cb(&cbp_curr, RCIN_NAVIO_DMA_NO_WIDE_BURSTS | RCIN_NAVIO_DMA_WAIT_RESP | RCIN_NAVIO_DMA_D_DREQ | RCIN_NAVIO_DMA_PER_MAP(2), RCIN_NAVIO_TIMER_BASE, phys_fifo_addr, 4, 0, (uint32_t) con_blocks->get_page(con_blocks->_phys_pages, cbp + sizeof(dma_cb_t)));
cbp += sizeof(dma_cb_t);
}
//Make last control block point to the first (to make circle)
cbp -= sizeof(dma_cb_t);
((dma_cb_t*)con_blocks->get_page(con_blocks->_virt_pages, cbp))->next = (uint32_t) con_blocks->get_page(con_blocks->_phys_pages, 0);
}
/*Initialise PCM
See BCM2835 documentation:
http://www.raspberrypi.org/wp-content/uploads/2012/02/BCM2835-ARM-Peripherals.pdf
*/
void LinuxRCInput_Navio::init_PCM()
{
pcm_reg[RCIN_NAVIO_PCM_CS_A] = 1; // Disable Rx+Tx, Enable PCM block
hal.scheduler->delay_microseconds(100);
clk_reg[RCIN_NAVIO_PCMCLK_CNTL] = 0x5A000006; // Source=PLLD (500MHz)
hal.scheduler->delay_microseconds(100);
clk_reg[RCIN_NAVIO_PCMCLK_DIV] = 0x5A000000 | ((50000/RCIN_NAVIO_SAMPLE_FREQ)<<12); // Set pcm div. If we need to configure DMA frequency.
hal.scheduler->delay_microseconds(100);
clk_reg[RCIN_NAVIO_PCMCLK_CNTL] = 0x5A000016; // Source=PLLD and enable
hal.scheduler->delay_microseconds(100);
pcm_reg[RCIN_NAVIO_PCM_TXC_A] = 0<<31 | 1<<30 | 0<<20 | 0<<16; // 1 channel, 8 bits
hal.scheduler->delay_microseconds(100);
pcm_reg[RCIN_NAVIO_PCM_MODE_A] = (10 - 1) << 10; //PCM mode
hal.scheduler->delay_microseconds(100);
pcm_reg[RCIN_NAVIO_PCM_CS_A] |= 1<<4 | 1<<3; // Clear FIFOs
hal.scheduler->delay_microseconds(100);
pcm_reg[RCIN_NAVIO_PCM_DREQ_A] = 64<<24 | 64<<8; // DMA Req when one slot is free?
hal.scheduler->delay_microseconds(100);
pcm_reg[RCIN_NAVIO_PCM_CS_A] |= 1<<9; // Enable DMA
hal.scheduler->delay_microseconds(100);
pcm_reg[RCIN_NAVIO_PCM_CS_A] |= 1<<2; // Enable Tx
hal.scheduler->delay_microseconds(100);
}
/*Initialise DMA
See BCM2835 documentation:
http://www.raspberrypi.org/wp-content/uploads/2012/02/BCM2835-ARM-Peripherals.pdf
*/
void LinuxRCInput_Navio::init_DMA()
{
dma_reg[RCIN_NAVIO_DMA_CS | RCIN_NAVIO_DMA_CHANNEL << 8] = RCIN_NAVIO_DMA_RESET; //Reset DMA
hal.scheduler->delay_microseconds(100);
dma_reg[RCIN_NAVIO_DMA_CS | RCIN_NAVIO_DMA_CHANNEL << 8] = RCIN_NAVIO_DMA_INT | RCIN_NAVIO_DMA_END;
dma_reg[RCIN_NAVIO_DMA_CONBLK_AD | RCIN_NAVIO_DMA_CHANNEL << 8] = reinterpret_cast<uint32_t>(con_blocks->get_page(con_blocks->_phys_pages, 0));//Set first control block address
dma_reg[RCIN_NAVIO_DMA_DEBUG | RCIN_NAVIO_DMA_CHANNEL << 8] = 7; // clear debug error flags
dma_reg[RCIN_NAVIO_DMA_CS | RCIN_NAVIO_DMA_CHANNEL << 8] = 0x10880001; // go, mid priority, wait for outstanding writes
}
//We must stop DMA when the process is killed
void LinuxRCInput_Navio::set_sigaction()
{
for (int i = 0; i < 64; i++) {
//catch all signals (like ctrl+c, ctrl+z, ...) to ensure DMA is disabled
struct sigaction sa;
memset(&sa, 0, sizeof(sa));
sa.sa_handler = LinuxRCInput_Navio::termination_handler;
sigaction(i, &sa, NULL);
}
}
//Initial setup of variables
LinuxRCInput_Navio::LinuxRCInput_Navio():
prev_tick(0),
delta_time(0),
curr_tick_inc(1000/RCIN_NAVIO_SAMPLE_FREQ),
curr_pointer(0),
curr_channel(0),
width_s0(0),
curr_signal(0),
last_signal(228),
state(RCIN_NAVIO_INITIAL_STATE)
{
int version = get_raspberry_pi_version();
set_physical_addresses(version);
//Init memory for buffer and for DMA control blocks. See comments in "init_ctrl_data()" to understand values "2" and "113"
circle_buffer = new Memory_table(RCIN_NAVIO_BUFFER_LENGTH * 2, version);
con_blocks = new Memory_table(RCIN_NAVIO_BUFFER_LENGTH * 113, version);
}
LinuxRCInput_Navio::~LinuxRCInput_Navio()
{
delete circle_buffer;
delete con_blocks;
}
void LinuxRCInput_Navio::deinit()
{
stop_dma();
}
//Initializing necessary registers
void LinuxRCInput_Navio::init_registers()
{
dma_reg = (uint32_t*)map_peripheral(dma_base, RCIN_NAVIO_DMA_LEN);
pcm_reg = (uint32_t*)map_peripheral(pcm_base, RCIN_NAVIO_PCM_LEN);
clk_reg = (uint32_t*)map_peripheral(clk_base, RCIN_NAVIO_CLK_LEN);
}
void LinuxRCInput_Navio::init(void*)
{
init_registers();
//Enable PPM input
enable_pin = hal.gpio->channel(PPM_INPUT_NAVIO);
enable_pin->mode(HAL_GPIO_INPUT);
//Configuration
set_sigaction();
init_ctrl_data();
init_PCM();
init_DMA();
//wait a bit to let DMA fill queues and come to stable sampling
hal.scheduler->delay(300);
//Reading first sample
curr_tick = *((uint64_t*) circle_buffer->get_page(circle_buffer->_virt_pages, curr_pointer));
prev_tick = curr_tick;
curr_pointer += 8;
curr_signal = *((uint8_t*) circle_buffer->get_page(circle_buffer->_virt_pages, curr_pointer)) & 0x10 ? 1 : 0;
last_signal = curr_signal;
curr_pointer++;
}
//Processing signal
void LinuxRCInput_Navio::_timer_tick()
{
int j;
void* x;
//Now we are getting address in which DMAC is writing at current moment
dma_cb_t* ad = (dma_cb_t*) con_blocks->get_virt_addr(dma_reg[RCIN_NAVIO_DMA_CONBLK_AD | RCIN_NAVIO_DMA_CHANNEL << 8]);
for(j = 1; j >= -1; j--){
x = circle_buffer->get_virt_addr((ad + j)->dst);
if(x != NULL) {
break;}
}
//How many bytes have DMA transfered (and we can process)?
counter = circle_buffer->bytes_available(curr_pointer, circle_buffer->get_offset(circle_buffer->_virt_pages, (uint32_t)x));
//We can't stay in method for a long time, because it may lead to delays
if (counter > RCIN_NAVIO_MAX_COUNTER) {
counter = RCIN_NAVIO_MAX_COUNTER;
}
//Processing ready bytes
for (;counter > 0x40;counter--) {
//Is it timer samle?
if (curr_pointer % (64) == 0) {
curr_tick = *((uint64_t*) circle_buffer->get_page(circle_buffer->_virt_pages, curr_pointer));
curr_pointer+=8;
counter-=8;
}
//Reading required bit
curr_signal = *((uint8_t*) circle_buffer->get_page(circle_buffer->_virt_pages, curr_pointer)) & 0x10 ? 1 : 0;
//If the signal changed
if (curr_signal != last_signal) {
delta_time = curr_tick - prev_tick;
prev_tick = curr_tick;
switch (state) {
case RCIN_NAVIO_INITIAL_STATE:
state = RCIN_NAVIO_ZERO_STATE;
break;
case RCIN_NAVIO_ZERO_STATE:
if (curr_signal == 0) {
width_s0 = (uint16_t) delta_time;
state = RCIN_NAVIO_ONE_STATE;
break;
}
else
break;
case RCIN_NAVIO_ONE_STATE:
if (curr_signal == 1) {
width_s1 = (uint16_t) delta_time;
state = RCIN_NAVIO_ZERO_STATE;
_process_rc_pulse(width_s0, width_s1);
break;
}
else
break;
}
}
last_signal = curr_signal;
curr_pointer++;
if (curr_pointer >= circle_buffer->get_page_count()*PAGE_SIZE) {
curr_pointer = 0;
}
curr_tick+=curr_tick_inc;
}
}
#endif // CONFIG_HAL_BOARD_SUBTYPE
Reference in New Issue View Git Blame Copy Permalink