diff --git a/.gitignore b/.gitignore index 66a8990f..88c045b3 100644 --- a/.gitignore +++ b/.gitignore @@ -3,9 +3,6 @@ __pycache__/ *.py[cod] *$py.class -# C extensions -*.so - # Unit test / coverage reports htmlcov/ .tox/ diff --git a/Firmware/.gitignore b/Firmware/.gitignore index a4c86dc2..abcbd123 100644 --- a/Firmware/.gitignore +++ b/Firmware/.gitignore @@ -26,3 +26,6 @@ STM32CubeMX #gdb log openocd.log + +# OS-specific files +.DS_Store \ No newline at end of file diff --git a/Firmware/.vscode/c_cpp_properties.json b/Firmware/.vscode/c_cpp_properties.json index 1d5ab7cf..299f69da 100644 --- a/Firmware/.vscode/c_cpp_properties.json +++ b/Firmware/.vscode/c_cpp_properties.json @@ -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__" diff --git a/Firmware/Board/v3/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc/usbd_cdc.h b/Firmware/Board/v3/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc/usbd_cdc.h index c029e22a..5a1cae25 100644 --- a/Firmware/Board/v3/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc/usbd_cdc.h +++ b/Firmware/Board/v3/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc/usbd_cdc.h @@ -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); /** * @} */ diff --git a/Firmware/Board/v3/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c b/Firmware/Board/v3/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c index 7fabf6ee..0427ca8c 100644 --- a/Firmware/Board/v3/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c +++ b/Firmware/Board/v3/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c @@ -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; } diff --git a/Firmware/Board/v3/Src/usbd_cdc_if.c b/Firmware/Board/v3/Src/usbd_cdc_if.c index 067470b4..adc19b02 100644 --- a/Firmware/Board/v3/Src/usbd_cdc_if.c +++ b/Firmware/Board/v3/Src/usbd_cdc_if.c @@ -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; } diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index 27d7be86..1ada1b1c 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -12,9 +12,8 @@ #include 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); diff --git a/Firmware/MotorControl/odrive_main.h b/Firmware/MotorControl/odrive_main.h index a3c33a1d..059a9c02 100644 --- a/Firmware/MotorControl/odrive_main.h +++ b/Firmware/MotorControl/odrive_main.h @@ -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; //::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 -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(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(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(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(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(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(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(this)); +} + +void AsciiProtocol::start() { + TransferHandle dummy; + rx_channel_->start_read(rx_buf_, &dummy, *static_cast(this)); +} diff --git a/Firmware/communication/ascii_protocol.hpp b/Firmware/communication/ascii_protocol.hpp index 145ee5e5..a90623fd 100644 --- a/Firmware/communication/ascii_protocol.hpp +++ b/Firmware/communication/ascii_protocol.hpp @@ -1,22 +1,48 @@ -#ifndef __ASCII_PROTOCOL_H -#define __ASCII_PROTOCOL_H +#ifndef __ASCII_PROTOCOL_HPP +#define __ASCII_PROTOCOL_HPP +#include -/* Includes ------------------------------------------------------------------*/ -#include +#define MAX_LINE_LENGTH ((size_t)256) -#include -#include -#include +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 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 diff --git a/Firmware/communication/communication.cpp b/Firmware/communication/communication.cpp index faac6f59..a2eee963 100644 --- a/Firmware/communication/communication.cpp +++ b/Firmware/communication/communication.cpp @@ -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 + + 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 } diff --git a/Firmware/communication/interface_i2c.cpp b/Firmware/communication/interface_i2c.cpp index d94ed58f..c4df0373 100644 --- a/Firmware/communication/interface_i2c.cpp +++ b/Firmware/communication/interface_i2c.cpp @@ -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); } +*/ diff --git a/Firmware/communication/interface_uart.cpp b/Firmware/communication/interface_uart.cpp index 74fa02c0..c2efe533 100644 --- a/Firmware/communication/interface_uart.cpp +++ b/Firmware/communication/interface_uart.cpp @@ -6,9 +6,12 @@ #include #include +#include +#include #include #include #include +#include #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& completer) final; + void cancel_write(TransferHandle transfer_handle) final; + void did_finish(); + + UART_HandleTypeDef *huart_; + Completer* completer_ = nullptr; + const uint8_t* tx_end_ = nullptr; +}; + +class Stm32UartRxStream : public AsyncStreamSource { +public: + void start_read(bufptr_t buffer, TransferHandle* handle, Completer& completer) final; + void cancel_read(TransferHandle transfer_handle) final; + void did_receive(uint8_t* buffer, size_t length); + + Completer* completer_ = nullptr; + bufptr_t rx_buf_ = {nullptr, nullptr}; +}; + +} + +using namespace fibre; + +void Stm32UartTxStream::start_write(cbufptr_t buffer, TransferHandle* handle, Completer& 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(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(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& completer) { + completer_ = &completer; + rx_buf_ = buffer; + if (handle) { + *handle = reinterpret_cast(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::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); + } } diff --git a/Firmware/communication/interface_uart.h b/Firmware/communication/interface_uart.h index a7df55bd..e649aeb1 100644 --- a/Firmware/communication/interface_uart.h +++ b/Firmware/communication/interface_uart.h @@ -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 +extern fibre::BufferedStreamSink<64> uart0_stdout_sink; +extern bool uart0_stdout_pending; +#endif + #endif // __INTERFACE_UART_HPP diff --git a/Firmware/communication/interface_usb.cpp b/Firmware/communication/interface_usb.cpp index 28171eda..403939f6 100644 --- a/Firmware/communication/interface_usb.cpp +++ b/Firmware/communication/interface_usb.cpp @@ -5,6 +5,8 @@ #include #include +#include +#include #include #include #include @@ -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(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& completer) final; + void cancel_write(TransferHandle transfer_handle) final; + void did_finish(); + + const uint8_t endpoint_num_; + bool connected_ = false; + Completer* 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& completer) final; + void cancel_read(TransferHandle transfer_handle) final; + void did_finish(); + + const uint8_t endpoint_num_; + bool connected_ = false; + Completer* completer_ = nullptr; + uint8_t* rx_end_ = nullptr; +}; + +} + +using namespace fibre; + +void Stm32UsbTxStream::start_write(cbufptr_t buffer, TransferHandle* handle, Completer& completer) { + if (handle) { + *handle = reinterpret_cast(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(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& completer) { + if (handle) { + *handle = reinterpret_cast(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::get_dummy()); + + if (odrv.config_.usb_cdc_protocol == ODrive::STREAM_PROTOCOL_TYPE_FIBRE) { + fibre_over_cdc.start(Completer::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() { diff --git a/Firmware/communication/interface_usb.h b/Firmware/communication/interface_usb.h index c78ecc5d..9314cd4e 100644 --- a/Firmware/communication/interface_usb.h +++ b/Firmware/communication/interface_usb.h @@ -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 +extern fibre::BufferedStreamSink<64> usb_cdc_stdout_sink; +extern bool usb_cdc_stdout_pending; +#endif + #endif // __INTERFACE_USB_HPP diff --git a/Firmware/fibre/.gitignore b/Firmware/fibre/.gitignore index d41efe55..d58ad3d0 100644 --- a/Firmware/fibre/.gitignore +++ b/Firmware/fibre/.gitignore @@ -1,4 +1,5 @@ __pycache__ .Trash* +/cpp/third_party /build /.tup diff --git a/Firmware/fibre/cpp/.gitattributes b/Firmware/fibre/cpp/.gitattributes new file mode 100644 index 00000000..4a382938 --- /dev/null +++ b/Firmware/fibre/cpp/.gitattributes @@ -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 diff --git a/Firmware/fibre/cpp/Makefile b/Firmware/fibre/cpp/Makefile new file mode 100644 index 00000000..8ae93b2c --- /dev/null +++ b/Firmware/fibre/cpp/Makefile @@ -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 diff --git a/Firmware/fibre/cpp/README.md b/Firmware/fibre/cpp/README.md new file mode 100644 index 00000000..33be1144 --- /dev/null +++ b/Firmware/fibre/cpp/README.md @@ -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`) diff --git a/Firmware/fibre/cpp/async_stream.hpp b/Firmware/fibre/cpp/async_stream.hpp new file mode 100644 index 00000000..bb233e00 --- /dev/null +++ b/Firmware/fibre/cpp/async_stream.hpp @@ -0,0 +1,188 @@ +#ifndef __FIBRE_ASYNC_STREAM_HPP +#define __FIBRE_ASYNC_STREAM_HPP + +#include "include/fibre/bufptr.hpp" // TODO: move this header +#include + +namespace fibre { + +enum StreamStatus { + kStreamOk, + kStreamCancelled, + kStreamClosed, + kStreamError +}; + +template +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 +static void safe_complete(Completer*& completer, TResults ... results) { + Completer* 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 { + virtual void on_write_finished(WriteResult result) = 0; + + void complete(WriteResult result) final { + on_write_finished(result); + } +}; + +struct ReadCompleter : Completer { + 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& 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& 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 \ No newline at end of file diff --git a/Firmware/fibre/cpp/compile_for_all_platforms.sh b/Firmware/fibre/cpp/compile_for_all_platforms.sh new file mode 100755 index 00000000..ca01651d --- /dev/null +++ b/Firmware/fibre/cpp/compile_for_all_platforms.sh @@ -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 + diff --git a/Firmware/fibre/cpp/endpoints_template.j2 b/Firmware/fibre/cpp/endpoints_template.j2 index 50669b77..4e467894 100644 --- a/Firmware/fibre/cpp/endpoints_template.j2 +++ b/Firmware/fibre/cpp/endpoints_template.j2 @@ -14,6 +14,7 @@ #define __FIBRE_ENDPOINTS_HPP #include +#include // 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 diff --git a/Firmware/fibre/cpp/event_loop.hpp b/Firmware/fibre/cpp/event_loop.hpp new file mode 100644 index 00000000..e4e6a144 --- /dev/null +++ b/Firmware/fibre/cpp/event_loop.hpp @@ -0,0 +1,45 @@ +#ifndef __FIBRE_EVENT_LOOP_HPP +#define __FIBRE_EVENT_LOOP_HPP + +#include + +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 \ No newline at end of file diff --git a/Firmware/fibre/cpp/include/fibre/bufptr.hpp b/Firmware/fibre/cpp/include/fibre/bufptr.hpp index 2ce3fefb..7726ed2b 100644 --- a/Firmware/fibre/cpp/include/fibre/bufptr.hpp +++ b/Firmware/fibre/cpp/include/fibre/bufptr.hpp @@ -1,6 +1,9 @@ #ifndef __FIBRE_BUFPTR_HPP #define __FIBRE_BUFPTR_HPP +#include +#include + namespace fibre { static inline bool soft_assert(bool expr) { return expr; } // TODO: implement @@ -24,10 +27,13 @@ struct generic_bufptr_t { template generic_bufptr_t(T (&begin)[I]) : generic_bufptr_t(begin, I) {} - generic_bufptr_t(const std::vector>& vector) + generic_bufptr_t(std::vector::type>& vector) : generic_bufptr_t(vector.data(), vector.size()) {} - generic_bufptr_t(const generic_bufptr_t>& other) + generic_bufptr_t(const std::vector::type>& vector) + : generic_bufptr_t(vector.data(), vector.size()) {} + + generic_bufptr_t(const generic_bufptr_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_; }; diff --git a/Firmware/fibre/cpp/include/fibre/cpp_utils.hpp b/Firmware/fibre/cpp/include/fibre/cpp_utils.hpp index b09aa400..285af103 100644 --- a/Firmware/fibre/cpp/include/fibre/cpp_utils.hpp +++ b/Firmware/fibre/cpp/include/fibre/cpp_utils.hpp @@ -82,7 +82,7 @@ public: #include #include #include -#include +//#include /* Backport features from C++14 and C++17 ------------------------------------*/ @@ -439,6 +439,88 @@ T& get(std::variant& val) { return std::get(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 +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(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 +optional make_optional(T&& val) { + return optional{std::forward(val)}; +} + } // namespace std #endif @@ -707,7 +789,7 @@ struct sstring { static constexpr std::array as_array() { return {CHARS...}; } template - constexpr bool operator==(const sstring & other) { + constexpr bool operator==(const sstring & other) const { return as_array() == other.as_array(); } }; @@ -786,13 +868,13 @@ using sstring_builder_t = typename sstring_builder::type; */ #define MAKE_SSTRING(literal) sstring_builder_t -namespace std { +/*namespace std { template static std::ostream& operator<<(std::ostream& stream, const sstring& val) { stream << val.chars; return stream; } -} +}*/ template struct join_sstring_impl; @@ -917,10 +999,10 @@ struct args_of { using type = std::tuple; }; -template -struct args_of> { - using type = std::tuple; -}; +//template +//struct args_of<_Mem_fn> { +// using type = std::tuple; +//}; template struct args_of { diff --git a/Firmware/fibre/cpp/include/fibre/libfibre.h b/Firmware/fibre/cpp/include/fibre/libfibre.h new file mode 100644 index 00000000..dd60832f --- /dev/null +++ b/Firmware/fibre/cpp/include/fibre/libfibre.h @@ -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 +#include + +#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 \ No newline at end of file diff --git a/Firmware/fibre/cpp/include/fibre/protocol.hpp b/Firmware/fibre/cpp/include/fibre/protocol.hpp index 2a01e220..9c039e7d 100644 --- a/Firmware/fibre/cpp/include/fibre/protocol.hpp +++ b/Firmware/fibre/cpp/include/fibre/protocol.hpp @@ -5,9 +5,6 @@ see protocol.md for the protocol specification #ifndef __PROTOCOL_HPP #define __PROTOCOL_HPP -// TODO: resolve assert -#define assert(expr) - #include #include #include @@ -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::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 std::enable_if_t::value, size_t> -write_le(T value, uint8_t* buffer) { - return write_le>(value, buffer); -} - -template<> -inline size_t write_le(float value, uint8_t* buffer) { - static_assert(CHAR_BIT * sizeof(float) == 32, "32 bit floating point expected"); - static_assert(std::numeric_limits::is_iec559, "IEEE 754 floating point expected"); - uint32_t value_as_uint32; - std::memcpy(&value_as_uint32, &value, sizeof(uint32_t)); - return write_le(value_as_uint32, buffer); -} - -template -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* value, const uint8_t* buffer) { - static_assert(CHAR_BIT * sizeof(float) == 32, "32 bit floating point expected"); - static_assert(std::numeric_limits::is_iec559, "IEEE 754 floating point expected"); - return read_le(reinterpret_cast(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 -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(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 @@ -384,7 +84,7 @@ template<> struct Codec { template<> struct Codec { static std::optional decode(cbufptr_t* buffer) { std::optional int_val = Codec::decode(buffer); - return int_val.has_value() ? std::optional(*reinterpret_cast(&int_val.value())) : std::nullopt; + return int_val.has_value() ? std::optional(*reinterpret_cast(&*int_val)) : std::nullopt; } static bool encode(float value, bufptr_t* buffer) { void* ptr = &value; @@ -395,7 +95,7 @@ template struct Codec::value>> { static std::optional decode(cbufptr_t* buffer) { std::optional int_val = SimpleSerializer::read(&(buffer->begin()), buffer->end()); - return int_val.has_value() ? std::make_optional(static_cast(int_val.value())) : std::nullopt; + return int_val.has_value() ? std::make_optional(static_cast(*int_val)) : std::nullopt; } static bool encode(T value, bufptr_t* buffer) { return SimpleSerializer::write(value, &(buffer->begin()), buffer->end()); } }; @@ -403,7 +103,7 @@ template<> struct Codec { static std::optional decode(cbufptr_t* buffer) { std::optional val0 = SimpleSerializer::read(&(buffer->begin()), buffer->end()); std::optional val1 = SimpleSerializer::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::write(value.endpoint_id, &(buffer->begin()), buffer->end()) @@ -413,29 +113,6 @@ template<> struct Codec { } -/* @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 { using type = void; +template<> struct format_traits_t { using type = void; static constexpr const char * fmt = "%lld"; static constexpr const char * fmtp = "%lld"; }; -template<> struct format_traits_t { using type = void; +template<> struct format_traits_t { using type = void; static constexpr const char * fmt = "%llu"; static constexpr const char * fmtp = "%llu"; }; -template<> struct format_traits_t { using type = void; +template<> struct format_traits_t { using type = void; static constexpr const char * fmt = "%ld"; static constexpr const char * fmtp = "%ld"; }; -template<> struct format_traits_t { using type = void; +template<> struct format_traits_t { 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 { using type = void; + static constexpr const char * fmt = "%d"; + static constexpr const char * fmtp = "%d"; +}; template<> struct format_traits_t { using type = void; static constexpr const char * fmt = "%ud"; static constexpr const char * fmtp = "%ud"; }; -template<> struct format_traits_t { using type = void; +template<> struct format_traits_t { using type = void; static constexpr const char * fmt = "%hd"; static constexpr const char * fmtp = "%hd"; }; -template<> struct format_traits_t { using type = void; +template<> struct format_traits_t { using type = void; static constexpr const char * fmt = "%hu"; static constexpr const char * fmtp = "%hu"; }; -template<> struct format_traits_t { using type = void; +template<> struct format_traits_t { using type = void; static constexpr const char * fmt = "%hhd"; static constexpr const char * fmtp = "%d"; }; -template<> struct format_traits_t { using type = void; +template<> struct format_traits_t { 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 value) const { T old_value = (*getter_)(ctx_); if (value.has_value()) { - (*setter_)(ctx_, value.value()); + (*setter_)(ctx_, *value); } return old_value; } diff --git a/Firmware/fibre/cpp/include/fibre/simple_serdes.hpp b/Firmware/fibre/cpp/include/fibre/simple_serdes.hpp index 32c09f27..2fc11077 100644 --- a/Firmware/fibre/cpp/include/fibre/simple_serdes.hpp +++ b/Firmware/fibre/cpp/include/fibre/simple_serdes.hpp @@ -1,8 +1,10 @@ #ifndef __FIBRE_SIMPLE_SERDES #define __FIBRE_SIMPLE_SERDES -//#include "stream.hpp" - +#include "cpp_utils.hpp" +#include "limits.h" +#include // TODO: make C++11 backport of this +#include template struct SimpleSerializer; @@ -73,5 +75,52 @@ inline bool write_le(T value, fibre::bufptr_t* buffer) { return LittleEndianSerializer::write(value, &buffer->begin(), buffer->end()); } +template::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 std::enable_if_t::value, size_t> +write_le(T value, uint8_t* buffer) { + return write_le>(value, buffer); +} + +template<> +inline size_t write_le(float value, uint8_t* buffer) { + static_assert(CHAR_BIT * sizeof(float) == 32, "32 bit floating point expected"); + static_assert(std::numeric_limits::is_iec559, "IEEE 754 floating point expected"); + uint32_t value_as_uint32; + std::memcpy(&value_as_uint32, &value, sizeof(uint32_t)); + return write_le(value_as_uint32, buffer); +} + +template +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* value, const uint8_t* buffer) { + static_assert(CHAR_BIT * sizeof(float) == 32, "32 bit floating point expected"); + static_assert(std::numeric_limits::is_iec559, "IEEE 754 floating point expected"); + return read_le(reinterpret_cast(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 +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 \ No newline at end of file diff --git a/Firmware/fibre/cpp/legacy_object_client.cpp b/Firmware/fibre/cpp/legacy_object_client.cpp new file mode 100644 index 00000000..abc1262d --- /dev/null +++ b/Firmware/fibre/cpp/legacy_object_client.cpp @@ -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 +#include + +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>; +using json_dict = std::vector, std::shared_ptr>>; +using json_value_variant = std::variant; + +struct json_value : json_value_variant { + //json_value(const json_value_variant& v) : json_value_variant{v} {} + template json_value(T&& arg) : json_value_variant{std::forward(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(key), std::make_shared(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(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>& 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 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 parse_arglist(const json_value& list_val) { + std::vector 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>& on_found_root_object, Completer& 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& 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 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(intf); +} + +std::shared_ptr 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 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(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(intf), + .known_to_application = false + }; + auto obj_ptr = std::make_shared(obj); + objects_.push_back(obj_ptr); + return obj_ptr; +} + +void LegacyObjectClient::receive_more_json() { + write_le(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(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(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(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}); + } +} \ No newline at end of file diff --git a/Firmware/fibre/cpp/legacy_object_client.hpp b/Firmware/fibre/cpp/legacy_object_client.hpp new file mode 100644 index 00000000..1f2e8453 --- /dev/null +++ b/Firmware/fibre/cpp/legacy_object_client.hpp @@ -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 +#include +#include +#include +#include + +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 inputs; + std::vector outputs; +}; + +struct FibreInterface; +struct LegacyObjectClient; +struct LegacyObject; + +struct LegacyFibreAttribute { + std::shared_ptr object; +}; + +struct FibreInterface { + std::string name; + std::unordered_map functions; + std::unordered_map attributes; +}; + +struct LegacyObject { + LegacyObjectClient* client; + size_t ep_num; + std::shared_ptr intf; + bool known_to_application; +}; + +class LegacyObjectClient : Completer { +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* completer; + }; + + LegacyObjectClient(LegacyProtocolPacketBased* protocol) : protocol_(protocol) {} + + void start(Completer>& on_found_root_object, Completer& on_lost_root_object); + + void start_call(size_t ep_num, LegacyFibreFunction* func, cbufptr_t input, bufptr_t output, CallContext** handle, Completer& completer); + void cancel_call(CallContext* handle); + + // For direct access by LegacyProtocolPacketBased and libfibre.cpp + uint16_t json_crc_ = 0; + Completer* on_lost_root_object_; + std::shared_ptr root_obj_; + std::vector> objects_; + void* user_data_; // used by libfibre to store the libfibre context pointer + +private: + std::shared_ptr get_property_interfaces(std::string codec, bool write); + std::shared_ptr load_object(json_value list_val); + void receive_more_json(); + void complete(EndpointOperationResult result); + + LegacyProtocolPacketBased* protocol_; + Completer>* on_found_root_object_; + uint8_t tx_buf_[4] = {0xff, 0xff, 0xff, 0xff}; + EndpointOperationHandle op_handle_ = 0; + std::vector json_; + CallContext* call_ = nullptr; // active call + std::vector pending_calls_; + std::unordered_map> rw_property_interfaces; + std::unordered_map> ro_property_interfaces; +}; + +} + +#endif // __FIBRE_LEGACY_OBJECT_MODEL_HPP \ No newline at end of file diff --git a/Firmware/fibre/cpp/legacy_protocol.cpp b/Firmware/fibre/cpp/legacy_protocol.cpp new file mode 100644 index 00000000..f9f052bf --- /dev/null +++ b/Firmware/fibre/cpp/legacy_protocol.cpp @@ -0,0 +1,520 @@ + + +#include "legacy_protocol.hpp" + +#include +#include +#include "logging.hpp" +#include "print_utils.hpp" +#include "async_stream.hpp" +#include +#include + +DEFINE_LOG_TOPIC(LEGACY_PROTOCOL); +USE_LOG_TOPIC(LEGACY_PROTOCOL); + +using namespace fibre; + + +/* PacketWrapper -------------------------------------------------------------*/ + +void PacketWrapper::start_write(cbufptr_t buffer, TransferHandle* handle, Completer& completer) { + if (handle) { + *handle = reinterpret_cast(this); + } + + if (state_ != kStateIdle) { + completer.complete({kStreamError, buffer.begin()}); + } + + // TODO: support buffer size >= 128 + if (buffer.size() >= 128) { + completer.complete({kStreamError, buffer.begin()}); + } + + completer_ = &completer; + + header_buf_[0] = CANONICAL_PREFIX; + header_buf_[1] = static_cast(buffer.size()); + header_buf_[2] = calc_crc8(CANONICAL_CRC8_INIT, header_buf_, 2); + + payload_buf_ = buffer; + + uint16_t crc16 = calc_crc16(CANONICAL_CRC16_INIT, buffer.begin(), buffer.size()); + trailer_buf_[0] = (uint8_t)((crc16 >> 8) & 0xff), + trailer_buf_[1] = (uint8_t)((crc16 >> 0) & 0xff); + + state_ = kStateSendingHeader; + expected_tx_end_ = header_buf_ + 3; + tx_channel_->start_write(header_buf_, &inner_transfer_handle_, *this); +} + +void PacketWrapper::cancel_write(TransferHandle transfer_handle) { + state_ = kStateCancelling; + tx_channel_->cancel_write(inner_transfer_handle_); +} + +void PacketWrapper::complete(WriteResult result) { + if (state_ == kStateCancelling) { + state_ = kStateIdle; + safe_complete(completer_, {kStreamCancelled, payload_buf_.begin()}); + return; + } + + if (result.status != kStreamOk) { + state_ = kStateIdle; + safe_complete(completer_, {result.status, payload_buf_.begin()}); + return; + } + + if (result.end < expected_tx_end_) { + tx_channel_->start_write({result.end, expected_tx_end_}, &inner_transfer_handle_, *this); + return; + } + + if (state_ == kStateSendingHeader) { + state_ = kStateSendingPayload; + expected_tx_end_ = payload_buf_.end(); + tx_channel_->start_write(payload_buf_, &inner_transfer_handle_, *this); + + } else if (state_ == kStateSendingPayload) { + state_ = kStateSendingTrailer; + expected_tx_end_ = trailer_buf_ + 2; + tx_channel_->start_write(trailer_buf_, &inner_transfer_handle_, *this); + + } else if (state_ == kStateSendingTrailer) { + state_ = kStateIdle; + safe_complete(completer_, {kStreamOk, payload_buf_.end()}); + } +} + + +/* PacketUnwrapper -----------------------------------------------------------*/ + +void PacketUnwrapper::start_read(bufptr_t buffer, TransferHandle* handle, Completer& completer) { + if (handle) { + *handle = reinterpret_cast(this); + } + + if (state_ != kStateIdle) { + completer.complete({kStreamError, buffer.begin()}); + } + + completer_ = &completer; + payload_buf_ = buffer; + + state_ = kStateReceivingHeader; + expected_rx_end_ = rx_buf_ + 3; + rx_channel_->start_read({rx_buf_, expected_rx_end_}, &inner_transfer_handle_, *this); +} + +void PacketUnwrapper::cancel_read(TransferHandle transfer_handle) { + state_ = kStateCancelling; + rx_channel_->cancel_read(inner_transfer_handle_); +} + +void PacketUnwrapper::complete(ReadResult result) { + // All code paths in this function must end with either of these two: + // - rx_channel_->start_read() to bounce back control to the underlying stream + // - safe_complete() to return control to the client + + if (state_ == kStateCancelling) { + state_ = kStateIdle; + safe_complete(completer_, {kStreamCancelled, payload_buf_.begin()}); + return; + } + + if (result.status != kStreamOk) { + state_ = kStateIdle; + safe_complete(completer_, {result.status, payload_buf_.begin()}); + return; + } + + if (result.end < expected_rx_end_) { + rx_channel_->start_read({result.end, expected_rx_end_}, &inner_transfer_handle_, *this); + return; + } + + if (state_ == kStateReceivingHeader) { + size_t n_discard; + + // Process header + if (rx_buf_[0] != CANONICAL_PREFIX) { + n_discard = 1; + } else if ((rx_buf_[1] & 0x80)) { + n_discard = 2; // TODO: support packets larger than 128 bytes + } else if (calc_crc8(CANONICAL_CRC8_INIT, rx_buf_, 3)) { + n_discard = 3; + } else { + state_ = kStateReceivingPayload; + payload_length_ = std::min(payload_buf_.size(), (size_t)rx_buf_[1]); + expected_rx_end_ = payload_buf_.begin() + payload_length_; + rx_channel_->start_read(payload_buf_.take(payload_length_), &inner_transfer_handle_, *this); + return; + } + + // Header was bad: discard the bad header bytes and receive more + memmove(rx_buf_, rx_buf_ + n_discard, sizeof(rx_buf_) - n_discard); + rx_channel_->start_read(bufptr_t{rx_buf_}.skip(3 - n_discard), &inner_transfer_handle_, *this); + + } else if (state_ == kStateReceivingPayload) { + expected_rx_end_ = rx_buf_ + 2; + state_ = kStateReceivingTrailer; + rx_channel_->start_read({rx_buf_, expected_rx_end_}, &inner_transfer_handle_, *this); + + } else if (state_ == kStateReceivingTrailer) { + uint16_t crc = calc_crc16(CANONICAL_CRC16_INIT, payload_buf_.begin(), payload_length_); + crc = calc_crc16(crc, rx_buf_, 2); + + if (!crc) { + state_ = kStateIdle; + safe_complete(completer_, {kStreamOk, payload_buf_.begin() + payload_length_}); + } else { + state_ = kStateReceivingHeader; + expected_rx_end_ = rx_buf_ + 3; + rx_channel_->start_read({rx_buf_, expected_rx_end_}, &inner_transfer_handle_, *this); + } + } +} + + +/* LegacyProtocolPacketBased -------------------------------------------------*/ + +#ifdef FIBRE_ENABLE_CLIENT + +/** + * @brief Starts a remote endpoint operation. + * + * @param endpoint_id: The endpoint ID to invoke the operation on. + * @param tx_buf: The tx_buf to write to the endpoint. Must remain valid until + * the completer is invoked. + * @param rx_length: The desired number of bytes to read from the endpoint. The + * actual returned buffer may be smaller. + * @param completer: The completer that will be notified once the operation + * completes (whether successful or not). + * The buffer given to the completer is only valid if the status is + * kStreamOk and until the completer returns. + * @param handle: The variable pointed to by this argument is set to a handle + * that can be passed to cancel_endpoint_operation() to cancel the + * ongoing operation. If the completer is invoked directly from within + * this function then the handle is not set later than invoking the + * completer. + */ +void LegacyProtocolPacketBased::start_endpoint_operation(uint16_t endpoint_id, cbufptr_t tx_buf, bufptr_t rx_buf, EndpointOperationHandle* handle, Completer& completer) { + if (tx_buf.size() + 8 >= tx_mtu_) { + FIBRE_LOG(E) << "packet too large"; + completer.complete({kStreamError, rx_buf.begin()}); + } + + if (rx_buf.size() > 0xffff) { + FIBRE_LOG(E) << "receive size larger than 65535 currently not supported"; + completer.complete({kStreamError, rx_buf.begin()}); + } + + outbound_seq_no_ = ((outbound_seq_no_ + 1) & 0x7fff); + + EndpointOperation op = { + .seqno = (uint16_t)(outbound_seq_no_ | 0x0080), // FIXME: we hardwire one bit of the seq-no to 1 to avoid conflicts with the ODrive ASCII protocol + .endpoint_id = endpoint_id, + .tx_buf = tx_buf, + .rx_buf = rx_buf, + .completer = &completer + }; + + if (handle) { + *handle = op.seqno | 0xffff0000; + } + + if (tx_handle_) { + FIBRE_LOG(D) << "Endpoint operation already in progress. Enqueuing this one."; + + // A TX operation is already in progress + if (pending_operation_.completer) { + // Previous endpoint operation was not yet sent. We don't support + // enqueuing multiple endpoint operations while the first didn't send yet. + FIBRE_LOG(E) << "previous endpoint operation still not sent"; + completer.complete({kStreamError, rx_buf.begin()}); + } else { + // Control is returned to start_endpoint_operation once TX completes + pending_operation_ = op; + } + return; + } + + start_endpoint_operation(op); +} + +void LegacyProtocolPacketBased::start_endpoint_operation(EndpointOperation op) { + write_le(op.seqno, tx_buf_); + write_le(op.endpoint_id | 0x8000, tx_buf_ + 2); + write_le(op.rx_buf.size(), tx_buf_ + 4); + + memcpy(tx_buf_ + 6, op.tx_buf.begin(), op.tx_buf.size()); + + uint16_t trailer = (op.endpoint_id & 0x7fff) == 0 ? + PROTOCOL_VERSION : client_.json_crc_; + + write_le(trailer, tx_buf_ + 6 + op.tx_buf.size()); + + expected_acks_[op.seqno] = op; + transmitting_op_ = op.seqno | 0xffff0000; + tx_channel_->start_write(cbufptr_t{tx_buf_}.take(8 + op.tx_buf.size()), &tx_handle_, *static_cast(this)); +} + + +void LegacyProtocolPacketBased::cancel_endpoint_operation(EndpointOperationHandle handle) { + if (!handle) { + return; + } + + uint16_t seqno = static_cast(handle & 0xffff); + + Completer* completer; + uint8_t* rx_end = nullptr; + + if (pending_operation_.seqno == seqno) { + completer = pending_operation_.completer; + rx_end = pending_operation_.rx_buf.begin(); + pending_operation_ = {}; + } + + auto it = expected_acks_.find(handle); + + if (it != expected_acks_.end()) { + completer = it->second.completer; + rx_end = it->second.rx_buf.begin(); + expected_acks_.erase(it); + } + + if (transmitting_op_ == handle) { + // Cancel the TX task because it belongs to the endpoint operation that + // is being cancelled. + tx_channel_->cancel_write(tx_handle_); + } else { + // Either we're waiting for an ack on this operation or it has not yet + // been sent. In both cases we can just complete immediately. + safe_complete(completer, {kStreamCancelled, rx_end}); + } +} + +#endif + +#ifdef FIBRE_ENABLE_SERVER + +// 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 offset = read_le(input_buffer); + + if (!offset.has_value()) { + // Didn't receive any offset + return false; + } else if (*offset == 0xffffffff) { + // If the offset is special value 0xFFFFFFFF, send back the JSON version ID instead + return write_le(json_version_id_, output_buffer); + } else if (*offset >= 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); + memcpy(output_buffer->begin(), embedded_json + *offset, n_copy); + *output_buffer = output_buffer->skip(n_copy); + return true; + } +} + +#endif + +void LegacyProtocolPacketBased::on_write_finished(WriteResult result) { + tx_handle_ = 0; + +#if FIBRE_ENABLE_CLIENT + if (transmitting_op_) { + uint16_t seqno = transmitting_op_ & 0xffff; + transmitting_op_ = 0; + + // If the TX task was a remote endpoint operation but didn't succeed + // we terminate that operation + if (result.status != kStreamOk) { + auto it = expected_acks_.find(seqno); + auto completer = it->second.completer; + auto rx_end = it->second.rx_buf.begin(); + expected_acks_.erase(it); + safe_complete(completer, {result.status, rx_end}); + } + } +#endif + + // TODO: should we prioritize the server or client side here? + +#if FIBRE_ENABLE_SERVER + if (rx_end_) { + // There is a write operation pending from the server side (i.e. an ack + // for a local endpoint operation). + uint8_t* rx_end = rx_end_; + rx_end_ = nullptr; + on_read_finished({kStreamOk, rx_end}); + return; + } +#endif + +#if FIBRE_ENABLE_CLIENT + if (pending_operation_.completer) { + // There is a write operation pending from the client side (i.e. an + // outgoing remote endpoint operation). + EndpointOperation op = pending_operation_; + pending_operation_ = {}; + start_endpoint_operation(op); + return; + } +#endif +} + +void LegacyProtocolPacketBased::on_read_finished(ReadResult result) { + TransferHandle dummy; + + if (result.status == kStreamClosed) { + FIBRE_LOG(D) << "RX stream closed."; + on_closed(kStreamClosed); + return; + } else if (result.status == kStreamCancelled) { + FIBRE_LOG(W) << "RX operation cancelled."; + // TODO: close stream + return; + } else if (result.status != kStreamOk) { + FIBRE_LOG(W) << "RX error. Not restarting."; + // TODO: we should distinguish between permanent and temporary errors. + // If we try to restart after a permanent error we might end up in a + // busy loop. + on_closed(kStreamError); + return; + } + + cbufptr_t rx_buf = cbufptr_t{rx_buf_, result.end}; + //FIBRE_LOG(D) << "got packet of length " << (result.end - rx_buf_) /*<< ": " << as_hex(rx_buf)*/; + + std::optional seq_no = read_le(&rx_buf); + + if (!seq_no.has_value()) { + FIBRE_LOG(W) << "packet too short"; + + } else if (*seq_no & 0x8000) { + +#ifdef FIBRE_ENABLE_CLIENT + + auto it = expected_acks_.find(*seq_no & 0x7fff); + + if (it == expected_acks_.end()) { + FIBRE_LOG(W) << "received unexpected ACK: " << (*seq_no & 0x7fff); + } else { + size_t n_copy = std::min((size_t)(result.end - rx_buf.begin()), it->second.rx_buf.size()); + memcpy(it->second.rx_buf.begin(), rx_buf.begin(), n_copy); + uint8_t* rx_end = it->second.rx_buf.begin() + n_copy; + auto completer = it->second.completer; + expected_acks_.erase(it); + safe_complete(completer, {kStreamOk, rx_end}); + } + +#else + FIBRE_LOG(W) << "received ack but client support is not compiled in"; +#endif + + } else { + +#ifdef FIBRE_ENABLE_SERVER + if (rx_buf.size() < 6) { + FIBRE_LOG(W) << "packet too short"; + rx_channel_->start_read(rx_buf_, &dummy, *static_cast(this)); + return; + } + + // 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(&rx_buf); + bool expect_response = endpoint_id & 0x8000; + endpoint_id &= 0x7fff; + + if (expect_response && tx_handle_) { + // The operation expects a response but the output channel is still + // busy. Stop receiving for now. This function will be invoked again + // once the TX operation is finished. + rx_end_ = result.end; + return; + } + + // 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 = *(rx_buf.end() - 2) | (*(rx_buf.end() - 1) << 8); + if (expected_trailer != actual_trailer) { + FIBRE_LOG(D) << "trailer mismatch for endpoint " << endpoint_id << ": expected " << as_hex(expected_trailer) << ", got " << as_hex(actual_trailer); + rx_channel_->start_read(rx_buf_, &dummy, *static_cast(this)); + return; + } + FIBRE_LOG(D) << "trailer ok for endpoint " << 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(&rx_buf); + + // Limit response length according to our local TX buffer size + if (expected_response_length > tx_mtu_ - 2) + expected_response_length = tx_mtu_ - 2; + + fibre::cbufptr_t input_buffer{rx_buf.begin(), rx_buf.end() - 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(*seq_no | 0x8000, tx_buf_); + + FIBRE_LOG(D) << "send packet: " << as_hex(cbufptr_t{tx_buf_, actual_response_length}); + tx_channel_->start_write({tx_buf_, actual_response_length}, &tx_handle_, *static_cast(this)); + } +#else + FIBRE_LOG(W) << "received request but server support is not compiled in"; +#endif + } + + rx_channel_->start_read(rx_buf_, &dummy, *static_cast(this)); +} + +void LegacyProtocolPacketBased::on_closed(StreamStatus status) { + +#ifdef FIBRE_ENABLE_CLIENT + // Cancel all ongoing endpoint operations + for (auto& item: expected_acks_) { + if (item.second.completer) + (*item.second.completer).complete({status, item.second.rx_buf.begin()}); + } + expected_acks_.clear(); + + // Report that the root object was lost + if (client_.on_lost_root_object_ && client_.root_obj_) { + client_.root_obj_ = nullptr; + client_.on_lost_root_object_->complete(&client_); + } +#endif + safe_complete(on_stopped_, this, status); +} + +#if FIBRE_ENABLE_CLIENT +void LegacyProtocolPacketBased::start(Completer>& on_found_root_object, Completer& on_lost_root_object, Completer& on_stopped) { +#else +void LegacyProtocolPacketBased::start(Completer& on_stopped) { +#endif + on_stopped_ = &on_stopped; + TransferHandle dummy; + rx_channel_->start_read(rx_buf_, &dummy, *static_cast(this)); + +#if FIBRE_ENABLE_CLIENT + if (on_stopped_) { + client_.start(on_found_root_object, on_lost_root_object); + } +#endif +} diff --git a/Firmware/fibre/cpp/legacy_protocol.hpp b/Firmware/fibre/cpp/legacy_protocol.hpp new file mode 100644 index 00000000..61a16cc1 --- /dev/null +++ b/Firmware/fibre/cpp/legacy_protocol.hpp @@ -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 +#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 { +public: + PacketWrapper(AsyncStreamSink* tx_channel) + : tx_channel_(tx_channel) {} + + void start_write(cbufptr_t buffer, TransferHandle* handle, Completer& 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* completer_; + + enum { + kStateIdle, + kStateCancelling, + kStateSendingHeader, + kStateSendingPayload, + kStateSendingTrailer + } state_ = kStateIdle; +}; + + +class PacketUnwrapper : public AsyncStreamSource, Completer { +public: + PacketUnwrapper(AsyncStreamSource* rx_channel) + : rx_channel_(rx_channel) {} + + void start_read(bufptr_t buffer, TransferHandle* handle, Completer& 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* 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* 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& completer); + void cancel_endpoint_operation(EndpointOperationHandle handle); + + LegacyObjectClient client_{this}; +#endif + +#ifdef FIBRE_ENABLE_CLIENT + void start(Completer>& on_found_root_object, Completer& on_lost_root_object, Completer& on_stopped); +#else + void start(Completer& 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* 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 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>& on_found_root_object, Completer& on_lost_root_object, Completer& on_stopped) { + inner_protocol_.start(on_found_root_object, on_lost_root_object, on_stopped); + } +#else + void start(Completer& on_stopped) { inner_protocol_.start(on_stopped); } +#endif + +private: + PacketUnwrapper unwrapper_; + PacketWrapper wrapper_; + LegacyProtocolPacketBased inner_protocol_{&unwrapper_, &wrapper_, 127}; +}; + +} + +#endif // __FIBRE_LEGACY_PROTOCOL_HPP diff --git a/Firmware/fibre/cpp/libfibre-macos-x86.dylib b/Firmware/fibre/cpp/libfibre-macos-x86.dylib new file mode 100644 index 00000000..b881a087 --- /dev/null +++ b/Firmware/fibre/cpp/libfibre-macos-x86.dylib @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ad86cbdadd274245b9220f9728c1fc05ae05a1f196510a35e78e37309b3d6adf +size 2195160 diff --git a/Firmware/fibre/cpp/libfibre.cpp b/Firmware/fibre/cpp/libfibre.cpp new file mode 100644 index 00000000..edf09689 --- /dev/null +++ b/Firmware/fibre/cpp/libfibre.cpp @@ -0,0 +1,511 @@ + +#include +#include "platform_support/libusb_transport.hpp" +#include "logging.hpp" +#include "print_utils.hpp" +#include "legacy_protocol.hpp" +#include "legacy_object_client.hpp" +#include "stdio.h" // TODO: remove +#include "string.h" +#include +#include "fibre/simple_serdes.hpp" + +DEFINE_LOG_TOPIC(LIBFIBRE); +USE_LOG_TOPIC(LIBFIBRE); + +static const struct LibFibreVersion libfibre_version = { 0, 1, 0 }; + +class FIBRE_PRIVATE ExternalEventLoop : public EventLoop { +public: + ExternalEventLoop(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) : + post_(post), + register_event_(register_event), + deregister_event_(deregister_event), + call_later_(call_later), + cancel_timer_(cancel_timer) {} + + int post(void (*callback)(void*), void *ctx) final { + return (*post_)(callback, ctx); + } + + int register_event(int event_fd, uint32_t events, void (*callback)(void*), void* ctx) final { + return (*register_event_)(event_fd, events, callback, ctx); + } + + int deregister_event(int event_fd) final { + return (*deregister_event_)(event_fd); + } + + struct EventLoopTimer* call_later(float delay, void (*callback)(void*), void *ctx) final { + return (*call_later_)(delay, callback, ctx); + } + + int cancel_timer(struct EventLoopTimer* timer) final { + return (*cancel_timer_)(timer); + } + +private: + 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_; +}; + +struct FIBRE_PRIVATE LibFibreCtx { + ExternalEventLoop* event_loop; + construct_object_cb_t on_construct_object; + destroy_object_cb_t on_destroy_object; + void* cb_ctx; + fibre::LibusbDiscoverer libusb_discoverer; + size_t n_discoveries = 0; +}; + +struct FIBRE_PRIVATE LibFibreDiscoveryCtx : + fibre::Completer, + fibre::Completer>, + fibre::Completer, + fibre::Completer +{ + void complete(fibre::ChannelDiscoveryResult result) final; + void complete(fibre::LegacyObjectClient* obj_client, std::shared_ptr intf) final; + void complete(fibre::LegacyObjectClient* obj_client) final; + void complete(fibre::LegacyProtocolPacketBased* protocol, fibre::StreamStatus status) final; + + fibre::LibusbDiscoverer::ChannelDiscoveryContext* libusb_discovery_ctx = nullptr; + on_found_object_cb_t on_found_object; + void* cb_ctx; + LibFibreCtx* ctx; + std::vector protocol_instances; +}; + + + +// Callback for start_channel_discovery() +void LibFibreDiscoveryCtx::complete(fibre::ChannelDiscoveryResult result) { + FIBRE_LOG(D) << "found channels!"; + + if (result.status != kFibreOk) { + FIBRE_LOG(W) << "discoverer stopped"; + return; + } + + if (!result.rx_channel || !result.tx_channel) { + FIBRE_LOG(W) << "unidirectional operation not supported yet"; + return; + } + + const size_t mtu = 64; // TODO: get MTU from channel specific data + + auto protocol = new fibre::LegacyProtocolPacketBased(result.rx_channel, result.tx_channel, mtu); + protocol->client_.user_data_ = ctx; + protocol_instances.push_back(protocol); + protocol->start(*this, *this, *this); +} + +// on_found_root_object callback for LegacyProtocolPacketBased::start() +void LibFibreDiscoveryCtx::complete(fibre::LegacyObjectClient* obj_client, std::shared_ptr obj) { + auto obj_cast = reinterpret_cast(obj.get()); // corresponding reverse cast in libfibre_get_attribute() + auto intf_cast = reinterpret_cast(obj->intf.get()); // corresponding reverse cast in libfibre_subscribe_to_interface() + + for (auto& obj: obj_client->objects_) { + // If the callback handler calls libfibre_get_attribute() before + // all objects were announced to the application then it's possible + // that during that function call some objects are already announced + // on-demand. + if (!obj->known_to_application) { + obj->known_to_application = true; + //FIBRE_LOG(D) << "constructing root object " << fibre::as_hex(reinterpret_cast(obj.get())); + if (ctx->on_construct_object) { + (*ctx->on_construct_object)(ctx->cb_ctx, + reinterpret_cast(obj.get()), + reinterpret_cast(obj->intf.get()), + obj->intf->name.size() ? obj->intf->name.data() : nullptr, obj->intf->name.size()); + } + } + } + + if (on_found_object) { + FIBRE_LOG(D) << "announcing root object " << fibre::as_hex(reinterpret_cast(obj_cast)); + (*on_found_object)(cb_ctx, obj_cast); + } +} + +// on_lost_root_object for LegacyProtocolPacketBased::start() +void LibFibreDiscoveryCtx::complete(fibre::LegacyObjectClient* obj_client) { + if (ctx->on_destroy_object) { + for (auto obj: obj_client->objects_) { + auto obj_cast = reinterpret_cast(obj.get()); + //FIBRE_LOG(D) << "destroying subobject " << fibre::as_hex(reinterpret_cast(obj_cast)); + (*ctx->on_destroy_object)(ctx->cb_ctx, obj_cast); + } + + obj_client->objects_.clear(); + } +} + +// on_stopped callback for LegacyProtocolPacketBased::start() +void LibFibreDiscoveryCtx::complete(fibre::LegacyProtocolPacketBased* protocol, fibre::StreamStatus status) { + delete protocol; +} + +const struct LibFibreVersion* libfibre_get_version() { + return &libfibre_version; +} + +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) +{ + if (!register_event || !deregister_event) { + FIBRE_LOG(E) << "invalid argument"; + return nullptr; + } + + LibFibreCtx* ctx = new LibFibreCtx(); + ctx->event_loop = new ExternalEventLoop(post, register_event, deregister_event, call_later, cancel_timer); + ctx->on_construct_object = construct_object; + ctx->on_destroy_object = destroy_object; + ctx->cb_ctx = cb_ctx; + + if (ctx->libusb_discoverer.init(ctx->event_loop) != 0) { + delete ctx; + FIBRE_LOG(E) << "failed to init libusb transport layer"; + return nullptr; + } + + FIBRE_LOG(D) << "opened (" << fibre::as_hex((uintptr_t)ctx) << ")"; + return ctx; +} + +void libfibre_close(LibFibreCtx* ctx) { + if (ctx->n_discoveries) { + FIBRE_LOG(W) << "there are still discovery processes ongoing"; + } + + ctx->libusb_discoverer.deinit(); + delete ctx->event_loop; + delete ctx; + + FIBRE_LOG(D) << "closed (" << fibre::as_hex((uintptr_t)ctx) << ")"; +} + +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) { + if (!ctx) { + FIBRE_LOG(E) << "invalid argument"; + if (on_stopped) { + (*on_stopped)(cb_ctx, kFibreInvalidArgument); + } + return; + } + + const char* prev_delim = specs; + + FIBRE_LOG(D) << "starting discovery with path \"" << std::string(specs, specs_len) << "\""; + + LibFibreDiscoveryCtx* discovery_ctx = new LibFibreDiscoveryCtx(); + discovery_ctx->on_found_object = on_found_object; + discovery_ctx->cb_ctx = cb_ctx; + discovery_ctx->ctx = ctx; + + if (handle) { + *handle = discovery_ctx; + } + + while (prev_delim < specs + specs_len) { + const char* next_delim = std::find(prev_delim, specs + specs_len, ';'); + const char* colon = std::find(prev_delim, next_delim, ':'); + const char* colon_end = std::min(colon + 1, next_delim); + + if ((colon - prev_delim) == strlen("usb") && std::equal(prev_delim, colon, "usb")) { + ctx->libusb_discoverer.start_channel_discovery(colon_end, next_delim - colon_end, + &discovery_ctx->libusb_discovery_ctx, *discovery_ctx); + } else { + FIBRE_LOG(W) << "transport layer \"" << std::string(prev_delim, colon - prev_delim) << "\" not implemented"; + } + + prev_delim = std::min(next_delim + 1, specs + specs_len); + } + + ctx->n_discoveries++; +} + +void libfibre_stop_discovery(LibFibreCtx* ctx, LibFibreDiscoveryCtx* discovery_ctx) { + if (!ctx->n_discoveries) { + FIBRE_LOG(W) << "stopping a discovery process but none is active"; + } else { + ctx->n_discoveries--; + } + + if (discovery_ctx->libusb_discovery_ctx) { + // TODO: implement "stopped" callback + ctx->libusb_discoverer.stop_channel_discovery(discovery_ctx->libusb_discovery_ctx); + } + + delete discovery_ctx; +} + +const char* transform_codec(std::string& codec) { + if (codec == "endpoint_ref") { + return "object_ref"; + } else { + return codec.data(); + } +} + +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) +{ + auto intf = reinterpret_cast(interface); // corresponding reverse cast in LibFibreDiscoveryCtx::complete() and libfibre_subscribe_to_interface() + + for (auto& func: intf->functions) { + std::vector input_names; + std::vector input_codecs; + std::vector output_names; + std::vector output_codecs; + for (auto& arg: func.second.inputs) { + input_names.push_back(arg.name.data()); + input_codecs.push_back(transform_codec(arg.codec)); + } + for (auto& arg: func.second.outputs) { + output_names.push_back(arg.name.data()); + output_codecs.push_back(transform_codec(arg.codec)); + } + input_names.push_back(nullptr); + input_codecs.push_back(nullptr); + output_names.push_back(nullptr); + output_codecs.push_back(nullptr); + + if (on_function_added) { + (*on_function_added)(cb_ctx, + reinterpret_cast(&func.second), // corresponding reverse cast in libfibre_start_call() + func.first.data(), func.first.size(), + input_names.data(), input_codecs.data(), + output_names.data(), output_codecs.data()); + } + } + + for (auto& attr: intf->attributes) { + if (on_attribute_added) { + (*on_attribute_added)(cb_ctx, + reinterpret_cast(&attr.second), // corresponding reverse cast in libfibre_get_attribute() + attr.first.data(), attr.first.size(), + reinterpret_cast(attr.second.object->intf.get()), // corresponding reverse cast in libfibre_subscribe_to_interface() + attr.second.object->intf->name.size() ? attr.second.object->intf->name.data() : nullptr, attr.second.object->intf->name.size() + ); + } + } +} + +FibreStatus libfibre_get_attribute(LibFibreObject* parent_obj, LibFibreAttribute* attr, LibFibreObject** child_obj_ptr) { + if (!parent_obj || !attr) { + return kFibreInvalidArgument; + } + + fibre::LegacyObject* parent_obj_cast = reinterpret_cast(parent_obj); + fibre::LegacyFibreAttribute* attr_cast = reinterpret_cast(attr); // corresponding reverse cast in libfibre_subscribe_to_interface() + auto& attributes = parent_obj_cast->intf->attributes; + + bool is_member = std::find_if(attributes.begin(), attributes.end(), + [&](std::pair& kv) { + return &kv.second == attr_cast; + }) != attributes.end(); + + if (!is_member) { + FIBRE_LOG(W) << "attempt to fetch attribute from an object that does not implement it"; + return kFibreInvalidArgument; + } + + LibFibreCtx* libfibre_ctx = reinterpret_cast(parent_obj_cast->client->user_data_); + fibre::LegacyObject* child_obj = attr_cast->object.get(); + + if (!attr_cast->object->known_to_application) { + attr_cast->object->known_to_application = true; + + if (libfibre_ctx->on_construct_object) { + //FIBRE_LOG(D) << "constructing subobject " << fibre::as_hex(reinterpret_cast(child_obj)); + (*libfibre_ctx->on_construct_object)(libfibre_ctx->cb_ctx, + reinterpret_cast(child_obj), + reinterpret_cast(child_obj->intf.get()), + child_obj->intf->name.size() ? child_obj->intf->name.data() : nullptr, child_obj->intf->name.size()); + } + } + + if (child_obj_ptr) { + *child_obj_ptr = reinterpret_cast(child_obj); + } + + return kFibreOk; +} + +/** + * @brief Inserts or removes the specified number of elements + * @param delta: Positive value: insert elements, negative value: remove elements + */ +void resize_at(std::vector& vec, size_t pos, ssize_t delta) { + if (delta > 0) { + std::fill_n(std::inserter(vec, vec.begin() + pos), delta, 0); + } else { + vec.erase(std::min(vec.begin() + pos, vec.end()), + std::min(vec.begin() + pos + -delta, vec.end())); + } +} + +void transcode(fibre::LegacyObjectClient* client, std::vector& buffer, const std::vector& args, bool to) { + size_t offset = 0; + + for (auto& arg: args) { + if (to && arg.codec == "endpoint_ref") { + fibre::cbufptr_t orig_range = fibre::cbufptr_t{buffer}.skip(offset).take(sizeof(uintptr_t)); + + uintptr_t val = *reinterpret_cast(orig_range.begin()); + + resize_at(buffer, offset, (ssize_t)4 - (ssize_t)sizeof(uintptr_t)); + fibre::bufptr_t transcoded_range = fibre::bufptr_t{buffer}.skip(offset).take(4); + + auto obj = reinterpret_cast(val); + write_le(obj ? obj->ep_num : 0, &transcoded_range); + write_le(obj ? obj->client->json_crc_ : 0, &transcoded_range); + + offset += 4; + + } else if (!to && arg.codec == "endpoint_ref") { + fibre::cbufptr_t orig_range = fibre::cbufptr_t{buffer}.skip(offset).take(4); + + uint16_t ep_num = *read_le(&orig_range); + uint16_t json_crc = *read_le(&orig_range); + + resize_at(buffer, offset, (ssize_t)sizeof(uintptr_t) - (ssize_t)4); + fibre::bufptr_t transcoded_range = fibre::bufptr_t{buffer}.skip(offset).take(sizeof(uintptr_t)); + + fibre::LegacyObject* obj_ptr = nullptr; + + if (ep_num && json_crc == client->json_crc_) { + for (auto& known_obj: client->objects_) { + if (known_obj->ep_num == ep_num) { + obj_ptr = known_obj.get(); + } + } + } + + FIBRE_LOG(D) << "placing transcoded ptr " << reinterpret_cast(obj_ptr); + *reinterpret_cast(transcoded_range.begin()) = reinterpret_cast(obj_ptr); + + offset += sizeof(uintptr_t); + + } else { + offset += arg.size; + } + } +} + +struct FIBRE_PRIVATE LibFibreCallContext : fibre::Completer { + void complete(fibre::LegacyObjectClient::CallResult output) final { + + FIBRE_LOG(D) << "received " << (output.end - rx_vec.data()) << " bytes "; + + // Prune vector to the end of valid data + rx_vec.erase(rx_vec.begin() + (output.end - rx_vec.data()), rx_vec.end()); + + transcode(obj->client, rx_vec, func->outputs, false); + + size_t len = std::min(rx_vec.size(), rx_buf.size()); + FIBRE_LOG(D) << "copying " << rx_vec.size() << " or " << rx_buf.size() << " bytes to output"; + std::copy(rx_vec.begin(), rx_vec.begin() + len, rx_buf.begin()); + FIBRE_LOG(D) << "result is " << as_hex(rx_buf); + + if (on_completed) { + (*on_completed)(ctx, output.status, rx_buf.begin() + len); + } + delete this; + } + + fibre::LegacyObject* obj = nullptr; + fibre::LegacyFibreFunction* func = nullptr; + void (*on_completed)(void*, FibreStatus, uint8_t*); + void* ctx; + fibre::cbufptr_t tx_buf; // application-owned buffer + fibre::bufptr_t rx_buf; // application-owned buffer + std::vector tx_vec; // libfibre-owned buffer used after transcoding from application buffer + std::vector rx_vec; // libfibre-owned buffer used before transcoding to application buffer + fibre::LegacyObjectClient::CallContext* handle; +}; + +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* cb_ctx) { + if (!obj || !func || !input || !output) { + if (on_completed) { + (*on_completed)(cb_ctx, kFibreInvalidArgument, output); + } + return; + } + + fibre::LegacyObject* obj_cast = reinterpret_cast(obj); + fibre::LegacyFibreFunction* func_cast = reinterpret_cast(func); + + bool is_member = std::find_if(obj_cast->intf->functions.begin(), obj_cast->intf->functions.end(), + [&](std::pair& kv) { + return &kv.second == func_cast; + }) != obj_cast->intf->functions.end(); + + if (!is_member) { + FIBRE_LOG(W) << "attempt to invoke function on an object that does not implement it"; + if (on_completed) { + (*on_completed)(cb_ctx, kFibreInvalidArgument, output); + } + return; + } + + std::vector tx_vec; + tx_vec.insert(tx_vec.begin(), input, input + input_length); + + + auto completer = new LibFibreCallContext(); + completer->obj = obj_cast; + completer->func = func_cast; + completer->on_completed = on_completed; + completer->ctx = cb_ctx; + completer->tx_buf = fibre::cbufptr_t{input, input + input_length}; + completer->rx_buf = fibre::bufptr_t{output, output + output_length}; + + completer->tx_vec.insert(completer->tx_vec.begin(), completer->tx_buf.begin(), completer->tx_buf.end()); + transcode(obj_cast->client, completer->tx_vec, func_cast->inputs, true); + + size_t output_size = 0; + for (auto& arg: func_cast->outputs) + output_size += arg.size; + FIBRE_LOG(D) << "sizing output to " << output_size; + completer->rx_vec.resize(output_size); + + if (handle) { + *handle = completer; + } + + obj_cast->client->start_call(obj_cast->ep_num, func_cast, + fibre::cbufptr_t{completer->tx_vec}, fibre::bufptr_t{completer->rx_vec}, + &completer->handle, *completer); +} + +void libfibre_cancel_call(LibFibreCallContext* handle) { + if (handle && handle->on_completed) { + handle->obj->client->cancel_call(handle->handle); + } +} diff --git a/Firmware/fibre/cpp/logging.cpp b/Firmware/fibre/cpp/logging.cpp new file mode 100644 index 00000000..31f37113 --- /dev/null +++ b/Firmware/fibre/cpp/logging.cpp @@ -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; +} + +} diff --git a/Firmware/fibre/cpp/logging.hpp b/Firmware/fibre/cpp/logging.hpp new file mode 100644 index 00000000..a4aeac62 --- /dev/null +++ b/Firmware/fibre/cpp/logging.hpp @@ -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 + +#include +#include + +#if defined(_WIN32) || defined(_WIN64) +#include "windows.h" +#endif + +#if defined(_WIN32) || defined(_WIN64) || defined(__linux__) || defined(__APPLE__) +#include +#include +#else + +// We don't want 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 +using TMutex = std::mutex; +using TLock = std::unique_lock; + +#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( \ + 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() { return (default_verbosity); } \ +template<> constexpr log_level_t get_max_log_verbosity() { 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(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 +constexpr log_level_t get_default_log_verbosity() { return FIBRE_DEFAULT_LOG_VERBOSITY; } + +template +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 +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(); + if (var_val) { + unsigned long num = strtoul(var_val, nullptr, 10); + log_level = (log_level_t)num; + } + + if (log_level > get_max_log_verbosity()) { + log_level = get_max_log_verbosity(); + } + return log_level; +} + + + +/* +template +void send_to_stream(TStream&& stream); + +template +void send_to_stream(TStream&& stream) { } + +template +void send_to_stream(TStream&& stream, T&& value, Ts&&... values) { + send_to_stream(std::forward(stream) << std::forward(value), std::forward(values)...); +}*/ + + +Logger* get_logger(); // defined in logging.cpp + +template +Logger::Entry make_log_entry(const char *filename, size_t line_no, const char *funcname) { + if (get_current_log_verbosity() < LEVEL) { + return {}; + } else { + Logger* logger = get_logger(); + return { std::cerr, LEVEL, TOPIC::get_label(), filename, line_no, funcname, logger->mutex_ }; + } +} + +template +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 NullStream& operator<<(T val) { return *this; } +}; + +#define FIBRE_LOG(level) NullStream() + +#endif + +#endif // __FIBRE_LOGGING_HPP diff --git a/Firmware/fibre/cpp/platform_support/libusb_transport.cpp b/Firmware/fibre/cpp/platform_support/libusb_transport.cpp new file mode 100644 index 00000000..4ed00dbc --- /dev/null +++ b/Firmware/fibre/cpp/platform_support/libusb_transport.cpp @@ -0,0 +1,660 @@ +/** + * @brief Transport provider: libusb + * + * Platform Compatibility: Linux, Windows, macOS + */ + +#include "libusb_transport.hpp" +#include "../logging.hpp" +#include "../print_utils.hpp" + +#include + +using namespace fibre; + +DEFINE_LOG_TOPIC(USB); +USE_LOG_TOPIC(USB); + +constexpr unsigned int kBulkTimeoutMs = 2000; + + +/* LibusbDiscoverer ----------------------------------------------------------*/ + +/** + * @brief Initializes the discoverer. + * + * Asynchronous tasks will be executed on the provided event_loop. + * + * @param event_loop: The event loop that is used to execute background tasks. The + * pointer must be non-null and initialized when this function is called. + * It must remain initialized until deinit() of this discoverer was called. + */ +int LibusbDiscoverer::init(EventLoop* event_loop) { + if (!event_loop) + return -1; + event_loop_ = event_loop; + + if (libusb_init(&libusb_ctx_) != LIBUSB_SUCCESS) { + FIBRE_LOG(E) << "libusb_init() failed: " << sys_err(); + return deinit(0), -1; + } + + // Fetch initial list of file-descriptors we have to monitor. + // Note: this will fail on Windows. Since this is used for epoll, we need a + // different approach for Windows anyway. + const struct libusb_pollfd** pollfds = libusb_get_pollfds(libusb_ctx_); + using_sparate_libusb_thread_ = !pollfds; + + if (!using_sparate_libusb_thread_) { + // This code path is taken on Linux + FIBRE_LOG(D) << "Using externally provided event loop"; + + // Check if libusb needs special time-based polling on this platform + if (libusb_pollfds_handle_timeouts(libusb_ctx_) == 0) { + FIBRE_LOG(D) << "Using time-based polling"; + } + + // libusb maintains a (dynamic) list of file descriptors that need to be + // monitored (via select/poll/epoll) so that I/O events can be processed when + // needed. Since we use the async libusb interface, we do the monitoring + // ourselves. That means we always need keep track of the libusb file + // descriptor list. + + // Subscribe to changes to the list of file-descriptors we have to monitor. + libusb_set_pollfd_notifiers(libusb_ctx_, + [](int fd, short events, void *user_data) { + ((LibusbDiscoverer*)user_data)->on_add_pollfd(fd, events); + }, + [](int fd, void *user_data) { + ((LibusbDiscoverer*)user_data)->on_remove_pollfd(fd); + }, this); + + // Fetch initial list of file-descriptors we have to monitor. + // Note: this will fail on Windows. Since this is used for epoll, we need a + // different approach for Windows anyway. + const struct libusb_pollfd** pollfds = libusb_get_pollfds(libusb_ctx_); + if (!pollfds) { + return deinit(2), -1; + } + + for (size_t i = 0; pollfds[i]; ++i) { + on_add_pollfd(pollfds[i]->fd, pollfds[i]->events); + } + libusb_free_pollfds(pollfds); + pollfds = nullptr; + + } else { + FIBRE_LOG(D) << "Using internal event loop thread"; + + // This code path is taken on Windows (which does not support epoll) + run_internal_event_loop_ = true; + internal_event_loop_thread_ = new std::thread([](void* ctx) { + ((LibusbDiscoverer*)ctx)->internal_event_loop(); + }, this); + } + + if (libusb_has_capability(LIBUSB_CAP_HAS_HOTPLUG)) { + // This code path is taken on Linux + FIBRE_LOG(D) << "Using libusb native hotplug detection"; + + // Subscribe to hotplug events + int result = libusb_hotplug_register_callback(libusb_ctx_, + (libusb_hotplug_event)(LIBUSB_HOTPLUG_EVENT_DEVICE_ARRIVED | LIBUSB_HOTPLUG_EVENT_DEVICE_LEFT), + LIBUSB_HOTPLUG_ENUMERATE /* trigger callback for all currently connected devices too */, + LIBUSB_HOTPLUG_MATCH_ANY, LIBUSB_HOTPLUG_MATCH_ANY, LIBUSB_HOTPLUG_MATCH_ANY, + [](struct libusb_context *ctx, struct libusb_device *dev, libusb_hotplug_event event, void *user_data){ + return ((LibusbDiscoverer*)user_data)->on_hotplug(dev, event); + }, this, &hotplug_callback_handle_); + if (LIBUSB_SUCCESS != result) { + FIBRE_LOG(E) << "Error subscribing to hotplug events"; + hotplug_callback_handle_ = 0; + return deinit(3), -1; + } + + } else { + // This code path is taken on Windows + FIBRE_LOG(D) << "Using periodic polling to discover devices"; + + poll_devices_now(); // this will also start a timer to poll again periodically + } + + if (!pollfds && libusb_has_capability(LIBUSB_CAP_HAS_HOTPLUG)) { + // The hotplug callback handler above is not yet thread-safe. To make it thread-safe + // we'd need to post it on the application's event loop. + FIBRE_LOG(E) << "Hotplug detection with separate libusb thread will cause trouble."; + } + + return 0; +} + +int LibusbDiscoverer::deinit(int stage) { + // TODO: verify that all devices are closed and hotplug detection is disabled + + if (stage > 3 && libusb_has_capability(LIBUSB_CAP_HAS_HOTPLUG)) { + libusb_hotplug_deregister_callback(libusb_ctx_, hotplug_callback_handle_); + } + + if (stage > 3 && device_polling_timer_) { + event_loop_->cancel_timer(device_polling_timer_); + device_polling_timer_ = nullptr; + } + + if (stage > 2 && !run_internal_event_loop_) { + // Deregister libusb events from our event loop. + const struct libusb_pollfd** pollfds = libusb_get_pollfds(libusb_ctx_); + if (pollfds) { + for (size_t i = 0; pollfds[i]; ++i) { + on_remove_pollfd(pollfds[i]->fd); + } + libusb_free_pollfds(pollfds); + pollfds = nullptr; + } + } + + if (stage > 1 && !run_internal_event_loop_) { + libusb_set_pollfd_notifiers(libusb_ctx_, nullptr, nullptr, nullptr); + } + + if (stage > 0 && run_internal_event_loop_) { + run_internal_event_loop_ = false; + libusb_interrupt_event_handler(libusb_ctx_); + internal_event_loop_thread_->join(); + delete internal_event_loop_thread_; + internal_event_loop_thread_ = nullptr; + } + + if (stage > 0) { + // TODO: we should probably deinit and close all connected channels + for (auto& dev: known_devices_) { + libusb_unref_device(dev.second.dev); + } + } + + // FIXME: the libusb_hotplug_deregister_callback call will still trigger a + // usb_handler event. We need to wait until this has finished before we + // truly discard libusb resources + // Update: is this still relevant? + //usleep(100000); + + if (stage > 0) { + libusb_exit(libusb_ctx_); + libusb_ctx_ = nullptr; + } + + event_loop_ = nullptr; + + return 0; +} + +bool try_parse_key(const char* begin, const char* end, const char* key, int* val) { + char buf[end - begin + 1]; + memcpy(buf, begin, end - begin); + buf[end - begin] = 0; + + char fmt1[strlen(key) + 6]; + memcpy(fmt1, key, strlen(key)); + memcpy(fmt1 + strlen(key), "=0x%x", 6); + + char fmt2[strlen(key) + 4]; + memcpy(fmt2, key, strlen(key)); + memcpy(fmt2 + strlen(key), "=%d", 4); + + return sscanf(buf, fmt1, val) == 1 + || sscanf(buf, fmt2, val) == 1; +} + +/** + * @brief Starts looking for Fibre devices accessible through USB. + * + * Multiple discovery requests can be active at the same time but beware that a + * channel will be announced to all matching subscribers so be careful with access + * multiplexing. + * + * If the function succeeds, an opaque context pointer is returned which must be + * passed to stop_channel_discovery() to terminate this particular request. + * + * @param specs: Specifies the constraints to consider. Must be either empty or + * of the format "key1=val1,key2=val2" where the available keys are: + * + * bus, address, idProduct, idVendor, bInterfaceClass, + * bInterfaceSubClass, bInterfaceProtocol + * + * The value can be either a integer in decimal or hexadecimal notation + * (0x1234). + * Omitted keys are ignored during filtering. + * + * @param on_found_channels: Invoked when a matching pair of RX/TX channels is found. + * This callback will also be called for any matching channels that already exist when + * the discovery is started. + */ +void LibusbDiscoverer::start_channel_discovery(const char* specs, size_t specs_len, ChannelDiscoveryContext** handle, Completer& on_found_channels) { + FIBRE_LOG(D) << "starting discovery with filter \"" << std::string(specs, specs_len) << "\""; + + const char* prev_delim = specs; + + InterfaceSpecs interface_specs; + + while (prev_delim < specs + specs_len) { + const char* next_delim = std::find(prev_delim, specs + specs_len, ','); + + bool success = try_parse_key(prev_delim, next_delim, "bus", &interface_specs.bus) + || try_parse_key(prev_delim, next_delim, "address", &interface_specs.address) + || try_parse_key(prev_delim, next_delim, "idVendor", &interface_specs.vendor_id) + || try_parse_key(prev_delim, next_delim, "idProduct", &interface_specs.product_id) + || try_parse_key(prev_delim, next_delim, "bInterfaceClass", &interface_specs.interface_class) + || try_parse_key(prev_delim, next_delim, "bInterfaceSubClass", &interface_specs.interface_subclass) + || try_parse_key(prev_delim, next_delim, "bInterfaceProtocol", &interface_specs.interface_protocol); + + if (!success) { + FIBRE_LOG(E) << "could not interpret channel discovery specs"; + on_found_channels.complete({kFibreInvalidArgument, nullptr, nullptr}); + return; + } + + prev_delim = std::min(next_delim + 1, specs + specs_len); + } + + ChannelDiscoveryContext* subscription = new ChannelDiscoveryContext{interface_specs, &on_found_channels}; + subscriptions_.push_back(subscription); + + for (auto& dev: known_devices_) { + consider_device(dev.second.dev, subscription); + } + + if (handle) { + *handle = subscription; + } + + return; +} + +/** + * @brief Stops an object discovery process that was started with start_channel_discovery(). + * + * Channels which were already discovered will remain open. However if the discovery is restarted + * it is possible that the same channels are returned again (their pointers need not match the old instance). + * + * The discovery must be considered still in progress until the callback is + * invoked with kFibreCancelled. + */ +int LibusbDiscoverer::stop_channel_discovery(ChannelDiscoveryContext* handle) { + auto it = std::find(subscriptions_.begin(), subscriptions_.end(), handle); + + if (it == subscriptions_.end()) { + FIBRE_LOG(E) << "Not an active subscription"; + return -1; + } + + subscriptions_.erase(it); + delete handle; + return 0; +} + +/** + * @brief Runs the event handling loop. This function blocks until + * run_internal_event_loop_ is false. + * + * This loop is only executed on Windows. On other platforms the provided EventLoop is used. + */ +void LibusbDiscoverer::internal_event_loop() { + while (run_internal_event_loop_) + libusb_handle_events(libusb_ctx_); +} + +void LibusbDiscoverer::on_event_loop_iteration() { + if (event_loop_timer_) { + FIBRE_LOG(D) << "cancelling event loop timer"; + event_loop_->cancel_timer(event_loop_timer_); + event_loop_timer_ = nullptr; + } + + timeval tv = { .tv_sec = 0, .tv_usec = 0 }; + if (libusb_handle_events_timeout(libusb_ctx_, &tv) != 0) { + FIBRE_LOG(E) << "libusb_handle_events_timeout() failed"; + } + + timeval timeout; + if (libusb_get_next_timeout(libusb_ctx_, &timeout)) { + float timeout_sec = (float)timeout.tv_sec + (float)timeout.tv_usec * 1e-6; + FIBRE_LOG(D) << "setting event loop timeout to " << timeout_sec << " s"; + event_loop_timer_ = event_loop_->call_later(timeout_sec, [](void* ctx) { + ((LibusbDiscoverer*)ctx)->on_event_loop_iteration(); + }, this); + } +} + +/** + * @brief Called when libusb wants to add a file descriptor to our event loop. + */ +void LibusbDiscoverer::on_add_pollfd(int fd, short events) { + event_loop_->register_event(fd, events, [](void* ctx) { + ((LibusbDiscoverer*)ctx)->on_event_loop_iteration(); + }, this); +} + +/** + * @brief Called when libusb wants to remove a file descriptor to our event loop. + */ +void LibusbDiscoverer::on_remove_pollfd(int fd) { + event_loop_->deregister_event(fd); +} + +/** + * @brief Called by libusb when a USB device was plugged in or out. + * + * If this function returns a non-zero value, libusb removes this filter. + */ +int LibusbDiscoverer::on_hotplug(struct libusb_device *dev, + libusb_hotplug_event event) { + uint8_t bus_number = libusb_get_bus_number(dev); + uint8_t dev_number = libusb_get_device_address(dev); + + if (LIBUSB_HOTPLUG_EVENT_DEVICE_ARRIVED == event) { + FIBRE_LOG(D) << "device arrived: bus " << (int)bus_number << ", " << (int)dev_number; + + for (auto& subscription: subscriptions_) { + consider_device(dev, subscription); + } + + // add empty placeholder to the list of known devices + known_devices_[bus_number << 8 | dev_number] = { + .dev = libusb_ref_device(dev), + .handle = nullptr + }; + + } else if (LIBUSB_HOTPLUG_EVENT_DEVICE_LEFT == event) { + FIBRE_LOG(D) << "device left: bus " << (int)bus_number << ", " << (int)dev_number; + + auto it = known_devices_.find(bus_number << 8 | dev_number); + + if (it != known_devices_.end()) { + for (auto& ep: it->second.ep_in) { + ep->deinit(); + } + for (auto& ep: it->second.ep_out) { + ep->deinit(); + } + if (it->second.handle) { + libusb_close(it->second.handle); + } + + known_devices_.erase(it); + } + + libusb_unref_device(dev); + + } else { + FIBRE_LOG(W) << "Unexpected event: " << event; + } + + return 0; +} + +void LibusbDiscoverer::poll_devices_now() { + FIBRE_LOG(D) << "poll_devices_now() called."; + + device_polling_timer_ = nullptr; + + libusb_device** list = nullptr; + ssize_t n_devices = libusb_get_device_list(libusb_ctx_, &list); + std::unordered_map current_devices; + + if (n_devices < 0) { + FIBRE_LOG(W) << "libusb_get_device_list() failed."; + } else { + for (ssize_t i = 0; i < n_devices; ++i) { + uint8_t bus_number = libusb_get_bus_number(list[i]); + uint8_t dev_number = libusb_get_device_address(list[i]); + current_devices[bus_number << 8 | dev_number] = list[i]; + } + + // Call on_hotplug for all new devices + for (auto& dev: current_devices) { + if (known_devices_.find(dev.first) == known_devices_.end()) { + on_hotplug(dev.second, LIBUSB_HOTPLUG_EVENT_DEVICE_ARRIVED); + } + } + + // Call on_hotplug for all lost devices + + std::vector lost_devices; + + for (auto& dev: known_devices_) { + if (current_devices.find(dev.first) == current_devices.end()) { + lost_devices.push_back(dev.second.dev); + } + } + + for (auto& dev: lost_devices) { + on_hotplug(dev, LIBUSB_HOTPLUG_EVENT_DEVICE_LEFT); + } + + libusb_free_device_list(list, 1 /* unref the devices */); + } + + // It's possible that the discoverer was deinited during this function. + if (event_loop_) { + device_polling_timer_ = event_loop_->call_later(1.0, [](void* ctx) { + ((LibusbDiscoverer*)ctx)->poll_devices_now(); + }, this); + } +} + +void LibusbDiscoverer::consider_device(struct libusb_device *device, ChannelDiscoveryContext* subscription) { + uint8_t bus_number = libusb_get_bus_number(device); + uint8_t dev_number = libusb_get_device_address(device); + + bool mismatch = (subscription->interface_specs.bus != -1 && bus_number != subscription->interface_specs.bus) + || (subscription->interface_specs.address != -1 && dev_number != subscription->interface_specs.address); + + if (mismatch) { + return; + } + + if (subscription->interface_specs.vendor_id != -1 || subscription->interface_specs.product_id != -1) { + struct libusb_device_descriptor dev_desc; + int result = libusb_get_device_descriptor(device, &dev_desc); + if (result != LIBUSB_SUCCESS) { + FIBRE_LOG(W) << "Failed to get device descriptor: " << result; + } + + mismatch = (subscription->interface_specs.vendor_id != -1 && dev_desc.idVendor != subscription->interface_specs.vendor_id) + || (subscription->interface_specs.product_id != -1 && dev_desc.idProduct != subscription->interface_specs.product_id); + + if (mismatch) { + return; + } + } + + static libusb_device_handle *handle = nullptr; + struct libusb_config_descriptor* config_desc = nullptr; + + if (libusb_get_active_config_descriptor(device, &config_desc) != LIBUSB_SUCCESS) { + FIBRE_LOG(E) << "Failed to get active config descriptor: " << sys_err(); + } else { + for (uint8_t i = 0; i < config_desc->bNumInterfaces; ++i) { + for (int j = 0; j < config_desc->interface[i].num_altsetting; ++j) { + // TODO: probably we should only chose one alt setting + const struct libusb_interface_descriptor* intf_desc = &(config_desc->interface[i].altsetting[j]); + + mismatch = (subscription->interface_specs.interface_class != -1 && intf_desc->bInterfaceClass != subscription->interface_specs.interface_class) + || (subscription->interface_specs.interface_subclass != -1 && intf_desc->bInterfaceSubClass != subscription->interface_specs.interface_subclass) + || (subscription->interface_specs.interface_protocol != -1 && intf_desc->bInterfaceProtocol != subscription->interface_specs.interface_protocol); + if (mismatch) { + continue; + } + + // We found a matching interface. Now find one bulk IN and one bulk OUT endpoint. + const libusb_endpoint_descriptor* libusb_ep_in = nullptr; + const libusb_endpoint_descriptor* libusb_ep_out = nullptr; + for (uint8_t k = 0; k < intf_desc->bNumEndpoints; ++k) { + if ((intf_desc->endpoint[k].bmAttributes & 0x03) == LIBUSB_TRANSFER_TYPE_BULK + && (intf_desc->endpoint[k].bEndpointAddress & 0x80) == LIBUSB_ENDPOINT_IN) { + libusb_ep_in = &intf_desc->endpoint[k]; + } else if ((intf_desc->endpoint[k].bmAttributes & 0x03) == LIBUSB_TRANSFER_TYPE_BULK + && (intf_desc->endpoint[k].bEndpointAddress & 0x80) == LIBUSB_ENDPOINT_OUT) { + libusb_ep_out = &intf_desc->endpoint[k]; + } + } + + Device& my_dev = known_devices_[bus_number << 8 | dev_number]; + + // If the same device was already returned in a previous discovery + // then it will already be open. + + if (!my_dev.handle) { + int result = libusb_open(device, &my_dev.handle); + if (LIBUSB_SUCCESS != result) { + FIBRE_LOG(E) << "Could not open USB device: " << result; + continue; + } + } + + int result = libusb_claim_interface(my_dev.handle, i); + if (LIBUSB_SUCCESS != result) { + FIBRE_LOG(E) << "Could not claim interface " << i << " on USB device: " << result; + continue; + } + + EventLoop* event_loop = using_sparate_libusb_thread_ ? event_loop_ : nullptr; + + LibusbBulkInEndpoint* ep_in = new LibusbBulkInEndpoint(); + if (libusb_ep_in && ep_in->init(event_loop, my_dev.handle, libusb_ep_in->bEndpointAddress)) { + my_dev.ep_in.push_back(ep_in); + } else { + delete ep_in; + ep_in = nullptr; + } + + LibusbBulkOutEndpoint* ep_out = new LibusbBulkOutEndpoint(); + if (libusb_ep_out && ep_out->init(event_loop, my_dev.handle, libusb_ep_out->bEndpointAddress)) { + my_dev.ep_out.push_back(ep_out); + } else { + delete ep_out; + ep_out = nullptr; + } + + if (subscription->on_found_channels) { + subscription->on_found_channels->complete({kFibreOk, ep_in, ep_out}); + } + } + } + + libusb_free_config_descriptor(config_desc); + config_desc = nullptr; + } +} + + +/* LibusbBulkEndpoint --------------------------------------------------------*/ + +template +bool LibusbBulkEndpoint::init(EventLoop* event_loop, libusb_device_handle* handle, uint8_t endpoint_id) { + event_loop_ = event_loop; + handle_ = handle; + transfer_ = libusb_alloc_transfer(0); + endpoint_id_ = endpoint_id; + return true; +} + +template +bool LibusbBulkEndpoint::deinit() { + if (completer_) { + FIBRE_LOG(E) << "Transfer still in progress. This is gonna be messy."; + } + + libusb_free_transfer(transfer_); + transfer_ = nullptr; + return true; +} + +template +void LibusbBulkEndpoint::start_transfer(bufptr_t buffer, TransferHandle* handle, Completer& completer) { + if (handle) { + *handle = reinterpret_cast(this); + } + + if (completer_) { + FIBRE_LOG(E) << "transfer already in progress"; + completer.complete({kStreamError, nullptr}); + return; + } + + if (!handle_) { + FIBRE_LOG(E) << "device not open"; + completer.complete({kStreamError, nullptr}); + return; + } + + auto direct_callback = [](struct libusb_transfer* transfer){ + ((LibusbBulkEndpoint*)transfer->user_data)->on_transfer_finished(); + }; + + // This callback is used if we start our own libusb thread + // separate from the application's event loop thread + auto indirect_callback = [](struct libusb_transfer* transfer){ + ((LibusbBulkEndpoint*)transfer->user_data)->event_loop_->post( + [](void* ctx) { + ((LibusbBulkEndpoint*)ctx)->on_transfer_finished(); + }, transfer->user_data + ); + }; + + //FIBRE_LOG(D) << "transfer of size " << buffer.size(); + libusb_fill_bulk_transfer(transfer_, handle_, endpoint_id_, + buffer.begin(), buffer.size(), + event_loop_ ? indirect_callback : direct_callback, + this, kBulkTimeoutMs); + + completer_ = &completer; + submit_transfer(); +} + +template +void LibusbBulkEndpoint::cancel_transfer(TransferHandle transfer_handle) { + if (!completer_) { + FIBRE_LOG(E) << "transfer not in progress"; + return; + } + + libusb_cancel_transfer(transfer_); +} + +template +void LibusbBulkEndpoint::submit_transfer() { + int result = libusb_submit_transfer(transfer_); + if (LIBUSB_SUCCESS == result) { + // ok + FIBRE_LOG(T) << "started USB transfer on EP " << as_hex(endpoint_id_); + } else if (LIBUSB_ERROR_NO_DEVICE == result) { + FIBRE_LOG(W) << "couldn't start USB transfer on EP " << as_hex(endpoint_id_) << ": " << libusb_error_name(result); + safe_complete(completer_, {kStreamClosed, nullptr}); + } else { + FIBRE_LOG(W) << "couldn't start USB transfer on EP " << as_hex(endpoint_id_) << ": " << libusb_error_name(result); + safe_complete(completer_, {kStreamError, nullptr}); + } +} + +template +void LibusbBulkEndpoint::on_transfer_finished() { + // We ignore timeouts here and just retry. If the application wishes to have + // a timeout on the transfer it can just call cancel_transfer() after a while. + if (transfer_->status == LIBUSB_TRANSFER_TIMED_OUT) { + submit_transfer(); + return; + } + + // On linux we get LIBUSB_TRANSFER_STALL on the RX pipe when the cable is plugged out + StreamStatus status = LIBUSB_TRANSFER_COMPLETED == transfer_->status ? kStreamOk : + LIBUSB_TRANSFER_CANCELLED == transfer_->status ? kStreamCancelled : + LIBUSB_TRANSFER_STALL == transfer_->status ? kStreamClosed : + LIBUSB_TRANSFER_NO_DEVICE == transfer_->status ? kStreamClosed : + kStreamError; + + (status == kStreamError ? FIBRE_LOG(W) : FIBRE_LOG(T)) + << "USB transfer on EP " << as_hex(endpoint_id_) << " finished with " << libusb_error_name(transfer_->status); + + uint8_t* end = std::max(transfer_->buffer + transfer_->actual_length, transfer_->buffer); + + safe_complete(completer_, {status, end}); +} diff --git a/Firmware/fibre/cpp/platform_support/libusb_transport.hpp b/Firmware/fibre/cpp/platform_support/libusb_transport.hpp new file mode 100644 index 00000000..5d383691 --- /dev/null +++ b/Firmware/fibre/cpp/platform_support/libusb_transport.hpp @@ -0,0 +1,124 @@ +#ifndef __FIBRE_USB_DISCOVERER_HPP +#define __FIBRE_USB_DISCOVERER_HPP + +#include "../event_loop.hpp" +#include "../async_stream.hpp" +#include + +#include +#include +#include +#include + +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* 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& on_found_channels); + int stop_channel_discovery(ChannelDiscoveryContext* handle); + +private: + struct Device { + struct libusb_device* dev; + struct libusb_device_handle* handle; + std::vector ep_in; + std::vector 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 known_devices_; // key: bus_number << 8 | dev_number + std::vector subscriptions_; +}; + +template +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& 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* completer_ = nullptr; +}; + +class FIBRE_PRIVATE LibusbBulkInEndpoint : public LibusbBulkEndpoint, public AsyncStreamSource { +public: + void start_read(bufptr_t buffer, TransferHandle* handle, Completer& completer) final { + start_transfer(buffer, handle, completer); + } + + void cancel_read(TransferHandle transfer_handle) final { + cancel_transfer(transfer_handle); + } +}; + +class FIBRE_PRIVATE LibusbBulkOutEndpoint : public LibusbBulkEndpoint, public AsyncStreamSink { +public: + void start_write(cbufptr_t buffer, TransferHandle* handle, Completer& 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 \ No newline at end of file diff --git a/Firmware/fibre/cpp/print_utils.hpp b/Firmware/fibre/cpp/print_utils.hpp new file mode 100644 index 00000000..2457494e --- /dev/null +++ b/Firmware/fibre/cpp/print_utils.hpp @@ -0,0 +1,136 @@ +#ifndef __FIBRE_PRINT_UTILS_HPP +#define __FIBRE_PRINT_UTILS_HPP + +#include +#include + +namespace fibre { + +template +constexpr size_t hex_digits() { + return (std::numeric_limits::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 +bool hex_string_to_int(const char * str, size_t length, TInt* output) { + constexpr size_t N_DIGITS = hex_digits(); + 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 +bool hex_string_to_int(const char * str, TInt* output) { + return hex_string_to_int(str, hex_digits(), output); +} + +template +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(&str[i * hex_digits()], &output[i])) + return false; + } + return true; +} + +template +bool hex_string_to_int_arr(const char * str, TInt (&output)[ICount]) { + return hex_string_to_int_arr(str, hex_digits() * ICount, output); +} + +// TODO: move to print_utils.hpp +template +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()] = '\0'; + + for (size_t i = 0; i < hex_digits(); ++i) { + str[prefix_length + hex_digits() - 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() + 3]; // 3 additional characters 0x and \0 +}; + +template +std::ostream& operator<<(std::ostream& stream, const HexPrinter& printer) { + // TODO: specialize for char + return stream << printer.to_string(); +} + +template +HexPrinter as_hex(T val, bool prefix = true) { return HexPrinter(val, prefix); } + +template +class HexArrayPrinter { +public: + HexArrayPrinter(T* ptr, size_t length) : ptr_(ptr), length_(length) {} + T* ptr_; + size_t length_; +}; + +template +TStream& operator<<(TStream& stream, const HexArrayPrinter& 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 +HexArrayPrinter as_hex(T (&val)[ILength]) { return HexArrayPrinter(val, ILength); } + +template +HexArrayPrinter as_hex(generic_bufptr_t buffer) { return HexArrayPrinter(buffer.begin(), buffer.size()); } + +} + +#endif // __FIBRE_PRINT_UTILS_HPP \ No newline at end of file diff --git a/Firmware/fibre/cpp/protocol.cpp b/Firmware/fibre/cpp/protocol.cpp deleted file mode 100644 index e8285c87..00000000 --- a/Firmware/fibre/cpp/protocol.cpp +++ /dev/null @@ -1,187 +0,0 @@ - -/* Includes ------------------------------------------------------------------*/ - -#include -#include - -#include -#include - -/* 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_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_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(length), - 0 - }; - header[2] = calc_crc8(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_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 offset = read_le(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(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(&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(&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(&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(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; -} diff --git a/Firmware/fibre/cpp/stream_utils.hpp b/Firmware/fibre/cpp/stream_utils.hpp new file mode 100644 index 00000000..fc865442 --- /dev/null +++ b/Firmware/fibre/cpp/stream_utils.hpp @@ -0,0 +1,177 @@ +#ifndef __FIBRE_STREAM_UTILS_HPP +#define __FIBRE_STREAM_UTILS_HPP + +#include "async_stream.hpp" +#include + +namespace fibre { + +template +class BufferedStreamSink : Completer { +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 +class AsyncStreamSinkMultiplexer : public AsyncStreamSink, Completer { +public: + AsyncStreamSinkMultiplexer(AsyncStreamSink& sink) : sink_(sink) {} + + void start_write(cbufptr_t buffer, TransferHandle* handle, Completer& 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*> slots_[NSlots]; + size_t active_slot_ = 0; + TransferHandle transfer_handle_ = 0; +}; + +} + +#endif // __FIBRE_STREAM_UTILS_HPP diff --git a/Firmware/fibre/python/fibre/__init__.py b/Firmware/fibre/python/fibre/__init__.py index 0b8ed1b4..216f2e06 100644 --- a/Firmware/fibre/python/fibre/__init__.py +++ b/Firmware/fibre/python/fibre/__init__.py @@ -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 diff --git a/Firmware/fibre/python/fibre/discovery.py b/Firmware/fibre/python/fibre/discovery.py deleted file mode 100644 index 72548ce8..00000000 --- a/Firmware/fibre/python/fibre/discovery.py +++ /dev/null @@ -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("= 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 diff --git a/Firmware/fibre/python/fibre/libfibre.py b/Firmware/fibre/python/fibre/libfibre.py new file mode 100644 index 00000000..038f51d7 --- /dev/null +++ b/Firmware/fibre/python/fibre/libfibre.py @@ -0,0 +1,678 @@ +#!/bin/python + +from ctypes import * +import asyncio +import os +from itertools import count, takewhile +import struct +from types import MethodType +import concurrent +import threading +import time +from fibre.utils import Logger, Event +import platform + +lib_names = { + ('Linux', 'x86_64'): 'libfibre-linux-amd64.so', + ('Linux', 'armv7l'): 'libfibre-linux-armhf.so', + ('Windows', 'AMD64'): 'libfibre-windows-amd64.dll', + ('Darwin', 'x86_64'): 'libfibre-macos-x86.dylib' +} + +system_desc = (platform.system(), platform.machine()) + +lib_dir = os.path.join( + os.path.dirname(os.path.dirname(os.path.dirname(os.path.realpath(__file__)))), + 'cpp') + +def test_path(path): + return path if os.path.isfile(path) else None + +lib_path = (test_path(os.path.join(lib_dir, 'libfibre.so')) or + test_path(os.path.join(lib_dir, 'libfibre.dll')) or + (test_path(os.path.join(lib_dir, lib_names[system_desc])) if (system_desc in lib_names) else None)) + +if lib_path is None: + raise ModuleNotFoundError("This package has no precompiled libfibre for your platform ({} {}). " + "Go to fibre/cpp/ and run `make` to compile libfibre for your platform.".format(*system_desc)) + +lib = windll.LoadLibrary(lib_path) if os.name == 'nt' else cdll.LoadLibrary(lib_path) + + +# libfibre definitions --------------------------------------------------------# + +PostSignature = CFUNCTYPE(c_void_p, CFUNCTYPE(None, c_void_p), POINTER(c_int)) +RegisterEventSignature = CFUNCTYPE(c_int, c_int, c_uint32, CFUNCTYPE(None, c_void_p), POINTER(c_int)) +DeregisterEventSignature = CFUNCTYPE(c_int, c_int) +CallLaterSignature = CFUNCTYPE(c_void_p, c_float, CFUNCTYPE(None, c_void_p), POINTER(c_int)) +CancelTimerSignature = CFUNCTYPE(c_int, c_void_p) +ConstructObjectSignature = CFUNCTYPE(None, c_void_p, c_void_p, c_void_p, c_void_p, c_size_t) +DestroyObjectSignature = CFUNCTYPE(None, c_void_p, c_void_p) + +OnFoundObjectSignature = CFUNCTYPE(None, c_void_p, c_void_p) +OnStoppedSignature = CFUNCTYPE(None, c_void_p, c_int) + +OnAttributeAddedSignature = CFUNCTYPE(None, c_void_p, c_void_p, c_void_p, c_size_t, c_void_p, c_void_p, c_size_t) +OnAttributeRemovedSignature = CFUNCTYPE(None, c_void_p, c_void_p) +OnFunctionAddedSignature = CFUNCTYPE(None, c_void_p, c_void_p, c_void_p, c_size_t, POINTER(c_char_p), POINTER(c_char_p), POINTER(c_char_p), POINTER(c_char_p)) +OnFunctionRemovedSignature = CFUNCTYPE(None, c_void_p, c_void_p) + +OnCallCompletedSignature = CFUNCTYPE(None, c_void_p, c_int, c_char_p) + +kFibreOk = 0 +kFibreCancelled = 1 +kFibreClosed = 2 +kFibreInvalidArgument = 3 +kFibreInternalError = 4 + +class LibFibreVersion(Structure): + _fields_ = [ + ("major", c_uint16), + ("minor", c_uint16), + ("patch", c_uint16), + ] + + def __repr__(self): + return "{}.{}.{}".format(self.major, self.minor, self.patch) + +libfibre_get_version = lib.libfibre_get_version +libfibre_get_version.argtypes = [] +libfibre_get_version.restype = POINTER(LibFibreVersion) + +version = libfibre_get_version().contents +if version.major != 0: + raise Exception("Incompatible libfibre version: {}".format(version)) + +libfibre_open = lib.libfibre_open +libfibre_open.argtypes = [PostSignature, RegisterEventSignature, DeregisterEventSignature, CallLaterSignature, CancelTimerSignature, ConstructObjectSignature, DestroyObjectSignature, c_void_p] +libfibre_open.restype = c_void_p + +libfibre_close = lib.libfibre_close +libfibre_close.argtypes = [c_void_p] +libfibre_close.restype = None + +libfibre_start_discovery = lib.libfibre_start_discovery +libfibre_start_discovery.argtypes = [c_void_p, c_char_p, c_size_t, c_void_p, OnFoundObjectSignature, OnStoppedSignature, c_void_p] +libfibre_start_discovery.restype = c_void_p + +libfibre_stop_discovery = lib.libfibre_stop_discovery +libfibre_stop_discovery.argtypes = [c_void_p, c_void_p] +libfibre_stop_discovery.restype = None + +libfibre_subscribe_to_interface = lib.libfibre_subscribe_to_interface +libfibre_subscribe_to_interface.argtypes = [c_void_p, OnAttributeAddedSignature, OnAttributeRemovedSignature, OnFunctionAddedSignature, OnFunctionRemovedSignature, c_void_p] +libfibre_subscribe_to_interface.restype = None + +libfibre_get_attribute = lib.libfibre_get_attribute +libfibre_get_attribute.argtypes = [c_void_p, c_void_p, POINTER(c_void_p)] +libfibre_get_attribute.restype = c_int + +libfibre_start_call = lib.libfibre_start_call +libfibre_start_call.argtypes = [c_void_p, c_void_p, c_char_p, c_size_t, c_char_p, c_size_t, c_void_p, OnCallCompletedSignature, c_void_p] +libfibre_start_call.restype = None + +libfibre_cancel_call = lib.libfibre_cancel_call +libfibre_cancel_call.argtypes = [c_void_p] +libfibre_cancel_call.restype = None + + +# libfibre wrapper ------------------------------------------------------------# + +class ObjectLostError(Exception): + def __init__(self): + super(Exception, self).__init__("the object disappeared") + +def _get_exception(status): + if status == kFibreOk: + return None + elif status == kFibreCancelled: + return asyncio.CancelledError() + elif status == kFibreClosed: + return ObjectLostError() + elif status == kFibreInvalidArgument: + return ArgumentError() + elif status == kFibreInternalError: + return Exception("internal libfibre error") + else: + return Exception("unknown libfibre error {}".format(status)) + +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, libfibre, value): + value = self._target_type(value) + return struct.pack(self._struct_format, value) + def deserialize(self, libfibre, buffer): + value = struct.unpack(self._struct_format, buffer) + value = value[0] if len(value) == 1 else value + return self._target_type(value) + +class ObjectPtrCodec(): + """ + Serializer/deserializer for an object reference + + libfibre transcodes object references internally from/to something that can + be sent over the wire and understood by the remote instance. + """ + def get_length(self): + return struct.calcsize("P") + def serialize(self, libfibre, value): + if value is None: + return struct.pack("P", 0) + elif isinstance(value, RemoteObject): + return struct.pack("P", value._obj_handle) + else: + raise TypeError("Expected value of type RemoteObject or None but got '{}'. An example for a RemoteObject is this expression: odrv0.axis0.controller._input_pos_property".format(type(value).__name__)) + def deserialize(self, libfibre, buffer): + handle = struct.unpack("P", buffer)[0] + return None if handle == 0 else libfibre._objects[handle] + + +codecs = { + 'int8': StructCodec(" " + print_arglist(self._outputs) if len(self._outputs) == 1 else + " -> (" + print_arglist(self._outputs) + ")") + +class RemoteAttribute(object): + def __init__(self, libfibre, attr_handle, intf_handle, intf_name, magic_getter, magic_setter): + self._libfibre = libfibre + self._attr_handle = attr_handle + self._intf_handle = intf_handle + self._intf_name = intf_name + self._magic_getter = magic_getter + self._magic_setter = magic_setter + + def _get_obj(self, instance): + py_intf = self._libfibre._load_py_intf(self._intf_name, self._intf_handle) + + obj_handle = c_void_p(0) + status = libfibre_get_attribute(instance._obj_handle, self._attr_handle, byref(obj_handle)) + if status != kFibreOk: + raise _get_exception(status) + + return self._libfibre._objects[obj_handle.value] + + def __get__(self, instance, owner): + if not instance: + return self + + if self._magic_getter: + return self._get_obj(instance).read() + else: + return self._get_obj(instance) + + def __set__(self, instance, val): + if self._magic_setter: + self._get_obj(instance).exchange(val) + else: + raise Exception("this attribute cannot be written to") + + +class RemoteObject(object): + """ + Base class for interfaces of remote objects. + """ + __sealed__ = False + + def __init__(self, libfibre, obj_handle): + self.__class__._refcount += 1 + + self._libfibre = libfibre + self._obj_handle = obj_handle + self._on_lost = concurrent.futures.Future() # TODO: maybe we can do this with conc + + # Ensure that assignments to undefined attributes raise an exception + self.__sealed__ = True + + def __setattr__(self, key, value): + if self.__sealed__ and not hasattr(self, key): + raise AttributeError("Attribute {} not found".format(key)) + object.__setattr__(self, key, value) + + #def __del__(self): + # print("unref") + # libfibre_unref_obj(self._obj_handle) + + def _dump(self, indent, depth): + if self._obj_handle is None: + return "[object lost]" + + try: + if depth <= 0: + return "..." + lines = [] + for key in dir(self.__class__): + if key.startswith('_'): + continue + class_member = getattr(self.__class__, key) + if isinstance(class_member, RemoteFunction): + lines.append(indent + class_member._dump(key)) + elif isinstance(class_member, RemoteAttribute): + val = getattr(self, key) + if isinstance(val, RemoteObject) and not class_member._magic_getter: + lines.append(indent + key + (": " if depth == 1 else ":\n") + val._dump(indent + " ", depth - 1)) + else: + if isinstance(val, RemoteObject) and class_member._magic_getter: + val_str = get_user_name(val) + else: + val_str = str(val) + property_type = str(class_member._get_obj(self).__class__.read._outputs[0][1]) + lines.append(indent + key + ": " + val_str + " (" + property_type + ")") + else: + lines.append(indent + key + ": " + str(type(val))) + except: + return "[failed to dump object]" + + return "\n".join(lines) + + def __str__(self): + return self._dump("", depth=2) + + def __repr__(self): + return self.__str__() + + def _destroy(self): + libfibre = self._libfibre + on_lost = self._on_lost + + self._libfibre = None + self._obj_handle = None + self._on_lost = None + + self.__class__._refcount -= 1 + if self.__class__._refcount == 0: + libfibre.interfaces.pop(self.__class__._handle) + + on_lost.set_result(True) + + +class LibFibre(): + def __init__(self): + self.loop = asyncio.get_event_loop() + + # We must keep a reference to these function objects so they don't get + # garbage collected. + self.c_post = PostSignature(self._post) + self.c_register_event = RegisterEventSignature(self._register_event) + self.c_deregister_event = DeregisterEventSignature(self._deregister_event) + self.c_call_later = CallLaterSignature(self._call_later) + self.c_cancel_timer = CancelTimerSignature(self._cancel_timer) + self.c_construct_object = ConstructObjectSignature(self._construct_object) + self.c_destroy_object = DestroyObjectSignature(self._destroy_object) + self.c_on_found_object = OnFoundObjectSignature(self._on_found_object) + self.c_on_discovery_stopped = OnStoppedSignature(self._on_discovery_stopped) + self.c_on_attribute_added = OnAttributeAddedSignature(self._on_attribute_added) + self.c_on_attribute_removed = OnAttributeRemovedSignature(self._on_attribute_removed) + self.c_on_function_added = OnFunctionAddedSignature(self._on_function_added) + self.c_on_function_removed = OnFunctionRemovedSignature(self._on_function_removed) + + self.timer_map = {} + self.eventfd_map = {} + self.interfaces = {} # key: libfibre handle, value: python class + self.discovery_processes = {} # key: ID, value: python dict + self._objects = {} # key: libfibre handle, value: pyhton class + + self.ctx = c_void_p(libfibre_open( + self.c_post, + self.c_register_event, self.c_deregister_event, + self.c_call_later, self.c_cancel_timer, + self.c_construct_object, self.c_destroy_object, None)) + + def _post(self, callback, ctx): + self.loop.call_soon_threadsafe(callback, ctx) + + def _register_event(self, event_fd, events, callback, ctx): + self.eventfd_map[event_fd] = events + if (events & 1): + self.loop.add_reader(event_fd, callback, ctx) + if (events & 4): + self.loop.add_writer(event_fd, callback, ctx) + if (events & 0xfffffffa): + raise Exception("unsupported event mask " + str(events)) + return 0 + + def _deregister_event(self, event_fd): + events = self.eventfd_map.pop(event_fd) + if (events & 1): + self.loop.remove_reader(event_fd) + if (events & 4): + self.loop.remove_writer(event_fd) + return 0 + + def _call_later(self, delay, callback, ctx): + timer_id = insert_with_new_id(self.timer_map, self.loop.call_later(delay, callback, ctx)) + return timer_id + + def _cancel_timer(self, timer_id): + self.timer_map.pop(timer_id).cancel() + return 0 + + def _load_py_intf(self, name, intf_handle): + """ + Creates a new python type for the specified libfibre interface handle or + returns the existing python type if one was already create before. + + Behind the scenes the python type will react to future events coming + from libfibre, such as functions/attributes being added/removed. + """ + if intf_handle in self.interfaces: + return self.interfaces[intf_handle] + else: + if name is None: + name = "anonymous_interface_" + str(intf_handle) + py_intf = self.interfaces[intf_handle] = type(name, (RemoteObject,), {'_handle': intf_handle, '_refcount': 0}) + #exit(1) + libfibre_subscribe_to_interface(intf_handle, self.c_on_attribute_added, self.c_on_attribute_removed, self.c_on_function_added, self.c_on_function_removed, intf_handle) + return py_intf + + def _construct_object(self, ctx, obj, intf, name, name_length): + #increment_libfibre_refcount() + name = None if name is None else string_at(name, name_length).decode('utf-8') + py_intf = self._load_py_intf(name, intf) + assert(not obj in self._objects) + self._objects[obj] = py_intf(self, obj) + + def _destroy_object(self, ctx, obj): + py_obj = self._objects.pop(obj) + py_obj._destroy() + #decrement_lib_refcount() + + def _on_found_object(self, ctx, obj): + py_obj = self._objects[obj] + # notify the subscriber + asyncio.ensure_future(self.discovery_processes[ctx]['callback'](py_obj)) + + def _on_discovery_stopped(self, ctx, result): + print("discovery stopped") + + def _on_attribute_added(self, ctx, attr, name, name_length, subintf, subintf_name, subintf_name_length): + name = string_at(name, name_length).decode('utf-8') + subintf_name = None if subintf_name is None else string_at(subintf_name, subintf_name_length).decode('utf-8') + intf = self.interfaces[ctx] + + magic_getter = not subintf_name is None and subintf_name.startswith("fibre.Property<") and subintf_name.endswith(">") + magic_setter = not subintf_name is None and subintf_name.startswith("fibre.Property") + + setattr(intf, name, RemoteAttribute(self, attr, subintf, subintf_name, magic_getter, magic_setter)) + if magic_getter or magic_setter: + setattr(intf, "_" + name + "_property", RemoteAttribute(self, attr, subintf, subintf_name, False, False)) + + def _on_attribute_removed(self, ctx, attr): + print("attribute removed") + + def _on_function_added(self, ctx, func, name, name_length, input_names, input_codecs, output_names, output_codecs): + name = string_at(name, name_length).decode('utf-8') + inputs = list(decode_arg_list(input_names, input_codecs)) + outputs = list(decode_arg_list(output_names, output_codecs)) + intf = self.interfaces[ctx] + setattr(intf, name, RemoteFunction(self, func, inputs, outputs)) + + def _on_function_removed(self, ctx, func): + print("function removed") + + def start_discovery(self, path, on_obj_discovered, cancellation_token): + buf = path.encode('ascii') + + discovery = { + 'handle': c_void_p(0), + 'callback': on_obj_discovered + } + discovery_id = insert_with_new_id(self.discovery_processes, discovery) + + cancellation_token.subscribe(lambda: libfibre_stop_discovery(self.ctx, discovery['handle'])) + libfibre_start_discovery(self.ctx, buf, len(buf), byref(discovery['handle']), self.c_on_found_object, self.c_on_discovery_stopped, discovery_id) + + + +libfibre = None + +def run_event_loop(): + global libfibre + global terminate_libfibre + + loop = asyncio.new_event_loop() + asyncio.set_event_loop(loop) + + terminate_libfibre = loop.create_future() + libfibre = LibFibre() + + libfibre.loop.run_until_complete(terminate_libfibre) + + libfibre_close(libfibre.ctx) + + # Detach all objects that still exist + # TODO: the proper way would be either of these + # - provide a libfibre function to destroy an object on-demand which we'd + # call before libfibre_close(). + # - have libfibre_close() report the destruction of all objects + + while len(libfibre._objects): + libfibre._objects.pop(list(libfibre._objects.keys())[0])._destroy() + assert(len(libfibre.interfaces) == 0) + + libfibre = None + + +lock = threading.Lock() +libfibre_refcount = 0 +libfibre_thread = None + +def increment_libfibre_refcount(): + global libfibre_refcount + global libfibre_thread + + with lock: + libfibre_refcount += 1 + #print("inc refcount to {}".format(libfibre_refcount)) + + if libfibre_refcount == 1: + libfibre_thread = threading.Thread(target = run_event_loop) + libfibre_thread.start() + + while libfibre is None: + time.sleep(0.1) + +def decrement_lib_refcount(): + global libfibre_refcount + global libfibre_thread + + with lock: + #print("dec refcount from {}".format(libfibre_refcount)) + libfibre_refcount -= 1 + + if libfibre_refcount == 0: + libfibre.loop.call_soon_threadsafe(lambda: terminate_libfibre.set_result(True)) + + # It's unlikely that releasing fibre from a fibre callback is ok. If + # there is a valid scenario for this then we can remove the assert. + assert(libfibre_thread != threading.current_thread()) + + libfibre_thread.join() + libfibre_thread = None + + +def find_all(path, serial_number, + on_object_discovered, + search_cancellation_token, + channel_termination_token, + logger): + """ + Starts scanning for Fibre objects that match the specified path spec and calls + the callback for each Fibre object that is found. + + This function is non-blocking and thread-safe. + """ + + async def on_object_discovered_filter(obj): + increment_libfibre_refcount() + channel_termination_token.subscribe(lambda: decrement_lib_refcount()) + if serial_number is None or (await fibre.utils.get_serial_number_str(obj)) == serial_number: + result = on_object_discovered(obj) + if not result is None: + await result + + increment_libfibre_refcount() + search_cancellation_token.subscribe(lambda: decrement_lib_refcount()) + + libfibre.loop.call_soon_threadsafe(lambda: libfibre.start_discovery( + path, + on_object_discovered_filter, + search_cancellation_token)) + +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 object is connected and then returns that object + """ + 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 + + +def get_user_name(obj): + """ + Can be overridden by the application to return the user-facing name of an + object. + """ + return "[anonymous object]" diff --git a/Firmware/fibre/python/fibre/protocol.py b/Firmware/fibre/python/fibre/protocol.py index c9d65735..d7d38e30 100644 --- a/Firmware/fibre/python/fibre/protocol.py +++ b/Firmware/fibre/python/fibre/protocol.py @@ -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) diff --git a/Firmware/fibre/python/fibre/remote_object.py b/Firmware/fibre/python/fibre/remote_object.py deleted file mode 100644 index 57364333..00000000 --- a/Firmware/fibre/python/fibre/remote_object.py +++ /dev/null @@ -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(" 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 = {} diff --git a/Firmware/fibre/python/fibre/serial_transport.py b/Firmware/fibre/python/fibre/serial_transport.py deleted file mode 100644 index e737dfca..00000000 --- a/Firmware/fibre/python/fibre/serial_transport.py +++ /dev/null @@ -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) diff --git a/Firmware/fibre/python/fibre/shell.py b/Firmware/fibre/python/fibre/shell.py index c5a7257a..6fbfdf1f 100644 --- a/Firmware/fibre/python/fibre/shell.py +++ b/Firmware/fibre/python/fibre/shell.py @@ -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") diff --git a/Firmware/fibre/python/fibre/tcp_transport.py b/Firmware/fibre/python/fibre/tcp_transport.py deleted file mode 100644 index 5cfb1692..00000000 --- a/Firmware/fibre/python/fibre/tcp_transport.py +++ /dev/null @@ -1,85 +0,0 @@ - -import sys -import socket -import time -import traceback -import fibre.protocol -from fibre.utils import wait_any, TimeoutError - -def noprint(x): - pass - -class TCPTransport(fibre.protocol.StreamSource, fibre.protocol.StreamSink): - def __init__(self, dest_addr, dest_port, logger): - # TODO: FIXME: use IPv6 - # Problem: getaddrinfo fails if the resolver returns an - # IPv4 address, but we are using AF_INET6 - #family = socket.AF_INET6 if socket.has_ipv6 else socket.AF_INET - family = socket.AF_INET - self.sock = socket.socket(family, socket.SOCK_STREAM) - # TODO: Determine the right address to use from the list - self.target = socket.getaddrinfo(dest_addr, dest_port, family)[0][4] - # TODO: this blocks until a connection is established, or the system cancels it - self.sock.connect(self.target) - - def process_bytes(self, buffer): - self.sock.send(buffer) - - 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. - """ - # convert deadline to seconds (floating point) - deadline = None if deadline is None else max(deadline - time.monotonic(), 0) - self.sock.settimeout(deadline) - try: - data = self.sock.recv(n_bytes, socket.MSG_WAITALL) # receive n_bytes - return data - except socket.timeout: - # if we got a timeout data will still be none, so we call recv again - # this time in non blocking state and see if we can get some data - try: - return self.sock.recv(n_bytes, socket.MSG_DONTWAIT) - except socket.timeout: - raise TimeoutError - - 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 {}".format(n_bytes, len(result))) - return result - - - -def discover_channels(path, serial_number, callback, cancellation_token, channel_termination_token, logger): - """ - Tries to connect to a TCP server based on the path spec. - This function blocks until cancellation_token is set. - Channels spawned by this function run until channel_termination_token is set. - """ - try: - dest_addr = ':'.join(path.split(":")[:-1]) - dest_port = int(path.split(":")[-1]) - except (ValueError, IndexError): - raise Exception('"{}" is not a valid TCP destination. The format should be something like "localhost:1234".' - .format(path)) - - while not cancellation_token.is_set(): - try: - tcp_transport = fibre.tcp_transport.TCPTransport(dest_addr, dest_port, logger) - stream2packet_input = fibre.protocol.PacketFromStreamConverter(tcp_transport) - packet2stream_output = fibre.protocol.StreamBasedPacketSink(tcp_transport) - channel = fibre.protocol.Channel( - "TCP device {}:{}".format(dest_addr, dest_port), - stream2packet_input, packet2stream_output, - channel_termination_token, logger) - except: - #logger.debug("TCP channel init failed. More info: " + traceback.format_exc()) - pass - else: - callback(channel) - wait_any(None, cancellation_token, channel._channel_broken) - time.sleep(1) diff --git a/Firmware/fibre/python/fibre/udp_transport.py b/Firmware/fibre/python/fibre/udp_transport.py deleted file mode 100644 index d0e4c2f0..00000000 --- a/Firmware/fibre/python/fibre/udp_transport.py +++ /dev/null @@ -1,57 +0,0 @@ - -import sys -import socket -import time -import traceback -import fibre.protocol -from fibre.utils import wait_any - -def noprint(x): - pass - -class UDPTransport(fibre.protocol.PacketSource, fibre.protocol.PacketSink): - def __init__(self, dest_addr, dest_port, logger): - # TODO: FIXME: use IPv6 - # Problem: getaddrinfo fails if the resolver returns an - # IPv4 address, but we are using AF_INET6 - #family = socket.AF_INET6 if socket.has_ipv6 else socket.AF_INET - family = socket.AF_INET - self.sock = socket.socket(family, socket.SOCK_DGRAM) - # TODO: Determine the right address to use from the list - self.target = socket.getaddrinfo(dest_addr,dest_port, family)[0][4] - - def process_packet(self, buffer): - self.sock.sendto(buffer, self.target) - - def get_packet(self, deadline): - # TODO: implement deadline - data, _ = self.sock.recvfrom(1024) - return data - -def discover_channels(path, serial_number, callback, cancellation_token, channel_termination_token, logger): - """ - Tries to connect to a UDP server based on the path spec. - This function blocks until cancellation_token is set. - Channels spawned by this function run until channel_termination_token is set. - """ - try: - dest_addr = ':'.join(path.split(":")[:-1]) - dest_port = int(path.split(":")[-1]) - except (ValueError, IndexError): - raise Exception('"{}" is not a valid UDP destination. The format should be something like "localhost:1234".' - .format(path)) - - while not cancellation_token.is_set(): - try: - udp_transport = fibre.udp_transport.UDPTransport(dest_addr, dest_port, logger) - channel = fibre.protocol.Channel( - "UDP device {}:{}".format(dest_addr, dest_port), - udp_transport, udp_transport, - channel_termination_token, logger) - except: - logger.debug("UDP channel init failed. More info: " + traceback.format_exc()) - pass - else: - callback(channel) - wait_any(None, cancellation_token, channel._channel_broken) - time.sleep(1) diff --git a/Firmware/fibre/python/fibre/usbbulk_transport.py b/Firmware/fibre/python/fibre/usbbulk_transport.py deleted file mode 100644 index 6643c8b2..00000000 --- a/Firmware/fibre/python/fibre/usbbulk_transport.py +++ /dev/null @@ -1,216 +0,0 @@ -# requires pyusb -# pip install --pre pyusb - -import usb.core -import usb.util -import sys -import time -import fibre.protocol -import traceback -import platform -from fibre.utils import TimeoutError - -# Currently we identify fibre-enabled devices by VID,PID -# TODO: identify by USB descriptors -WELL_KNOWN_VID_PID_PAIRS = [ - (0x1209, 0x0D31), - (0x1209, 0x0D32), - (0x1209, 0x0D33) -] - -class USBBulkTransport(fibre.protocol.PacketSource, fibre.protocol.PacketSink): - def __init__(self, dev, logger): - self._logger = logger - self.dev = dev - self.intf = None - self._name = "USB device {}:{}".format(dev.idVendor, dev.idProduct) - self._was_damaged = False - - ## - # information about the connected device - ## - def info(self): - # loop through configurations - string = "" - for cfg in self.dev: - string += "ConfigurationValue {0}\n".format(cfg.bConfigurationValue) - for intf in cfg: - string += "\tInterfaceNumber {0},{1}\n".format(intf.bInterfaceNumber, intf.bAlternateSetting) - for ep in intf: - string += "\t\tEndpointAddress {0}\n".format(ep.bEndpointAddress) - return string - - def init(self): - # Under some conditions, the Linux USB/libusb stack ends up in a corrupt - # state where there are a few packets in a receive queue but a call - # to epr.read() does not return these packet until a new packet arrives. - # This undesirable queue can be cleared by resetting the device. - # On windows this would cause file-not-found errors in subsequent dev calls - if platform.system() != 'Windows': - self.dev.reset() - - #self.dev.set_configuration() # no args: set first configuration - - # Find the best interface - self.cfg = self.dev.get_active_configuration() - custom_interfaces = [i for i in self.cfg.interfaces() if i.bInterfaceClass == 0x00 and i.bInterfaceSubClass == 0x01] - cdc_interfaces = [i for i in self.cfg.interfaces() if i.bInterfaceClass == 0x0a and i.bInterfaceSubClass == 0x00] - all_compatible_interfaces = custom_interfaces + cdc_interfaces - if len(all_compatible_interfaces) == 0: - raise Exception("the device has no compatible interfaces") - self.intf = all_compatible_interfaces[0] - - # Try to detach kernel driver from interface - try: - if self.dev.is_kernel_driver_active(self.intf.bInterfaceNumber): - self.dev.detach_kernel_driver(self.intf.bInterfaceNumber) - self._logger.debug("Detached Kernel Driver") - else: - self._logger.debug("Kernel Driver was not attached") - except NotImplementedError: - pass #is_kernel_driver_active not implemented on Windows - - # find write endpoint (first OUT endpoint) - self.epw = usb.util.find_descriptor(self.intf, - custom_match = \ - lambda e: \ - usb.util.endpoint_direction(e.bEndpointAddress) == \ - usb.util.ENDPOINT_OUT - ) - assert self.epw is not None - self._logger.debug("EndpointAddress for writing {}".format(self.epw.bEndpointAddress)) - # find read endpoint (first IN endpoint) - self.epr = usb.util.find_descriptor(self.intf, - custom_match = \ - lambda e: \ - usb.util.endpoint_direction(e.bEndpointAddress) == \ - usb.util.ENDPOINT_IN - ) - assert self.epr is not None - self._logger.debug("EndpointAddress for reading {}".format(self.epr.bEndpointAddress)) - - def deinit(self): - if not self.intf is None: - usb.util.release_interface(self.dev, self.intf) - - def process_packet(self, usbBuffer): - try: - ret = self.epw.write(usbBuffer, 0) - if self._was_damaged: - self._logger.debug("Recovered from USB halt/stall condition") - self._was_damaged = False - return ret - except usb.core.USBError as ex: - if ex.errno == 19 or ex.errno == 32: # "no such device", "pipe error" - raise fibre.protocol.ChannelBrokenException() - elif ex.errno is None or ex.errno == 60 or ex.errno == 110: # timeout - raise TimeoutError() - else: - self._logger.debug("error in usbbulk_transport.py, process_packet") - self._logger.debug(traceback.format_exc()) - self._logger.debug("halt condition: {}".format(ex.errno)) - self._logger.debug(str(ex)) - # Try resetting halt/stall condition - try: - self.deinit() - self.init() - except usb.core.USBError: - raise fibre.protocol.ChannelBrokenException() - # Retry transfer - self._was_damaged = True - raise fibre.protocol.ChannelDamagedException() - - def get_packet(self, deadline): - try: - bufferLen = self.epr.wMaxPacketSize - timeout = max(int((deadline - time.monotonic()) * 1000), 0) - ret = self.epr.read(bufferLen, timeout) - if self._was_damaged: - self._logger.debug("Recovered from USB halt/stall condition") - self._was_damaged = False - return bytearray(ret) - except usb.core.USBError as ex: - if ex.errno == 19 or ex.errno == 32: # "no such device", "pipe error" - raise fibre.protocol.ChannelBrokenException() - elif ex.errno is None or ex.errno == 60 or ex.errno == 110: # timeout - raise TimeoutError() - else: - self._logger.debug("error in usbbulk_transport.py, process_packet") - self._logger.debug(traceback.format_exc()) - self._logger.debug("halt condition: {}".format(ex.errno)) - self._logger.debug(str(ex)) - # Try resetting halt/stall condition - try: - self.deinit() - self.init() - except usb.core.USBError: - raise fibre.protocol.ChannelBrokenException() - # Retry transfer - self._was_damaged = True - raise fibre.protocol.ChannelDamagedException() - - -def discover_channels(path, serial_number, callback, cancellation_token, channel_termination_token, logger): - """ - Scans for USB devices 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 or path == "": - bus = None - address = None - else: - try: - bus = int(path.split(":")[0]) - address = int(path.split(":")[1]) - except (ValueError, IndexError): - raise Exception("{} is not a valid USB path specification. " - "Expected a string of the format BUS:DEVICE where BUS " - "and DEVICE are integers.".format(path)) - - known_devices = [] - def device_matcher(device): - #print(" test {:04X}:{:04X}".format(device.idVendor, device.idProduct)) - try: - if (device.bus, device.address) in known_devices: - return False - if bus != None and device.bus != bus: - return False - if address != None and device.address != address: - return False - if serial_number != None and device.serial_number != serial_number: - return False - if (device.idVendor, device.idProduct) not in WELL_KNOWN_VID_PID_PAIRS: - return False - except: - return False - return True - - while not cancellation_token.is_set(): - # logger.debug("USB discover loop") - devices = usb.core.find(find_all=True, custom_match=device_matcher) - for usb_device in devices: - try: - bulk_device = USBBulkTransport(usb_device, logger) - logger.debug(bulk_device.info()) - bulk_device.init() - channel = fibre.protocol.Channel( - "USB device bus {} device {}".format(usb_device.bus, usb_device.address), - bulk_device, bulk_device, channel_termination_token, logger) - channel.usb_device = usb_device # for debugging only - except usb.core.USBError as ex: - if ex.errno == 13: - # TODO: this is an ODrive specific message and should live outside of the fibre library - logger.warn("I found a USB device that looks like an ODrive (bus {}, device {}) but I can't access it. Try running `sudo odrivetool udev-setup`, then unplug and replug the device.".format(usb_device.bus, usb_device.address)) - known_devices.append((usb_device.bus, usb_device.address)) - elif ex.errno == 16: - logger.debug("USB device busy. I'll reset it and try again.") - usb_device.reset() - continue - else: - logger.warn("USB device init failed (bus {}, device {}). Ignoring this device. More info: ".format(usb_device.bus, usb_device.address) + traceback.format_exc()) - known_devices.append((usb_device.bus, usb_device.address)) - else: - known_devices.append((usb_device.bus, usb_device.address)) - callback(channel) - time.sleep(1) diff --git a/Firmware/fibre/python/fibre/utils.py b/Firmware/fibre/python/fibre/utils.py index d0bbe946..4a6f8965 100644 --- a/Firmware/fibre/python/fibre/utils.py +++ b/Firmware/fibre/python/fibre/utils.py @@ -66,13 +66,14 @@ class Event(): Invokes the specified handler exactly once as soon as the specified event is set. If the event is already set, the handler is invoked immediately. + The subscribers are called in the reverse order in which they subscribed. Returns a function that can be invoked to unsubscribe. """ if handler is None: raise TypeError self._mutex.acquire() try: - self._subscribers.append(handler) + self._subscribers.insert(0, handler) if self._evt.is_set(): handler() finally: diff --git a/Firmware/fibre/python/setup.py b/Firmware/fibre/python/setup.py index 9edccf6b..85f266f3 100644 --- a/Firmware/fibre/python/setup.py +++ b/Firmware/fibre/python/setup.py @@ -76,9 +76,7 @@ setup( license='MIT', url = 'https://github.com/samuelsadok/fibre', keywords = ['communication', 'transport-layer', 'rpc'], - install_requires = [ - 'appdirs', # Used to find caching directory - ], + install_requires = [], #package_data={'': ['version.txt']}, classifiers = [], ) diff --git a/Firmware/freertos_vars.h b/Firmware/freertos_vars.h index 64a7e784..8eb9a268 100644 --- a/Firmware/freertos_vars.h +++ b/Firmware/freertos_vars.h @@ -6,9 +6,8 @@ // List of semaphores extern osSemaphoreId sem_usb_irq; -extern osSemaphoreId sem_uart_dma; -extern osSemaphoreId sem_usb_rx; -extern osSemaphoreId sem_usb_tx; +extern osMessageQId uart_event_queue; +extern osMessageQId usb_event_queue; extern osSemaphoreId sem_can; extern osThreadId defaultTaskHandle; diff --git a/Firmware/odrive-interface.yaml b/Firmware/odrive-interface.yaml index 43126cbd..bb5f37c4 100644 --- a/Firmware/odrive-interface.yaml +++ b/Firmware/odrive-interface.yaml @@ -140,7 +140,18 @@ interfaces: This setting has no effect if `enable_can0` is also true. This setting has no effect on ODrive v3.2 or earlier. Changing this setting requires a reboot. - enable_ascii_protocol_on_usb: bool + usb_cdc_protocol: + type: StreamProtocolType + doc: | + The protocol that's being run on the device's virtual COM port on + USB. + Note that the ODrive has two independent interfaces on USB: One + is the virtual COM port (affected by this option) and the other + one is a vendor specific interface which always runs Fibre. + So changing this option does not affect the working of odrivetool. + uart0_protocol: StreamProtocolType + uart1_protocol: StreamProtocolType + uart2_protocol: StreamProtocolType max_regen_current: float32 brake_resistance: type: float32 @@ -961,6 +972,23 @@ valuetypes: Enc2: {doc: This mode is not supported on ODrive v3.x.} MechBrake: {doc: This is to support external mechanical brakes.} + ODrive.StreamProtocolType: + values: + Fibre: + doc: | + Machine-to-machine protocol which gives access to all features of the + ODrive. This protocol is used by the official odrivetool and GUI. + Developers who wish to interact with this protocol are advised to do + so through libfibre. + Ascii: + doc: | + Human readable protocol designed for easy implementation for cases + where the use of libfibre is not desired or feasible. + Refer to [this page](ascii-protocol.md) for details. + Stdout: {doc: Output of printf(). Only intended for developers who modify + ODrive firmware.} + AsciiAndStdout: {doc: Combination of `Ascii` and `Stdout`.} + ODrive.Can.Protocol: values: {Simple: } diff --git a/Firmware/tup.config.default b/Firmware/tup.config.default index b2d49106..70ee66d0 100644 --- a/Firmware/tup.config.default +++ b/Firmware/tup.config.default @@ -1,8 +1,6 @@ # Copy this file to tup.config and adapt it to your needs # make sure this fits your board #CONFIG_BOARD_VERSION=v3.5-24V -CONFIG_USB_PROTOCOL=native -CONFIG_UART_PROTOCOL=ascii CONFIG_DEBUG=false CONFIG_DOCTEST=false diff --git a/ODrive_Workspace.code-workspace b/ODrive_Workspace.code-workspace index 8c1fe5c3..34f5b335 100644 --- a/ODrive_Workspace.code-workspace +++ b/ODrive_Workspace.code-workspace @@ -62,7 +62,46 @@ "iomanip": "cpp", "optional": "cpp", "sstream": "cpp", - "utils.h": "c" + "utils.h": "c", + "atomic": "cpp", + "hash_map": "cpp", + "strstream": "cpp", + "bit": "cpp", + "bitset": "cpp", + "cinttypes": "cpp", + "codecvt": "cpp", + "compare": "cpp", + "complex": "cpp", + "concepts": "cpp", + "forward_list": "cpp", + "list": "cpp", + "map": "cpp", + "set": "cpp", + "unordered_set": "cpp", + "iterator": "cpp", + "memory_resource": "cpp", + "numeric": "cpp", + "random": "cpp", + "ratio": "cpp", + "string": "cpp", + "hash_set": "cpp", + "slist": "cpp", + "mutex": "cpp", + "ranges": "cpp", + "shared_mutex": "cpp", + "stop_token": "cpp", + "thread": "cpp", + "cfenv": "cpp", + "typeindex": "cpp", + "valarray": "cpp", + "variant": "cpp", + "types": "cpp", + "config": "cpp", + "containers": "cpp", + "readerwriter": "cpp", + "charconv": "cpp", + "image": "cpp", + "imageutils": "cpp" } } } diff --git a/docs/developer-guide.md b/docs/developer-guide.md index 1088de8d..e49fa5b9 100644 --- a/docs/developer-guide.md +++ b/docs/developer-guide.md @@ -109,18 +109,6 @@ To customize the compile time parameters, copy or rename the file `Firmware/tup. __CONFIG_BOARD_VERSION__: The board version you're using. Can be `v3.1`, `v3.2`, `v3.3`, `v3.4-24V`, `v3.4-48V`, `v3.5-24V`, `v3.5-48V`, etc. Check for a label on the upper side of the ODrive to find out which version you have. Some ODrive versions don't specify the voltage: in that case you can read the value of the main capacitors: 120uF are 48V ODrives, 470uF are 24V ODrives. -__CONFIG_USB_PROTOCOL__: Defines which protocol the ODrive should use on the USB interface. - * `native`: The native ODrive protocol. Use this if you want to use the python tools in this repo. Can maybe work with macOS. - * `native-stream`: Like the native ODrive protocol, but the ODrive will treat the USB connection exactly as if it was a UART connection. __You may need to use this if you're on macOS__. This is necessary because macOS doesn't grant our python tools sufficient low-level access to treat the device as the USB device that it is. - * `none`: Disable USB. The device will still show up when plugged in but it will ignore any commands. - - **Note**: There is a second USB interface that is always a serial port. - -__CONFIG_UART_PROTOCOL__: Defines which protocol the ODrive should use on the UART interface (GPIO1 and GPIO2). Note that UART is only supported on ODrive v3.3 and higher. - * `native`: The native ODrive protocol. Use this if you're connecting the ODrive to a PC using UART and want to use the python tools to control and setup the ODrive. - * `ascii`: The ASCII protocol. Use this option if you control the ODrive with an Arduino. The ODrive Arduino library is not yet updated to the native protocol. - * `none`: Disable UART. - __CONFIG_DEBUG__: Defines wether debugging will be enabled when compiling the firmware; specifically the `-g -gdwarf-2` flags. Note that printf debugging will only function if your tup.config specifies the `USB_PROTOCOL` or `UART_PROTOCOL` as stdout and `DEBUG_PRINT` is defined. See the IDE specific documentation for more information. You can also modify the compile-time defaults for all `.config` parameters. You will find them if you search for `AxisConfig`, `MotorConfig`, etc. diff --git a/docs/hoverboard.md b/docs/hoverboard.md index a743a21c..6c9d51c3 100644 --- a/docs/hoverboard.md +++ b/docs/hoverboard.md @@ -131,11 +131,11 @@ We also have to reboot to activate the PWM input. ```txt odrv0.config.gpio3_pwm_mapping.min = -200 odrv0.config.gpio3_pwm_mapping.max = 200 -odrv0.config.gpio3_pwm_mapping.endpoint = odrv0.axis0.controller._remote_attributes['input_vel'] +odrv0.config.gpio3_pwm_mapping.endpoint = odrv0.axis0.controller._input_vel_property odrv0.config.gpio4_pwm_mapping.min = -200 odrv0.config.gpio4_pwm_mapping.max = 200 -odrv0.config.gpio4_pwm_mapping.endpoint = odrv0.axis1.controller._remote_attributes['input_vel'] +odrv0.config.gpio4_pwm_mapping.endpoint = odrv0.axis1.controller._input_vel_property odrv0.save_configuration() odrv0.reboot() diff --git a/docs/interfaces.md b/docs/interfaces.md index 0b9cb6c3..dc8b8e04 100644 --- a/docs/interfaces.md +++ b/docs/interfaces.md @@ -7,12 +7,21 @@ The ODrive can be controlled over various ports and protocols. If you're comfort ### Table of contents -- [Pinout](#pinout) -- [Native Protocol](#native-protocol) -- [ASCII protocol](#ascii-protocol) -- [Step/direction](#stepdirection) -- [RC PWM input](#rc-pwm-input) -- [Ports](#ports) +- [Interfaces](#interfaces) + - [Table of contents](#table-of-contents) + - [Pinout](#pinout) + - [Native Protocol](#native-protocol) + - [Python](#python) + - [Other languages](#other-languages) + - [ASCII protocol](#ascii-protocol) + - [Arduino](#arduino) + - [Step/direction](#stepdirection) + - [RC PWM input](#rc-pwm-input) + - [Analog input](#analog-input) + - [Ports](#ports) + - [USB](#usb) + - [UART](#uart) + - [CAN Simple Protocol](#can-simple-protocol) @@ -118,7 +127,7 @@ Any of the numerical parameters that are writable from the ODrive Tool can be ho odrv0.config.gpio4_mode = GPIO_MODE_PWM0 odrv0.config.gpio4_pwm_mapping.min = -2 odrv0.config.gpio4_pwm_mapping.max = 2 - odrv0.config.gpio4_pwm_mapping.endpoint = odrv0.axis0.controller._remote_attributes['input_pos'] + odrv0.config.gpio4_pwm_mapping.endpoint = odrv0.axis0.controller._input_pos_property ``` Note: you can disable the input by setting `odrv0.config.gpio4_pwm_mapping.endpoint = None` 4. Save the configuration and reboot diff --git a/tools/odrive/configuration.py b/tools/odrive/configuration.py index ff220a8d..52ae57cb 100644 --- a/tools/odrive/configuration.py +++ b/tools/odrive/configuration.py @@ -2,33 +2,51 @@ import json import os import tempfile -import fibre.remote_object +import fibre.libfibre from odrive.utils import OperationAbortedException, yes_no_prompt -def get_dict(obj, is_config_object): +def obj_to_path(root, obj): + for k in dir(root): + v = getattr(root, k) + if not k.startswith('_') and isinstance(v, fibre.libfibre.RemoteObject): + if v == obj: + return k + subpath = obj_to_path(v, obj) + if not subpath is None: + return k + "." + subpath + return None + +def get_dict(root, obj, is_config_object): result = {} - for (k,v) in obj._remote_attributes.items(): - if isinstance(v, fibre.remote_object.RemoteProperty) and is_config_object: - result[k] = v.get_value() - elif isinstance(v, fibre.remote_object.RemoteObject): - sub_dict = get_dict(v, k == 'config') + + for k in dir(obj): + v = getattr(obj, k) + if k.startswith('_') and k.endswith('_property') and is_config_object: + v = v.read() + if isinstance(v, fibre.libfibre.RemoteObject): + v = obj_to_path(root, v) + result[k[1:-9]] = v + elif not k.startswith('_') and isinstance(v, fibre.libfibre.RemoteObject): + sub_dict = get_dict(root, v, (k == 'config') or is_config_object) if sub_dict != {}: result[k] = sub_dict + return result def set_dict(obj, path, config_dict): errors = [] for (k,v) in config_dict.items(): name = path + ("." if path != "" else "") + k - if not k in obj._remote_attributes: + if not k in dir(obj): errors.append("Could not restore {}: property not found on device".format(name)) continue - remote_attribute = obj._remote_attributes[k] - if isinstance(remote_attribute, fibre.remote_object.RemoteObject): - errors += set_dict(remote_attribute, name, v) + if isinstance(v, dict): + errors += set_dict(getattr(obj, k), name, v) else: try: - remote_attribute.set_value(v) + remote_attribute = getattr(obj, '_' + k + '_property') + #if isinstance(v, str) and isinstance() + remote_attribute.exchange(v) except Exception as ex: errors.append("Could not restore {}: {}".format(name, str(ex))) return errors @@ -54,7 +72,7 @@ def backup_config(device, filename, logger): if not yes_no_prompt("The file {} already exists. Do you want to override it?".format(filename), True): raise OperationAbortedException() - data = get_dict(device, False) + data = get_dict(device, device, False) with open(filename, 'w') as file: json.dump(data, file) logger.info("Configuration saved.") diff --git a/tools/odrive/dfu.py b/tools/odrive/dfu.py index bfeb0a81..19d0cddc 100755 --- a/tools/odrive/dfu.py +++ b/tools/odrive/dfu.py @@ -232,7 +232,7 @@ def put_into_dfu_mode(device, cancellation_token): print("Putting device {} into DFU mode...".format(device.__channel__.usb_device.serial_number)) try: device.enter_dfu_mode() - except fibre.ChannelBrokenException: + except fibre.ObjectLostError: pass # this is expected because the device reboots if platform.system() == "Windows": show_deferred_message("Still waiting for the device to reappear.\n" diff --git a/tools/odrive/shell.py b/tools/odrive/shell.py index a146871a..c1ee7b58 100644 --- a/tools/odrive/shell.py +++ b/tools/odrive/shell.py @@ -41,37 +41,17 @@ interactive_variables = {} discovered_devices = [] -def did_discover_device(odrive, logger, app_shutdown_token): - """ - Handles the discovery of new devices by displaying a - message and making the device available to the interactive - console - """ - serial_number = odrive.serial_number if hasattr(odrive, 'serial_number') else "[unknown serial number]" - if serial_number in discovered_devices: - verb = "Reconnected" - index = discovered_devices.index(serial_number) - else: - verb = "Connected" - discovered_devices.append(serial_number) - index = len(discovered_devices) - 1 - interactive_name = "odrv" + str(index) +def benchmark(odrv): + import asyncio + import time - # Publish new ODrive to interactive console - interactive_variables[interactive_name] = odrive - globals()[interactive_name] = odrive # Add to globals so tab complete works - logger.notify("{} to ODrive {:012X} as {}".format(verb, serial_number, interactive_name)) + async def measure_async(): + start = time.monotonic() + futures = [odrv.vbus_voltage for i in range(1000)] +# data = [await f for f in futures] +# print("took " + str(time.monotonic() - start) + " seconds. Average is " + str(sum(data) / len(data))) - # Subscribe to disappearance of the device - odrive.__channel__._channel_broken.subscribe(lambda: did_lose_device(interactive_name, logger, app_shutdown_token)) - -def did_lose_device(interactive_name, logger, app_shutdown_token): - """ - Handles the disappearance of a device by displaying - a message. - """ - if not app_shutdown_token.is_set(): - logger.warn("Oh no {} disappeared".format(interactive_name)) + fibre.libfibre.libfibre.loop.call_soon_threadsafe(lambda: asyncio.ensure_future(measure_async())) def launch_shell(args, logger, app_shutdown_token): """ @@ -84,6 +64,7 @@ def launch_shell(args, logger, app_shutdown_token): interactive_variables = { 'start_liveplotter': start_liveplotter, 'dump_errors': dump_errors, + 'benchmark': benchmark, 'oscilloscope_dump': oscilloscope_dump, 'dump_interrupts': dump_interrupts, 'dump_dma': dump_dma, diff --git a/tools/odrive/tests/analog_input_test.py b/tools/odrive/tests/analog_input_test.py index 3e4041e9..5fa8e096 100644 --- a/tools/odrive/tests/analog_input_test.py +++ b/tools/odrive/tests/analog_input_test.py @@ -93,7 +93,7 @@ class TestAnalogInput(): odrive.disable_mappings() setattr(odrive.handle.config, 'gpio' + str(analog_in_num+1) + '_mode', GPIO_MODE_ANALOG_IN) - analog_mapping.endpoint = odrive.handle.axis0.controller._remote_attributes['input_pos'] + analog_mapping.endpoint = odrive.handle.axis0.controller._input_pos_property analog_mapping.min = min_val analog_mapping.max = max_val odrive.save_config_and_reboot() diff --git a/tools/odrive/tests/can_test.py b/tools/odrive/tests/can_test.py index a9275e24..75a4671f 100644 --- a/tools/odrive/tests/can_test.py +++ b/tools/odrive/tests/can_test.py @@ -225,9 +225,10 @@ class TestSimpleCAN(): test_assert_eq([msg['current_state'] for msg in heartbeats], [1] * len(heartbeats)) logger.debug('testing reboot...') + test_assert_eq(odrive.handle._on_lost.done(), False) my_cmd('reboot') time.sleep(0.5) - if len(odrive.handle._remote_attributes) != 0: + if not odrive.handle._on_lost is None: raise TestFailed("device didn't seem to reboot") odrive.handle = None time.sleep(2.0) diff --git a/tools/odrive/tests/nvm_test.py b/tools/odrive/tests/nvm_test.py index 72ed2061..3fe0f984 100644 --- a/tools/odrive/tests/nvm_test.py +++ b/tools/odrive/tests/nvm_test.py @@ -27,7 +27,7 @@ class TestStoreAndReboot(): odrive.handle.save_configuration() try: odrive.handle.reboot() - except fibre.ChannelBrokenException: + except fibre.ObjectLostError: pass # this is expected odrive.handle = None time.sleep(2) diff --git a/tools/odrive/tests/old_tests.py b/tools/odrive/tests/old_tests.py index fac90f1d..48ae48c2 100644 --- a/tools/odrive/tests/old_tests.py +++ b/tools/odrive/tests/old_tests.py @@ -225,7 +225,7 @@ class TestFlashAndErase(ODriveTest): # this is a firmware issue since it persists when unplugging/replugging # but goes away when power cycling the device odrv_ctx.handle.reboot() - except fibre.ChannelBrokenException: + except fibre.ObjectLostError: pass # this is expected time.sleep(0.5) diff --git a/tools/odrive/tests/pwm_input_test.py b/tools/odrive/tests/pwm_input_test.py index 82586638..78829b8a 100644 --- a/tools/odrive/tests/pwm_input_test.py +++ b/tools/odrive/tests/pwm_input_test.py @@ -71,7 +71,7 @@ class TestPwmInput(): ][odrive_gpio_num - 1] setattr(odrive.handle.config, 'gpio' + str(odrive_gpio_num) + '_mode', GPIO_MODE_PWM0) - pwm_mapping.endpoint = odrive.handle.axis0.controller._remote_attributes['input_pos'] + pwm_mapping.endpoint = odrive.handle.axis0.controller._input_pos_property pwm_mapping.min = min_val pwm_mapping.max = max_val diff --git a/tools/odrive/tests/test_runner.py b/tools/odrive/tests/test_runner.py index 4f8c807b..fe534abf 100644 --- a/tools/odrive/tests/test_runner.py +++ b/tools/odrive/tests/test_runner.py @@ -259,7 +259,7 @@ class ODriveComponent(Component): self.handle.save_configuration() try: self.handle.reboot() - except fibre.ChannelBrokenException: + except fibre.ObjectLostError: pass # this is expected self.handle = None time.sleep(2) @@ -268,7 +268,7 @@ class ODriveComponent(Component): def erase_config_and_reboot(self): try: self.handle.erase_configuration() - except fibre.ChannelBrokenException: + except fibre.ObjectLostError: pass # this is expected self.handle = None time.sleep(2) diff --git a/tools/odrive/utils.py b/tools/odrive/utils.py index 329cda81..1e6fd522 100755 --- a/tools/odrive/utils.py +++ b/tools/odrive/utils.py @@ -6,8 +6,6 @@ import threading import platform import subprocess import os -import numpy as np -import matplotlib.pyplot as plt from fibre.utils import Event import odrive.enums from odrive.enums import * @@ -35,6 +33,9 @@ _VT100Colors = { } def calculate_thermistor_coeffs(degree, Rload, R_25, Beta, Tmin, Tmax, plot = False): + import numpy as np + import matplotlib.pyplot as plt + T_25 = 25 + 273.15 #Kelvin temps = np.linspace(Tmin, Tmax, 1000) tempsK = temps + 273.15 @@ -70,7 +71,7 @@ def set_motor_thermistor_coeffs(axis, Rload, R_25, Beta, Tmin, TMax): axis.motor_thermistor.config.poly_coefficient_3 = float(coeffs[0]) def dump_errors(odrv, clear=False): - axes = [(name, axis) for name, axis in odrv._remote_attributes.items() if 'axis' in name] + axes = [(name, getattr(odrv, name)) for name in dir(odrv) if name.startswith('axis')] axes.sort() for name, axis in axes: print(name) diff --git a/tools/odrivetool b/tools/odrivetool index 60ed99bb..571eabc6 100755 --- a/tools/odrivetool +++ b/tools/odrivetool @@ -5,6 +5,15 @@ ODrive command line utility from __future__ import print_function import sys + +# We require Python 3.5 for the "async def" syntax. +if sys.version_info <= (3, 5): + print("Your Python version (Python {}.{}) is too old. Please install Python 3.5 or newer.".format( + sys.version_info.major, sys.version_info.minor + )) + exit(1) + +import sys import os import argparse import time @@ -13,7 +22,6 @@ import math sys.path.insert(0, os.path.join(os.path.dirname(os.path.dirname( os.path.realpath(__file__))), "Firmware", "fibre", "python")) -import fibre.discovery from fibre import Logger, Event import odrive from odrive.utils import OperationAbortedException @@ -105,7 +113,7 @@ parser.add_argument("-v", "--verbose", action="store_true", parser.add_argument("--version", action="store_true", help="print version information and exit") -parser.set_defaults(path="usb") +parser.set_defaults(path="usb:idVendor=0x1209,idProduct=0x0D32,bInterfaceClass=0,bInterfaceSubClass=1,bInterfaceProtocol=0") args = parser.parse_args() # Default command diff --git a/tools/setup.py b/tools/setup.py index 83d5c1ed..0a29e5b5 100644 --- a/tools/setup.py +++ b/tools/setup.py @@ -110,13 +110,11 @@ try: keywords = ['odrive', 'motor', 'motor control'], install_requires = [ 'ipython', # Used to do the interactive parts of the odrivetool - 'PyUSB', # Required to access USB devices from Python through libusb - 'PySerial', # Required to access serial devices from Python + 'PyUSB', # Only required for DFU. Normal communication happens through libfibre. 'requests', # Used to by DFU to load firmware files 'IntelHex', # Used to by DFU to download firmware from github 'matplotlib', # Required to run the liveplotter 'monotonic', # For compatibility with older python versions - 'appdirs', # Used to find caching directory 'pywin32 >= 222; platform_system == "Windows"' # Required for fancy terminal features on Windows ], package_data={'': ['version.txt']}, diff --git a/tools/setup_hall_as_index.py b/tools/setup_hall_as_index.py index 8ba871a8..feb66850 100644 --- a/tools/setup_hall_as_index.py +++ b/tools/setup_hall_as_index.py @@ -93,5 +93,5 @@ if save_and_reboot: odrv.save_configuration() try: odrv.reboot() - except odrive.fibre.ChannelBrokenException: + except odrive.fibre.ObjectLostError: pass