#include #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DARK || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_OBAL_V1 || \ CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_CANZERO #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "GPIO.h" #include "RCInput_RPI.h" #ifdef DEBUG #define debug(fmt, args ...) do { fprintf(stderr,"[RCInput_RPI]: %s:%d: " fmt, __FUNCTION__, __LINE__, ## args); } while (0) #else #define debug(fmt, args ...) #endif #define PAGE_SIZE (4*1024) //Parametres #define RCIN_RPI_BUFFER_LENGTH 4 #define RCIN_RPI_SAMPLE_FREQ 125 #define RCIN_RPI_DMA_CHANNEL 0 #define RCIN_RPI_MAX_SIZE_LINE 50 #define RCIN_RPI_MAX_COUNTER (RCIN_RPI_BUFFER_LENGTH * PAGE_SIZE * 2) // 1 circle_buffer #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH #define RCIN_RPI_SIG_HIGH 0 #define RCIN_RPI_SIG_LOW 1 // Each gpio stands for a rcinput channel, // the first one in RcChnGpioTbl is channel 1 in receiver static uint16_t RcChnGpioTbl[RCIN_RPI_CHN_NUM] = { RPI_GPIO_<5>(), RPI_GPIO_<6>(), RPI_GPIO_<12>(), RPI_GPIO_<13>(), RPI_GPIO_<19>(), RPI_GPIO_<20>(), RPI_GPIO_<21>(), RPI_GPIO_<26>() }; #elif (CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_OBAL_V1) #define RCIN_RPI_SIG_HIGH 0 #define RCIN_RPI_SIG_LOW 1 static uint16_t RcChnGpioTbl[RCIN_RPI_CHN_NUM] = { RPI_GPIO_<5>(), RPI_GPIO_<6>(), RPI_GPIO_<13>(), RPI_GPIO_<19>(), RPI_GPIO_<26>(), RPI_GPIO_<21>(), RPI_GPIO_<20>(), RPI_GPIO_<16>() }; #else #define RCIN_RPI_SIG_HIGH 1 #define RCIN_RPI_SIG_LOW 0 static uint16_t RcChnGpioTbl[RCIN_RPI_CHN_NUM] = { #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO #define PAGE_SIZE (4*1024) NAVIO_GPIO_PPM_IN #else RPI_GPIO_<4>() #endif }; #endif // CONFIG_HAL_BOARD_SUBTYPE //Memory Addresses #define RCIN_RPI_RPI0_DMA_BASE 0x20007000 #define RCIN_RPI_RPI0_CLK_BASE 0x20101000 #define RCIN_RPI_RPI0_PCM_BASE 0x20203000 #define RCIN_RPI_RPI1_DMA_BASE 0x20007000 #define RCIN_RPI_RPI1_CLK_BASE 0x20101000 #define RCIN_RPI_RPI1_PCM_BASE 0x20203000 #define RCIN_RPI_RPI2_DMA_BASE 0x3F007000 #define RCIN_RPI_RPI2_CLK_BASE 0x3F101000 #define RCIN_RPI_RPI2_PCM_BASE 0x3F203000 #define RCIN_RPI_RPI3_DMA_BASE 0x3F007000 #define RCIN_RPI_RPI3_CLK_BASE 0x3F101000 #define RCIN_RPI_RPI3_PCM_BASE 0x3F203000 #define RCIN_RPI_RPI4_DMA_BASE 0xFE007000 #define RCIN_RPI_RPI4_CLK_BASE 0xFE101000 #define RCIN_RPI_RPI4_PCM_BASE 0xFE203000 #define RCIN_RPI_GPIO_LEV0_ADDR 0x7e200034 #define RCIN_RPI_DMA_LEN 0x1000 #define RCIN_RPI_CLK_LEN 0xA8 #define RCIN_RPI_PCM_LEN 0x24 #define RCIN_RPI_TIMER_BASE 0x7e003004 #define RCIN_RPI_DMA_SRC_INC (1<<8) #define RCIN_RPI_DMA_DEST_INC (1<<4) #define RCIN_RPI_DMA_NO_WIDE_BURSTS (1<<26) #define RCIN_RPI_DMA_WAIT_RESP (1<<3) #define RCIN_RPI_DMA_D_DREQ (1<<6) #define RCIN_RPI_DMA_PER_MAP(x) ((x)<<16) #define RCIN_RPI_DMA_END (1<<1) #define RCIN_RPI_DMA_RESET (1<<31) #define RCIN_RPI_DMA_INT (1<<2) #define RCIN_RPI_DMA_CS (0x00/4) #define RCIN_RPI_DMA_CONBLK_AD (0x04/4) #define RCIN_RPI_DMA_DEBUG (0x20/4) #define RCIN_RPI_PCM_CS_A (0x00/4) #define RCIN_RPI_PCM_FIFO_A (0x04/4) #define RCIN_RPI_PCM_MODE_A (0x08/4) #define RCIN_RPI_PCM_RXC_A (0x0c/4) #define RCIN_RPI_PCM_TXC_A (0x10/4) #define RCIN_RPI_PCM_DREQ_A (0x14/4) #define RCIN_RPI_PCM_INTEN_A (0x18/4) #define RCIN_RPI_PCM_INT_STC_A (0x1c/4) #define RCIN_RPI_PCM_GRAY (0x20/4) #define RCIN_RPI_PCMCLK_CNTL 0x26 #define RCIN_RPI_PCMCLK_DIV 0x27 #define RCIN_RPI_PLL_CLK 50000 /* * Defaults to 107.14MHz in the firmware setup. * The PLL channel that used to generate the 100MHz now runs at 750MHz instead of 500, * so the fw uses 750/7. * see: https://www.raspberrypi.org/forums/viewtopic.php?t=251902 */ #define RCIN_RPI4_PLL_CLK 70000 extern const AP_HAL::HAL& hal; using namespace Linux; volatile uint32_t *RCInput_RPI::pcm_reg; volatile uint32_t *RCInput_RPI::clk_reg; volatile uint32_t *RCInput_RPI::dma_reg; Memory_table::Memory_table() { _page_count = 0; } // Init Memory table Memory_table::Memory_table(const uint32_t page_count, const LINUX_BOARD_TYPE version) { uint32_t i; int fdMem, file; // Cache coherent adresses depends on RPI's version uint32_t bus = version == LINUX_BOARD_TYPE::RPI_ZERO_1 ? 0x40000000 : 0xC0000000; uint64_t pageInfo; void *offset; _virt_pages = (void **)calloc(page_count, sizeof(void *)); _phys_pages = (void **)calloc(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"); printf("Make sure that CONFIG_STRICT_DEVMEM is disabled\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"); 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); // 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); if (_virt_pages[i] == MAP_FAILED) { fprintf(stderr, "Failed to map cache coherent physical page (%m)\n"); exit(-1); } if (::read(file, &pageInfo, 8) < 8) { fprintf(stderr, "Failed to read pagemap\n"); exit(-1); } _phys_pages[i] = (void *)((uintptr_t)(pageInfo * PAGE_SIZE) | bus); } // Map physical addresses to virtual memory for (i = 0; i < _page_count; i++) { munmap(_virt_pages[i], PAGE_SIZE); void * prev_virt = _virt_pages[i]; _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 == LINUX_BOARD_TYPE::RPI_ZERO_1 ? 0xFFFFFFFFFFFFFFFF : ~bus))); if (_virt_pages[i] == MAP_FAILED) { fprintf(stderr, "Failed phys2virt prev_virt=%p phys_page=%p %m\n", prev_virt, _phys_pages[i]); printf("Make sure that CONFIG_STRICT_DEVMEM is disabled\n"); exit(-1); } 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 nullptr; } return (uint8_t *)pages[(uint32_t)addr / PAGE_SIZE] + addr % PAGE_SIZE; } //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 ((uintptr_t)_phys_pages[i] == (((uintptr_t)phys_addr) & 0xFFFFF000)) { return (void *)((uintptr_t)_virt_pages[i] + (phys_addr & 0xFFF)); } } return nullptr; } // 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 uint64_t addr) const { uint32_t i = 0; for (; i < _page_count; i++) { if ((uintptr_t) pages[i] == (addr & 0xFFFFFFFFFFFFF000) ) { 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; } // Physical addresses of peripheral depends on Raspberry Pi's version void RCInput_RPI::set_physical_addresses() { if (_version == LINUX_BOARD_TYPE::RPI_ZERO_1) { // 1 & zero are the same dma_base = RCIN_RPI_RPI1_DMA_BASE; clk_base = RCIN_RPI_RPI1_CLK_BASE; pcm_base = RCIN_RPI_RPI1_PCM_BASE; } else if (_version == LINUX_BOARD_TYPE::RPI_2_3_ZERO2) { // 2 & 3 are the same dma_base = RCIN_RPI_RPI2_DMA_BASE; clk_base = RCIN_RPI_RPI2_CLK_BASE; pcm_base = RCIN_RPI_RPI2_PCM_BASE; } else if (_version == LINUX_BOARD_TYPE::RPI_4) { dma_base = RCIN_RPI_RPI4_DMA_BASE; clk_base = RCIN_RPI_RPI4_CLK_BASE; pcm_base = RCIN_RPI_RPI4_PCM_BASE; } else { fprintf(stderr,"Unknown Linux Board version!\n"); exit(-1); } } // 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; if (fd < 0) { printf("Failed to open /dev/mem: %m\n"); printf("Make sure that CONFIG_STRICT_DEVMEM is disabled\n"); return nullptr; } 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); exit(-1); } close(fd); 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) { (*cbp)->info = mode; (*cbp)->src = source; (*cbp)->dst = dest; (*cbp)->length = length; (*cbp)->next = next_cb; (*cbp)->stride = stride; } void RCInput_RPI::stop_dma() { dma_reg[RCIN_RPI_DMA_CS | RCIN_RPI_DMA_CHANNEL << 8] = 0; } /* We need to be sure that the DMA is stopped upon termination */ void RCInput_RPI::termination_handler(int signum) { stop_dma(); AP_HAL::panic("Interrupted: %s", strsignal(signum)); } // 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. /* We are transferring 8 bytes of GPIO register. Every 7th iteration we are sampling TIMER register, which length is 8 bytes. So, for every 7 samples of GPIO we need 7 * 8 + 8 = 64 bytes of buffer. Value 7 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 7 iterations (64 bytes of buffer) we need 7 control blocks for GPIO sampling, 7 control blocks for setting frequency and 1 control block for sampling timer, so, we need 7 + 7 + 1 = 15 control blocks. For integer value, we need 15 pages of control blocks. Each control block length is 32 bytes. In 15 pages we will have (15 * 4096 / 32) = 15 * 128 control blocks. 15 * 128 control blocks = 64 * 128 bytes of buffer = 2 pages of buffer. So, for 7 * 64 * 2 iteration we init DMA for sampling GPIO and timer to ((7 * 8 + 8) * 64 * 2) = 8192 bytes = 2 pages of buffer. */ for (uint32_t i = 0; i < 7 * 128 * RCIN_RPI_BUFFER_LENGTH; i++) { // Transfer timer every 7th sample if (i % 7 == 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_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); } // Transfer GPIO (8 bytes) 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), 8, 0, (uintptr_t)con_blocks->get_page(con_blocks->_phys_pages, 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.25 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); } /*Initialise PCM See BCM2835 documentation: http://www.raspberrypi.org/wp-content/uploads/2012/02/BCM2835-ARM-Peripherals.pdf See BCM2711 documentation: https://datasheets.raspberrypi.org/bcm2711/bcm2711-peripherals.pdf */ void RCInput_RPI::init_PCM() { pcm_reg[RCIN_RPI_PCM_CS_A] = 1; // Disable Rx+Tx, Enable PCM block hal.scheduler->delay_microseconds(100); clk_reg[RCIN_RPI_PCMCLK_CNTL] = 0x5A000006; // Source=PLLD (500MHz) hal.scheduler->delay_microseconds(100); if (_version != LINUX_BOARD_TYPE::RPI_4) { clk_reg[RCIN_RPI_PCMCLK_DIV] = 0x5A000000 | ((RCIN_RPI_PLL_CLK/RCIN_RPI_SAMPLE_FREQ)<<12); // Set pcm div for BCM2835 500MHZ clock. If we need to configure DMA frequency. } else { // RPI-4 clk_reg[RCIN_RPI_PCMCLK_DIV] = 0x5A000000 | ((RCIN_RPI4_PLL_CLK/RCIN_RPI_SAMPLE_FREQ)<< 12); // Set pcm div for BCM2711 700MHz clock. If we need to configure DMA frequency. } hal.scheduler->delay_microseconds(100); clk_reg[RCIN_RPI_PCMCLK_CNTL] = 0x5A000016; // Source=PLLD and enable hal.scheduler->delay_microseconds(100); pcm_reg[RCIN_RPI_PCM_TXC_A] = 0<<31 | 1<<30 | 0<<20 | 0<<16; // 1 channel, 8 bits hal.scheduler->delay_microseconds(100); pcm_reg[RCIN_RPI_PCM_MODE_A] = (10 - 1) << 10; //PCM mode 0b0010010000000000 hal.scheduler->delay_microseconds(100); pcm_reg[RCIN_RPI_PCM_CS_A] |= 1<<4 | 1<<3; // Clear FIFOs hal.scheduler->delay_microseconds(100); pcm_reg[RCIN_RPI_PCM_DREQ_A] = 64<<24 | 64<<8; // DMA Req when one slot is free? hal.scheduler->delay_microseconds(100); pcm_reg[RCIN_RPI_PCM_CS_A] |= 1<<9; // Enable DMA hal.scheduler->delay_microseconds(100); pcm_reg[RCIN_RPI_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 RCInput_RPI::init_DMA() { dma_reg[RCIN_RPI_DMA_CS | RCIN_RPI_DMA_CHANNEL << 8] = RCIN_RPI_DMA_RESET; //Reset DMA hal.scheduler->delay_microseconds(100); dma_reg[RCIN_RPI_DMA_CS | RCIN_RPI_DMA_CHANNEL << 8] = RCIN_RPI_DMA_INT | RCIN_RPI_DMA_END; dma_reg[RCIN_RPI_DMA_CONBLK_AD | RCIN_RPI_DMA_CHANNEL << 8] = reinterpret_cast(con_blocks->get_page(con_blocks->_phys_pages, 0));//Set first control block address dma_reg[RCIN_RPI_DMA_DEBUG | RCIN_RPI_DMA_CHANNEL << 8] = 7; // clear debug error flags dma_reg[RCIN_RPI_DMA_CS | RCIN_RPI_DMA_CHANNEL << 8] = 0x10880001; // go, mid priority, wait for outstanding writes } // We must stop DMA when the process is killed void RCInput_RPI::set_sigaction() { struct sigaction sa, sa_old; memset(&sa_old, 0, sizeof(sa)); memset(&sa, 0, sizeof(sa)); /* Ignore signals */ sa.sa_handler = SIG_IGN; sigaction(SIGWINCH, &sa, nullptr); sigaction(SIGTTOU, &sa, nullptr); sigaction(SIGTTIN, &sa, nullptr); /* * Catch all other signals to ensure DMA is disabled - some of them may * already be handled elsewhere in cases we consider normal termination. * In those cases the teardown() method must be called. */ for (int i = 0; i < NSIG; i++) { sigaction(i, nullptr, &sa_old); if (sa_old.sa_handler == nullptr) { sa.sa_handler = RCInput_RPI::termination_handler; sigaction(i, &sa, nullptr); } } } // Initial setup of variables RCInput_RPI::RCInput_RPI(): curr_tick_inc(1000/RCIN_RPI_SAMPLE_FREQ), curr_pointer(0), curr_channel(0) { } RCInput_RPI::~RCInput_RPI() { delete circle_buffer; delete con_blocks; } void RCInput_RPI::teardown() { stop_dma(); } //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); } void RCInput_RPI::init() { uint64_t signal_states(0); #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 _version = LINUX_BOARD_TYPE::RPI_2_3_ZERO2; #else _version = UtilRPI::from(hal.util)->detect_linux_board_type(); #endif set_physical_addresses(); // Init memory for buffer and for DMA control blocks. // See comments in "init_ctrl_data()" to understand values "2" and "15" circle_buffer = NEW_NOTHROW Memory_table(RCIN_RPI_BUFFER_LENGTH * 2, _version); con_blocks = NEW_NOTHROW Memory_table(RCIN_RPI_BUFFER_LENGTH * 15, _version); init_registers(); // Enable PPM or PWM input for (uint32_t i = 0; i < RCIN_RPI_CHN_NUM; ++i) { rc_channels[i].enable_pin = hal.gpio->channel(RcChnGpioTbl[i]); rc_channels[i].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)); curr_pointer += 8; signal_states = *((uint64_t *)circle_buffer->get_page(circle_buffer->_virt_pages, curr_pointer)); for (uint32_t i = 0; i < RCIN_RPI_CHN_NUM; ++i) { rc_channels[i].prev_tick = curr_tick; rc_channels[i].curr_signal = (signal_states & (1 << RcChnGpioTbl[i])) ? RCIN_RPI_SIG_HIGH : RCIN_RPI_SIG_LOW; rc_channels[i].last_signal = rc_channels[i].curr_signal; } curr_pointer += 8; set_num_channels(RCIN_RPI_CHN_NUM); _initialized = true; } // Processing signal void RCInput_RPI::_timer_tick() { uint32_t counter = 0; uint64_t signal_states(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]); if (!ad) { debug("DMA sampling stopped, restarting...\n"); init_ctrl_data(); init_PCM(); init_DMA(); return; } const uint32_t offset = con_blocks->get_offset(con_blocks->_virt_pages,(uintptr_t)ad); for (int j = 1; j >= -1; j--) { // Get address of next or previous (dma_cb_t) ad = (dma_cb_t *)con_blocks->get_page(con_blocks->_virt_pages,offset + (uint32_t)(sizeof(dma_cb_t) * j)); if (!ad) { continue ; } void *x = circle_buffer->get_virt_addr((ad)->dst); if (x != nullptr) { counter = circle_buffer->bytes_available(curr_pointer, circle_buffer->get_offset(circle_buffer->_virt_pages, (uintptr_t)x)); break; } } 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) { debug("%5d sample(s) dropped\n", (counter - RCIN_RPI_MAX_COUNTER) / 0x8); counter = RCIN_RPI_MAX_COUNTER; } // Processing ready bytes for (;counter > 0x40;) { // 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 signal_states = *((uint64_t *)circle_buffer->get_page(circle_buffer->_virt_pages, curr_pointer)); for (uint32_t i = 0; i < RCIN_RPI_CHN_NUM; ++i) { rc_channels[i].curr_signal = (signal_states & (1 << RcChnGpioTbl[i])) ? RCIN_RPI_SIG_HIGH : RCIN_RPI_SIG_LOW; // If the signal changed if (rc_channels[i].curr_signal != rc_channels[i].last_signal) { rc_channels[i].delta_time = curr_tick - rc_channels[i].prev_tick; rc_channels[i].prev_tick = curr_tick; switch (rc_channels[i].state) { case RCIN_RPI_INITIAL_STATE: rc_channels[i].state = RCIN_RPI_ZERO_STATE; break; case RCIN_RPI_ZERO_STATE: if (rc_channels[i].curr_signal == 0) { rc_channels[i].width_s0 = (uint16_t)rc_channels[i].delta_time; rc_channels[i].state = RCIN_RPI_ONE_STATE; } break; case RCIN_RPI_ONE_STATE: if (rc_channels[i].curr_signal == 1) { rc_channels[i].width_s1 = (uint16_t)rc_channels[i].delta_time; rc_channels[i].state = RCIN_RPI_ZERO_STATE; if (1 == RCIN_RPI_CHN_NUM) { _process_rc_pulse(rc_channels[i].width_s0, rc_channels[i].width_s1); } else { _process_pwm_pulse(i, rc_channels[i].width_s0, rc_channels[i].width_s1); } } break; } } rc_channels[i].last_signal = rc_channels[i].curr_signal; } curr_pointer += 8; counter -= 8; if (curr_pointer >= circle_buffer->get_page_count() * PAGE_SIZE) { curr_pointer = 0; } curr_tick += curr_tick_inc; } } #endif // CONFIG_HAL_BOARD_SUBTYPE