|
| 1 | +#include "freertos/FreeRTOS.h" |
| 2 | +#include "freertos/queue.h" |
| 3 | +#include "esp_log.h" |
| 4 | +#include "esp_timer.h" |
| 5 | +#include "esp_twai.h" |
| 6 | +#include "esp_twai_types.h" |
| 7 | +#include "esp_check.h" |
| 8 | +#include "esp_isotp.h" |
| 9 | +#include <stdarg.h> |
| 10 | +#include <stdlib.h> |
| 11 | +#include <string.h> |
| 12 | + |
| 13 | +static const char *TAG = "esp_isotp"; |
| 14 | +static esp_isotp_handle_t s_isotp_handle = NULL; |
| 15 | + |
| 16 | +/** |
| 17 | + * @brief TWAI receive callback function |
| 18 | + * @note This function runs in ISR context |
| 19 | + */ |
| 20 | +static IRAM_ATTR bool esp_isotp_rx_callback(twai_node_handle_t handle, const twai_rx_done_event_data_t *edata, void *user_ctx) |
| 21 | +{ |
| 22 | + esp_isotp_handle_t link_handle = (esp_isotp_handle_t) user_ctx; |
| 23 | + |
| 24 | + ESP_RETURN_ON_FALSE(link_handle != NULL, false, TAG, "Invalid link handle"); |
| 25 | + ESP_RETURN_ON_FALSE(link_handle->rx_queue != NULL, false, TAG, "Invalid RX queue"); |
| 26 | + |
| 27 | + BaseType_t higher_priority_task_woken = pdFALSE; |
| 28 | + isotp_rx_queue_item_t queue_item = {0}; |
| 29 | + twai_frame_t rx_frame = { |
| 30 | + .buffer = queue_item.data, |
| 31 | + .buffer_len = sizeof(queue_item.data), |
| 32 | + }; |
| 33 | + |
| 34 | + if (twai_node_receive_from_isr(handle, &rx_frame) == ESP_OK) { |
| 35 | + if (rx_frame.header.id == link_handle->link.receive_arbitration_id) { |
| 36 | + queue_item.header = rx_frame.header; |
| 37 | + queue_item.data_len = rx_frame.buffer_len; |
| 38 | + xQueueSendFromISR(link_handle->rx_queue, &queue_item, &higher_priority_task_woken); |
| 39 | + } |
| 40 | + } |
| 41 | + return (higher_priority_task_woken == pdTRUE); |
| 42 | +} |
| 43 | + |
| 44 | +/** |
| 45 | + * @brief Get current timestamp in milliseconds |
| 46 | + */ |
| 47 | +uint32_t isotp_user_get_ms(void) |
| 48 | +{ |
| 49 | + return (uint32_t)(esp_timer_get_time() / 1000); |
| 50 | +} |
| 51 | + |
| 52 | +/** |
| 53 | + * @brief Send CAN frame through TWAI driver |
| 54 | + */ |
| 55 | +int isotp_user_send_can(const uint32_t arbitration_id, const uint8_t *data, const uint8_t size) |
| 56 | +{ |
| 57 | + ESP_RETURN_ON_FALSE(s_isotp_handle != NULL, ISOTP_RET_ERROR, TAG, "Invalid handle"); |
| 58 | + |
| 59 | + twai_frame_t tx_msg = {0}; |
| 60 | + tx_msg.header.id = arbitration_id; |
| 61 | + tx_msg.header.ide = false; |
| 62 | + tx_msg.header.rtr = false; |
| 63 | + tx_msg.buffer = (uint8_t *)data; |
| 64 | + tx_msg.buffer_len = size; |
| 65 | + |
| 66 | + esp_err_t ret = twai_node_transmit(s_isotp_handle->twai_node, &tx_msg, 0); |
| 67 | + if (ret != ESP_OK) { |
| 68 | + ESP_LOGE(TAG, "Failed to send CAN frame: %s", esp_err_to_name(ret)); |
| 69 | + return ISOTP_RET_ERROR; |
| 70 | + } |
| 71 | + return ISOTP_RET_OK; |
| 72 | +} |
| 73 | + |
| 74 | +/** |
| 75 | + * @brief Debug output function for ISO-TP library |
| 76 | + */ |
| 77 | +void isotp_user_debug(const char *message, ...) |
| 78 | +{ |
| 79 | + va_list args; |
| 80 | + va_start(args, message); |
| 81 | + esp_log_writev(ESP_LOG_DEBUG, "isotp_c", message, args); |
| 82 | + va_end(args); |
| 83 | +} |
| 84 | + |
| 85 | +esp_isotp_handle_t esp_isotp_new(const esp_isotp_config_t *config) |
| 86 | +{ |
| 87 | + esp_err_t ret = ESP_OK; |
| 88 | + ESP_RETURN_ON_FALSE(config != NULL, NULL, TAG, "Invalid configuration"); |
| 89 | + ESP_RETURN_ON_FALSE(config->twai_node != NULL, NULL, TAG, "Invalid TWAI node"); |
| 90 | + |
| 91 | + // Allocate memory for handle |
| 92 | + esp_isotp_handle_t handle = calloc(1, sizeof(struct esp_isotp_link)); |
| 93 | + ESP_RETURN_ON_FALSE(handle != NULL, NULL, TAG, "Failed to allocate memory for ISO-TP link"); |
| 94 | + s_isotp_handle = handle; |
| 95 | + |
| 96 | + handle->rx_queue = config->rx_queue; |
| 97 | + handle->twai_node = config->twai_node; |
| 98 | + |
| 99 | + handle->tx_buffer = calloc(config->tx_buffer_size, sizeof(uint8_t)); |
| 100 | + ESP_GOTO_ON_FALSE(handle->tx_buffer, ESP_ERR_NO_MEM, err_tx_buf, TAG, "Failed to allocate memory for TX buffer"); |
| 101 | + |
| 102 | + handle->rx_buffer = calloc(config->rx_buffer_size, sizeof(uint8_t)); |
| 103 | + ESP_GOTO_ON_FALSE(handle->rx_buffer, ESP_ERR_NO_MEM, err_rx_buf, TAG, "Failed to allocate memory for RX buffer"); |
| 104 | + |
| 105 | + // Initialize ISO-TP link with static buffers |
| 106 | + isotp_init_link(&handle->link, config->tx_id, handle->tx_buffer, |
| 107 | + config->tx_buffer_size, handle->rx_buffer, config->rx_buffer_size); |
| 108 | + handle->link.receive_arbitration_id = config->rx_id; |
| 109 | + |
| 110 | + // Register TWAI callback |
| 111 | + twai_event_callbacks_t cbs = { |
| 112 | + .on_rx_done = esp_isotp_rx_callback, |
| 113 | + }; |
| 114 | + ret = twai_node_register_event_callbacks(handle->twai_node, &cbs, handle); |
| 115 | + ESP_GOTO_ON_ERROR(ret, err_callbacks, TAG, "Failed to register event callbacks: %s", esp_err_to_name(ret)); |
| 116 | + |
| 117 | + // Enable TWAI node |
| 118 | + ret = twai_node_enable(handle->twai_node); |
| 119 | + ESP_GOTO_ON_ERROR(ret, err_enable, TAG, "Failed to enable TWAI node: %s", esp_err_to_name(ret)); |
| 120 | + |
| 121 | + ESP_LOGI(TAG, "New ISO-TP link created, TX: 0x%lX, RX: 0x%lX", config->tx_id, config->rx_id); |
| 122 | + return handle; |
| 123 | + |
| 124 | +err_enable: |
| 125 | +err_callbacks: |
| 126 | + free(handle->rx_buffer); |
| 127 | +err_rx_buf: |
| 128 | + free(handle->tx_buffer); |
| 129 | +err_tx_buf: |
| 130 | + free(handle); |
| 131 | + return NULL; |
| 132 | +} |
| 133 | + |
| 134 | +esp_err_t esp_isotp_poll(esp_isotp_handle_t handle) |
| 135 | +{ |
| 136 | + ESP_RETURN_ON_FALSE(handle != NULL, ESP_ERR_INVALID_ARG, TAG, "Invalid handle"); |
| 137 | + |
| 138 | + // Process received queue data |
| 139 | + isotp_rx_queue_item_t rx_item; |
| 140 | + while (xQueueReceive(handle->rx_queue, &rx_item, 0) == pdTRUE) { |
| 141 | + isotp_on_can_message(&handle->link, rx_item.data, rx_item.data_len); |
| 142 | + } |
| 143 | + |
| 144 | + // Run ISO-TP state machine |
| 145 | + isotp_poll(&handle->link); |
| 146 | + |
| 147 | + return ESP_OK; |
| 148 | +} |
| 149 | + |
| 150 | +int esp_isotp_send(esp_isotp_handle_t handle, const uint8_t *data, uint16_t size) |
| 151 | +{ |
| 152 | + ESP_RETURN_ON_FALSE(handle != NULL, ISOTP_RET_ERROR, TAG, "Invalid handle"); |
| 153 | + ESP_RETURN_ON_FALSE(data != NULL, ISOTP_RET_ERROR, TAG, "Invalid data"); |
| 154 | + |
| 155 | + return isotp_send(&handle->link, data, size); |
| 156 | +} |
| 157 | + |
| 158 | +int esp_isotp_receive(esp_isotp_handle_t handle, uint8_t *data, uint16_t size) |
| 159 | +{ |
| 160 | + uint16_t out_size = 0; |
| 161 | + ESP_RETURN_ON_FALSE(handle != NULL, ISOTP_RET_ERROR, TAG, "Invalid handle"); |
| 162 | + ESP_RETURN_ON_FALSE(data != NULL, ISOTP_RET_ERROR, TAG, "Invalid data"); |
| 163 | + |
| 164 | + int ret = isotp_receive(&handle->link, data, size, &out_size); |
| 165 | + if (ret == ISOTP_RET_OK) { |
| 166 | + return out_size; |
| 167 | + } else if (ret == ISOTP_RET_NO_DATA) { |
| 168 | + return 0; |
| 169 | + } |
| 170 | + return ret; |
| 171 | +} |
| 172 | + |
| 173 | +esp_err_t esp_isotp_delete(esp_isotp_handle_t handle) |
| 174 | +{ |
| 175 | + esp_err_t ret = ESP_OK; |
| 176 | + ESP_RETURN_ON_FALSE(handle != NULL, ESP_ERR_INVALID_ARG, TAG, "Invalid handle"); |
| 177 | + |
| 178 | + // Clear callback |
| 179 | + twai_event_callbacks_t cbs = { .on_rx_done = NULL }; |
| 180 | + ret = twai_node_register_event_callbacks(handle->twai_node, &cbs, NULL); |
| 181 | + ESP_GOTO_ON_ERROR(ret, clean, TAG, "Failed to unregister callbacks: %s", esp_err_to_name(ret)); |
| 182 | + |
| 183 | + // Disable TWAI node |
| 184 | + ret = twai_node_disable(handle->twai_node); |
| 185 | + ESP_GOTO_ON_ERROR(ret, clean, TAG, "Failed to disable TWAI node: %s", esp_err_to_name(ret)); |
| 186 | + |
| 187 | +clean: |
| 188 | + // Free buffers |
| 189 | + if (handle->tx_buffer) { |
| 190 | + free(handle->tx_buffer); |
| 191 | + } |
| 192 | + if (handle->rx_buffer) { |
| 193 | + free(handle->rx_buffer); |
| 194 | + } |
| 195 | + free(handle); |
| 196 | + return ret; |
| 197 | +} |
0 commit comments