diff --git a/libraries/AP_HAL_Linux/RCInput_RPI.cpp b/libraries/AP_HAL_Linux/RCInput_RPI.cpp index 860196761b..e1b5aaded1 100644 --- a/libraries/AP_HAL_Linux/RCInput_RPI.cpp +++ b/libraries/AP_HAL_Linux/RCInput_RPI.cpp @@ -95,45 +95,45 @@ Memory_table::Memory_table() _page_count = 0; } -//Init Memory table +// 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 + // Cache coherent adresses depends on RPI's version uint32_t bus = version == 1 ? 0x40000000 : 0xC0000000; uint64_t pageInfo; - void* offset; + void *offset; - _virt_pages = (void**)malloc(page_count * sizeof(void*)); - _phys_pages = (void**)malloc(page_count * sizeof(void*)); + _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 | O_CLOEXEC)) < 0) { - fprintf(stderr,"Failed to open /dev/mem\n"); + fprintf(stderr, "Failed to open /dev/mem\n"); exit(-1); } if ((file = open("/proc/self/pagemap", O_RDWR | O_SYNC | O_CLOEXEC)) < 0) { - fprintf(stderr,"Failed to open /proc/self/pagemap\n"); + 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, ((uintptr_t)offset)/PAGE_SIZE*8, SEEK_SET); + // 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, ((uintptr_t)offset) / PAGE_SIZE * 8, SEEK_SET); - //Get list of available cache coherent physical addresses + // 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*)((uintptr_t)(pageInfo*PAGE_SIZE) | bus); + _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 *)((uintptr_t)(pageInfo * PAGE_SIZE) | bus); } - //Map physical addresses to virtual memory + // 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, ((uintptr_t)_phys_pages[i] & (version == 1 ? 0xFFFFFFFF : ~bus))); + _virt_pages[i] = mmap(_virt_pages[i], PAGE_SIZE, PROT_READ | PROT_WRITE, MAP_SHARED | MAP_FIXED | MAP_NORESERVE | MAP_LOCKED, fdMem, ((uintptr_t)_phys_pages[i] & (version == 1 ? 0xFFFFFFFF : ~bus))); memset(_virt_pages[i], 0xee, PAGE_SIZE); } close(file); @@ -146,31 +146,33 @@ Memory_table::~Memory_table() 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 +// 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 nullptr; } - return (uint8_t*)pages[(uint32_t) addr / 4096] + addr % 4096; + 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 +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 .. + // 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 ((uintptr_t) _phys_pages[i] == (((uintptr_t) phys_addr) & 0xFFFFF000)) { - return (void*) ((uintptr_t) _virt_pages[i] + (phys_addr & 0xFFF)); + if ((uintptr_t)_phys_pages[i] == (((uintptr_t)phys_addr) & 0xFFFFF000)) { + return (void *)((uintptr_t)_virt_pages[i] + (phys_addr & 0xFFF)); } } return nullptr; } -// FIXME: in-congruent function style see above -// This function returns offset from the beginning of the buffer using virtual address and memory_table. +// 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; @@ -182,13 +184,12 @@ uint32_t Memory_table::get_offset(void ** const pages, const uint32_t addr) cons return -1; } -//How many bytes are available for reading in circle buffer? +// 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 { + } else { return _page_count * PAGE_SIZE - (read_addr - write_addr); } } @@ -198,32 +199,31 @@ uint32_t Memory_table::get_page_count() const return _page_count; } -//Physical addresses of peripheral depends on Raspberry Pi's version +// Physical addresses of peripheral depends on Raspberry Pi's version void RCInput_RPI::set_physical_addresses(int version) { if (version == 1) { dma_base = RCIN_RPI_RPI1_DMA_BASE; clk_base = RCIN_RPI_RPI1_CLK_BASE; pcm_base = RCIN_RPI_RPI1_PCM_BASE; - } - else if (version == 2) { + } else if (version == 2) { dma_base = RCIN_RPI_RPI2_DMA_BASE; clk_base = RCIN_RPI_RPI2_CLK_BASE; pcm_base = RCIN_RPI_RPI2_PCM_BASE; } } -//Map peripheral to virtual memory -void* RCInput_RPI::map_peripheral(uint32_t base, uint32_t len) +// Map peripheral to virtual memory +void *RCInput_RPI::map_peripheral(uint32_t base, uint32_t len) { int fd = open("/dev/mem", O_RDWR | O_CLOEXEC); - void * vaddr; + void *vaddr; if (fd < 0) { printf("Failed to open /dev/mem: %m\n"); return nullptr; } - vaddr = mmap(nullptr, len, PROT_READ|PROT_WRITE, MAP_SHARED, fd, base); + vaddr = mmap(nullptr, 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); } @@ -232,8 +232,8 @@ void* RCInput_RPI::map_peripheral(uint32_t base, uint32_t len) return vaddr; } -//Method to init DMA control block -void RCInput_RPI::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) +// Method to init DMA control block +void RCInput_RPI::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; @@ -255,18 +255,18 @@ void RCInput_RPI::termination_handler(int signum) AP_HAL::panic("Interrupted: %s", strsignal(signum)); } - -//This function is used to init DMA control blocks (setting sampling GPIO register, destination adresses, synchronization) +// This function is used to init DMA control blocks (setting sampling GPIO +// register, destination adresses, synchronization) void RCInput_RPI::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. + 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" @@ -280,62 +280,58 @@ void RCInput_RPI::init_ctrl_data() 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_RPI_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); + for (uint32_t i = 0; i < 56 * 128 * RCIN_RPI_BUFFER_LENGTH; i++) { + //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_RPI_DMA_NO_WIDE_BURSTS | RCIN_RPI_DMA_WAIT_RESP | RCIN_RPI_DMA_DEST_INC | RCIN_RPI_DMA_SRC_INC, RCIN_RPI_TIMER_BASE, - (uintptr_t) circle_buffer->get_page(circle_buffer->_phys_pages, dest), - 8, - 0, - (uintptr_t) con_blocks->get_page(con_blocks->_phys_pages, - cbp + sizeof(dma_cb_t) ) ); - - dest += 8; - cbp += sizeof(dma_cb_t); - } + init_dma_cb(&cbp_curr, RCIN_RPI_DMA_NO_WIDE_BURSTS | RCIN_RPI_DMA_WAIT_RESP | RCIN_RPI_DMA_DEST_INC | RCIN_RPI_DMA_SRC_INC, RCIN_RPI_TIMER_BASE, + (uintptr_t)circle_buffer->get_page(circle_buffer->_phys_pages, dest), + 8, + 0, + (uintptr_t)con_blocks->get_page(con_blocks->_phys_pages, + 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_RPI_DMA_NO_WIDE_BURSTS | RCIN_RPI_DMA_WAIT_RESP, RCIN_RPI_GPIO_LEV0_ADDR, - (uintptr_t) circle_buffer->get_page(circle_buffer->_phys_pages, dest), - 1, - 0, - (uintptr_t) con_blocks->get_page(con_blocks->_phys_pages, - cbp + sizeof(dma_cb_t) ) ); - - dest += 1; - cbp += sizeof(dma_cb_t); + dest += 8; + 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, + // Transfer GPIO (1 byte) + cbp_curr = (dma_cb_t *)con_blocks->get_page(con_blocks->_virt_pages, cbp); + init_dma_cb(&cbp_curr, RCIN_RPI_DMA_NO_WIDE_BURSTS | RCIN_RPI_DMA_WAIT_RESP, RCIN_RPI_GPIO_LEV0_ADDR, + (uintptr_t)circle_buffer->get_page(circle_buffer->_phys_pages, dest), + 1, + 0, + (uintptr_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_RPI_DMA_NO_WIDE_BURSTS | RCIN_RPI_DMA_WAIT_RESP | RCIN_RPI_DMA_D_DREQ | RCIN_RPI_DMA_PER_MAP(2), - RCIN_RPI_TIMER_BASE, phys_fifo_addr, - 4, - 0, - (uintptr_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 = (uintptr_t) con_blocks->get_page(con_blocks->_phys_pages, 0); -} + cbp_curr = (dma_cb_t *)con_blocks->get_page(con_blocks->_virt_pages, cbp); + init_dma_cb(&cbp_curr, RCIN_RPI_DMA_NO_WIDE_BURSTS | RCIN_RPI_DMA_WAIT_RESP | RCIN_RPI_DMA_D_DREQ | RCIN_RPI_DMA_PER_MAP(2), + RCIN_RPI_TIMER_BASE, phys_fifo_addr, + 4, + 0, + (uintptr_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 = (uintptr_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 RCInput_RPI::init_PCM() { pcm_reg[RCIN_RPI_PCM_CS_A] = 1; // Disable Rx+Tx, Enable PCM block @@ -363,7 +359,7 @@ void RCInput_RPI::init_PCM() /*Initialise DMA See BCM2835 documentation: http://www.raspberrypi.org/wp-content/uploads/2012/02/BCM2835-ARM-Peripherals.pdf -*/ + */ void RCInput_RPI::init_DMA() { dma_reg[RCIN_RPI_DMA_CS | RCIN_RPI_DMA_CHANNEL << 8] = RCIN_RPI_DMA_RESET; //Reset DMA @@ -375,7 +371,7 @@ void RCInput_RPI::init_DMA() } -//We must stop DMA when the process is killed +// We must stop DMA when the process is killed void RCInput_RPI::set_sigaction() { for (int i = 0; i < NSIG; i++) { @@ -393,7 +389,7 @@ void RCInput_RPI::set_sigaction() } } -//Initial setup of variables +// Initial setup of variables RCInput_RPI::RCInput_RPI(): circle_buffer{nullptr}, con_blocks{nullptr}, @@ -423,9 +419,9 @@ void RCInput_RPI::teardown() //Initializing necessary registers void RCInput_RPI::init_registers() { - dma_reg = (uint32_t*)map_peripheral(dma_base, RCIN_RPI_DMA_LEN); - pcm_reg = (uint32_t*)map_peripheral(pcm_base, RCIN_RPI_PCM_LEN); - clk_reg = (uint32_t*)map_peripheral(clk_base, RCIN_RPI_CLK_LEN); + dma_reg = (uint32_t *)map_peripheral(dma_base, RCIN_RPI_DMA_LEN); + pcm_reg = (uint32_t *)map_peripheral(pcm_base, RCIN_RPI_PCM_LEN); + clk_reg = (uint32_t *)map_peripheral(clk_base, RCIN_RPI_CLK_LEN); } void RCInput_RPI::init() @@ -440,68 +436,74 @@ void RCInput_RPI::init() con_blocks = new Memory_table(RCIN_RPI_BUFFER_LENGTH * 113, version); init_registers(); - - //Enable PPM input + + // Enable PPM input enable_pin = hal.gpio->channel(PPM_INPUT_RPI); enable_pin->mode(HAL_GPIO_INPUT); - //Configuration + // Configuration set_sigaction(); init_ctrl_data(); init_PCM(); init_DMA(); - //wait a bit to let DMA fill queues and come to stable sampling + // 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)); + // 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; + curr_signal = *((uint8_t *)circle_buffer->get_page(circle_buffer->_virt_pages, curr_pointer)) & 0x10 ? 1 : 0; last_signal = curr_signal; curr_pointer++; _initialized = true; } - -//Processing signal +// Processing signal void RCInput_RPI::_timer_tick() { - int j; - void* x; + uint32_t counter = 0; if (!_initialized) { return; } - //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_RPI_DMA_CONBLK_AD | RCIN_RPI_DMA_CHANNEL << 8]); - for(j = 1; j >= -1; j--){ - x = circle_buffer->get_virt_addr((ad + j)->dst); - if(x != nullptr) { - break;} + // 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_RPI_DMA_CONBLK_AD | RCIN_RPI_DMA_CHANNEL << 8]); + for (int j = 1; j >= -1; j--) { + void *x = circle_buffer->get_virt_addr((ad + j)->dst); + if (x != nullptr) { + counter = circle_buffer->bytes_available(curr_pointer, + circle_buffer->get_offset(circle_buffer->_virt_pages, (uintptr_t)x)); + break; + } } - - //How many bytes have DMA transferred (and we can process)? - counter = circle_buffer->bytes_available(curr_pointer, circle_buffer->get_offset(circle_buffer->_virt_pages, (uintptr_t)x)); - //We can't stay in method for a long time, because it may lead to delays + + if (counter == 0) { + return; + } + + // How many bytes have DMA transferred (and we can process)? + // We can't stay in method for a long time, because it may lead to delays if (counter > RCIN_RPI_MAX_COUNTER) { counter = RCIN_RPI_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; + // Processing ready bytes + for (; counter > 0x40; counter--) { + // Is it timer sample? + 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 + + // 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; @@ -511,29 +513,27 @@ void RCInput_RPI::_timer_tick() break; case RCIN_RPI_ZERO_STATE: if (curr_signal == 0) { - width_s0 = (uint16_t) delta_time; + width_s0 = (uint16_t)delta_time; state = RCIN_RPI_ONE_STATE; - break; } - else - break; + break; case RCIN_RPI_ONE_STATE: if (curr_signal == 1) { - width_s1 = (uint16_t) delta_time; + width_s1 = (uint16_t)delta_time; state = RCIN_RPI_ZERO_STATE; _process_rc_pulse(width_s0, width_s1); - break; } - else - break; + break; } } + last_signal = curr_signal; curr_pointer++; - if (curr_pointer >= circle_buffer->get_page_count()*PAGE_SIZE) { + if (curr_pointer >= circle_buffer->get_page_count() * PAGE_SIZE) { curr_pointer = 0; } - curr_tick+=curr_tick_inc; + curr_tick += curr_tick_inc; } } -#endif // CONFIG_HAL_BOARD_SUBTYPE + +#endif diff --git a/libraries/AP_HAL_Linux/RCInput_RPI.h b/libraries/AP_HAL_Linux/RCInput_RPI.h index 7b455a21a3..3b5db726ba 100644 --- a/libraries/AP_HAL_Linux/RCInput_RPI.h +++ b/libraries/AP_HAL_Linux/RCInput_RPI.h @@ -104,7 +104,6 @@ private: uint32_t curr_tick_inc; uint32_t curr_pointer; uint32_t curr_channel; - uint32_t counter; uint16_t width_s0; uint16_t width_s1;