/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : Main program body
  ******************************************************************************
  */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "dma.h"
#include "iwdg.h"
#include "spi.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include <stdio.h>
#include <string.h>

#include "CH390.h"
#include "CH390_Interface.h"
#include "SEGGER_RTT.h"
#include "config.h"
#include "ethernetif.h"
#include "ch390_runtime.h"
#include "lwip/init.h"
#include "lwip/timeouts.h"
#include "tcp_client.h"
#include "tcp_server.h"
#include "uart_trans.h"
/* USER CODE END Includes */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
#define LED_PIN             GPIO_PIN_13
#define LED_PORT            GPIOC
#define APP_ROUTE_BUFFER_SIZE 256u
#define STACK_GUARD_WORD     0xA5A5A5A5u
#define APP_HEALTH_CHECK_INTERVAL_MS 5000u
/* USER CODE END PD */

/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN PV */
static volatile uint16_t g_led_blink_ticks = 0;
static uint8_t g_clock_fallback_to_hsi = 0u;
volatile uint8_t g_uart1_rx_probe_byte = 0u;
static uint8_t g_stack_guard_reported = 0u;
static uint8_t g_mux_response_frame[272];
static uint8_t g_links_started = 0u;
/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */
static void LED_Init(void);
static void LED_StartBlink(void);
static void BootDiag_ReportCh390(void);
static void App_Init(void);
static void App_Poll(void);
static void App_ConfigureLinks(const device_config_t *cfg);
static void App_RouteRawUartTraffic(void);
static void App_RouteMuxUartTraffic(void);
static void App_RouteTcpTraffic(void);
static void StackGuard_Init(void);
static void StackGuard_Check(void);
static bool App_SendToUart(uint8_t uart_index, uint8_t src_id, uint8_t dst_mask, const uint8_t *data, uint16_t len);
static bool App_SendTcpServerPayload(uint8_t instance, const uint8_t *data, uint16_t len);
static bool App_SendTcpClientPayload(uint8_t instance, const uint8_t *data, uint16_t len);
/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
extern uint32_t Stack_Mem[];

static void LED_Init(void)
{
    HAL_GPIO_WritePin(LED_PORT, LED_PIN, GPIO_PIN_SET);
}

static void StackGuard_Init(void)
{
    Stack_Mem[0] = STACK_GUARD_WORD;
    g_stack_guard_reported = 0u;
}

static void StackGuard_Check(void)
{
    if (Stack_Mem[0] != STACK_GUARD_WORD) {
        if (g_stack_guard_reported == 0u) {
            g_stack_guard_reported = 1u;
            SEGGER_RTT_WriteString(0, "ERROR: Main stack guard overwritten\r\n");
        }
        __disable_irq();
        NVIC_SystemReset();
    }
}

static void LED_StartBlink(void)
{
    if (HAL_TIM_Base_Start_IT(&htim4) != HAL_OK) {
        Error_Handler();
    }
}

void LED_Toggle(void)
{
    HAL_GPIO_TogglePin(LED_PORT, LED_PIN);
}

void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
    if (htim->Instance == TIM4) {
        g_led_blink_ticks++;
        if (g_led_blink_ticks >= 1000u) {
            g_led_blink_ticks = 0u;
            LED_Toggle();
        }
    }
}

static void BootDiag_ReportCh390(void)
{
    ch390_diag_t diag;
    const device_config_t *cfg = config_get();
    uint8_t mac_hw[6];

    ch390_runtime_get_diag(&diag);
    ch390_get_mac(mac_hw);

    SEGGER_RTT_printf(0,
                      "CH390 VID=0x%04X PID=0x%04X REV=0x%02X LINK=%u MAC=%02X:%02X:%02X:%02X:%02X:%02X\r\n",
                      diag.vendor_id,
                      diag.product_id,
                      diag.revision,
                      diag.link_up,
                      mac_hw[0], mac_hw[1], mac_hw[2], mac_hw[3], mac_hw[4], mac_hw[5]);
    SEGGER_RTT_printf(0,
                      "NET cfg IP=%u.%u.%u.%u MASK=%u.%u.%u.%u GW=%u.%u.%u.%u MUX=%u\r\n",
                      cfg->net.ip[0], cfg->net.ip[1], cfg->net.ip[2], cfg->net.ip[3],
                      cfg->net.mask[0], cfg->net.mask[1], cfg->net.mask[2], cfg->net.mask[3],
                      cfg->net.gw[0], cfg->net.gw[1], cfg->net.gw[2], cfg->net.gw[3],
                      cfg->mux_mode);
}

static void App_ConfigureLinks(const device_config_t *cfg)
{
    tcp_server_instance_config_t server_cfg;
    tcp_client_instance_config_t client_cfg;

    (void)tcp_server_init_all();
    (void)tcp_client_init_all();

    server_cfg.enabled = (cfg->links[CONFIG_LINK_S1].enabled != 0u);
    server_cfg.port = cfg->links[CONFIG_LINK_S1].local_port;
    (void)tcp_server_config(0u, &server_cfg);

    server_cfg.enabled = (cfg->links[CONFIG_LINK_S2].enabled != 0u);
    server_cfg.port = cfg->links[CONFIG_LINK_S2].local_port;
    (void)tcp_server_config(1u, &server_cfg);

    memcpy(client_cfg.remote_ip, cfg->links[CONFIG_LINK_C1].remote_ip, sizeof(client_cfg.remote_ip));
    client_cfg.local_port = cfg->links[CONFIG_LINK_C1].local_port;
    client_cfg.remote_port = cfg->links[CONFIG_LINK_C1].remote_port;
    client_cfg.reconnect_interval_ms = TCP_CLIENT_RECONNECT_DELAY_MS;
    client_cfg.enabled = (cfg->links[CONFIG_LINK_C1].enabled != 0u);
    client_cfg.auto_reconnect = true;
    (void)tcp_client_config(0u, &client_cfg);

    memcpy(client_cfg.remote_ip, cfg->links[CONFIG_LINK_C2].remote_ip, sizeof(client_cfg.remote_ip));
    client_cfg.local_port = cfg->links[CONFIG_LINK_C2].local_port;
    client_cfg.remote_port = cfg->links[CONFIG_LINK_C2].remote_port;
    client_cfg.enabled = (cfg->links[CONFIG_LINK_C2].enabled != 0u);
    (void)tcp_client_config(1u, &client_cfg);
}

static void App_StartLinksIfNeeded(void)
{
    int any_failed;

    if ((g_links_started != 0u) || !netif_is_link_up(&ch390_netif)) {
        return;
    }

    any_failed = 0;
    for (uint8_t i = 0; i < TCP_SERVER_INSTANCE_COUNT; ++i) {
        if (tcp_server_start(i) != 0) {
            any_failed = 1;
        }
    }
    for (uint8_t i = 0; i < TCP_CLIENT_INSTANCE_COUNT; ++i) {
        if (tcp_client_connect(i) != 0) {
            any_failed = 1;
        }
    }

    if (any_failed) {
        SEGGER_RTT_WriteString(0, "NET links start partially failed, will retry\r\n");
        return;
    }

    g_links_started = 1u;
    SEGGER_RTT_WriteString(0, "NET links started after link-up\r\n");
}

static void App_StopLinksIfNeeded(void)
{
    if (netif_is_link_up(&ch390_netif)) {
        return;
    }

    if (g_links_started != 0u) {
        for (uint8_t i = 0; i < TCP_CLIENT_INSTANCE_COUNT; ++i) {
            (void)tcp_client_disconnect(i);
        }
        for (uint8_t i = 0; i < TCP_SERVER_INSTANCE_COUNT; ++i) {
            (void)tcp_server_stop(i);
        }
        SEGGER_RTT_WriteString(0, "NET links stopped after link-down\r\n");
    }

    g_links_started = 0u;
}

static void App_Init(void)
{
    const device_config_t *cfg;
    ip4_addr_t ipaddr;
    ip4_addr_t netmask;
    ip4_addr_t gateway;
    uart_config_t uart_cfg;

    (void)config_init();
    cfg = config_get();

    (void)uart_trans_init();
    uart_cfg.baudrate = cfg->uart_baudrate[0];
    (void)uart_trans_config(UART_CHANNEL_U0, &uart_cfg);
    uart_cfg.baudrate = cfg->uart_baudrate[1];
    (void)uart_trans_config(UART_CHANNEL_U1, &uart_cfg);
    (void)uart_trans_start(UART_CHANNEL_U0);
    (void)uart_trans_start(UART_CHANNEL_U1);

    SEGGER_RTT_Init();
    StackGuard_Init();
    SEGGER_RTT_WriteString(0, "\r\nTCP2UART boot\r\n");
    if (g_clock_fallback_to_hsi != 0u) {
        SEGGER_RTT_WriteString(0, "WARN: HSE start failed, fallback to HSI PLL\r\n");
    }

    lwip_init();
    IP4_ADDR(&ipaddr, cfg->net.ip[0], cfg->net.ip[1], cfg->net.ip[2], cfg->net.ip[3]);
    IP4_ADDR(&netmask, cfg->net.mask[0], cfg->net.mask[1], cfg->net.mask[2], cfg->net.mask[3]);
    IP4_ADDR(&gateway, cfg->net.gw[0], cfg->net.gw[1], cfg->net.gw[2], cfg->net.gw[3]);
    lwip_netif_init(&ipaddr, &netmask, &gateway);
    App_ConfigureLinks(cfg);
    BootDiag_ReportCh390();

    if (HAL_UART_Receive_IT(&huart1, (uint8_t *)&g_uart1_rx_probe_byte, 1u) != HAL_OK) {
        Error_Handler();
    }
}

static bool App_SendTcpServerPayload(uint8_t instance, const uint8_t *data, uint16_t len)
{
    return tcp_server_send(instance, data, len) == (int)len;
}

static bool App_SendTcpClientPayload(uint8_t instance, const uint8_t *data, uint16_t len)
{
    return tcp_client_send(instance, data, len) == (int)len;
}

static bool App_SendToUart(uint8_t uart_index, uint8_t src_id, uint8_t dst_mask, const uint8_t *data, uint16_t len)
{
    const device_config_t *cfg = config_get();
    uart_channel_t channel = (uart_index == LINK_UART_U1) ? UART_CHANNEL_U1 : UART_CHANNEL_U0;
    uint16_t written;

    if (cfg->mux_mode == MUX_MODE_FRAME) {
        uint8_t frame[APP_ROUTE_BUFFER_SIZE + 6u];
        uint16_t frame_len = 0u;
        if (uart_mux_encode_frame(src_id, dst_mask, data, len, frame, &frame_len, sizeof(frame))) {
            written = uart_trans_write(channel, frame, frame_len);
            return written == frame_len;
        }
        return false;
    } else {
        written = uart_trans_write(channel, data, len);
        return written == len;
    }
}

static void App_RouteTcpTraffic(void)
{
    const device_config_t *cfg = config_get();
    uint8_t buffer[APP_ROUTE_BUFFER_SIZE];

    for (uint8_t i = 0; i < TCP_SERVER_INSTANCE_COUNT; ++i) {
        int rc = tcp_server_recv(i, buffer, sizeof(buffer));
        if (rc > 0) {
            uint8_t link_index = (i == 0u) ? CONFIG_LINK_S1 : CONFIG_LINK_S2;
            if (!App_SendToUart(cfg->links[link_index].uart,
                                config_link_index_to_endpoint(link_index),
                                config_uart_index_to_endpoint(cfg->links[link_index].uart),
                                buffer,
                                (uint16_t)rc)) {
                return;
            }
        }
    }

    for (uint8_t i = 0; i < TCP_CLIENT_INSTANCE_COUNT; ++i) {
        int rc = tcp_client_recv(i, buffer, sizeof(buffer));
        if (rc > 0) {
            uint8_t link_index = (i == 0u) ? CONFIG_LINK_C1 : CONFIG_LINK_C2;
            if (!App_SendToUart(cfg->links[link_index].uart,
                                config_link_index_to_endpoint(link_index),
                                config_uart_index_to_endpoint(cfg->links[link_index].uart),
                                buffer,
                                (uint16_t)rc)) {
                return;
            }
        }
    }
}

static void App_RouteRawUartTraffic(void)
{
    const device_config_t *cfg = config_get();
    uint8_t buffer[APP_ROUTE_BUFFER_SIZE];
    uint16_t len;

    len = uart_trans_read(UART_CHANNEL_U0, buffer, sizeof(buffer));
    if (len > 0u) {
        bool routed_ok = true;
        for (uint8_t i = 0; i < CONFIG_LINK_COUNT; ++i) {
            bool sent = true;
            if (cfg->links[i].enabled == 0u || cfg->links[i].uart != LINK_UART_U0) {
                continue;
            }
            if (i == CONFIG_LINK_S1) {
                sent = App_SendTcpServerPayload(0u, buffer, len);
            } else if (i == CONFIG_LINK_S2) {
                sent = App_SendTcpServerPayload(1u, buffer, len);
            } else if (i == CONFIG_LINK_C1) {
                sent = App_SendTcpClientPayload(0u, buffer, len);
            } else if (i == CONFIG_LINK_C2) {
                sent = App_SendTcpClientPayload(1u, buffer, len);
            }

            if (!sent) {
                routed_ok = false;
            }
        }

        if (!routed_ok) {
            return;
        }
    }

    len = uart_trans_read(UART_CHANNEL_U1, buffer, sizeof(buffer));
    if (len > 0u) {
        bool routed_ok = true;
        for (uint8_t i = 0; i < CONFIG_LINK_COUNT; ++i) {
            bool sent = true;
            if (cfg->links[i].enabled == 0u || cfg->links[i].uart != LINK_UART_U1) {
                continue;
            }
            if (i == CONFIG_LINK_S1) {
                sent = App_SendTcpServerPayload(0u, buffer, len);
            } else if (i == CONFIG_LINK_S2) {
                sent = App_SendTcpServerPayload(1u, buffer, len);
            } else if (i == CONFIG_LINK_C1) {
                sent = App_SendTcpClientPayload(0u, buffer, len);
            } else if (i == CONFIG_LINK_C2) {
                sent = App_SendTcpClientPayload(1u, buffer, len);
            }

            if (!sent) {
                routed_ok = false;
            }
        }

        if (!routed_ok) {
            return;
        }
    }
}

static void App_RouteMuxUartTraffic(void)
{
    uart_mux_frame_t frame;
    const device_config_t *cfg = config_get();
    bool routed_ok;

    while (uart_mux_try_extract_frame(UART_CHANNEL_U0, &frame)) {
#if defined(DEBUG) && (DEBUG != 0)
        SEGGER_RTT_printf(0, "Mux frame from UART0: src_id=%u dst_mask=0x%02X len=%u\r\n", frame.src_id, frame.dst_mask, frame.payload_len);
#endif
        if (frame.dst_mask == 0u) {
            at_result_t result;
            char *response_text = (char *)&g_mux_response_frame[5];
            if (config_build_response_frame(frame.payload, frame.payload_len, response_text, (uint16_t)(sizeof(g_mux_response_frame) - 6u), &result)) {
                uint16_t response_len = (uint16_t)strlen(response_text);
                uint16_t frame_len = 0u;
                if (uart_mux_encode_frame(config_uart_index_to_endpoint(LINK_UART_U0), 0u, (const uint8_t *)response_text, response_len, g_mux_response_frame, &frame_len, sizeof(g_mux_response_frame))) {
                    if (uart_trans_write(UART_CHANNEL_U0, g_mux_response_frame, frame_len) != frame_len) {
                        return;
                    }
                }
                if (result == AT_NEED_REBOOT) {
                    static const char hint[] = "Note: Use AT+SAVE then AT+RESET to apply changes\r\n";
                    response_len = (uint16_t)strlen(hint);
                    if (uart_mux_encode_frame(config_uart_index_to_endpoint(LINK_UART_U0), 0u, (const uint8_t *)hint, response_len, g_mux_response_frame, &frame_len, sizeof(g_mux_response_frame))) {
                        if (uart_trans_write(UART_CHANNEL_U0, g_mux_response_frame, frame_len) != frame_len) {
                            return;
                        }
                    }
                }
            }
            continue;
        }

        routed_ok = true;
        if ((frame.dst_mask & ENDPOINT_S1) != 0u) {
            routed_ok = App_SendTcpServerPayload(0u, frame.payload, frame.payload_len) && routed_ok;
        }
        if ((frame.dst_mask & ENDPOINT_S2) != 0u) {
            routed_ok = App_SendTcpServerPayload(1u, frame.payload, frame.payload_len) && routed_ok;
        }
        if ((frame.dst_mask & ENDPOINT_C1) != 0u) {
            routed_ok = App_SendTcpClientPayload(0u, frame.payload, frame.payload_len) && routed_ok;
        }
        if ((frame.dst_mask & ENDPOINT_C2) != 0u) {
            routed_ok = App_SendTcpClientPayload(1u, frame.payload, frame.payload_len) && routed_ok;
        }
        if ((frame.dst_mask & ENDPOINT_UART3) != 0u && cfg->links[CONFIG_LINK_S2].uart == LINK_UART_U1) {
            routed_ok = App_SendToUart(LINK_UART_U1, frame.src_id, ENDPOINT_UART3, frame.payload, frame.payload_len) && routed_ok;
        }

        if (!routed_ok) {
            return;
        }
    }

    while (uart_mux_try_extract_frame(UART_CHANNEL_U1, &frame)) {
#if defined(DEBUG) && (DEBUG != 0)
        SEGGER_RTT_printf(0, "Mux frame from UART1: src_id=%u dst_mask=0x%02X len=%u\r\n", frame.src_id, frame.dst_mask, frame.payload_len);
#endif
        if (frame.dst_mask == 0u) {
            at_result_t result;
            char *response_text = (char *)&g_mux_response_frame[5];
            if (config_build_response_frame(frame.payload, frame.payload_len, response_text, (uint16_t)(sizeof(g_mux_response_frame) - 6u), &result)) {
                uint16_t response_len = (uint16_t)strlen(response_text);
                uint16_t frame_len = 0u;
                if (uart_mux_encode_frame(config_uart_index_to_endpoint(LINK_UART_U1), 0u, (const uint8_t *)response_text, response_len, g_mux_response_frame, &frame_len, sizeof(g_mux_response_frame))) {
                    if (uart_trans_write(UART_CHANNEL_U1, g_mux_response_frame, frame_len) != frame_len) {
                        return;
                    }
                }
                if (result == AT_NEED_REBOOT) {
                    static const char hint[] = "Note: Use AT+SAVE then AT+RESET to apply changes\r\n";
                    response_len = (uint16_t)strlen(hint);
                    if (uart_mux_encode_frame(config_uart_index_to_endpoint(LINK_UART_U1), 0u, (const uint8_t *)hint, response_len, g_mux_response_frame, &frame_len, sizeof(g_mux_response_frame))) {
                        if (uart_trans_write(UART_CHANNEL_U1, g_mux_response_frame, frame_len) != frame_len) {
                            return;
                        }
                    }
                }
            }
            continue;
        }

        routed_ok = true;
        if ((frame.dst_mask & ENDPOINT_S1) != 0u) {
            routed_ok = App_SendTcpServerPayload(0u, frame.payload, frame.payload_len) && routed_ok;
        }
        if ((frame.dst_mask & ENDPOINT_S2) != 0u) {
            routed_ok = App_SendTcpServerPayload(1u, frame.payload, frame.payload_len) && routed_ok;
        }
        if ((frame.dst_mask & ENDPOINT_C1) != 0u) {
            routed_ok = App_SendTcpClientPayload(0u, frame.payload, frame.payload_len) && routed_ok;
        }
        if ((frame.dst_mask & ENDPOINT_C2) != 0u) {
            routed_ok = App_SendTcpClientPayload(1u, frame.payload, frame.payload_len) && routed_ok;
        }
        if ((frame.dst_mask & ENDPOINT_UART2) != 0u) {
            routed_ok = App_SendToUart(LINK_UART_U0, frame.src_id, ENDPOINT_UART2, frame.payload, frame.payload_len) && routed_ok;
        }

        if (!routed_ok) {
            return;
        }
    }
}

static void App_Poll(void)
{
    static uint32_t s_health_check_tick;
    uint32_t now;

    ethernetif_poll();
    ethernetif_check_link();
    sys_check_timeouts();
    App_StopLinksIfNeeded();
    App_StartLinksIfNeeded();
    tcp_client_poll();
    uart_trans_poll();
    StackGuard_Check();
    config_poll();
    App_RouteTcpTraffic();

    if (config_get()->mux_mode == MUX_MODE_FRAME) {
        App_RouteMuxUartTraffic();
    } else {
        App_RouteRawUartTraffic();
    }

    now = HAL_GetTick();
    if ((now - s_health_check_tick) >= APP_HEALTH_CHECK_INTERVAL_MS) {
        s_health_check_tick = now;
        ch390_runtime_health_check(&ch390_netif);
    }

    if (config_is_reset_requested()) {
        config_clear_reset_requested();
        NVIC_SystemReset();
    }

    if (hiwdg.Instance == IWDG) {
        HAL_IWDG_Refresh(&hiwdg);
    }
}
/* USER CODE END 0 */

int main(void)
{
  HAL_Init();
  SystemClock_Config();
  MX_GPIO_Init();
  MX_DMA_Init();
  MX_USART1_UART_Init();
  MX_USART2_UART_Init();
  MX_USART3_UART_Init();
  MX_SPI1_Init();
  MX_TIM4_Init();

  LED_Init();
  LED_StartBlink();
  App_Init();

  while (1)
  {
    App_Poll();
  }
}

void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  g_clock_fallback_to_hsi = 0u;
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_LSI|RCC_OSCILLATORTYPE_HSE;
  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
  RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1;
  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
  RCC_OscInitStruct.LSIState = RCC_LSI_ON;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) {
    RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_LSI | RCC_OSCILLATORTYPE_HSI;
    RCC_OscInitStruct.HSEState = RCC_HSE_OFF;
    RCC_OscInitStruct.HSIState = RCC_HSI_ON;
    RCC_OscInitStruct.LSIState = RCC_LSI_ON;
    RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
    RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI_DIV2;
    RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL16;
    if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) {
      Error_Handler();
    }
    g_clock_fallback_to_hsi = 1u;
  }

  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
  {
    Error_Handler();
  }
}

#ifdef __GNUC__
int _write(int file, char *ptr, int len)
{
    (void)file;
    return (int)SEGGER_RTT_Write(0, ptr, (unsigned)len);
}
#else
int fputc(int ch, FILE *f)
{
    (void)f;
    SEGGER_RTT_PutChar(0, (char)ch);
    return ch;
}
#endif

void Debug_TrapWithRttHint(const char *tag)
{
    SEGGER_RTT_WriteString(0, "\r\nTRAP: ");
    SEGGER_RTT_WriteString(0, tag);
    SEGGER_RTT_WriteString(0, "\r\n");
    __disable_irq();
    __BKPT(0);
    while (1)
    {
    }
}

void Error_Handler(void)
{
  Debug_TrapWithRttHint("Error_Handler");
}

#ifdef USE_FULL_ASSERT
void assert_failed(uint8_t *file, uint32_t line)
{
  (void)file;
  (void)line;
  Debug_TrapWithRttHint("assert_failed");
}
#endif