1 /*
2 * Copyright (c) 2024 Nordic Semiconductor ASA
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6 #include <errno.h>
7 #include <errno.h>
8 #include <stdbool.h>
9 #include <stdint.h>
10 #include <string.h>
11
12 #include <zephyr/bluetooth/audio/audio.h>
13 #include <zephyr/kernel.h>
14 #include <zephyr/logging/log.h>
15 #include <zephyr/logging/log_core.h>
16 #include <zephyr/net_buf.h>
17 #include <zephyr/sys/printk.h>
18 #include <zephyr/sys_clock.h>
19
20 #include <lc3.h>
21 #include <sys/errno.h>
22 #include <math.h>
23
24 #include "stream_lc3.h"
25 #include "stream_tx.h"
26
27 LOG_MODULE_REGISTER(lc3, LOG_LEVEL_INF);
28
29 #define LC3_MAX_SAMPLE_RATE 48000U
30 #define LC3_MAX_FRAME_DURATION_US 10000U
31 #define LC3_MAX_NUM_SAMPLES ((LC3_MAX_FRAME_DURATION_US * LC3_MAX_SAMPLE_RATE) / USEC_PER_SEC)
32 /* codec does clipping above INT16_MAX - 3000 */
33 #define AUDIO_VOLUME (INT16_MAX - 3000)
34 #define AUDIO_TONE_FREQUENCY_HZ 400
35
36 static int16_t audio_buf[LC3_MAX_NUM_SAMPLES];
37 /**
38 * Use the math lib to generate a sine-wave using 16 bit samples into a buffer.
39 *
40 * @param stream The TX stream to generate and fill the sine wave for
41 */
fill_audio_buf_sin(struct tx_stream * stream)42 static void fill_audio_buf_sin(struct tx_stream *stream)
43 {
44 const unsigned int num_samples =
45 (stream->lc3_tx.frame_duration_us * stream->lc3_tx.freq_hz) / USEC_PER_SEC;
46 const int sine_period_samples = stream->lc3_tx.freq_hz / AUDIO_TONE_FREQUENCY_HZ;
47 const float step = 2 * 3.1415f / sine_period_samples;
48
49 for (unsigned int i = 0; i < num_samples; i++) {
50 const float sample = sinf(i * step);
51
52 audio_buf[i] = (int16_t)(AUDIO_VOLUME * sample);
53 }
54 }
55
extract_lc3_config(struct tx_stream * stream)56 static int extract_lc3_config(struct tx_stream *stream)
57 {
58 const struct bt_audio_codec_cfg *codec_cfg = stream->bap_stream->codec_cfg;
59 struct stream_lc3_tx *lc3_tx = &stream->lc3_tx;
60 int ret;
61
62 LOG_INF("Extracting LC3 configuration values");
63
64 ret = bt_audio_codec_cfg_get_freq(codec_cfg);
65 if (ret >= 0) {
66 ret = bt_audio_codec_cfg_freq_to_freq_hz(ret);
67 if (ret > 0) {
68 if (LC3_CHECK_SR_HZ(ret)) {
69 lc3_tx->freq_hz = (uint32_t)ret;
70 } else {
71 LOG_ERR("Unsupported sampling frequency for LC3: %d", ret);
72
73 return ret;
74 }
75 } else {
76 LOG_ERR("Invalid frequency: %d", ret);
77
78 return ret;
79 }
80 } else {
81 LOG_ERR("Could not get frequency: %d", ret);
82
83 return ret;
84 }
85
86 ret = bt_audio_codec_cfg_get_frame_dur(codec_cfg);
87 if (ret >= 0) {
88 ret = bt_audio_codec_cfg_frame_dur_to_frame_dur_us(ret);
89 if (ret > 0) {
90 if (LC3_CHECK_DT_US(ret)) {
91 lc3_tx->frame_duration_us = (uint32_t)ret;
92 } else {
93 LOG_ERR("Unsupported frame duration for LC3: %d", ret);
94
95 return ret;
96 }
97 } else {
98 LOG_ERR("Invalid frame duration: %d", ret);
99
100 return ret;
101 }
102 } else {
103 LOG_ERR("Could not get frame duration: %d", ret);
104
105 return ret;
106 }
107
108 ret = bt_audio_codec_cfg_get_chan_allocation(codec_cfg, &lc3_tx->chan_allocation, false);
109 if (ret != 0) {
110 LOG_DBG("Could not get channel allocation: %d", ret);
111 lc3_tx->chan_allocation = BT_AUDIO_LOCATION_MONO_AUDIO;
112 }
113
114 lc3_tx->chan_cnt = bt_audio_get_chan_count(lc3_tx->chan_allocation);
115
116 ret = bt_audio_codec_cfg_get_frame_blocks_per_sdu(codec_cfg, true);
117 if (ret >= 0) {
118 lc3_tx->frame_blocks_per_sdu = (uint8_t)ret;
119 }
120
121 ret = bt_audio_codec_cfg_get_octets_per_frame(codec_cfg);
122 if (ret >= 0) {
123 lc3_tx->octets_per_frame = (uint16_t)ret;
124 } else {
125 LOG_ERR("Could not get octets per frame: %d", ret);
126
127 return ret;
128 }
129
130 return 0;
131 }
132
encode_frame(struct tx_stream * stream,uint8_t index,struct net_buf * out_buf)133 static bool encode_frame(struct tx_stream *stream, uint8_t index, struct net_buf *out_buf)
134 {
135 const uint16_t octets_per_frame = stream->lc3_tx.octets_per_frame;
136 int lc3_ret;
137
138 /* Generate sine wave */
139 fill_audio_buf_sin(stream);
140
141 lc3_ret = lc3_encode(stream->lc3_tx.encoder, LC3_PCM_FORMAT_S16, audio_buf, 1,
142 octets_per_frame, net_buf_tail(out_buf));
143 if (lc3_ret < 0) {
144 LOG_ERR("LC3 encoder failed - wrong parameters?: %d", lc3_ret);
145
146 return false;
147 }
148
149 out_buf->len += octets_per_frame;
150
151 return true;
152 }
153
encode_frame_block(struct tx_stream * stream,struct net_buf * out_buf)154 static bool encode_frame_block(struct tx_stream *stream, struct net_buf *out_buf)
155 {
156 for (uint8_t i = 0U; i < stream->lc3_tx.chan_cnt; i++) {
157 /* We provide the total number of decoded frames to `decode_frame` for logging
158 * purposes
159 */
160 if (!encode_frame(stream, i, out_buf)) {
161 return false;
162 }
163 }
164
165 return true;
166 }
167
stream_lc3_add_data(struct tx_stream * stream,struct net_buf * buf)168 void stream_lc3_add_data(struct tx_stream *stream, struct net_buf *buf)
169 {
170 for (uint8_t i = 0U; i < stream->lc3_tx.frame_blocks_per_sdu; i++) {
171 if (!encode_frame_block(stream, buf)) {
172 break;
173 }
174 }
175 }
176
stream_lc3_init(struct tx_stream * stream)177 int stream_lc3_init(struct tx_stream *stream)
178 {
179 int err;
180
181 err = extract_lc3_config(stream);
182 if (err != 0) {
183 memset(&stream->lc3_tx, 0, sizeof(stream->lc3_tx));
184
185 return err;
186 }
187
188 /* Fill audio buffer with Sine wave only once and repeat encoding the same tone frame */
189 LOG_INF("Initializing sine wave data");
190 fill_audio_buf_sin(stream);
191
192 LOG_INF("Setting up LC3 encoder");
193 stream->lc3_tx.encoder =
194 lc3_setup_encoder(stream->lc3_tx.frame_duration_us, stream->lc3_tx.freq_hz, 0,
195 &stream->lc3_tx.encoder_mem);
196
197 if (stream->lc3_tx.encoder == NULL) {
198 LOG_ERR("Failed to setup LC3 encoder");
199
200 memset(&stream->lc3_tx, 0, sizeof(stream->lc3_tx));
201
202 return -ENOEXEC;
203 }
204
205 return 0;
206 }
207
