Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-04 15:08:39 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_Linux: RCInput_RPI: fix coding style

Browse Source 
- Remove trailing whitespaces
 - Remove some uneeded comments
 - Fix indentation
 - Replace some breaks inside the loop by checking in the loop itself
This commit is contained in:
Lucas De Marchi 2016-01-13 11:08:30 -02:00
parent 79d56073f7
commit 51fd0b3d55
1 changed files with 72 additions and 85 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

157
libraries/AP_HAL_Linux/RCInput_RPI.cpp
View File

@ -52,7 +52,7 @@ static uint16_t RcChnGpioTbl[RCIN_RPI_CHN_NUM] = {
static uint16_t RcChnGpioTbl[RCIN_RPI_CHN_NUM] = { static uint16_t RcChnGpioTbl[RCIN_RPI_CHN_NUM] = {
RPI_GPIO_4 RPI_GPIO_4
}; };
#endif // CONFIG_HAL_BOARD_SUBTYPE #endif
//Memory Addresses //Memory Addresses
#define RCIN_RPI_RPI1_DMA_BASE 0x20007000 #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_TIMER_BASE 0x7e003004
#define RCIN_RPI_DMA_SRC_INC (1<<8) #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_NO_WIDE_BURSTS (1<<26)
#define RCIN_RPI_DMA_WAIT_RESP (1<<3) #define RCIN_RPI_DMA_WAIT_RESP (1<<3)
#define RCIN_RPI_DMA_D_DREQ (1<<6) #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*)); _virt_pages = (void**)malloc(page_count * sizeof(void*));
_phys_pages = (void**)malloc(page_count * sizeof(void*)); _phys_pages = (void**)malloc(page_count * sizeof(void*));
_page_count = page_count; _page_count = page_count;
if ((fdMem = open("/dev/mem", O_RDWR | O_SYNC)) < 0) { if ((fdMem = open("/dev/mem", O_RDWR | O_SYNC)) < 0) {
fprintf(stderr,"Failed to open /dev/mem\n"); fprintf(stderr,"Failed to open /dev/mem\n");
exit(-1); exit(-1);
@ -142,7 +142,7 @@ Memory_table::Memory_table(uint32_t page_count, int version)
//Get list of available cache coherent physical addresses //Get list of available cache coherent physical addresses
for (i = 0; i < _page_count; i++) { 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); _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); _phys_pages[i] = (void*)((uintptr_t)(pageInfo*PAGE_SIZE) | bus);
} }
@ -162,20 +162,19 @@ Memory_table::~Memory_table()
free(_phys_pages); 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 void* Memory_table::get_page(void** const pages, uint32_t addr) const
{ {
if (addr >= PAGE_SIZE * _page_count) { if (addr >= PAGE_SIZE * _page_count) {
return NULL; return NULL;
} }
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. // 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 ..
uint32_t i = 0; uint32_t i = 0;
for (; i < _page_count; i++) { for (; i < _page_count; i++) {
if ((uintptr_t) _phys_pages[i] == (((uintptr_t) phys_addr) & 0xFFFFF000)) { 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; return NULL;
} }
// FIXME: in-congruent function style see above // Return 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 Memory_table::get_offset(void ** const pages, const uint32_t addr) const
{ {
uint32_t i = 0; uint32_t i = 0;
for (; i < _page_count; i++) { for (; i < _page_count; i++) {
if ((uintptr_t) pages[i] == (addr & 0xFFFFF000) ) { if ((uintptr_t) pages[i] == (addr & 0xFFFFF000) ) {
return (i*PAGE_SIZE + (addr & 0xFFF)); return (i * PAGE_SIZE + (addr & 0xFFF));
} }
} }
return -1; 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) { if (write_addr > read_addr) {
return (write_addr - read_addr); return (write_addr - read_addr);
} } else {
else {
return _page_count * PAGE_SIZE - (read_addr - write_addr); 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; dma_base = RCIN_RPI_RPI1_DMA_BASE;
clk_base = RCIN_RPI_RPI1_CLK_BASE; clk_base = RCIN_RPI_RPI1_CLK_BASE;
pcm_base = RCIN_RPI_RPI1_PCM_BASE; pcm_base = RCIN_RPI_RPI1_PCM_BASE;
} } else if (version == 2) {
else if (version == 2) {
dma_base = RCIN_RPI_RPI2_DMA_BASE; dma_base = RCIN_RPI_RPI2_DMA_BASE;
clk_base = RCIN_RPI_RPI2_CLK_BASE; clk_base = RCIN_RPI_RPI2_CLK_BASE;
pcm_base = RCIN_RPI_RPI2_PCM_BASE; pcm_base = RCIN_RPI_RPI2_PCM_BASE;
@ -280,8 +276,8 @@ void RCInput_RPI::init_ctrl_data()
uint32_t cbp = 0; uint32_t cbp = 0;
dma_cb_t* cbp_curr; dma_cb_t* cbp_curr;
//Set fifo addr (for delay) //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. //Init dma control blocks.
/*We are transferring 8 bytes of GPIO register. Every 7th iteration we are /*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 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"); // fprintf(stderr, "ERROR SEARCH1\n");
uint32_t i = 0; uint32_t i = 0;
for (i = 0; i < 7 * 128 * RCIN_RPI_BUFFER_LENGTH; i++) // 7 * 128 * 8 == 7168 for (i = 0; i < 7 * 128 * RCIN_RPI_BUFFER_LENGTH; i++) {
{ //Transfer timer every 7th sample
//Transfer timer every 7th sample if (i % 7 == 0) {
if(i % 7 == 0) { cbp_curr = (dma_cb_t*)con_blocks->get_page(con_blocks->_virt_pages, cbp);
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, 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), (uintptr_t) circle_buffer->get_page(circle_buffer->_phys_pages, dest),
8, 8,
0, 0,
(uintptr_t) con_blocks->get_page(con_blocks->_phys_pages, (uintptr_t) con_blocks->get_page(con_blocks->_phys_pages,
cbp + sizeof(dma_cb_t) ) ); cbp + sizeof(dma_cb_t) ) );
dest += 8;
cbp += sizeof(dma_cb_t);
}
// Transfer GPIO (8 bytes) dest += 8;
cbp_curr = (dma_cb_t*)con_blocks->get_page(con_blocks->_virt_pages, cbp); cbp += sizeof(dma_cb_t);
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; // Transfer GPIO (8 bytes)
cbp += sizeof(dma_cb_t); 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) dest += 8;
/* DMA is waiting data request signal (DREQ) from PCM. PCM is set for 1 MhZ freqency, so, cbp += sizeof(dma_cb_t);
each sample of GPIO is limited by writing to PCA queue.
*/ // Delay (for setting sampling frequency)
cbp_curr = (dma_cb_t*)con_blocks->get_page(con_blocks->_virt_pages, cbp); /* DMA is waiting data request signal (DREQ) from PCM. PCM is set for 1 MhZ freqency, so,
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), each sample of GPIO is limited by writing to PCA queue.
RCIN_RPI_TIMER_BASE, phys_fifo_addr, */
4, cbp_curr = (dma_cb_t*)con_blocks->get_page(con_blocks->_virt_pages, cbp);
0, 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),
(uintptr_t)con_blocks->get_page(con_blocks->_phys_pages, RCIN_RPI_TIMER_BASE, phys_fifo_addr,
cbp + sizeof(dma_cb_t) ) ); 4,
0,
cbp += sizeof(dma_cb_t); (uintptr_t)con_blocks->get_page(con_blocks->_phys_pages,
} cbp + sizeof(dma_cb_t) ) );
//Make last control block point to the first (to make circle)
cbp += sizeof(dma_cb_t);
}
//Make last control block point to the first (to make circle)
cbp -= sizeof(dma_cb_t); 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); ((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 /*Initialise PCM
See BCM2835 documentation: See BCM2835 documentation:
http://www.raspberrypi.org/wp-content/uploads/2012/02/BCM2835-ARM-Peripherals.pdf http://www.raspberrypi.org/wp-content/uploads/2012/02/BCM2835-ARM-Peripherals.pdf
*/ */
void RCInput_RPI::init_PCM() void RCInput_RPI::init_PCM()
{ {
pcm_reg[RCIN_RPI_PCM_CS_A] = 1; // Disable Rx+Tx, Enable PCM block 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 /*Initialise DMA
See BCM2835 documentation: See BCM2835 documentation:
http://www.raspberrypi.org/wp-content/uploads/2012/02/BCM2835-ARM-Peripherals.pdf http://www.raspberrypi.org/wp-content/uploads/2012/02/BCM2835-ARM-Peripherals.pdf
*/ */
void RCInput_RPI::init_DMA() void RCInput_RPI::init_DMA()
{ {
dma_reg[RCIN_RPI_DMA_CS | RCIN_RPI_DMA_CHANNEL << 8] = RCIN_RPI_DMA_RESET; //Reset 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_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<uintptr_t>(con_blocks->get_page(con_blocks->_phys_pages, 0));//Set first control block address dma_reg[RCIN_RPI_DMA_CONBLK_AD | RCIN_RPI_DMA_CHANNEL << 8] = reinterpret_cast<uintptr_t>(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_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 //We must stop DMA when the process is killed
void RCInput_RPI::set_sigaction() 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 //catch all signals (like ctrl+c, ctrl+z, ...) to ensure DMA is disabled
struct sigaction sa; struct sigaction sa;
memset(&sa, 0, sizeof(sa)); memset(&sa, 0, sizeof(sa));
@ -408,9 +403,9 @@ RCInput_RPI::RCInput_RPI():
curr_tick_inc(1000/RCIN_RPI_SAMPLE_FREQ), curr_tick_inc(1000/RCIN_RPI_SAMPLE_FREQ),
curr_pointer(0), curr_pointer(0),
curr_channel(0) curr_channel(0)
{ {
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2
int version = 2; int version = 2;
#else #else
int version = UtilRPI::from(hal.util)->get_rpi_version(); int version = UtilRPI::from(hal.util)->get_rpi_version();
#endif #endif
@ -435,14 +430,14 @@ void RCInput_RPI::deinit()
//Initializing necessary registers //Initializing necessary registers
void RCInput_RPI::init_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); pcm_reg = (uint32_t*)map_peripheral(pcm_base, RCIN_RPI_PCM_LEN);
clk_reg = (uint32_t*)map_peripheral(clk_base, RCIN_RPI_CLK_LEN); clk_reg = (uint32_t*)map_peripheral(clk_base, RCIN_RPI_CLK_LEN);
} }
void RCInput_RPI::init() void RCInput_RPI::init()
{ {
uint64_t signal_states(0); uint64_t signal_states = 0;
init_registers(); init_registers();
@ -481,17 +476,15 @@ void RCInput_RPI::init()
void RCInput_RPI::_timer_tick() void RCInput_RPI::_timer_tick()
{ {
int j; int j;
void* x; void *x;
uint64_t signal_states(0); uint64_t signal_states(0);
//Now we are getting address in which DMAC is writing at current moment //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]); 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--){ for (j = 1; j >= -1 && x == nullptr; j--) {
x = circle_buffer->get_virt_addr((ad + j)->dst); x = circle_buffer->get_virt_addr((ad + j)->dst);
if(x != NULL) {
break;}
} }
//How many bytes have DMA transfered (and we can process)? //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)); 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 //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) { if (rc_channels[i].curr_signal == 0) {
rc_channels[i].width_s0 = (uint16_t)rc_channels[i].delta_time; rc_channels[i].width_s0 = (uint16_t)rc_channels[i].delta_time;
rc_channels[i].state = RCIN_RPI_ONE_STATE; rc_channels[i].state = RCIN_RPI_ONE_STATE;
break;
}
else {
break;
} }
break;
case RCIN_RPI_ONE_STATE: case RCIN_RPI_ONE_STATE:
if (rc_channels[i].curr_signal == 1) { if (rc_channels[i].curr_signal == 1) {
rc_channels[i].width_s1 = (uint16_t)rc_channels[i].delta_time; rc_channels[i].width_s1 = (uint16_t)rc_channels[i].delta_time;
@ -540,12 +530,9 @@ void RCInput_RPI::_timer_tick()
#else #else
_process_rc_pulse(rc_channels[i].width_s0, _process_rc_pulse(rc_channels[i].width_s0,
rc_channels[i].width_s1); rc_channels[i].width_s1);
#endif // CONFIG_HAL_BOARD_SUBTYPE #endif
break;
}
else{
break;
} }
break;
} }
} }
rc_channels[i].last_signal = rc_channels[i].curr_signal; 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) { if (curr_pointer >= circle_buffer->get_page_count()*PAGE_SIZE) {
curr_pointer = 0; curr_pointer = 0;
} }
curr_tick+=curr_tick_inc; curr_tick += curr_tick_inc;
} }
} }
#endif // CONFIG_HAL_BOARD_SUBTYPE #endif