From 51fd0b3d554327b173607d27819b1aefceb8ae2c Mon Sep 17 00:00:00 2001 From: Lucas De Marchi Date: Wed, 13 Jan 2016 11:08:30 -0200 Subject: [PATCH] AP_HAL_Linux: RCInput_RPI: fix coding style - Remove trailing whitespaces - Remove some uneeded comments - Fix indentation - Replace some breaks inside the loop by checking in the loop itself --- libraries/AP_HAL_Linux/RCInput_RPI.cpp | 157 ++++++++++++------------- 1 file changed, 72 insertions(+), 85 deletions(-) diff --git a/libraries/AP_HAL_Linux/RCInput_RPI.cpp b/libraries/AP_HAL_Linux/RCInput_RPI.cpp index 48ab957c71..f4e14175da 100644 --- a/libraries/AP_HAL_Linux/RCInput_RPI.cpp +++ b/libraries/AP_HAL_Linux/RCInput_RPI.cpp @@ -52,7 +52,7 @@ static uint16_t RcChnGpioTbl[RCIN_RPI_CHN_NUM] = { static uint16_t RcChnGpioTbl[RCIN_RPI_CHN_NUM] = { RPI_GPIO_4 }; -#endif // CONFIG_HAL_BOARD_SUBTYPE +#endif //Memory Addresses #define RCIN_RPI_RPI1_DMA_BASE 0x20007000 @@ -70,7 +70,7 @@ static uint16_t RcChnGpioTbl[RCIN_RPI_CHN_NUM] = { #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_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) @@ -124,7 +124,7 @@ Memory_table::Memory_table(uint32_t page_count, int version) _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); @@ -142,7 +142,7 @@ Memory_table::Memory_table(uint32_t page_count, int version) //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); + ::read(file, &pageInfo, 8); _phys_pages[i] = (void*)((uintptr_t)(pageInfo*PAGE_SIZE) | bus); } @@ -162,20 +162,19 @@ 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. +/* Return 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 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 +// 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)) { @@ -185,14 +184,13 @@ void* Memory_table::get_virt_addr(const uint32_t phys_addr) const 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. +// Return 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 ((uintptr_t) pages[i] == (addr & 0xFFFFF000) ) { - return (i*PAGE_SIZE + (addr & 0xFFF)); + return (i * PAGE_SIZE + (addr & 0xFFF)); } } return -1; @@ -203,8 +201,7 @@ uint32_t Memory_table::bytes_available(const uint32_t read_addr, const uint32_t { if (write_addr > read_addr) { return (write_addr - read_addr); - } - else { + } else { return _page_count * PAGE_SIZE - (read_addr - write_addr); } } @@ -221,8 +218,7 @@ void RCInput_RPI::set_physical_addresses(int version) 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; @@ -280,8 +276,8 @@ void RCInput_RPI::init_ctrl_data() uint32_t cbp = 0; dma_cb_t* cbp_curr; //Set fifo addr (for delay) - phys_fifo_addr = ((pcm_base + 0x04) & 0x00FFFFFF) | 0x7e000000; - + 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 @@ -298,51 +294,50 @@ void RCInput_RPI::init_ctrl_data() */ // fprintf(stderr, "ERROR SEARCH1\n"); - uint32_t i = 0; - for (i = 0; i < 7 * 128 * RCIN_RPI_BUFFER_LENGTH; i++) // 7 * 128 * 8 == 7168 - { - //Transfer timer every 7th sample - if(i % 7 == 0) { - cbp_curr = (dma_cb_t*)con_blocks->get_page(con_blocks->_virt_pages, cbp); + uint32_t i = 0; + for (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_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 (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); + } - 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) ) ); - // 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) + 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, + 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); } @@ -351,7 +346,7 @@ void RCInput_RPI::init_ctrl_data() /*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 @@ -379,7 +374,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 @@ -387,14 +382,14 @@ void RCInput_RPI::init_DMA() 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 + 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() { - for (int i = 0; i < 64; i++) { + 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)); @@ -408,9 +403,9 @@ RCInput_RPI::RCInput_RPI(): curr_tick_inc(1000/RCIN_RPI_SAMPLE_FREQ), curr_pointer(0), curr_channel(0) -{ +{ #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 - int version = 2; + int version = 2; #else int version = UtilRPI::from(hal.util)->get_rpi_version(); #endif @@ -435,14 +430,14 @@ void RCInput_RPI::deinit() //Initializing necessary registers void RCInput_RPI::init_registers() { - dma_reg = (uint32_t*)map_peripheral(dma_base, RCIN_RPI_DMA_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() { - uint64_t signal_states(0); + uint64_t signal_states = 0; init_registers(); @@ -481,17 +476,15 @@ void RCInput_RPI::init() void RCInput_RPI::_timer_tick() { int j; - void* x; + void *x; uint64_t signal_states(0); //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 != NULL) { - break;} + for (j = 1; j >= -1 && x == nullptr; j--) { + x = circle_buffer->get_virt_addr((ad + j)->dst); } - + //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, (uintptr_t)x)); //We can't stay in method for a long time, because it may lead to delays @@ -525,11 +518,8 @@ void RCInput_RPI::_timer_tick() 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; - } - else { - break; } + 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; @@ -540,12 +530,9 @@ void RCInput_RPI::_timer_tick() #else _process_rc_pulse(rc_channels[i].width_s0, rc_channels[i].width_s1); -#endif // CONFIG_HAL_BOARD_SUBTYPE - break; - } - else{ - break; +#endif } + break; } } rc_channels[i].last_signal = rc_channels[i].curr_signal; @@ -555,7 +542,7 @@ void RCInput_RPI::_timer_tick() 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