sepfy · Aug 27, 2024 · Aug 28, 2024 · Aug 28, 2024 · Aug 28, 2024 · Aug 29, 2024
diff --git a/.gitignore b/.gitignore
@@ -1 +1,2 @@
 build
+builders/cmake/_anyka_v500_uclibc_chacam-release-static
diff --git a/builders/cmake/CMakeLists.txt b/builders/cmake/CMakeLists.txt
@@ -0,0 +1,53 @@
+cmake_minimum_required (VERSION 3.5)
+
+project (libpeer)
+
+LIST(APPEND CMAKE_MODULE_PATH "$ENV{AWOXCVS}/AwoxSoftware/builders/cmake")
+include (aw2)
+
+set_subdirectory (peer-module)
+get_filename_component(ccCompDir ${CMAKE_CURRENT_LIST_DIR}/../.. ABSOLUTE)
+
+set (sourceFiles
+		${ccCompDir}/src/address.c
+		${ccCompDir}/src/agent.c
+		${ccCompDir}/src/base64.c
+		${ccCompDir}/src/buffer.c
+		${ccCompDir}/src/dtls_srtp.c
+		${ccCompDir}/src/ice.c
+		${ccCompDir}/src/mdns.c
+		${ccCompDir}/src/peer.c
+		${ccCompDir}/src/peer_connection.c
+#		${ccCompDir}/src/peer_signaling.c
+		${ccCompDir}/src/ports.c
+		${ccCompDir}/src/rtcp.c
+		${ccCompDir}/src/rtp.c
+		${ccCompDir}/src/sctp.c
+		${ccCompDir}/src/sdp.c
+		${ccCompDir}/src/socket.c
+#		${ccCompDir}/src/ssl_transport.c
+		${ccCompDir}/src/stun.c
+#		${ccCompDir}/src/tcp.c
+#		${ccCompDir}/src/udp.c
+		${ccCompDir}/src/utils.c
+)
+
+set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=c99")
+
+add_subdirectory_once (mbedtls-module ${awRootDir}/AwoxSoftware/Libs/External/mbedtls${mbedtls_version}/builders/cmake)
+add_subdirectory_once (usrsctp-module ${awRootDir}/AwoxSoftware/Libs/External/usrsctp/builders/cmake)
+add_subdirectory_once (libsrtp2-module ${ccRootDir}/components/libsrtp2/builders/cmake)
+add_subdirectory_once (cjson-module ${ccRootDir}/components/cJSON/builders/cmake)
+list(APPEND link_Libraries cjson mbedtls mbedcrypto mbedx509 libsrtp2 usrsctp-static)
+
+add_definitions(-DICE_LITE -DCONFIG_MQTT=0 -DCONFIG_HTTP=0 -DLOG_LEVEL=LEVEL_DEBUG -DLOG_REDIRECT=1 -DBUFFER_USE_MUTEX=1 -DAGENT_MAX_CANDIDATES=20 -DAGENT_MAX_CANDIDATE_PAIRS=400)
+add_library(peer ${sourceFiles})
+target_link_libraries(peer ${link_Libraries})
+
+include_directories(peer ${ccCompDir})
+
+target_include_directories (peer PUBLIC ${ccCompDir}/src)
+
+install(TARGETS peer ARCHIVE DESTINATION ${AW_ARCHIVE_DIR})
+install(TARGETS peer LIBRARY DESTINATION ${AW_LIBRARY_DIR})
+
diff --git a/src/agent.c b/src/agent.c
@@ -291,7 +291,7 @@ void agent_get_local_description(Agent* agent, char* description, int length) {
   }
 
   // remove last \n
-  description[strlen(description)] = '\0';
+//  description[strlen(description)] = '\0';
   LOGD("local description:\n%s", description);
 }
 
@@ -301,15 +301,24 @@ int agent_send(Agent* agent, const uint8_t* buf, int len) {
 
 static void agent_create_binding_response(Agent* agent, StunMessage* msg, Address* addr) {
   char username[584];
-  char mapped_address[8];
+  char mapped_address[16];
+	uint8_t mask[16];
   StunHeader* header;
   stun_msg_create(msg, STUN_CLASS_RESPONSE | STUN_METHOD_BINDING);
   header = (StunHeader*)msg->buf;
   memcpy(header->transaction_id, agent->transaction_id, sizeof(header->transaction_id));
   snprintf(username, sizeof(username), "%s:%s", agent->local_ufrag, agent->remote_ufrag);
-  // TODO: XOR-MAPPED-ADDRESS
-  stun_set_mapped_address(mapped_address, NULL, addr);
-  stun_msg_write_attr(msg, STUN_ATTR_TYPE_MAPPED_ADDRESS, 8, mapped_address);
+
+  // MAPPED-ADDRESS
+	memset(mask, 0, 16);
+  stun_set_mapped_address(mapped_address, mask, addr);
+  stun_msg_write_attr(msg, STUN_ATTR_TYPE_MAPPED_ADDRESS, (addr->family == AF_INET) ? 8 : 16, mapped_address);
+
+	// XOR-MAPPED-ADDRESS
+	stun_setup_xor_address_mask(mask, header->transaction_id);
+	stun_set_mapped_address(mapped_address, mask, &agent->nominated_pair->remote->addr);
+	stun_msg_write_attr(msg, STUN_ATTR_TYPE_XOR_MAPPED_ADDRESS, (addr->family == AF_INET) ? 8 : 16, mapped_address);
+
   stun_msg_write_attr(msg, STUN_ATTR_TYPE_USERNAME, strlen(username), username);
   stun_msg_finish(msg, STUN_CREDENTIAL_SHORT_TERM, agent->local_upwd, strlen(agent->local_upwd));
 }
@@ -404,16 +413,23 @@ void agent_set_remote_description(Agent* agent, char* description) {
       strncpy(agent->remote_upwd, line_start + strlen("a=ice-pwd:"), line_end - line_start - strlen("a=ice-pwd:"));
 
     } else if (strncmp(line_start, "a=candidate:", strlen("a=candidate:")) == 0) {
+
+	  if ( agent->remote_candidates_count < AGENT_MAX_CANDIDATES) {
       if (ice_candidate_from_description(&agent->remote_candidates[agent->remote_candidates_count], line_start, line_end) == 0) {
         agent->remote_candidates_count++;
       }
     }
+	  else
+		  LOGE("Too many ICE candidates\n");
+	}
 
     line_start = line_end + 2;
   }
 
   LOGD("remote ufrag: %s", agent->remote_ufrag);
   LOGD("remote upwd: %s", agent->remote_upwd);
+  LOGD("local candidates: %d", agent->local_candidates_count);
+  LOGD("remote candidates: %d", agent->remote_candidates_count);
 
   // Please set gather candidates before set remote description
   for (i = 0; i < agent->local_candidates_count; i++) {

diff --git a/src/buffer.c b/src/buffer.c
@@ -1,13 +1,27 @@
-#include <stdint.h>
 #include <stdio.h>
 #include <stdlib.h>
+#include <stdint.h>
 #include <string.h>
 
-#include "buffer.h"
 #include "utils.h"
+#include "buffer.h"
+
+#if BUFFER_USE_MUTEX
+# include <pthread.h>
+# define PEER_MUTEX_INIT(X)    static pthread_mutex_t X = PTHREAD_MUTEX_INITIALIZER
+# define PEER_MUTEX_LOCK(X)    pthread_mutex_lock(X)
+# define PEER_MUTEX_UNLOCK(X)  pthread_mutex_unlock(X)
+#else
+# define PEER_MUTEX_INIT(X)
+# define PEER_MUTEX_LOCK(X)
+# define PEER_MUTEX_UNLOCK(X)
+#endif
+
+PEER_MUTEX_INIT(g_buffer_mutex);
 
 Buffer* buffer_new(int size) {
-  Buffer* rb;
+
+  Buffer *rb;
   rb = (Buffer*)calloc(1, sizeof(Buffer));
 
   rb->data = (uint8_t*)calloc(1, size);
@@ -18,86 +32,96 @@ Buffer* buffer_new(int size) {
   return rb;
 }
 
-void buffer_clear(Buffer* rb) {
-  rb->head = 0;
-  rb->tail = 0;
+void buffer_clear(Buffer *rb) {
+  PEER_MUTEX_LOCK(&g_buffer_mutex);
+  if (rb) {
+    rb->head = 0;
+    rb->tail = 0;
+  }
+  PEER_MUTEX_UNLOCK(&g_buffer_mutex);
 }
 
-void buffer_free(Buffer* rb) {
+void buffer_free(Buffer *rb) {
+  PEER_MUTEX_LOCK(&g_buffer_mutex);
   if (rb) {
     free(rb->data);
     rb->data = NULL;
-    rb = NULL;
+    free(rb);  // Now properly freeing the buffer itself
   }
+  PEER_MUTEX_UNLOCK(&g_buffer_mutex);
 }
 
-int buffer_push_tail(Buffer* rb, const uint8_t* data, int size) {
-  int free_space = (rb->size + rb->head - rb->tail - 1) % rb->size;
+int buffer_push_tail(Buffer *rb, const uint8_t *data, int size) {
+  PEER_MUTEX_LOCK(&g_buffer_mutex);
 
+  int free_space = (rb->size + rb->head - rb->tail - 1) % rb->size;
   int align_size = ALIGN32(size + 4);
 
   if (align_size > free_space) {
     LOGE("no enough space");
+    PEER_MUTEX_UNLOCK(&g_buffer_mutex);
     return -1;
   }
 
   int tail_end = (rb->tail + align_size) % rb->size;
 
   if (tail_end < rb->tail) {
+    // Handle wrap-around
     if (rb->head < align_size) {
       LOGE("no enough space");
+      PEER_MUTEX_UNLOCK(&g_buffer_mutex);
       return -1;
     }
 
-    int* p = (int*)(rb->data + rb->tail);
+    int *p = (int*)(rb->data + rb->tail);
     *p = size;
-    memcpy(rb->data, data, size);
-    rb->tail = size;
-
+    memcpy(rb->data + rb->tail + 4, data, rb->size - rb->tail - 4);
+    memcpy(rb->data, data + (rb->size - rb->tail - 4), tail_end);
   } else {
-    int* p = (int*)(rb->data + rb->tail);
+    int *p = (int*)(rb->data + rb->tail);
     *p = size;
     memcpy(rb->data + rb->tail + 4, data, size);
-    rb->tail = tail_end;
   }
 
+  rb->tail = tail_end;
+
+  PEER_MUTEX_UNLOCK(&g_buffer_mutex);
   return size;
 }
 
-uint8_t* buffer_peak_head(Buffer* rb, int* size) {
+uint8_t* buffer_peak_head(Buffer *rb, int *size) {
+  PEER_MUTEX_LOCK(&g_buffer_mutex);
   if (!rb || rb->head == rb->tail) {
+    (*size) = 0;
+    PEER_MUTEX_UNLOCK(&g_buffer_mutex);
     return NULL;
   }
 
   *size = *((int*)(rb->data + rb->head));
 
   int align_size = ALIGN32(*size + 4);
-
   int head_end = (rb->head + align_size) % rb->size;
 
-  if (head_end < rb->head) {
-    return rb->data;
+  PEER_MUTEX_UNLOCK(&g_buffer_mutex);
 
+  if (head_end < rb->head) {
+    return rb->data;  // Returns pointer to start of buffer in case of wrap-around
   } else {
     return rb->data + (rb->head + 4);
   }
 }
 
-void buffer_pop_head(Buffer* rb) {
+void buffer_pop_head(Buffer *rb) {
+  PEER_MUTEX_LOCK(&g_buffer_mutex);
+
   if (!rb || rb->head == rb->tail) {
+    PEER_MUTEX_UNLOCK(&g_buffer_mutex);
     return;
   }
 
-  int* size = (int*)(rb->data + rb->head);
-
+  int *size = (int*)(rb->data + rb->head);
   int align_size = ALIGN32(*size + 4);
+  rb->head = (rb->head + align_size) % rb->size;
 
-  int head_end = (rb->head + align_size) % rb->size;
-
-  if (head_end < rb->head) {
-    rb->head = *size;
-
-  } else {
-    rb->head = rb->head + align_size;
-  }
+  PEER_MUTEX_UNLOCK(&g_buffer_mutex);
 }
diff --git a/src/config.h b/src/config.h
@@ -16,16 +16,19 @@
 #define DATA_RB_DATA_LENGTH (SCTP_MTU * 128)
 #endif
 
-#define AUDIO_LATENCY 20  // ms
+#define AUDIO_LATENCY 20 // ms
 #define KEEPALIVE_CONNCHECK 10000
 #define CONFIG_IPV6 0
 // default use wifi interface
 #define IFR_NAME "w"
 
-// #define LOG_LEVEL LEVEL_DEBUG
+//#define LOG_LEVEL LEVEL_DEBUG
+#ifndef LOG_REDIRECT
 #define LOG_REDIRECT 0
+#endif
 
-// Disable MQTT and HTTP signaling
-// #define DISABLE_PEER_SIGNALING 1
+#ifndef BUFFER_USE_MUTEX
+#define BUFFER_USE_MUTEX  0
+#endif
 
-#endif  // CONFIG_H_
+#endif // CONFIG_H_
diff --git a/src/peer_connection.c b/src/peer_connection.c
@@ -51,6 +51,8 @@ struct PeerConnection {
 
   uint32_t remote_assrc;
   uint32_t remote_vssrc;
+
+	SdpBundleInfo bundle_info;
 };
 
 static void peer_connection_outgoing_rtp_packet(uint8_t* data, size_t size, void* user_data) {
@@ -167,6 +169,11 @@ PeerConnection* peer_connection_create(PeerConfiguration* config) {
 
   memcpy(&pc->config, config, sizeof(PeerConfiguration));
 
+  if (pc->config.video_ssrc == 0)
+    pc->config.video_ssrc = (uint32_t) rand();
+  if (pc->config.audio_ssrc == 0)
+    pc->config.audio_ssrc = (uint32_t) rand();
+
   pc->agent.mode = AGENT_MODE_CONTROLLED;
 
   memset(&pc->sctp, 0, sizeof(pc->sctp));
@@ -180,7 +187,7 @@ PeerConnection* peer_connection_create(PeerConfiguration* config) {
     LOGI("Audio allocates heap size: %d", AUDIO_RB_DATA_LENGTH);
     pc->audio_rb = buffer_new(AUDIO_RB_DATA_LENGTH);
 
-    rtp_encoder_init(&pc->artp_encoder, pc->config.audio_codec,
+    rtp_encoder_init(&pc->artp_encoder, pc->config.audio_codec, pc->config.audio_ssrc,
                      peer_connection_outgoing_rtp_packet, (void*)pc);
 
     rtp_decoder_init(&pc->artp_decoder, pc->config.audio_codec,
@@ -191,11 +198,11 @@ PeerConnection* peer_connection_create(PeerConfiguration* config) {
     LOGI("Video allocates heap size: %d", VIDEO_RB_DATA_LENGTH);
     pc->video_rb = buffer_new(VIDEO_RB_DATA_LENGTH);
 
-    rtp_encoder_init(&pc->vrtp_encoder, pc->config.video_codec,
-                     peer_connection_outgoing_rtp_packet, (void*)pc);
+    rtp_encoder_init(&pc->vrtp_encoder, pc->config.video_codec, pc->config.video_ssrc,
+     peer_connection_outgoing_rtp_packet, (void*)pc);
 
     rtp_decoder_init(&pc->vrtp_decoder, pc->config.video_codec,
-                     pc->config.onvideotrack, pc->config.user_data);
+     pc->config.onvideotrack, pc->config.user_data);
   }
 
   return pc;
@@ -250,7 +257,7 @@ int peer_connection_datachannel_send_sid(PeerConnection* pc, char* message, size
     return sctp_outgoing_data(&pc->sctp, message, len, PPID_BINARY, sid);
 }
 
-static char* peer_connection_dtls_role_setup_value(DtlsSrtpRole d) {
+char* peer_connection_dtls_role_setup_value(DtlsSrtpRole d) {
   return d == DTLS_SRTP_ROLE_SERVER ? "a=setup:passive" : "a=setup:active";
 }
 
@@ -276,54 +283,7 @@ static void peer_connection_state_new(PeerConnection* pc, DtlsSrtpRole role) {
   }
 
   agent_get_local_description(&pc->agent, description, sizeof(pc->temp_buf));
-
-  memset(&pc->local_sdp, 0, sizeof(pc->local_sdp));
-  // TODO: check if we have video or audio codecs
-  sdp_create(&pc->local_sdp,
-             pc->config.video_codec != CODEC_NONE,
-             pc->config.audio_codec != CODEC_NONE,
-             pc->config.datachannel);
-
-  if (pc->config.video_codec == CODEC_H264) {
-    sdp_append_h264(&pc->local_sdp);
-    sdp_append(&pc->local_sdp, "a=fingerprint:sha-256 %s", pc->dtls_srtp.local_fingerprint);
-    sdp_append(&pc->local_sdp, peer_connection_dtls_role_setup_value(role));
-    strcat(pc->local_sdp.content, description);
-  }
-
-  switch (pc->config.audio_codec) {
-    case CODEC_PCMA:
-
-      sdp_append_pcma(&pc->local_sdp);
-      sdp_append(&pc->local_sdp, "a=fingerprint:sha-256 %s", pc->dtls_srtp.local_fingerprint);
-      sdp_append(&pc->local_sdp, peer_connection_dtls_role_setup_value(role));
-      strcat(pc->local_sdp.content, description);
-      break;
-
-    case CODEC_PCMU:
-
-      sdp_append_pcmu(&pc->local_sdp);
-      sdp_append(&pc->local_sdp, "a=fingerprint:sha-256 %s", pc->dtls_srtp.local_fingerprint);
-      sdp_append(&pc->local_sdp, peer_connection_dtls_role_setup_value(role));
-      strcat(pc->local_sdp.content, description);
-      break;
-
-    case CODEC_OPUS:
-      sdp_append_opus(&pc->local_sdp);
-      sdp_append(&pc->local_sdp, "a=fingerprint:sha-256 %s", pc->dtls_srtp.local_fingerprint);
-      sdp_append(&pc->local_sdp, peer_connection_dtls_role_setup_value(role));
-      strcat(pc->local_sdp.content, description);
-
-    default:
-      break;
-  }
-
-  if (pc->config.datachannel) {
-    sdp_append_datachannel(&pc->local_sdp);
-    sdp_append(&pc->local_sdp, "a=fingerprint:sha-256 %s", pc->dtls_srtp.local_fingerprint);
-    sdp_append(&pc->local_sdp, peer_connection_dtls_role_setup_value(role));
-    strcat(pc->local_sdp.content, description);
-  }
+  sdp_create(&pc->local_sdp, &pc->config, &pc->bundle_info, &pc->dtls_srtp, peer_connection_dtls_role_setup_value(role), description);
 
   pc->b_local_description_created = 1;
 
@@ -444,45 +404,30 @@ int peer_connection_loop(PeerConnection* pc) {
   return 0;
 }
 
-void peer_connection_set_remote_description(PeerConnection* pc, const char* sdp_text) {
-  char* start = (char*)sdp_text;
-  char* line = NULL;
-  char buf[256];
-  char* val_start = NULL;
-  uint32_t* ssrc = NULL;
-  DtlsSrtpRole role = DTLS_SRTP_ROLE_SERVER;
-
-  while ((line = strstr(start, "\r\n"))) {
-    line = strstr(start, "\r\n");
-    strncpy(buf, start, line - start);
-    buf[line - start] = '\0';
-
-    if (strstr(buf, "a=setup:passive")) {
-      pc->agent.mode = AGENT_MODE_CONTROLLING;
-      role = DTLS_SRTP_ROLE_CLIENT;
-    }
+void peer_connection_parse_remote_description(PeerConnection *pc, const char *sdp_text) {
+  int role;
 
-    if (strstr(buf, "a=fingerprint")) {
-      strncpy(pc->dtls_srtp.remote_fingerprint, buf + 22, DTLS_SRTP_FINGERPRINT_LENGTH);
-    }
+  sdp_parse(&pc->agent, &pc->dtls_srtp, &pc->bundle_info, &role, sdp_text);
+  pc->bundle_info.parsed = 1;
+}
 
-    if (strstr(buf, "a=mid:video")) {
-      ssrc = &pc->remote_vssrc;
-    } else if (strstr(buf, "a=mid:audio")) {
-      ssrc = &pc->remote_assrc;
-    }
+void peer_connection_set_remote_description(PeerConnection *pc, const char *sdp_text) {
 
-    if ((val_start = strstr(buf, "a=ssrc:")) && ssrc) {
-      *ssrc = strtoul(val_start + 7, NULL, 10);
-      LOGD("SSRC: %" PRIu32, *ssrc);
-    }
+  if (pc->bundle_info.parsed == 0)
+    peer_connection_parse_remote_description(pc, sdp_text);
 
-    start = line + 2;
+  for (int i = 0 ; i < pc->bundle_info.count ; i++) {
+    if (strcmp(pc->bundle_info.info[i].mid_type, "audio") == 0)
+      pc->remote_assrc = pc->bundle_info.info[i].ssrc;
+    else if (strcmp(pc->bundle_info.info[i].mid_type, "video") == 0)
+      pc->remote_vssrc = pc->bundle_info.info[i].ssrc;
   }
 
-  if (!pc->b_local_description_created) {
-    peer_connection_state_new(pc, role);
-  }
+// TODO
+//  if (!pc->b_local_description_created) {
+ //   peer_connection_state_new(pc, role);
+//  }
+//
 
   agent_set_remote_description(&pc->agent, (char*)sdp_text);
   STATE_CHANGED(pc, PEER_CONNECTION_CHECKING);
@@ -554,3 +499,4 @@ char* peer_connection_lookup_sid_label(PeerConnection* pc, uint16_t sid) {
   }
   return NULL;  // Not found
 }
+
diff --git a/src/peer_connection.h b/src/peer_connection.h
@@ -67,6 +67,8 @@ typedef struct PeerConfiguration {
   void (*on_request_keyframe)(void* userdata);
   void* user_data;
 
+  uint32_t audio_ssrc;
+  uint32_t video_ssrc;
 } PeerConfiguration;
 
 typedef struct PeerConnection PeerConnection;
@@ -98,6 +100,8 @@ int peer_connection_send_audio(PeerConnection* pc, const uint8_t* packet, size_t
 
 int peer_connection_send_video(PeerConnection* pc, const uint8_t* packet, size_t bytes);
 
+void peer_connection_parse_remote_description(PeerConnection* pc, const char* sdp);
+
 void peer_connection_set_remote_description(PeerConnection* pc, const char* sdp);
 
 void peer_connection_create_offer(PeerConnection* pc);

diff --git a/src/rtp.c b/src/rtp.c
@@ -179,7 +179,7 @@ static int rtp_encoder_encode_generic(RtpEncoder* rtp_encoder, uint8_t* buf, siz
   return 0;
 }
 
-void rtp_encoder_init(RtpEncoder* rtp_encoder, MediaCodec codec, RtpOnPacket on_packet, void* user_data) {
+void rtp_encoder_init(RtpEncoder* rtp_encoder, MediaCodec codec, uint32_t ssrc, RtpOnPacket on_packet, void* user_data) {
   rtp_encoder->on_packet = on_packet;
   rtp_encoder->user_data = user_data;
   rtp_encoder->timestamp = 0;
@@ -188,25 +188,25 @@ void rtp_encoder_init(RtpEncoder* rtp_encoder, MediaCodec codec, RtpOnPacket on_
   switch (codec) {
     case CODEC_H264:
       rtp_encoder->type = PT_H264;
-      rtp_encoder->ssrc = SSRC_H264;
+      rtp_encoder->ssrc = ssrc;
       rtp_encoder->timestamp_increment = 90000 / 30;  // 30 FPS.
       rtp_encoder->encode_func = rtp_encoder_encode_h264;
       break;
     case CODEC_PCMA:
       rtp_encoder->type = PT_PCMA;
-      rtp_encoder->ssrc = SSRC_PCMA;
+      rtp_encoder->ssrc = ssrc;
       rtp_encoder->timestamp_increment = AUDIO_LATENCY * 8000 / 1000;
       rtp_encoder->encode_func = rtp_encoder_encode_generic;
       break;
     case CODEC_PCMU:
       rtp_encoder->type = PT_PCMU;
-      rtp_encoder->ssrc = SSRC_PCMU;
+      rtp_encoder->ssrc = ssrc;
       rtp_encoder->timestamp_increment = AUDIO_LATENCY * 8000 / 1000;
       rtp_encoder->encode_func = rtp_encoder_encode_generic;
       break;
     case CODEC_OPUS:
       rtp_encoder->type = PT_OPUS;
-      rtp_encoder->ssrc = SSRC_OPUS;
+      rtp_encoder->ssrc = ssrc;
       rtp_encoder->timestamp_increment = AUDIO_LATENCY * 48000 / 1000;
       rtp_encoder->encode_func = rtp_encoder_encode_generic;
       break;

diff --git a/src/rtp.h b/src/rtp.h
@@ -97,9 +97,9 @@ struct RtpEncoder {
 
 int rtp_packet_validate(uint8_t* packet, size_t size);
 
-void rtp_encoder_init(RtpEncoder* rtp_encoder, MediaCodec codec, RtpOnPacket on_packet, void* user_data);
+void rtp_encoder_init(RtpEncoder *rtp_encoder, MediaCodec codec, uint32_t ssrc, RtpOnPacket on_packet, void *user_data);
 
-int rtp_encoder_encode(RtpEncoder* rtp_encoder, uint8_t* data, size_t size);
+int rtp_encoder_encode(RtpEncoder *rtp_encoder, uint8_t *data, size_t size);
 
 void rtp_decoder_init(RtpDecoder* rtp_decoder, MediaCodec codec, RtpOnPacket on_packet, void* user_data);
 

diff --git a/src/sdp.c b/src/sdp.c
@@ -1,7 +1,152 @@
-#include <stdarg.h>
 #include <stdio.h>
+#include <stdarg.h>
 
 #include "sdp.h"
+#include "rtp.h"
+#include "dtls_srtp.h"
+#include "peer_connection.h"
+#include "utils.h"
+#include "agent.h"
+
+void sdp_parse_bundle_line(const char* line, SdpBundleInfo *bundle_info) {
+  const char* token;
+  char line_copy[MAX_SDP_LINE_LENGTH];  // Buffer to make a copy of the input line
+
+  bundle_info->count = 0;  // Initialize the count of bundle IDs
+
+  // Make a copy of the input line to tokenize
+  strncpy(line_copy, line, sizeof(line_copy) - 1);
+  line_copy[sizeof(line_copy) - 1] = '\0';  // Ensure null-terminated
+
+  // Skip the prefix
+  token = strtok(line_copy, " ");
+
+  // Parse each token (media ID) and store it as an integer
+  while ((token != NULL) && (bundle_info->count < MAX_BUNDLE_IDS) && (strlen(token) < MAX_BUNDLE_ID_LENGTH)) {
+    strcpy(bundle_info->info[bundle_info->count].mid_name, token);
+    bundle_info->info[bundle_info->count].mid_type[0] = 0;
+    bundle_info->info[bundle_info->count].ssrc = 0;
+    bundle_info->count++;
+    token = strtok(NULL, " ");
+  }
+}
+
+void sdp_set_bundle_info(SdpBundleInfo *bundle_info, char mid_name[MAX_BUNDLE_ID_LENGTH], char mid_type[MAX_BUNDLE_ID_NAME_LENGTH], uint32_t ssrc_id) {
+  LOGD("Adding %s,%s,%lu\n", mid_name, mid_type, ssrc_id);
+  for (int i = 0 ; i < bundle_info->count ; i++) {
+    if (strcmp(bundle_info->info[i].mid_name, mid_name) == 0) {
+      strcpy(bundle_info->info[i].mid_type, mid_type);
+      bundle_info->info[i].ssrc = ssrc_id;
+      LOGD("Added[%d] %s,%s,%lu\n", i, mid_name, mid_type, ssrc_id);
+      break;
+    }
+  }
+}
+
+void sdp_parse(struct Agent* agent, struct DtlsSrtp* dtls_srtp, SdpBundleInfo* bundle_info, int* role, const char* sdp_text) {
+  char *start = (char*)sdp_text;
+  char *line = NULL;
+  char buf[MAX_SDP_LINE_LENGTH], mid_type[MAX_BUNDLE_ID_NAME_LENGTH], mid_name[MAX_BUNDLE_ID_LENGTH];
+  size_t len;
+  char *end;
+  int has_info = 0;
+  uint32_t ssrc_id = 0;
+
+  (*role) = DTLS_SRTP_ROLE_SERVER;
+  mid_type[0] = mid_name[0] = 0;
+  bundle_info->count = 0;
+  while ((line = strstr(start, "\r\n")) != NULL) {
+    len = line - start;
+    if (len < MAX_SDP_LINE_LENGTH) {
+      strncpy(buf, start, len);
+      buf[len] = '\0';
+
+      if (strstr(buf, "a=group:BUNDLE") != NULL)
+        sdp_parse_bundle_line(&buf[14], bundle_info);
+
+      if (strstr(buf, "m=")) {
+        if (has_info)
+          sdp_set_bundle_info(bundle_info, mid_name, mid_type, ssrc_id);
+
+        has_info = 0;
+        if ((end = strstr(&buf[2], " ")) != NULL) {
+          len = end - &buf[2];
+          if (len < MAX_BUNDLE_ID_NAME_LENGTH) {
+            strncpy(mid_type, &buf[2], len);
+            mid_type[len] = '\0';
+            LOGD("m= '%s'\n", mid_type);
+            mid_name[0] = 0;
+            ssrc_id = 0;
+            has_info = 1;
+          }
+        }
+      }
+
+      if ((strstr(buf, "a=mid:") != NULL) && (mid_type[0] != 0)) {
+        if (strlen(&buf[6]) < MAX_BUNDLE_ID_LENGTH)
+          strcpy(mid_name, &buf[6]);
+        LOGD("a=mid: '%s'\n", mid_name);
+      }
+
+      if (strstr(buf, "a=ssrc:") != NULL) {
+        ssrc_id = strtoul(&buf[7], NULL, 10);
+        LOGD("a=ssrc: '%s' = %lu\n", &buf[7], ssrc_id);
+      }
+
+      if (strstr(buf, "a=setup:passive")) {
+        agent->mode = AGENT_MODE_CONTROLLING;
+        (*role) = DTLS_SRTP_ROLE_CLIENT;
+      }
+
+      if (strstr(buf, "a=fingerprint")) {
+        strncpy(dtls_srtp->remote_fingerprint, buf + 22, DTLS_SRTP_FINGERPRINT_LENGTH);
+      }
+    }
+    start = line + 2;
+  }
+
+  if (has_info)
+    sdp_set_bundle_info(bundle_info, mid_name, mid_type, ssrc_id);
+
+  LOGI("Detected %d bundle entries", bundle_info->count);
+  for (int i = 0 ; i < bundle_info->count ; i++)
+    LOGI("%d] type='%s' name='%s' ssrc=%lu", i, bundle_info->info[i].mid_type, bundle_info->info[i].mid_name, (unsigned long) bundle_info->info[i].ssrc);
+}
+
+void sdp_append_bundle(Sdp *sdp, struct PeerConfiguration *config, SdpBundleInfo *bundle_info) {
+
+  char bundle[16 + ((MAX_BUNDLE_ID_LENGTH + 1) * MAX_BUNDLE_IDS)];
+  char *name = NULL;
+
+  sdp_append(sdp, "v=0");
+  sdp_append(sdp, "o=- 1495799811084970 1495799811084970 IN IP4 0.0.0.0");
+  sdp_append(sdp, "s=-");
+  sdp_append(sdp, "t=0 0");
+#if ICE_LITE
+  sdp_append(sdp, "a=ice-lite");
+#endif
+
+  memset(bundle, 0, sizeof(bundle));
+
+  strcat(bundle, "a=group:BUNDLE");
+  for (int i = 0; i < bundle_info->count ; i++) {
+    if ((strcmp(bundle_info->info[i].mid_type, "audio") == 0) && (config->audio_codec != CODEC_NONE))
+      name = bundle_info->info[i].mid_name;
+    else if ((strcmp(bundle_info->info[i].mid_type, "video") == 0) && (config->video_codec != CODEC_NONE))
+      name = bundle_info->info[i].mid_name;
+    else if ((strcmp(bundle_info->info[i].mid_type, "application") == 0) && config->datachannel)
+      name = bundle_info->info[i].mid_name;
+
+    if (name != NULL) {
+      strcat(bundle, " ");
+      strcat(bundle, name);
+      name = NULL;
+    }
+  }
+
+  sdp_append(sdp, bundle);
+  sdp_append(sdp, "a=msid-semantic: WMS myStream");
+}
 
 int sdp_append(Sdp* sdp, const char* format, ...) {
   va_list argptr;
@@ -25,84 +170,109 @@ void sdp_reset(Sdp* sdp) {
   memset(sdp->content, 0, sizeof(sdp->content));
 }
 
-void sdp_append_h264(Sdp* sdp) {
+void sdp_append_h264(Sdp* sdp, const char* mid, uint32_t ssrc) {
+
   sdp_append(sdp, "m=video 9 UDP/TLS/RTP/SAVPF 96 102");
   sdp_append(sdp, "a=rtcp-fb:102 nack");
   sdp_append(sdp, "a=rtcp-fb:102 nack pli");
   sdp_append(sdp, "a=fmtp:96 profile-level-id=42e01f;level-asymmetry-allowed=1");
   sdp_append(sdp, "a=fmtp:102 profile-level-id=42e01f;packetization-mode=1;level-asymmetry-allowed=1");
   sdp_append(sdp, "a=rtpmap:96 H264/90000");
   sdp_append(sdp, "a=rtpmap:102 H264/90000");
-  sdp_append(sdp, "a=ssrc:1 cname:webrtc-h264");
+  sdp_append(sdp, "a=ssrc:%lu cname:webrtc-h264", ssrc);
   sdp_append(sdp, "a=sendrecv");
-  sdp_append(sdp, "a=mid:video");
+  sdp_append(sdp, "a=mid:%s", mid);
   sdp_append(sdp, "c=IN IP4 0.0.0.0");
   sdp_append(sdp, "a=rtcp-mux");
 }
 
-void sdp_append_pcma(Sdp* sdp) {
+void sdp_append_pcma(Sdp* sdp, const char* mid, uint32_t ssrc) {
+
   sdp_append(sdp, "m=audio 9 UDP/TLS/RTP/SAVP 8");
   sdp_append(sdp, "a=rtpmap:8 PCMA/8000");
-  sdp_append(sdp, "a=ssrc:4 cname:webrtc-pcma");
+  sdp_append(sdp, "a=ssrc:%lu cname:webrtc-pcma", ssrc);
   sdp_append(sdp, "a=sendrecv");
-  sdp_append(sdp, "a=mid:audio");
+  sdp_append(sdp, "a=mid:%s", mid);
   sdp_append(sdp, "c=IN IP4 0.0.0.0");
   sdp_append(sdp, "a=rtcp-mux");
 }
 
-void sdp_append_pcmu(Sdp* sdp) {
-  sdp_append(sdp, "m=audio 9 UDP/TLS/RTP/SAVP 0");
+void sdp_append_pcmu(Sdp* sdp, const char* mid, uint32_t ssrc) {
+
+  sdp_append(sdp, "m=audio 9 UDP/TLS/RTP/SAVPF 0");
   sdp_append(sdp, "a=rtpmap:0 PCMU/8000");
-  sdp_append(sdp, "a=ssrc:5 cname:webrtc-pcmu");
+  sdp_append(sdp, "a=ssrc:%lu cname:webrtc-pcmu", ssrc);
   sdp_append(sdp, "a=sendrecv");
-  sdp_append(sdp, "a=mid:audio");
+  sdp_append(sdp, "a=mid:%s", mid);
   sdp_append(sdp, "c=IN IP4 0.0.0.0");
   sdp_append(sdp, "a=rtcp-mux");
 }
 
-void sdp_append_opus(Sdp* sdp) {
+void sdp_append_opus(Sdp* sdp, const char* mid, uint32_t ssrc) {
+
   sdp_append(sdp, "m=audio 9 UDP/TLS/RTP/SAVP 111");
   sdp_append(sdp, "a=rtpmap:111 opus/48000/2");
-  sdp_append(sdp, "a=ssrc:6 cname:webrtc-opus");
+  sdp_append(sdp, "a=ssrc:%lu cname:webrtc-opus", ssrc);
   sdp_append(sdp, "a=sendrecv");
-  sdp_append(sdp, "a=mid:audio");
+  sdp_append(sdp, "a=mid:%s", mid);
   sdp_append(sdp, "c=IN IP4 0.0.0.0");
   sdp_append(sdp, "a=rtcp-mux");
 }
 
-void sdp_append_datachannel(Sdp* sdp) {
+void sdp_append_datachannel(Sdp* sdp, const char* mid) {
+
   sdp_append(sdp, "m=application 50712 UDP/DTLS/SCTP webrtc-datachannel");
-  sdp_append(sdp, "a=mid:datachannel");
+  sdp_append(sdp, "a=mid:%s", mid);
   sdp_append(sdp, "a=sctp-port:5000");
   sdp_append(sdp, "c=IN IP4 0.0.0.0");
   sdp_append(sdp, "a=max-message-size:262144");
 }
 
-void sdp_create(Sdp* sdp, int b_video, int b_audio, int b_datachannel) {
-  char bundle[64];
-  sdp_append(sdp, "v=0");
-  sdp_append(sdp, "o=- 1495799811084970 1495799811084970 IN IP4 0.0.0.0");
-  sdp_append(sdp, "s=-");
-  sdp_append(sdp, "t=0 0");
-  sdp_append(sdp, "a=msid-semantic: iot");
-#if ICE_LITE
-  sdp_append(sdp, "a=ice-lite");
-#endif
-  memset(bundle, 0, sizeof(bundle));
 
-  strcat(bundle, "a=group:BUNDLE");
+void sdp_create(Sdp *local_sdp, struct PeerConfiguration *config, SdpBundleInfo *bundle_info, DtlsSrtp *dtls_srtp, const char* role, char description[CONFIG_MTU]) {
+  memset(local_sdp, 0, sizeof(*local_sdp));
+  // TODO: check if we have video or audio codecs
+  sdp_append_bundle(local_sdp, config, bundle_info);
 
-  if (b_video) {
-    strcat(bundle, " video");
-  }
+  for (int i = 0; i < bundle_info->count ; i++) {
+    if ((strcmp(bundle_info->info[i].mid_type, "audio") == 0) && (config->audio_codec != CODEC_NONE)) {
+      switch (config->audio_codec) {
+        case CODEC_PCMA:
+          sdp_append_pcma(local_sdp, bundle_info->info[i].mid_name, config->audio_ssrc);
+          sdp_append(local_sdp, "a=fingerprint:sha-256 %s", dtls_srtp->local_fingerprint);
+          sdp_append(local_sdp, role);
+          strcat(local_sdp->content, description);
+          break;
 
-  if (b_audio) {
-    strcat(bundle, " audio");
-  }
+        case CODEC_PCMU:
+          sdp_append_pcmu(local_sdp, bundle_info->info[i].mid_name, config->audio_ssrc);
+          sdp_append(local_sdp, "a=fingerprint:sha-256 %s", dtls_srtp->local_fingerprint);
+          sdp_append(local_sdp, role);
+          strcat(local_sdp->content, description);
+          break;
 
-  if (b_datachannel) {
-    strcat(bundle, " datachannel");
-  }
+        case CODEC_OPUS:
+          sdp_append_opus(local_sdp, bundle_info->info[i].mid_name, config->audio_ssrc);
+          sdp_append(local_sdp, "a=fingerprint:sha-256 %s", dtls_srtp->local_fingerprint);
+          sdp_append(local_sdp, role);
+          strcat(local_sdp->content, description);
+          break;
 
-  sdp_append(sdp, bundle);
+        default:
+          break;
+      }
+    } else if ((strcmp(bundle_info->info[i].mid_type, "video") == 0) && (config->video_codec == CODEC_H264)) {
+      sdp_append_h264(local_sdp, bundle_info->info[i].mid_name, config->video_ssrc);
+      sdp_append(local_sdp, "a=fingerprint:sha-256 %s", dtls_srtp->local_fingerprint);
+      sdp_append(local_sdp, role);
+      strcat(local_sdp->content, description);
+    }  else if ((strcmp(bundle_info->info[i].mid_type, "application") == 0) && config->datachannel) {
+      sdp_append_datachannel(local_sdp, bundle_info->info[i].mid_name);
+      sdp_append(local_sdp, "a=fingerprint:sha-256 %s", dtls_srtp->local_fingerprint);
+      sdp_append(local_sdp, role);
+      strcat(local_sdp->content, description);
+    }
+  }
+  sdp_append(local_sdp, "a=end-of-candidates");
 }
+
diff --git a/src/sdp.h b/src/sdp.h
@@ -2,6 +2,8 @@
 #define SDP_H_
 
 #include <string.h>
+#include <stdint.h>
+#include "config.h"
 
 #define SDP_CONTENT_LENGTH 10240
 #define SDP_ATTR_LENGTH 128
@@ -15,20 +17,48 @@ typedef struct Sdp {
 
 } Sdp;
 
-void sdp_append_h264(Sdp* sdp);
+#define MAX_SDP_LINE_LENGTH  256
+#define MAX_BUNDLE_IDS 4              // Maximum number of bundle IDs to store
+#define MAX_BUNDLE_ID_LENGTH 12        // Maximum length of bundle ID
+#define MAX_BUNDLE_ID_NAME_LENGTH 12  // Maximum number of bundle ID name
 
-void sdp_append_pcma(Sdp* sdp);
+typedef struct SdpBundleId {
 
-void sdp_append_pcmu(Sdp* sdp);
+  uint32_t ssrc;
+  char mid_name[MAX_BUNDLE_ID_LENGTH];
+  char mid_type[MAX_BUNDLE_ID_NAME_LENGTH];
 
-void sdp_append_opus(Sdp* sdp);
+} SdpBundleId;
 
-void sdp_append_datachannel(Sdp* sdp);
+typedef struct SdpBundleInfo {
 
-void sdp_create(Sdp* sdp, int b_video, int b_audio, int b_datachannel);
+  int parsed;
+  int count;
+  SdpBundleId info[MAX_BUNDLE_IDS];
+
+} SdpBundleInfo;
+
+struct PeerConfiguration;
+struct PeerConnection;
+struct DtlsSrtp;
+struct Agent;
+
+void sdp_append_h264(Sdp* sdp, const char* mid, uint32_t ssrc);
+
+void sdp_append_pcma(Sdp* sdp, const char* mid, uint32_t ssrc);
+
+void sdp_append_pcmu(Sdp* sdp, const char* mid, uint32_t ssrc);
+
+void sdp_append_opus(Sdp* sdp, const char* mid, uint32_t ssrc);
+
+void sdp_append_datachannel(Sdp* sdp, const char* mid);
+
+void sdp_create(Sdp *sdp, struct PeerConfiguration *config, SdpBundleInfo *bundle_info, struct DtlsSrtp *dtls_srtp, const char* role, char description[CONFIG_MTU]);
 
 int sdp_append(Sdp* sdp, const char* format, ...);
 
 void sdp_reset(Sdp* sdp);
 
-#endif  // SDP_H_
+void sdp_parse(struct Agent* agent, struct DtlsSrtp* dtls_srtp, SdpBundleInfo* bundle_info, int* role, const char* sdp_text);
+
+#endif // SDP_H_
diff --git a/src/stun.c b/src/stun.c
@@ -58,43 +58,108 @@ void stun_msg_create(StunMessage* msg, uint16_t type) {
   msg->size = sizeof(StunHeader);
 }
 
-void stun_set_mapped_address(char* value, uint8_t* mask, Address* addr) {
-  uint8_t* family = (uint8_t*)(value + 1);
-  uint16_t* port = (uint16_t*)(value + 2);
-  uint8_t* ipv4 = (uint8_t*)(value + 4);
+void stun_setup_xor_address_mask(uint8_t mask[16], uint32_t transaction_id[3]) {
+  // The magic cookie in the mask has to be stored in network order
+  uint32_t cookie = htonl(MAGIC_COOKIE);
 
-  *family = 0x01;
-  *port = htons(addr->port);
+  memcpy(&mask[0], &cookie, 4);
+  memcpy(&mask[4], transaction_id, 12);
 
-  memcpy(ipv4, &addr->sin.sin_addr, 4);
+  LOGD("MASK = %02x%02x%02x%02x %02x%02x%02x%02x %02x%02x%02x%02x %02x%02x%02x%02x", mask[0], mask[1], mask[2], mask[3], mask[4], mask[5], mask[6], mask[7],
+       mask[8], mask[9], mask[10], mask[11], mask[12], mask[13], mask[14], mask[15]);
+}
+
+void xor_u8(const uint8_t *data, int count, const uint8_t *mask, uint8_t *result) {
+  for (int i = 0; i < count; i++)
+    result[i] = data[i] ^ mask[i];
+}
+
+void xor_u16(const uint16_t *data, int count, const uint8_t *mask, uint8_t *result) {
+  for (int i = 0; i < count; i++) {
+    result[i * 2] = ((data[i] >> 8) & 0xFF) ^ mask[i * 2];
+    result[(i * 2) + 1] = (data[i] & 0xFF) ^ mask[(i * 2) + 1];
+  }
+}
 
-  // LOGD("XOR Mapped Address Family: 0x%02x", *family);
-  // LOGD("XOR Mapped Address Port: %d", *port);
-  // LOGD("XOR Mapped Address Address: %d.%d.%d.%d", ipv4[0], ipv4[1], ipv4[2], ipv4[3]);
+void xor_u32(const uint32_t *data, int count, const uint8_t *mask, uint8_t *result) {
+  for (int i = 0; i < count; i++) {
+    result[i * 2] = ((data[i] >> 24) & 0xFF) ^ mask[i * 2];
+    result[(i * 2) + 1] = ((data[i] >> 16) & 0xFF) ^ mask[(i * 2) + 1];
+    result[(i * 2) + 2] = ((data[i] >> 8) & 0xFF) ^ mask[(i * 2) + 2];
+    result[(i * 2) + 3] = (data[i] & 0xFF) ^ mask[(i * 2) + 3];
+  }
 }
 
-void stun_get_mapped_address(char* value, uint8_t* mask, Address* addr) {
-  int i;
+void stun_set_mapped_address(char *value, uint8_t *mask, Address *addr) {
+
+  uint8_t *value_data = (uint8_t *) value;
+  uint16_t *value_port = (uint16_t *) &value[2];
+  uint8_t *value_ip = (uint8_t *) &value[4];
+  uint8_t *value_ipv4 = (uint8_t *) value_ip;
+  uint16_t *value_ipv6 = (uint16_t *) value_ip;
+  uint16_t port = htons(addr->port);
   char addr_string[ADDRSTRLEN];
-  uint32_t* addr32 = (uint32_t*)&addr->sin.sin_addr;
-  uint16_t* addr16 = (uint16_t*)&addr->sin6.sin6_addr;
-  uint8_t family = value[1];
-  if (family == 0x02) {
-    addr_set_family(addr, AF_INET6);
-    for (i = 0; i < 8; i++) {
-      addr16[i] = (*(uint16_t*)(value + 4 + 2 * i) ^ *(uint16_t*)(mask + 2 * i));
-    }
-  } else if (family == 0x01) {
-    addr_set_family(addr, AF_INET);
-    *addr32 = (*(uint32_t*)(value + 4) ^ *(uint32_t*)mask);
+
+/*
+	addr->ipv4[0] = 109;
+	addr->ipv4[1] = 205;
+	addr->ipv4[2] = 6;
+	addr->ipv4[3] = 205;
+	addr->port = 0xc51a;
+	port = htons(addr->port);*/
+
+  LOGD("MASK = %02x%02x%02x%02x %02x%02x%02x%02x %02x%02x%02x%02x %02x%02x%02x%02x", mask[0], mask[1], mask[2], mask[3], mask[4], mask[5], mask[6], mask[7], mask[8], mask[9],
+       mask[10], mask[11], mask[12], mask[13], mask[14], mask[15]);
+
+  value_data[0] = 0;
+  value_data[1] = (addr->family == AF_INET) ? 0x01 : 0x02;
+  xor_u16(&port, 1, mask, (uint8_t *) &value[2]);
+  LOGD("XOR2= %02x%02x (%02x)", value_data[2], value_data[3], port);
+
+  if (addr->family == AF_INET) {
+    xor_u32(&addr->sin.sin_addr.s_addr, 1, mask, value_ip);
+    addr_to_string(addr, addr_string, sizeof(addr_string));
+    LOGD("      STUN Address IPV4: %s:%u", addr_string, (unsigned) addr->port);
+    LOGD("STUN Address XORed IPV4: %u.%u.%u.%u:%u", (unsigned) value_ipv4[0], (unsigned) value_ipv4[1], (unsigned) value_ipv4[2], (unsigned) value_ipv4[3], (unsigned) value_port[0]);
+  } else {
+    xor_u16(addr->sin6.sin6_addr.__in6_u.__u6_addr16, 8, mask, value_ip);
+    addr_to_string(addr, addr_string, sizeof(addr_string));
+    LOGD("      STUN Address IPV6: %s %u", addr_string, (unsigned) addr->port);
+    LOGD("STUN Address XORed IPV6: %x:%x:%x:%x:%x:%x:%x:%x %u", (unsigned) value_ipv6[0], (unsigned) value_ipv6[1], (unsigned) value_ipv6[2], (unsigned) value_ipv6[3], (unsigned) value_ipv6[4], (unsigned) value_ipv6[5], (unsigned) value_ipv6[6], (unsigned) value_ipv6[7], (unsigned) value_port[0]);
   }
+}
 
-  addr_to_string(addr, addr_string, sizeof(addr_string));
-  addr_set_port(addr, ntohs(*(uint16_t*)(value + 2) ^ *(uint16_t*)mask));
+void stun_get_mapped_address(char *value, uint8_t *mask, Address *addr) {
+  uint8_t *value_data = (uint8_t *) value;
+  uint16_t *value_port = (uint16_t *) &value[2];
+  uint8_t *value_ip = (uint8_t *) &value[4];
+  uint32_t *value_ipv4 = (uint32_t *) value_ip;
+  uint16_t *value_ipv6 = (uint16_t *) value_ip;
+  char addr_string[ADDRSTRLEN];
 
+  addr_set_family(addr, (value_data[1] == 0x01) ? AF_INET : AF_INET6);
+  // To deobfuscate the port, XOR the port number with the most significant 16 bits of the magic cookie (0x2112).
+//	addr->port = ntohs(value_port[0] ^ (* ((uint16_t*) mask)));
+  xor_u16(value_port, 1, mask, (uint8_t *) &addr->port);
+  //addr->port = ntohs(addr->port);
+  addr_set_port(addr, ntohs(addr->port));
+  //addr->port = value_port[0] ^ (* ((uint16_t*) mask));
+  LOGD("family: %d, %d", AF_INET, AF_INET6);
   LOGD("XOR Mapped Address Family: 0x%02x", addr->family);
   LOGD("XOR Mapped Address Port: %d", addr->port);
-  LOGD("XOR Mapped Address Address: %s", addr_string);
+
+  if (addr->family == AF_INET6) {
+    xor_u16(value_ipv6, 8, mask, (uint8_t *) addr->sin6.sin6_addr.__in6_u.__u6_addr16);
+    addr_to_string(addr, addr_string, sizeof(addr_string));
+    LOGD("STUN Get Address XORed IPV6: %x:%x:%x:%x:%x:%x:%x:%x %u", (unsigned) value_ipv6[0], (unsigned) value_ipv6[1], (unsigned) value_ipv6[2], (unsigned) value_ipv6[3], (unsigned) value_ipv6[4], (unsigned) value_ipv6[5], (unsigned) value_ipv6[6], (unsigned) value_ipv6[7], (unsigned) value_port[0]);
+    LOGD("      Get STUN Address IPV6: %x:%x:%x:%x:%x:%x:%x:%x %u", addr_string, (unsigned) addr->port);
+  } else if (addr->family == AF_INET) {
+    // To deobfuscate the IP address, XOR the IPv4 address (in binary format) with the first 32 bits of the transaction ID.
+    xor_u32(value_ipv4, 1, mask, (uint8_t *) &addr->sin.sin_addr.s_addr);
+    addr_to_string(addr, addr_string, sizeof(addr_string));
+    LOGD("STUN XORed Address IPV4: %u.%u.%u.%u:%u", (unsigned) value_ip[3], (unsigned) value_ip[2], (unsigned) value_ip[1], (unsigned) value_ip[0], (unsigned) value_port[0]);
+    LOGD("STUN Clear Address IPV4: %s:%u", addr_string, (unsigned) addr->port);
+  }
 }
 
 void stun_parse_msg_buf(StunMessage* msg) {
@@ -126,12 +191,13 @@ void stun_parse_msg_buf(StunMessage* msg) {
 
   while (pos < length) {
     StunAttribute* attr = (StunAttribute*)(msg->buf + pos);
-    memset(mask, 0, sizeof(mask));
+
     // LOGD("Attribute Type: 0x%04x", ntohs(attr->type));
     // LOGD("Attribute Length: %d", ntohs(attr->length));
 
     switch (ntohs(attr->type)) {
       case STUN_ATTR_TYPE_MAPPED_ADDRESS:
+				memset(mask, 0, sizeof(mask));
         stun_get_mapped_address(attr->value, mask, &msg->mapped_addr);
         break;
       case STUN_ATTR_TYPE_USERNAME:
@@ -162,14 +228,12 @@ void stun_parse_msg_buf(StunMessage* msg) {
         LOGD("Nonce %s", msg->nonce);
         break;
       case STUN_ATTR_TYPE_XOR_RELAYED_ADDRESS:
-        *((uint32_t*)mask) = htonl(MAGIC_COOKIE);
-        memcpy(mask + 4, header->transaction_id, sizeof(header->transaction_id));
+				stun_setup_xor_address_mask(mask, header->transaction_id);
         LOGD("XOR Relayed Address");
         stun_get_mapped_address(attr->value, mask, &msg->relayed_addr);
         break;
       case STUN_ATTR_TYPE_XOR_MAPPED_ADDRESS:
-        *((uint32_t*)mask) = htonl(MAGIC_COOKIE);
-        memcpy(mask + 4, header->transaction_id, sizeof(header->transaction_id));
+				stun_setup_xor_address_mask(mask, header->transaction_id);
         stun_get_mapped_address(attr->value, mask, &msg->mapped_addr);
         break;
       case STUN_ATTR_TYPE_PRIORITY:
@@ -207,6 +271,7 @@ void stun_parse_binding_response(char* buf, size_t len, Address* addr) {
     // LOGD("Attribute Length: %d", ntohs(attr->length));
 
     if (ntohs(attr->type) == STUN_ATTR_TYPE_MAPPED_ADDRESS) {
+			memset(mask, 0, 16);
       stun_get_mapped_address(attr->value, mask, addr);
 
     } else if (ntohs(attr->type) == STUN_ATTR_TYPE_XOR_MAPPED_ADDRESS) {

diff --git a/src/stun.h b/src/stun.h
@@ -89,6 +89,8 @@ struct StunMessage {
   size_t size;
 };
 
+void stun_setup_xor_address_mask(uint8_t mask[16], uint32_t transaction_id[3]);
+
 void stun_msg_create(StunMessage* msg, uint16_t type);
 
 void stun_set_mapped_address(char* value, uint8_t* mask, Address* addr);

diff --git a/src/utils.h b/src/utils.h
@@ -1,9 +1,9 @@
 #ifndef UTILS_H_
 #define UTILS_H_
 
-#include <stdint.h>
 #include <stdio.h>
 #include <stdlib.h>
+#include <stdint.h>
 #include "config.h"
 
 #define LEVEL_ERROR 0x00
@@ -21,12 +21,12 @@
 #endif
 
 #if LOG_REDIRECT
-void peer_log(char* level_tag, const char* file_name, int line_number, const char* fmt, ...);
+void peer_log(char *level_tag, const char *file_name, int line_number, const char *fmt, ...);
 #define LOG_PRINT(level_tag, fmt, ...) \
   peer_log(level_tag, __FILE__, __LINE__, fmt, ##__VA_ARGS__)
 #else
 #define LOG_PRINT(level_tag, fmt, ...) \
-  fprintf(stdout, "%s\t%s\t%d\t" fmt "\n", level_tag, __FILE__, __LINE__, ##__VA_ARGS__)
+  fprintf(stdout, "%s\t%s\t%d\t" fmt"\n", level_tag, __FILE__, __LINE__, ##__VA_ARGS__)
 #endif
 
 #if LOG_LEVEL >= LEVEL_DEBUG
Original file line number	Diff line number	Diff line change
		@@ -1 +1,2 @@
		build
		builders/cmake/_anyka_v500_uclibc_chacam-release-static