mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-03 15:58:53 +08:00
PX4IO driver: Fix code style
This commit is contained in:
Lorenz Meier
+401
-222
File diff suppressed because it is too large
Load Diff
@@ -31,11 +31,11 @@
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file px4io_i2c.cpp
|
||||
*
|
||||
* I2C interface for PX4IO
|
||||
*/
|
||||
/**
|
||||
* @file px4io_i2c.cpp
|
||||
*
|
||||
* I2C interface for PX4IO
|
||||
*/
|
||||
|
||||
/* XXX trim includes */
|
||||
#include <px4_config.h>
|
||||
@@ -94,8 +94,10 @@ PX4IO_I2C::init()
|
||||
int ret;
|
||||
|
||||
ret = I2C::init();
|
||||
if (ret != OK)
|
||||
|
||||
if (ret != OK) {
|
||||
goto out;
|
||||
}
|
||||
|
||||
/* XXX really should do something more here */
|
||||
|
||||
@@ -133,8 +135,11 @@ PX4IO_I2C::write(unsigned address, void *data, unsigned count)
|
||||
msgv[1].length = 2 * count;
|
||||
|
||||
int ret = transfer(msgv, 2);
|
||||
if (ret == OK)
|
||||
|
||||
if (ret == OK) {
|
||||
ret = count;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
@@ -161,8 +166,11 @@ PX4IO_I2C::read(unsigned address, void *data, unsigned count)
|
||||
msgv[1].length = 2 * count;
|
||||
|
||||
int ret = transfer(msgv, 2);
|
||||
if (ret == OK)
|
||||
|
||||
if (ret == OK) {
|
||||
ret = count;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
||||
@@ -31,11 +31,11 @@
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file px4io_serial.cpp
|
||||
*
|
||||
* Serial interface for PX4IO
|
||||
*/
|
||||
/**
|
||||
* @file px4io_serial.cpp
|
||||
*
|
||||
* Serial interface for PX4IO
|
||||
*/
|
||||
|
||||
/* XXX trim includes */
|
||||
#include <px4_config.h>
|
||||
@@ -159,7 +159,7 @@ private:
|
||||
|
||||
/* do not allow top copying this class */
|
||||
PX4IO_serial(PX4IO_serial &);
|
||||
PX4IO_serial& operator = (const PX4IO_serial &);
|
||||
PX4IO_serial &operator = (const PX4IO_serial &);
|
||||
|
||||
};
|
||||
|
||||
@@ -199,6 +199,7 @@ PX4IO_serial::~PX4IO_serial()
|
||||
stm32_dmastop(_tx_dma);
|
||||
stm32_dmafree(_tx_dma);
|
||||
}
|
||||
|
||||
if (_rx_dma != nullptr) {
|
||||
stm32_dmastop(_rx_dma);
|
||||
stm32_dmafree(_rx_dma);
|
||||
@@ -232,8 +233,9 @@ PX4IO_serial::~PX4IO_serial()
|
||||
perf_free(_pc_idle);
|
||||
perf_free(_pc_badidle);
|
||||
|
||||
if (g_interface == this)
|
||||
if (g_interface == this) {
|
||||
g_interface = nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
int
|
||||
@@ -243,6 +245,7 @@ PX4IO_serial::init()
|
||||
/* allocate DMA */
|
||||
_tx_dma = stm32_dmachannel(PX4IO_SERIAL_TX_DMAMAP);
|
||||
_rx_dma = stm32_dmachannel(PX4IO_SERIAL_RX_DMAMAP);
|
||||
|
||||
if ((_tx_dma == nullptr) || (_rx_dma == nullptr)) {
|
||||
return -1;
|
||||
}
|
||||
@@ -305,19 +308,22 @@ PX4IO_serial::ioctl(unsigned operation, unsigned &arg)
|
||||
for (;;) {
|
||||
while (!(rSR & USART_SR_TXE))
|
||||
;
|
||||
|
||||
rDR = 0x55;
|
||||
}
|
||||
|
||||
return 0;
|
||||
|
||||
case 1:
|
||||
{
|
||||
case 1: {
|
||||
unsigned fails = 0;
|
||||
|
||||
for (unsigned count = 0;; count++) {
|
||||
uint16_t value = count & 0xffff;
|
||||
|
||||
if (write((PX4IO_PAGE_TEST << 8) | PX4IO_P_TEST_LED, &value, 1) != 0)
|
||||
if (write((PX4IO_PAGE_TEST << 8) | PX4IO_P_TEST_LED, &value, 1) != 0) {
|
||||
fails++;
|
||||
|
||||
}
|
||||
|
||||
if (count >= 5000) {
|
||||
lowsyslog("==== test 1 : %u failures ====\n", fails);
|
||||
perf_print_counter(_pc_txns);
|
||||
@@ -333,12 +339,15 @@ PX4IO_serial::ioctl(unsigned operation, unsigned &arg)
|
||||
count = 0;
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
case 2:
|
||||
lowsyslog("test 2\n");
|
||||
return 0;
|
||||
}
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
@@ -353,20 +362,24 @@ PX4IO_serial::write(unsigned address, void *data, unsigned count)
|
||||
uint8_t offset = address & 0xff;
|
||||
const uint16_t *values = reinterpret_cast<const uint16_t *>(data);
|
||||
|
||||
if (count > PKT_MAX_REGS)
|
||||
if (count > PKT_MAX_REGS) {
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
sem_wait(&_bus_semaphore);
|
||||
|
||||
int result;
|
||||
|
||||
for (unsigned retries = 0; retries < 3; retries++) {
|
||||
|
||||
_dma_buffer.count_code = count | PKT_CODE_WRITE;
|
||||
_dma_buffer.page = page;
|
||||
_dma_buffer.offset = offset;
|
||||
memcpy((void *)&_dma_buffer.regs[0], (void *)values, (2 * count));
|
||||
for (unsigned i = count; i < PKT_MAX_REGS; i++)
|
||||
|
||||
for (unsigned i = count; i < PKT_MAX_REGS; i++) {
|
||||
_dma_buffer.regs[i] = 0x55aa;
|
||||
}
|
||||
|
||||
/* XXX implement check byte */
|
||||
|
||||
@@ -386,13 +399,16 @@ PX4IO_serial::write(unsigned address, void *data, unsigned count)
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
perf_count(_pc_retries);
|
||||
}
|
||||
|
||||
sem_post(&_bus_semaphore);
|
||||
|
||||
if (result == OK)
|
||||
if (result == OK) {
|
||||
result = count;
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
@@ -403,12 +419,14 @@ PX4IO_serial::read(unsigned address, void *data, unsigned count)
|
||||
uint8_t offset = address & 0xff;
|
||||
uint16_t *values = reinterpret_cast<uint16_t *>(data);
|
||||
|
||||
if (count > PKT_MAX_REGS)
|
||||
if (count > PKT_MAX_REGS) {
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
sem_wait(&_bus_semaphore);
|
||||
|
||||
int result;
|
||||
|
||||
for (unsigned retries = 0; retries < 3; retries++) {
|
||||
|
||||
_dma_buffer.count_code = count | PKT_CODE_READ;
|
||||
@@ -428,14 +446,16 @@ PX4IO_serial::read(unsigned address, void *data, unsigned count)
|
||||
result = -EINVAL;
|
||||
perf_count(_pc_protoerrs);
|
||||
|
||||
/* compare the received count with the expected count */
|
||||
/* compare the received count with the expected count */
|
||||
|
||||
} else if (PKT_COUNT(_dma_buffer) != count) {
|
||||
|
||||
/* IO returned the wrong number of registers - no point retrying */
|
||||
result = -EIO;
|
||||
perf_count(_pc_protoerrs);
|
||||
|
||||
/* successful read */
|
||||
/* successful read */
|
||||
|
||||
} else {
|
||||
|
||||
/* copy back the result */
|
||||
@@ -444,13 +464,16 @@ PX4IO_serial::read(unsigned address, void *data, unsigned count)
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
perf_count(_pc_retries);
|
||||
}
|
||||
|
||||
sem_post(&_bus_semaphore);
|
||||
|
||||
if (result == OK)
|
||||
if (result == OK) {
|
||||
result = count;
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
@@ -517,14 +540,16 @@ PX4IO_serial::_wait_complete()
|
||||
/* compute the deadline for a 10ms timeout */
|
||||
struct timespec abstime;
|
||||
clock_gettime(CLOCK_REALTIME, &abstime);
|
||||
abstime.tv_nsec += 10*1000*1000;
|
||||
if (abstime.tv_nsec >= 1000*1000*1000) {
|
||||
abstime.tv_nsec += 10 * 1000 * 1000;
|
||||
|
||||
if (abstime.tv_nsec >= 1000 * 1000 * 1000) {
|
||||
abstime.tv_sec++;
|
||||
abstime.tv_nsec -= 1000*1000*1000;
|
||||
abstime.tv_nsec -= 1000 * 1000 * 1000;
|
||||
}
|
||||
|
||||
/* wait for the transaction to complete - 64 bytes @ 1.5Mbps ~426µs */
|
||||
int ret;
|
||||
|
||||
for (;;) {
|
||||
ret = sem_timedwait(&_completion_semaphore, &abstime);
|
||||
|
||||
@@ -539,6 +564,7 @@ PX4IO_serial::_wait_complete()
|
||||
/* check packet CRC - corrupt packet errors mean IO receive CRC error */
|
||||
uint8_t crc = _dma_buffer.crc;
|
||||
_dma_buffer.crc = 0;
|
||||
|
||||
if ((crc != crc_packet(&_dma_buffer)) | (PKT_CODE(_dma_buffer) == PKT_CODE_CORRUPT)) {
|
||||
perf_count(_pc_crcerrs);
|
||||
ret = -EIO;
|
||||
@@ -588,6 +614,7 @@ PX4IO_serial::_do_rx_dma_callback(unsigned status)
|
||||
|
||||
/* check for packet overrun - this will occur after DMA completes */
|
||||
uint32_t sr = rSR;
|
||||
|
||||
if (sr & (USART_SR_ORE | USART_SR_RXNE)) {
|
||||
(void)rDR;
|
||||
status = DMA_STATUS_TEIF;
|
||||
@@ -607,8 +634,10 @@ PX4IO_serial::_do_rx_dma_callback(unsigned status)
|
||||
int
|
||||
PX4IO_serial::_interrupt(int irq, void *context)
|
||||
{
|
||||
if (g_interface != nullptr)
|
||||
if (g_interface != nullptr) {
|
||||
g_interface->_do_interrupt();
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -619,10 +648,10 @@ PX4IO_serial::_do_interrupt()
|
||||
(void)rDR; /* read DR to clear status */
|
||||
|
||||
if (sr & (USART_SR_ORE | /* overrun error - packet was too big for DMA or DMA was too slow */
|
||||
USART_SR_NE | /* noise error - we have lost a byte due to noise */
|
||||
USART_SR_FE)) { /* framing error - start/stop bit lost or line break */
|
||||
|
||||
/*
|
||||
USART_SR_NE | /* noise error - we have lost a byte due to noise */
|
||||
USART_SR_FE)) { /* framing error - start/stop bit lost or line break */
|
||||
|
||||
/*
|
||||
* If we are in the process of listening for something, these are all fatal;
|
||||
* abort the DMA with an error.
|
||||
*/
|
||||
@@ -649,6 +678,7 @@ PX4IO_serial::_do_interrupt()
|
||||
|
||||
/* verify that the received packet is complete */
|
||||
size_t length = sizeof(_dma_buffer) - stm32_dmaresidual(_rx_dma);
|
||||
|
||||
if ((length < 1) || (length < PKT_SIZE(_dma_buffer))) {
|
||||
perf_count(_pc_badidle);
|
||||
|
||||
|
||||
@@ -126,8 +126,10 @@ PX4IO_Uploader::upload(const char *filenames[])
|
||||
/* look for the bootloader for 150 ms */
|
||||
for (int i = 0; i < 15; i++) {
|
||||
ret = sync();
|
||||
|
||||
if (ret == OK) {
|
||||
break;
|
||||
|
||||
} else {
|
||||
usleep(10000);
|
||||
}
|
||||
@@ -143,6 +145,7 @@ PX4IO_Uploader::upload(const char *filenames[])
|
||||
}
|
||||
|
||||
struct stat st;
|
||||
|
||||
if (stat(filename, &st) != 0) {
|
||||
log("Failed to stat %s - %d\n", filename, (int)errno);
|
||||
tcsetattr(_io_fd, TCSANOW, &t_original);
|
||||
@@ -150,6 +153,7 @@ PX4IO_Uploader::upload(const char *filenames[])
|
||||
_io_fd = -1;
|
||||
return -errno;
|
||||
}
|
||||
|
||||
fw_size = st.st_size;
|
||||
|
||||
if (_fw_fd == -1) {
|
||||
@@ -180,6 +184,7 @@ PX4IO_Uploader::upload(const char *filenames[])
|
||||
if (ret == OK) {
|
||||
if (bl_rev <= BL_REV) {
|
||||
log("found bootloader revision: %d", bl_rev);
|
||||
|
||||
} else {
|
||||
log("found unsupported bootloader revision %d, exiting", bl_rev);
|
||||
tcsetattr(_io_fd, TCSANOW, &t_original);
|
||||
@@ -205,6 +210,7 @@ PX4IO_Uploader::upload(const char *filenames[])
|
||||
|
||||
if (bl_rev <= 2) {
|
||||
ret = verify_rev2(fw_size);
|
||||
|
||||
} else {
|
||||
/* verify rev 3 and higher. Every version *needs* to be verified. */
|
||||
ret = verify_rev3(fw_size);
|
||||
@@ -240,7 +246,7 @@ PX4IO_Uploader::upload(const char *filenames[])
|
||||
|
||||
// sleep for enough time for the IO chip to boot. This makes
|
||||
// forceupdate more reliably startup IO again after update
|
||||
up_udelay(100*1000);
|
||||
up_udelay(100 * 1000);
|
||||
|
||||
return ret;
|
||||
}
|
||||
@@ -274,12 +280,15 @@ int
|
||||
PX4IO_Uploader::recv_bytes(uint8_t *p, unsigned count)
|
||||
{
|
||||
int ret = OK;
|
||||
|
||||
while (count--) {
|
||||
ret = recv_byte_with_timeout(p++, 5000);
|
||||
|
||||
if (ret != OK)
|
||||
if (ret != OK) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
@@ -296,9 +305,11 @@ PX4IO_Uploader::drain()
|
||||
ret = recv_byte_with_timeout(&c, 40);
|
||||
|
||||
#ifdef UDEBUG
|
||||
|
||||
if (ret == OK) {
|
||||
log("discard 0x%02x", c);
|
||||
}
|
||||
|
||||
#endif
|
||||
} while (ret == OK);
|
||||
}
|
||||
@@ -309,8 +320,11 @@ PX4IO_Uploader::send(uint8_t c)
|
||||
#ifdef UDEBUG
|
||||
log("send 0x%02x", c);
|
||||
#endif
|
||||
if (write(_io_fd, &c, 1) != 1)
|
||||
|
||||
if (write(_io_fd, &c, 1) != 1) {
|
||||
return -errno;
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
@@ -318,11 +332,15 @@ int
|
||||
PX4IO_Uploader::send(uint8_t *p, unsigned count)
|
||||
{
|
||||
int ret;
|
||||
|
||||
while (count--) {
|
||||
ret = send(*p++);
|
||||
if (ret != OK)
|
||||
|
||||
if (ret != OK) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
@@ -334,13 +352,15 @@ PX4IO_Uploader::get_sync(unsigned timeout)
|
||||
|
||||
ret = recv_byte_with_timeout(c, timeout);
|
||||
|
||||
if (ret != OK)
|
||||
if (ret != OK) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
ret = recv_byte_with_timeout(c + 1, timeout);
|
||||
|
||||
if (ret != OK)
|
||||
if (ret != OK) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
if ((c[0] != PROTO_INSYNC) || (c[1] != PROTO_OK)) {
|
||||
log("bad sync 0x%02x,0x%02x", c[0], c[1]);
|
||||
@@ -356,8 +376,9 @@ PX4IO_Uploader::sync()
|
||||
drain();
|
||||
|
||||
/* complete any pending program operation */
|
||||
for (unsigned i = 0; i < (PROG_MULTI_MAX + 6); i++)
|
||||
for (unsigned i = 0; i < (PROG_MULTI_MAX + 6); i++) {
|
||||
send(0);
|
||||
}
|
||||
|
||||
send(PROTO_GET_SYNC);
|
||||
send(PROTO_EOC);
|
||||
@@ -375,8 +396,9 @@ PX4IO_Uploader::get_info(int param, uint32_t &val)
|
||||
|
||||
ret = recv_bytes((uint8_t *)&val, sizeof(val));
|
||||
|
||||
if (ret != OK)
|
||||
if (ret != OK) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
return get_sync();
|
||||
}
|
||||
@@ -395,14 +417,17 @@ static int read_with_retry(int fd, void *buf, size_t n)
|
||||
{
|
||||
int ret;
|
||||
uint8_t retries = 0;
|
||||
|
||||
do {
|
||||
ret = read(fd, buf, n);
|
||||
} while (ret == -1 && retries++ < 100);
|
||||
|
||||
if (retries != 0) {
|
||||
printf("read of %u bytes needed %u retries\n",
|
||||
(unsigned)n,
|
||||
(unsigned)retries);
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
@@ -415,6 +440,7 @@ PX4IO_Uploader::program(size_t fw_size)
|
||||
size_t sent = 0;
|
||||
|
||||
file_buf = new uint8_t[PROG_MULTI_MAX];
|
||||
|
||||
if (!file_buf) {
|
||||
log("Can't allocate program buffer");
|
||||
return -ENOMEM;
|
||||
@@ -430,13 +456,15 @@ PX4IO_Uploader::program(size_t fw_size)
|
||||
while (sent < fw_size) {
|
||||
/* get more bytes to program */
|
||||
size_t n = fw_size - sent;
|
||||
|
||||
if (n > PROG_MULTI_MAX) {
|
||||
n = PROG_MULTI_MAX;
|
||||
}
|
||||
|
||||
count = read_with_retry(_fw_fd, file_buf, n);
|
||||
|
||||
if (count != (ssize_t)n) {
|
||||
log("firmware read of %u bytes at %u failed -> %d errno %d",
|
||||
log("firmware read of %u bytes at %u failed -> %d errno %d",
|
||||
(unsigned)n,
|
||||
(unsigned)sent,
|
||||
(int)count,
|
||||
@@ -478,32 +506,37 @@ PX4IO_Uploader::verify_rev2(size_t fw_size)
|
||||
send(PROTO_EOC);
|
||||
ret = get_sync();
|
||||
|
||||
if (ret != OK)
|
||||
if (ret != OK) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
while (sent < fw_size) {
|
||||
/* get more bytes to verify */
|
||||
size_t n = fw_size - sent;
|
||||
|
||||
if (n > sizeof(file_buf)) {
|
||||
n = sizeof(file_buf);
|
||||
}
|
||||
|
||||
count = read_with_retry(_fw_fd, file_buf, n);
|
||||
|
||||
if (count != (ssize_t)n) {
|
||||
log("firmware read of %u bytes at %u failed -> %d errno %d",
|
||||
log("firmware read of %u bytes at %u failed -> %d errno %d",
|
||||
(unsigned)n,
|
||||
(unsigned)sent,
|
||||
(int)count,
|
||||
(int)errno);
|
||||
}
|
||||
|
||||
if (count == 0)
|
||||
if (count == 0) {
|
||||
break;
|
||||
}
|
||||
|
||||
sent += count;
|
||||
|
||||
if (count < 0)
|
||||
if (count < 0) {
|
||||
return -errno;
|
||||
}
|
||||
|
||||
ASSERT((count % 4) == 0);
|
||||
|
||||
@@ -564,13 +597,15 @@ PX4IO_Uploader::verify_rev3(size_t fw_size_local)
|
||||
/* read through the firmware file again and calculate the checksum*/
|
||||
while (bytes_read < fw_size_local) {
|
||||
size_t n = fw_size_local - bytes_read;
|
||||
|
||||
if (n > sizeof(file_buf)) {
|
||||
n = sizeof(file_buf);
|
||||
}
|
||||
|
||||
count = read_with_retry(_fw_fd, file_buf, n);
|
||||
|
||||
if (count != (ssize_t)n) {
|
||||
log("firmware read of %u bytes at %u failed -> %d errno %d",
|
||||
log("firmware read of %u bytes at %u failed -> %d errno %d",
|
||||
(unsigned)n,
|
||||
(unsigned)bytes_read,
|
||||
(int)count,
|
||||
@@ -581,9 +616,11 @@ PX4IO_Uploader::verify_rev3(size_t fw_size_local)
|
||||
if (count == 0) {
|
||||
break;
|
||||
}
|
||||
|
||||
/* stop if the file cannot be read */
|
||||
if (count < 0)
|
||||
if (count < 0) {
|
||||
return -errno;
|
||||
}
|
||||
|
||||
/* calculate crc32 sum */
|
||||
sum = crc32part((uint8_t *)&file_buf, sizeof(file_buf), sum);
|
||||
@@ -601,7 +638,7 @@ PX4IO_Uploader::verify_rev3(size_t fw_size_local)
|
||||
send(PROTO_GET_CRC);
|
||||
send(PROTO_EOC);
|
||||
|
||||
ret = recv_bytes((uint8_t*)(&crc), sizeof(crc));
|
||||
ret = recv_bytes((uint8_t *)(&crc), sizeof(crc));
|
||||
|
||||
if (ret != OK) {
|
||||
log("did not receive CRC checksum");
|
||||
@@ -621,7 +658,7 @@ int
|
||||
PX4IO_Uploader::reboot()
|
||||
{
|
||||
send(PROTO_REBOOT);
|
||||
up_udelay(100*1000); // Ensure the farend is in wait for char.
|
||||
up_udelay(100 * 1000); // Ensure the farend is in wait for char.
|
||||
send(PROTO_EOC);
|
||||
|
||||
return OK;
|
||||
|
||||
Reference in New Issue
Block a user