static void
rx_handle_packet(void)
{
/* check packet CRC */
uint8_t crc = dma_packet.crc;
dma_packet.crc = 0;
if (crc != crc_packet(&dma_packet)) {
perf_count(pc_crcerr);
/* send a CRC error reply */
dma_packet.count_code = PKT_CODE_CORRUPT;
dma_packet.page = 0xff;
dma_packet.offset = 0xff;
return;
}
if (PKT_CODE(dma_packet) == PKT_CODE_WRITE) {
/* it's a blind write - pass it on */
if (registers_set(dma_packet.page, dma_packet.offset, &dma_packet.regs[0], PKT_COUNT(dma_packet))) {
perf_count(pc_regerr);
dma_packet.count_code = PKT_CODE_ERROR;
} else {
dma_packet.count_code = PKT_CODE_SUCCESS;
}
return;
}
if (PKT_CODE(dma_packet) == PKT_CODE_READ) {
/* it's a read - get register pointer for reply */
unsigned count;
uint16_t *registers;
if (registers_get(dma_packet.page, dma_packet.offset, ®isters, &count) < 0) {
perf_count(pc_regerr);
dma_packet.count_code = PKT_CODE_ERROR;
} else {
/* constrain reply to requested size */
if (count > PKT_MAX_REGS)
count = PKT_MAX_REGS;
if (count > PKT_COUNT(dma_packet))
count = PKT_COUNT(dma_packet);
/* copy reply registers into DMA buffer */
memcpy((void *)&dma_packet.regs[0], registers, count * 2);
dma_packet.count_code = count | PKT_CODE_SUCCESS;
}
return;
}
/* send a bad-packet error reply */
dma_packet.count_code = PKT_CODE_CORRUPT;
dma_packet.page = 0xff;
dma_packet.offset = 0xfe;
}
static void
i2c_rx_complete(void)
{
rx_len = sizeof(rx_buf) - stm32_dmaresidual(rx_dma);
stm32_dmastop(rx_dma);
if (rx_len >= 2) {
selected_page = rx_buf[0];
selected_offset = rx_buf[1];
/* work out how many registers are being written */
unsigned count = (rx_len - 2) / 2;
if (count > 0) {
registers_set(selected_page, selected_offset, (const uint16_t *)&rx_buf[2], count);
} else {
/* no registers written, must be an address cycle */
uint16_t *regs;
unsigned reg_count;
/* work out which registers are being addressed */
int ret = registers_get(selected_page, selected_offset, ®s, ®_count);
if (ret == 0) {
tx_buf = (uint8_t *)regs;
tx_len = reg_count * 2;
} else {
tx_buf = junk_buf;
tx_len = sizeof(junk_buf);
}
/* disable interrupts while reconfiguring DMA for the selected registers */
irqstate_t flags = irqsave();
stm32_dmastop(tx_dma);
i2c_tx_setup();
irqrestore(flags);
}
}
/* prepare for the next transaction */
i2c_rx_setup();
}
static void
serial_callback(void *arg, const void *data, unsigned length)
{
const uint8_t *message = (const uint8_t *)data;
/* malformed frame, ignore it */
if (length < 2)
return;
/* it's a write operation, pass it to the register API */
if (message[0] & PX4IO_PAGE_WRITE) {
registers_set(message[0] & ~PX4IO_PAGE_WRITE, message[1],
(const uint16_t *)&message[2],
(length - 2) / 2);
return;
}
/* it's a read - must contain length byte */
if (length != 3)
return;
uint16_t *registers;
unsigned count;
tx_buf[0] = message[0];
tx_buf[1] = message[1];
/* get registers for response, send an empty reply on error */
if (registers_get(message[0], message[1], ®isters, &count) < 0)
count = 0;
/* fill buffer with message, limited by length */
#define TX_MAX ((sizeof(tx_buf) - 2) / 2)
if (count > TX_MAX)
count = TX_MAX;
if (count > message[2])
count = message[2];
memcpy(&tx_buf[2], registers, count * 2);
/* try to send the message */
hx_stream_send(if_stream, tx_buf, count * 2 + 2);
}
