From 46982b0cabf3925f905bd739015aae91cc5f0ffa Mon Sep 17 00:00:00 2001
From: Milanka Ringwald <mila@ringwald.ch>
Date: Mon, 18 Nov 2019 11:25:04 +0100
Subject: [PATCH] avdtp_source: add avdtp_source_stream_send_media_payload to
 api [a2367]

---
 src/classic/avdtp_source.c | 73 ++++++++++++++++++++++++++++++++++++++
 src/classic/avdtp_source.h |  5 +--
 2 files changed, 76 insertions(+), 2 deletions(-)

diff --git a/src/classic/avdtp_source.c b/src/classic/avdtp_source.c
index 60889d45a..60b930e41 100644
--- a/src/classic/avdtp_source.c
+++ b/src/classic/avdtp_source.c
@@ -53,6 +53,7 @@
 #include "classic/avdtp_source.h"
 
 static avdtp_context_t * avdtp_source_context;
+#define AVDTP_MEDIA_PAYLOAD_HEADER_SIZE 12
 
 static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size);
 
@@ -178,3 +179,75 @@ void avdtp_source_init(avdtp_context_t * avdtp_context){
     l2cap_register_service(&packet_handler, BLUETOOTH_PSM_AVDTP, 0xffff, LEVEL_2);
 }
 
+
+static void avdtp_source_setup_media_header(uint8_t * media_packet, int size, int *offset, uint8_t marker, uint16_t sequence_number){
+    if (size < AVDTP_MEDIA_PAYLOAD_HEADER_SIZE){
+        log_error("small outgoing buffer");
+        return;
+    }
+
+    uint8_t  rtp_version = 2;
+    uint8_t  padding = 0;
+    uint8_t  extension = 0;
+    uint8_t  csrc_count = 0;
+    uint8_t  payload_type = 0x60;
+    // uint16_t sequence_number = stream_endpoint->sequence_number;
+    uint32_t timestamp = btstack_run_loop_get_time_ms();
+    uint32_t ssrc = 0x11223344;
+
+    // rtp header (min size 12B)
+    int pos = 0;
+    // int mtu = l2cap_get_remote_mtu_for_local_cid(stream_endpoint->l2cap_media_cid);
+
+    media_packet[pos++] = (rtp_version << 6) | (padding << 5) | (extension << 4) | csrc_count;
+    media_packet[pos++] = (marker << 1) | payload_type;
+    big_endian_store_16(media_packet, pos, sequence_number);
+    pos += 2;
+    big_endian_store_32(media_packet, pos, timestamp);
+    pos += 4;
+    big_endian_store_32(media_packet, pos, ssrc); // only used for multicast
+    pos += 4;
+    *offset = pos;
+}
+
+static void avdtp_source_copy_media_payload(uint8_t * media_packet, int size, int * offset, uint8_t * storage, int num_bytes_to_copy, uint8_t num_frames){
+    if (size < num_bytes_to_copy + 1){
+        log_error("small outgoing buffer: buffer size %u, but need %u", size, num_bytes_to_copy + 1);
+        return;
+    }
+    
+    int pos = *offset;
+    media_packet[pos++] = num_frames; // (fragmentation << 7) | (starting_packet << 6) | (last_packet << 5) | num_frames;
+    memcpy(media_packet + pos, storage, num_bytes_to_copy);
+    pos += num_bytes_to_copy;
+    *offset = pos;
+}
+
+int avdtp_source_stream_send_media_payload(uint16_t avdtp_cid, uint8_t local_seid, uint8_t * storage, int num_bytes_to_copy, uint8_t num_frames, uint8_t marker){
+    if (avdtp_source_context->avdtp_cid != avdtp_cid){
+        log_error("avdtp source: avdtp cid 0x%02x not known, expected 0x%02x", avdtp_cid, avdtp_source_context->avdtp_cid);
+        return 0;
+    }
+    avdtp_stream_endpoint_t * stream_endpoint = avdtp_stream_endpoint_for_seid(local_seid, avdtp_source_context);
+    if (!stream_endpoint) {
+        log_error("avdtp source: no stream_endpoint with seid %d", local_seid);
+        return 0;
+    }
+    
+    if (stream_endpoint->l2cap_media_cid == 0){
+        log_error("avdtp source: no media connection for seid %d", local_seid);
+        return 0;
+    } 
+
+    int size = l2cap_get_remote_mtu_for_local_cid(stream_endpoint->l2cap_media_cid);
+    int offset = 0;
+
+    l2cap_reserve_packet_buffer();
+    uint8_t * media_packet = l2cap_get_outgoing_buffer();
+
+    avdtp_source_setup_media_header(media_packet, size, &offset, marker, stream_endpoint->sequence_number);
+    avdtp_source_copy_media_payload(media_packet, size, &offset, storage, num_bytes_to_copy, num_frames);
+    stream_endpoint->sequence_number++;
+    l2cap_send_prepared(stream_endpoint->l2cap_media_cid, offset);
+    return size;
+}
\ No newline at end of file
diff --git a/src/classic/avdtp_source.h b/src/classic/avdtp_source.h
index 691c71ef6..9edefcd12 100644
--- a/src/classic/avdtp_source.h
+++ b/src/classic/avdtp_source.h
@@ -232,7 +232,7 @@ uint8_t avdtp_source_suspend(uint16_t avdtp_cid, uint8_t local_seid);
  */
 avdtp_stream_endpoint_t * avdtp_source_create_stream_endpoint(avdtp_sep_type_t sep_type, avdtp_media_type_t media_type);
 
-#if 0
+
 /**
  * @brief Send media payload.
  * @param avdtp_cid         AVDTP channel identifyer.
@@ -243,8 +243,9 @@ avdtp_stream_endpoint_t * avdtp_source_create_stream_endpoint(avdtp_sep_type_t s
  * @param marker
  * @return max_media_payload_size_without_media_header
  */
-int     avdtp_source_stream_send_media_payload(uint16_t avddp_cid, uint8_t local_seid, uint8_t * storage, int num_bytes_to_copy, uint8_t num_frames, uint8_t marker);
+int avdtp_source_stream_send_media_payload(uint16_t avdtp_cid, uint8_t local_seid, uint8_t * storage, int num_bytes_to_copy, uint8_t num_frames, uint8_t marker);
 
+#if 0
 /**
  * @brief Request to send a media packet. Packet can be then sent on reception of AVDTP_SUBEVENT_STREAMING_CAN_SEND_MEDIA_PACKET_NOW event.
  * @param avdtp_cid         AVDTP channel identifyer.