Mirrors_Framework/ODrive
1
0
Fork 0
mirror of https://github.com/odriverobotics/ODrive.git synced 2026-09-23 09:03:40 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

refactor Fibre to AsyncStream architecture

Browse Source 
The previous implementation was based on blocking calls, i.e. the
platform dependent output channel would block on a write operation and
by extension the Fibre decoder could also block when given data by an
input channel. The revised implementation in this commit is based around
a fully asynchronous (i.e. non-blocking)  stream interface.
This commit is contained in:
Samuel Sadok
2020-09-15 12:57:39 +02:00
parent d2b469eca0
commit a1c67e6ba1
75 changed files with 5761 additions and 2078 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitignore
-3
View File
@@ -3,9 +3,6 @@ __pycache__/
*.py[cod]
*$py.class
# C extensions
*.so
# Unit test / coverage reports
htmlcov/
.tox/
Firmware/.gitignore
+3
View File
@@ -26,3 +26,6 @@ STM32CubeMX
#gdb log
openocd.log
# OS-specific files
.DS_Store
Firmware/.vscode/c_cpp_properties.json
Vendored
+6 -1
View File
@@ -11,7 +11,8 @@
"HW_VERSION_MAJOR=3",
"HW_VERSION_MINOR=6",
"HW_VERSION_VOLTAGE=56",
"USB_PROTOCOL_NATIVE",
"FIBRE_ENABLE_SERVER",
"FIBRE_ENABLE_CLIENT",
"__weak=\"__attribute__((weak))\"",
"__packed=\"__attribute__((__packed__))\"",
"__GNUC__"
@@ -32,6 +33,8 @@
"HW_VERSION_MAJOR=3",
"HW_VERSION_MINOR=6",
"HW_VERSION_VOLTAGE=56",
"FIBRE_ENABLE_SERVER",
"FIBRE_ENABLE_CLIENT",
"__weak=\"__attribute__((weak))\"",
"__packed=\"__attribute__((__packed__))\"",
"__GNUC__"
@@ -52,6 +55,8 @@
"HW_VERSION_MAJOR=3",
"HW_VERSION_MINOR=4",
"HW_VERSION_VOLTAGE=24",
"FIBRE_ENABLE_SERVER",
"FIBRE_ENABLE_CLIENT",
"__weak=\"__attribute__((weak))\"",
"__packed=\"__attribute__((__packed__))\"",
"__GNUC__"
Firmware/Board/v3/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc/usbd_cdc.h
+2 -3
View File
@@ -111,7 +111,6 @@ typedef struct _USBD_CDC_Itf
typedef struct
{
uint8_t* Buffer;
uint32_t Length;
volatile uint8_t State;
}
@@ -165,9 +164,9 @@ uint8_t USBD_CDC_SetTxBuffer (USBD_HandleTypeDef *pdev,
uint8_t USBD_CDC_SetRxBuffer (USBD_HandleTypeDef *pdev,
uint8_t *pbuff, uint8_t endpoint_pair);
uint8_t USBD_CDC_ReceivePacket (USBD_HandleTypeDef *pdev, uint8_t endpoint_pair);
uint8_t USBD_CDC_ReceivePacket (USBD_HandleTypeDef *pdev, uint8_t* buf, uint16_t len, uint8_t endpoint_num);
uint8_t USBD_CDC_TransmitPacket (USBD_HandleTypeDef *pdev, uint8_t endpoint_pair);
uint8_t USBD_CDC_TransmitPacket (USBD_HandleTypeDef *pdev, uint8_t* buf, size_t len, uint8_t endpoint_num);
/**
* @}
*/
Firmware/Board/v3/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c
+18 -109
View File
@@ -418,29 +418,7 @@ static uint8_t USBD_CDC_Init (USBD_HandleTypeDef *pdev,
hcdc->CDC_Rx.State = 0;
hcdc->ODRIVE_Tx.State = 0;
hcdc->ODRIVE_Rx.State = 0;
if(pdev->dev_speed == USBD_SPEED_HIGH )
{
/* Prepare Out endpoint to receive next packet */
USBD_LL_PrepareReceive(pdev,
CDC_OUT_EP,
hcdc->CDC_Rx.Buffer,
CDC_DATA_HS_OUT_PACKET_SIZE);
}
else
{
/* Prepare Out endpoint to receive next packet */
USBD_LL_PrepareReceive(pdev,
CDC_OUT_EP,
hcdc->CDC_Rx.Buffer,
CDC_DATA_FS_OUT_PACKET_SIZE);
}
/* Prepare ODrive Out endpoint to receive next packet */
USBD_LL_PrepareReceive(pdev,
ODRIVE_OUT_EP,
hcdc->ODRIVE_Rx.Buffer,
pdev->dev_speed == USBD_SPEED_HIGH ? CDC_DATA_HS_OUT_PACKET_SIZE : CDC_DATA_FS_OUT_PACKET_SIZE);
}
return ret;
}
@@ -571,12 +549,14 @@ static uint8_t USBD_CDC_DataIn (USBD_HandleTypeDef *pdev, uint8_t epnum)
if(pdev->pClassData != NULL)
{
// NOTE: We would logically expect xx_IN_EP here, but we actually get the xx_OUT_EP
if (epnum == CDC_OUT_EP)
if (epnum == CDC_OUT_EP) {
hcdc->CDC_Tx.State = 0;
if (epnum == ODRIVE_OUT_EP)
osMessagePut(usb_event_queue, 3, 0);
}
if (epnum == ODRIVE_OUT_EP) {
hcdc->ODRIVE_Tx.State = 0;
//Note: We could use independent semaphores for simoultainous USB transmission.
osSemaphoreRelease(sem_usb_tx);
osMessagePut(usb_event_queue, 4, 0);
}
return USBD_OK;
}
else
@@ -612,7 +592,7 @@ static uint8_t USBD_CDC_DataOut (USBD_HandleTypeDef *pdev, uint8_t epnum)
NAKed till the end of the application Xfer */
if(pdev->pClassData != NULL)
{
((USBD_CDC_ItfTypeDef *)pdev->pUserData)->Receive(hEP_Rx->Buffer, &hEP_Rx->Length, epnum);
((USBD_CDC_ItfTypeDef *)pdev->pUserData)->Receive(NULL, &hEP_Rx->Length, epnum);
return USBD_OK;
}
@@ -717,59 +697,6 @@ uint8_t USBD_CDC_RegisterInterface (USBD_HandleTypeDef *pdev,
return ret;
}
/**
* @brief USBD_CDC_SetTxBuffer
* @param pdev: device instance
* @param pbuff: Tx Buffer
* @retval status
*/
uint8_t USBD_CDC_SetTxBuffer (USBD_HandleTypeDef *pdev,
uint8_t *pbuff,
uint16_t length,
uint8_t endpoint_pair)
{
USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef*) pdev->pClassData;
USBD_CDC_EP_HandleTypeDef* hEP_Tx;
if (endpoint_pair == CDC_OUT_EP) {
hEP_Tx = &hcdc->CDC_Tx;
} else if (endpoint_pair == ODRIVE_OUT_EP) {
hEP_Tx = &hcdc->ODRIVE_Tx;
} else {
return USBD_FAIL;
}
hEP_Tx->Buffer = pbuff;
hEP_Tx->Length = length;
return USBD_OK;
}
/**
* @brief USBD_CDC_SetRxBuffer
* @param pdev: device instance
* @param pbuff: Rx Buffer
* @retval status
*/
uint8_t USBD_CDC_SetRxBuffer (USBD_HandleTypeDef *pdev,
uint8_t *pbuff, uint8_t endpoint_pair)
{
USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef*) pdev->pClassData;
USBD_CDC_EP_HandleTypeDef* hEP_Rx;
if (endpoint_pair == CDC_OUT_EP) {
hEP_Rx = &hcdc->CDC_Rx;
} else if (endpoint_pair == ODRIVE_OUT_EP) {
hEP_Rx = &hcdc->ODRIVE_Rx;
} else {
return USBD_FAIL;
}
hEP_Rx->Buffer = pbuff;
return USBD_OK;
}
/**
* @brief USBD_CDC_DataOut
@@ -778,7 +705,7 @@ uint8_t USBD_CDC_SetRxBuffer (USBD_HandleTypeDef *pdev,
* @param epnum: endpoint number
* @retval status
*/
uint8_t USBD_CDC_TransmitPacket(USBD_HandleTypeDef *pdev, uint8_t endpoint_pair)
uint8_t USBD_CDC_TransmitPacket(USBD_HandleTypeDef *pdev, uint8_t* buf, size_t len, uint8_t endpoint_num)
{
USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef*) pdev->pClassData;
@@ -786,13 +713,10 @@ uint8_t USBD_CDC_TransmitPacket(USBD_HandleTypeDef *pdev, uint8_t endpoint_pair
{
// Select Endpoint
USBD_CDC_EP_HandleTypeDef* hEP_Tx;
uint8_t in_ep;
if (endpoint_pair == CDC_OUT_EP) {
if (endpoint_num == CDC_IN_EP) {
hEP_Tx = &hcdc->CDC_Tx;
in_ep = CDC_IN_EP;
} else if (endpoint_pair == ODRIVE_OUT_EP) {
} else if (endpoint_num == ODRIVE_IN_EP) {
hEP_Tx = &hcdc->ODRIVE_Tx;
in_ep = ODRIVE_IN_EP;
} else {
return USBD_FAIL;
}
@@ -804,9 +728,9 @@ uint8_t USBD_CDC_TransmitPacket(USBD_HandleTypeDef *pdev, uint8_t endpoint_pair
/* Transmit next packet */
USBD_LL_Transmit(pdev,
in_ep,
hEP_Tx->Buffer,
hEP_Tx->Length);
endpoint_num,
buf,
len);
return USBD_OK;
}
@@ -828,31 +752,16 @@ uint8_t USBD_CDC_TransmitPacket(USBD_HandleTypeDef *pdev, uint8_t endpoint_pair
* @param pdev: device instance
* @retval status
*/
uint8_t USBD_CDC_ReceivePacket(USBD_HandleTypeDef *pdev, uint8_t endpoint_pair)
uint8_t USBD_CDC_ReceivePacket(USBD_HandleTypeDef *pdev, uint8_t* buf, uint16_t len, uint8_t endpoint_num)
{
USBD_CDC_HandleTypeDef *hcdc = (USBD_CDC_HandleTypeDef*) pdev->pClassData;
/* Suspend or Resume USB Out process */
if(pdev->pClassData != NULL)
{
// Select Endpoint
USBD_CDC_EP_HandleTypeDef* hEP_Rx;
uint8_t out_ep;
if (endpoint_pair == CDC_OUT_EP) {
hEP_Rx = &hcdc->CDC_Rx;
out_ep = CDC_OUT_EP;
} else if (endpoint_pair == ODRIVE_OUT_EP) {
hEP_Rx = &hcdc->ODRIVE_Rx;
out_ep = ODRIVE_OUT_EP;
} else {
return USBD_FAIL;
}
/* Prepare Out endpoint to receive next packet */
USBD_LL_PrepareReceive(pdev,
out_ep,
hEP_Rx->Buffer,
pdev->dev_speed == USBD_SPEED_HIGH ? CDC_DATA_HS_OUT_PACKET_SIZE : CDC_DATA_FS_OUT_PACKET_SIZE);
endpoint_num,
buf,
len);
return USBD_OK;
}
Firmware/Board/v3/Src/usbd_cdc_if.c
+6 -23
View File
@@ -114,15 +114,6 @@
* @brief Private variables.
* @{
*/
/* Create buffer for reception and transmission */
/* It's up to user to redefine and/or remove those define */
/** Received data over USB are stored in this buffer */
uint8_t CDCRxBufferFS[APP_RX_DATA_SIZE];
uint8_t ODRIVERxBufferFS[APP_RX_DATA_SIZE];
/** Data to send over USB CDC are stored in this buffer */
uint8_t CDCTxBufferFS[APP_TX_DATA_SIZE];
uint8_t ODRIVETxBufferFS[APP_TX_DATA_SIZE];
/* USER CODE BEGIN PRIVATE_VARIABLES */
/* USER CODE END PRIVATE_VARIABLES */
@@ -179,10 +170,7 @@ static int8_t CDC_Init_FS(void)
{
/* USER CODE BEGIN 3 */
/* Set Application Buffers */
USBD_CDC_SetTxBuffer(&hUsbDeviceFS, CDCTxBufferFS, 0, CDC_OUT_EP);
USBD_CDC_SetRxBuffer(&hUsbDeviceFS, CDCRxBufferFS, CDC_OUT_EP);
USBD_CDC_SetTxBuffer(&hUsbDeviceFS, ODRIVETxBufferFS, 0, ODRIVE_OUT_EP);
USBD_CDC_SetRxBuffer(&hUsbDeviceFS, ODRIVERxBufferFS, ODRIVE_OUT_EP);
osMessagePut(usb_event_queue, 1, 0);
return (USBD_OK);
/* USER CODE END 3 */
}
@@ -194,6 +182,7 @@ static int8_t CDC_Init_FS(void)
static int8_t CDC_DeInit_FS(void)
{
/* USER CODE BEGIN 4 */
osMessagePut(usb_event_queue, 2, 0);
return (USBD_OK);
/* USER CODE END 4 */
}
@@ -323,13 +312,10 @@ uint8_t CDC_Transmit_FS(uint8_t* Buf, uint16_t Len, uint8_t endpoint_pair)
// Select EP
USBD_CDC_EP_HandleTypeDef* hEP_Tx;
uint8_t* TxBuff;
if (endpoint_pair == CDC_OUT_EP) {
if (endpoint_pair == CDC_IN_EP) {
hEP_Tx = &hcdc->CDC_Tx;
TxBuff = CDCTxBufferFS;
} else if (endpoint_pair == ODRIVE_OUT_EP) {
} else if (endpoint_pair == ODRIVE_IN_EP) {
hEP_Tx = &hcdc->ODRIVE_Tx;
TxBuff = ODRIVETxBufferFS;
} else {
return USBD_FAIL;
}
@@ -337,11 +323,8 @@ uint8_t CDC_Transmit_FS(uint8_t* Buf, uint16_t Len, uint8_t endpoint_pair)
// Check for ongoing transmission
if (hEP_Tx->State != 0)
return USBD_BUSY;
// memcpy Buf into UserTxBufferFS
memcpy(TxBuff, Buf, Len);
// Update Len
USBD_CDC_SetTxBuffer(&hUsbDeviceFS, TxBuff, Len, endpoint_pair);
result = USBD_CDC_TransmitPacket(&hUsbDeviceFS, endpoint_pair);
result = USBD_CDC_TransmitPacket(&hUsbDeviceFS, Buf, Len, endpoint_pair);
/* USER CODE END 7 */
return result;
}
Firmware/MotorControl/main.cpp
+8 -14
View File
@@ -12,9 +12,8 @@
#include <communication/interface_can.hpp>
osSemaphoreId sem_usb_irq;
osSemaphoreId sem_uart_dma;
osSemaphoreId sem_usb_rx;
osSemaphoreId sem_usb_tx;
osMessageQId uart_event_queue;
osMessageQId usb_event_queue;
osSemaphoreId sem_can;
osThreadId usb_irq_thread;
@@ -538,18 +537,13 @@ extern "C" int main(void) {
sem_usb_irq = osSemaphoreCreate(osSemaphore(sem_usb_irq), 1);
osSemaphoreWait(sem_usb_irq, 0);
// Create a semaphore for UART DMA and remove a token
osSemaphoreDef(sem_uart_dma);
sem_uart_dma = osSemaphoreCreate(osSemaphore(sem_uart_dma), 1);
// Create an event queue for UART
osMessageQDef(uart_event_queue, 4, uint32_t);
uart_event_queue = osMessageCreate(osMessageQ(uart_event_queue), NULL);
// Create a semaphore for USB RX
osSemaphoreDef(sem_usb_rx);
sem_usb_rx = osSemaphoreCreate(osSemaphore(sem_usb_rx), 1);
osSemaphoreWait(sem_usb_rx, 0); // Remove a token.
// Create a semaphore for USB TX
osSemaphoreDef(sem_usb_tx);
sem_usb_tx = osSemaphoreCreate(osSemaphore(sem_usb_tx), 1);
// Create an event queue for USB
osMessageQDef(usb_event_queue, 7, uint32_t);
usb_event_queue = osMessageCreate(osMessageQ(usb_event_queue), NULL);
osSemaphoreDef(sem_can);
sem_can = osSemaphoreCreate(osSemaphore(sem_can), 1);
Firmware/MotorControl/odrive_main.h
+5 -2
View File
@@ -50,7 +50,7 @@ typedef struct {
} SystemStats_t;
struct PWMMapping_t {
endpoint_ref_t endpoint;
endpoint_ref_t endpoint = {0, 0};
float min = 0;
float max = 0;
};
@@ -69,7 +69,10 @@ struct BoardConfig_t {
uint32_t uart2_baudrate = 115200;
bool enable_can0 = true;
bool enable_i2c0 = false;
bool enable_ascii_protocol_on_usb = true;
ODriveIntf::StreamProtocolType uart0_protocol = ODriveIntf::STREAM_PROTOCOL_TYPE_ASCII_AND_STDOUT;
ODriveIntf::StreamProtocolType uart1_protocol = ODriveIntf::STREAM_PROTOCOL_TYPE_ASCII_AND_STDOUT;
ODriveIntf::StreamProtocolType uart2_protocol = ODriveIntf::STREAM_PROTOCOL_TYPE_ASCII_AND_STDOUT;
ODriveIntf::StreamProtocolType usb_cdc_protocol = ODriveIntf::STREAM_PROTOCOL_TYPE_ASCII_AND_STDOUT;
float max_regen_current = 0.0f;
float brake_resistance = DEFAULT_BRAKE_RESISTANCE;
float dc_bus_undervoltage_trip_level = 8.0f; //<! [V] minimum voltage below which the motor stops operating
Firmware/Tupfile.lua
+8 -22
View File
@@ -84,30 +84,15 @@ else
end
buildsuffix = boardversion
-- USB I/O settings
if tup.getconfig("USB_PROTOCOL") == "native" or tup.getconfig("USB_PROTOCOL") == "" then
FLAGS += "-DUSB_PROTOCOL_NATIVE"
elseif tup.getconfig("USB_PROTOCOL") == "native-stream" then
FLAGS += "-DUSB_PROTOCOL_NATIVE_STREAM_BASED"
elseif tup.getconfig("USB_PROTOCOL") == "stdout" then
FLAGS += "-DUSB_PROTOCOL_STDOUT"
elseif tup.getconfig("USB_PROTOCOL") == "none" then
FLAGS += "-DUSB_PROTOCOL_NONE"
else
error("unknown USB protocol")
-- --not
-- TODO: remove this setting
if tup.getconfig("USB_PROTOCOL") ~= "native" and tup.getconfig("USB_PROTOCOL") ~= "" then
error("CONFIG_USB_PROTOCOL is deprecated")
end
-- UART I/O settings
if tup.getconfig("UART_PROTOCOL") == "native" then
FLAGS += "-DUART_PROTOCOL_NATIVE"
elseif tup.getconfig("UART_PROTOCOL") == "ascii" or tup.getconfig("UART_PROTOCOL") == "" then
FLAGS += "-DUART_PROTOCOL_ASCII"
elseif tup.getconfig("UART_PROTOCOL") == "stdout" then
FLAGS += "-DUART_PROTOCOL_STDOUT"
elseif tup.getconfig("UART_PROTOCOL") == "none" then
FLAGS += "-DUART_PROTOCOL_NONE"
else
error("unknown UART protocol "..tup.getconfig("UART_PROTOCOL"))
if tup.getconfig("UART_PROTOCOL") ~= "ascii" and tup.getconfig("UART_PROTOCOL") ~= "" then
error("CONFIG_UART_PROTOCOL is deprecated")
end
-- GPIO settings
@@ -133,6 +118,7 @@ FLAGS += '-DUSE_HAL_DRIVER'
FLAGS += '-mthumb'
FLAGS += '-mfloat-abi=hard'
FLAGS += { '-Wall', '-Wdouble-promotion', '-Wfloat-conversion', '-fdata-sections', '-ffunction-sections'}
FLAGS += '-DFIBRE_ENABLE_SERVER'
-- linker flags
LDFLAGS += board.ldflags
@@ -206,7 +192,7 @@ sources = {
'communication/interface_usb.cpp',
'communication/interface_can.cpp',
'communication/interface_i2c.cpp',
'fibre/cpp/protocol.cpp',
'fibre/cpp/legacy_protocol.cpp',
'FreeRTOS-openocd.c',
'autogen/version.c'
}
Firmware/communication/ascii_protocol.cpp
+158 -128
View File
@@ -16,13 +16,14 @@
#include "autogen/type_info.hpp"
#include "communication/interface_can.hpp"
using namespace fibre;
/* Private macros ------------------------------------------------------------*/
/* Private typedef -----------------------------------------------------------*/
/* Global constant data ------------------------------------------------------*/
/* Global variables ----------------------------------------------------------*/
/* Private constant data -----------------------------------------------------*/
#define MAX_LINE_LENGTH 256
#define TO_STR_INNER(s) #s
#define TO_STR(s) TO_STR_INNER(s)
@@ -32,53 +33,45 @@ static Introspectable root_obj = ODriveTypeInfo<ODrive>::make_introspectable(odr
/* Private function prototypes -----------------------------------------------*/
void cmd_set_position(char * pStr, StreamSink& response_channel, bool use_checksum);
void cmd_set_position_wl(char * pStr, StreamSink& response_channel, bool use_checksum);
void cmd_set_velocity(char * pStr, StreamSink& response_channel, bool use_checksum);
void cmd_set_torque(char * pStr, StreamSink& response_channel, bool use_checksum);
void cmd_set_trapezoid_trajectory(char * pStr, StreamSink& response_channel, bool use_checksum);
void cmd_get_feedback(char * pStr, StreamSink& response_channel, bool use_checksum);
void cmd_help(char * pStr, StreamSink& response_channel, bool use_checksum);
void cmd_info_dump(char * pStr, StreamSink& response_channel, bool use_checksum);
void cmd_system_ctrl(char * pStr, StreamSink& response_channel, bool use_checksum);
void cmd_read_property(char * pStr, StreamSink& response_channel, bool use_checksum);
void cmd_write_property(char * pStr, StreamSink& response_channel, bool use_checksum);
void cmd_update_axis_wdg(char * pStr, StreamSink& response_channel, bool use_checksum);
void cmd_unknown(char * pStr, StreamSink& response_channel, bool use_checksum);
/* Function implementations --------------------------------------------------*/
// @brief Sends a line on the specified output.
template<typename ... TArgs>
void respond(StreamSink& output, bool include_checksum, const char * fmt, TArgs&& ... args) {
char response[64]; // Hardcoded max buffer size. We silently truncate the output if it's too long for the buffer.
size_t len = snprintf(response, sizeof(response), fmt, std::forward<TArgs>(args)...);
len = std::min(len, sizeof(response));
output.process_bytes((uint8_t*)response, len, nullptr); // TODO: use process_all instead
void AsciiProtocol::respond(bool include_checksum, const char * fmt, TArgs&& ... args) {
size_t len = snprintf(tx_buf_, sizeof(tx_buf_), fmt, std::forward<TArgs>(args)...);
// Silently truncate the output if it's too long for the buffer.
len = std::min(len, sizeof(tx_buf_));
if (include_checksum) {
uint8_t checksum = 0;
for (size_t i = 0; i < len; ++i)
checksum ^= response[i];
len = snprintf(response, sizeof(response), "*%u", checksum);
len = std::min(len, sizeof(response));
output.process_bytes((uint8_t*)response, len, nullptr);
checksum ^= tx_buf_[i];
len += snprintf(tx_buf_ + len, sizeof(tx_buf_) - len, "*%u", checksum);
} else {
len += snprintf(tx_buf_ + len, sizeof(tx_buf_) - len, "\r\n");
}
output.process_bytes((const uint8_t*)"\r\n", 2, nullptr);
// Silently truncate the output if it's too long for the buffer.
len = std::min(len, sizeof(tx_buf_));
tx_end_ = (const uint8_t*)tx_buf_ + len;
tx_channel_->start_write({(const uint8_t*)tx_buf_, tx_end_}, &tx_handle_, *static_cast<WriteCompleter*>(this));
}
// @brief Executes an ASCII protocol command
// @param buffer buffer of ASCII encoded characters
// @param len size of the buffer
void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& response_channel) {
void AsciiProtocol::process_line(cbufptr_t buffer) {
static_assert(sizeof(char) == sizeof(uint8_t));
// scan line to find beginning of checksum and prune comment
uint8_t checksum = 0;
size_t checksum_start = SIZE_MAX;
for (size_t i = 0; i < len; ++i) {
if (buffer[i] == ';') { // ';' is the comment start char
len = i;
for (size_t i = 0; i < buffer.size(); ++i) {
if (buffer.begin()[i] == ';') { // ';' is the comment start char
buffer = buffer.take(i);
break;
}
if (checksum_start > i) {
@@ -92,8 +85,8 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink&
// copy everything into a local buffer so we can insert null-termination
char cmd[MAX_LINE_LENGTH + 1];
if (len > MAX_LINE_LENGTH) len = MAX_LINE_LENGTH;
memcpy(cmd, buffer, len);
size_t len = std::min(buffer.size(), MAX_LINE_LENGTH);
memcpy(cmd, buffer.begin(), len);
cmd[len] = 0; // null-terminate
// optional checksum validation
@@ -110,19 +103,19 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink&
// check incoming packet type
switch(cmd[0]) {
case 'p': cmd_set_position(cmd, response_channel, use_checksum); break; // position control
case 'q': cmd_set_position_wl(cmd, response_channel, use_checksum); break; // position control with limits
case 'v': cmd_set_velocity(cmd, response_channel, use_checksum); break; // velocity control
case 'c': cmd_set_torque(cmd, response_channel, use_checksum); break; // current control
case 't': cmd_set_trapezoid_trajectory(cmd, response_channel, use_checksum); break; // trapezoidal trajectory
case 'f': cmd_get_feedback(cmd, response_channel, use_checksum); break; // feedback
case 'h': cmd_help(cmd, response_channel, use_checksum); break; // Help
case 'i': cmd_info_dump(cmd, response_channel, use_checksum); break; // Dump device info
case 's': cmd_system_ctrl(cmd, response_channel, use_checksum); break; // System
case 'r': cmd_read_property(cmd, response_channel, use_checksum); break; // read property
case 'w': cmd_write_property(cmd, response_channel, use_checksum); break; // write property
case 'u': cmd_update_axis_wdg(cmd, response_channel, use_checksum); break; // Update axis watchdog.
default : cmd_unknown(nullptr, response_channel, use_checksum); break;
case 'p': cmd_set_position(cmd, use_checksum); break; // position control
case 'q': cmd_set_position_wl(cmd, use_checksum); break; // position control with limits
case 'v': cmd_set_velocity(cmd, use_checksum); break; // velocity control
case 'c': cmd_set_torque(cmd, use_checksum); break; // current control
case 't': cmd_set_trapezoid_trajectory(cmd, use_checksum); break; // trapezoidal trajectory
case 'f': cmd_get_feedback(cmd, use_checksum); break; // feedback
case 'h': cmd_help(cmd, use_checksum); break; // Help
case 'i': cmd_info_dump(cmd, use_checksum); break; // Dump device info
case 's': cmd_system_ctrl(cmd, use_checksum); break; // System
case 'r': cmd_read_property(cmd, use_checksum); break; // read property
case 'w': cmd_write_property(cmd, use_checksum); break; // write property
case 'u': cmd_update_axis_wdg(cmd, use_checksum); break; // Update axis watchdog.
default : cmd_unknown(nullptr, use_checksum); break;
}
}
@@ -130,15 +123,15 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink&
// @param pStr buffer of ASCII encoded values
// @param response_channel reference to the stream to respond on
// @param use_checksum bool to indicate whether a checksum is required on response
void cmd_set_position(char * pStr, StreamSink& response_channel, bool use_checksum) {
void AsciiProtocol::cmd_set_position(char * pStr, bool use_checksum) {
unsigned motor_number;
float pos_setpoint, vel_feed_forward, torque_feed_forward;
int numscan = sscanf(pStr, "p %u %f %f %f", &motor_number, &pos_setpoint, &vel_feed_forward, &torque_feed_forward);
if (numscan < 2) {
respond(response_channel, use_checksum, "invalid command format");
respond(use_checksum, "invalid command format");
} else if (motor_number >= AXIS_COUNT) {
respond(response_channel, use_checksum, "invalid motor %u", motor_number);
respond(use_checksum, "invalid motor %u", motor_number);
} else {
Axis& axis = axes[motor_number];
axis.controller_.config_.control_mode = Controller::CONTROL_MODE_POSITION_CONTROL;
@@ -156,15 +149,15 @@ void cmd_set_position(char * pStr, StreamSink& response_channel, bool use_checks
// @param pStr buffer of ASCII encoded values
// @param response_channel reference to the stream to respond on
// @param use_checksum bool to indicate whether a checksum is required on response
void cmd_set_position_wl(char * pStr, StreamSink& response_channel, bool use_checksum) {
void AsciiProtocol::cmd_set_position_wl(char * pStr, bool use_checksum) {
unsigned motor_number;
float pos_setpoint, vel_limit, torque_lim;
int numscan = sscanf(pStr, "q %u %f %f %f", &motor_number, &pos_setpoint, &vel_limit, &torque_lim);
if (numscan < 2) {
respond(response_channel, use_checksum, "invalid command format");
respond(use_checksum, "invalid command format");
} else if (motor_number >= AXIS_COUNT) {
respond(response_channel, use_checksum, "invalid motor %u", motor_number);
respond(use_checksum, "invalid motor %u", motor_number);
} else {
Axis& axis = axes[motor_number];
axis.controller_.config_.control_mode = Controller::CONTROL_MODE_POSITION_CONTROL;
@@ -182,14 +175,14 @@ void cmd_set_position_wl(char * pStr, StreamSink& response_channel, bool use_che
// @param pStr buffer of ASCII encoded values
// @param response_channel reference to the stream to respond on
// @param use_checksum bool to indicate whether a checksum is required on response
void cmd_set_velocity(char * pStr, StreamSink& response_channel, bool use_checksum) {
void AsciiProtocol::cmd_set_velocity(char * pStr, bool use_checksum) {
unsigned motor_number;
float vel_setpoint, torque_feed_forward;
int numscan = sscanf(pStr, "v %u %f %f", &motor_number, &vel_setpoint, &torque_feed_forward);
if (numscan < 2) {
respond(response_channel, use_checksum, "invalid command format");
respond(use_checksum, "invalid command format");
} else if (motor_number >= AXIS_COUNT) {
respond(response_channel, use_checksum, "invalid motor %u", motor_number);
respond(use_checksum, "invalid motor %u", motor_number);
} else {
Axis& axis = axes[motor_number];
axis.controller_.config_.control_mode = Controller::CONTROL_MODE_VELOCITY_CONTROL;
@@ -204,14 +197,14 @@ void cmd_set_velocity(char * pStr, StreamSink& response_channel, bool use_checks
// @param pStr buffer of ASCII encoded values
// @param response_channel reference to the stream to respond on
// @param use_checksum bool to indicate whether a checksum is required on response
void cmd_set_torque(char * pStr, StreamSink& response_channel, bool use_checksum) {
void AsciiProtocol::cmd_set_torque(char * pStr, bool use_checksum) {
unsigned motor_number;
float torque_setpoint;
if (sscanf(pStr, "c %u %f", &motor_number, &torque_setpoint) < 2) {
respond(response_channel, use_checksum, "invalid command format");
respond(use_checksum, "invalid command format");
} else if (motor_number >= AXIS_COUNT) {
respond(response_channel, use_checksum, "invalid motor %u", motor_number);
respond(use_checksum, "invalid motor %u", motor_number);
} else {
Axis& axis = axes[motor_number];
axis.controller_.config_.control_mode = Controller::CONTROL_MODE_TORQUE_CONTROL;
@@ -224,14 +217,14 @@ void cmd_set_torque(char * pStr, StreamSink& response_channel, bool use_checksum
// @param pStr buffer of ASCII encoded values
// @param response_channel reference to the stream to respond on
// @param use_checksum bool to indicate whether a checksum is required on response
void cmd_set_trapezoid_trajectory(char * pStr, StreamSink& response_channel, bool use_checksum) {
void AsciiProtocol::cmd_set_trapezoid_trajectory(char * pStr, bool use_checksum) {
unsigned motor_number;
float goal_point;
if (sscanf(pStr, "t %u %f", &motor_number, &goal_point) < 2) {
respond(response_channel, use_checksum, "invalid command format");
respond(use_checksum, "invalid command format");
} else if (motor_number >= AXIS_COUNT) {
respond(response_channel, use_checksum, "invalid motor %u", motor_number);
respond(use_checksum, "invalid motor %u", motor_number);
} else {
Axis& axis = axes[motor_number];
axis.controller_.config_.input_mode = Controller::INPUT_MODE_TRAP_TRAJ;
@@ -246,16 +239,16 @@ void cmd_set_trapezoid_trajectory(char * pStr, StreamSink& response_channel, boo
// @param pStr buffer of ASCII encoded values
// @param response_channel reference to the stream to respond on
// @param use_checksum bool to indicate whether a checksum is required on response
void cmd_get_feedback(char * pStr, StreamSink& response_channel, bool use_checksum) {
void AsciiProtocol::cmd_get_feedback(char * pStr, bool use_checksum) {
unsigned motor_number;
if (sscanf(pStr, "f %u", &motor_number) < 1) {
respond(response_channel, use_checksum, "invalid command format");
respond(use_checksum, "invalid command format");
} else if (motor_number >= AXIS_COUNT) {
respond(response_channel, use_checksum, "invalid motor %u", motor_number);
respond(use_checksum, "invalid motor %u", motor_number);
} else {
Axis& axis = axes[motor_number];
respond(response_channel, use_checksum, "%f %f",
respond(use_checksum, "%f %f",
(double)axis.encoder_.pos_estimate_,
(double)axis.encoder_.vel_estimate_);
}
@@ -265,43 +258,43 @@ void cmd_get_feedback(char * pStr, StreamSink& response_channel, bool use_checks
// @param pStr buffer of ASCII encoded values
// @param response_channel reference to the stream to respond on
// @param use_checksum bool to indicate whether a checksum is required on response
void cmd_help(char * pStr, StreamSink& response_channel, bool use_checksum) {
void AsciiProtocol::cmd_help(char * pStr, bool use_checksum) {
(void)pStr;
respond(response_channel, use_checksum, "Please see documentation for more details");
respond(response_channel, use_checksum, "");
respond(response_channel, use_checksum, "Available commands syntax reference:");
respond(response_channel, use_checksum, "Position: q axis pos vel-lim I-lim");
respond(response_channel, use_checksum, "Position: p axis pos vel-ff I-ff");
respond(response_channel, use_checksum, "Velocity: v axis vel I-ff");
respond(response_channel, use_checksum, "Torque: c axis T");
respond(response_channel, use_checksum, "");
respond(response_channel, use_checksum, "Properties start at odrive root, such as axis0.requested_state");
respond(response_channel, use_checksum, "Read: r property");
respond(response_channel, use_checksum, "Write: w property value");
respond(response_channel, use_checksum, "");
respond(response_channel, use_checksum, "Save config: ss");
respond(response_channel, use_checksum, "Erase config: se");
respond(response_channel, use_checksum, "Reboot: sr");
respond(use_checksum, "Please see documentation for more details");
respond(use_checksum, "");
respond(use_checksum, "Available commands syntax reference:");
respond(use_checksum, "Position: q axis pos vel-lim I-lim");
respond(use_checksum, "Position: p axis pos vel-ff I-ff");
respond(use_checksum, "Velocity: v axis vel I-ff");
respond(use_checksum, "Torque: c axis T");
respond(use_checksum, "");
respond(use_checksum, "Properties start at odrive root, such as axis0.requested_state");
respond(use_checksum, "Read: r property");
respond(use_checksum, "Write: w property value");
respond(use_checksum, "");
respond(use_checksum, "Save config: ss");
respond(use_checksum, "Erase config: se");
respond(use_checksum, "Reboot: sr");
}
// @brief Gets the hardware, firmware and serial details
// @param pStr buffer of ASCII encoded values
// @param response_channel reference to the stream to respond on
// @param use_checksum bool to indicate whether a checksum is required on response
void cmd_info_dump(char * pStr, StreamSink& response_channel, bool use_checksum) {
// respond(response_channel, use_checksum, "Signature: %#x", STM_ID_GetSignature());
// respond(response_channel, use_checksum, "Revision: %#x", STM_ID_GetRevision());
// respond(response_channel, use_checksum, "Flash Size: %#x KiB", STM_ID_GetFlashSize());
respond(response_channel, use_checksum, "Hardware version: %d.%d-%dV", odrv.hw_version_major_, odrv.hw_version_minor_, odrv.hw_version_variant_);
respond(response_channel, use_checksum, "Firmware version: %d.%d.%d", odrv.fw_version_major_, odrv.fw_version_minor_, odrv.fw_version_revision_);
respond(response_channel, use_checksum, "Serial number: %s", serial_number_str);
void AsciiProtocol::cmd_info_dump(char * pStr, bool use_checksum) {
// respond(use_checksum, "Signature: %#x", STM_ID_GetSignature());
// respond(use_checksum, "Revision: %#x", STM_ID_GetRevision());
// respond(use_checksum, "Flash Size: %#x KiB", STM_ID_GetFlashSize());
respond(use_checksum, "Hardware version: %d.%d-%dV", odrv.hw_version_major_, odrv.hw_version_minor_, odrv.hw_version_variant_);
respond(use_checksum, "Firmware version: %d.%d.%d", odrv.fw_version_major_, odrv.fw_version_minor_, odrv.fw_version_revision_);
respond(use_checksum, "Serial number: %s", serial_number_str);
}
// @brief Executes the system control command
// @param pStr buffer of ASCII encoded values
// @param response_channel reference to the stream to respond on
// @param use_checksum bool to indicate whether a checksum is required on response
void cmd_system_ctrl(char * pStr, StreamSink& response_channel, bool use_checksum) {
void AsciiProtocol::cmd_system_ctrl(char * pStr, bool use_checksum) {
switch (pStr[1])
{
case 's': odrv.save_configuration(); break; // Save config
@@ -315,20 +308,20 @@ void cmd_system_ctrl(char * pStr, StreamSink& response_channel, bool use_checksu
// @param pStr buffer of ASCII encoded values
// @param response_channel reference to the stream to respond on
// @param use_checksum bool to indicate whether a checksum is required on response
void cmd_read_property(char * pStr, StreamSink& response_channel, bool use_checksum) {
void AsciiProtocol::cmd_read_property(char * pStr, bool use_checksum) {
char name[MAX_LINE_LENGTH];
if (sscanf(pStr, "r %255s", name) < 1) {
respond(response_channel, use_checksum, "invalid command format");
respond(use_checksum, "invalid command format");
} else {
Introspectable property = root_obj.get_child(name, sizeof(name));
const StringConvertibleTypeInfo* type_info = dynamic_cast<const StringConvertibleTypeInfo*>(property.get_type_info());
if (!type_info) {
respond(response_channel, use_checksum, "invalid property");
respond(use_checksum, "invalid property");
} else {
char response[10];
bool success = type_info->get_string(property, response, sizeof(response));
respond(response_channel, use_checksum, success ? response : "not implemented");
respond(use_checksum, success ? response : "not implemented");
}
}
}
@@ -337,21 +330,21 @@ void cmd_read_property(char * pStr, StreamSink& response_channel, bool use_check
// @param pStr buffer of ASCII encoded values
// @param response_channel reference to the stream to respond on
// @param use_checksum bool to indicate whether a checksum is required on response
void cmd_write_property(char * pStr, StreamSink& response_channel, bool use_checksum) {
void AsciiProtocol::cmd_write_property(char * pStr, bool use_checksum) {
char name[MAX_LINE_LENGTH];
char value[MAX_LINE_LENGTH];
if (sscanf(pStr, "w %255s %255s", name, value) < 1) {
respond(response_channel, use_checksum, "invalid command format");
respond(use_checksum, "invalid command format");
} else {
Introspectable property = root_obj.get_child(name, sizeof(name));
const StringConvertibleTypeInfo* type_info = dynamic_cast<const StringConvertibleTypeInfo*>(property.get_type_info());
if (!type_info) {
respond(response_channel, use_checksum, "invalid property");
respond(use_checksum, "invalid property");
} else {
bool success = type_info->set_string(property, value, sizeof(value));
if (!success) {
respond(response_channel, use_checksum, "not implemented");
respond(use_checksum, "not implemented");
}
}
}
@@ -361,13 +354,13 @@ void cmd_write_property(char * pStr, StreamSink& response_channel, bool use_chec
// @param pStr buffer of ASCII encoded values
// @param response_channel reference to the stream to respond on
// @param use_checksum bool to indicate whether a checksum is required on response
void cmd_update_axis_wdg(char * pStr, StreamSink& response_channel, bool use_checksum) {
void AsciiProtocol::cmd_update_axis_wdg(char * pStr, bool use_checksum) {
unsigned motor_number;
if (sscanf(pStr, "u %u", &motor_number) < 1) {
respond(response_channel, use_checksum, "invalid command format");
respond(use_checksum, "invalid command format");
} else if (motor_number >= AXIS_COUNT) {
respond(response_channel, use_checksum, "invalid motor %u", motor_number);
respond(use_checksum, "invalid motor %u", motor_number);
} else {
axes[motor_number].watchdog_feed();
}
@@ -377,39 +370,76 @@ void cmd_update_axis_wdg(char * pStr, StreamSink& response_channel, bool use_che
// @param pStr buffer of ASCII encoded values
// @param response_channel reference to the stream to respond on
// @param use_checksum bool to indicate whether a checksum is required on response
void cmd_unknown(char * pStr, StreamSink& response_channel, bool use_checksum) {
void AsciiProtocol::cmd_unknown(char * pStr, bool use_checksum) {
(void)pStr;
respond(response_channel, use_checksum, "unknown command");
respond(use_checksum, "unknown command");
}
// @brief Parses the received ASCII char stream
// @param buffer buffer of ASCII encoded values
// @param response_channel reference to the stream to respond on
// @param use_checksum bool to indicate whether a checksum is required on response
void ASCII_protocol_parse_stream(const uint8_t* buffer, size_t len, StreamSink& response_channel) {
static uint8_t parse_buffer[MAX_LINE_LENGTH];
static bool read_active = true;
static uint32_t parse_buffer_idx = 0;
while (len--) {
// if the line becomes too long, reset buffer and wait for the next line
if (parse_buffer_idx >= MAX_LINE_LENGTH) {
read_active = false;
parse_buffer_idx = 0;
}
// Fetch the next char
uint8_t c = *(buffer++);
bool is_end_of_line = (c == '\r' || c == '\n' || c == '!');
if (is_end_of_line) {
if (read_active)
ASCII_protocol_process_line(parse_buffer, parse_buffer_idx, response_channel);
parse_buffer_idx = 0;
read_active = true;
} else {
if (read_active) {
parse_buffer[parse_buffer_idx++] = c;
}
}
void AsciiProtocol::on_write_finished(WriteResult result) {
tx_handle_ = 0;
if (result.status == kStreamOk && result.end < tx_end_) {
// Not everything was written. Try again.
tx_channel_->start_write({result.end, tx_end_}, &tx_handle_, *static_cast<WriteCompleter*>(this));
return;
}
if (rx_end_) {
uint8_t* rx_end = rx_end_;
rx_end_ = nullptr;
on_read_finished({kStreamOk, rx_end});
}
}
void AsciiProtocol::on_read_finished(ReadResult result) {
if (result.status != kStreamOk) {
return;
}
for (;;) {
uint8_t* end_of_line = std::find_if(rx_buf_, result.end, [](uint8_t c) {
return c == '\r' || c == '\n' || c == '!';
});
if (end_of_line >= result.end) {
break;
}
if (read_active_) {
if (tx_handle_) {
// TX is busy - inhibit processing of the incoming data until
// on_write_finished() is invoked.
rx_end_ = result.end;
return;
}
process_line({rx_buf_, end_of_line});
} else {
// Ignoring this line cause it didn't start at a new-line character
read_active_ = true;
}
// Discard the processed bytes and shift the remainder to the beginning of the buffer
size_t n_remaining = result.end - end_of_line - 1;
memmove(rx_buf_, end_of_line + 1, n_remaining);
result.end = rx_buf_ + n_remaining;
}
// No more new-line characters in buffer
if (result.end >= rx_buf_ + sizeof(rx_buf_)) {
// If the line becomes too long, reset buffer and wait for the next line
result.end = rx_buf_;
read_active_ = false;
}
TransferHandle dummy;
rx_channel_->start_read({result.end, rx_buf_ + sizeof(rx_buf_)}, &dummy, *static_cast<ReadCompleter*>(this));
}
void AsciiProtocol::start() {
TransferHandle dummy;
rx_channel_->start_read(rx_buf_, &dummy, *static_cast<ReadCompleter*>(this));
}
Firmware/communication/ascii_protocol.hpp
+41 -15
View File
@@ -1,22 +1,48 @@
#ifndef __ASCII_PROTOCOL_H
#define __ASCII_PROTOCOL_H
#ifndef __ASCII_PROTOCOL_HPP
#define __ASCII_PROTOCOL_HPP
#include <fibre/../../async_stream.hpp>
/* Includes ------------------------------------------------------------------*/
#include <fibre/protocol.hpp>
#define MAX_LINE_LENGTH ((size_t)256)
#include <stdlib.h>
#include <stdint.h>
#include <stdbool.h>
class AsciiProtocol : fibre::ReadCompleter, fibre::WriteCompleter {
public:
AsciiProtocol(fibre::AsyncStreamSource* rx_channel, fibre::AsyncStreamSink* tx_channel)
: rx_channel_(rx_channel), tx_channel_(tx_channel) {}
/* Exported types ------------------------------------------------------------*/
/* Exported constants --------------------------------------------------------*/
/* Exported variables --------------------------------------------------------*/
/* Exported macro ------------------------------------------------------------*/
/* Exported functions --------------------------------------------------------*/
void start();
/* Exported functions --------------------------------------------------------*/
void ASCII_protocol_parse_stream(const uint8_t* buffer, size_t len, StreamSink& response_channel);
private:
void cmd_set_position(char * pStr, bool use_checksum);
void cmd_set_position_wl(char * pStr, bool use_checksum);
void cmd_set_velocity(char * pStr, bool use_checksum);
void cmd_set_torque(char * pStr, bool use_checksum);
void cmd_set_trapezoid_trajectory(char * pStr, bool use_checksum);
void cmd_get_feedback(char * pStr, bool use_checksum);
void cmd_help(char * pStr, bool use_checksum);
void cmd_info_dump(char * pStr, bool use_checksum);
void cmd_system_ctrl(char * pStr, bool use_checksum);
void cmd_read_property(char * pStr, bool use_checksum);
void cmd_write_property(char * pStr, bool use_checksum);
void cmd_update_axis_wdg(char * pStr, bool use_checksum);
void cmd_unknown(char * pStr, bool use_checksum);
template<typename ... TArgs> void respond(bool include_checksum, const char * fmt, TArgs&& ... args);
void process_line(fibre::cbufptr_t buffer);
void on_write_finished(fibre::WriteResult result);
void on_read_finished(fibre::ReadResult result);
#endif /* __ASCII_PROTOCOL_H */
fibre::AsyncStreamSource* rx_channel_ = nullptr;
fibre::AsyncStreamSink* tx_channel_ = nullptr;
fibre::TransferHandle tx_handle_ = 0; // non-zero while a TX operation is in progress
uint8_t* rx_end_ = nullptr; // non-zero if an RX operation has finished but wasn't handled yet because the TX channel was busy
const uint8_t* tx_end_ = nullptr;
uint8_t rx_buf_[MAX_LINE_LENGTH];
bool read_active_ = true;
char tx_buf_[64];
};
#endif // __ASCII_PROTOCOL_HPP
Firmware/communication/communication.cpp
+25 -8
View File
@@ -39,7 +39,7 @@ size_t oscilloscope_pos = 0;
/* Function implementations --------------------------------------------------*/
void init_communication(void) {
printf("hi!\r\n");
//printf("hi!\r\n");
if (odrv.config_.enable_uart0 && uart0) {
start_uart_server();
@@ -56,19 +56,36 @@ void init_communication(void) {
}
}
#include <fibre/../../async_stream.hpp>
extern "C" {
int _write(int file, const char* data, int len);
}
// @brief This is what printf calls internally
int _write(int file, const char* data, int len) {
#ifdef USB_PROTOCOL_STDOUT
usb_stream_output_ptr->process_bytes((const uint8_t *)data, len, nullptr);
#endif
#ifdef UART_PROTOCOL_STDOUT
uart_stream_output_ptr->process_bytes((const uint8_t *)data, len, nullptr);
#endif
return len;
fibre::cbufptr_t buf{(const uint8_t*)data, (const uint8_t*)data + len};
if (odrv.config_.uart0_protocol == ODrive::STREAM_PROTOCOL_TYPE_STDOUT ||
odrv.config_.uart0_protocol == ODrive::STREAM_PROTOCOL_TYPE_ASCII_AND_STDOUT) {
uart0_stdout_sink.write(buf);
if (!uart0_stdout_pending) {
uart0_stdout_pending = true;
osMessagePut(uart_event_queue, 3, 0);
}
}
if (odrv.config_.usb_cdc_protocol == ODrive::STREAM_PROTOCOL_TYPE_STDOUT ||
odrv.config_.usb_cdc_protocol == ODrive::STREAM_PROTOCOL_TYPE_ASCII_AND_STDOUT) {
usb_cdc_stdout_sink.write(buf);
if (!usb_cdc_stdout_pending) {
usb_cdc_stdout_pending = true;
osMessagePut(usb_event_queue, 7, 0);
}
}
return len; // Always pretend that we processed everything
}
Firmware/communication/interface_i2c.cpp
+6 -3
View File
@@ -9,6 +9,8 @@
#define I2C_TX_BUFFER_SIZE 128
I2CStats_t i2c_stats_;
/*
TODO: add support back
static uint8_t i2c_rx_buffer[I2C_RX_BUFFER_PREAMBLE_SIZE + I2C_RX_BUFFER_SIZE];
static uint8_t i2c_tx_buffer[I2C_TX_BUFFER_SIZE];
@@ -23,13 +25,13 @@ public:
size_t get_free_space() { return SIZE_MAX; }
} i2c1_packet_output;
BidirectionalPacketBasedChannel i2c1_channel(i2c1_packet_output);
*/
void start_i2c_server() {
// CAN H = SDA
// CAN L = SCL
HAL_I2C_EnableListen_IT(&hi2c1);
//HAL_I2C_EnableListen_IT(&hi2c1);
}
/*
void i2c_handle_packet(I2C_HandleTypeDef *hi2c) {
size_t received = sizeof(i2c_rx_buffer) - hi2c->XferCount;
if (received > I2C_RX_BUFFER_PREAMBLE_SIZE) {
@@ -84,3 +86,4 @@ void HAL_I2C_ErrorCallback(I2C_HandleTypeDef *hi2c) {
// Continue listening
HAL_I2C_EnableListen_IT(hi2c);
}
*/
Firmware/communication/interface_uart.cpp
+143 -60
View File
@@ -6,9 +6,12 @@
#include <MotorControl/utils.hpp>
#include <fibre/protocol.hpp>
#include <fibre/../../async_stream.hpp>
#include <fibre/../../legacy_protocol.hpp>
#include <usart.h>
#include <cmsis_os.h>
#include <freertos_vars.h>
#include <odrive_main.h>
#define UART_TX_BUFFER_SIZE 64
#define UART_RX_BUFFER_SIZE 64
@@ -26,78 +29,156 @@ extern UART_HandleTypeDef* uart0;
static UART_HandleTypeDef* huart_ = uart0; // defined in board.cpp.
const uint32_t stack_size_uart_thread = 4096; // Bytes
namespace fibre {
class UARTSender : public StreamSink {
class Stm32UartTxStream : public AsyncStreamSink {
public:
int process_bytes(const uint8_t* buffer, size_t length, size_t* processed_bytes) {
// Loop to ensure all bytes get sent
while (length) {
size_t chunk = length < UART_TX_BUFFER_SIZE ? length : UART_TX_BUFFER_SIZE;
// wait for USB interface to become ready
// TODO: implement ring buffer to get a more continuous stream of data
// if (osSemaphoreWait(sem_uart_dma, deadline_to_timeout(deadline_ms)) != osOK)
if (osSemaphoreWait(sem_uart_dma, PROTOCOL_SERVER_TIMEOUT_MS) != osOK)
return -1;
// transmit chunk
memcpy(tx_buf_, buffer, chunk);
if (HAL_UART_Transmit_DMA(huart_, tx_buf_, chunk) != HAL_OK)
return -1;
buffer += chunk;
length -= chunk;
if (processed_bytes)
*processed_bytes += chunk;
}
return 0;
Stm32UartTxStream(UART_HandleTypeDef* huart) : huart_(huart) {}
void start_write(cbufptr_t buffer, TransferHandle* handle, Completer<WriteResult>& completer) final;
void cancel_write(TransferHandle transfer_handle) final;
void did_finish();
UART_HandleTypeDef *huart_;
Completer<WriteResult>* completer_ = nullptr;
const uint8_t* tx_end_ = nullptr;
};
class Stm32UartRxStream : public AsyncStreamSource {
public:
void start_read(bufptr_t buffer, TransferHandle* handle, Completer<ReadResult>& completer) final;
void cancel_read(TransferHandle transfer_handle) final;
void did_receive(uint8_t* buffer, size_t length);
Completer<ReadResult>* completer_ = nullptr;
bufptr_t rx_buf_ = {nullptr, nullptr};
};
}
using namespace fibre;
void Stm32UartTxStream::start_write(cbufptr_t buffer, TransferHandle* handle, Completer<WriteResult>& completer) {
size_t chunk = std::min(buffer.size(), (size_t)UART_TX_BUFFER_SIZE);
completer_ = &completer;
tx_end_ = buffer.begin() + chunk;
if (handle) {
*handle = reinterpret_cast<TransferHandle>(this);
}
size_t get_free_space() { return SIZE_MAX; }
private:
uint8_t tx_buf_[UART_TX_BUFFER_SIZE];
} uart_stream_output;
StreamSink* uart_stream_output_ptr = &uart_stream_output;
if (HAL_UART_Transmit_DMA(huart_, const_cast<uint8_t*>(buffer.begin()), chunk) != HAL_OK) {
completer_ = nullptr;
tx_end_ = nullptr;
completer.complete({kStreamError, buffer.begin()});
}
}
StreamBasedPacketSink uart_packet_output(uart_stream_output);
BidirectionalPacketBasedChannel uart_channel(uart_packet_output);
StreamToPacketSegmenter uart_stream_input(uart_channel);
void Stm32UartTxStream::cancel_write(TransferHandle transfer_handle) {
// not implemented
}
void Stm32UartTxStream::did_finish() {
const uint8_t* tx_end = tx_end_;
tx_end_ = nullptr;
safe_complete(completer_, {kStreamOk, tx_end});
}
void Stm32UartRxStream::start_read(bufptr_t buffer, TransferHandle* handle, Completer<ReadResult>& completer) {
completer_ = &completer;
rx_buf_ = buffer;
if (handle) {
*handle = reinterpret_cast<TransferHandle>(this);
}
}
void Stm32UartRxStream::cancel_read(TransferHandle transfer_handle) {
// not implemented
}
void Stm32UartRxStream::did_receive(uint8_t* buffer, size_t length) {
// This can be called even if there was no RX operation in progress
bufptr_t rx_buf = rx_buf_;
if (completer_ && rx_buf.begin()) {
rx_buf_ = {nullptr, nullptr};
size_t chunk = std::min(length, rx_buf.size());
memcpy(rx_buf.begin(), buffer, chunk);
safe_complete(completer_, {kStreamOk, rx_buf.begin() + chunk});
}
}
Stm32UartTxStream uart_tx_stream(huart_);
Stm32UartRxStream uart_rx_stream;
LegacyProtocolStreamBased fibre_over_uart(&uart_rx_stream, &uart_tx_stream);
fibre::AsyncStreamSinkMultiplexer<2> uart_tx_multiplexer(uart_tx_stream);
fibre::BufferedStreamSink<64> uart0_stdout_sink(uart_tx_multiplexer); // Used in communication.cpp
AsciiProtocol ascii_over_uart(&uart_rx_stream, &uart_tx_multiplexer);
bool uart0_stdout_pending = false;
static void uart_server_thread(void * ctx) {
(void) ctx;
if (odrv.config_.uart0_protocol == ODrive::STREAM_PROTOCOL_TYPE_FIBRE) {
fibre_over_uart.start(Completer<LegacyProtocolPacketBased*, StreamStatus>::get_dummy());
} else if (odrv.config_.uart0_protocol == ODrive::STREAM_PROTOCOL_TYPE_ASCII
|| odrv.config_.uart0_protocol == ODrive::STREAM_PROTOCOL_TYPE_ASCII_AND_STDOUT) {
ascii_over_uart.start();
}
for (;;) {
// Check for UART errors and restart receive DMA transfer if required
if (huart_->RxState != HAL_UART_STATE_BUSY_RX) {
HAL_UART_AbortReceive(huart_);
HAL_UART_Receive_DMA(huart_, dma_rx_buffer, sizeof(dma_rx_buffer));
dma_last_rcv_idx = 0;
}
// Fetch the circular buffer "write pointer", where it would write next
uint32_t new_rcv_idx = UART_RX_BUFFER_SIZE - huart_->hdmarx->Instance->NDTR;
if (new_rcv_idx > UART_RX_BUFFER_SIZE) { // defensive programming
osEvent event = osMessageGet(uart_event_queue, osWaitForever);
if (event.status != osEventMessage) {
continue;
}
// deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS);
// Process bytes in one or two chunks (two in case there was a wrap)
if (new_rcv_idx < dma_last_rcv_idx) {
uart_stream_input.process_bytes(dma_rx_buffer + dma_last_rcv_idx,
UART_RX_BUFFER_SIZE - dma_last_rcv_idx, nullptr); // TODO: use process_all
ASCII_protocol_parse_stream(dma_rx_buffer + dma_last_rcv_idx,
UART_RX_BUFFER_SIZE - dma_last_rcv_idx, uart_stream_output);
dma_last_rcv_idx = 0;
}
if (new_rcv_idx > dma_last_rcv_idx) {
uart_stream_input.process_bytes(dma_rx_buffer + dma_last_rcv_idx,
new_rcv_idx - dma_last_rcv_idx, nullptr); // TODO: use process_all
ASCII_protocol_parse_stream(dma_rx_buffer + dma_last_rcv_idx,
new_rcv_idx - dma_last_rcv_idx, uart_stream_output);
dma_last_rcv_idx = new_rcv_idx;
}
switch (event.value.v) {
case 1: {
// This event is triggered by the control loop at 8kHz. This should be
// enough for most applications.
// At 1Mbaud/s that corresponds to at most 12.5 bytes which can arrive
// during the sleep period.
// The thread is woken up by the control loop at 8kHz. This should be
// enough for most applications.
// At 1Mbaud/s that corresponds to at most 12.5 bytes which can arrive
// during the sleep period.
osThreadSuspend(nullptr);
// Check for UART errors and restart receive DMA transfer if required
if (huart_->RxState != HAL_UART_STATE_BUSY_RX) {
HAL_UART_AbortReceive(&huart4);
HAL_UART_Receive_DMA(&huart4, dma_rx_buffer, sizeof(dma_rx_buffer));
dma_last_rcv_idx = 0;
}
// Fetch the circular buffer "write pointer", where it would write next
uint32_t new_rcv_idx = UART_RX_BUFFER_SIZE - huart_->hdmarx->Instance->NDTR;
if (new_rcv_idx > UART_RX_BUFFER_SIZE) { // defensive programming
continue;
}
// Process bytes in one or two chunks (two in case there was a wrap)
if (new_rcv_idx < dma_last_rcv_idx) {
uart_rx_stream.did_receive(dma_rx_buffer + dma_last_rcv_idx,
UART_RX_BUFFER_SIZE - dma_last_rcv_idx);
dma_last_rcv_idx = 0;
}
if (new_rcv_idx > dma_last_rcv_idx) {
uart_rx_stream.did_receive(dma_rx_buffer + dma_last_rcv_idx,
new_rcv_idx - dma_last_rcv_idx);
dma_last_rcv_idx = new_rcv_idx;
}
} break;
case 2: {
uart_tx_stream.did_finish();
} break;
case 3: { // stdout has data
uart0_stdout_pending = false;
uart0_stdout_sink.maybe_start_async_write();
} break;
}
}
}
@@ -116,10 +197,12 @@ void start_uart_server() {
void uart_poll() {
if (uart_thread) { // the thread is only started if UART is enabled
osThreadResume(uart_thread);
osMessagePut(uart_event_queue, 1, 0);
}
}
void HAL_UART_TxCpltCallback(UART_HandleTypeDef* huart) {
osSemaphoreRelease(sem_uart_dma);
if (huart == &huart4) {
osMessagePut(uart_event_queue, 2, 0);
}
}
Firmware/communication/interface_uart.h
+8 -3
View File
@@ -1,10 +1,8 @@
#ifndef __INTERFACE_UART_HPP
#define __INTERFACE_UART_HPP
#ifdef __cplusplus
#include "fibre/protocol.hpp"
extern StreamSink* uart_stream_output_ptr;
#ifdef __cplusplus
extern "C" {
#endif
@@ -20,4 +18,11 @@ void uart_poll(void);
}
#endif
#ifdef __cplusplus
#include <fibre/../../stream_utils.hpp>
extern fibre::BufferedStreamSink<64> uart0_stdout_sink;
extern bool uart0_stdout_pending;
#endif
#endif // __INTERFACE_UART_HPP
Firmware/communication/interface_usb.cpp
+179 -132
View File
@@ -5,6 +5,8 @@
#include <MotorControl/utils.hpp>
#include <fibre/protocol.hpp>
#include <fibre/../../async_stream.hpp>
#include <fibre/../../legacy_protocol.hpp>
#include <usbd_cdc.h>
#include <usbd_cdc_if.h>
#include <usb_device.h>
@@ -17,141 +19,194 @@ osThreadId usb_thread;
const uint32_t stack_size_usb_thread = 4096; // Bytes
USBStats_t usb_stats_;
class USBSender : public PacketSink {
namespace fibre {
class Stm32UsbTxStream : public AsyncStreamSink {
public:
USBSender(uint8_t endpoint_pair, const osSemaphoreId& sem_usb_tx)
: endpoint_pair_(endpoint_pair), sem_usb_tx_(sem_usb_tx) {}
Stm32UsbTxStream(uint8_t endpoint_num) : endpoint_num_(endpoint_num) {}
int process_packet(const uint8_t* buffer, size_t length) {
// cannot send partial packets
if (length > USB_TX_DATA_SIZE)
return -1;
// wait for USB interface to become ready
if (osSemaphoreWait(sem_usb_tx_, PROTOCOL_SERVER_TIMEOUT_MS) != osOK) {
// If the host resets the device it might be that the TX-complete handler is never called
// and the sem_usb_tx_ semaphore is never released. To handle this we just override the
// TX buffer if this wait times out. The implication is that the channel is no longer lossless.
// TODO: handle endpoint reset properly
usb_stats_.tx_overrun_cnt++;
}
// transmit packet
uint8_t status = CDC_Transmit_FS(
const_cast<uint8_t*>(buffer) /* casting this const away is safe because...
well... it's not actually. Stupid STM. */, length, endpoint_pair_);
if (status != USBD_OK) {
osSemaphoreRelease(sem_usb_tx_);
return -1;
}
usb_stats_.tx_cnt++;
return 0;
}
private:
uint8_t endpoint_pair_;
const osSemaphoreId& sem_usb_tx_;
void start_write(cbufptr_t buffer, TransferHandle* handle, Completer<WriteResult>& completer) final;
void cancel_write(TransferHandle transfer_handle) final;
void did_finish();
const uint8_t endpoint_num_;
bool connected_ = false;
Completer<WriteResult>* completer_ = nullptr;
const uint8_t* tx_end_ = nullptr;
};
// Note we could have independent semaphores here to allow concurrent transmission
USBSender usb_packet_output_cdc(CDC_OUT_EP, sem_usb_tx);
USBSender usb_packet_output_native(ODRIVE_OUT_EP, sem_usb_tx);
class TreatPacketSinkAsStreamSink : public StreamSink {
class Stm32UsbRxStream : public AsyncStreamSource {
public:
TreatPacketSinkAsStreamSink(PacketSink& output) : output_(output) {}
int process_bytes(const uint8_t* buffer, size_t length, size_t* processed_bytes) {
// Loop to ensure all bytes get sent
while (length) {
size_t chunk = length < USB_TX_DATA_SIZE ? length : USB_TX_DATA_SIZE;
if (output_.process_packet(buffer, chunk) != 0)
return -1;
buffer += chunk;
length -= chunk;
if (processed_bytes)
*processed_bytes += chunk;
}
return 0;
Stm32UsbRxStream(uint8_t endpoint_num) : endpoint_num_(endpoint_num) {}
void start_read(bufptr_t buffer, TransferHandle* handle, Completer<ReadResult>& completer) final;
void cancel_read(TransferHandle transfer_handle) final;
void did_finish();
const uint8_t endpoint_num_;
bool connected_ = false;
Completer<ReadResult>* completer_ = nullptr;
uint8_t* rx_end_ = nullptr;
};
}
using namespace fibre;
void Stm32UsbTxStream::start_write(cbufptr_t buffer, TransferHandle* handle, Completer<WriteResult>& completer) {
if (handle) {
*handle = reinterpret_cast<TransferHandle>(this);
}
size_t get_free_space() { return SIZE_MAX; }
private:
PacketSink& output_;
} usb_stream_output(usb_packet_output_cdc);
// This is used by the printf feature. Hence the above statics, and below seemingly random ptr (it's externed)
// TODO: less spaghetti code
StreamSink* usb_stream_output_ptr = &usb_stream_output;
if (!connected_) {
completer.complete({kStreamClosed, buffer.begin()});
return;
}
#if defined(USB_PROTOCOL_NATIVE)
BidirectionalPacketBasedChannel usb_channel(usb_packet_output_native);
#elif defined(USB_PROTOCOL_NATIVE_STREAM_BASED)
StreamBasedPacketSink usb_packetized_output(usb_stream_output);
BidirectionalPacketBasedChannel usb_channel(usb_packetized_output);
StreamToPacketSegmenter usb_native_stream_input(usb_channel);
#endif
// Note on MTU: on the physical layer, a full speed device can transmit up
// to 64 bytes of payload per bulk package. However a single logical
// transfer can consist of multiple 64 byte packets terminated by a 0 byte
// packet. Currently we don't implement this segmentation. Therefore we
// must ensure that all packets are < 64 bytes, otherwise the host will wait
// for more.
if (buffer.size() >= USB_TX_DATA_SIZE) {
completer.complete({kStreamError, buffer.begin()});
return;
}
struct USBInterface {
uint8_t* rx_buf = nullptr;
uint32_t rx_len = 0;
bool data_pending = false;
uint8_t out_ep;
uint8_t in_ep;
USBSender& usb_sender;
};
if (completer_ || tx_end_) {
completer.complete({kStreamError, buffer.begin()});
return;
}
// Note: statics make this less modular.
// Note: we use a single rx semaphore and loop over data_pending to allow a single pump loop thread
static USBInterface CDC_interface = {
.rx_buf = nullptr,
.rx_len = 0,
.data_pending = false,
.out_ep = CDC_OUT_EP,
.in_ep = CDC_IN_EP,
.usb_sender = usb_packet_output_cdc,
};
static USBInterface ODrive_interface = {
.rx_buf = nullptr,
.rx_len = 0,
.data_pending = false,
.out_ep = ODRIVE_OUT_EP,
.in_ep = ODRIVE_IN_EP,
.usb_sender = usb_packet_output_native,
};
completer_ = &completer;
tx_end_ = buffer.end();
if (CDC_Transmit_FS(const_cast<uint8_t*>(buffer.begin()), buffer.size(), endpoint_num_) != USBD_OK) {
tx_end_ = nullptr;
safe_complete(completer_, {kStreamError, buffer.begin()});
}
}
void Stm32UsbTxStream::cancel_write(TransferHandle transfer_handle) {
// not implemented
}
void Stm32UsbTxStream::did_finish() {
const uint8_t* tx_end = tx_end_;
tx_end_ = nullptr;
safe_complete(completer_, {connected_ ? kStreamOk : kStreamClosed, tx_end});
}
void Stm32UsbRxStream::start_read(bufptr_t buffer, TransferHandle* handle, Completer<ReadResult>& completer) {
if (handle) {
*handle = reinterpret_cast<TransferHandle>(this);
}
if (!connected_) {
completer.complete({kStreamClosed, buffer.begin()});
return;
}
if (completer_ || rx_end_) {
completer.complete({kStreamError, buffer.begin()});
return;
}
completer_ = &completer;
rx_end_ = buffer.begin(); // the pointer is updated at the end of the transfer
if (USBD_CDC_ReceivePacket(&usb_dev_handle, buffer.begin(), buffer.size(), endpoint_num_) != USBD_OK) {
rx_end_ = nullptr;
safe_complete(completer_, {kStreamError, buffer.begin()});
return;
}
}
void Stm32UsbRxStream::cancel_read(TransferHandle transfer_handle) {
// not implemented
}
void Stm32UsbRxStream::did_finish() {
uint8_t* rx_end = rx_end_;
rx_end_ = nullptr;
safe_complete(completer_, {connected_ ? kStreamOk : kStreamClosed, rx_end});
}
Stm32UsbTxStream usb_cdc_tx_stream(CDC_IN_EP);
Stm32UsbTxStream usb_native_tx_stream(ODRIVE_IN_EP);
Stm32UsbRxStream usb_cdc_rx_stream(CDC_OUT_EP);
Stm32UsbRxStream usb_native_rx_stream(ODRIVE_OUT_EP);
LegacyProtocolStreamBased fibre_over_cdc(&usb_cdc_rx_stream, &usb_cdc_tx_stream);
LegacyProtocolPacketBased fibre_over_usb(&usb_native_rx_stream, &usb_native_tx_stream, USB_TX_DATA_SIZE - 1); // See note on MTU above
fibre::AsyncStreamSinkMultiplexer<2> usb_cdc_tx_multiplexer(usb_cdc_tx_stream);
fibre::BufferedStreamSink<64> usb_cdc_stdout_sink(usb_cdc_tx_multiplexer); // Used in communication.cpp
AsciiProtocol ascii_over_cdc(&usb_cdc_rx_stream, &usb_cdc_tx_multiplexer);
bool usb_cdc_stdout_pending = false;
static void usb_server_thread(void * ctx) {
(void) ctx;
for (;;) {
// const uint32_t usb_check_timeout = 1; // ms
osStatus sem_stat = osSemaphoreWait(sem_usb_rx, osWaitForever);
if (sem_stat == osOK) {
usb_stats_.rx_cnt++;
osEvent event = osMessageGet(usb_event_queue, osWaitForever);
// CDC Interface
if (CDC_interface.data_pending) {
CDC_interface.data_pending = false;
if (odrv.config_.enable_ascii_protocol_on_usb) {
ASCII_protocol_parse_stream(CDC_interface.rx_buf,
CDC_interface.rx_len, usb_stream_output);
} else {
#if defined(USB_PROTOCOL_NATIVE)
usb_channel.process_packet(CDC_interface.rx_buf, CDC_interface.rx_len);
#elif defined(USB_PROTOCOL_NATIVE_STREAM_BASED)
usb_native_stream_input.process_bytes(
CDC_interface.rx_buf, CDC_interface.rx_len, nullptr);
#endif
if (event.status != osEventMessage) {
continue;
}
usb_stats_.rx_cnt++;
switch (event.value.v) {
case 1: { // USB connected event
usb_cdc_tx_stream.connected_ = true;
usb_native_tx_stream.connected_ = true;
usb_cdc_rx_stream.connected_ = true;
usb_native_rx_stream.connected_ = true;
fibre_over_usb.start(Completer<LegacyProtocolPacketBased*, StreamStatus>::get_dummy());
if (odrv.config_.usb_cdc_protocol == ODrive::STREAM_PROTOCOL_TYPE_FIBRE) {
fibre_over_cdc.start(Completer<LegacyProtocolPacketBased*, StreamStatus>::get_dummy());
} else if (odrv.config_.usb_cdc_protocol == ODrive::STREAM_PROTOCOL_TYPE_ASCII
|| odrv.config_.usb_cdc_protocol == ODrive::STREAM_PROTOCOL_TYPE_ASCII_AND_STDOUT) {
ascii_over_cdc.start();
}
USBD_CDC_ReceivePacket(&usb_dev_handle, CDC_interface.out_ep); // Allow next packet
}
} break;
// Native Interface
if (ODrive_interface.data_pending) {
ODrive_interface.data_pending = false;
#if defined(USB_PROTOCOL_NATIVE)
usb_channel.process_packet(ODrive_interface.rx_buf, ODrive_interface.rx_len);
#elif defined(USB_PROTOCOL_NATIVE_STREAM_BASED)
usb_native_stream_input.process_bytes(
ODrive_interface.rx_buf, ODrive_interface.rx_len, nullptr);
#endif
USBD_CDC_ReceivePacket(&usb_dev_handle, ODrive_interface.out_ep); // Allow next packet
}
case 2: { // USB disconnected event
usb_cdc_tx_stream.connected_ = false;
usb_native_tx_stream.connected_ = false;
usb_cdc_rx_stream.connected_ = false;
usb_native_rx_stream.connected_ = false;
usb_cdc_tx_stream.did_finish();
usb_native_tx_stream.did_finish();
usb_cdc_rx_stream.did_finish();
usb_native_rx_stream.did_finish();
} break;
case 3: { // TX on CDC interface done
usb_cdc_tx_stream.did_finish();
} break;
case 4: { // TX on custom interface done
usb_native_tx_stream.did_finish();
} break;
case 5: { // RX on CDC interface done
usb_cdc_rx_stream.did_finish();
} break;
case 6: { // RX on custom interface done
usb_native_rx_stream.did_finish();
} break;
case 7: { // stdout has data
usb_cdc_stdout_pending = false;
usb_cdc_stdout_sink.maybe_start_async_write();
} break;
}
}
}
@@ -159,21 +214,13 @@ static void usb_server_thread(void * ctx) {
// Called from CDC_Receive_FS callback function, this allows the communication
// thread to handle the incoming data
void usb_rx_process_packet(uint8_t *buf, uint32_t len, uint8_t endpoint_pair) {
USBInterface* usb_iface;
if (endpoint_pair == CDC_interface.out_ep) {
usb_iface = &CDC_interface;
} else if (endpoint_pair == ODrive_interface.out_ep) {
usb_iface = &ODrive_interface;
} else {
return;
if (endpoint_pair == CDC_OUT_EP && usb_cdc_rx_stream.rx_end_) {
usb_cdc_rx_stream.rx_end_ += len;
osMessagePut(usb_event_queue, 5, 0);
} else if (endpoint_pair == ODRIVE_OUT_EP && usb_native_rx_stream.rx_end_) {
usb_native_rx_stream.rx_end_ += len;
osMessagePut(usb_event_queue, 6, 0);
}
// We don't allow the next USB packet until the previous one has been processed completely.
// Therefore it's safe to write to these vars directly since we know previous processing is complete.
usb_iface->rx_buf = buf;
usb_iface->rx_len = len;
usb_iface->data_pending = true;
osSemaphoreRelease(sem_usb_rx);
}
void start_usb_server() {
Firmware/communication/interface_usb.h
+8 -3
View File
@@ -1,10 +1,8 @@
#ifndef __INTERFACE_USB_HPP
#define __INTERFACE_USB_HPP
#ifdef __cplusplus
#include "fibre/protocol.hpp"
extern StreamSink* usb_stream_output_ptr;
#ifdef __cplusplus
extern "C" {
#endif
@@ -29,4 +27,11 @@ void start_usb_server(void);
}
#endif
#ifdef __cplusplus
#include <fibre/../../stream_utils.hpp>
extern fibre::BufferedStreamSink<64> usb_cdc_stdout_sink;
extern bool usb_cdc_stdout_pending;
#endif
#endif // __INTERFACE_USB_HPP
Firmware/fibre/.gitignore
+1
View File
@@ -1,4 +1,5 @@
__pycache__
.Trash*
/cpp/third_party
/build
/.tup
Firmware/fibre/cpp/.gitattributes
+3
View File
@@ -0,0 +1,3 @@
libfibre*.so filter=lfs diff=lfs merge=lfs -text
libfibre*.dll filter=lfs diff=lfs merge=lfs -text
libfibre*.dylib filter=lfs diff=lfs merge=lfs -text
Firmware/fibre/cpp/Makefile
+55
View File
@@ -0,0 +1,55 @@
# pkgconf --cflags libusb
# pkgconf --libs libusb
# Prerequisites:
# Ubuntu: libusb-dev
ifeq ($(OS),Windows_NT)
target = windows
ifeq ($(PROCESSOR_ARCHITEW6432),AMD64)
outname = libfibre-windows-amd64.dll
else
$(error unsupported platform)
endif
else ifeq ($(shell uname -s),Linux)
target = linux
ifeq ($(shell uname -m),x86_64)
outname = libfibre-linux-amd64.so
else ifeq (($(shell uname -m),armv7l)
outname = libfibre-linux-armhf.so
else
$(error unsupported platform)
endif
else ifeq ($(shell uname -s),Darwin)
target = macos
ifeq ($(shell uname -m),x86_64)
outname = libfibre-macos-multiarch.dylib
else
$(error unsupported platform)
endif
endif
FILES=libfibre.cpp \
platform_support/libusb_transport.cpp \
legacy_protocol.cpp \
legacy_object_client.cpp \
logging.cpp
linux:
g++ -shared -o $(outname) -fPIC -std=c++11 -I/usr/include/libusb-1.0 -I./include -DFIBRE_COMPILE -DFIBRE_ENABLE_CLIENT \
$(FILES) \
-lusb-1.0
linux-cross:
arm-linux-gnueabihf-g++ -march=armv7 -shared -o libfibre.so -fPIC -std=c++11 -I/usr/include/libusb-1.0 -I./include -DFIBRE_COMPILE -DFIBRE_ENABLE_CLIENT \
$(FILES) \
-lusb
windows:
g++ -shared -o $(outname) -fPIC -std=c++11 -I./third_party/libusb-windows/libusb-1.0.23/include/libusb-1.0 -I./include -DFIBRE_COMPILE -DFIBRE_ENABLE_CLIENT \
$(FILES) \
-static-libgcc -Wl,-Bstatic -lstdc++ ./third_party/libusb-windows/libusb-1.0.23/MinGW64/static/libusb-1.0.a -Wl,-Bdynamic
# nm -D libfibre.so | grep T
Firmware/fibre/cpp/README.md
+38
View File
@@ -0,0 +1,38 @@
## Platform Compatibility
| | Windows | macOS [1] | Linux |
|-----|---------|-----------|-------|
| USB | yes | yes | yes |
- [1] macOS 10.9 (Mavericks) or later
## `libfibre` API
Refer to [libfibre.h](include/fibre/libfibre.h) for documentation of the API.
## Build instructions
### Windows
1. Download MinGW from [here](https://sourceforge.net/projects/mingw-w64/files/Toolchains%20targetting%20Win32/Personal%20Builds/mingw-builds/installer/mingw-w64-install.exe/download) and install it.
2. Add `C:\Program Files\mingw-w64\x86_64-8.1.0-posix-seh-rt_v6-rev0\mingw64\bin` (or similar) to your `PATH` environment variable.
3. Download the libusb binaries from [here](`https://github.com/libusb/libusb/releases/download/v1.0.23/libusb-1.0.23.7z`) and unpack them to `third_party/libusb-windows` (such that the file `third_party/libusb-windows/libusb-1.0.23/MinGW64/static/libusb-1.0.a` exists).
4. Navigate to this directory and run `make`
### Ubuntu
1. `sudo apt-get libusb-1.0-dev`
2. Navigate to this directory and run `make`
### macOS
1. `brew install libusb`
2. Navigate to this directory and run `make`
### Cross-compile `libfibre` on Linux for all other platforms
The file `./compile_for_all_platforms.sh` cross-compiles libfibre for all supported platforms. This is mainly intended for CI to generate releases. It written to run on Arch Linux only. Check the script to see which packages need to be installed first.
## Notes for Contributors
- Fibre currently targets C++11 to maximize compatibility with other projects
- Notes on platform independent programming:
- Don't use the keyword `interface` (defined as a macro on Windows in `rpc.h`)
Firmware/fibre/cpp/async_stream.hpp
+188
View File
@@ -0,0 +1,188 @@
#ifndef __FIBRE_ASYNC_STREAM_HPP
#define __FIBRE_ASYNC_STREAM_HPP
#include "include/fibre/bufptr.hpp" // TODO: move this header
#include <stdint.h>
namespace fibre {
enum StreamStatus {
kStreamOk,
kStreamCancelled,
kStreamClosed,
kStreamError
};
template<typename ... TResults>
class Completer {
public:
virtual void complete(TResults ... result) = 0;
static Completer& get_dummy() {
static struct DummyCompleter : Completer {
void complete(TResults ... result) {}
} dummy;
return dummy;
}
};
/**
* @brief Safe wrapper around Completer::complete.
*
* This function takes a reference to a completer pointer and only invokes the
* completer if it's not null. Before invoking the completer, the pointer is
* cleared.
*/
template<typename ... TResults>
static void safe_complete(Completer<TResults...>*& completer, TResults ... results) {
Completer<TResults...>* tmp = completer;
completer = nullptr;
if (tmp) {
tmp->complete(results...);
}
}
struct ReadResult {
StreamStatus status;
/**
* @brief The pointer to one position after the last byte that was
* transferred.
* This must always be in [buffer.begin(), buffer.end()], even if the
* transfer was not succesful.
* If the status is kStreamError or kStreamCancelled then the accuracy
* of this field is not guaranteed.
*/
unsigned char* end;
};
struct WriteResult {
StreamStatus status;
/**
* @brief The pointer to one position after the last byte that was
* transferred.
* This must always be in [buffer.begin(), buffer.end()], even if the
* transfer was not succesful.
* If the status is kStreamError or kStreamCancelled then the accuracy
* of this field is not guaranteed.
*/
const unsigned char* end;
};
struct WriteCompleter : Completer<WriteResult> {
virtual void on_write_finished(WriteResult result) = 0;
void complete(WriteResult result) final {
on_write_finished(result);
}
};
struct ReadCompleter : Completer<ReadResult> {
virtual void on_read_finished(ReadResult result) = 0;
void complete(ReadResult result) final {
on_read_finished(result);
}
};
using TransferHandle = uintptr_t;
/**
* @brief Base class for asynchronous stream sources.
*/
class AsyncStreamSource {
public:
/**
* @brief Starts a read operation. Once the read operation completes,
* on_finished.complete() is called.
*
* Most implementations only allow one transfer to be active at a time.
*
* TODO: specify if `completer` can be called directly within this function.
*
* @param buffer: The buffer where the data to be written shall be fetched from.
* Must remain valid until `completer` is satisfied.
* @param handle: The variable pointed to by this argument is set to an
* opaque transfer handle that can be passed to cancel_read() as
* long as the operation has not yet completed.
* If the completer is invoked directly from start_read() then the
* handle is not modified after this invokation. That means it's safe
* for the completion handler to reuse the handle variable.
* @param completer: The completer that will be completed once the operation
* finishes, whether successful or not.
* Must remain valid until it is satisfied.
*/
virtual void start_read(bufptr_t buffer, TransferHandle* handle, Completer<ReadResult>& completer) = 0;
/**
* @brief Cancels an operation that was previously started with start_read().
*
* The transfer is cancelled asynchronously and the associated completer
* will eventually be completed with kStreamCancelled. Until then the
* transfer must be considered still in progress and associated resources
* must not be freed.
*
* TODO: specify if an implementation is allowed to return something other
* than kStreamCancelled when the transfer was cancelled.
*
* This function must not be called once the stream has started to invoke
* the associated completion handler. It must also not be called twice for
* the same transfer.
*/
virtual void cancel_read(TransferHandle transfer_handle) = 0;
};
/**
* @brief Base class for asynchronous stream sources.
*
* Thread-safety: Implementations are generally not required to provide thread
* safety. Users should only call the functions of this class on the same thread
* as the event loop on which the stream runs.
*/
class AsyncStreamSink {
public:
/**
* @brief Starts a write operation. Once the write operation completes,
* on_finished.complete() is called.
*
* Most implementations only allow one transfer to be active at a time.
*
* TODO: specify if `completer` can be called directly within this function.
*
* @param buffer: The buffer where the data to be written shall be fetched from.
* Must remain valid until `completer` is satisfied.
* @param handle: The variable pointed to by this argument is set to an
* opaque transfer handle that can be passed to cancel_write() as
* long as the operation has not yet completed.
* If the completer is invoked directly from start_write() then the
* handle is not modified after this invokation. That means it's safe
* for the completion handler to reuse the handle variable.
* @param completer: The completer that will be completed once the operation
* finishes, whether successful or not.
* Must remain valid until it is satisfied.
*/
virtual void start_write(cbufptr_t buffer, TransferHandle* handle, Completer<WriteResult>& completer) = 0;
/**
* @brief Cancels an operation that was previously started with start_write().
*
* The transfer is cancelled asynchronously and the associated completer
* will eventually be completed with kStreamCancelled. Until then the
* transfer must be considered still in progress and associated resources
* must not be freed.
*
* TODO: specify if an implementation is allowed to return something other
* than kStreamCancelled when the transfer was cancelled.
*
* This function must not be called once the stream has started to invoke
* the associated completion handler. It must also not be called twice for
* the same transfer.
*/
virtual void cancel_write(TransferHandle transfer_handle) = 0;
};
}
#endif // __FIBRE_ASYNC_STREAM_HPP
Firmware/fibre/cpp/compile_for_all_platforms.sh
Executable
+178
View File
@@ -0,0 +1,178 @@
#!/bin/bash
set -euo pipefail
# Prerequisites:
# Arch Linux:
# gcc binutils
# arm-linux-gnueabihf-gcc arm-linux-gnueabihf-binutils
# mingw-w64-gcc mingw-w64-binutils
# p7zip
# apple-darwin-osxcross
# TODO: support C++11
mkdir -p third_party
function download_deb_pkg() {
dir="$1"
url="$2"
file="$(sed 's|^.*/\([^/]*\)$|\1|' <<< "$url")"
pushd third_party > /dev/null
if ! [ -f "${file}" ]; then
wget "${url}"
fi
if ! [ -d "${dir}/usr" ]; then
ar x "${file}" "data.tar.xz"
mkdir -p "${dir}"
tar -xvf "data.tar.xz" -C "${dir}"
fi
popd > /dev/null
}
function compile_libusb() {
arch_name="$1"
arch="$2"
libusb_version=1.0.23
pushd third_party > /dev/null
if ! [ -f "libusb-${libusb_version}.tar.bz2" ]; then
wget "https://github.com/libusb/libusb/releases/download/v${libusb_version}/libusb-${libusb_version}.tar.bz2"
fi
if ! [ -d "libusb-${libusb_version}" ]; then
tar -xvf "libusb-${libusb_version}.tar.bz2"
fi
mkdir -p "libusb-${libusb_version}/build-${arch_name}"
pushd "libusb-${libusb_version}/build-${arch_name}" > /dev/null
unset LDFLAGS
if ! [ -f "libusb/.libs/libusb-1.0.a" ]; then
../configure --host="$arch" \
--enable-static \
--prefix=/opt/osxcross/ \
--disable-dependency-tracking
# They broke parallel building in libusb 1.20
make
fi
popd > /dev/null
popd > /dev/null
}
download_deb_pkg libusb-dev-amd64 "http://mirrors.kernel.org/ubuntu/pool/main/libu/libusb-1.0/libusb-1.0-0-dev_1.0.23-2build1_amd64.deb"
download_deb_pkg libusb-amd64 "http://mirrors.kernel.org/ubuntu/pool/main/libu/libusb-1.0/libusb-1.0-0_1.0.23-2build1_amd64.deb"
download_deb_pkg libusb-i386 "http://mirrors.kernel.org/ubuntu/pool/main/libu/libusb-1.0/libusb-1.0-0_1.0.23-2build1_i386.deb"
download_deb_pkg libusb-dev-i386 "http://mirrors.kernel.org/ubuntu/pool/main/libu/libusb-1.0/libusb-1.0-0-dev_1.0.23-2build1_i386.deb"
download_deb_pkg libusb-armhf "http://mirrordirector.raspbian.org/raspbian/pool/main/libu/libusb-1.0/libusb-1.0-0_1.0.23-2_armhf.deb"
download_deb_pkg libusb-dev-armhf "http://mirrordirector.raspbian.org/raspbian/pool/main/libu/libusb-1.0/libusb-1.0-0-dev_1.0.23-2_armhf.deb"
download_deb_pkg libstdc++-linux-armhf "http://mirrors.kernel.org/ubuntu/pool/universe/g/gcc-10-cross/libstdc++-10-dev-armhf-cross_10-20200411-0ubuntu1cross1_all.deb"
#compile_libusb 'x86_64-apple-darwin' # fails with "sys/sysctl.h: No such file or directory"
_architectures=(
#'arm-linux-gnueabihf'
#'x86_64-apple-darwin'
#'x86_64-pc-linux-gnu'
#'i686-w64-mingw32'
#'x86_64-w64-mingw32'
)
FILES=('libfibre.cpp'
'platform_support/libusb_transport.cpp'
'legacy_protocol.cpp'
'legacy_object_client.cpp'
'logging.cpp')
### Raspberry Pi
#arm-linux-gnueabihf-g++ -shared -o libfibre-linux-armhf.so -fPIC -std=c++11 -I./include -DFIBRE_COMPILE -DFIBRE_ENABLE_CLIENT \
# -I./third_party/libusb-dev-armhf/usr/include/libusb-1.0 \
# "${FILES[@]}" \
# ./third_party/libusb-armhf/lib/arm-linux-gnueabihf/libusb-1.0.so.0.2.0 \
# -lpthread \
# -L./third_party/libstdc++-linux-armhf/usr/lib/gcc-cross/arm-linux-gnueabihf/10 \
# -Wl,--unresolved-symbols=ignore-in-shared-libs -static-libstdc++
### Windows
mkdir -p "third_party/libusb-windows"
pushd "third_party/libusb-windows" > /dev/null
if [ ! -f libusb-1.0.23.7z ]; then
wget "https://github.com/libusb/libusb/releases/download/v1.0.23/libusb-1.0.23.7z"
fi
if [ ! -f "libusb-1.0.23/libusb-1.0.def" ]; then
7z x -o"libusb-1.0.23" "libusb-1.0.23.7z"
fi
popd > /dev/null
#x86_64-w64-mingw32-g++ -shared -o libfibre-windows-amd64.dll -fPIC -std=c++11 -I./include -DFIBRE_COMPILE -DFIBRE_ENABLE_CLIENT \
# -I./third_party/libusb-windows/libusb-1.0.23/include/libusb-1.0 \
# "${FILES[@]}" \
# -static-libgcc \
# -Wl,-Bstatic \
# -lstdc++ \
# ./third_party/libusb-windows/libusb-1.0.23/MinGW64/static/libusb-1.0.a \
# -Wl,-Bdynamic
oldprefix="Users/phracker/Documents/Xcode-beta.app/Contents/Developer/Platforms/MacOSX.platform/Developer/SDKs/MacOSX.sdk"
newprefix="/opt/osxcross/SDK/MacOSX10.13.sdk"
while IFS= read -r link; do
destination="$(readlink "$link")"
pruned_destination="${destination#"$oldprefix"}"
if [ "${oldprefix}${pruned_destination}" == "${destination}" ]; then
sudo mv -T "${newprefix}${pruned_destination}" "$link"
fi
done <<< "$(find /opt/osxcross/SDK/MacOSX10.13.sdk/System/Library/Frameworks/IOKit.framework -xtype l)"
# Link are broken:
# …ions/Current/Headers $ ls -l IOReturn.h
# lrwxrwxrwx 1 root root 189 Dec 26 2019 IOReturn.h -> Users/phracker/Documents/Xcode-beta.app/Contents/Developer/Platforms/MacOSX.platform/Developer/SDKs/MacOSX.sdk/System/Library/Frameworks/Kernel.framework/Versions/A/Headers/IOKit/IOReturn.h
# Fix with:
# sudo ln -sf /opt/osxcross/SDK/MacOSX10.13.sdk/System/Library/Frameworks/Kernel.framework/Versions/A/Headers/IOKit/IOReturn.h IOReturn.h
export PATH="/opt/osxcross/bin:$PATH"
export LD_LIBRARY_PATH="/opt/osxcross/lib"
export CFLAGS='-I/opt/osxcross/SDK/MacOSX10.13.sdk/usr/include -arch i386 -arch x86_64'
export MACOSX_DEPLOYMENT_TARGET='10.9'
CC='o64-clang' \
compile_libusb 'macos-amd64' 'x86_64-apple-darwin17'
#mkdir -p "third_party/libusb-src"
#pushd "third_party/libusb-src" > /dev/null
#if [ ! -f v1.0.23.tar.gz ]; then
# wget "https://github.com/libusb/libusb/archive/v1.0.23.tar.gz"
#fi
#if [ ! -f "libusb-1.0.23/README.md" ]; then
# tar -xvf "v1.0.23.tar.gz"
#fi
#export CC=o64-clang++
#mkdir -p "third_party/libusb-src/build-macos-amd64"
#../configure
#pushd "third_party/libusb-src" > /dev/null
#./configure
#popd > /dev/null
o64-clang++ -shared -o libfibre-macos-x86.dylib -fPIC -std=c++11 -I./include -DFIBRE_COMPILE -DFIBRE_ENABLE_CLIENT \
-I./third_party/libusb-windows/libusb-1.0.23/include/libusb-1.0 \
-arch x86_64 -arch i386 \
"${FILES[@]}" \
-static-libstdc++ \
./third_party/libusb-1.0.23/build-macos-amd64/libusb/.libs/libusb-1.0.a \
-framework CoreFoundation -framework IOKit
#"${FILES[@]}" \
#-arch i386 \
#-Wl,-Bstatic \
#./third_party/libusb-1.0.23/build-macos-amd64/libusb/libusb-1.0.la \
#-Wl,-Bdynamic
# \
#-Wl,-Bstatic \
#-lgcc \
#-lstdc++ \
#./third_party/libusb-windows/libusb-1.0.23/MinGW64/static/libusb-1.0.a \
#-Wl,-Bdynamic
Firmware/fibre/cpp/endpoints_template.j2
+1
View File
@@ -14,6 +14,7 @@
#define __FIBRE_ENDPOINTS_HPP
#include <fibre/introspection.hpp>
#include <fibre/../../legacy_protocol.hpp>
// Note: with -Og the functions with large switch statements reserves a huge amount
// of stack space because they reserves separate space for the stack frame of each
Firmware/fibre/cpp/event_loop.hpp
+45
View File
@@ -0,0 +1,45 @@
#ifndef __FIBRE_EVENT_LOOP_HPP
#define __FIBRE_EVENT_LOOP_HPP
#include <stdint.h>
struct EventLoopTimer;
/**
* @brief Base class for event loops.
*
* Thread-safety: The functions of this class must not be assumed to be thread-safe.
* Generally the functions of an event loop are only safe to be called from the
* event loop's thread itself.
*/
class EventLoop {
public:
/**
* @brief Registers a callback for immediate execution on the event loop thread.
*/
virtual int post(void (*callback)(void*), void *ctx) = 0;
virtual int register_event(int event_fd, uint32_t events, void (*callback)(void*), void* ctx) = 0;
virtual int deregister_event(int event_fd) = 0;
/**
* @brief Registers a callback to be called at a later point in time.
*
* This returns an opaque handler which can be used to cancel the timer.
*
* @param delay: The delay from now in seconds.
* TOOD: specify if OS sleep time is counted in.
*/
virtual struct EventLoopTimer* call_later(float delay, void (*callback)(void*), void *ctx) = 0;
/**
* @brief Cancels a timer which was previously started by call_later().
*
* Must not be called after invokation of the callback has started.
* This also means that cancel_timer() must not be called from within the
* callback of the timer itself.
*/
virtual int cancel_timer(EventLoopTimer* timer) = 0;
};
#endif // __FIBRE_EVENT_LOOP_HPP
Firmware/fibre/cpp/include/fibre/bufptr.hpp
+9 -2
View File
@@ -1,6 +1,9 @@
#ifndef __FIBRE_BUFPTR_HPP
#define __FIBRE_BUFPTR_HPP
#include <stdlib.h>
#include <vector>
namespace fibre {
static inline bool soft_assert(bool expr) { return expr; } // TODO: implement
@@ -24,10 +27,13 @@ struct generic_bufptr_t {
template<size_t I>
generic_bufptr_t(T (&begin)[I]) : generic_bufptr_t(begin, I) {}
generic_bufptr_t(const std::vector<std::remove_const_t<T>>& vector)
generic_bufptr_t(std::vector<typename std::remove_const<T>::type>& vector)
: generic_bufptr_t(vector.data(), vector.size()) {}
generic_bufptr_t(const generic_bufptr_t<std::remove_const_t<T>>& other)
generic_bufptr_t(const std::vector<typename std::remove_const<T>::type>& vector)
: generic_bufptr_t(vector.data(), vector.size()) {}
generic_bufptr_t(const generic_bufptr_t<typename std::remove_const<T>::type>& other)
: generic_bufptr_t(other.begin_, other.end_) {}
generic_bufptr_t& operator+=(size_t num) {
@@ -81,6 +87,7 @@ struct generic_bufptr_t {
T& back() const { return *(end() - 1); }
T& operator[](size_t idx) { return *(begin() + idx); }
private:
T* begin_;
T* end_;
};
Firmware/fibre/cpp/include/fibre/cpp_utils.hpp
+90 -8
View File
@@ -82,7 +82,7 @@ public:
#include <tuple>
#include <functional>
#include <unordered_map>
#include <ostream>
//#include <ostream>
/* Backport features from C++14 and C++17 ------------------------------------*/
@@ -439,6 +439,88 @@ T& get(std::variant<Ts...>& val) {
return std::get<index>(val);
}
/// Tag type to disengage optional objects.
struct nullopt_t {
// Do not user-declare default constructor at all for
// optional_value = {} syntax to work.
// nullopt_t() = delete;
// Used for constructing nullopt.
enum class _Construct { _Token };
// Must be constexpr for nullopt_t to be literal.
explicit constexpr nullopt_t(_Construct) { }
};
constexpr nullopt_t nullopt { nullopt_t::_Construct::_Token };
template<typename T>
class optional {
public:
using storage_t = char[sizeof(T)];
optional() : has_value_(false) {}
optional(nullopt_t val) : has_value_(false) {}
optional(const optional & other) : has_value_(other.has_value_) {
if (has_value_)
new ((T*)content_) T{*(T*)other.content_};
}
optional(optional&& other) : has_value_(other.has_value_) {
if (has_value_)
new ((T*)content_) T{*(T*)other.content_};
}
optional(T& arg) {
new ((T*)content_) T{arg};
has_value_ = true;
}
optional(T&& arg) {
new ((T*)content_) T{std::forward<T>(arg)};
has_value_ = true;
}
~optional() {
if (has_value_)
((T*)content_)->~T();
}
inline optional& operator=(const optional & other) {
~*this();
new (this) optional{other};
return *this;
}
inline bool operator==(const optional& rhs) const {
return (!has_value_ && !rhs.has_value_) || (*(T*)content_ == *(T*)rhs.content_);
}
inline bool operator!=(const optional& rhs) const {
return !(*this == rhs);
}
inline T& operator*() {
return *(T*)content_;
}
inline T& operator->() {
return *(T*)content_;
}
storage_t content_;
size_t has_value_;
size_t has_value() const { return has_value_; }
};
template<typename T>
optional<T> make_optional(T&& val) {
return optional<T>{std::forward<T>(val)};
}
} // namespace std
#endif
@@ -707,7 +789,7 @@ struct sstring {
static constexpr std::array<char, sizeof...(CHARS)> as_array() { return {CHARS...}; }
template<char ... OTHER_CHARS>
constexpr bool operator==(const sstring<OTHER_CHARS...> & other) {
constexpr bool operator==(const sstring<OTHER_CHARS...> & other) const {
return as_array() == other.as_array();
}
};
@@ -786,13 +868,13 @@ using sstring_builder_t = typename sstring_builder<LENGTH, CHARS...>::type;
*/
#define MAKE_SSTRING(literal) sstring_builder_t<sizeof(literal)-1, MACRO_GET_64(literal, 0)>
namespace std {
/*namespace std {
template<char ... CHARS>
static std::ostream& operator<<(std::ostream& stream, const sstring<CHARS...>& val) {
stream << val.chars;
return stream;
}
}
}*/
template<typename TDelimiter, typename ... TStr>
struct join_sstring_impl;
@@ -917,10 +999,10 @@ struct args_of<TRet(TArgs...)> {
using type = std::tuple<TArgs...>;
};
template<typename TRet, typename TObj, typename... TArgs>
struct args_of<std::_Mem_fn<TRet (TObj::*)(TArgs...)>> {
using type = std::tuple<TObj*, TArgs...>;
};
//template<typename TRet, typename TObj, typename... TArgs>
//struct args_of<_Mem_fn<TRet (TObj::*)(TArgs...)>> {
// using type = std::tuple<TObj*, TArgs...>;
//};
template<typename TRet, typename TObj, typename... TArgs>
struct args_of<TRet (TObj::*)(TArgs...) const> {
Firmware/fibre/cpp/include/fibre/libfibre.h
+348
View File
@@ -0,0 +1,348 @@
/**
* @brief Fibre C library
*
* The library is fully asynchronous and runs on an application-managed event
* loop. This integration happens with the call to libfibre_open(), where the
* application must pass a couple of functions that libfibre will use to put
* tasks on the event loop.
*
* Some general things to note:
* - None of the library's functions are blocking.
* - None of the library's functions can be expected to be thread-safe, they
* should not be invoked from any other thread than the one that runs the
* event loop.
* - Callbacks that the user passes to a libfibre function are always executed
* on the event loop thread.
* - All of the library's functions can be expected reentry-safe. That means
* you can call into any libfibre function from any callback handler that
* libfibre invokes.
*/
#ifndef __LIBFIBRE_H
#define __LIBFIBRE_H
#include <stdint.h>
#include <stdlib.h>
#if defined(_MSC_VER)
# define DLL_EXPORT __declspec(dllexport)
# define DLL_IMPORT __declspec(dllimport)
#elif defined(__GNUC__)
# define DLL_EXPORT __attribute__((visibility("default")))
# define DLL_IMPORT
# if __GNUC__ > 4
# define DLL_LOCAL __attribute__((visibility("hidden")))
# else
# define DLL_LOCAL
# endif
#else
# error("Don't know how to export shared object libraries")
#endif
#ifdef FIBRE_COMPILE
# define FIBRE_PUBLIC DLL_EXPORT
# define FIBRE_PRIVATE DLL_LOCAL
#else
# define FIBRE_PUBLIC DLL_IMPORT
#endif
#define FIBRE_PRIVATE DLL_LOCAL
#ifdef __cplusplus
extern "C" {
#endif
struct LibFibreCtx;
struct LibFibreDiscoveryCtx;
struct LibFibreCallContext;
struct LibFibreObject;
struct LibFibreInterface;
struct LibFibreFunction;
struct LibFibreAttribute;
enum FibreStatus {
kFibreOk,
kFibreCancelled,
kFibreClosed,
kFibreInvalidArgument,
kFibreInternalError
};
struct LibFibreVersion {
uint16_t major;
uint16_t minor;
uint16_t patch;
};
typedef int (*post_cb_t)(void (*callback)(void*), void* cb_ctx);
typedef int (*register_event_cb_t)(int fd, uint32_t events, void (*callback)(void*), void* cb_ctx);
typedef int (*deregister_event_cb_t)(int fd);
typedef struct EventLoopTimer* (*call_later_cb_t)(float delay, void (*callback)(void*), void* cb_ctx);
typedef int (*cancel_timer_cb_t)(struct EventLoopTimer* timer);
/**
* @brief construct_object callback type for libfibre_open().
*
* @param ctx: The user data that was passed to libfibre_open().
* @param obj: An object handle. This handle is valid until the invokation of
* destroy_object(). It is unique at any point in time but can be reused
* after destroy_object().
* @param intf: A handle for the interface that this object implements.
* The handle may be identical to an interface handle announced for a
* previous object.
* The interface handle is valid until the last object that implements it
* is destroyed.
* @param intf_name: The ASCII-encoded name of the interface. Can be NULL be for
* anonymous interfaces. If not NULL, it is only valid for the duration
* of the callback and must not be freed by the application.
*/
typedef void (*construct_object_cb_t)(void* ctx, LibFibreObject* obj, LibFibreInterface* intf, const char* intf_name, size_t intf_name_length);
typedef void (*destroy_object_cb_t)(void* ctx, LibFibreObject* obj);
typedef void (*on_found_object_cb_t)(void*, LibFibreObject*);
typedef void (*on_stopped_cb_t)(void*, FibreStatus);
typedef void (*on_attribute_added_cb_t)(void*, LibFibreAttribute*, const char* name, size_t name_length, LibFibreInterface*, const char* intf_name, size_t intf_name_length);
typedef void (*on_attribute_removed_cb_t)(void*, LibFibreAttribute*);
typedef void (*on_function_added_cb_t)(void*, LibFibreFunction*, const char* name, size_t name_length, const char** input_names, const char** input_codecs, const char** output_names, const char** output_codecs);
typedef void (*on_function_removed_cb_t)(void*, LibFibreFunction*);
/**
* @brief Completion callback type for libfibre_start_call().
*
* @param ctx: The user data that was passed to libfibre_start_call().
* @param status: The status of the function call.
* kFibreOk indicates successful completion of the function call. All
* other error codes make no guarantee whether the call was executed or
* not.
* kFibreClosed indicates that the underlying object was lost during the
* function call. The call may or may not have succeeded.
* @param rx_end: Points to the address after the last byte written to the
* output buffer. This pointer always points to a valid position in the
* buffer (or the end of the buffer), even if the call failed. However if
* the status is not kFibreOk then the pointer may not precisely indicate
* the received data range.
*/
typedef void (*on_call_completed_cb_t)(void* ctx, FibreStatus status, uint8_t* rx_end);
/**
* @brief Returns the version of the libfibre library.
*
* The returned struct must not be freed.
*
* The version adheres to Semantic Versioning, that means breaking changes of
* the ABI can be detected by an increment of the major version number (unless
* it's zero).
*
* Even if breaking changes are introduced, we promise to keep this function
* backwards compatible.
*/
const struct LibFibreVersion* libfibre_get_version();
/**
* @brief Opens and initializes a Fibre context.
*
* @param post: Called by libfibre when it wants the application to run
* a callback on the application's event loop.
* This is the only callback that libfibre can invoke from a different
* thread than the event loop thread itself. The application must
* ensure that this callback is thread-safe.
* This allows libfibre to run other threads internally while keeping
* threading promises made to the application.
* @param register_event: TODO: this is a Linux specific callback. Need to use
* IOCP on Windows.
* @param deregister_event: TODO: this is a Linux specific callback. Need to use
* IOCP on Windows.
* @param call_later: Called by libfibre to ask the application to call a
* certain callback after a certain amount of time on the same thread
* on which libfibre_open() was called. The application should return
* an opaque handle that libfibre can use to cancel the timer.
* @param cancel_timer: Called by libfibre to ask the application to cancel a
* callback timer previously enqueued with call_later().
* @param construct_object: Called by libfibre for every remote object that is
* allocated. This can be a root object that was just discovered or an
* object that was created as a result of operations like
* libfibre_get_attribute(). The application can use this to construct
* a corresponding object in the application's environment.
* @param destroy_object: Called by libfibre when a remote object is lost, for
* instance because all channels that provided connection to the object
* broke down.
* An object pointer must no longer be used during or after the call to
* destroy_object().
* @param cb_ctx: Arbitrary user data passed to construct_object() and
* destroy_object().
*/
FIBRE_PUBLIC struct LibFibreCtx* libfibre_open(
post_cb_t post,
register_event_cb_t register_event,
deregister_event_cb_t deregister_event,
call_later_cb_t call_later,
cancel_timer_cb_t cancel_timer,
construct_object_cb_t construct_object,
destroy_object_cb_t destroy_object,
void* cb_ctx);
/**
* @brief Closes a context that was previously opened with libfibre_open().
*
* This function must not be invoked before all ongoing discovery processes
* are stopped and all channels are closed.
*/
FIBRE_PUBLIC void libfibre_close(struct LibFibreCtx* ctx);
/**
* @brief Starts looking for Fibre objects that match the specifications.
*
* TODO: specify if specs needs to remain valid for the duration of discovery.
*
* @param ctx: The libfibre context that was obtained from libfibre_open().
* @param specs: Pointer to an ASCII string encoding the channel specifications.
* Must remain valid for the duration of the discovery.
*
* The specification has the format:
* "transport_provider1:args1;transport_provider2:args2"
* Transport providers are for example "usb", "serial", etc.
* Refer to the transport provider's documentation to see what arguments
* it takes.
*
* Example:
* "usb:idVendor=0x1209,idVendor=0x0d32;serial:path=/dev/ttyACM0"
* This will look for channels on USB devices with VID:PID 1209:0d32 and
* on the serial port /dev/ttyACM0.
* @param on_found_object: Invoked for every object that is found. Objects are
* first passed to the construct_object() callback of libfibre_open()
* before they are passed to this callback. An application should use
* the destroy_object() callback of the libfibre_open() function to
* detect the loss of objects.
* @param on_stopped: Invoked when the discovery stops for any reason, including
* a corresponding call to libfibre_stop_discovery().
* @param cb_ctx: Arbitrary user data passed to the callbacks.
* @returns: An opaque handle which should be passed to libfibre_stop_discovery().
*/
FIBRE_PUBLIC void libfibre_start_discovery(LibFibreCtx* ctx, const char* specs, size_t specs_len, struct LibFibreDiscoveryCtx** handle,
on_found_object_cb_t on_found_object,
on_stopped_cb_t on_stopped, void* cb_ctx);
/**
* @brief Stops an ongoing discovery process that was previously started with
* libfibre_start_discovery().
*
* The discovery is stopped asynchronously. That means it must still be
* considered ongoing until the on_stopped callback which was passed to
* libfibre_start_discovery() is invoked. Once this callback is invoked,
* libfibre_stop_discovery() must no longer be called.
*/
FIBRE_PUBLIC void libfibre_stop_discovery(LibFibreCtx* ctx, LibFibreDiscoveryCtx* discovery_ctx);
/**
* @brief Subscribes to changes on the interface.
*
* All functions and attributes which are already part of the interface by the
* time this function is called are also announced to the subscriber.
*
* @param interface: An interface handle that was obtained in the callback of
* libfibre_start_discovery().
* @param on_attribute_added: Invoked when an attribute is added to the
* interface. The name and intf_name buffers are only valid for the
* duration of the callback and must not be freed by the application.
* The attribute handle remains valid until the corresponding call to
* on_attribute_removed().
* @param on_attribute_removed: Invoked when an attribute is removed from the
* interface, including when the interface is being torn down. This is
* called exactly once for every call to on_attribute_added().
* @param on_function_added: Invoked when a function is added to the
* interface. The input_names, input_codecs, output_names and
* output_codecs arguments are null terminated lists of null terminated
* strings. The name buffer and the four lists are only valid for the
* duration of the callback and must not be freed by the application.
* The function handle remains valid until the corresponding call to
* on_function_removed().
* @param on_function_removed: Invoked when a function is removed from the
* interface, including when the interface is being torn down. This is
* called exactly once for every call to on_function_added().
* @param cb_ctx: Arbitrary user data passed to the callbacks.
*/
FIBRE_PUBLIC void libfibre_subscribe_to_interface(LibFibreInterface* interface,
on_attribute_added_cb_t on_attribute_added,
on_attribute_removed_cb_t on_attribute_removed,
on_function_added_cb_t on_function_added,
on_function_removed_cb_t on_function_removed,
void* cb_ctx);
/**
* @brief Returns the object that corresponds the the specified attribute of
* another object.
*
* This function runs purely locally and therefore returns a result immediately.
*
* TODO: it might be useful to allow this operation to go through to the remote
* device.
* TODO: Specify whether the returned object handle must be identical for
* repeated calls.
*
* @param parent_obj: An object handle that was obtained in the callback of
* libfibre_start_discovery() or from a previous call to
* libfibre_get_attribute().
* @param attr: An attribute handle that was obtained in the on_attribute_added()
* callback of libfibre_subscribe_to_interface().
* @param child_obj_ptr: If and only if the function succeeds, the variable that
* this argument points to is set to the requested subobject. The returned
* object handle is only guaranteed to remain valid until the next
* iteration of the libfibre event loop or until any other libfibre
* function (other than libfibre_ref_obj()) is invoked. If the
* application intends to keep the object handle around it must call
* libfibre_ref_obj() immediately.
* @returns: kFibreOk or kFibreInvalidArgument
*/
FIBRE_PUBLIC FibreStatus libfibre_get_attribute(LibFibreObject* parent_obj, LibFibreAttribute* attr, LibFibreObject** child_obj_ptr);
/**
* @brief Starts the invokation of a function call.
*
* Once the call has completed (whether successful, failed or cancelled), the
* provided callback will be invoked.
* Until then, the ongoing call can be aborted with libfibre_cancel_call().
*
* @param obj: An object handle that was obtained in the callback of
* libfibre_start_discovery() or from a call to libfibre_get_attribute().
* @param func: A function handle that was obtained in the on_function_added()
* callback of libfibre_subscribe_to_interface().
* @param input: The buffer that contains the input arguments. Must remain valid
* until on_completed() is called.
* @param output: The buffer where the output arguments will be written. Must
* remain valid until on_completed() is called.
* @param handle: The variable being pointed to by this argument is set to a
* handle that can be passed to libfibre_cancel_call() to cancel the
* started call. If on_complete() is invoked directly during
* libfibre_start_call() then the handle variable is not updated later
* than this invokation.
* @param on_completed: Called when the operation completes, whether successful
* or not.
*/
FIBRE_PUBLIC void libfibre_start_call(LibFibreObject* obj, LibFibreFunction* func, const uint8_t *input, size_t input_length, uint8_t *output, size_t output_length, LibFibreCallContext** handle, on_call_completed_cb_t on_completed, void* ctx);
/**
* @brief Cancels an ongoing function call.
*
* This must not be called twice for the same call and must not be called once
* the completion callback of the function call is invoked.
*
* The completion callback associated with this call will still be invoked after
* the call is cancelled. Until then, the call must still be considered in
* progress.
*
* After calling this function, the function call that was cancelled may or may
* not still go into effect.
*
* @param handle: The function call handle that was obtained by a call to
* libfibre_cancel_call().
*/
FIBRE_PUBLIC void libfibre_cancel_call(LibFibreCallContext* handle);
#ifdef __cplusplus
}
#endif
#endif // __LIBFIBRE_H
Firmware/fibre/cpp/include/fibre/protocol.hpp
+18 -338
View File
@@ -5,9 +5,6 @@ see protocol.md for the protocol specification
#ifndef __PROTOCOL_HPP
#define __PROTOCOL_HPP
// TODO: resolve assert
#define assert(expr)
#include <functional>
#include <limits>
#include <cmath>
@@ -21,66 +18,10 @@ see protocol.md for the protocol specification
#include "bufptr.hpp"
#include "simple_serdes.hpp"
// Note that this option cannot be used to debug UART because it prints on UART
//#define DEBUG_FIBRE
#ifdef DEBUG_FIBRE
#define LOG_FIBRE(...) do { printf(__VA_ARGS__); } while (0)
#else
#define LOG_FIBRE(...) ((void) 0)
#endif
// Default CRC-8 Polynomial: x^8 + x^5 + x^4 + x^2 + x + 1
// Can protect a 4 byte payload against toggling of up to 5 bits
// source: https://users.ece.cmu.edu/~koopman/crc/index.html
constexpr uint8_t CANONICAL_CRC8_POLYNOMIAL = 0x37;
constexpr uint8_t CANONICAL_CRC8_INIT = 0x42;
constexpr size_t CRC8_BLOCKSIZE = 4;
// Default CRC-16 Polynomial: 0x9eb2 x^16 + x^13 + x^12 + x^11 + x^10 + x^8 + x^6 + x^5 + x^2 + 1
// Can protect a 135 byte payload against toggling of up to 5 bits
// source: https://users.ece.cmu.edu/~koopman/crc/index.html
// Also known as CRC-16-DNP
constexpr uint16_t CANONICAL_CRC16_POLYNOMIAL = 0x3d65;
constexpr uint16_t CANONICAL_CRC16_INIT = 0x1337;
constexpr uint8_t CANONICAL_PREFIX = 0xAA;
/* move to fibre_config.h ******************************/
typedef size_t endpoint_id_t;
struct ReceiverState {
endpoint_id_t endpoint_id;
size_t length;
uint16_t seqno_thread;
uint16_t seqno;
bool expect_ack;
bool expect_response;
bool enforce_ordering;
};
/*******************************************************/
constexpr uint16_t PROTOCOL_VERSION = 1;
// This value must not be larger than USB_TX_DATA_SIZE defined in usbd_cdc_if.h
constexpr uint16_t TX_BUF_SIZE = 32; // does not work with 64 for some reason
constexpr uint16_t RX_BUF_SIZE = 128; // larger values than 128 have currently no effect because of protocol limitations
// Maximum time we allocate for processing and responding to a request
constexpr uint32_t PROTOCOL_SERVER_TIMEOUT_MS = 10;
typedef struct {
uint16_t json_crc = 0;
uint16_t endpoint_id = 0;
uint16_t json_crc;
uint16_t endpoint_id;
} endpoint_ref_t;
@@ -97,247 +38,6 @@ bool set_endpoint_from_float(endpoint_ref_t endpoint_ref, float value);
}
template<typename T, typename = typename std::enable_if_t<!std::is_const<T>::value>>
inline size_t write_le(T value, uint8_t* buffer){
//TODO: add static_assert that this is still a little endian machine
std::memcpy(&buffer[0], &value, sizeof(value));
return sizeof(value);
}
template<typename T>
typename std::enable_if_t<std::is_const<T>::value, size_t>
write_le(T value, uint8_t* buffer) {
return write_le<std::remove_const_t<T>>(value, buffer);
}
template<>
inline size_t write_le<float>(float value, uint8_t* buffer) {
static_assert(CHAR_BIT * sizeof(float) == 32, "32 bit floating point expected");
static_assert(std::numeric_limits<float>::is_iec559, "IEEE 754 floating point expected");
uint32_t value_as_uint32;
std::memcpy(&value_as_uint32, &value, sizeof(uint32_t));
return write_le<uint32_t>(value_as_uint32, buffer);
}
template<typename T>
inline size_t read_le(T* value, const uint8_t* buffer){
// TODO: add static_assert that this is still a little endian machine
std::memcpy(value, buffer, sizeof(*value));
return sizeof(*value);
}
template<>
inline size_t read_le<float>(float* value, const uint8_t* buffer) {
static_assert(CHAR_BIT * sizeof(float) == 32, "32 bit floating point expected");
static_assert(std::numeric_limits<float>::is_iec559, "IEEE 754 floating point expected");
return read_le(reinterpret_cast<uint32_t*>(value), buffer);
}
// @brief Reads a value of type T from the buffer.
// @param buffer Pointer to the buffer to be read. The pointer is updated by the number of bytes that were read.
// @param length The number of available bytes in buffer. This value is updated to subtract the bytes that were read.
template<typename T>
static inline T read_le(const uint8_t** buffer, size_t* length) {
T result;
size_t cnt = read_le(&result, *buffer);
*buffer += cnt;
*length -= cnt;
return result;
}
class PacketSink {
public:
// @brief Get the maximum packet length (aka maximum transmission unit)
// A packet size shall take no action and return an error code if the
// caller attempts to send an oversized packet.
//virtual size_t get_mtu() = 0;
// @brief Processes a packet.
// The blocking behavior shall depend on the thread-local deadline_ms variable.
// @return: 0 on success, otherwise a non-zero error code
// TODO: define what happens when the packet is larger than what the implementation can handle.
virtual int process_packet(const uint8_t* buffer, size_t length) = 0;
};
class StreamSink {
public:
// @brief Processes a chunk of bytes that is part of a continuous stream.
// The blocking behavior shall depend on the thread-local deadline_ms variable.
// @param processed_bytes: if not NULL, shall be incremented by the number of
// bytes that were consumed.
// @return: 0 on success, otherwise a non-zero error code
virtual int process_bytes(const uint8_t* buffer, size_t length, size_t* processed_bytes) = 0;
// @brief Returns the number of bytes that can still be written to the stream.
// Shall return SIZE_MAX if the stream has unlimited lenght.
// TODO: deprecate
virtual size_t get_free_space() = 0;
/*int process_bytes(const uint8_t* buffer, size_t length) {
size_t processed_bytes = 0;
return process_bytes(buffer, length, &processed_bytes);
}*/
};
class StreamSource {
public:
// @brief Generate a chunk of bytes that are part of a continuous stream.
// The blocking behavior shall depend on the thread-local deadline_ms variable.
// @param generated_bytes: if not NULL, shall be incremented by the number of
// bytes that were written to buffer.
// @return: 0 on success, otherwise a non-zero error code
virtual int get_bytes(uint8_t* buffer, size_t length, size_t* generated_bytes) = 0;
// @brief Returns the number of bytes that can still be written to the stream.
// Shall return SIZE_MAX if the stream has unlimited lenght.
// TODO: deprecate
//virtual size_t get_free_space() = 0;
};
class StreamToPacketSegmenter : public StreamSink {
public:
explicit StreamToPacketSegmenter(PacketSink& output) :
output_(output)
{
};
int process_bytes(const uint8_t *buffer, size_t length, size_t* processed_bytes) override;
size_t get_free_space() { return SIZE_MAX; }
private:
uint8_t header_buffer_[3] = {0};
size_t header_index_ = 0;
uint8_t packet_buffer_[RX_BUF_SIZE] = {0};
size_t packet_index_ = 0;
size_t packet_length_ = 0;
PacketSink& output_;
};
class StreamBasedPacketSink : public PacketSink {
public:
explicit StreamBasedPacketSink(StreamSink& output) :
output_(output)
{
};
//size_t get_mtu() { return SIZE_MAX; }
int process_packet(const uint8_t *buffer, size_t length) override;
private:
StreamSink& output_;
};
// @brief: Represents a stream sink that's based on an underlying packet sink.
// A single call to process_bytes may result in multiple packets being sent.
class PacketBasedStreamSink : public StreamSink {
public:
explicit PacketBasedStreamSink(PacketSink& packet_sink) : _packet_sink(packet_sink) {}
~PacketBasedStreamSink() {}
int process_bytes(const uint8_t* buffer, size_t length, size_t* processed_bytes) override {
// Loop to ensure all bytes get sent
while (length) {
size_t chunk = length;
// send chunk as packet
if (_packet_sink.process_packet(buffer, chunk))
return -1;
buffer += chunk;
length -= chunk;
if (processed_bytes)
*processed_bytes += chunk;
}
return 0;
}
size_t get_free_space() { return SIZE_MAX; }
private:
PacketSink& _packet_sink;
};
// Implements the StreamSink interface by writing into a fixed size
// memory buffer.
class MemoryStreamSink : public StreamSink {
public:
MemoryStreamSink(uint8_t *buffer, size_t length) :
buffer_(buffer),
buffer_length_(length) {}
// Returns 0 on success and -1 if the buffer could not accept everything because it became full
int process_bytes(const uint8_t* buffer, size_t length, size_t* processed_bytes) override {
size_t chunk = length < buffer_length_ ? length : buffer_length_;
memcpy(buffer_, buffer, chunk);
buffer_ += chunk;
buffer_length_ -= chunk;
if (processed_bytes)
*processed_bytes += chunk;
return chunk == length ? 0 : -1;
}
size_t get_free_space() { return buffer_length_; }
private:
uint8_t * buffer_;
size_t buffer_length_;
};
// Implements the StreamSink interface by discarding the first couple of bytes
// and then forwarding the rest to another stream.
class NullStreamSink : public StreamSink {
public:
NullStreamSink(size_t skip, StreamSink& follow_up_stream) :
skip_(skip),
follow_up_stream_(follow_up_stream) {}
// Returns 0 on success and -1 if the buffer could not accept everything because it became full
int process_bytes(const uint8_t* buffer, size_t length, size_t* processed_bytes) override {
if (skip_ < length) {
buffer += skip_;
length -= skip_;
if (processed_bytes)
*processed_bytes += skip_;
skip_ = 0;
return follow_up_stream_.process_bytes(buffer, length, processed_bytes);
} else {
skip_ -= length;
if (processed_bytes)
*processed_bytes += length;
return 0;
}
}
size_t get_free_space() override { return skip_ + follow_up_stream_.get_free_space(); }
private:
size_t skip_;
StreamSink& follow_up_stream_;
};
// Implements the StreamSink interface by calculating the CRC16 checksum
// on the data that is sent to it.
class CRC16Calculator : public StreamSink {
public:
explicit CRC16Calculator(uint16_t crc16_init) :
crc16_(crc16_init) {}
int process_bytes(const uint8_t* buffer, size_t length, size_t* processed_bytes) override{
crc16_ = calc_crc16<CANONICAL_CRC16_POLYNOMIAL>(crc16_, buffer, length);
if (processed_bytes)
*processed_bytes += length;
return 0;
}
size_t get_free_space() override { return SIZE_MAX; }
uint16_t get_crc16() { return crc16_; }
private:
uint16_t crc16_;
};
namespace fibre {
template<typename T, typename = void>
@@ -384,7 +84,7 @@ template<> struct Codec<uint64_t> {
template<> struct Codec<float> {
static std::optional<float> decode(cbufptr_t* buffer) {
std::optional<uint32_t> int_val = Codec<uint32_t>::decode(buffer);
return int_val.has_value() ? std::optional<float>(*reinterpret_cast<float*>(&int_val.value())) : std::nullopt;
return int_val.has_value() ? std::optional<float>(*reinterpret_cast<float*>(&*int_val)) : std::nullopt;
}
static bool encode(float value, bufptr_t* buffer) {
void* ptr = &value;
@@ -395,7 +95,7 @@ template<typename T>
struct Codec<T, std::enable_if_t<std::is_enum<T>::value>> {
static std::optional<T> decode(cbufptr_t* buffer) {
std::optional<int32_t> int_val = SimpleSerializer<int32_t, false>::read(&(buffer->begin()), buffer->end());
return int_val.has_value() ? std::make_optional(static_cast<T>(int_val.value())) : std::nullopt;
return int_val.has_value() ? std::make_optional(static_cast<T>(*int_val)) : std::nullopt;
}
static bool encode(T value, bufptr_t* buffer) { return SimpleSerializer<int32_t, false>::write(value, &(buffer->begin()), buffer->end()); }
};
@@ -403,7 +103,7 @@ template<> struct Codec<endpoint_ref_t> {
static std::optional<endpoint_ref_t> decode(cbufptr_t* buffer) {
std::optional<uint16_t> val0 = SimpleSerializer<uint16_t, false>::read(&(buffer->begin()), buffer->end());
std::optional<uint16_t> val1 = SimpleSerializer<uint16_t, false>::read(&(buffer->begin()), buffer->end());
return (val0.has_value() && val1.has_value()) ? std::make_optional(endpoint_ref_t{val1.value(), val0.value()}) : std::nullopt;
return (val0.has_value() && val1.has_value()) ? std::make_optional(endpoint_ref_t{*val1, *val0}) : std::nullopt;
}
static bool encode(endpoint_ref_t value, bufptr_t* buffer) {
return SimpleSerializer<uint16_t, false>::write(value.endpoint_id, &(buffer->begin()), buffer->end())
@@ -413,29 +113,6 @@ template<> struct Codec<endpoint_ref_t> {
}
/* @brief Handles the communication protocol on one channel.
*
* When instantiated with a list of endpoints and an output packet sink,
* objects of this class will handle packets passed into process_packet,
* pass the relevant data to the corresponding endpoints and dispatch response
* packets on the output.
*/
class BidirectionalPacketBasedChannel : public PacketSink {
public:
explicit BidirectionalPacketBasedChannel(PacketSink& output) :
output_(output)
{ }
//size_t get_mtu() {
// return SIZE_MAX;
//}
int process_packet(const uint8_t* buffer, size_t length) override;
private:
PacketSink& output_;
uint8_t tx_buf_[TX_BUF_SIZE] = {0};
};
/* ToString / FromString functions -------------------------------------------*/
/*
* These functions are currently not used by Fibre and only here to
@@ -451,40 +128,43 @@ struct format_traits_t;
// static constexpr const char * fmt = "%f";
// static constexpr const char * fmtp = "%f";
// };
template<> struct format_traits_t<int64_t> { using type = void;
template<> struct format_traits_t<long long> { using type = void;
static constexpr const char * fmt = "%lld";
static constexpr const char * fmtp = "%lld";
};
template<> struct format_traits_t<uint64_t> { using type = void;
template<> struct format_traits_t<unsigned long long> { using type = void;
static constexpr const char * fmt = "%llu";
static constexpr const char * fmtp = "%llu";
};
template<> struct format_traits_t<int32_t> { using type = void;
template<> struct format_traits_t<long> { using type = void;
static constexpr const char * fmt = "%ld";
static constexpr const char * fmtp = "%ld";
};
template<> struct format_traits_t<uint32_t> { using type = void;
template<> struct format_traits_t<unsigned long> { using type = void;
static constexpr const char * fmt = "%lu";
static constexpr const char * fmtp = "%lu";
};
// TODO: change all overloads to fundamental int type space
template<> struct format_traits_t<int> { using type = void;
static constexpr const char * fmt = "%d";
static constexpr const char * fmtp = "%d";
};
template<> struct format_traits_t<unsigned int> { using type = void;
static constexpr const char * fmt = "%ud";
static constexpr const char * fmtp = "%ud";
};
template<> struct format_traits_t<int16_t> { using type = void;
template<> struct format_traits_t<short> { using type = void;
static constexpr const char * fmt = "%hd";
static constexpr const char * fmtp = "%hd";
};
template<> struct format_traits_t<uint16_t> { using type = void;
template<> struct format_traits_t<unsigned short> { using type = void;
static constexpr const char * fmt = "%hu";
static constexpr const char * fmtp = "%hu";
};
template<> struct format_traits_t<int8_t> { using type = void;
template<> struct format_traits_t<char> { using type = void;
static constexpr const char * fmt = "%hhd";
static constexpr const char * fmtp = "%d";
};
template<> struct format_traits_t<uint8_t> { using type = void;
template<> struct format_traits_t<unsigned char> { using type = void;
static constexpr const char * fmt = "%hhu";
static constexpr const char * fmtp = "%u";
};
@@ -590,7 +270,7 @@ struct Property {
T exchange(std::optional<T> value) const {
T old_value = (*getter_)(ctx_);
if (value.has_value()) {
(*setter_)(ctx_, value.value());
(*setter_)(ctx_, *value);
}
return old_value;
}
Firmware/fibre/cpp/include/fibre/simple_serdes.hpp
+51 -2
View File
@@ -1,8 +1,10 @@
#ifndef __FIBRE_SIMPLE_SERDES
#define __FIBRE_SIMPLE_SERDES
//#include "stream.hpp"
#include "cpp_utils.hpp"
#include "limits.h"
#include <optional> // TODO: make C++11 backport of this
#include <cstring>
template<typename T, bool BigEndian, typename = void>
struct SimpleSerializer;
@@ -73,5 +75,52 @@ inline bool write_le(T value, fibre::bufptr_t* buffer) {
return LittleEndianSerializer<T>::write(value, &buffer->begin(), buffer->end());
}
template<typename T, typename = typename std::enable_if_t<!std::is_const<T>::value>>
inline size_t write_le(T value, uint8_t* buffer){
//TODO: add static_assert that this is still a little endian machine
std::memcpy(&buffer[0], &value, sizeof(value));
return sizeof(value);
}
template<typename T>
typename std::enable_if_t<std::is_const<T>::value, size_t>
write_le(T value, uint8_t* buffer) {
return write_le<std::remove_const_t<T>>(value, buffer);
}
template<>
inline size_t write_le<float>(float value, uint8_t* buffer) {
static_assert(CHAR_BIT * sizeof(float) == 32, "32 bit floating point expected");
static_assert(std::numeric_limits<float>::is_iec559, "IEEE 754 floating point expected");
uint32_t value_as_uint32;
std::memcpy(&value_as_uint32, &value, sizeof(uint32_t));
return write_le<uint32_t>(value_as_uint32, buffer);
}
template<typename T>
inline size_t read_le(T* value, const uint8_t* buffer){
// TODO: add static_assert that this is still a little endian machine
std::memcpy(value, buffer, sizeof(*value));
return sizeof(*value);
}
template<>
inline size_t read_le<float>(float* value, const uint8_t* buffer) {
static_assert(CHAR_BIT * sizeof(float) == 32, "32 bit floating point expected");
static_assert(std::numeric_limits<float>::is_iec559, "IEEE 754 floating point expected");
return read_le(reinterpret_cast<uint32_t*>(value), buffer);
}
// @brief Reads a value of type T from the buffer.
// @param buffer Pointer to the buffer to be read. The pointer is updated by the number of bytes that were read.
// @param length The number of available bytes in buffer. This value is updated to subtract the bytes that were read.
template<typename T>
static inline T read_le(const uint8_t** buffer, size_t* length) {
T result;
size_t cnt = read_le(&result, *buffer);
*buffer += cnt;
*length -= cnt;
return result;
}
#endif
Firmware/fibre/cpp/legacy_object_client.cpp
+483
View File
@@ -0,0 +1,483 @@
#include "legacy_object_client.hpp"
#include "legacy_protocol.hpp"
#include "include/fibre/simple_serdes.hpp"
#include "logging.hpp"
#include "print_utils.hpp"
#include "include/fibre/crc.hpp"
#include <variant>
#include <algorithm>
DEFINE_LOG_TOPIC(LEGACY_OBJ);
USE_LOG_TOPIC(LEGACY_OBJ);
using namespace fibre;
struct json_error {
const char* ptr;
std::string str;
};
struct json_value;
using json_list = std::vector<std::shared_ptr<json_value>>;
using json_dict = std::vector<std::pair<std::shared_ptr<json_value>, std::shared_ptr<json_value>>>;
using json_value_variant = std::variant<std::string, int, json_list, json_dict, json_error>;
struct json_value : json_value_variant {
//json_value(const json_value_variant& v) : json_value_variant{v} {}
template<typename T> json_value(T&& arg) : json_value_variant{std::forward<T>(arg)} {}
//json_value_variant v;
};
// helper functions
bool json_is_str(json_value val) { return val.index() == 0; }
bool json_is_int(json_value val) { return val.index() == 1; }
bool json_is_list(json_value val) { return val.index() == 2; }
bool json_is_dict(json_value val) { return val.index() == 3; }
bool json_is_err(json_value val) { return val.index() == 4; }
std::string json_as_str(json_value val) { return std::get<0>(val); }
int json_as_int(json_value val) { return std::get<1>(val); }
json_list json_as_list(json_value val) { return std::get<2>(val); }
json_dict json_as_dict(json_value val) { return std::get<3>(val); }
json_error json_as_err(json_value val) { return std::get<4>(val); }
json_value json_make_error(const char* ptr, std::string str) {
return {json_error{ptr, str}};
}
void json_skip_whitespace(const char** begin, const char* end) {
while (*begin < end && std::isspace(**begin)) {
(*begin)++;
}
}
bool json_comp(const char* begin, const char* end, char c) {
return begin < end && *begin == c;
}
json_value json_parse(const char** begin, const char* end) {
// skip whitespace
if (*begin >= end) {
return json_make_error(*begin, "expected value but got EOF");
}
if (json_comp(*begin, end, '{')) {
// parse dict
(*begin)++; // consume leading '{'
json_dict dict;
bool expect_comma = false;
json_skip_whitespace(begin, end);
while (!json_comp(*begin, end, '}')) {
if (expect_comma) {
if (!json_comp(*begin, end, ',')) {
return json_make_error(*begin, "expected ',' or '}'");
}
(*begin)++; // consume comma
json_skip_whitespace(begin, end);
}
expect_comma = true;
// Parse key-value pair
json_value key = json_parse(begin, end);
if (json_is_err(key)) return key;
json_skip_whitespace(begin, end);
if (!json_comp(*begin, end, ':')) {
return json_make_error(*begin, "expected :");
}
(*begin)++;
json_value val = json_parse(begin, end);
if (json_is_err(val)) return val;
dict.push_back({std::make_shared<json_value>(key), std::make_shared<json_value>(val)});
json_skip_whitespace(begin, end);
}
(*begin)++;
return {dict};
} else if (json_comp(*begin, end, '[')) {
// parse list
(*begin)++; // consume leading '['
json_list list;
bool expect_comma = false;
json_skip_whitespace(begin, end);
while (!json_comp(*begin, end, ']')) {
if (expect_comma) {
if (!json_comp(*begin, end, ',')) {
return json_make_error(*begin, "expected ',' or ']'");
}
(*begin)++; // consume comma
json_skip_whitespace(begin, end);
}
expect_comma = true;
// Parse item
json_value val = json_parse(begin, end);
if (json_is_err(val)) return val;
list.push_back(std::make_shared<json_value>(val));
json_skip_whitespace(begin, end);
}
(*begin)++; // consume trailing ']'
return {list};
} else if (json_comp(*begin, end, '"')) {
// parse string
(*begin)++; // consume leading '"'
std::string str;
while (!json_comp(*begin, end, '"')) {
if (*begin >= end) {
return json_make_error(*begin, "expected '\"' but got EOF");
}
if (json_comp(*begin, end, '\\')) {
return json_make_error(*begin, "escaped strings not supported");
}
str.push_back(**begin);
(*begin)++;
}
(*begin)++; // consume trailing '"'
return {str};
} else if (std::isdigit(**begin)) {
// parse int
std::string str;
while (*begin < end && std::isdigit(**begin)) {
str.push_back(**begin);
(*begin)++;
}
return {std::stoi(str)}; // note: this can throw an exception if the int is too long
} else {
return json_make_error(*begin, "unexpected character '" + std::string(*begin, *begin + 1) + "'");
}
}
json_value json_dict_find(json_dict dict, std::string key) {
auto it = std::find_if(dict.begin(), dict.end(),
[&](std::pair<std::shared_ptr<json_value>, std::shared_ptr<json_value>>& kv){
return json_is_str(*kv.first) && json_as_str(*kv.first) == key;
});
return (it == dict.end()) ? json_make_error(nullptr, "key not found") : *it->second;
}
std::unordered_map<std::string, size_t> codecs = {
{"bool", 1},
{"int8", 1},
{"uint8", 1},
{"int16", 2},
{"uint16", 2},
{"int32", 4},
{"uint32", 4},
{"int64", 6},
{"uint64", 6},
{"float", 4},
{"endpoint_ref", 4}
};
size_t get_codec_size(std::string codec) {
auto it = codecs.find(codec);
return (it == codecs.end()) ? 0 : it->second;
}
std::vector<LegacyFibreArg> parse_arglist(const json_value& list_val) {
std::vector<LegacyFibreArg> arglist;
for (auto& arg : json_is_list(list_val) ? json_as_list(list_val) : json_list()) {
if (!json_is_dict(*arg)) {
FIBRE_LOG(W) << "arglist is invalid";
continue;
}
auto dict = json_as_dict(*arg);
json_value name_val = json_dict_find(dict, "name");
json_value id_val = json_dict_find(dict, "id");
json_value type_val = json_dict_find(dict, "type");
if (!json_is_str(name_val) || !json_is_int(id_val) || ((int)(size_t)json_as_int(id_val) != json_as_int(id_val)) || !json_is_str(type_val)) {
FIBRE_LOG(W) << "arglist is invalid";
continue;
}
arglist.push_back({
json_as_str(name_val),
json_as_str(type_val),
(size_t)json_as_int(id_val),
get_codec_size(json_as_str(type_val))
});
}
return arglist;
}
void LegacyObjectClient::start(Completer<LegacyObjectClient*, std::shared_ptr<LegacyObject>>& on_found_root_object, Completer<LegacyObjectClient*>& on_lost_root_object) {
FIBRE_LOG(D) << "start";
on_found_root_object_ = &on_found_root_object;
on_lost_root_object_ = &on_lost_root_object;
json_.clear();
receive_more_json();
}
void LegacyObjectClient::start_call(size_t ep_num, LegacyFibreFunction* func, cbufptr_t input, bufptr_t output, CallContext** handle, Completer<CallResult>& completer) {
CallContext* call = new CallContext();
call->ep_num = ep_num;
call->tx_buf = input;
call->rx_buf = output;
call->func = func;
call->completer = &completer;
if (op_handle_) {
FIBRE_LOG(D) << "Call in progress. Enqueuing this call.";
// An operation is already in progress. Enqueue this one.
pending_calls_.push_back(call);
} else {
// No endpoint operation is in progress. Start this call immediately
FIBRE_LOG(D) << "No call in progress. Starting call now.";
call_ = call;
complete({kStreamOk, nullptr});
}
}
void LegacyObjectClient::cancel_call(CallContext* handle) {
if (call_ == handle) {
protocol_->cancel_endpoint_operation(op_handle_);
} else {
auto it = std::find(pending_calls_.begin(), pending_calls_.end(), handle);
if (it != pending_calls_.end()) {
CallContext* call = *it;
pending_calls_.erase(it);
safe_complete(call->completer, {kFibreCancelled, call->rx_buf.end()});
delete call;
}
}
}
std::shared_ptr<FibreInterface> LegacyObjectClient::get_property_interfaces(std::string codec, bool write) {
auto& dict = write ? rw_property_interfaces : ro_property_interfaces;
auto it = dict.find(codec);
if (it != dict.end()) {
return it->second;
}
FibreInterface intf;
size_t size = get_codec_size(codec);
if (!size) {
FIBRE_LOG(W) << "unknown size for codec " << codec;
}
intf.name = std::string{} + "fibre.Property<" + (write ? "readwrite" : "readonly") + " " + codec + ">";
intf.functions["read"] = {0, {}, {{"value", codec, 0, size}}};
if (write) {
intf.functions["exchange"] = {0, {{"newval", codec, 0, size}}, {{"oldval", codec, 0, size}}};
}
return dict[codec] = std::make_shared<FibreInterface>(intf);
}
std::shared_ptr<LegacyObject> LegacyObjectClient::load_object(json_value list_val) {
FibreInterface intf;
if (!json_is_list(list_val)) {
FIBRE_LOG(W) << "interface members must be a list";
return nullptr;
}
for (auto& item: json_as_list(list_val)) {
if (!json_is_dict(*item)) {
FIBRE_LOG(W) << "expected dict";
continue;
}
auto dict = json_as_dict(*item);
json_value type = json_dict_find(dict, "type");
json_value name_val = json_dict_find(dict, "name");
std::string name = json_is_str(name_val) ? json_as_str(name_val) : "[anonymous]";
if (json_is_str(type) && json_as_str(type) == "object") {
std::shared_ptr<LegacyObject> subobj = load_object(json_dict_find(dict, "members"));
intf.attributes[name] = {subobj};
} else if (json_is_str(type) && json_as_str(type) == "function") {
json_value id = json_dict_find(dict, "id");
if (!json_is_int(id) || ((int)(size_t)json_as_int(id) != json_as_int(id))) {
continue;
}
intf.functions[name] = {
(size_t)json_as_int(id),
parse_arglist(json_dict_find(dict, "inputs")),
parse_arglist(json_dict_find(dict, "outputs")),
};
} else if (json_is_str(type) && json_as_str(type) == "json") {
// Ignore
} else if (json_is_str(type)) {
std::string type_str = json_as_str(type);
json_value access = json_dict_find(dict, "access");
std::string access_str = json_is_str(access) ? json_as_str(access) : "r";
bool can_write = access_str.find('w') != std::string::npos;
json_value id = json_dict_find(dict, "id");
if (!json_is_int(id) || ((int)(size_t)json_as_int(id) != json_as_int(id))) {
continue;
}
LegacyObject subobj{
.client = this,
.ep_num = (size_t)json_as_int(id),
.intf = get_property_interfaces(type_str, can_write),
.known_to_application = false
};
auto subobj_ptr = std::make_shared<LegacyObject>(subobj);
objects_.push_back(subobj_ptr);
intf.attributes[name] = {subobj_ptr};
} else {
FIBRE_LOG(W) << "unsupported codec";
}
}
LegacyObject obj{
.client = this,
.ep_num = 0,
.intf = std::make_shared<FibreInterface>(intf),
.known_to_application = false
};
auto obj_ptr = std::make_shared<LegacyObject>(obj);
objects_.push_back(obj_ptr);
return obj_ptr;
}
void LegacyObjectClient::receive_more_json() {
write_le<uint32_t>(json_.size(), tx_buf_);
json_.resize(json_.size() + 1024);
bufptr_t rx_buf = {json_.data() + json_.size() - 1024, json_.data() + json_.size()};
protocol_->start_endpoint_operation(0, tx_buf_, rx_buf, &op_handle_, *this);
}
void LegacyObjectClient::complete(EndpointOperationResult result) {
op_handle_ = 0;
if (result.status == kStreamCancelled) {
if (call_) {
auto call = call_;
call_ = nullptr;
safe_complete(call->completer, {kFibreCancelled, call->rx_buf.end()});
delete call;
}
return;
} else if (result.status == kStreamClosed) {
if (call_) {
auto call = call_;
call_ = nullptr;
safe_complete(call->completer, {kFibreClosed, call->rx_buf.end()});
delete call;
}
return;
} else if (result.status != kStreamOk) {
FIBRE_LOG(W) << "endpoint operation failed"; // TODO: add retry logic
if (call_) {
auto call = call_;
call_ = nullptr;
safe_complete(call->completer, {kFibreInternalError, call->rx_buf.end()});
delete call;
}
return;
}
if (call_) {
// The endpoint operation that completed belongs to the active call
LegacyFibreFunction* func = call_->func;
if (call_->ep_num && !call_->progress) {
// Read/write/exchange property
FIBRE_LOG(D) << "starting property transaction on " << call_->ep_num << " with tx buf len " << call_->tx_buf.size() << " and rx len " << call_->rx_buf.size();
call_->progress++;
protocol_->start_endpoint_operation(call_->ep_num, call_->tx_buf, call_->rx_buf, &op_handle_, *this);
} else if (!call_->ep_num && call_->progress < func->inputs.size()) {
// Write input arg
size_t argnum = call_->progress;
call_->progress++;
cbufptr_t current_buf = call_->tx_buf.take(func->inputs[argnum].size);
call_->tx_buf = call_->tx_buf.skip(func->inputs[argnum].size);
protocol_->start_endpoint_operation(func->inputs[argnum].ep_num, current_buf, call_->rx_buf.take(0), &op_handle_, *this);
} else if (!call_->ep_num && call_->progress == func->inputs.size()) {
// Trigger
// call_->tx_buf should be empty by now
call_->progress++;
protocol_->start_endpoint_operation(func->ep_num, call_->tx_buf, call_->rx_buf.take(0), &op_handle_, *this);
} else if (!call_->ep_num && call_->progress < func->inputs.size() + 1 + func->outputs.size()) {
// Read output arg
size_t argnum = call_->progress - func->inputs.size() - 1;
call_->progress++;
bufptr_t current_buf = call_->rx_buf.take(func->outputs[argnum].size);
call_->rx_buf = call_->rx_buf.skip(func->outputs[argnum].size);
protocol_->start_endpoint_operation(func->outputs[argnum].ep_num, call_->tx_buf, current_buf, &op_handle_, *this);
} else {
CallContext* call = call_;
call_ = nullptr;
FIBRE_LOG(D) << "call completed!";
safe_complete(call->completer, {kFibreOk, call->rx_buf.end()});
delete call;
}
} else {
// The endpoint operation that completed belongs to the JSON fetch process
size_t n_received = result.rx_end - json_.data() - json_.size() + 1024;
json_.resize(json_.size() - 1024 + n_received);
if (n_received) {
receive_more_json();
return;
} else {
FIBRE_LOG(D) << "received JSON of length " << json_.size();
//FIBRE_LOG(D) << "JSON: " << str{json_.data(), json_.data() + json_.size()};
const char *begin = reinterpret_cast<const char*>(json_.data());
auto val = json_parse(&begin, begin + json_.size());
if (json_is_err(val)) {
size_t pos = json_as_err(val).ptr - reinterpret_cast<const char*>(json_.data());
FIBRE_LOG(E) << "JSON parsing error: " << json_as_err(val).str << " at position " << pos;
return;
} else if (!json_is_list(val)) {
FIBRE_LOG(E) << "JSON data must be a list";
return;
}
FIBRE_LOG(D) << "sucessfully parsed JSON";
root_obj_ = load_object(val);
json_crc_ = calc_crc16<CANONICAL_CRC16_POLYNOMIAL>(PROTOCOL_VERSION, json_.data(), json_.size());
if (root_obj_) {
safe_complete(on_found_root_object_, this, root_obj_);
}
}
}
// Start next call in the queue if any
// It's possible that the next function call was already started on one of
// the callbacks above.
if (!call_ && pending_calls_.size()) {
call_ = pending_calls_[0];
pending_calls_.erase(pending_calls_.begin());
complete({kStreamOk, nullptr});
}
}
Firmware/fibre/cpp/legacy_object_client.hpp
+107
View File
@@ -0,0 +1,107 @@
#ifndef __FIBRE_LEGACY_OBJECT_MODEL_HPP
#define __FIBRE_LEGACY_OBJECT_MODEL_HPP
//#include "legacy_protocol.hpp"
#include "async_stream.hpp"
#include <fibre/libfibre.h>
#include <unordered_map>
#include <vector>
#include <memory>
#include <string>
struct json_value;
namespace fibre {
struct EndpointOperationResult {
StreamStatus status;
uint8_t* rx_end;
};
using EndpointOperationHandle = uint32_t;
struct LegacyProtocolPacketBased;
struct LegacyFibreArg {
std::string name;
std::string codec;
size_t ep_num;
size_t size;
};
struct LegacyFibreFunction {
size_t ep_num; // 0 for property read/write/exchange functions
std::vector<LegacyFibreArg> inputs;
std::vector<LegacyFibreArg> outputs;
};
struct FibreInterface;
struct LegacyObjectClient;
struct LegacyObject;
struct LegacyFibreAttribute {
std::shared_ptr<LegacyObject> object;
};
struct FibreInterface {
std::string name;
std::unordered_map<std::string, LegacyFibreFunction> functions;
std::unordered_map<std::string, LegacyFibreAttribute> attributes;
};
struct LegacyObject {
LegacyObjectClient* client;
size_t ep_num;
std::shared_ptr<FibreInterface> intf;
bool known_to_application;
};
class LegacyObjectClient : Completer<EndpointOperationResult> {
public:
struct CallResult {
FibreStatus status;
uint8_t* end;
};
struct CallContext {
size_t progress = 0;
size_t ep_num;
cbufptr_t tx_buf;
bufptr_t rx_buf;
LegacyFibreFunction* func;
Completer<CallResult>* completer;
};
LegacyObjectClient(LegacyProtocolPacketBased* protocol) : protocol_(protocol) {}
void start(Completer<LegacyObjectClient*, std::shared_ptr<LegacyObject>>& on_found_root_object, Completer<LegacyObjectClient*>& on_lost_root_object);
void start_call(size_t ep_num, LegacyFibreFunction* func, cbufptr_t input, bufptr_t output, CallContext** handle, Completer<CallResult>& completer);
void cancel_call(CallContext* handle);
// For direct access by LegacyProtocolPacketBased and libfibre.cpp
uint16_t json_crc_ = 0;
Completer<LegacyObjectClient*>* on_lost_root_object_;
std::shared_ptr<LegacyObject> root_obj_;
std::vector<std::shared_ptr<LegacyObject>> objects_;
void* user_data_; // used by libfibre to store the libfibre context pointer
private:
std::shared_ptr<FibreInterface> get_property_interfaces(std::string codec, bool write);
std::shared_ptr<LegacyObject> load_object(json_value list_val);
void receive_more_json();
void complete(EndpointOperationResult result);
LegacyProtocolPacketBased* protocol_;
Completer<LegacyObjectClient*, std::shared_ptr<LegacyObject>>* on_found_root_object_;
uint8_t tx_buf_[4] = {0xff, 0xff, 0xff, 0xff};
EndpointOperationHandle op_handle_ = 0;
std::vector<uint8_t> json_;
CallContext* call_ = nullptr; // active call
std::vector<CallContext*> pending_calls_;
std::unordered_map<std::string, std::shared_ptr<FibreInterface>> rw_property_interfaces;
std::unordered_map<std::string, std::shared_ptr<FibreInterface>> ro_property_interfaces;
};
}
#endif // __FIBRE_LEGACY_OBJECT_MODEL_HPP
Firmware/fibre/cpp/legacy_protocol.cpp
+520
View File
File diff suppressed because it is too large Load Diff
Firmware/fibre/cpp/legacy_protocol.hpp
+165
View File
@@ -0,0 +1,165 @@
#ifndef __FIBRE_LEGACY_PROTOCOL_HPP
#define __FIBRE_LEGACY_PROTOCOL_HPP
#include "async_stream.hpp"
#ifdef FIBRE_ENABLE_CLIENT
#include "legacy_object_client.hpp"
#include <unordered_map>
#endif
namespace fibre {
// Default CRC-8 Polynomial: x^8 + x^5 + x^4 + x^2 + x + 1
// Can protect a 4 byte payload against toggling of up to 5 bits
// source: https://users.ece.cmu.edu/~koopman/crc/index.html
constexpr uint8_t CANONICAL_CRC8_POLYNOMIAL = 0x37;
constexpr uint8_t CANONICAL_CRC8_INIT = 0x42;
// Default CRC-16 Polynomial: 0x9eb2 x^16 + x^13 + x^12 + x^11 + x^10 + x^8 + x^6 + x^5 + x^2 + 1
// Can protect a 135 byte payload against toggling of up to 5 bits
// source: https://users.ece.cmu.edu/~koopman/crc/index.html
// Also known as CRC-16-DNP
constexpr uint16_t CANONICAL_CRC16_POLYNOMIAL = 0x3d65;
constexpr uint16_t CANONICAL_CRC16_INIT = 0x1337;
constexpr uint8_t CANONICAL_PREFIX = 0xAA;
constexpr uint16_t PROTOCOL_VERSION = 1;
class PacketWrapper : public AsyncStreamSink, Completer<WriteResult> {
public:
PacketWrapper(AsyncStreamSink* tx_channel)
: tx_channel_(tx_channel) {}
void start_write(cbufptr_t buffer, TransferHandle* handle, Completer<WriteResult>& completer) final;
void cancel_write(TransferHandle transfer_handle) final;
private:
void complete(WriteResult result);
AsyncStreamSink* tx_channel_;
TransferHandle inner_transfer_handle_;
uint8_t header_buf_[3];
uint8_t trailer_buf_[2];
const uint8_t* expected_tx_end_;
cbufptr_t payload_buf_ = {nullptr, nullptr};
Completer<WriteResult>* completer_;
enum {
kStateIdle,
kStateCancelling,
kStateSendingHeader,
kStateSendingPayload,
kStateSendingTrailer
} state_ = kStateIdle;
};
class PacketUnwrapper : public AsyncStreamSource, Completer<ReadResult> {
public:
PacketUnwrapper(AsyncStreamSource* rx_channel)
: rx_channel_(rx_channel) {}
void start_read(bufptr_t buffer, TransferHandle* handle, Completer<ReadResult>& completer) final;
void cancel_read(TransferHandle transfer_handle) final;
private:
void complete(ReadResult result);
AsyncStreamSource* rx_channel_;
TransferHandle inner_transfer_handle_;
uint8_t rx_buf_[3];
uint8_t* expected_rx_end_;
size_t payload_length_ = 0;
bufptr_t payload_buf_ = {nullptr, nullptr};
Completer<ReadResult>* completer_;
enum {
kStateIdle,
kStateCancelling,
kStateReceivingHeader,
kStateReceivingPayload,
kStateReceivingTrailer
} state_ = kStateIdle;
};
struct LegacyProtocolPacketBased : ReadCompleter, WriteCompleter {
public:
LegacyProtocolPacketBased(AsyncStreamSource* rx_channel, AsyncStreamSink* tx_channel, size_t tx_mtu)
: rx_channel_(rx_channel), tx_channel_(tx_channel), tx_mtu_(std::min(tx_mtu, sizeof(tx_buf_))) {}
AsyncStreamSource* rx_channel_ = nullptr;
AsyncStreamSink* tx_channel_ = nullptr;
size_t tx_mtu_;
uint8_t tx_buf_[128];
uint8_t rx_buf_[128];
TransferHandle tx_handle_ = 0; // non-zero while a TX operation is in progress
uint8_t* rx_end_ = nullptr; // non-zero if an RX operation has finished but wasn't handled yet because the TX channel was busy
Completer<LegacyProtocolPacketBased*, StreamStatus>* on_stopped_ = nullptr;
#ifdef FIBRE_ENABLE_CLIENT
void start_endpoint_operation(uint16_t endpoint_id, cbufptr_t tx_buf, bufptr_t rx_buf, EndpointOperationHandle* handle, Completer<EndpointOperationResult>& completer);
void cancel_endpoint_operation(EndpointOperationHandle handle);
LegacyObjectClient client_{this};
#endif
#ifdef FIBRE_ENABLE_CLIENT
void start(Completer<LegacyObjectClient*, std::shared_ptr<LegacyObject>>& on_found_root_object, Completer<LegacyObjectClient*>& on_lost_root_object, Completer<LegacyProtocolPacketBased*, StreamStatus>& on_stopped);
#else
void start(Completer<LegacyProtocolPacketBased*, StreamStatus>& on_stopped);
#endif
private:
#ifdef FIBRE_ENABLE_CLIENT
struct EndpointOperation {
uint16_t seqno;
uint16_t endpoint_id;
cbufptr_t tx_buf;
bufptr_t rx_buf;
Completer<EndpointOperationResult>* completer;
};
void start_endpoint_operation(EndpointOperation op);
uint16_t outbound_seq_no_ = 0;
EndpointOperation pending_operation_{.completer = nullptr}; // operation that is waiting for TX
EndpointOperationHandle transmitting_op_ = 0; // operation that is in TX
std::unordered_map<uint16_t, EndpointOperation> expected_acks_; // operations that are waiting for RX
#endif
void on_write_finished(WriteResult result);
void on_read_finished(ReadResult result);
void on_closed(StreamStatus status);
};
struct LegacyProtocolStreamBased {
public:
LegacyProtocolStreamBased(AsyncStreamSource* rx_channel, AsyncStreamSink* tx_channel)
: unwrapper_(rx_channel), wrapper_(tx_channel) {}
#ifdef FIBRE_ENABLE_CLIENT
void start(Completer<LegacyObjectClient*, std::shared_ptr<LegacyObject>>& on_found_root_object, Completer<LegacyObjectClient*>& on_lost_root_object, Completer<LegacyProtocolPacketBased*, StreamStatus>& on_stopped) {
inner_protocol_.start(on_found_root_object, on_lost_root_object, on_stopped);
}
#else
void start(Completer<LegacyProtocolPacketBased*, StreamStatus>& on_stopped) { inner_protocol_.start(on_stopped); }
#endif
private:
PacketUnwrapper unwrapper_;
PacketWrapper wrapper_;
LegacyProtocolPacketBased inner_protocol_{&unwrapper_, &wrapper_, 127};
};
}
#endif // __FIBRE_LEGACY_PROTOCOL_HPP
Firmware/fibre/cpp/libfibre-macos-x86.dylib
LFS
BIN
View File
Binary file not shown.
Firmware/fibre/cpp/libfibre.cpp
+511
View File
File diff suppressed because it is too large Load Diff
Firmware/fibre/cpp/logging.cpp
+20
View File
@@ -0,0 +1,20 @@
#include "logging.hpp"
#if !defined(_WIN32) && !defined(_WIN64) && !defined(__linux__) && !defined(__APPLE__)
namespace std {
StdoutStream cerr;
}
#endif
namespace fibre {
Logger logger{};
Logger* get_logger() {
return &logger;
}
}
Firmware/fibre/cpp/logging.hpp
+339
View File
@@ -0,0 +1,339 @@
/**
* @brief Provides logging facilities
*
* Log entries are associated with user defined topics. A user can define a
* topic using DEFINE_LOG_TOPIC(topicname) and activate the topic for the
* current scope using USE_LOG_TOPIC(topicname).
*
* Currently all log entries are posted to stderr in a thread-safe way.
*
* Whether an event is actually logged depends on the current log verbosity of
* the corresponding topic. The log verbosity is defined by the following
* sources (in order of their precedence).
*
* 0. maximum log verbosity setting (see below)
* 1. runtime environment variable "FIBRE_LOG_[topicname]"
* 2. runtime environment variable "FIBRE_LOG"
* 3. topic specific default log verbosity defined using CONFIG_LOG_TOPIC(...)
* 4. FIBRE_DEFAULT_LOG_VERBOSITY defined before this file (using #define or -D compiler flag)
* 5. FIBRE_DEFAULT_LOG_VERBOSITY defined in this file
*
* The maximum log verbosity can be defined separately from the default log
* verbosity. The maximum log verbosity bound always applies, regardless of how
* the actual log verbosity is specified. This allows keeping the binary small
* by optimizing away unnecessary log entries at compile time.
* The maximum log verbosity is defined by the following sources (in order of
* their precedence):
*
* 1. topic specific max log verbosity defined using CONFIG_LOG_TOPIC(...)
* 2. FIBRE_MAX_LOG_VERBOSITY defined before this file (using #define or -D compiler flag)
* 3. FIBRE_MAX_LOG_VERBOSITY defined in this file
*
* TODO: ensure that the optimizer can indeed strip the unused strings (currently not the case)
*
*
* Example:
*
* @code
*
* DEFINE_LOG_TOPIC(MAIN);
* USE_LOG_TOPIC(MAIN);
*
* int main(void) {
* FIBRE_LOG(D) << "Hello Log!";
* if (open("inexistent_file", O_RDONLY) < 0) {
* FIBRE_LOG(E) << "Could not open file: " << sys_err();
* }
* return 0;
* }
*
* @endcode
*
* Using logging in header files is possible but undocumented (TODO: fix)
*/
#ifndef __FIBRE_LOGGING_HPP
#define __FIBRE_LOGGING_HPP
// TODO: support lite-version of logging on embedded systems
#if defined(_WIN32) || defined(_WIN64) || defined(__linux__) || defined(__APPLE__)
#include <fibre/cpp_utils.hpp>
#include <string.h>
#include <chrono>
#if defined(_WIN32) || defined(_WIN64)
#include "windows.h"
#endif
#if defined(_WIN32) || defined(_WIN64) || defined(__linux__) || defined(__APPLE__)
#include <iostream>
#include <iomanip>
#else
// We don't want <iostream> included on an embedded system as it makes the
// binary huge.
struct StdoutStream : std::ostream {
void operator <<(const char * str) {
printf("%s", str);
}
};
namespace std {
extern StdoutStream cerr;
}
#endif
#if defined(_WIN32) || defined(_WIN64) || defined(__linux__)
#include <mutex>
using TMutex = std::mutex;
using TLock = std::unique_lock<TMutex>;
#else
using TMutex = int;
struct TLock {
TLock() {}
TLock(TMutex) {}
};
#endif
namespace fibre {
// Maximum log verbosity that should be compiled into the binary.
// Log entries with a higher verbosity should be optimized away.
#ifndef FIBRE_MAX_LOG_VERBOSITY
# define FIBRE_MAX_LOG_VERBOSITY LOG_LEVEL_T
#endif
// Default log verbosity that should be used for all topic. This may be
// overridden by other sources, see description in the beginning of this file.
#ifndef FIBRE_DEFAULT_LOG_VERBOSITY
# define FIBRE_DEFAULT_LOG_VERBOSITY LOG_LEVEL_W
#endif
/**
* @brief Generates one log entry.
*
* The log entry will be associates with the topic specified in "USE_LOG_TOPIC".
*
* Note that the log entry must only be used in the statement it is generated. (TODO: fix)
*
* @param level: Can be one of "E", "W", "D", or other levels defined in
* log_level_t.
* @returns a stream for writing into the log entry
*/
#define FIBRE_LOG(level) \
fibre::make_log_entry<current_log_topic, fibre::LOG_LEVEL_ ## level>( \
fibre::get_file_name(MAKE_SSTRING(__FILE__){}), __LINE__, __func__ \
).get_stream()
/**
* @brief Defines a log topic. A log topic must be defined exactly once in every
* translation unit it is used.
*/
#define DEFINE_LOG_TOPIC(name) \
struct LOG_TOPIC_ ## name { \
static const char * get_label() { \
static const char label[] = #name; \
return label; \
} \
}
/**
* @brief Activates the use of the specified log topic for the current scope
* (and all subscopes)
*/
#define USE_LOG_TOPIC(name) using current_log_topic = LOG_TOPIC_ ## name
/**
* @brief Overrides the general log verbosity settings for a specific topic.
* If used, this should be placed in the same scope as the corresponding
* DEFINE_LOG_TOPIC.
*/
#define CONFIG_LOG_TOPIC(topic, default_verbosity, max_verbosity) \
template<> constexpr log_level_t get_default_log_verbosity<LOG_TOPIC_ ## topic>() { return (default_verbosity); } \
template<> constexpr log_level_t get_max_log_verbosity<LOG_TOPIC_ ## topic>() { return (max_verbosity); }
/** @brief Log verbosity levels */
enum log_level_t {
LOG_LEVEL_F = 0, // fatal
LOG_LEVEL_E = 1, // error
LOG_LEVEL_W = 2, // warning
LOG_LEVEL_I = 3, // info
LOG_LEVEL_D = 4, // debug
LOG_LEVEL_T = 5, // trace
};
class NullBuffer : public std::streambuf {
public:
int overflow(int c) { return c; }
};
// Source: https://stackoverflow.com/questions/15845505/how-to-get-higher-precision-fractions-of-a-second-in-a-printout-of-current-tim
static std::string get_local_time() {
auto now(std::chrono::system_clock::now());
auto seconds_since_epoch(
std::chrono::duration_cast<std::chrono::seconds>(now.time_since_epoch()));
// Construct time_t using 'seconds_since_epoch' rather than 'now' since it is
// implementation-defined whether the value is rounded or truncated.
std::time_t now_t(
std::chrono::system_clock::to_time_t(
std::chrono::system_clock::time_point(seconds_since_epoch)));
char temp[10];
if (!std::strftime(temp, 10, "%H:%M:%S.", std::localtime(&now_t)))
return "";
return std::string(temp) +
std::to_string((now.time_since_epoch() - seconds_since_epoch).count());
}
class Logger {
public:
class Entry {
public:
Entry() : base_stream_(null_stream), lock_() {}
Entry(std::ostream& base_stream, log_level_t level, const char* topic, const char* filename, size_t line_no, const char *funcname, TMutex& mutex)
: base_stream_(base_stream), lock_(mutex)
{
switch (level) {
case LOG_LEVEL_W:
base_stream << "\x1b[93;1m";
break;
case LOG_LEVEL_E:
case LOG_LEVEL_F:
base_stream << "\x1b[91;1m";
break;
default:
break;
}
base_stream << get_local_time() << " ";
base_stream << std::dec << "[" << topic << "] ";
//base_stream << std::dec << filename << ":" << line_no << " in " << funcname << "(): ";
}
~Entry() { get_stream() << "\x1b[0m" << std::endl; }
std::ostream& get_stream() { return base_stream_; };
private:
NullBuffer null_buffer{};
std::ostream null_stream{&null_buffer};
std::ostream& base_stream_;
TLock lock_;
};
TMutex mutex_;
};
template<typename TOPIC>
constexpr log_level_t get_default_log_verbosity() { return FIBRE_DEFAULT_LOG_VERBOSITY; }
template<typename TOPIC>
constexpr log_level_t get_max_log_verbosity() { return FIBRE_MAX_LOG_VERBOSITY; }
/**
* @brief Resolves the currently active log verbosity for the given topic.
* See top of this file for a detailed description of the algorithm.
*/
template<typename TOPIC>
log_level_t get_current_log_verbosity() {
char var_name[sizeof("FIBRE_LOG_") + strlen(TOPIC::get_label())];
strcpy(var_name, "FIBRE_LOG_");
strcat(var_name, TOPIC::get_label());
// TODO: provide a way to disable the
const char * var_val = std::getenv(var_name);
if (!var_val) {
var_val = std::getenv("FIBRE_LOG");
}
log_level_t log_level = get_default_log_verbosity<TOPIC>();
if (var_val) {
unsigned long num = strtoul(var_val, nullptr, 10);
log_level = (log_level_t)num;
}
if (log_level > get_max_log_verbosity<TOPIC>()) {
log_level = get_max_log_verbosity<TOPIC>();
}
return log_level;
}
/*
template<typename TStream, typename T, typename... Ts>
void send_to_stream(TStream&& stream);
template<typename TStream>
void send_to_stream(TStream&& stream) { }
template<typename TStream, typename T, typename... Ts>
void send_to_stream(TStream&& stream, T&& value, Ts&&... values) {
send_to_stream(std::forward<TStream>(stream) << std::forward<T>(value), std::forward<Ts>(values)...);
}*/
Logger* get_logger(); // defined in logging.cpp
template<typename TOPIC, log_level_t LEVEL>
Logger::Entry make_log_entry(const char *filename, size_t line_no, const char *funcname) {
if (get_current_log_verbosity<TOPIC>() < LEVEL) {
return {};
} else {
Logger* logger = get_logger();
return { std::cerr, LEVEL, TOPIC::get_label(), filename, line_no, funcname, logger->mutex_ };
}
}
template<typename TFilepath>
constexpr const char * get_file_name(TFilepath file_path) {
return (file_path /*file_path.after_last_index_of('/')*/).c_str(); // TODO: extract file name (without path)
}
}
/**
* @brief Tag type to print the last system error
*
* The statement `std::out << sys_err();` will print the last system error
* in the following format: "error description (errno)".
* This is based on `GetLastError()` (Windows) or `errno` (all other systems).
*/
struct sys_err {};
namespace std {
static inline std::ostream& operator<<(std::ostream& stream, const sys_err&) {
#if defined(_WIN32) || defined(_WIN64)
auto error_code = GetLastError();
#else
auto error_code = errno;
#endif
return stream << strerror(error_code) << " (" << error_code << ")";
}
}
#else
#define DEFINE_LOG_TOPIC(topic)
#define USE_LOG_TOPIC(topic)
struct NullStream {
template<typename T> NullStream& operator<<(T val) { return *this; }
};
#define FIBRE_LOG(level) NullStream()
#endif
#endif // __FIBRE_LOGGING_HPP
Firmware/fibre/cpp/platform_support/libusb_transport.cpp
+660
View File
File diff suppressed because it is too large Load Diff
Firmware/fibre/cpp/platform_support/libusb_transport.hpp
+124
View File
@@ -0,0 +1,124 @@
#ifndef __FIBRE_USB_DISCOVERER_HPP
#define __FIBRE_USB_DISCOVERER_HPP
#include "../event_loop.hpp"
#include "../async_stream.hpp"
#include <fibre/libfibre.h>
#include <libusb.h>
#include <thread>
#include <vector>
#include <unordered_map>
namespace fibre {
class LibusbBulkInEndpoint;
class LibusbBulkOutEndpoint;
struct ChannelDiscoveryResult {
FibreStatus status;
AsyncStreamSource* rx_channel;
AsyncStreamSink* tx_channel;
};
class FIBRE_PRIVATE LibusbDiscoverer {
public:
struct InterfaceSpecs {
int bus = -1; // -1 to ignore
int address = -1; // -1 to ignore
int vendor_id = -1; // -1 to ignore
int product_id = -1; // -1 to ignore
int interface_class = -1; // -1 to ignore
int interface_subclass = -1; // -1 to ignore
int interface_protocol = -1; // -1 to ignore
};
struct ChannelDiscoveryContext {
InterfaceSpecs interface_specs;
Completer<ChannelDiscoveryResult>* on_found_channels;
};
int init(EventLoop* event_loop);
int deinit() { return deinit(INT_MAX); }
void start_channel_discovery(const char* specs, size_t specs_len, ChannelDiscoveryContext** handle, Completer<ChannelDiscoveryResult>& on_found_channels);
int stop_channel_discovery(ChannelDiscoveryContext* handle);
private:
struct Device {
struct libusb_device* dev;
struct libusb_device_handle* handle;
std::vector<LibusbBulkInEndpoint*> ep_in;
std::vector<LibusbBulkOutEndpoint*> ep_out;
};
int deinit(int stage);
void internal_event_loop();
void on_event_loop_iteration();
void on_add_pollfd(int fd, short events);
void on_remove_pollfd(int fd);
int on_hotplug(struct libusb_device *dev, libusb_hotplug_event event);
void poll_devices_now();
void consider_device(struct libusb_device *device, ChannelDiscoveryContext* subscription);
EventLoop* event_loop_ = nullptr;
bool using_sparate_libusb_thread_; // true on Windows. Initialized in init()
libusb_context *libusb_ctx_ = nullptr; // libusb session
libusb_hotplug_callback_handle hotplug_callback_handle_ = 0;
bool run_internal_event_loop_ = false;
std::thread* internal_event_loop_thread_;
EventLoopTimer* device_polling_timer_;
EventLoopTimer* event_loop_timer_ = nullptr;
std::unordered_map<uint16_t, Device> known_devices_; // key: bus_number << 8 | dev_number
std::vector<ChannelDiscoveryContext*> subscriptions_;
};
template<typename TRes>
class FIBRE_PRIVATE LibusbBulkEndpoint {
public:
bool init(EventLoop* event_loop, struct libusb_device_handle* handle, uint8_t endpoint_id);
bool deinit();
protected:
void start_transfer(bufptr_t buffer, TransferHandle* handle, Completer<TRes>& completer);
void cancel_transfer(TransferHandle transfer_handle);
private:
void submit_transfer();
void on_transfer_finished();
EventLoop* event_loop_ = nullptr; // only non-null on Windows where we use a separate libusb thread
struct libusb_device_handle* handle_ = nullptr;
uint8_t endpoint_id_ = 0;
struct libusb_transfer* transfer_ = nullptr;
Completer<TRes>* completer_ = nullptr;
};
class FIBRE_PRIVATE LibusbBulkInEndpoint : public LibusbBulkEndpoint<ReadResult>, public AsyncStreamSource {
public:
void start_read(bufptr_t buffer, TransferHandle* handle, Completer<ReadResult>& completer) final {
start_transfer(buffer, handle, completer);
}
void cancel_read(TransferHandle transfer_handle) final {
cancel_transfer(transfer_handle);
}
};
class FIBRE_PRIVATE LibusbBulkOutEndpoint : public LibusbBulkEndpoint<WriteResult>, public AsyncStreamSink {
public:
void start_write(cbufptr_t buffer, TransferHandle* handle, Completer<WriteResult>& completer) final {
start_transfer({
(unsigned char*)buffer.begin(),
buffer.size()
}, handle, completer);
}
void cancel_write(TransferHandle transfer_handle) final {
cancel_transfer(transfer_handle);
}
};
}
#endif // __FIBRE_USB_DISCOVERER_HPP
Firmware/fibre/cpp/print_utils.hpp
+136
View File
@@ -0,0 +1,136 @@
#ifndef __FIBRE_PRINT_UTILS_HPP
#define __FIBRE_PRINT_UTILS_HPP
#include <string>
#include <ostream>
namespace fibre {
template<typename T>
constexpr size_t hex_digits() {
return (std::numeric_limits<T>::digits + 3) / 4;
}
/* @brief Converts a hexadecimal digit to a uint8_t.
* @param output If not null, the digit's value is stored in this output
* Returns true if the char is a valid hex digit, false otherwise
*/
static bool hex_digit_to_byte(char ch, uint8_t* output) {
uint8_t nil_output = 0;
if (!output)
output = &nil_output;
if (ch >= '0' && ch <= '9')
return (*output) = ch - '0', true;
if (ch >= 'a' && ch <= 'f')
return (*output) = ch - 'a' + 10, true;
if (ch >= 'A' && ch <= 'F')
return (*output) = ch - 'A' + 10, true;
return false;
}
/* @brief Converts a hex string to an integer
* @param output If not null, the result is stored in this output
* Returns true if the string represents a valid hex value, false otherwise.
*/
template<typename TInt>
bool hex_string_to_int(const char * str, size_t length, TInt* output) {
constexpr size_t N_DIGITS = hex_digits<TInt>();
TInt result = 0;
if (length > N_DIGITS)
length = N_DIGITS;
for (size_t i = 0; i < length && str[i]; i++) {
uint8_t digit = 0;
if (!hex_digit_to_byte(str[i], &digit))
return false;
result <<= 4;
result += digit;
}
if (output)
*output = result;
return true;
}
template<typename TInt>
bool hex_string_to_int(const char * str, TInt* output) {
return hex_string_to_int<TInt>(str, hex_digits<TInt>(), output);
}
template<typename TInt, size_t ICount>
bool hex_string_to_int_arr(const char * str, size_t length, TInt (&output)[ICount]) {
for (size_t i = 0; i < ICount; i++) {
if (!hex_string_to_int<TInt>(&str[i * hex_digits<TInt>()], &output[i]))
return false;
}
return true;
}
template<typename TInt, size_t ICount>
bool hex_string_to_int_arr(const char * str, TInt (&output)[ICount]) {
return hex_string_to_int_arr(str, hex_digits<TInt>() * ICount, output);
}
// TODO: move to print_utils.hpp
template<typename T>
class HexPrinter {
public:
HexPrinter(T val, bool prefix) : val_(val) /*, prefix_(prefix)*/ {
const char digits[] = "0123456789abcdef";
size_t prefix_length = prefix ? 2 : 0;
if (prefix) {
str[0] = '0';
str[1] = 'x';
}
str[prefix_length + hex_digits<T>()] = '\0';
for (size_t i = 0; i < hex_digits<T>(); ++i) {
str[prefix_length + hex_digits<T>() - i - 1] = digits[val & 0xf];
val >>= 4;
}
}
std::string to_string() const { return str; }
void to_string(char* buf) const {
for (size_t i = 0; (i < sizeof(str)) && str[i]; ++i)
buf[i] = str[i];
}
T val_;
//bool prefix_;
char str[hex_digits<T>() + 3]; // 3 additional characters 0x and \0
};
template<typename T>
std::ostream& operator<<(std::ostream& stream, const HexPrinter<T>& printer) {
// TODO: specialize for char
return stream << printer.to_string();
}
template<typename T>
HexPrinter<T> as_hex(T val, bool prefix = true) { return HexPrinter<T>(val, prefix); }
template<typename T>
class HexArrayPrinter {
public:
HexArrayPrinter(T* ptr, size_t length) : ptr_(ptr), length_(length) {}
T* ptr_;
size_t length_;
};
template<typename TStream, typename T>
TStream& operator<<(TStream& stream, const HexArrayPrinter<T>& printer) {
for (size_t pos = 0; pos < printer.length_; ++pos) {
stream << " " << as_hex(printer.ptr_[pos]);
if (((pos + 1) % 16) == 0)
stream << "\n";
}
return stream;
}
template<typename T, size_t ILength>
HexArrayPrinter<T> as_hex(T (&val)[ILength]) { return HexArrayPrinter<T>(val, ILength); }
template<typename T>
HexArrayPrinter<T> as_hex(generic_bufptr_t<T> buffer) { return HexArrayPrinter<T>(buffer.begin(), buffer.size()); }
}
#endif // __FIBRE_PRINT_UTILS_HPP
Firmware/fibre/cpp/protocol.cpp
-187
View File
@@ -1,187 +0,0 @@
/* Includes ------------------------------------------------------------------*/
#include <memory>
#include <stdlib.h>
#include <fibre/protocol.hpp>
#include <fibre/crc.hpp>
/* Private defines -----------------------------------------------------------*/
/* Private macros ------------------------------------------------------------*/
/* Private typedef -----------------------------------------------------------*/
/* Global constant data ------------------------------------------------------*/
/* Global variables ----------------------------------------------------------*/
/* Private constant data -----------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
/* Private function prototypes -----------------------------------------------*/
static void hexdump(const uint8_t* buf, size_t len);
/* Function implementations --------------------------------------------------*/
#if 0
void hexdump(const uint8_t* buf, size_t len) {
for (size_t pos = 0; pos < len; ++pos) {
printf(" %02x", buf[pos]);
if ((((pos + 1) % 16) == 0) || ((pos + 1) == len))
printf("\r\n");
osDelay(2);
}
}
#else
void hexdump(const uint8_t* buf, size_t len) {
(void) buf;
(void) len;
}
#endif
int StreamToPacketSegmenter::process_bytes(const uint8_t *buffer, size_t length, size_t* processed_bytes) {
int result = 0;
while (length--) {
if (header_index_ < sizeof(header_buffer_)) {
// Process header byte
header_buffer_[header_index_++] = *buffer;
if (header_index_ == 1 && header_buffer_[0] != CANONICAL_PREFIX) {
header_index_ = 0;
} else if (header_index_ == 2 && (header_buffer_[1] & 0x80)) {
header_index_ = 0; // TODO: support packets larger than 128 bytes
} else if (header_index_ == 3 && calc_crc8<CANONICAL_CRC8_POLYNOMIAL>(CANONICAL_CRC8_INIT, header_buffer_, 3)) {
header_index_ = 0;
} else if (header_index_ == 3) {
packet_length_ = header_buffer_[1] + 2;
}
} else if (packet_index_ < sizeof(packet_buffer_)) {
// Process payload byte
packet_buffer_[packet_index_++] = *buffer;
}
// If both header and packet are fully received, hand it on to the packet processor
if (header_index_ == 3 && packet_index_ == packet_length_) {
if (calc_crc16<CANONICAL_CRC16_POLYNOMIAL>(CANONICAL_CRC16_INIT, packet_buffer_, packet_length_) == 0) {
result |= output_.process_packet(packet_buffer_, packet_length_ - 2);
}
header_index_ = packet_index_ = packet_length_ = 0;
}
buffer++;
if (processed_bytes)
(*processed_bytes)++;
}
return result;
}
int StreamBasedPacketSink::process_packet(const uint8_t *buffer, size_t length) {
// TODO: support buffer size >= 128
if (length >= 128)
return -1;
LOG_FIBRE("send header\r\n");
uint8_t header[] = {
CANONICAL_PREFIX,
static_cast<uint8_t>(length),
0
};
header[2] = calc_crc8<CANONICAL_CRC8_POLYNOMIAL>(CANONICAL_CRC8_INIT, header, 2);
if (output_.process_bytes(header, sizeof(header), nullptr))
return -1;
LOG_FIBRE("send payload:\r\n");
hexdump(buffer, length);
if (output_.process_bytes(buffer, length, nullptr))
return -1;
LOG_FIBRE("send crc16\r\n");
uint16_t crc16 = calc_crc16<CANONICAL_CRC16_POLYNOMIAL>(CANONICAL_CRC16_INIT, buffer, length);
uint8_t crc16_buffer[] = {
(uint8_t)((crc16 >> 8) & 0xff),
(uint8_t)((crc16 >> 0) & 0xff)
};
if (output_.process_bytes(crc16_buffer, 2, nullptr))
return -1;
LOG_FIBRE("sent!\r\n");
return 0;
}
// Returns part of the JSON interface definition.
bool fibre::endpoint0_handler(fibre::cbufptr_t* input_buffer, fibre::bufptr_t* output_buffer) {
// The request must contain a 32 bit integer to specify an offset
std::optional<uint32_t> offset = read_le<uint32_t>(input_buffer);
if (!offset.has_value()) {
// Didn't receive any offset
return false;
} else if (offset.value() == 0xffffffff) {
// If the offset is special value 0xFFFFFFFF, send back the JSON version ID instead
return write_le<uint32_t>(json_version_id_, output_buffer);
} else if (offset.value() >= embedded_json_length) {
// Attempt to read beyond the buffer end - return empty response
return true;
} else {
// Return part of the json file
size_t n_copy = std::min(output_buffer->size(), embedded_json_length - (size_t)offset.value());
memcpy(output_buffer->begin(), embedded_json + offset.value(), n_copy);
*output_buffer = output_buffer->skip(n_copy);
return true;
}
}
int BidirectionalPacketBasedChannel::process_packet(const uint8_t* buffer, size_t length) {
LOG_FIBRE("got packet of length %d: \r\n", length);
hexdump(buffer, length);
if (length < 4)
return -1;
uint16_t seq_no = read_le<uint16_t>(&buffer, &length);
if (seq_no & 0x8000) {
// TODO: ack handling
} else {
// TODO: think about some kind of ordering guarantees
// currently the seq_no is just used to associate a response with a request
uint16_t endpoint_id = read_le<uint16_t>(&buffer, &length);
bool expect_response = endpoint_id & 0x8000;
endpoint_id &= 0x7fff;
// Verify packet trailer. The expected trailer value depends on the selected endpoint.
// For endpoint 0 this is just the protocol version, for all other endpoints it's a
// CRC over the entire JSON descriptor tree (this may change in future versions).
uint16_t expected_trailer = endpoint_id ? fibre::json_crc_ : PROTOCOL_VERSION;
uint16_t actual_trailer = buffer[length - 2] | (buffer[length - 1] << 8);
if (expected_trailer != actual_trailer) {
LOG_FIBRE("trailer mismatch for endpoint %d: expected %04x, got %04x\r\n", endpoint_id, expected_trailer, actual_trailer);
return -1;
}
LOG_FIBRE("trailer ok for endpoint %d\r\n", endpoint_id);
// TODO: if more bytes than the MTU were requested, should we abort or just return as much as possible?
uint16_t expected_response_length = read_le<uint16_t>(&buffer, &length);
// Limit response length according to our local TX buffer size
if (expected_response_length > sizeof(tx_buf_) - 2)
expected_response_length = sizeof(tx_buf_) - 2;
fibre::cbufptr_t input_buffer{buffer, length - 2};
fibre::bufptr_t output_buffer{tx_buf_ + 2, expected_response_length};
fibre::endpoint_handler(endpoint_id, &input_buffer, &output_buffer);
// Send response
if (expect_response) {
size_t actual_response_length = expected_response_length - output_buffer.size() + 2;
write_le<uint16_t>(seq_no | 0x8000, tx_buf_);
LOG_FIBRE("send packet:\r\n");
hexdump(tx_buf_, actual_response_length);
output_.process_packet(tx_buf_, actual_response_length);
}
}
return 0;
}
Firmware/fibre/cpp/stream_utils.hpp
+177
View File
@@ -0,0 +1,177 @@
#ifndef __FIBRE_STREAM_UTILS_HPP
#define __FIBRE_STREAM_UTILS_HPP
#include "async_stream.hpp"
#include <string.h>
namespace fibre {
template<size_t I>
class BufferedStreamSink : Completer<WriteResult> {
public:
BufferedStreamSink(AsyncStreamSink& sink) : sink_(sink) {}
/**
* @brief Enqueues as much of the specified buffer as possible.
*
* Thread safety: only one write call is allowed at a time. The write call
* can be on a different thread from the underlying stream's event loop.
* (TODO: this is not true yet, see comment in function)
*/
void write(cbufptr_t buf) {
// We subtract 1 from the read index because we never want the write
// pointer to catch up with the read pointer, cause then
// `write_idx_ == read_idx_` could mean both "full" and "empty".
size_t read_idx = (read_idx_ + I - 1) % I; // read_idx_ could change during this function
if (write_idx_ > read_idx) {
size_t n_copy = std::min(I - write_idx_, buf.size());
memcpy(buffer_ + write_idx_, buf.begin(), n_copy);
write_idx_ = (write_idx_ + n_copy) % I;
buf = buf.skip(n_copy);
}
size_t n_copy = std::min(read_idx - write_idx_, buf.size());
memcpy(buffer_ + write_idx_, buf.begin(), n_copy);
write_idx_ = (write_idx_ + n_copy) % I;
//if (!__atomic_exchange_n(&is_active_, true, __ATOMIC_SEQ_CST)) {
// // TODO: calling the sink in here breaks the rule that async
// // functions must only be called on the event loop thread.
// // But to do that we need to implement a proper event loop where we
// // can enqueue calls.
// maybe_start_async_write();
//}
}
void maybe_start_async_write() {
if (is_active_) {
// nothing to do
} else if (read_idx_ < write_idx_) {
is_active_ = true;
sink_.start_write({buffer_ + read_idx_, buffer_ + write_idx_}, &transfer_handle_, *this);
} else if (read_idx_ > write_idx_) {
is_active_ = true;
sink_.start_write({buffer_ + read_idx_, buffer_ + I}, &transfer_handle_, *this);
} else {
// nothing to do
}
}
private:
void complete(WriteResult result) final {
is_active_ = false;
transfer_handle_ = 0;
if (result.status == kStreamOk) {
if (result.end < buffer_ || result.end > (buffer_ + I)) {
for (;;)
transfer_handle_ = 0;
}
read_idx_ = (result.end - buffer_) % I;
maybe_start_async_write();
}
}
uint8_t buffer_[I];
// Both indices are in [0, I)
// They are equal if the buffer is empty (no valid data).
size_t write_idx_ = 0; // [0, I)
size_t read_idx_ = 0; // [0, I)
bool is_active_ = false;
TransferHandle transfer_handle_ = 0;
AsyncStreamSink& sink_;
};
/**
* @brief Buffers up to NSlots concurrent async write requests.
*
* This can be used to wrap sinks that can only handle one concurrent write
* operation at a time but are written to by multiple independent sources.
*/
template<size_t NSlots>
class AsyncStreamSinkMultiplexer : public AsyncStreamSink, Completer<WriteResult> {
public:
AsyncStreamSinkMultiplexer(AsyncStreamSink& sink) : sink_(sink) {}
void start_write(cbufptr_t buffer, TransferHandle* handle, Completer<WriteResult>& completer) final {
for (size_t i = 0; i < NSlots; ++i) {
auto& [slot_in_use, slot_buf, slot_completer] = slots_[i];
if (!__atomic_exchange_n(&slot_in_use, true, __ATOMIC_SEQ_CST)) {
slot_buf = buffer;
slot_completer = &completer;
if (handle) {
*handle = i + 1; // returning a valid handle of 0 is not a good idea
}
// If the underlying sink wasn't busy, start it now.
if (active_slot_ == 0) {
active_slot_ = i + 1;
sink_.start_write(slot_buf, &transfer_handle_, *this);
}
return;
}
}
if (handle) {
*handle = 0;
}
completer.complete({kStreamError, buffer.begin()});
}
void cancel_write(TransferHandle transfer_handle) final {
if (transfer_handle == active_slot_) {
// This transfer is the one that the underlying sink is busy with.
sink_.cancel_write(transfer_handle_);
} else {
// This transfer is only enqueued but not yet started.
auto& [slot_in_use, slot_buf, slot_completer] = slots_[transfer_handle - 1];
auto completer = slot_completer;
auto end = slot_buf.end();
slot_in_use = false;
safe_complete(completer, {kStreamCancelled, end});
}
}
private:
void complete(fibre::WriteResult result) final {
transfer_handle_ = 0;
auto& [slot_in_use, slot_buf, slot_completer] = slots_[active_slot_ - 1];
auto completer = slot_completer;
slot_in_use = false;
safe_complete(completer, result);
// Select new slot before announcing completion of the old
size_t active_slot = 0;
for (size_t i = 0; i < NSlots; ++i) {
auto& [slot_in_use, slot_buf, slot_completer] = slots_[i];
if (slot_in_use) {
active_slot = i + 1;
break;
}
}
// Start next slot
active_slot_ = active_slot;
if (active_slot) {
auto& [slot_in_use, slot_buf, slot_completer] = slots_[active_slot - 1];
sink_.start_write(slot_buf, &transfer_handle_, *this);
}
}
AsyncStreamSink& sink_;
std::tuple<bool, cbufptr_t, Completer<WriteResult>*> slots_[NSlots];
size_t active_slot_ = 0;
TransferHandle transfer_handle_ = 0;
};
}
#endif // __FIBRE_STREAM_UTILS_HPP
Firmware/fibre/python/fibre/__init__.py
+1 -2
View File
@@ -1,5 +1,4 @@
from .discovery import find_any, find_all
from .utils import Event, Logger, TimeoutError
from .protocol import ChannelBrokenException, ChannelDamagedException
from .shell import launch_shell
from .libfibre import find_all, find_any, ObjectLostError
Firmware/fibre/python/fibre/discovery.py
-179
View File
@@ -1,179 +0,0 @@
"""
Provides functions for the discovery of Fibre nodes
"""
import sys
import json
import time
import threading
import traceback
import struct
import fibre.protocol
import fibre.utils
import fibre.remote_object
from fibre.utils import Event, Logger
from fibre.protocol import ChannelBrokenException, TimeoutError
import appdirs
import os
# Load all installed transport layers
channel_types = {}
try:
import fibre.usbbulk_transport
channel_types['usb'] = fibre.usbbulk_transport.discover_channels
except ImportError:
pass
try:
import fibre.serial_transport
channel_types['serial'] = fibre.serial_transport.discover_channels
except ImportError:
pass
try:
import fibre.tcp_transport
channel_types['tcp'] = fibre.tcp_transport.discover_channels
except ImportError:
pass
try:
import fibre.udp_transport
channel_types['udp'] = fibre.udp_transport.discover_channels
except ImportError:
pass
def noprint(text):
pass
def find_all(path, serial_number,
did_discover_object_callback,
search_cancellation_token,
channel_termination_token,
logger):
"""
Starts scanning for Fibre nodes that match the specified path spec and calls
the callback for each Fibre node that is found.
This function is non-blocking.
"""
def did_discover_channel(channel):
"""
Inits an object from a given channel and then calls did_discover_object_callback
with the created object
This queries the endpoint 0 on that channel to gain information
about the interface, which is then used to init the corresponding object.
"""
try:
logger.debug("Connecting to device on " + channel._name)
cache_dir = appdirs.user_cache_dir("odrivetool")
cache_path = None
# Fetch the json version tag to check cache (only supported on firmware v0.5 or later)
try:
json_version_tag = channel.remote_endpoint_operation(0, struct.pack("<I", 0xffffffff), True, 4)
json_version_tag = struct.unpack("<I", json_version_tag)[0]
logger.debug("Device reported JSON version ID: {:08d}".format(json_version_tag))
cache_path = os.path.join(cache_dir, 'fibre_schema_cache_{:08d}'.format(json_version_tag))
except:
logger.debug("Failed to get JSON checksum")
# Check cache
json_data = None
try:
if not cache_path is None:
with open(cache_path, 'rb') as fp:
json_crc16 = fibre.protocol.calc_crc16(fibre.protocol.PROTOCOL_VERSION, fp.read())
fp.seek(0)
json_data = json.load(fp)
except:
logger.debug("Failed load JSON cache file {}".format(cache_path))
# Fallback to loading JSON from device
if json_data is None:
# Downloading json data
logger.info("Downloading json data from ODrive... (this might take a while)")
json_bytes = channel.remote_endpoint_read_buffer(0)
try:
json_string = json_bytes.decode("ascii")
except UnicodeDecodeError:
logger.debug("Device responded on endpoint 0 with something that is not ASCII")
raise UnicodeDecodeError
json_crc16 = fibre.protocol.calc_crc16(fibre.protocol.PROTOCOL_VERSION, json_bytes)
json_data = json.loads(json_string)
# Save JSON to cache
if not cache_path is None:
logger.debug("Creating new JSON cache file {}".format(cache_path))
os.makedirs(cache_dir, exist_ok=True)
with open(cache_path, 'w+') as json_cache:
json_cache.write(json_string)
logger.debug("Saved JSON to cache file {}".format(cache_path))
channel._interface_definition_crc = json_crc16
logger.debug("JSON: " + str(json_data).replace("{'name'", "\n{'name'"))
json_data = {"name": "fibre_node", "members": json_data}
obj = fibre.remote_object.RemoteObject(json_data, None, channel, logger)
obj.__dict__['_json_data'] = json_data['members']
obj.__dict__['_json_crc'] = json_crc16
device_serial_number = fibre.utils.get_serial_number_str(obj)
if serial_number != None and device_serial_number != serial_number:
logger.debug("Ignoring device with serial number {}".format(device_serial_number))
return
did_discover_object_callback(obj)
except Exception:
logger.debug("Unexpected exception after discovering channel: " + traceback.format_exc())
# For each connection type, kick off an appropriate discovery loop
for search_spec in path.split(','):
prefix = search_spec.split(':')[0]
the_rest = ':'.join(search_spec.split(':')[1:])
if prefix in channel_types:
t = threading.Thread(target=channel_types[prefix],
args=(the_rest, serial_number, did_discover_channel, search_cancellation_token, channel_termination_token, logger))
t.daemon = True
t.start()
else:
raise Exception("Invalid path spec \"{}\"".format(search_spec))
def find_any(path="usb", serial_number=None,
search_cancellation_token=None, channel_termination_token=None,
timeout=None, logger=Logger(verbose=False), find_multiple=False):
"""
Blocks until the first matching Fibre node is connected and then returns that node
"""
result = []
done_signal = Event(search_cancellation_token)
def did_discover_object(obj):
result.append(obj)
if find_multiple:
if len(result) >= int(find_multiple):
done_signal.set()
else:
done_signal.set()
find_all(path, serial_number, did_discover_object, done_signal, channel_termination_token, logger)
try:
done_signal.wait(timeout=timeout)
except TimeoutError:
if not find_multiple:
return None
finally:
done_signal.set() # terminate find_all
if find_multiple:
return result
else:
return result[0] if len(result) > 0 else None
Firmware/fibre/python/fibre/libfibre.py
+678
View File
File diff suppressed because it is too large Load Diff
Firmware/fibre/python/fibre/protocol.py
+3 -3
View File
@@ -74,7 +74,7 @@ class ChannelDamagedException(Exception):
"""
pass
class ChannelBrokenException(Exception):
class ObjectLostError(Exception):
"""
Raised when the channel is permanently broken
"""
@@ -305,13 +305,13 @@ class Channel(PacketSink):
# Wait for ACK until the resend timeout is exceeded
try:
if wait_any(self._resend_timeout, ack_event, self._channel_broken) != 0:
raise ChannelBrokenException()
raise ObjectLostError()
except TimeoutError:
attempt += 1
continue # resend
return self._responses.pop(seq_no)
# TODO: record channel statistics
raise ChannelBrokenException() # Too many resend attempts
raise ObjectLostError() # Too many resend attempts
finally:
self._expected_acks.pop(seq_no)
self._responses.pop(seq_no, None)
Firmware/fibre/python/fibre/remote_object.py
-264
View File
@@ -1,264 +0,0 @@
"""
Provides functions for the discovery of Fibre nodes
"""
import sys
import json
import struct
import threading
import fibre.protocol
class ObjectDefinitionError(Exception):
pass
codecs = {}
class StructCodec():
"""
Generic serializer/deserializer based on struct pack
"""
def __init__(self, struct_format, target_type):
self._struct_format = struct_format
self._target_type = target_type
def get_length(self):
return struct.calcsize(self._struct_format)
def serialize(self, value):
value = self._target_type(value)
return struct.pack(self._struct_format, value)
def deserialize(self, buffer):
value = struct.unpack(self._struct_format, buffer)
value = value[0] if len(value) == 1 else value
return self._target_type(value)
class RemoteProperty():
"""
Used internally by dynamically created objects to translate
property assignments and fetches into endpoint operations on the
object's associated channel
"""
def __init__(self, json_data, parent):
self._parent = parent
self.__channel__ = parent.__channel__
id_str = json_data.get("id", None)
if id_str is None:
raise ObjectDefinitionError("unspecified endpoint ID")
self._id = int(id_str)
self._name = json_data.get("name", None)
if self._name is None:
self._name = "[anonymous]"
type_str = json_data.get("type", None)
if type_str is None:
raise ObjectDefinitionError("unspecified type")
# Find all codecs that match the type_str and build a dictionary
# of the form {type1: codec1, type2: codec2}
eligible_types = {k: v[type_str] for (k,v) in codecs.items() if type_str in v}
if not eligible_types:
raise ObjectDefinitionError("unsupported codec {}".format(type_str))
# TODO: better heuristics to select a matching type (i.e. prefer non lossless)
eligible_types = list(eligible_types.items())
self._property_type = eligible_types[0][0]
self._codec = eligible_types[0][1]
access_mode = json_data.get("access", "r")
self._can_read = 'r' in access_mode
self._can_write = 'w' in access_mode
def get_value(self):
buffer = self._parent.__channel__.remote_endpoint_operation(self._id, None, True, self._codec.get_length())
return self._codec.deserialize(buffer)
def set_value(self, value):
buffer = self._codec.serialize(value)
# TODO: Currenly we wait for an ack here. Settle on the default guarantee.
self._parent.__channel__.remote_endpoint_operation(self._id, buffer, True, 0)
def _dump(self):
if self._name == "serial_number":
# special case: serial number should be displayed in hex (TODO: generalize)
val_str = "{:012X}".format(self.get_value())
elif self._name == "error":
# special case: errors should be displayed in hex (TODO: generalize)
val_str = "0x{:04X}".format(self.get_value())
else:
val_str = str(self.get_value())
return "{} = {} ({})".format(self._name, val_str, self._property_type.__name__)
class EndpointRefCodec():
"""
Serializer/deserializer for an endpoint reference
"""
def get_length(self):
return struct.calcsize("<HH")
def serialize(self, value):
if value is None:
(ep_id, ep_crc) = (0, 0)
elif isinstance(value, RemoteProperty):
(ep_id, ep_crc) = (value._id, value.__channel__._interface_definition_crc)
else:
raise TypeError("Expected value of type RemoteProperty or None but got '{}'. En example for a RemoteProperty is this expression: odrv0.axis0.controller._remote_attributes['pos_setpoint']".format(type(value).__name__))
return struct.pack("<HH", ep_id, ep_crc)
def deserialize(self, buffer):
return struct.unpack("<HH", buffer)
codecs[int] = {
'int8': StructCodec("<b", int),
'uint8': StructCodec("<B", int),
'int16': StructCodec("<h", int),
'uint16': StructCodec("<H", int),
'int32': StructCodec("<i", int),
'uint32': StructCodec("<I", int),
'int64': StructCodec("<q", int),
'uint64': StructCodec("<Q", int)
}
codecs[bool] = {
'bool': StructCodec("<?", bool)
}
codecs[float] = {
'float': StructCodec("<f", float)
}
codecs[RemoteProperty] = {
'endpoint_ref': EndpointRefCodec()
}
class RemoteFunction(object):
"""
Represents a callable function that maps to a function call on a remote object
"""
def __init__(self, json_data, parent):
self._parent = parent
id_str = json_data.get("id", None)
if id_str is None:
raise ObjectDefinitionError("unspecified endpoint ID")
self._trigger_id = int(id_str)
self._name = json_data.get("name", None)
if self._name is None:
self._name = "[anonymous]"
self._inputs = []
for param_json in json_data.get("arguments", []) + json_data.get("inputs", []): # TODO: deprecate "arguments" keyword
param_json["mode"] = "r"
self._inputs.append(RemoteProperty(param_json, parent))
self._outputs = []
for param_json in json_data.get("outputs", []): # TODO: deprecate "arguments" keyword
param_json["mode"] = "r"
self._outputs.append(RemoteProperty(param_json, parent))
def __call__(self, *args):
if (len(self._inputs) != len(args)):
raise TypeError("expected {} arguments but have {}".format(len(self._inputs), len(args)))
for i in range(len(args)):
self._inputs[i].set_value(args[i])
self._parent.__channel__.remote_endpoint_operation(self._trigger_id, None, True, 0)
if len(self._outputs) > 0:
return self._outputs[0].get_value()
def _dump(self):
return "{}({})".format(self._name, ", ".join("{}: {}".format(x._name, x._property_type.__name__) for x in self._inputs))
class RemoteObject(object):
"""
Object with functions and properties that map to remote endpoints
"""
def __init__(self, json_data, parent, channel, logger):
"""
Creates an object that implements the specified JSON type description by
communicating over the provided channel
"""
# Directly write to __dict__ to avoid calling __setattr__ too early
object.__getattribute__(self, "__dict__")["_remote_attributes"] = {}
object.__getattribute__(self, "__dict__")["__sealed__"] = False
# Assign once more to make linter happy
self._remote_attributes = {}
self.__sealed__ = False
self.__channel__ = channel
self.__parent__ = parent
# Build attribute list from JSON
for member_json in json_data.get("members", []):
member_name = member_json.get("name", None)
if member_name is None:
logger.debug("ignoring unnamed attribute")
continue
try:
type_str = member_json.get("type", None)
if type_str == "object":
attribute = RemoteObject(member_json, self, channel, logger)
elif type_str == "function":
attribute = RemoteFunction(member_json, self)
elif type_str != None:
attribute = RemoteProperty(member_json, self)
else:
raise ObjectDefinitionError("no type information")
except ObjectDefinitionError as ex:
logger.debug("malformed member {}: {}".format(member_name, str(ex)))
continue
self._remote_attributes[member_name] = attribute
self.__dict__[member_name] = attribute
# Ensure that from here on out assignments to undefined attributes
# raise an exception
self.__sealed__ = True
channel._channel_broken.subscribe(self._tear_down)
def _dump(self, indent, depth):
if depth <= 0:
return "..."
lines = []
for key, val in self._remote_attributes.items():
if isinstance(val, RemoteObject):
val_str = indent + key + (": " if depth == 1 else ":\n") + val._dump(indent + " ", depth - 1)
else:
val_str = indent + val._dump()
lines.append(val_str)
return "\n".join(lines)
def __str__(self):
return self._dump("", depth=2)
def __repr__(self):
return self.__str__()
def __getattribute__(self, name):
attr = object.__getattribute__(self, "_remote_attributes").get(name, None)
if isinstance(attr, RemoteProperty):
if attr._can_read:
return attr.get_value()
else:
raise Exception("Cannot read from property {}".format(name))
elif attr != None:
return attr
else:
return object.__getattribute__(self, name)
#raise AttributeError("Attribute {} not found".format(name))
def __setattr__(self, name, value):
attr = object.__getattribute__(self, "_remote_attributes").get(name, None)
if isinstance(attr, RemoteProperty):
if attr._can_write:
attr.set_value(value)
else:
raise Exception("Cannot write to property {}".format(name))
elif not object.__getattribute__(self, "__sealed__") or name in object.__getattribute__(self, "__dict__"):
object.__getattribute__(self, "__dict__")[name] = value
else:
raise AttributeError("Attribute {} not found".format(name))
def _tear_down(self):
# Clear all remote members
for k in self._remote_attributes.keys():
self.__dict__.pop(k)
self._remote_attributes = {}
Firmware/fibre/python/fibre/serial_transport.py
-108
View File
@@ -1,108 +0,0 @@
"""
Provides classes that implement the StreamSource/StreamSink and
PacketSource/PacketSink interfaces for serial ports.
"""
import os
import re
import time
import traceback
import serial
import serial.tools.list_ports
import fibre
from fibre.utils import TimeoutError
# TODO: make this customizable
DEFAULT_BAUDRATE = 115200
class SerialStreamTransport(fibre.protocol.StreamSource, fibre.protocol.StreamSink):
def __init__(self, port, baud):
self._timeout = 1
self._dev = serial.Serial(port, baud, timeout=self._timeout)
def process_bytes(self, bytes):
self._dev.write(bytes)
def get_bytes(self, n_bytes, deadline):
"""
Returns n bytes unless the deadline is reached, in which case the bytes
that were read up to that point are returned. If deadline is None the
function blocks forever. A deadline before the current time corresponds
to non-blocking mode.
"""
# Only set new timeout value if it is reasonably different from the old one (e.g. 20% as below)
# Otherwise it adds significant overhead (at least under Win10) as the port is reset with every reconfiguration
if deadline is None and self._timeout is not None:
self._timeout = None
self._dev.timeout = None
elif deadline is not None:
new_timeout = max(deadline - time.monotonic(), 0)
if abs(new_timeout - self._timeout) > self._timeout * 0.2:
self._timeout = new_timeout
self._dev.timeout = new_timeout
return self._dev.read(n_bytes)
def get_bytes_or_fail(self, n_bytes, deadline):
result = self.get_bytes(n_bytes, deadline)
if len(result) < n_bytes:
raise TimeoutError("expected {} bytes but got only {}", n_bytes, len(result))
return result
def close(self):
self._dev.close()
def find_dev_serial_ports():
try:
return ['/dev/' + x for x in os.listdir('/dev')]
except FileNotFoundError:
return []
def find_pyserial_ports():
return [x.device for x in serial.tools.list_ports.comports()]
def discover_channels(path, serial_number, callback, cancellation_token, channel_termination_token, logger):
"""
Scans for serial ports that match the path spec.
This function blocks until cancellation_token is set.
Channels spawned by this function run until channel_termination_token is set.
"""
if path == None:
# This regex should match all desired port names on macOS,
# Linux and Windows but might match some incorrect port names.
regex = r'^(/dev/tty\.usbmodem.*|/dev/ttyACM.*|COM[0-9]+)$'
else:
regex = "^" + path + "$"
known_devices = []
def device_matcher(port_name):
if port_name in known_devices:
return False
return bool(re.match(regex, port_name))
def did_disconnect(port_name, device):
device.close()
# TODO: yes there is a race condition here in case you wonder.
known_devices.pop(known_devices.index(port_name))
while not cancellation_token.is_set():
all_ports = find_pyserial_ports() + find_dev_serial_ports()
new_ports = filter(device_matcher, all_ports)
for port_name in new_ports:
try:
serial_device = SerialStreamTransport(port_name, DEFAULT_BAUDRATE)
input_stream = fibre.protocol.PacketFromStreamConverter(serial_device)
output_stream = fibre.protocol.StreamBasedPacketSink(serial_device)
channel = fibre.protocol.Channel(
"serial port {}@{}".format(port_name, DEFAULT_BAUDRATE),
input_stream, output_stream, channel_termination_token, logger)
channel.serial_device = serial_device
except serial.serialutil.SerialException:
logger.debug("Serial device init failed. Ignoring this port. More info: " + traceback.format_exc())
known_devices.append(port_name)
else:
known_devices.append(port_name)
channel._channel_broken.subscribe(lambda: did_disconnect(port_name, serial_device))
callback(channel)
time.sleep(1)
Firmware/fibre/python/fibre/shell.py
+27 -7
View File
@@ -4,7 +4,7 @@ import platform
import threading
import fibre
def did_discover_device(device,
async def did_discover_device(device,
interactive_variables, discovered_devices,
branding_short, branding_long,
logger, app_shutdown_token):
@@ -13,7 +13,7 @@ def did_discover_device(device,
message and making the device available to the interactive
console
"""
serial_number = '{:012X}'.format(device.serial_number) if hasattr(device, 'serial_number') else "[unknown serial number]"
serial_number = '{:012X}'.format(await device.serial_number) if hasattr(device, 'serial_number') else "[unknown serial number]"
if serial_number in discovered_devices:
verb = "Reconnected"
index = discovered_devices.index(serial_number)
@@ -29,7 +29,7 @@ def did_discover_device(device,
logger.notify("{} to {} {} as {}".format(verb, branding_long, serial_number, interactive_name))
# Subscribe to disappearance of the device
device.__channel__._channel_broken.subscribe(lambda: did_lose_device(interactive_name, logger, app_shutdown_token))
device._on_lost.add_done_callback(lambda x: did_lose_device(interactive_name, logger, app_shutdown_token))
def did_lose_device(interactive_name, logger, app_shutdown_token):
"""
@@ -39,6 +39,24 @@ def did_lose_device(interactive_name, logger, app_shutdown_token):
if not app_shutdown_token.is_set():
logger.warn("Oh no {} disappeared".format(interactive_name))
def get_user_name(interactive_variables, obj):
queue = [(k, v) for k, v in interactive_variables.items() if isinstance(v, fibre.libfibre.RemoteObject)]
if not isinstance(obj, fibre.libfibre.RemoteObject):
return None
while len(queue):
k, v = queue.pop(0)
if v == obj:
return k
for key in dir(v.__class__):
class_member = getattr(v.__class__, key)
if not key.startswith('_') and isinstance(class_member, fibre.libfibre.RemoteAttribute):
queue.append((k + "." + (key if not class_member._magic_getter else "_" + key + "_property"), class_member._get_obj(v)))
return "anonymous_remote_object_" + str(self._obj_handle)
def launch_shell(args,
interactive_variables,
print_banner, print_help,
@@ -55,6 +73,8 @@ def launch_shell(args,
discovered_devices = []
globals().update(interactive_variables)
fibre.libfibre.get_user_name = lambda obj: get_user_name(interactive_variables, obj)
# Connect to device
logger.debug("Waiting for {}...".format(branding_long))
fibre.find_all(args.path, args.serial_number,
@@ -90,10 +110,10 @@ def launch_shell(args,
console.runcode = console.run_cell
interact = console
# Catch ChannelBrokenException (since disconnect is not always an error)
# Catch ObjectLostError (since disconnect is not always an error)
default_exception_hook = console._showtraceback
def filtered_exception_hook(ex_class, ex, trace):
if(ex_class.__module__+'.'+ex_class.__name__ != 'fibre.protocol.ChannelBrokenException'):
if(ex_class.__module__+'.'+ex_class.__name__ != 'fibre.libfibre.ObjectLostError'):
default_exception_hook(ex_class,ex,trace)
console._showtraceback = filtered_exception_hook
@@ -112,11 +132,11 @@ def launch_shell(args,
console = code.InteractiveConsole(locals=interactive_variables)
interact = lambda: console.interact(banner='')
# Catch ChannelBrokenException (since disconnect is not alway an error)
# Catch ObjectLostError (since disconnect is not alway an error)
console.runcode("import sys")
console.runcode("default_exception_hook = sys.excepthook")
console.runcode("def filtered_exception_hook(ex_class, ex, trace):\n"
" if ex_class.__module__ + '.' + ex_class.__name__ != 'fibre.protocol.ChannelBrokenException':\n"
" if ex_class.__module__ + '.' + ex_class.__name__ != 'fibre.libfibre.ObjectLostError':\n"
" default_exception_hook(ex_class,ex,trace)")
console.runcode("sys.excepthook=filtered_exception_hook")

Some files were not shown because too many files have changed in this diff Show More