2_way_Tochie/main/main.c

#include <stdio.h>
#include <string.h>
#include <unistd.h>

#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

#include "esp_wifi.h"
#include "esp_event.h"
#include "esp_log.h"
#include "nvs_flash.h"

#include "lwip/sockets.h"

#include <stdbool.h>
#include "mbedtls/md5.h"

#define WIFI_SSID "WIFI-95E4 2.4"
#define WIFI_PASS "beard1354canyon"

#define ASTERISK_IP "10.0.0.167"
#define SIP_PORT 5060

static const char *TAG = "SIP";
static volatile bool wifi_ready = false;

/* ================= WIFI ================= */

static void wifi_event_handler(void *arg, esp_event_base_t event_base,
                               int32_t event_id, void *event_data)
{
    if (event_base == WIFI_EVENT && event_id == WIFI_EVENT_STA_START) {
        esp_wifi_connect();
    }
    else if (event_base == WIFI_EVENT && event_id == WIFI_EVENT_STA_DISCONNECTED) {
        ESP_LOGI(TAG, "WiFi disconnected, retrying...");
        wifi_ready = false;
        esp_wifi_connect();
    }
    else if (event_base == IP_EVENT && event_id == IP_EVENT_STA_GOT_IP) {
        ESP_LOGI(TAG, "WiFi connected!");
        wifi_ready = true;
    }
}

void wifi_init(void)
{
    nvs_flash_init();
    esp_netif_init();
    esp_event_loop_create_default();
    esp_netif_create_default_wifi_sta();

    wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT();
    esp_wifi_init(&cfg);

    esp_event_handler_register(WIFI_EVENT, ESP_EVENT_ANY_ID, &wifi_event_handler, NULL);
    esp_event_handler_register(IP_EVENT, IP_EVENT_STA_GOT_IP, &wifi_event_handler, NULL);

    wifi_config_t wifi_config = {
        .sta = {
            .ssid = WIFI_SSID,
            .password = WIFI_PASS,
        },
    };

    esp_wifi_set_mode(WIFI_MODE_STA);
    esp_wifi_set_config(WIFI_IF_STA, &wifi_config);
    esp_wifi_start();
}

/* ================= MD5 HELPERS ================= */

static void md5_calc(const unsigned char *input, size_t len, unsigned char output[16])
{
    mbedtls_md5_context ctx;
    mbedtls_md5_init(&ctx);
    mbedtls_md5_starts(&ctx);
    mbedtls_md5_update(&ctx, input, len);
    mbedtls_md5_finish(&ctx, output);
    mbedtls_md5_free(&ctx);
}

static void md5_to_hex(unsigned char *md5, char *out)
{
    for (int i = 0; i < 16; i++) {
        sprintf(out + i * 2, "%02x", md5[i]);
    }
}

/* ================= SIP DIGEST ================= */

void sip_compute_response(
    const char *username,
    const char *realm,
    const char *password,
    const char *nonce,
    const char *uri,
    char *out_response)
{
    unsigned char ha1_md5[16], ha2_md5[16], final_md5[16];
    char ha1_hex[64], ha2_hex[64], final_str[256];
    char ha1[128], ha2[128];

    snprintf(ha1, sizeof(ha1), "%s:%s:%s", username, realm, password);
    md5_calc((unsigned char*)ha1, strlen(ha1), ha1_md5);
    md5_to_hex(ha1_md5, ha1_hex);

    snprintf(ha2, sizeof(ha2), "REGISTER:%s", uri);
    md5_calc((unsigned char*)ha2, strlen(ha2), ha2_md5);
    md5_to_hex(ha2_md5, ha2_hex);

    snprintf(final_str, sizeof(final_str),
             "%s:%s:%s", ha1_hex, nonce, ha2_hex);

    md5_calc((unsigned char*)final_str, strlen(final_str), final_md5);
    md5_to_hex(final_md5, out_response);
}

/* ================= SIP TASK ================= */

void sip_register_task(void *pvParameters)
{
    struct sockaddr_in server = {0};
    server.sin_family = AF_INET;
    server.sin_port = htons(SIP_PORT);
    server.sin_addr.s_addr = inet_addr(ASTERISK_IP);

    int sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);

    if (sock < 0) {
        ESP_LOGE(TAG, "Socket failed");
        vTaskDelete(NULL);
        return;
    }

    struct timeval timeout = {
        .tv_sec = 3,
        .tv_usec = 0
    };

    setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout));

    /* ================= FIRST REGISTER ================= */

    char register_msg[512];

    snprintf(register_msg, sizeof(register_msg),
        "REGISTER sip:%s SIP/2.0\r\n"
        "Via: SIP/2.0/UDP 0.0.0.0:5062\r\n"
        "From: <sip:1001@%s>;tag=1234\r\n"
        "To: <sip:1001@%s>\r\n"
        "Call-ID: esp32-0001\r\n"
        "CSeq: 1 REGISTER\r\n"
        "Contact: <sip:1001@0.0.0.0:5062>\r\n"
        "Expires: 300\r\n"
        "Content-Length: 0\r\n"
        "\r\n",
        ASTERISK_IP,
        ASTERISK_IP,
        ASTERISK_IP);

    ESP_LOGI(TAG, "Sending REGISTER...");
    sendto(sock, register_msg, strlen(register_msg), 0,
           (struct sockaddr*)&server, sizeof(server));

    /* ================= RECEIVE 401 ================= */

    char response[1024];
    socklen_t addr_len = sizeof(server);

    int len = recvfrom(sock, response, sizeof(response) - 1, 0,
                       (struct sockaddr*)&server, &addr_len);

    if (len <= 0) {
        ESP_LOGE(TAG, "No response");
        close(sock);
        vTaskDelete(NULL);
        return;
    }

    response[len] = '\0';
    printf("401 RESPONSE:\n%s\n", response);

    if (!strstr(response, "401")) {
        ESP_LOGE(TAG, "Expected 401 Unauthorized");
        close(sock);
        vTaskDelete(NULL);
        return;
    }

    /* ================= EXTRACT NONCE ================= */

    char sip_nonce[256] = {0};

    char *n = strstr(response, "nonce=\"");
    if (!n) {
        ESP_LOGE(TAG, "No nonce found");
        close(sock);
        vTaskDelete(NULL);
        return;
    }

    n += 7;

    char *end = strchr(n, '"');
    if (!end) {
        ESP_LOGE(TAG, "Bad nonce format");
        close(sock);
        vTaskDelete(NULL);
        return;
    }

    size_t nlen = end - n;

    if (nlen >= sizeof(sip_nonce)) {
        ESP_LOGE(TAG, "Nonce too long");
        close(sock);
        vTaskDelete(NULL);
        return;
    }

    strncpy(sip_nonce, n, nlen);
    sip_nonce[nlen] = '\0';

    ESP_LOGI(TAG, "Nonce: %s", sip_nonce);

    /* ================= EXTRACT REALM ================= */

    char sip_realm[128] = {0};

    char *r = strstr(response, "realm=\"");
    if (r) {
        r += 7;
        char *rend = strchr(r, '"');
        if (rend) {
            size_t rlen = rend - r;
            if (rlen < sizeof(sip_realm)) {
                strncpy(sip_realm, r, rlen);
                sip_realm[rlen] = '\0';
            }
        }
    }

    if (sip_realm[0] == '\0') {
        ESP_LOGE(TAG, "Realm not found");
        close(sock);
        vTaskDelete(NULL);
        return;
    }

    ESP_LOGI(TAG, "Realm: %s", sip_realm);

    /* ================= AUTH DIGEST ================= */

    char response_hash[64];

    sip_compute_response(
        "1001",
        sip_realm,
        "secret123",
        sip_nonce,
        "sip:10.0.0.167",
        response_hash
    );

    /* ================= AUTH REGISTER ================= */

    char auth[1024];

    snprintf(auth, sizeof(auth),
        "REGISTER sip:%s SIP/2.0\r\n"
        "Via: SIP/2.0/UDP 0.0.0.0:5062\r\n"
        "From: <sip:1001@%s>;tag=1234\r\n"
        "To: <sip:1001@%s>\r\n"
        "Call-ID: esp32-0001\r\n"
        "CSeq: 2 REGISTER\r\n"
        "Contact: <sip:1001@0.0.0.0:5062>\r\n"
        "Authorization: Digest username=\"1001\", realm=\"%s\", nonce=\"%s\", uri=\"sip:%s\", response=\"%s\"\r\n"
        "Expires: 300\r\n"
        "Content-Length: 0\r\n"
        "\r\n",
        ASTERISK_IP,
        ASTERISK_IP,
        ASTERISK_IP,
        sip_realm,
        sip_nonce,
        ASTERISK_IP,
        response_hash);

    ESP_LOGI(TAG, "Sending AUTH REGISTER...");
    sendto(sock, auth, strlen(auth), 0,
           (struct sockaddr*)&server, sizeof(server));

    /* ================= FINAL RESPONSE ================= */

    char final_resp[1024];

    int len2 = recvfrom(sock,
                        final_resp,
                        sizeof(final_resp) - 1,
                        0,
                        (struct sockaddr*)&server,
                        &addr_len);

    if (len2 > 0) {
        final_resp[len2] = '\0';
        printf("FINAL RESPONSE:\n%s\n", final_resp);

        if (strstr(final_resp, "200 OK")) {
            ESP_LOGI(TAG, "🎉 SIP REGISTER SUCCESS");
        } else {
            ESP_LOGW(TAG, "REGISTER FAILED");
        }
    } else {
        ESP_LOGW(TAG, "No final response");
    }

    close(sock);
    vTaskDelete(NULL);
}

/* ================= MAIN ================= */

void app_main(void)
{
    ESP_LOGI(TAG, "Starting SIP client...");

    wifi_init();

    while (!wifi_ready) {
        vTaskDelay(pdMS_TO_TICKS(500));
    }

    xTaskCreate(sip_register_task,
                "sip_register",
                12288,
                NULL,
                5,
                NULL);
}