rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

Added start at a modified PipX packet handler.

Browse Source
This commit is contained in:
Brian Webb 2012-03-15 19:29:54 -07:00
parent d73895de7d
commit b34b849453
15 changed files with 837 additions and 1757 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

0
flight/PipXtreme/aes.c → flight/Libraries/aes.c
View File

0
flight/PipXtreme/crc.c → flight/Libraries/crc.c
View File

0
flight/PipXtreme/inc/aes.h → flight/Libraries/inc/aes.h
View File

0
flight/PipXtreme/inc/crc.h → flight/Libraries/inc/crc.h
View File

93
flight/Libraries/inc/packet_handler.h Normal file
View File

@ -0,0 +1,93 @@
/**
******************************************************************************
* @addtogroup OpenPilotSystem OpenPilot System
* @{
* @addtogroup OpenPilotLibraries OpenPilot System Libraries
* @{
*
* @file packet_handler.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief A packet handler for handeling radio packet transmission.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef __PACKET_HANDLER_H__
#define __PACKET_HANDLER_H__
// Public types
typedef enum {
PACKET_TYPE_NONE = 0,
PACKET_TYPE_CONNECT, // for requesting a connection
PACKET_TYPE_DISCONNECT, // to tell the other modem they cannot connect to us
PACKET_TYPE_READY, // tells the other modem we are ready to accept more data
PACKET_TYPE_NOTREADY, // tells the other modem we're not ready to accept more data - we can also send user data in this packet type
PACKET_TYPE_DATARATE, // for changing the RF data rate
PACKET_TYPE_PING, // used to check link is still up
PACKET_TYPE_ADJUST_TX_PWR, // used to ask the other modem to adjust it's tx power
PACKET_TYPE_DATA, // data packet (packet contains user data)
PACKET_TYPE_ACKED_DATA, // data packet that requies an ACK
PACKET_TYPE_RECEIVER, // Receiver relay values
PACKET_TYPE_ACK
} PHPacketType;
typedef struct {
uint32_t source_id;
uint32_t destination_id;
uint8_t type;
uint8_t tx_seq;
uint8_t rx_seq;
uint8_t data_size;
} PHPacketHeader;
#define PH_MAX_DATA (255 - sizeof(PHPacketHeader))
typedef struct {
PHPacketHeader header;
uint8_t data[PH_MAX_DATA];
} PHPacket, *PHPacketHandle;
typedef struct {
uint8_t txWinSize;
uint16_t maxConnections;
uint32_t id;
uint8_t (*output_stream)(PHPacketHandle packet);
void (*set_baud)(uint32_t baud);
void (*data_handler)(uint8_t *data, uint8_t len);
void (*receiver_handler)(uint8_t *data, uint8_t len);
} PacketHandlerConfig;
typedef int32_t (*PHOutputStream)(PHPacketHandle packet);
typedef void* PHInstHandle;
// Public functions
PHInstHandle PHInitialize(PacketHandlerConfig *cfg);
uint32_t PHConnect(PHInstHandle h, uint32_t dest_id);
PHPacketHandle PHGetTXPacket(PHInstHandle h);
PHPacketHandle PHReserveTXPacket(PHInstHandle h);
void PHReleaseLock(PHInstHandle h, bool keep_packet);
void PHReleaseTXPacket(PHInstHandle h, PHPacketHandle p);
uint8_t PHTransmitPacket(PHInstHandle h, PHPacketHandle p);
uint8_t PHReceivePacket(PHInstHandle h, PHPacketHandle p);
#endif // __PACKET_HANDLER_H__
/**
* @}
* @}
*/

290
flight/Libraries/packet_handler.c Normal file
View File

@ -0,0 +1,290 @@
/**
******************************************************************************
* @addtogroup OpenPilotSystem OpenPilot System
* @{
* @addtogroup OpenPilotLibraries OpenPilot System Libraries
* @{
*
* @file packet_handler.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief A packet handler for handeling radio packet transmission.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "openpilot.h"
#include "packet_handler.h"
#include "aes.h"
#include "crc.h"
// Private types and constants
typedef struct {
PacketHandlerConfig cfg;
PHPacket *tx_packets;
uint8_t tx_win_start;
uint8_t tx_win_end;
uint16_t tx_seq_id;
PHPacket rx_packet;
PHOutputStream stream;
xSemaphoreHandle lock;
} PHPacketData, *PHPacketDataHandle;
// Private functions
static uint8_t PHLSendAck(PHPacketDataHandle data, PHPacketHandle p);
static uint8_t PHLTransmitPacket(PHPacketDataHandle data, PHPacketHandle p);
/**
* Initialize the Packet Handler library
* \param[in] txWinSize The transmission window size (number of tx packet buffers).
* \param[in] streme A callback function for transmitting the packet.
* \param[in] id The source ID of transmitter.
* \return PHInstHandle The Pachet Handler instance data.
* \return 0 Failure
*/
PHInstHandle PHInitialize(PacketHandlerConfig *cfg)
{
// Allocate the primary structure
PHPacketDataHandle data = pvPortMalloc(sizeof(PHPacketData));
if (!data)
return 0;
data->cfg = *cfg;
data->tx_seq_id = 0;
// Allocate the tx packet window
data->tx_packets = pvPortMalloc(sizeof(PHPacket) * data->cfg.txWinSize);
// Initialize the window
data->tx_win_start = data->tx_win_end = 0;
for (uint8_t i = 0; i < data->cfg.txWinSize; ++i)
data->tx_packets[i].header.type = PACKET_TYPE_NONE;
// Create the lock
data->lock = xSemaphoreCreateRecursiveMutex();
// Return the structure.
return (PHInstHandle)data;
}
/**
* Get a packet out of the transmit buffer.
* \param[in] h The packet handler instance data pointer.
* \param[in] dest_id The destination ID of this connection
* \return PHPacketHandle A pointer to the packet buffer.
* \return 0 No packets buffers avaiable in the transmit window.
*/
uint32_t PHConnect(PHInstHandle h, uint32_t dest_id)
{
return 1;
}
/**
* Temporarily reserve the next packet in the TX packet window.
* This function places a tempoary hold on the next TX packet and
* retains the packet handler lock.
*
* NOTE: PHReleaseLock must be called to release the lock and retain
* or release the reserved packet.
*
* \param[in] h The packet handler instance data pointer.
* \return PHPacketHandle A pointer to the packet buffer.
* \return 0 No packets buffers avaiable in the transmit window.
*/
PHPacketHandle PHReserveTXPacket(PHInstHandle h)
{
PHPacketDataHandle data = (PHPacketDataHandle)h;
// Lock
xSemaphoreTakeRecursive(data->lock, portMAX_DELAY);
// Is the window full?
uint8_t next_end = (data->tx_win_end + 1) % data->cfg.txWinSize;
if(next_end == data->tx_win_start) {
// Release the lock
xSemaphoreGiveRecursive(data->lock);
return NULL;
}
// Return a pointer to the packet at the end of the TX window.
return data->tx_packets + data->tx_win_end;
}
/**
* Get a packet out of the transmit buffer and keep the lock.
* NOTE: PHReleaseLock must be called to release the lock.
* \param[in] h The packet handler instance data pointer.
* \param[in] keep_packet Maintain a permanent (until released) lock on the packet.
*/
void PHReleaseLock(PHInstHandle h, bool keep_packet)
{
PHPacketDataHandle data = (PHPacketDataHandle)h;
uint8_t next_end = (data->tx_win_end + 1) % data->cfg.txWinSize;
// Increment the end index if packet is being kept.
if (keep_packet)
data->tx_win_end = next_end;
// Release lock
xSemaphoreGiveRecursive(data->lock);
}
/**
* Get a packet out of the transmit buffer.
* \param[in] h The packet handler instance data pointer.
* \return PHPacketHandle A pointer to the packet buffer.
* \return 0 No packets buffers avaiable in the transmit window.
*/
PHPacketHandle PHGetTXPacket(PHInstHandle h)
{
PHPacketHandle p = PHReserveTXPacket(h);
PHReleaseLock(p, 1);
return p;
}
/**
* Release a packet from the transmit packet buffer window.
* \param[in] h The packet handler instance data pointer.
* \param[in] p A pointer to the packet buffer.
* \return Nothing
*/
void PHReleaseTXPacket(PHInstHandle h, PHPacketHandle p)
{
PHPacketDataHandle data = (PHPacketDataHandle)h;
// Lock
xSemaphoreTakeRecursive(data->lock, portMAX_DELAY);
// Change the packet type so we know this packet is unused.
p->header.type = PACKET_TYPE_NONE;
// If this packet is at the start of the window, increment the start index.
while ((data->tx_win_start != data->tx_win_end) &&
(data->tx_packets[data->tx_win_start].header.type == PACKET_TYPE_NONE))
data->tx_win_start = (data->tx_win_start + 1) % data->cfg.txWinSize;
// Release lock
xSemaphoreGiveRecursive(data->lock);
}
/**
* Transmit a packet from the transmit packet buffer window.
* \param[in] h The packet handler instance data pointer.
* \param[in] p A pointer to the packet buffer.
* \return 1 Success
* \return 0 Failure
*/
uint8_t PHTransmitPacket(PHInstHandle h, PHPacketHandle p)
{
PHPacketDataHandle data = (PHPacketDataHandle)h;
// Try to transmit the packet.
if (!PHLTransmitPacket(data, p))
return 0;
// If this packet doesn't require an ACK, remove it from the TX window.
switch (p->header.type) {
case PACKET_TYPE_READY:
case PACKET_TYPE_NOTREADY:
case PACKET_TYPE_DATA:
case PACKET_TYPE_RECEIVER:
PHReleaseTXPacket(h, p);
break;
}
return 1;
}
/**
* Process a packet that has been received.
* \param[in] h The packet handler instance data pointer.
* \param[in] p A pointer to the packet buffer.
* \return 1 Success
* \return 0 Failure
*/
uint8_t PHReceivePacket(PHInstHandle h, PHPacketHandle p)
{
PHPacketDataHandle data = (PHPacketDataHandle)h;
switch (p->header.type) {
case PACKET_TYPE_ACKED_DATA:
// Send the ACK
PHLSendAck(data, p);
// Pass on the data.
if(data->cfg.data_handler)
data->cfg.data_handler(p->data, p->header.data_size);
break;
case PACKET_TYPE_RECEIVER:
// Pass on the data to the receiver handler.
if(data->cfg.receiver_handler)
data->cfg.receiver_handler(p->data, p->header.data_size);
break;
}
return 1;
}
/**
* Transmit a packet from the transmit packet buffer window.
* \param[in] data The packet handler instance data pointer.
* \param[in] p A pointer to the packet buffer.
* \return 1 Success
* \return 0 Failure
*/
static uint8_t PHLTransmitPacket(PHPacketDataHandle data, PHPacketHandle p)
{
// Set the sequence ID to the current ID.
p->header.tx_seq = data->tx_seq_id++;
// Transmit the packet using the output stream.
if(!data->cfg.output_stream(p))
return 0;
return 1;
}
/**
* Send an ACK packet.
* \param[in] data The packet handler instance data pointer.
* \param[in] p A pointer to the packet buffer of the packet to be ACKed.
* \return 1 Success
* \return 0 Failure
*/
static uint8_t PHLSendAck(PHPacketDataHandle data, PHPacketHandle p)
{
// Create the ACK message
PHPacketHeader ack;
ack.source_id = data->cfg.id;
ack.destination_id = p->header.source_id;
ack.type = PACKET_TYPE_ACK;
ack.rx_seq = p->header.tx_seq;
ack.data_size = 0;
// Set the packet.
PHLTransmitPacket(data, (PHPacketHandle)&ack);
return 1;
}

63
flight/Modules/RadioComBridge/RadioComBridge.c
View File

@ -41,12 +41,14 @@
static void radio2ComBridgeTask(void *parameters);
static void com2RadioBridgeTask(void *parameters);
static int32_t transmitData(uint8_t * data, int32_t length);
static void updateSettings();
// ****************
// Private constants
#define STACK_SIZE_BYTES 280
//#define STACK_SIZE_BYTES 280
#define STACK_SIZE_BYTES 350
#define TASK_PRIORITY (tskIDLE_PRIORITY + 1)
#define BRIDGE_BUF_LEN 10
@ -66,6 +68,14 @@ typedef struct {
// The com ports
uint32_t com_port;
uint32_t radio_port;
// The UAVTalk connection on the com side.
UAVTalkConnection uavTalkCon;
// Error statistics.
uint32_t com_tx_errors;
uint32_t radio_tx_errors;
} RadioComBridgeData;
// ****************
@ -74,11 +84,10 @@ typedef struct {
static RadioComBridgeData *data;
/**
* Initialise the module
* Start the module
* \return -1 if initialisation failed
* \return 0 on success
*/
static int32_t RadioComBridgeStart(void)
{
if(data) {
@ -90,6 +99,7 @@ static int32_t RadioComBridgeStart(void)
return -1;
}
/**
* Initialise the module
* \return -1 if initialisation failed
@ -104,13 +114,21 @@ static int32_t RadioComBridgeInitialize(void)
return -1;
// TODO: Get from settings object
data->com_port = PIOS_COM_TELEM_USB;
data->radio_port = PIOS_COM_RFM22B_RF;
data->com_port = PIOS_COM_BRIDGE_COM;
data->radio_port = PIOS_COM_BRIDGE_RADIO;
// Allocate the com buffers.
data->radio2com_buf = pvPortMalloc(BRIDGE_BUF_LEN);
PIOS_Assert(data->radio2com_buf);
data->com2radio_buf = pvPortMalloc(BRIDGE_BUF_LEN);
PIOS_Assert(data->com2radio_buf);
// Initialise UAVTalk
data->uavTalkCon = UAVTalkInitialize(&transmitData);
// Initialize the statistics.
data->com_tx_errors = 0;
data->radio_tx_errors = 0;
updateSettings();
@ -119,46 +137,65 @@ static int32_t RadioComBridgeInitialize(void)
MODULE_INITCALL(RadioComBridgeInitialize, RadioComBridgeStart)
/**
* Main task. It does not return.
* The radio to com bridge task.
*/
static void radio2ComBridgeTask(void *parameters)
{
/* Handle radio -> usart/usb direction */
volatile uint32_t tx_errors = 0;
while (1) {
uint32_t rx_bytes;
// Receive data from the radio port
rx_bytes = PIOS_COM_ReceiveBuffer(data->radio_port, data->radio2com_buf, BRIDGE_BUF_LEN, 500);
if (rx_bytes > 0) {
/* Bytes available to transfer */
/* Send the received data to the com port */
if (PIOS_COM_SendBuffer(data->com_port, data->radio2com_buf, rx_bytes) != rx_bytes) {
/* Error on transmit */
tx_errors++;
data->com_tx_errors++;
}
}
}
}
/**
* The com to radio bridge task.
*/
static void com2RadioBridgeTask(void * parameters)
{
/* Handle usart/usb -> radio direction */
volatile uint32_t tx_errors = 0;
while (1) {
uint32_t rx_bytes;
// Receive data from the com port
rx_bytes = PIOS_COM_ReceiveBuffer(data->com_port, data->com2radio_buf, BRIDGE_BUF_LEN, 500);
if (rx_bytes > 0) {
/* Bytes available to transfer */
/* Send the received data to the radio port */
if (PIOS_COM_SendBuffer(data->radio_port, data->com2radio_buf, rx_bytes) != rx_bytes) {
/* Error on transmit */
tx_errors++;
data->radio_tx_errors++;
}
// Pass the data through UAVTalk
for (uint8_t i = 0; i < rx_bytes; i++)
UAVTalkProcessInputStream(data->uavTalkCon, data->com2radio_buf[i]);
}
}
}
/**
* Transmit data buffer to the com port.
* \param[in] buf Data buffer to send
* \param[in] length Length of buffer
* \return -1 on failure
* \return number of bytes transmitted on success
*/
static int32_t transmitData(uint8_t *buf, int32_t length)
{
return PIOS_COM_SendBuffer(data->com_port, buf, length);
}
static void updateSettings()
{
if (data->com_port) {

8
flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h
View File

@ -159,7 +159,15 @@ extern uint32_t pios_com_rfm22b_id;
#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id)
#define PIOS_COM_VCP_USB (pios_com_vcp_usb_id)
#define PIOS_COM_RFM22B_RF (pios_com_rfm22b_id)
/*
#define PIOS_COM_DEBUG PIOS_COM_TELEM_SERIAL
#define PIOS_COM_RADIO_TEMP PIOS_COM_FLEXI
#define PIOS_COM_BRIDGE_COM PIOS_COM_TELEM_USB
*/
#define PIOS_COM_DEBUG PIOS_COM_TELEM_SERIAL
#define PIOS_COM_RADIO_TEMP PIOS_COM_FLEXI
#define PIOS_COM_BRIDGE_COM PIOS_COM_TELEM_SERIAL
#define PIOS_COM_BRIDGE_RADIO PIOS_COM_RFM22B_RF
//------------------------
// PIOS_RCVR

93
flight/PiOS/Common/pios_rfm22b.c
View File

@ -33,9 +33,9 @@
#if defined(PIOS_INCLUDE_RFM22B)
#include <pios_rfm22b_priv.h>
#include <packet_handler.h>
#define OUTPUT_COM PIOS_COM_FLEXI
#include <pios_rfm22b_priv.h>
/* Provide a COM driver */
static void PIOS_RFM22B_ChangeBaud(uint32_t rfm22b_id, uint32_t baud);
@ -44,6 +44,9 @@ static void PIOS_RFM22B_RegisterTxCallback(uint32_t rfm22b_id, pios_com_callback
static void PIOS_RFM22B_TxStart(uint32_t rfm22b_id, uint16_t tx_bytes_avail);
static void PIOS_RFM22B_RxStart(uint32_t rfm22b_id, uint16_t rx_bytes_avail);
static uint8_t PIOS_RFM22B_send_packet(PHPacketHandle packet);
static void PIOS_RFM22B_receive_data(uint8_t *data, uint8_t len);
static void PIOS_RFM22B_Timer_Callback(uint32_t dev_id);
const struct pios_com_driver pios_rfm22b_com_driver = {
@ -69,7 +72,8 @@ struct pios_rfm22b_dev {
pios_com_callback tx_out_cb;
uint32_t tx_out_context;
uint32_t rx_dropped;
PHInstHandle packet_handler;
PHPacketHandle cur_tx_packet;
};
static bool PIOS_RFM22B_validate(struct pios_rfm22b_dev * rfm22b_dev)
@ -116,15 +120,29 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, const struct pios_rfm22b_cfg *cfg)
struct pios_rfm22b_dev * rfm22b_dev;
// Allocate the device structure.
rfm22b_dev = (struct pios_rfm22b_dev *) PIOS_RFM22B_alloc();
if (!rfm22b_dev)
return(-1);
/* Bind the configuration to the device instance */
// Bind the configuration to the device instance
rfm22b_dev->cfg = cfg;
/* Configure the countdown timer and register the tick callback. */
rfm22b_dev->countdown_timer = (uint32_t)((float)(rfm22b_dev->cfg->send_timeout) / 0.625);
// Configure the packet handler.
PacketHandlerConfig phcfg = {
.txWinSize = cfg->txWinSize,
.maxConnections = cfg->maxConnections,
.id = cfg->id,
.output_stream = PIOS_RFM22B_send_packet,
.set_baud = 0,
.data_handler = PIOS_RFM22B_receive_data,
.receiver_handler = 0,
};
rfm22b_dev->packet_handler = PHInitialize(&phcfg);
rfm22b_dev->cur_tx_packet = 0;
// Configure the countdown timer and register the tick callback.
rfm22b_dev->countdown_timer = (uint32_t)((float)(rfm22b_dev->cfg->sendTimeout) / 0.625);
if (!PIOS_RTC_RegisterTickCallback(PIOS_RFM22B_Timer_Callback, (uint32_t)rfm22b_dev)) {
PIOS_DEBUG_Assert(0);
}
@ -150,18 +168,34 @@ static void PIOS_RFM22B_TxStart(uint32_t rfm22b_id, uint16_t tx_bytes_avail)
bool valid = PIOS_RFM22B_validate(rfm22b_dev);
PIOS_Assert(valid);
bool need_yield = false;
if (rfm22b_dev->tx_out_cb) {
uint8_t buf[16];
uint16_t bytes_to_send = (rfm22b_dev->tx_out_cb)(rfm22b_dev->tx_out_context, buf, 16, NULL, &need_yield);
if(bytes_to_send > 0)
PIOS_COM_SendBuffer(OUTPUT_COM, buf, bytes_to_send);
// Get a TX packet if necessary
PHPacketHandle p = rfm22b_dev->cur_tx_packet;
if (p == NULL) {
p = PHGetTXPacket(rfm22b_dev->packet_handler);
// If we couldn't get a packet, we can't transmit anything.
if (!p)
return;
// Initialize the packet.
p->header.type = PACKET_TYPE_DATA;
p->header.data_size = 0;
rfm22b_dev->cur_tx_packet = p;
}
#if defined(PIOS_INCLUDE_FREERTOS)
if (need_yield)
vPortYieldFromISR();
#endif /* PIOS_INCLUDE_FREERTOS */
// Get some data.
bool need_yield = false;
uint16_t bytes_to_send = (rfm22b_dev->tx_out_cb)(rfm22b_dev->tx_out_context, p->data + p->header.data_size, PH_MAX_DATA - p->header.data_size, NULL, &need_yield);
p->header.data_size += bytes_to_send;
// Send the packet if the data size is over the minimum threshold.
if (p->header.data_size >= rfm22b_dev->cfg->minPacketSize) {
PHTransmitPacket(rfm22b_dev->packet_handler, p);
rfm22b_dev->cur_tx_packet = NULL;
// Reset the countdown timer.
rfm22b_dev->countdown_timer = (uint32_t)((float)(rfm22b_dev->cfg->sendTimeout) / 0.625);
}
}
/**
@ -208,6 +242,16 @@ static void PIOS_RFM22B_RegisterTxCallback(uint32_t rfm22b_id, pios_com_callback
rfm22b_dev->tx_out_cb = tx_out_cb;
}
static uint8_t PIOS_RFM22B_send_packet(PHPacketHandle packet)
{
PIOS_COM_SendBuffer(PIOS_COM_RADIO_TEMP, packet->data, packet->header.data_size);
return 1;
}
static void PIOS_RFM22B_receive_data(uint8_t *data, uint8_t len)
{
}
static void PIOS_RFM22B_Timer_Callback(uint32_t dev_id) {
/* Recover our device context */
struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)dev_id;
@ -220,18 +264,31 @@ static void PIOS_RFM22B_Timer_Callback(uint32_t dev_id) {
bool need_yield = false;
if (rfm22b_dev->rx_in_cb) {
uint8_t buf[16];
uint32_t rx_bytes = PIOS_COM_ReceiveBuffer(OUTPUT_COM, buf, 16, 0);
uint32_t rx_bytes = PIOS_COM_ReceiveBuffer(PIOS_COM_RADIO_TEMP, buf, 16, 0);
if (rx_bytes > 0)
(rfm22b_dev->rx_in_cb)(rfm22b_dev->rx_in_context, buf, rx_bytes, NULL, &need_yield);
}
#if defined(PIOS_INCLUDE_FREERTOS)
if (need_yield)
vPortYieldFromISR();
#endif /* PIOS_INCLUDE_FREERTOS */
/*
* RTC runs at 625Hz.
*/
if(--rfm22b_dev->countdown_timer > 0)
return;
rfm22b_dev->countdown_timer = (uint32_t)((float)(rfm22b_dev->cfg->send_timeout) / 0.625);
rfm22b_dev->countdown_timer = (uint32_t)((float)(rfm22b_dev->cfg->sendTimeout) / 0.625);
// Send the current packet if there is one.
if (rfm22b_dev->cur_tx_packet) {
PHTransmitPacket(rfm22b_dev->packet_handler, rfm22b_dev->cur_tx_packet);
rfm22b_dev->cur_tx_packet = NULL;
}
}
#endif

6
flight/PiOS/inc/pios_rfm22b_priv.h
View File

@ -38,7 +38,11 @@
extern const struct pios_com_driver pios_rfm22b_com_driver;
struct pios_rfm22b_cfg {
uint32_t send_timeout;
uint32_t sendTimeout;
uint8_t minPacketSize;
uint8_t txWinSize;
uint8_t maxConnections;
uint32_t id;
};
extern int32_t PIOS_RFM22B_Init(uint32_t *rfb22b_id, const struct pios_rfm22b_cfg *cfg);

3
flight/PipXtreme/Makefile
View File

@ -220,6 +220,9 @@ SRC += $(PIOSCOMMON)/pios_rfm22b.c
SRC += $(FLIGHTLIB)/fifo_buffer.c
SRC += $(FLIGHTLIB)/CoordinateConversions.c
SRC += $(FLIGHTLIB)/taskmonitor.c
SRC += $(FLIGHTLIB)/aes.c
SRC += $(FLIGHTLIB)/crc.c
SRC += $(FLIGHTLIB)/packet_handler.c
## CMSIS for STM32
SRC += $(CMSISDIR)/core_cm3.c

2
flight/PipXtreme/System/inc/pios_config.h
View File

@ -35,7 +35,7 @@
/* Enable/Disable PiOS Modules */
#define PIOS_INCLUDE_DELAY
#define PIOS_INCLUDE_I2C
//#define PIOS_INCLUDE_I2C
#define PIOS_INCLUDE_IRQ
#define PIOS_INCLUDE_LED
#define PIOS_INCLUDE_IAP

11
flight/PipXtreme/System/pios_board.c
View File

@ -669,7 +669,11 @@ const struct pios_usb_cdc_cfg pios_usb_cdc_cfg = {
#include <pios_rfm22b_priv.h>
const struct pios_rfm22b_cfg pios_rfm22b_cfg = {
.send_timeout = 15, /* ms */
.sendTimeout = 15, /* ms */
.minPacketSize = 0,
.txWinSize = 4,
.maxConnections = 1,
.id = 0x36249acb
};
#endif /* PIOS_INCLUDE_RFM22B */
@ -694,10 +698,8 @@ void PIOS_Board_Init(void) {
HwSettingsInitialize();
#ifndef ERASE_FLASH
/* Initialize watchdog as early as possible to catch faults during init */
PIOS_WDG_Init();
#endif
/* Initialize the alarms library */
AlarmsInitialize();
@ -854,6 +856,9 @@ void PIOS_Board_Init(void) {
}
#endif /* PIOS_INCLUDE_RFM22B */
PIOS_COM_SendString(PIOS_COM_DEBUG, "Hello DEBUG\n\r");
PIOS_COM_SendString(PIOS_COM_FLEXI, "Hello Flexi\n\r");
PIOS_COM_SendString(PIOS_COM_TELEM_SERIAL, "Hello Telem Serial\n\r");
PIOS_COM_SendString(PIOS_COM_VCP_USB, "Hello VCP\n\r");
/* Remap AFIO pin */
GPIO_PinRemapConfig( GPIO_Remap_SWJ_NoJTRST, ENABLE);

101
flight/PipXtreme/inc/packet_handler.h
View File

@ -1,9 +1,13 @@
/**
******************************************************************************
* @addtogroup OpenPilotSystem OpenPilot System
* @{
* @addtogroup OpenPilotLibraries OpenPilot System Libraries
* @{
*
* @file packet_handler.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Modem packet handling routines
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief A packet handler for handeling radio packet transmission.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
@ -26,51 +30,64 @@
#ifndef __PACKET_HANDLER_H__
#define __PACKET_HANDLER_H__
#include "stdint.h"
// Public types
typedef enum {
PACKET_TYPE_NONE = 0,
PACKET_TYPE_CONNECT, // for requesting a connection
PACKET_TYPE_DISCONNECT, // to tell the other modem they cannot connect to us
PACKET_TYPE_READY, // tells the other modem we are ready to accept more data
PACKET_TYPE_NOTREADY, // tells the other modem we're not ready to accept more data - we can also send user data in this packet type
PACKET_TYPE_DATARATE, // for changing the RF data rate
PACKET_TYPE_PING, // used to check link is still up
PACKET_TYPE_ADJUST_TX_PWR, // used to ask the other modem to adjust it's tx power
PACKET_TYPE_DATA, // data packet (packet contains user data)
PACKET_TYPE_ACKED_DATA, // data packet that requies an ACK
PACKET_TYPE_RECEIVER, // Receiver relay values
PACKET_TYPE_ACK
} PHPacketType;
// *****************************************************************************
typedef struct {
uint32_t source_id;
uint32_t destination_id;
uint8_t type;
uint8_t tx_seq;
uint8_t rx_seq;
uint8_t data_size;
} PHPacketHeader;
#define PH_MAX_CONNECTIONS 1 // maximum number of remote connections
#define PH_MAX_DATA (255 - sizeof(PHPacketHeader))
typedef struct {
PHPacketHeader header;
uint8_t data[PH_MAX_DATA];
} PHPacket, *PHPacketHandle;
// *****************************************************************************
typedef struct {
uint8_t txWinSize;
uint16_t maxConnections;
uint32_t id;
uint8_t (*output_stream)(PHPacketHandle packet);
void (*set_baud)(uint32_t baud);
void (*data_handler)(uint8_t *data, uint8_t len);
void (*receiver_handler)(uint8_t *data, uint8_t len);
} PacketHandlerConfig;
void ph_1ms_tick(void);
void ph_process(void);
typedef int32_t (*PHOutputStream)(PHPacketHandle packet);
bool ph_connected(const int connection_index);
typedef void* PHInstHandle;
uint16_t ph_putData_free(const int connection_index);
uint16_t ph_putData(const int connection_index, const void *data, uint16_t len);
// Public functions
PHInstHandle PHInitialize(PacketHandlerConfig *cfg);
uint32_t PHConnect(PHInstHandle h, uint32_t dest_id);
PHPacketHandle PHGetTXPacket(PHInstHandle h);
PHPacketHandle PHReserveTXPacket(PHInstHandle h);
void PHReleaseLock(PHInstHandle h, bool keep_packet);
void PHReleaseTXPacket(PHInstHandle h, PHPacketHandle p);
uint8_t PHTransmitPacket(PHInstHandle h, PHPacketHandle p);
uint8_t PHReceivePacket(PHInstHandle h, PHPacketHandle p);
uint16_t ph_getData_used(const int connection_index);
uint16_t ph_getData(const int connection_index, void *data, uint16_t len);
#endif // __PACKET_HANDLER_H__
void ph_setFastPing(bool fast);
uint16_t ph_getRetries(const int connection_index);
uint8_t ph_getCurrentLinkState(const int connection_index);
int16_t ph_getLastRSSI(const int connection_index);
int32_t ph_getLastAFC(const int connection_index);
void ph_setNominalCarrierFrequency(uint32_t frequency_hz);
uint32_t ph_getNominalCarrierFrequency(void);
void ph_setDatarate(uint32_t datarate_bps);
uint32_t ph_getDatarate(void);
void ph_setTxPower(uint8_t tx_power);
uint8_t ph_getTxPower(void);
void ph_set_AES128_key(const void *key);
int ph_set_remote_serial_number(int connection_index, uint32_t sn);
void ph_set_remote_encryption(int connection_index, bool enabled, const void *key);
void ph_deinit(void);
void ph_init(uint32_t our_sn);
// *****************************************************************************
#endif
/**
* @}
* @}
*/

1924
flight/PipXtreme/packet_handler.c
View File

@ -1,8 +1,13 @@
/******************************************************************************
/**
******************************************************************************
* @addtogroup OpenPilotSystem OpenPilot System
* @{
* @addtogroup OpenPilotLibraries OpenPilot System Libraries
* @{
*
* @file packet_handler.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Modem packet handling routines
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief A packet handler for handeling radio packet transmission.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
@ -22,1703 +27,264 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
// ********
// We use 128-bit AES CBC encryption if encryption is enabled
// encrypted packet format
// 16-byte CBC .. 1st byte must not be zero
// 4-byte source id
// 4-byte destination id
// 1-byte packet type
// 1-byte tx sequence value
// 1-byte rx sequence value
// 1-byte data size
// 4-byte crc of entire packet not including CBC bytes
// unencrypted packet format
// 1-byte null byte .. set to zero to indicate packet is not encrypted
// 4-byte source id
// 4-byte destination id
// 1-byte packet type
// 1-byte tx sequence value
// 1-byte rx sequence value
// 1-byte data size
// 4-byte crc of entire packet not including the null byte
// ********
#include <string.h> // memmove
#include "main.h"
#include "rfm22b.h"
#include "fifo_buffer.h"
#include "openpilot.h"
#include "packet_handler.h"
#include "aes.h"
#include "crc.h"
#include "saved_settings.h"
#include "packet_handler.h"
#if defined(PIOS_COM_DEBUG)
// #define PACKET_DEBUG
#endif
// Private types and constants
typedef struct {
PacketHandlerConfig cfg;
PHPacket *tx_packets;
uint8_t tx_win_start;
uint8_t tx_win_end;
uint16_t tx_seq_id;
PHPacket rx_packet;
PHOutputStream stream;
xSemaphoreHandle lock;
} PHPacketData, *PHPacketDataHandle;
// *****************************************************************************
// Private functions
static uint8_t PHLSendAck(PHPacketDataHandle data, PHPacketHandle p);
static uint8_t PHLTransmitPacket(PHPacketDataHandle data, PHPacketHandle p);
#define PH_FIFO_BUFFER_SIZE 2048 // FIFO buffer size
// *****************************************************************************
#define AES_BLOCK_SIZE 16 // AES encryption does it in 16-byte blocks ONLY
// default aes 128-bit encryption key
const uint8_t default_aes_key[AES_BLOCK_SIZE] = {0x65, 0x3b, 0x71, 0x89, 0x4a, 0xf4, 0xc8, 0xcb, 0x18, 0xd4, 0x9b, 0x4d, 0x4a, 0xbe, 0xc8, 0x37};
// *****************************************************************************
#define RETRY_RECONNECT_COUNT 60 // if transmission retries this many times then reset the link to the remote modem
#define PACKET_TYPE_DATA_COMP_BIT 0x80 // data compressed bit. if set then the data in the packet is compressed
#define PACKET_TYPE_MASK 0x7f // packet type mask
enum {
PACKET_TYPE_NONE = 0,
PACKET_TYPE_CONNECT, // for requesting a connection
PACKET_TYPE_CONNECT_ACK, // ack
PACKET_TYPE_DISCONNECT, // to tell the other modem they cannot connect to us
PACKET_TYPE_DATA, // data packet (packet contains user data)
PACKET_TYPE_DATA_ACK, // ack
PACKET_TYPE_READY, // tells the other modem we are ready to accept more data
PACKET_TYPE_READY_ACK, // ack
PACKET_TYPE_NOTREADY, // tells the other modem we're not ready to accept more data - we can also send user data in this packet type
PACKET_TYPE_NOTREADY_ACK, // ack
PACKET_TYPE_DATARATE, // for changing the RF data rate
PACKET_TYPE_DATARATE_ACK, // ack
PACKET_TYPE_PING, // used to check link is still up
PACKET_TYPE_PONG, // ack
PACKET_TYPE_ADJUST_TX_PWR, // used to ask the other modem to adjust it's tx power
PACKET_TYPE_ADJUST_TX_PWR_ACK // ack
};
#define BROADCAST_ADDR 0xffffffff
//#pragma pack(push)
//#pragma pack(1)
typedef struct
/**
* Initialize the Packet Handler library
* \param[in] txWinSize The transmission window size (number of tx packet buffers).
* \param[in] streme A callback function for transmitting the packet.
* \param[in] id The source ID of transmitter.
* \return PHInstHandle The Pachet Handler instance data.
* \return 0 Failure
*/
PHInstHandle PHInitialize(PacketHandlerConfig *cfg)
{
uint32_t source_id;
uint32_t destination_id;
uint8_t type;
uint8_t tx_seq;
uint8_t rx_seq;
uint8_t data_size;
uint32_t crc;
} __attribute__((__packed__)) t_packet_header;
// Allocate the primary structure
PHPacketDataHandle data = pvPortMalloc(sizeof(PHPacketData));
if (!data)
return 0;
data->cfg = *cfg;
data->tx_seq_id = 0;
// this structure must be a multiple of 'AES_BLOCK_SIZE' bytes in size and no more than 255 bytes in size
typedef struct
{
uint8_t cbc[AES_BLOCK_SIZE]; // AES encryption Cipher-Block-Chaining key .. 1st byte must not be zero - to indicate the packet is encrypted
t_packet_header header;
uint8_t data[240 - sizeof(t_packet_header) - AES_BLOCK_SIZE];
} __attribute__((__packed__)) t_encrypted_packet;
// Allocate the tx packet window
data->tx_packets = pvPortMalloc(sizeof(PHPacket) * data->cfg.txWinSize);
// this structure must be no more than 255 bytes in size (255 = the maximum packet size)
typedef struct
{
uint8_t null_byte; // this must be set to zero - to indicate the packet is unencrypted
t_packet_header header;
uint8_t data[255 - sizeof(t_packet_header) - 1];
} __attribute__((__packed__)) t_unencrypted_packet;
// Initialize the window
data->tx_win_start = data->tx_win_end = 0;
for (uint8_t i = 0; i < data->cfg.txWinSize; ++i)
data->tx_packets[i].header.type = PACKET_TYPE_NONE;
//#pragma pack(pop)
// Create the lock
data->lock = xSemaphoreCreateRecursiveMutex();
// *****************************************************************************
// link state for each remote connection
enum {
LINK_DISCONNECTED = 0,
LINK_CONNECTING,
LINK_CONNECTED
};
typedef struct
{
uint32_t serial_number; // their serial number
uint8_t tx_buffer[PH_FIFO_BUFFER_SIZE] __attribute__ ((aligned(4)));
t_fifo_buffer tx_fifo_buffer; // holds the data to be transmitted to the other modem
uint8_t rx_buffer[PH_FIFO_BUFFER_SIZE] __attribute__ ((aligned(4)));
t_fifo_buffer rx_fifo_buffer; // holds the data received from the other modem
uint8_t link_state; // holds our current RF link state
uint8_t tx_sequence; // incremented with each data packet transmitted, sent in every packet transmitted
uint8_t tx_sequence_data_size; // the size of data we sent in our last packet
uint8_t rx_sequence; // incremented with each data packet received contain data, sent in every packet transmitted
volatile uint16_t tx_packet_timer; // ms .. used for packet timing
uint16_t tx_retry_time_slots; // add's some random packet transmission timing - to try to prevent transmission collisions
uint16_t tx_retry_time_slot_len; // ms .. " " "
uint16_t tx_retry_time; // ms .. " " "
uint16_t tx_retry_counter; // incremented on each transmission, reset back to '0' when we receive an ack to our transmission
volatile uint16_t data_speed_timer; // used for calculating the transmit/receive data rate
volatile uint32_t tx_data_speed_count; // incremented with the number of data bits we send in our transmit packets
volatile uint32_t tx_data_speed; // holds the number of data bits we have sent each second
volatile uint32_t rx_data_speed_count; // incremented with the number of data bits we send in our transmit packets
volatile uint32_t rx_data_speed; // holds the number of data bits we have received each second
uint16_t ping_time; // ping timer
uint16_t fast_ping_time; // ping timer
bool pinging; // TRUE if we are doing a ping test with the other modem - to check if it is still present
bool rx_not_ready_mode; // TRUE if we have told the other modem we cannot receive data (due to buffer filling up).
// we set it back to FALSE when our received buffer starts to empty
volatile int16_t ready_to_send_timer; // ms .. used to hold off packet transmission to wait a bit for data to mount up for transmission (improves data thru-put speed)
volatile int32_t not_ready_timer; // ms .. >= 0 while we have been asked not to send anymore data to the other modem, -1 when we are allowed to send data
bool send_encrypted; // TRUE if we are to AES encrypt in every packet we transmit
int16_t rx_rssi_dBm; // the strength of the received packet
int32_t rx_afc_Hz; // the frequency offset of the received packet
} t_connection;
// *****************************************************************************
uint32_t our_serial_number = 0; // our serial number
t_connection connection[PH_MAX_CONNECTIONS]; // holds each connection state
uint8_t aes_key[AES_BLOCK_SIZE] __attribute__ ((aligned(4))); // holds the aes encryption key - the same for ALL connections
uint8_t dec_aes_key[AES_BLOCK_SIZE] __attribute__ ((aligned(4))); // holds the pre-calculated decryption key
uint8_t enc_cbc[AES_BLOCK_SIZE] __attribute__ ((aligned(4))); // holds the tx aes cbc bytes
uint8_t ph_tx_buffer[256] __attribute__ ((aligned(4))); // holds the transmit packet
uint8_t ph_rx_buffer[256] __attribute__ ((aligned(4))); // holds the received packet
int16_t rx_rssi_dBm;
int32_t rx_afc_Hz;
bool fast_ping;
// *****************************************************************************
// return TRUE if we are connected to the remote modem
bool ph_connected(const int connection_index)
{
if (connection_index < 0 || connection_index >= PH_MAX_CONNECTIONS)
return FALSE;
t_connection *conn = &connection[connection_index];
return (conn->link_state == LINK_CONNECTED);
// Return the structure.
return (PHInstHandle)data;
}
// *****************************************************************************
// public tx buffer functions
uint16_t ph_putData_free(const int connection_index)
{ // return the free space size
if (connection_index < 0 || connection_index >= PH_MAX_CONNECTIONS)
return 0;
return fifoBuf_getFree(&connection[connection_index].tx_fifo_buffer);
}
uint16_t ph_putData(const int connection_index, const void *data, uint16_t len)
{ // add data to our tx buffer to be sent
if (connection_index < 0 || connection_index >= PH_MAX_CONNECTIONS)
return 0;
return fifoBuf_putData(&connection[connection_index].tx_fifo_buffer, data, len);
}
// *****************************************************************************
// public rx buffer functions
uint16_t ph_getData_used(const int connection_index)
{ // return the number of bytes available in the rx buffer
if (connection_index < 0 || connection_index >= PH_MAX_CONNECTIONS)
return 0;
return fifoBuf_getUsed(&connection[connection_index].rx_fifo_buffer);
}
uint16_t ph_getData(const int connection_index, void *data, uint16_t len)
{ // get data from our rx buffer
if (connection_index < 0 || connection_index >= PH_MAX_CONNECTIONS)
return 0;
return fifoBuf_getData(&connection[connection_index].rx_fifo_buffer, data, len);
}
// *****************************************************************************
// start a connection to another modem
int ph_startConnect(int connection_index, uint32_t sn)
/**
* Get a packet out of the transmit buffer.
* \param[in] h The packet handler instance data pointer.
* \param[in] dest_id The destination ID of this connection
* \return PHPacketHandle A pointer to the packet buffer.
* \return 0 No packets buffers avaiable in the transmit window.
*/
uint32_t PHConnect(PHInstHandle h, uint32_t dest_id)
{
random32 = updateCRC32(random32, 0xff);
if (connection_index < 0 || connection_index >= PH_MAX_CONNECTIONS)
return -1;
t_connection *conn = &connection[connection_index];
conn->link_state = LINK_DISCONNECTED;
LINK_LED_OFF;
conn->serial_number = sn;
conn->tx_sequence = 0;
conn->tx_sequence_data_size = 0;
conn->rx_sequence = 0;
// fifoBuf_init(&conn->tx_fifo_buffer, conn->tx_buffer, PH_FIFO_BUFFER_SIZE);
// fifoBuf_init(&conn->rx_fifo_buffer, conn->rx_buffer, PH_FIFO_BUFFER_SIZE);
conn->tx_packet_timer = 0;
conn->tx_retry_time_slots = 5;
uint32_t ms = 1280000ul / rfm22_getDatarate();
if (ms < 10) ms = 10;
else
if (ms > 32000) ms = 32000;
conn->tx_retry_time_slot_len = ms;
conn->tx_retry_time = conn->tx_retry_time_slot_len * 4 + (random32 % conn->tx_retry_time_slots) * conn->tx_retry_time_slot_len * 4;
conn->tx_retry_counter = 0;
conn->data_speed_timer = 0;
conn->tx_data_speed_count = 0;
conn->tx_data_speed = 0;
conn->rx_data_speed_count = 0;
conn->rx_data_speed = 0;
conn->ping_time = 8000 + (random32 % 100) * 10;
conn->fast_ping_time = 600 + (random32 % 50) * 10;
conn->pinging = false;
conn->rx_not_ready_mode = false;
conn->ready_to_send_timer = -1;
conn->not_ready_timer = -1;
// conn->send_encrypted = true;
// conn->send_encrypted = false;
conn->rx_rssi_dBm = -200;
conn->rx_afc_Hz = 0;
if (sn != 0 && sn == our_serial_number)
return -2; // same as our own
if (sn == BROADCAST_ADDR)
{
return -3;
}
if (conn->serial_number != 0)
conn->link_state = LINK_CONNECTING;
return connection_index;
return 1;
}
// *****************************************************************************
// return a byte for the tx packet transmission.
//
// return value < 0 if no more bytes available, otherwise return byte to be sent
int16_t ph_TxDataByteCallback(void)
/**
* Temporarily reserve the next packet in the TX packet window.
* This function places a tempoary hold on the next TX packet and
* retains the packet handler lock.
*
* NOTE: PHReleaseLock must be called to release the lock and retain
* or release the reserved packet.
*
* \param[in] h The packet handler instance data pointer.
* \return PHPacketHandle A pointer to the packet buffer.
* \return 0 No packets buffers avaiable in the transmit window.
*/
PHPacketHandle PHReserveTXPacket(PHInstHandle h)
{
return -1;
}
PHPacketDataHandle data = (PHPacketDataHandle)h;
// *************************************************************
// we are being given a block of received bytes
//
// return TRUE to continue current packet receive, otherwise return FALSE to halt current packet reception
// Lock
xSemaphoreTakeRecursive(data->lock, portMAX_DELAY);
bool ph_RxDataCallback(void *data, uint8_t len)
{
return true;
}
// Is the window full?
uint8_t next_end = (data->tx_win_end + 1) % data->cfg.txWinSize;
if(next_end == data->tx_win_start) {
// *****************************************************************************
// transmit a packet
// Release the lock
xSemaphoreGiveRecursive(data->lock);
bool ph_sendPacket(int connection_index, bool encrypt, uint8_t packet_type, bool send_immediately)
{
uint8_t key[AES_BLOCK_SIZE];
t_connection *conn = NULL;
// ***********
t_encrypted_packet *encrypted_packet = (t_encrypted_packet*)&ph_tx_buffer; // point to the tx buffer
t_unencrypted_packet *unencrypted_packet = (t_unencrypted_packet*)&ph_tx_buffer; // point to the tx buffer
t_packet_header *header;
uint8_t *data;
uint16_t max_data_size;
if (encrypt)
{
header = (t_packet_header *)&encrypted_packet->header;
data = (uint8_t *)&encrypted_packet->data;
max_data_size = sizeof(encrypted_packet->data);
}
else
{
header = (t_packet_header *)&unencrypted_packet->header;
data = (uint8_t *)&unencrypted_packet->data;
max_data_size = sizeof(unencrypted_packet->data);
}
// ***********
if (!rfm22_txReady())
return false;
if ((packet_type & PACKET_TYPE_MASK) == PACKET_TYPE_NONE)
return false;
if (connection_index >= PH_MAX_CONNECTIONS)
return false;
if (connection_index >= 0)
conn = (t_connection *)&connection[connection_index];
else
return false;
// ******************
// stuff
uint8_t pack_type = packet_type & PACKET_TYPE_MASK;
bool data_packet = (pack_type == PACKET_TYPE_DATA || pack_type == PACKET_TYPE_NOTREADY);
// ******************
// calculate how many user data bytes we are going to add to the packet
uint16_t data_size = 0;
if (data_packet && conn)
{ // we're adding user data to the packet
data_size = fifoBuf_getUsed(&connection[connection_index].tx_fifo_buffer); // the number of data bytes waiting to be sent
if (data_size > max_data_size)
data_size = max_data_size;
if (conn->tx_sequence_data_size > 0)
{ // we are re-sending data the same data
if (data_size > conn->tx_sequence_data_size)
data_size = conn->tx_sequence_data_size;
}
}
// ******************
// calculate the total packet size (including null data bytes if we have to add null data byte in AES encrypted packets)
uint32_t packet_size;
if (encrypt)
{
packet_size = AES_BLOCK_SIZE + sizeof(t_packet_header) + data_size;
// total packet size must be a multiple of 'AES_BLOCK_SIZE' bytes - aes encryption works on 16-byte blocks
packet_size = (packet_size + (AES_BLOCK_SIZE - 1)) & ~(AES_BLOCK_SIZE - 1);
}
else
{
packet_size = 1 + sizeof(t_packet_header) + data_size;
}
// ******************
// construct the packets entire header
if (encrypt)
{
memmove(key, aes_key, sizeof(key)); // fetch the encryption key
aes_encrypt_cbc_128(enc_cbc, key, NULL); // help randomize the CBC bytes
// ensure the 1st byte is not zero - to indicate this packet is encrypted
while (enc_cbc[0] == 0)
{
random32 = updateCRC32(random32, 0xff);
enc_cbc[0] ^= random32;
}
memmove(encrypted_packet->cbc, enc_cbc, AES_BLOCK_SIZE); // copy the AES CBC bytes into the packet
}
else
unencrypted_packet->null_byte = 0; // packet is not encrypted
header->source_id = our_serial_number; // our serial number
// header->destination_id = BROADCAST_ADDR; // broadcast packet
header->destination_id = conn->serial_number; // the other modems serial number
header->type = packet_type; // packet type
header->tx_seq = conn->tx_sequence; // our TX sequence number
header->rx_seq = conn->rx_sequence; // our RX sequence number
header->data_size = data_size; // the number of user data bytes in the packet
header->crc = 0; // the CRC of the header and user data bytes
// ******************
// add the user data to the packet
if (data_packet)
{ // we're adding user data to the packet
fifoBuf_getDataPeek(&connection[connection_index].tx_fifo_buffer, data, data_size);
if (encrypt)
{ // zero unused bytes
if (data_size < max_data_size)
memset(data + data_size, 0, max_data_size - data_size);
}
conn->tx_sequence_data_size = data_size; // remember how much data we are sending in this packet
}
// ******************
// complete the packet header by adding the CRC
if (encrypt)
header->crc = updateCRC32Data(0xffffffff, header, packet_size - AES_BLOCK_SIZE);
else
header->crc = updateCRC32Data(0xffffffff, header, packet_size - 1);
// ******************
// encrypt the packet
if (encrypt)
{ // encrypt the packet .. 'AES_BLOCK_SIZE' bytes at a time
uint8_t *p = (uint8_t *)encrypted_packet;
// encrypt the cbc
memmove(key, aes_key, sizeof(key)); // fetch the encryption key
aes_encrypt_cbc_128(p, key, NULL); // encrypt block of data (the CBC bytes)
p += AES_BLOCK_SIZE;
// encrypt the rest of the packet
for (uint16_t i = AES_BLOCK_SIZE; i < packet_size; i += AES_BLOCK_SIZE)
{
memmove(key, aes_key, sizeof(key)); // fetch the encryption key
aes_encrypt_cbc_128(p, key, enc_cbc); // encrypt block of data
p += AES_BLOCK_SIZE;
}
}
// ******************
// send the packet
int32_t res = rfm22_sendData(&ph_tx_buffer, packet_size, send_immediately);
// ******************
if (data_size > 0 && conn->tx_retry_counter == 0)
conn->tx_data_speed_count += data_size * 8; // + the number of data bits we just sent .. used for calculating the transmit data rate
// ******************
// debug stuff
#if defined(PACKET_DEBUG)
DEBUG_PRINTF("T-PACK ");
switch (pack_type)
{
case PACKET_TYPE_NONE: DEBUG_PRINTF("none"); break;
case PACKET_TYPE_CONNECT: DEBUG_PRINTF("connect"); break;
case PACKET_TYPE_CONNECT_ACK: DEBUG_PRINTF("connect_ack"); break;
case PACKET_TYPE_DISCONNECT: DEBUG_PRINTF("disconnect"); break;
case PACKET_TYPE_DATA: DEBUG_PRINTF("data"); break;
case PACKET_TYPE_DATA_ACK: DEBUG_PRINTF("data_ack"); break;
case PACKET_TYPE_READY: DEBUG_PRINTF("ready"); break;
case PACKET_TYPE_READY_ACK: DEBUG_PRINTF("ready_ack"); break;
case PACKET_TYPE_NOTREADY: DEBUG_PRINTF("notready"); break;
case PACKET_TYPE_NOTREADY_ACK: DEBUG_PRINTF("notready_ack"); break;
case PACKET_TYPE_DATARATE: DEBUG_PRINTF("datarate"); break;
case PACKET_TYPE_DATARATE_ACK: DEBUG_PRINTF("datarate_ack"); break;
case PACKET_TYPE_PING: DEBUG_PRINTF("ping"); break;
case PACKET_TYPE_PONG: DEBUG_PRINTF("pong"); break;
case PACKET_TYPE_ADJUST_TX_PWR: DEBUG_PRINTF("PACKET_TYPE_ADJUST_TX_PWR"); break;
case PACKET_TYPE_ADJUST_TX_PWR_ACK: DEBUG_PRINTF("PACKET_TYPE_ADJUST_TX_PWR_ACK"); break;
default: DEBUG_PRINTF("UNKNOWN [%d]", pack_type); break;
}
DEBUG_PRINTF(" tseq:%d rseq:%d", conn->tx_sequence, conn->rx_sequence);
DEBUG_PRINTF(" drate:%dbps", conn->tx_data_speed);
if (data_size > 0) DEBUG_PRINTF(" data_size:%d", data_size);
if (conn->tx_retry_counter > 0) DEBUG_PRINTF(" retry:%d", conn->tx_retry_counter);
DEBUG_PRINTF("\r\n");
#endif
// ******************
switch (pack_type)
{
case PACKET_TYPE_CONNECT:
case PACKET_TYPE_DISCONNECT:
case PACKET_TYPE_DATA:
case PACKET_TYPE_READY:
case PACKET_TYPE_NOTREADY:
case PACKET_TYPE_DATARATE:
case PACKET_TYPE_PING:
case PACKET_TYPE_ADJUST_TX_PWR:
if (conn->tx_retry_counter < 0xffff)
conn->tx_retry_counter++;
break;
case PACKET_TYPE_CONNECT_ACK:
case PACKET_TYPE_DATA_ACK:
case PACKET_TYPE_READY_ACK:
case PACKET_TYPE_NOTREADY_ACK:
case PACKET_TYPE_DATARATE_ACK:
case PACKET_TYPE_PONG:
case PACKET_TYPE_ADJUST_TX_PWR_ACK:
break;
case PACKET_TYPE_NONE:
break;
}
return (res >= packet_size);
}
// *****************************************************************************
void ph_processPacket2(bool was_encrypted, t_packet_header *header, uint8_t *data)
{ // process the received decrypted error-free packet
USB_LED_TOGGLE; // TEST ONLY
// ***********
// fetch the data compressed bit
bool compressed_data = (header->type & PACKET_TYPE_DATA_COMP_BIT) != 0;
// fetch the packet type
uint8_t packet_type = header->type & PACKET_TYPE_MASK;
// fetch the number of data bytes in the packet
uint16_t data_size = header->data_size;
// update the ramdon number
random32 = updateCRC32(random32, 0xff);
// *********************
// debug stuff
/*
#if defined(PACKET_DEBUG)
if (data_size > 0)
{
DEBUG_PRINTF("rx packet:");
for (uint16_t i = 0; i < data_size; i++)
DEBUG_PRINTF(" %u", data[i]);
DEBUG_PRINTF("\r\n");
}
#endif
*/
// ***********
// debug stuff
#if defined(PACKET_DEBUG)
DEBUG_PRINTF("R-PACK ");
switch (packet_type)
{
case PACKET_TYPE_NONE: DEBUG_PRINTF("none"); break;
case PACKET_TYPE_CONNECT: DEBUG_PRINTF("connect"); break;
case PACKET_TYPE_CONNECT_ACK: DEBUG_PRINTF("connect_ack"); break;
case PACKET_TYPE_DISCONNECT: DEBUG_PRINTF("disconnect"); break;
case PACKET_TYPE_DATA: DEBUG_PRINTF("data"); break;
case PACKET_TYPE_DATA_ACK: DEBUG_PRINTF("data_ack"); break;
case PACKET_TYPE_READY: DEBUG_PRINTF("ready"); break;
case PACKET_TYPE_READY_ACK: DEBUG_PRINTF("ready_ack"); break;
case PACKET_TYPE_NOTREADY: DEBUG_PRINTF("notready"); break;
case PACKET_TYPE_NOTREADY_ACK: DEBUG_PRINTF("notready_ack"); break;
case PACKET_TYPE_DATARATE: DEBUG_PRINTF("datarate"); break;
case PACKET_TYPE_DATARATE_ACK: DEBUG_PRINTF("datarate_ack"); break;
case PACKET_TYPE_PING: DEBUG_PRINTF("ping"); break;
case PACKET_TYPE_PONG: DEBUG_PRINTF("pong"); break;
case PACKET_TYPE_ADJUST_TX_PWR: DEBUG_PRINTF("PACKET_TYPE_ADJUST_TX_PWR"); break;
case PACKET_TYPE_ADJUST_TX_PWR_ACK: DEBUG_PRINTF("PACKET_TYPE_ADJUST_TX_PWR_ACK"); break;
default: DEBUG_PRINTF("UNKNOWN [%d]", packet_type); break;
}
DEBUG_PRINTF(" tseq-%d rseq-%d", header->tx_seq, header->rx_seq);
// DEBUG_PRINTF(" drate:%dbps", conn->rx_data_speed);
if (data_size > 0) DEBUG_PRINTF(" data_size:%d", data_size);
DEBUG_PRINTF(" %ddBm", rx_rssi_dBm);
DEBUG_PRINTF(" %dHz", rx_afc_Hz);
DEBUG_PRINTF("\r\n");
#endif
// *********************
if (header->source_id == our_serial_number)
return; // it's our own packet .. ignore it
if (header->destination_id == BROADCAST_ADDR)
{ // it's a broadcast packet
// todo:
return;
}
if (header->destination_id != our_serial_number)
return; // the packet is not meant for us
// *********************
// find out which remote connection this packet is from
int connection_index = 0;
while (connection_index < PH_MAX_CONNECTIONS)
{
uint32_t sn = connection[connection_index].serial_number;
if (sn != 0)
{ // connection used
if (header->source_id == sn)
break; // found it
}
connection_index++;
}
if (connection_index >= PH_MAX_CONNECTIONS)
{ // the packet is from an unknown source ID (unknown modem)
if (packet_type != PACKET_TYPE_NONE)
{ // send a disconnect packet back to them
// ph_sendPacket(-1, was_encrypted, PACKET_TYPE_DISCONNECT, true);
}
return;
}
t_connection *conn = &connection[connection_index];
// ***********
conn->rx_rssi_dBm = rx_rssi_dBm; // remember the packets signal strength
conn->rx_afc_Hz = rx_afc_Hz; // remember the packets frequency offset
// ***********
// decompress the data
if (compressed_data && data_size > 0)
{
// todo:
}
// ***********
if (packet_type == PACKET_TYPE_NONE)
return;
if (packet_type == PACKET_TYPE_DISCONNECT)
{
conn->link_state = LINK_DISCONNECTED;
LINK_LED_OFF;
return;
}
if (packet_type == PACKET_TYPE_CONNECT)
{
LINK_LED_ON;
// fifoBuf_init(&conn->tx_fifo_buffer, conn->tx_buffer, PH_FIFO_BUFFER_SIZE);
// fifoBuf_init(&conn->rx_fifo_buffer, conn->rx_buffer, PH_FIFO_BUFFER_SIZE);
conn->tx_packet_timer = 0;
conn->tx_retry_counter = 0;
conn->tx_retry_time = conn->tx_retry_time_slot_len + (random32 % conn->tx_retry_time_slots) * conn->tx_retry_time_slot_len;
conn->rx_sequence = header->tx_seq;
conn->tx_sequence = 0;
conn->tx_sequence_data_size = 0;
conn->data_speed_timer = 0;
conn->tx_data_speed_count = 0;
conn->tx_data_speed = 0;
conn->rx_data_speed_count = 0;
conn->rx_data_speed = 0;
conn->ping_time = 8000 + (random32 % 100) * 10;
conn->fast_ping_time = 600 + (random32 % 50) * 10;
conn->pinging = false;
conn->rx_not_ready_mode = false;
conn->ready_to_send_timer = -1;
conn->not_ready_timer = -1;
conn->link_state = LINK_CONNECTED;
// send an ack back
if (ph_sendPacket(connection_index, conn->send_encrypted, PACKET_TYPE_CONNECT_ACK, true))
{
conn->tx_packet_timer = 0;
}
return;
}
if (packet_type == PACKET_TYPE_CONNECT_ACK)
{
LINK_LED_ON;
if (conn->link_state != LINK_CONNECTING)
{ // reset the link
ph_set_remote_serial_number(connection_index, conn->serial_number);
return;
}
conn->tx_packet_timer = 0;
conn->tx_retry_counter = 0;
conn->tx_retry_time = conn->tx_retry_time_slot_len + (random32 % conn->tx_retry_time_slots) * conn->tx_retry_time_slot_len;
conn->rx_sequence = header->tx_seq;
conn->tx_sequence = 0;
conn->tx_sequence_data_size = 0;
conn->data_speed_timer = 0;
conn->tx_data_speed_count = 0;
conn->tx_data_speed = 0;
conn->rx_data_speed_count = 0;
conn->rx_data_speed = 0;
conn->ping_time = 8000 + (random32 % 100) * 10;
conn->fast_ping_time = 600 + (random32 % 50) * 10;
conn->pinging = false;
conn->rx_not_ready_mode = false;
conn->ready_to_send_timer = -1;
conn->not_ready_timer = -1;
conn->link_state = LINK_CONNECTED;
return;
}
if (conn->link_state == LINK_CONNECTING)
{ // we are trying to connect to them .. reply with a connect request packet
if (ph_sendPacket(connection_index, conn->send_encrypted, PACKET_TYPE_CONNECT, true))
{
conn->tx_packet_timer = 0;
conn->tx_retry_time = conn->tx_retry_time_slot_len * 4 + (random32 % conn->tx_retry_time_slots) * conn->tx_retry_time_slot_len * 4;
}
return;
}
if (conn->link_state != LINK_CONNECTED)
{ // they have sent us a packet when we are not in a connected state - start a connection
ph_startConnect(connection_index, conn->serial_number);
return;
}
// check to make sure it's a wanted packet type
switch (packet_type)
{
case PACKET_TYPE_DATA:
case PACKET_TYPE_DATA_ACK:
case PACKET_TYPE_READY:
case PACKET_TYPE_READY_ACK:
case PACKET_TYPE_NOTREADY:
case PACKET_TYPE_NOTREADY_ACK:
case PACKET_TYPE_DATARATE:
case PACKET_TYPE_DATARATE_ACK:
case PACKET_TYPE_PING:
case PACKET_TYPE_PONG:
case PACKET_TYPE_ADJUST_TX_PWR:
case PACKET_TYPE_ADJUST_TX_PWR_ACK:
break;
default:
return;
}
if ((conn->tx_sequence_data_size > 0) && (header->rx_seq == (uint8_t)(conn->tx_sequence + 1)))
{ // they received our last data packet
// remove the data we have sent and they have acked
fifoBuf_removeData(&conn->tx_fifo_buffer, conn->tx_sequence_data_size);
conn->tx_sequence++;
conn->tx_retry_counter = 0;
conn->tx_sequence_data_size = 0;
conn->not_ready_timer = -1; // stop timer
}
uint16_t size = fifoBuf_getUsed(&conn->tx_fifo_buffer); // the size of data waiting to be sent
if (packet_type == PACKET_TYPE_DATA || packet_type == PACKET_TYPE_DATA_ACK)
{
if (packet_type == PACKET_TYPE_DATA && header->tx_seq == conn->rx_sequence)
{ // the packet number is what we expected
if (data_size > 0)
{ // save the data
conn->rx_data_speed_count += data_size * 8; // + the number of data bits we just received
uint16_t num = fifoBuf_getFree(&conn->rx_fifo_buffer);
if (num < data_size)
{ // error .. we don't have enough space left in our fifo buffer to save the data .. discard it and tell them to hold off a sec
// conn->rx_not_ready_mode = true;
}
else
{ // save the received data into our fifo buffer
fifoBuf_putData(&conn->rx_fifo_buffer, data, data_size);
conn->rx_sequence++;
conn->rx_not_ready_mode = false;
}
}
}
if (size >= 200 || (conn->ready_to_send_timer >= 10 && size > 0) || (conn->tx_sequence_data_size > 0 && size > 0))
{ // send data
uint8_t pack_type = PACKET_TYPE_DATA;
if (conn->rx_not_ready_mode)
pack_type = PACKET_TYPE_NOTREADY;
if (ph_sendPacket(connection_index, conn->send_encrypted, pack_type, true))
{
conn->tx_packet_timer = 0;
conn->tx_retry_time = conn->tx_retry_time_slot_len + (random32 % conn->tx_retry_time_slots) * conn->tx_retry_time_slot_len;
return;
}
}
if (packet_type == PACKET_TYPE_DATA)
{ // send an ack back
uint8_t pack_type = PACKET_TYPE_DATA_ACK;
if (conn->rx_not_ready_mode)
pack_type = PACKET_TYPE_NOTREADY_ACK;
if (ph_sendPacket(connection_index, conn->send_encrypted, pack_type, true))
{
conn->tx_packet_timer = 0;
conn->tx_retry_time = conn->tx_retry_time_slot_len + (random32 % conn->tx_retry_time_slots) * conn->tx_retry_time_slot_len;
return;
}
}
return;
}
if (packet_type == PACKET_TYPE_READY)
{
conn->not_ready_timer = -1; // stop timer
// send an ack back
if (ph_sendPacket(connection_index, conn->send_encrypted, PACKET_TYPE_READY_ACK, true))
{
conn->tx_packet_timer = 0;
conn->tx_retry_time = conn->tx_retry_time_slot_len * 4 + (random32 % conn->tx_retry_time_slots) * conn->tx_retry_time_slot_len * 4;
return;
}
return;
}
if (packet_type == PACKET_TYPE_READY_ACK)
{
conn->rx_not_ready_mode = false;
return;
}
if (packet_type == PACKET_TYPE_NOTREADY)
{
// conn->not_ready_timer = 0; // start timer
if (header->tx_seq == conn->rx_sequence)
{ // the packet number is what we expected
if (data_size > 0)
{ // save the data
conn->rx_data_speed_count += data_size * 8; // + the number of data bits we just received
uint16_t num = fifoBuf_getFree(&conn->rx_fifo_buffer);
if (num < data_size)
{ // error .. we don't have enough space left in our fifo buffer to save the data .. discard it and tell them to hold off a sec
// conn->rx_not_ready_mode = true;
}
else
{ // save the received data into our fifo buffer
fifoBuf_putData(&conn->rx_fifo_buffer, data, data_size);
conn->rx_sequence++;
conn->rx_not_ready_mode = false;
}
}
}
// send an ack back
if (ph_sendPacket(connection_index, conn->send_encrypted, PACKET_TYPE_NOTREADY_ACK, true))
{
conn->tx_packet_timer = 0;
conn->tx_retry_time = conn->tx_retry_time_slot_len * 4 + (random32 % conn->tx_retry_time_slots) * conn->tx_retry_time_slot_len * 4;
return;
}
return;
}
if (packet_type == PACKET_TYPE_NOTREADY_ACK)
{
return;
}
if (packet_type == PACKET_TYPE_PING)
{ // send a pong back
if (ph_sendPacket(connection_index, conn->send_encrypted, PACKET_TYPE_PONG, true))
{
conn->tx_packet_timer = 0;
conn->tx_retry_time = conn->tx_retry_time_slot_len + (random32 % conn->tx_retry_time_slots) * conn->tx_retry_time_slot_len;
}
return;
}
if (packet_type == PACKET_TYPE_PONG)
{
if (conn->pinging)
{
conn->pinging = false;
conn->tx_retry_counter = 0;
}
return;
}
if (packet_type == PACKET_TYPE_DATARATE)
{
// send an ack back
if (ph_sendPacket(connection_index, conn->send_encrypted, PACKET_TYPE_DATARATE_ACK, true))
{
conn->tx_packet_timer = 0;
conn->tx_retry_time = conn->tx_retry_time_slot_len + (random32 % conn->tx_retry_time_slots) * conn->tx_retry_time_slot_len;
}
return;
}
if (packet_type == PACKET_TYPE_DATARATE_ACK)
{
return;
}
if (packet_type == PACKET_TYPE_ADJUST_TX_PWR)
{
// send an ack back
if (ph_sendPacket(connection_index, conn->send_encrypted, PACKET_TYPE_ADJUST_TX_PWR_ACK, true))
{
conn->tx_packet_timer = 0;
conn->tx_retry_time = conn->tx_retry_time_slot_len + (random32 % conn->tx_retry_time_slots) * conn->tx_retry_time_slot_len;
}
return;
}
if (packet_type == PACKET_TYPE_ADJUST_TX_PWR_ACK)
{
return;
}
// *********************
}
void ph_processRxPacket(void)
{
uint32_t crc1, crc2;
uint8_t key[AES_BLOCK_SIZE];
register uint8_t *p;
// ***********
// fetch the received packet
uint16_t packet_size = rfm22_receivedLength();
if (packet_size == 0)
return; // nothing received
if (packet_size > sizeof(ph_rx_buffer))
{ // packet too big .. discard it
rfm22_receivedDone();
return;
}
rx_rssi_dBm = rfm22_receivedRSSI(); // fetch the packets signal stength
rx_afc_Hz = rfm22_receivedAFCHz(); // fetch the packets frequency offset
// copy the received packet into our own buffer
memmove(ph_rx_buffer, rfm22_receivedPointer(), packet_size);
rfm22_receivedDone(); // the received packet has been saved
// *********************
// if the 1st byte in the packet is not zero, then the packet is encrypted
bool encrypted = (ph_rx_buffer[0] != 0);
// ***********
t_encrypted_packet *encrypted_packet = (t_encrypted_packet *)&ph_rx_buffer; // point to the rx buffer
t_unencrypted_packet *unencrypted_packet = (t_unencrypted_packet *)&ph_rx_buffer; // point to the rx buffer
t_packet_header *header;
uint8_t *data;
uint16_t min_packet_size;
uint16_t max_data_size;
if (encrypted)
{
header = (t_packet_header *)&encrypted_packet->header;
data = (uint8_t *)&encrypted_packet->data;
min_packet_size = AES_BLOCK_SIZE + sizeof(t_packet_header);
max_data_size = sizeof(encrypted_packet->data);
}
else
{
header = (t_packet_header *)&unencrypted_packet->header;
data = (uint8_t *)&unencrypted_packet->data;
min_packet_size = 1 + sizeof(t_packet_header);
max_data_size = sizeof(unencrypted_packet->data);
}
if (packet_size < min_packet_size)
{ // packet too small .. discard it
return;
}
random32 = updateCRC32(random32 ^ header->crc, 0xff); // help randomize the random number
// *********************
// help to randomize the tx aes cbc bytes by using the received packet
p = (uint8_t *)&ph_rx_buffer;
for (uint16_t i = 0; i < packet_size; i += AES_BLOCK_SIZE)
{
for (int j = AES_BLOCK_SIZE - 1; j >= 0; j--)
enc_cbc[j] ^= *p++;
}
// *********************
// check the packet size
if (encrypted)
{
if ((packet_size & (AES_BLOCK_SIZE - 1)) != 0)
return; // packet must be a multiple of 'AES_BLOCK_SIZE' bytes in length - for the aes decryption
}
// *********************
// decrypt the packet
if (encrypted)
{
p = (uint8_t *)encrypted_packet; // point to the received packet
// decrypt the cbc
memmove(key, (void *)dec_aes_key, sizeof(key)); // fetch the decryption key
aes_decrypt_cbc_128(p, key, NULL); // decrypt the cbc bytes
p += AES_BLOCK_SIZE;
// decrypt the rest of the packet
for (uint16_t i = AES_BLOCK_SIZE; i < packet_size; i += AES_BLOCK_SIZE)
{
memmove(key, (void *)dec_aes_key, sizeof(key)); // fetch the decryption key
aes_decrypt_cbc_128(p, key, (void *)encrypted_packet->cbc);
p += AES_BLOCK_SIZE;
}
}
// *********************
#if defined(PACKET_DEBUG)
DEBUG_PRINTF("rx packet: ");
DEBUG_PRINTF("%s", encrypted ? "encrypted " : "unencrypted");
if (encrypted)
{
for (int i = 0; i < AES_BLOCK_SIZE; i++)
DEBUG_PRINTF("%02X", encrypted_packet->cbc[i]);
}
DEBUG_PRINTF(" %08X %08X %u %u %u %u %08X\r\n",
header->source_id,
header->destination_id,
header->type,
header->tx_seq,
header->rx_seq,
header->data_size,
header->crc);
if (header->data_size > 0)
{
DEBUG_PRINTF("rx packet [%u]: ", header->data_size);
for (int i = 0; i < header->data_size; i++)
DEBUG_PRINTF("%02X", data[i]);
DEBUG_PRINTF("\r\n");
}
#endif
// *********************
uint32_t data_size = header->data_size;
if (packet_size < (min_packet_size + data_size))
return; // packet too small
#if defined(PACKET_DEBUG)
// DEBUG_PRINTF("rx packet size: %d\r\n", packet_size);
#endif
// *********************
// check the packet is error free
crc1 = header->crc;
header->crc = 0;
if (encrypted)
crc2 = updateCRC32Data(0xffffffff, header, packet_size - AES_BLOCK_SIZE);
else
crc2 = updateCRC32Data(0xffffffff, header, packet_size - 1);
if (crc1 != crc2)
{ // corrupt packet
#if defined(PACKET_DEBUG)
if (encrypted)
DEBUG_PRINTF("ENC-R-PACK corrupt %08X %08X\r\n", crc1, crc2);
else
DEBUG_PRINTF("R-PACK corrupt %08X %08X\r\n", crc1, crc2);
#endif
return;
}
// *********************
// process the data
ph_processPacket2(encrypted, header, data);
// *********************
}
// *****************************************************************************
// do all the link/packet handling stuff - request connection/disconnection, send data, acks etc
void ph_processLinks(int connection_index)
{
if (connection_index < 0 || connection_index >= PH_MAX_CONNECTIONS)
return;
random32 = updateCRC32(random32, 0xff);
t_connection *conn = &connection[connection_index];
bool canTx = (!rfm22_transmitting() && rfm22_channelIsClear());// TRUE is we can transmit
bool timeToRetry = (rfm22_txReady() && conn->tx_packet_timer >= conn->tx_retry_time);
bool tomanyRetries = (conn->tx_retry_counter >= RETRY_RECONNECT_COUNT);
if (conn->tx_retry_counter > 3)
conn->rx_rssi_dBm = -200;
switch (conn->link_state)
{
case LINK_DISCONNECTED:
if (!canTx)
{
conn->tx_packet_timer = 0;
break;
}
if (!rfm22_txReady() || conn->tx_packet_timer < 60000)
break;
if (our_serial_number != 0 && conn->serial_number != 0)
{ // try to reconnect with the remote modem
ph_startConnect(connection_index, conn->serial_number);
break;
}
break;
case LINK_CONNECTING:
if (!canTx)
{
conn->tx_packet_timer = 0;
break;
}
if (!timeToRetry)
break;
if (ph_sendPacket(connection_index, conn->send_encrypted, PACKET_TYPE_CONNECT, false))
{
conn->tx_packet_timer = 0;
conn->tx_retry_time = conn->tx_retry_time_slot_len * 4 + (random32 % conn->tx_retry_time_slots) * conn->tx_retry_time_slot_len * 4;
break;
}
break;
case LINK_CONNECTED:
if (!canTx)
{
conn->tx_packet_timer = 0;
break;
}
if (!timeToRetry)
break;
if (tomanyRetries)
{ // reset the link if we have sent tomany retries
ph_startConnect(connection_index, conn->serial_number);
break;
}
if (conn->pinging)
{ // we are trying to ping them
if (ph_sendPacket(connection_index, conn->send_encrypted, PACKET_TYPE_PING, false))
{
conn->tx_packet_timer = 0;
conn->tx_retry_time = conn->tx_retry_time_slot_len * 4 + (random32 % conn->tx_retry_time_slots) * conn->tx_retry_time_slot_len * 4;
}
break;
}
uint16_t ping_time = conn->ping_time;
if (fast_ping) ping_time = conn->fast_ping_time;
if (conn->tx_packet_timer >= ping_time)
{ // start pinging
if (ph_sendPacket(connection_index, conn->send_encrypted, PACKET_TYPE_PING, false))
{
conn->ping_time = 8000 + (random32 % 100) * 10;
conn->fast_ping_time = 600 + (random32 % 50) * 10;
conn->tx_packet_timer = 0;
conn->tx_retry_time = conn->tx_retry_time_slot_len * 4 + (random32 % conn->tx_retry_time_slots) * conn->tx_retry_time_slot_len * 4;
conn->pinging = true;
}
break;
}
// ***********
// exit rx not ready mode if we have space in our rx buffer for more data
/*
if (conn->rx_not_ready_mode)
{
uint16_t size = fifoBuf_getFree(&conn->rx_fifo_buffer);
if (size >= conn->rx_fifo_buffer.buf_size / 6)
{ // leave 'rx not ready' mode
if (ph_sendPacket(connection_index, conn->send_encrypted, PACKET_TYPE_READY, false))
{
conn->tx_packet_timer = 0;
conn->tx_retry_time = conn->tx_retry_time_slot_len + (random32 % conn->tx_retry_time_slots) * conn->tx_retry_time_slot_len;
break;
}
}
}
*/
// ***********
// send data packets
// if (conn->not_ready_timer < 0)
{
uint16_t size = fifoBuf_getUsed(&conn->tx_fifo_buffer);
if (size == 0)
conn->ready_to_send_timer = -1; // no data to send
else
if (conn->ready_to_send_timer < 0)
conn->ready_to_send_timer = 0; // start timer
if (size >= 200 || (conn->ready_to_send_timer >= saved_settings.rts_time && size > 0) || (conn->tx_sequence_data_size > 0 && size > 0))
{ // send data
uint8_t pack_type = PACKET_TYPE_DATA;
if (conn->rx_not_ready_mode)
pack_type = PACKET_TYPE_NOTREADY;
if (ph_sendPacket(connection_index, conn->send_encrypted, pack_type, false))
{
conn->tx_packet_timer = 0;
conn->tx_retry_time = conn->tx_retry_time_slot_len + (random32 % conn->tx_retry_time_slots) * conn->tx_retry_time_slot_len;
}
break;
}
}
// ***********
break;
default: // we should never end up here - maybe we should do a reboot?
conn->link_state = LINK_DISCONNECTED;
/*
// disable all interrupts
PIOS_IRQ_Disable();
// turn off all leds
USB_LED_OFF;
LINK_LED_OFF;
RX_LED_OFF;
TX_LED_OFF;
PIOS_SYS_Reset();
while (1);
*/
break;
}
}
// *****************************************************************************
void ph_setFastPing(bool fast)
{
fast_ping = fast;
}
// *****************************************************************************
uint8_t ph_getCurrentLinkState(const int connection_index)
{
if (connection_index < 0 || connection_index >= PH_MAX_CONNECTIONS)
return 0;
return connection[connection_index].link_state;
}
// *****************************************************************************
uint16_t ph_getRetries(const int connection_index)
{
if (connection_index < 0 || connection_index >= PH_MAX_CONNECTIONS)
return 0;
return connection[connection_index].tx_retry_counter;
}
// *****************************************************************************
int16_t ph_getLastRSSI(const int connection_index)
{
if (connection_index < 0 || connection_index >= PH_MAX_CONNECTIONS)
return 0;
return connection[connection_index].rx_rssi_dBm;
}
int32_t ph_getLastAFC(const int connection_index)
{
if (connection_index < 0 || connection_index >= PH_MAX_CONNECTIONS)
return 0;
return connection[connection_index].rx_afc_Hz;
}
// *****************************************************************************
// set/get the carrier frequency
void ph_setNominalCarrierFrequency(uint32_t frequency_hz)
{
rfm22_setNominalCarrierFrequency(frequency_hz);
}
uint32_t ph_getNominalCarrierFrequency(void)
{
return rfm22_getNominalCarrierFrequency();
}
// *****************************************************************************
// set/get the RF datarate
void ph_setDatarate(uint32_t datarate_bps)
{
rfm22_setDatarate(datarate_bps, TRUE);
uint32_t ms = 1280000ul / rfm22_getDatarate();
if (ms < 10) ms = 10;
else
if (ms > 32000) ms = 32000;
for (int i = 0; i < PH_MAX_CONNECTIONS; i++)
connection[i].tx_retry_time_slot_len = ms;
}
uint32_t ph_getDatarate(void)
{
return rfm22_getDatarate();
}
// *****************************************************************************
void ph_setTxPower(uint8_t tx_power)
{
rfm22_setTxPower(tx_power);
}
uint8_t ph_getTxPower(void)
{
return rfm22_getTxPower();
}
// *****************************************************************************
// set the AES encryption key
void ph_set_AES128_key(const void *key)
{
if (!key)
return;
memmove(aes_key, key, sizeof(aes_key));
// create the AES decryption key
aes_decrypt_key_128_create(aes_key, (void *)&dec_aes_key);
}
// *****************************************************************************
int ph_set_remote_serial_number(int connection_index, uint32_t sn)
{
random32 = updateCRC32(random32, 0xff);
if (ph_startConnect(connection_index, sn) >= 0)
{
t_connection *conn = &connection[connection_index];
// wipe any user data present in the buffers
fifoBuf_init(&conn->tx_fifo_buffer, conn->tx_buffer, PH_FIFO_BUFFER_SIZE);
fifoBuf_init(&conn->rx_fifo_buffer, conn->rx_buffer, PH_FIFO_BUFFER_SIZE);
return connection_index;
}
return -4;
}
void ph_set_remote_encryption(int connection_index, bool enabled, const void *key)
{
if (connection_index < 0 || connection_index >= PH_MAX_CONNECTIONS)
return;
ph_set_AES128_key(key);
connection[connection_index].send_encrypted = enabled;
}
// *****************************************************************************
// can be called from an interrupt if you wish.
// call this once every ms
void ph_1ms_tick(void)
{
if (booting) return;
if (saved_settings.mode == MODE_NORMAL)
{
// help randomize the encryptor cbc bytes
register uint32_t *cbc = (uint32_t *)&enc_cbc;
for (int i = 0; i < sizeof(enc_cbc) / 4; i++)
{
random32 = updateCRC32(random32, 0xff);
*cbc++ ^= random32;
}
for (int i = 0; i < PH_MAX_CONNECTIONS; i++)
{
t_connection *conn = &connection[i];
if (conn->tx_packet_timer < 0xffff)
conn->tx_packet_timer++;
if (conn->link_state == LINK_CONNECTED)
{ // we are connected
if (conn->ready_to_send_timer >= 0 && conn->ready_to_send_timer < 0x7fff)
conn->ready_to_send_timer++;
if (conn->not_ready_timer >= 0 && conn->not_ready_timer < 0x7fffffff)
conn->not_ready_timer++;
if (conn->data_speed_timer < 0xffff)
{
if (++conn->data_speed_timer >= 1000)
{ // 1 second gone by
conn->data_speed_timer = 0;
conn->tx_data_speed = (conn->tx_data_speed + conn->tx_data_speed_count) >> 1;
conn->tx_data_speed_count = 0;
conn->rx_data_speed = (conn->rx_data_speed + conn->rx_data_speed_count) >> 1;
conn->rx_data_speed_count = 0;
}
}
}
else
{ // we are not connected
if (conn->data_speed_timer) conn->data_speed_timer = 0;
if (conn->tx_data_speed_count) conn->tx_data_speed_count = 0;
if (conn->tx_data_speed) conn->tx_data_speed = 0;
if (conn->rx_data_speed_count) conn->rx_data_speed_count = 0;
if (conn->rx_data_speed) conn->rx_data_speed = 0;
}
}
}
}
// *****************************************************************************
// call this as often as possible - not from an interrupt
void ph_process(void)
{
if (booting) return;
if (saved_settings.mode == MODE_NORMAL)
{
ph_processRxPacket();
for (int i = 0; i < PH_MAX_CONNECTIONS; i++)
ph_processLinks(i);
}
else
{
}
}
// *****************************************************************************
void ph_disconnectAll(void)
{
for (int i = 0; i < PH_MAX_CONNECTIONS; i++)
{
random32 = updateCRC32(random32, 0xff);
t_connection *conn = &connection[i];
conn->serial_number = 0;
conn->tx_sequence = 0;
conn->tx_sequence_data_size = 0;
conn->rx_sequence = 0;
fifoBuf_init(&conn->tx_fifo_buffer, conn->tx_buffer, PH_FIFO_BUFFER_SIZE);
fifoBuf_init(&conn->rx_fifo_buffer, conn->rx_buffer, PH_FIFO_BUFFER_SIZE);
conn->link_state = LINK_DISCONNECTED;
conn->tx_packet_timer = 0;
conn->tx_retry_time_slots = 5;
conn->tx_retry_time_slot_len = 40;
conn->tx_retry_time = conn->tx_retry_time_slot_len * 4 + (random32 % conn->tx_retry_time_slots) * conn->tx_retry_time_slot_len * 4;
conn->tx_retry_counter = 0;
conn->data_speed_timer = 0;
conn->tx_data_speed_count = 0;
conn->tx_data_speed = 0;
conn->rx_data_speed_count = 0;
conn->rx_data_speed = 0;
conn->ping_time = 8000 + (random32 % 100) * 10;
conn->fast_ping_time = 600 + (random32 % 50) * 10;
conn->pinging = false;
conn->rx_not_ready_mode = false;
conn->ready_to_send_timer = -1;
conn->not_ready_timer = -1;
conn->send_encrypted = false;
conn->rx_rssi_dBm = -200;
conn->rx_afc_Hz = 0;
}
}
// *****************************************************************************
void ph_deinit(void)
{
ph_disconnectAll();
}
void ph_init(uint32_t our_sn)
{
our_serial_number = our_sn; // remember our own serial number
fast_ping = false;
ph_disconnectAll();
// set the AES encryption key using the default AES key
ph_set_AES128_key(default_aes_key);
// try too randomise the tx AES CBC bytes
for (uint32_t j = 0, k = 0; j < 123 + (random32 & 1023); j++)
{
random32 = updateCRC32(random32, 0xff);
enc_cbc[k] ^= random32 >> 3;
if (++k >= sizeof(enc_cbc)) k = 0;
return NULL;
}
// ******
rfm22_init_normal(saved_settings.min_frequency_Hz, saved_settings.max_frequency_Hz, rfm22_freqHopSize());
rfm22_TxDataByte_SetCallback(ph_TxDataByteCallback);
rfm22_RxData_SetCallback(ph_RxDataCallback);
rfm22_setFreqCalibration(saved_settings.rf_xtal_cap);
ph_setNominalCarrierFrequency(saved_settings.frequency_Hz);
ph_setDatarate(saved_settings.max_rf_bandwidth);
ph_setTxPower(saved_settings.max_tx_power);
ph_set_remote_encryption(0, saved_settings.aes_enable, (const void *)saved_settings.aes_key);
ph_set_remote_serial_number(0, saved_settings.destination_id);
// ******
// Return a pointer to the packet at the end of the TX window.
return data->tx_packets + data->tx_win_end;
}
// *****************************************************************************
/**
* Get a packet out of the transmit buffer and keep the lock.
* NOTE: PHReleaseLock must be called to release the lock.
* \param[in] h The packet handler instance data pointer.
* \param[in] keep_packet Maintain a permanent (until released) lock on the packet.
*/
void PHReleaseLock(PHInstHandle h, bool keep_packet)
{
PHPacketDataHandle data = (PHPacketDataHandle)h;
uint8_t next_end = (data->tx_win_end + 1) % data->cfg.txWinSize;
// Increment the end index if packet is being kept.
if (keep_packet)
data->tx_win_end = next_end;
// Release lock
xSemaphoreGiveRecursive(data->lock);
}
/**
* Get a packet out of the transmit buffer.
* \param[in] h The packet handler instance data pointer.
* \return PHPacketHandle A pointer to the packet buffer.
* \return 0 No packets buffers avaiable in the transmit window.
*/
PHPacketHandle PHGetTXPacket(PHInstHandle h)
{
PHPacketHandle p = PHReserveTXPacket(h);
PHReleaseLock(p, 1);
return p;
}
/**
* Release a packet from the transmit packet buffer window.
* \param[in] h The packet handler instance data pointer.
* \param[in] p A pointer to the packet buffer.
* \return Nothing
*/
void PHReleaseTXPacket(PHInstHandle h, PHPacketHandle p)
{
PHPacketDataHandle data = (PHPacketDataHandle)h;
// Lock
xSemaphoreTakeRecursive(data->lock, portMAX_DELAY);
// Change the packet type so we know this packet is unused.
p->header.type = PACKET_TYPE_NONE;
// If this packet is at the start of the window, increment the start index.
while ((data->tx_win_start != data->tx_win_end) &&
(data->tx_packets[data->tx_win_start].header.type == PACKET_TYPE_NONE))
data->tx_win_start = (data->tx_win_start + 1) % data->cfg.txWinSize;
// Release lock
xSemaphoreGiveRecursive(data->lock);
}
/**
* Transmit a packet from the transmit packet buffer window.
* \param[in] h The packet handler instance data pointer.
* \param[in] p A pointer to the packet buffer.
* \return 1 Success
* \return 0 Failure
*/
uint8_t PHTransmitPacket(PHInstHandle h, PHPacketHandle p)
{
PHPacketDataHandle data = (PHPacketDataHandle)h;
// Try to transmit the packet.
if (!PHLTransmitPacket(data, p))
return 0;
// If this packet doesn't require an ACK, remove it from the TX window.
switch (p->header.type) {
case PACKET_TYPE_READY:
case PACKET_TYPE_NOTREADY:
case PACKET_TYPE_DATA:
case PACKET_TYPE_RECEIVER:
PHReleaseTXPacket(h, p);
break;
}
return 1;
}
/**
* Process a packet that has been received.
* \param[in] h The packet handler instance data pointer.
* \param[in] p A pointer to the packet buffer.
* \return 1 Success
* \return 0 Failure
*/
uint8_t PHReceivePacket(PHInstHandle h, PHPacketHandle p)
{
PHPacketDataHandle data = (PHPacketDataHandle)h;
switch (p->header.type) {
case PACKET_TYPE_ACKED_DATA:
// Send the ACK
PHLSendAck(data, p);
// Pass on the data.
if(data->cfg.data_handler)
data->cfg.data_handler(p->data, p->header.data_size);
break;
case PACKET_TYPE_RECEIVER:
// Pass on the data to the receiver handler.
if(data->cfg.receiver_handler)
data->cfg.receiver_handler(p->data, p->header.data_size);
break;
}
return 1;
}
/**
* Transmit a packet from the transmit packet buffer window.
* \param[in] data The packet handler instance data pointer.
* \param[in] p A pointer to the packet buffer.
* \return 1 Success
* \return 0 Failure
*/
static uint8_t PHLTransmitPacket(PHPacketDataHandle data, PHPacketHandle p)
{
// Set the sequence ID to the current ID.
p->header.tx_seq = data->tx_seq_id++;
// Transmit the packet using the output stream.
if(!data->cfg.output_stream(p))
return 0;
return 1;
}
/**
* Send an ACK packet.
* \param[in] data The packet handler instance data pointer.
* \param[in] p A pointer to the packet buffer of the packet to be ACKed.
* \return 1 Success
* \return 0 Failure
*/
static uint8_t PHLSendAck(PHPacketDataHandle data, PHPacketHandle p)
{
// Create the ACK message
PHPacketHeader ack;
ack.source_id = data->cfg.id;
ack.destination_id = p->header.source_id;
ack.type = PACKET_TYPE_ACK;
ack.rx_seq = p->header.tx_seq;
ack.data_size = 0;
// Set the packet.
PHLTransmitPacket(data, (PHPacketHandle)&ack);
return 1;
}