SHTP BOM085 UART HAL port attemt

sh2_uart_hal.cpp

#include <stdio.h>
#include <string.h>
#include "sh2_uart_hal.hpp"
#include "sh2_hal.h"
#include "sh2_err.h"
#include "driver/uart.h"
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "esp_timer.h"
#include "driver/gpio.h"
#include "rom/ets_sys.h"

int BUF_SIZE = 1024 * 2;
int KILL_EVT_TYPE = 0xdeadbeef;

int writeWithDelay(uint8_t *bytes, int len)
{
    for (int i = 0; i < len; i++)
    {
        if (uart_write_bytes(UART_NUM_1, (const char *)&bytes[i], 1) == -1)
        {
            return -1;
        }
        ets_delay_us(100);
    }
    return 0;
}

typedef struct
{
    unsigned int len;
    uint8_t dataFrame[SH2_HAL_MAX_TRANSFER_OUT]; // escaped frame with protocol byte
} TxQueueItem_t;

void uart_tx_task(void *pvParameters)
{
    // set log to dbg
    esp_log_level_set(TX_TASK_TAG, ESP_LOG_DEBUG);
    ESP_LOGD("UART", "uart_tx_task");
    
    int ticksPerMicrosecond = configTICK_RATE_HZ / 1000000;

    const char *TAG = TX_TASK_TAG;
    uart_hal_t *pHal = (uart_hal_t *)pvParameters;
    TxState_t txState = TX_IDLE;

    TxQueueItem_t currentItem = {
        .len = 0,
        .dataFrame = {0}};

    // Killing the uart handler sets the event queue to NULL so we use this to exit the loop
    // here as well and clean up the task & queue
    while (pHal->event_queue != NULL)
    {
        vTaskDelay(500 / portTICK_PERIOD_MS);

        if (txState == TX_IDLE)
        {
            if (uxQueueMessagesWaiting(pHal->tx_frame_queue) > 0)
            {
                // We have a frame to send. Send the BSQ first.
                txState = TX_SENDING_BSQ;
                uint8_t bsq[3] = {RFC1662_FLAG, PROTOCOL_CONTROL, RFC1662_FLAG};
                if (writeWithDelay(bsq, 3) == -1) {
                    ESP_LOGE(TAG, "couldn't send BSQ");
                    txState = TX_IDLE;
                    continue;
                }
                txState = TX_SENDING_FRAME;
            }
        }

        if (txState == TX_SENDING_FRAME)
        {
            if (xQueueReceive(pHal->tx_frame_queue, &currentItem, portMAX_DELAY) == pdFALSE)
            {
                txState = TX_IDLE;
                continue;
            }

            while (pHal->lastBSN < currentItem.len)
            {
                vTaskDelay(1 / portTICK_PERIOD_MS);
            }

            // Send the frame
            uint8_t *frame = currentItem.dataFrame;
            if (writeWithDelay(frame, currentItem.len) == -1) {
                ESP_LOGE(TAG, "couldn't send frame");
                xQueueSendToFront(pHal->tx_frame_queue, &currentItem, portMAX_DELAY);
                txState = TX_IDLE;
                continue;
            }
        }
    }
    vTaskDelete(NULL);
    vQueueDelete(pHal->tx_frame_queue);
    vRingbufferDelete(pHal->shtp_frame_buf);
    pHal->tx_frame_queue = NULL;
}

void uart_event_task(void *pvParameters)
{
    const char *TAG = RX_TASK_TAG;
    uart_event_t event;
    uint8_t *dtmp = (uint8_t *)malloc(BUF_SIZE);
    uart_hal_t *pHal = (uart_hal_t *)pvParameters;
    RFC1622Frame dataFrame(SH2_HAL_MAX_TRANSFER_IN);
    bool active = true;
    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
    uint8_t frameWithTimetamp[sizeof(uint32_t) + SH2_HAL_MAX_TRANSFER_IN];

    // set loglevel to debug
    esp_log_level_set(TAG, ESP_LOG_DEBUG);
    ESP_LOGD("UART", "uart_event_task");

    while (active)
    {
        if (xHigherPriorityTaskWoken)
        {
            portYIELD_FROM_ISR();
            xHigherPriorityTaskWoken = pdFALSE;
        }
        // Waiting for UART event.
        if (xQueueReceive(pHal->event_queue, (void *)&event, (TickType_t)portMAX_DELAY))
        {
            bzero(dtmp, BUF_SIZE);
            if (event.type == KILL_EVT_TYPE)
            {
                ESP_LOGI(TAG, "KILL_EVT_TYPE received, exiting uart_event_task");
                active = false;
                break;
            }
            switch (event.type)
            {
            // Event of UART receving data
            /*We'd better handler data event fast, there would be much more data events than
            other types of events. If we take too much time on data event, the queue might
            be full.*/
            case UART_DATA:
            {
                ESP_LOGI(TAG, "[UART DATA]: %d", event.size);
                // Read data from UART ring buffer
                uart_read_bytes(pHal->uart_num, dtmp, event.size, portMAX_DELAY);
                // Construct a frame from the data
                int i = 0;
                while (i < event.size)
                {
                    dataFrame.rx(dtmp[i++]);
                    // If a frame is ready, send it to the shtp frame buffer
                    if (dataFrame.isRxFrameReady())
                    {
                        uint8_t *frame = dataFrame.getRxFrame();
                        if (dataFrame.getRxFrameSize() > 0 && frame[0] == PROTOCOL_CONTROL)
                        {
                            // Process controll protocoll (BSN received)
                            ESP_LOGI(TAG, "BSN received");
                            pHal->lastBSN = (frame[2] << 8) + frame[1];
                            // Frame consumed, reset frame
                            dataFrame.rxResetFrame();
                            continue;
                        }

                        // Normal frame, send to shtp frame buffer
                        uint8_t *frameWithoutProtocol = frame + 1;
                        unsigned int frameWithoutProtocolSize = dataFrame.getRxFrameSize() - 1;
                        // Create a copy of the frame with a 4 byte timestamp prepended
                        uint32_t t_us = esp_timer_get_time();
                        memcpy(frameWithTimetamp, &t_us, sizeof(uint32_t));
                        memcpy(frameWithTimetamp + sizeof(uint32_t), frameWithoutProtocol, frameWithoutProtocolSize);

                        BaseType_t res = xRingbufferSendFromISR(
                            pHal->shtp_frame_buf,
                            frameWithTimetamp,
                            frameWithoutProtocolSize + sizeof(uint32_t),
                            &xHigherPriorityTaskWoken);
                        if (res != pdTRUE)
                        {
                            ESP_LOGE(TAG, "couldn't send frame to shtp frame buffer");
                        }
                        // Reset the frame
                        dataFrame.rxResetFrame();
                    }
                }
                break;
            }
            // Event of HW FIFO overflow detected
            case UART_FIFO_OVF:
            {
                ESP_LOGI(TAG, "hw fifo overflow");
                // If fifo overflow happened, you should consider adding flow control for your application.
                // The ISR has already reset the rx FIFO,
                // As an example, we directly flush the rx buffer here in order to read more data.
                uart_flush_input(pHal->uart_num);
                dataFrame.reset();
                xQueueReset(pHal->event_queue);
                break;
            }
            // Event of UART ring buffer full
            case UART_BUFFER_FULL:
            {
                ESP_LOGI(TAG, "ring buffer full");
                // If buffer full happened, you should consider increasing your buffer size
                // As an example, we directly flush the rx buffer here in order to read more data.
                uart_flush_input(pHal->uart_num);
                dataFrame.reset();
                xQueueReset(pHal->event_queue);
                break;
            }
            // Event of UART RX break detected
            case UART_BREAK:
            {
                ESP_LOGI(TAG, "uart rx break");
                dataFrame.reset();
                break;
            }
            // Event of UART frame error
            case UART_FRAME_ERR:
            {
                ESP_LOGI(TAG, "uart frame error");
                dataFrame.reset();
                break;
            }
            // Others
            default:
                ESP_LOGI(TAG, "uart event type: %d", event.type);
                break;
            }
        }
    }
    free(dtmp);
    dtmp = NULL;
    vTaskDelete(NULL);
    uart_driver_delete(pHal->uart_num);
}

int shtp_uart_hal_open(sh2_Hal_t *self)
{
    ESP_LOGD("UART", "shtp_uart_hal_open");

    // GPIO18 is RESET, set to output and no pull
    gpio_config_t io_conf;
    io_conf.intr_type = GPIO_INTR_DISABLE;
    io_conf.mode = GPIO_MODE_OUTPUT;
    io_conf.pull_down_en = GPIO_PULLDOWN_ENABLE;
    io_conf.pull_up_en = GPIO_PULLUP_DISABLE;
    io_conf.pin_bit_mask = (1ULL << GPIO_NUM_18);
    gpio_config(&io_conf);

    // Set GPIO 19 PS1 output, set as output and force to 1
    io_conf.intr_type = GPIO_INTR_DISABLE;
    io_conf.mode = GPIO_MODE_OUTPUT;
    io_conf.pull_down_en = GPIO_PULLDOWN_ENABLE;
    io_conf.pull_up_en = GPIO_PULLUP_DISABLE;
    io_conf.pin_bit_mask = (1ULL << GPIO_NUM_19);
    gpio_config(&io_conf);
    gpio_set_level(GPIO_NUM_19, 1);

    // Pull reset low
    gpio_set_level(GPIO_NUM_18, 0);
    vTaskDelay(10 / portTICK_PERIOD_MS);
    gpio_set_level(GPIO_NUM_18, 1);

    uart_hal_t *pHal = (uart_hal_t *)self;
    // The UART is configured for 3Mkb/s, 8 data bits, 1 stop bit and no parity
    uart_config_t uart_config = {
        .baud_rate = 3000000,
        .data_bits = UART_DATA_8_BITS,
        .parity = UART_PARITY_DISABLE,
        .stop_bits = UART_STOP_BITS_1,
        .flow_ctrl = UART_HW_FLOWCTRL_DISABLE,
        .source_clk = UART_SCLK_DEFAULT};
    if (pHal->uart_num >= UART_NUM_MAX)
    {
        ESP_LOGE("UART", "shtp_uart_hal_open failed, invalid uart_num %d", pHal->uart_num);
        return SH2_ERR_BAD_PARAM;
    }
    if (pHal->event_queue != NULL)
    {
        ESP_LOGE("UART", "shtp_uart_hal_open failed, this uart has been opened already");
        return SH2_ERR;
    }

    // we won't use a buffer for sending data
    ESP_ERROR_CHECK(uart_driver_install(pHal->uart_num, BUF_SIZE, 0, 10, &pHal->event_queue, 0));
    ESP_ERROR_CHECK(uart_param_config(pHal->uart_num, &uart_config));
    // Set default UART pins
    ESP_ERROR_CHECK(uart_set_pin(pHal->uart_num, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE));

    // Create a task to handle UART event from ISR
    if (xTaskCreate(uart_event_task, "uart_event_task", 2048, (void *)pHal, 12, &pHal->tsk_hdl) != pdTRUE)
    {
        ESP_LOGE("UART", "shtp_uart_hal_open failed, couldn't create uart_event_task");
        return SH2_ERR;
    }
    if (xTaskCreate(uart_tx_task, "uart_tx_task", 2048, (void *)pHal, 12, &pHal->tsk_tx) != pdTRUE)
    {
        ESP_LOGE("UART", "shtp_uart_hal_open failed, couldn't create uart_tx_task");
        return SH2_ERR;
    }

    vTaskDelay(4000 / portTICK_PERIOD_MS);
    
    return SH2_OK;
}

void shtp_uart_hal_close(sh2_Hal_t *self)
{
    uart_hal_t *pHal = (uart_hal_t *)self;
    if (pHal->event_queue != NULL)
    {
        uart_event_t evt;
        evt.type = (uart_event_type_t)KILL_EVT_TYPE;
        // Send a kill event to the uart_event_task
        xQueueSend(pHal->event_queue, (void *)&evt, portMAX_DELAY);
        pHal->tsk_hdl = NULL;
        pHal->event_queue = NULL;
        pHal->tsk_tx = NULL;
    }
}

uint32_t shtp_uart_hal_get_time_us(sh2_Hal_t *self)
{
    return esp_timer_get_time();
}

int shtp_uart_hal_write(sh2_Hal_t *self, uint8_t *buf, unsigned int len)
{
    ESP_LOGD("UART", "shtp_uart_hal_write");

    uart_hal_t *pHal = (uart_hal_t *)self;
    if ((buf == nullptr) || (len == 0))
    {
        return SH2_ERR_BAD_PARAM;
    }

    RFC1622Frame frame(SH2_HAL_MAX_TRANSFER_OUT);
    frame.txStartFrame();
    uint8_t protocolByte = PROTOCOL_SHTP;
    frame.txWrite(&protocolByte, 1);
    int bytesWritten = frame.txWrite(buf, len);
    if (bytesWritten == 0)
    {
        ESP_LOGE("SHTP", "shtp_uart_hal_write: frame too large");
        return SH2_ERR_BAD_PARAM;
    }
    if (frame.txEndFrame() == 0)
    {
        ESP_LOGE("SHTP", "shtp_uart_hal_write: frame too large");
        return SH2_ERR_BAD_PARAM;
    }

    TxQueueItem_t item = {
        .len = frame.getTxFrameSize(),
    };
    memcpy(item.dataFrame, frame.getTxFrame(), item.len);
    xQueueSend(pHal->tx_frame_queue, &item, portMAX_DELAY);

    return len;
}

/**
 * This function needs to:
 * - Deliver a whole frame to caller, if one is ready
 */
int shtp_uart_hal_read(sh2_Hal_t *self, uint8_t *pBuffer, unsigned int len, uint32_t *t_us)
{
    ESP_LOGD("UART", "shtp_uart_hal_read");
    uart_hal_t *pHal = (uart_hal_t *)self;
    size_t itemSize;
    void *item = xRingbufferReceive(pHal->shtp_frame_buf, &itemSize, portMAX_DELAY);
    if (itemSize > len)
    {
        ESP_LOGE("SHTP", "shtp_uart_hal_read: passed frame buffer too small, discarding frame of size %d", itemSize);
        vRingbufferReturnItem(pHal->shtp_frame_buf, item);
        return 0;
    }
    if (item != NULL)
    {
        // Set the timestamp (first 4 bytes prepended to the frame)
        *t_us = *(uint32_t *)item;
        // Copy the frame
        int frameSize = itemSize - sizeof(uint32_t);
        memcpy(pBuffer, item + sizeof(uint32_t), frameSize);
        // Return the frame buffer to the ringbuffer so it can be freed
        vRingbufferReturnItem(pHal->shtp_frame_buf, item);
        return frameSize;
    }

    return 0;
}

RFC1622Frame::RFC1622Frame(unsigned int rxFrameSize)
{
    this->rxFrame = (uint8_t *)malloc(rxFrameSize);
    this->rxFrameSize = rxFrameSize;
}

void RFC1622Frame::reset()
{
    this->rxFrameBufIdx = 0;
    this->rxFrameReady = false;
    this->rxState = OUTSIDE_FRAME;
}

void RFC1622Frame::rxResetFrame()
{
    this->rxFrameBufIdx = 0;
    this->rxFrameReady = false;
}

void RFC1622Frame::rxAddToFrame(uint8_t c)
{
    if (this->rxFrameBufIdx < this->rxFrameSize)
    {
        this->rxFrame[this->rxFrameBufIdx++] = c;
    }
    else
    {
        ESP_LOGE("RFC1622Frame", "rxAddToFrame: frame buffer overflow");
        this->rxFrameBufIdx++;
    }
}

void RFC1622Frame::rx(uint8_t c)
{
    // Use state machine to build up chars into frames for delivery.
    switch (this->rxState)
    {
    case OUTSIDE_FRAME:
        // Look for start of frame
        if (c == RFC1662_FLAG)
        {
            // Init frame in progress
            this->rxResetFrame();
            this->rxState = INSIDE_FRAME;
        }
        break;
    case INSIDE_FRAME:
        // Look for end of frame
        if (c == RFC1662_FLAG)
        {
            if (this->rxFrameBufIdx > 0)
            {
                // Frame is done
                this->rxFrameReady = true;
                this->rxState = OUTSIDE_FRAME;
            }
            else
            {
                // Treat second consec flag as another start flag.
                this->rxState = INSIDE_FRAME;
            }
        }
        else if (c == RFC1662_ESCAPE)
        {
            // Go to escaped state so next char can be a flag or escape
            this->rxState = ESCAPED;
        }
        else
        {
            this->rxAddToFrame(c);
        }
        break;
    case ESCAPED:
        this->rxAddToFrame(c ^ 0x20);
        this->rxState = INSIDE_FRAME;
        break;
    default:
        // Bad state.  Recover by resetting to outside frame state
        this->rxState = OUTSIDE_FRAME;
        ESP_LOGE("RFC1622Frame", "rx: bad state");
        break;
    }
}

RFC1622Frame::~RFC1622Frame()
{
    if (this->rxFrame)
    {
        free(this->rxFrame);
    }
}

void RFC1622Frame::txStartFrame()
{
    this->rxFrameBufIdx = 0;
    this->rxFrame[this->rxFrameBufIdx++] = RFC1662_FLAG;
}

unsigned int RFC1622Frame::txWrite(uint8_t *pSrc, uint32_t len)
{
    uint8_t *pDst = this->rxFrame;
    for (int i = 0; i < len; i++)
    {
        if ((pSrc[i] == RFC1662_FLAG) ||
            (pSrc[i] == RFC1662_ESCAPE))
        {
            if (this->rxFrameBufIdx + 2 >= this->rxFrameSize)
            {
                // Overflow
                return 0;
            }
            // Store escaped character
            pDst[this->rxFrameBufIdx++] = RFC1662_ESCAPE;
            pDst[this->rxFrameBufIdx++] = pSrc[i] ^ 0x20;
        }
        else
        {
            if (this->rxFrameBufIdx + 1 >= this->rxFrameSize)
            {
                // Overflow
                return 0;
            }
            // store the character normally
            pDst[this->rxFrameBufIdx++] = pSrc[i];
        }
    }
    return this->rxFrameBufIdx + 1;
}

int RFC1622Frame::txEndFrame()
{
    if (this->rxFrameBufIdx + 1 >= this->rxFrameSize)
    {
        // Overflow
        return 0;
    }
    this->rxFrame[this->rxFrameBufIdx++] = RFC1662_FLAG;
    return 1;
}

sh2_Hal_t *shtp_uart_hal_init(uart_hal_t *pHal, UART_NUM uart_num)
{
    pHal->sh2_hal.open = shtp_uart_hal_open;
    pHal->sh2_hal.close = shtp_uart_hal_close;
    pHal->sh2_hal.read = shtp_uart_hal_read;
    pHal->sh2_hal.write = shtp_uart_hal_write;
    pHal->sh2_hal.getTimeUs = shtp_uart_hal_get_time_us;

    pHal->event_queue = NULL;
    pHal->tsk_hdl = NULL;
    pHal->lastBSN = 0;
    pHal->tsk_tx = NULL;
    pHal->lastBSN = 0;

    pHal->uart_num = uart_num;

    // Ringbuffer for SHTP frames (max 5 full frames)
    pHal->shtp_frame_buf = xRingbufferCreate(SH2_HAL_MAX_TRANSFER_IN / 8 * 5 + 8 * 5, RINGBUF_TYPE_NOSPLIT);
    pHal->tx_frame_queue = xQueueCreate(5, sizeof(TxQueueItem_t));
    return &pHal->sh2_hal;
}

sh2_uart_hal.hpp

#pragma once

#include "sh2_hal.h"
#include "freertos/FreeRTOS.h"
#include "freertos/queue.h"
#include "freertos/task.h"
#include "freertos/ringbuf.h"

#define RFC1662_FLAG (0x7e)
#define RFC1662_ESCAPE (0x7d)

#define TX_TASK_TAG ("SH2_TX")
#define RX_TASK_TAG ("SH2_RX")

#define PROTOCOL_CONTROL (0)
#define PROTOCOL_SHTP (1)

enum UART_NUM {
    UART_NUM_0 = 0,
    UART_NUM_1,
    UART_NUM_2,
    UART_NUM_MAX
};

typedef struct {
    sh2_Hal_t sh2_hal;
    UART_NUM uart_num;
    QueueHandle_t event_queue;
    TaskHandle_t tsk_hdl;
    RingbufHandle_t shtp_frame_buf;
    /** last BSN bytes available */
    uint16_t lastBSN;
    TaskHandle_t tsk_tx;
    QueueHandle_t tx_frame_queue;
} uart_hal_t;

sh2_Hal_t *shtp_uart_hal_init(uart_hal_t *pHal, UART_NUM uart_num);


// RFC1622 frame decode
typedef enum {
    OUTSIDE_FRAME,   // Waiting for start of frame
    INSIDE_FRAME,    // Inside frame until end of frame
    ESCAPED,         // Inside frame, after escape char
} RxState_t;

typedef enum {
    TX_IDLE,
    TX_SENDING_BSQ,
    TX_SENDING_FRAME,
} TxState_t;

class RFC1622Frame {
private:
    uint8_t *rxFrame;
    uint8_t rxFrameSize;
    uint32_t rxFrameBufIdx;
    bool rxFrameReady;
    RxState_t rxState;
public:
    // Constructor
    RFC1622Frame(unsigned int rxFrameSize);

    // Returns number of bytes written. Can be less than len if buffer is full.
    void txStartFrame();
    unsigned int txWrite(uint8_t *pSrc, uint32_t len);
    int txEndFrame();

    void reset();
    void rxResetFrame();
    void rxAddToFrame(uint8_t c);
    void rx(uint8_t c);

    bool isRxFrameReady() { return this->rxFrameReady; }
    unsigned int getRxFrameSize() {
        if (!this->rxFrameReady)
            return 0;
        return this->rxFrameBufIdx + 1;
    }
    uint8_t *getRxFrame() {
        if (!this->rxFrameReady)
            return NULL;
        return this->rxFrame;
    }
    uint8_t *getTxFrame() {
        return this->rxFrame;
    }
    unsigned int getTxFrameSize() {
        return this->rxFrameBufIdx + 1;
    }

    // destructor
    ~RFC1622Frame();
};