audio.cpp 13.3 KB
Newer Older
1
/**
2 3 4
 * @file audio.cpp
 * @brief Provides audio interface for streamer
 * @copyright Copyright (C) 2017 Elphel Inc.
5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21
 * @author AUTHOR <EMAIL>
 *
 * @par License:
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

22 23 24 25 26 27 28 29
#include "audio.h"
#include "helper.h"

#include <iostream>

#include <sys/ioctl.h>
#include <arpa/inet.h>

30 31 32 33
//#undef AUDIO_DEBUG
#define AUDIO_DEBUG
//#undef AUDIO_DEBUG_2
#define AUDIO_DEBUG_2
34 35

#ifdef AUDIO_DEBUG
36 37 38 39 40
	#define D(s_port, a) \
	do { \
		cerr << __FILE__ << ": " << __FUNCTION__ << ": " << __LINE__ << ": sensor port: " << s_port << " "; \
		a; \
	} while (0)
41
#else
42
	#define D(s_port, a)
43 44 45
#endif

#ifdef AUDIO_DEBUG_2
46 47 48 49 50
	#define D2(s_port, a) \
	do { \
		cerr << __FILE__ << ": " << __FUNCTION__ << ": " << __LINE__ << ": sensor port: " << s_port << " "; \
		a; \
	} while (0)
51
#else
52
	#define D2(s_port, a)
53 54
#endif

55 56
#define SAMPLE_TIME               20                            //< restrict ALSA to have this period, in milliseconds
#define BUFFER_TIME               1000                          //< approximate buffer duration for ALSA, in milliseconds
57
#define LEN                       1200                          //< the size of data buffer for RTP packet, in bytes
58
#define DEFAULT_SND_DEVICE        "plughw:0,0"                  //< default sound card
59 60 61

using namespace std;

62
Audio::Audio(int port, bool enable, Parameters *pars, string &dev_name, int sample_rate, int channels) {
63 64 65 66
	snd_pcm_hw_params_t *hw_params;
	snd_pcm_sw_params_t *sw_params;
	_present = false;
	stream_name = "audio";
67
	params = pars;
68
	sensor_port = port;
69 70

	// normalize audio settings
71 72 73 74 75 76
	if (sample_rate == 0)
		sample_rate = SAMPLE_RATE;
	if (sample_rate > 48000)
		sample_rate = 48000;
	if (sample_rate < 11025)
		sample_rate = 11025;
77
	_sample_rate = sample_rate;
78 79 80 81 82 83
	if (channels == 0)
		channels = SAMPLE_CHANNELS;
	if (channels < 1)
		channels = 1;
	if (channels > 2)
		channels = 2;
84 85 86 87
	_channels = channels;
	_volume = 65535;
	_volume *= 90;
	_volume /= 100;
88

89 90 91 92 93
	SSRC = 10;
	// here sbuffer_len in samples, not bytes
	sbuffer_len = _sample_rate * SAMPLE_TIME;
	sbuffer_len /= 1000;
	sbuffer_len -= sbuffer_len % 2;
94
	D(sensor_port, cerr << "sbuffer_len == " << sbuffer_len << endl);
95 96
	_ptype = 97;
	sbuffer = NULL;
97
	packet_buffer = NULL;
98

99 100
	if (enable) {
		// open ALSA for capturing
101 102
		int err;
		sbuffer = new short[sbuffer_len * 2 * _channels];
103
		packet_buffer = new unsigned char[LEN + 20];
104
		bool init_ok = false;
105 106 107

		if (dev_name == "")
			dev_name = DEFAULT_SND_DEVICE;
108
		while (true) {
109 110 111 112 113 114 115
			/* Previously, "default" device was used in snd_pcm_open(), but this resulted in incorrect time stamps
			 * reported by snd_pcm_status_get_tstamp() (probably a bug in ALSA plugins). The time stamps did not correspond
			 * to system time, but rather started from 0 after reboot and then increased. Opening "plughw:0,0" device
			 * produces correct time stamps, thus this is the default device name now and it can be set from command line.
			 */
			D2(sensor_port, cerr << "Trying to open " << dev_name << endl);
			if ((err = snd_pcm_open(&capture_handle, dev_name.c_str(), SND_PCM_STREAM_CAPTURE, 0)) < 0)
116 117
				break;
			snd_pcm_hw_params_alloca(&hw_params);
118 119 120 121 122 123
			if ((err = snd_pcm_hw_params_any(capture_handle, hw_params)) < 0)
				break;
			if ((err = snd_pcm_hw_params_set_access(capture_handle, hw_params, SND_PCM_ACCESS_RW_INTERLEAVED)) < 0)
				break;
			if ((err = snd_pcm_hw_params_set_format(capture_handle, hw_params, SND_PCM_FORMAT_S16_BE)) < 0)
				break;
124
			unsigned int t = _sample_rate;
125
			if ((err = snd_pcm_hw_params_set_rate_near(capture_handle, hw_params, &t, 0)) < 0)
126 127
				break;
			unsigned int period_time = SAMPLE_TIME * 1000;
128
			if ((err = snd_pcm_hw_params_set_period_time_near(capture_handle, hw_params, &period_time, 0)) < 0)
129
				break;
130
			D(sensor_port, cerr << "period_time == " << period_time << endl);
131
			unsigned int buffer_time = BUFFER_TIME * 1000;
132
			if ((err = snd_pcm_hw_params_set_buffer_time_near(capture_handle, hw_params, &buffer_time, 0)) < 0)
133
				break;
134
			D(sensor_port, cerr << "buffer_time == " << buffer_time << endl);
135
			_sample_rate = t;
136
			if ((err = snd_pcm_hw_params_set_channels(capture_handle, hw_params, _channels)) < 0)
137
				break;
138
			if ((err = snd_pcm_hw_params(capture_handle, hw_params)) < 0)
139 140 141
				break;

			snd_pcm_sw_params_alloca(&sw_params);
142
			if ((err = snd_pcm_sw_params_current(capture_handle, sw_params)) < 0)
143
				break;
144
			if ((err = snd_pcm_sw_params_set_tstamp_mode(capture_handle, sw_params, SND_PCM_TSTAMP_ENABLE)) < 0)
145
				break;
146
			if ((err = snd_pcm_sw_params_set_tstamp_type(capture_handle, sw_params, SND_PCM_TSTAMP_TYPE_GETTIMEOFDAY)) < 0)
147
				break;
148
			if ((err = snd_pcm_sw_params(capture_handle, sw_params)) < 0)
149 150 151 152
				break;
			init_ok = true;
			break;
		}
153 154
		if (init_ok) {
			D(sensor_port, cerr << "Audio init: ok; with sample rate: " << _sample_rate	<< "; and channels: " << _channels << endl);
155 156 157 158
			_present = true;
			_play = false;
			set_volume(_volume);
			// create thread...
159
			init_pthread((void *) this);
160
		} else {
161
			D(sensor_port, cerr << "Audio: init FAIL!" << endl);
162
			D(sensor_port, cerr << "ALSA error message: " << snd_strerror(err) << endl);
163 164 165 166 167 168
			_present = false;
		}
	}
}

Audio::~Audio(void) {
169
	if (_present) {
170 171 172 173
		snd_pcm_drop(capture_handle);
		snd_pcm_close(capture_handle);
		snd_config_update_free_global();
	}
174
	if (sbuffer != NULL)
175
		delete[] sbuffer;
176 177
	if (packet_buffer != NULL)
		delete[] packet_buffer;
178 179 180
}

void Audio::set_capture_volume(int nvolume) {
181
	if (_present == false)
182 183 184 185 186 187 188 189 190 191 192
		return;
	snd_mixer_t *mixer;
	snd_mixer_elem_t *elem;
	snd_mixer_selem_id_t *sid;
	// allocated at stack, so can't free it!
	snd_mixer_selem_id_alloca(&sid);

	snd_mixer_open(&mixer, 0);
	snd_mixer_attach(mixer, "default");
	snd_mixer_selem_register(mixer, NULL, NULL);
	snd_mixer_load(mixer);
193 194

	for (elem = snd_mixer_first_elem(mixer); elem; elem = snd_mixer_elem_next(elem)) {
195
		snd_mixer_selem_get_id(elem, sid);
196
		if (!snd_mixer_selem_is_active(elem))
197 198 199
			continue;
		// set volume at percents for capture elements
		snd_mixer_elem_t *selem = snd_mixer_find_selem(mixer, sid);
200
		if (selem == NULL) {
201 202 203 204
			break;
		}
		long volume_min = 0;
		long volume_max = 0;
205
		if (snd_mixer_selem_get_capture_volume_range(selem, &volume_min, &volume_max) == 0) {
206
			// set volume only for capture
207
			if (nvolume > 65535)
208
				nvolume = 65535;
209
			if (nvolume < 0)
210 211 212 213 214 215 216
				nvolume = 0;
			long long vol_new = volume_max;
			vol_new *= nvolume;
			vol_new /= 65535;
			long vol = 0;
			snd_mixer_selem_get_capture_volume(selem, SND_MIXER_SCHN_FRONT_LEFT, &vol);
			snd_mixer_selem_set_capture_volume_all(selem, vol_new);
217 218 219 220 221 222 223
			D(sensor_port, cerr << "element " << snd_mixer_selem_id_get_name(sid) << " - OLD min vol == "
							<< volume_min << "; max vol == " << volume_max << "; volume == " << vol
							<< endl);
			D(sensor_port, snd_mixer_selem_get_capture_volume(selem, SND_MIXER_SCHN_FRONT_LEFT, &vol));
			D(sensor_port, cerr << "element " << snd_mixer_selem_id_get_name(sid) << " - NEW min vol == "
							<< volume_min << "; max vol == " << volume_max << "; volume == " << vol
							<< endl);
224 225 226 227 228 229
		}
	}
	_volume = nvolume;
	snd_mixer_close(mixer);
}

230 231 232 233
/**
 * @brief Start audio stream if it was enabled
 * The time delta between FPGA time and time stamps reported by ALSA is calculated before stream is started.
 * This delta is applied to time stamps received from ALSA. Previously, this delta was calculated as time difference
234 235
 * between FPGA time and system time reported by \e getsystimeofday(), but this was changed because of some problems
 * with getting system time stamps from sound device opened as "default" ("plughw" is used now). See git commit
236 237 238 239 240 241
 * 58b954f239b695b7deda7a33657841d2c64476ae (or earlier) for previous implementation.
 * @param   ip   ip/port pair for socket
 * @param   port ip/port pair for socket
 * @param   ttl  ttl value for socket
 * @return  None
 */
242
void Audio::Start(string ip, long port, int ttl) {
243
	if (!_present)
244
		return;
245
	D(sensor_port, cerr << "Audio ---> Start !!!" << endl);
246 247 248 249 250 251
	timestamp_rtcp = 0;
	f_tv.tv_sec = 0;
	f_tv.tv_usec = 0;
	snd_pcm_prepare(capture_handle);
	snd_pcm_reset(capture_handle);

252 253 254
	// get FPGA/ALSA time delta
	delta_fpga_alsa = get_delta_fpga_alsa();
	D2(sensor_port, cerr << "FPGA/ALSA time delta = " << delta_fpga_alsa << " us" << endl);
255 256 257 258 259

	RTP_Stream::Start(ip, port, ttl);
}

void Audio::Stop(void) {
260
	if (!_present)
261
		return;
262
	D(sensor_port, cerr << "Audio ---> Stop !!!" << endl);
263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291
	RTP_Stream::Stop();
	f_tv.tv_sec = 0;
	f_tv.tv_usec = 0;
}

long Audio::process(void) {
	long ret = 0;
	snd_pcm_status_t *status;
	snd_pcm_status_alloca(&status);
	snd_timestamp_t ts;
	unsigned long avail;

	for(int i = 0; i < 4; i++) {
		snd_pcm_status(capture_handle, status);
		long slen = snd_pcm_readi(capture_handle, sbuffer, sbuffer_len);
		if(slen > 0) {
			// update statistics for RTCP
			avail = (unsigned long)snd_pcm_status_get_avail(status);
			if(avail > 0) {
				snd_pcm_status_get_tstamp(status, &ts);
				timestamp = timestamp_rtcp;
				timestamp += avail;
				if(timestamp > 0xFFFFFFFF)
					timestamp &= 0xFFFFFFFF;
				f_tv.tv_sec = ts.tv_sec;
				f_tv.tv_usec = ts.tv_usec;
				// correct A time to V time for RTCP sync packets
				f_tv.tv_sec -= 1;
				f_tv.tv_usec += 1000000;
292 293
				f_tv.tv_sec -= delta_fpga_alsa / 1000000;
				f_tv.tv_usec -= delta_fpga_alsa % 1000000;
294 295 296 297 298
				f_tv.tv_sec += f_tv.tv_usec / 1000000;
				f_tv.tv_usec = f_tv.tv_usec % 1000000;
			}
			ret += slen;
			process_send(slen);
299
			// check again that buffer is not empty
300 301 302
			snd_pcm_status(capture_handle, status);
			avail = (unsigned long)snd_pcm_status_get_avail(status);
			if(avail >= (unsigned long)sbuffer_len) {
303
				D2(sensor_port, cerr << "process() again - available " << avail << " samples" << endl);
304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322
				continue;
			}
			return ret;
		}
		if(slen < 0) {
			cerr << "audio process(): slen < 0" << endl;
			break;
		}
		if(slen == 0) {
			cerr << "audio process(): slen == 0" << endl;
			break;
		}
	}
	return ret;
}

long Audio::process_send(long sample_len) {
	unsigned short pnum;

323
	void *m = (void *) sbuffer;
324 325 326 327 328 329

	int i;
	long offset = 0;
	int to_send = sample_len * 2 * _channels;
	int size = to_send;
	long count = 0;
330

331
	uint32_t ts;
332 333
	for (;;) {
		if (to_send == 0)
334
			break;
335
		if (to_send > LEN) {
336 337 338 339 340 341 342 343 344 345
			i = LEN;
			to_send -= i;
		} else {
			i = to_send;
			to_send = 0;
		}
		ts = htonl(timestamp_rtcp);
		uint32_t ts_delta = i / 2;
		ts_delta /= _channels;
		timestamp_rtcp += ts_delta;
346
		if (timestamp_rtcp > 0xFFFFFFFF)
347 348 349 350 351
			timestamp_rtcp &= 0xFFFFFFFF;
		packet_num++;
		pnum = htons(packet_num);
		count += i;
		// fill RTP header
352
		packet_buffer[0] = 0x80;
353
		if (count >= size)
354
			packet_buffer[1] = _ptype + 0x80;
355
		else
356 357 358 359
			packet_buffer[1] = _ptype;
		memcpy((void *) &packet_buffer[2], (void *) &pnum, 2);
		memcpy((void *) &packet_buffer[4], (void *) &ts, 4);
		memcpy((void *) &packet_buffer[8], (void *) &SSRC, 4);
360
		// fill data
361
		memcpy((void *) &packet_buffer[12], (void *) ((char *) m + offset), i);
362 363 364
		offset += i;
		// send packet
		rtp_packets++;
365 366
		rtp_octets += i;                                        // total amount of payload data transmitted
		rtp_socket->send((void *)packet_buffer, i + 12);
367
	}
368 369 370

	return sample_len;
}
371 372 373 374 375 376 377 378 379 380 381 382 383 384 385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 402 403 404 405 406 407 408 409 410 411 412 413 414 415 416 417 418 419 420 421 422

/**
 * @brief Calculate the difference between FPGA time and time stamps reported by ALSA.
 * This function gets two time stamps from FPGA and one time stamp from ALSA driver between the first two,
 * then calculates mean value of the two FPGA samples and finds the delta between the mean value and
 * ALSA time stamp. This value is applied to each time stamp in sound data packet to compensate the difference.
 * @return   The time delta in microseconds
 */
long long Audio::get_delta_fpga_alsa(void)
{
	snd_pcm_status_t *pcm_status;
	snd_timestamp_t snd_ts;
	struct timeval snd_tv;
	struct timeval tv_1, tv_2;
	struct timeval delta_tv;
	unsigned long delta_us;
	long long ret_val;
	bool fpga_greater = false;

	snd_pcm_status_alloca(&pcm_status);
	tv_1 = params->get_fpga_time();
	snd_pcm_status(capture_handle, pcm_status);
	snd_pcm_status_get_tstamp(pcm_status, &snd_ts);
	tv_2 = params->get_fpga_time();
	snd_tv.tv_sec = snd_ts.tv_sec;
	snd_tv.tv_usec = snd_ts.tv_usec;
	delta_us = time_delta_us(tv_2, tv_1);
	delta_us /= 2;
	tv_1 = time_plus_us(tv_1, delta_us);

	if (tv_1.tv_sec > snd_tv.tv_sec)
		fpga_greater = true;
	else if (tv_1.tv_sec == snd_tv.tv_sec)
		if (tv_1.tv_usec > snd_tv.tv_usec)
			fpga_greater = true;
	struct timeval *tv_g = &snd_tv;                             // greater time value
	struct timeval *tv_s = &tv_1;                               // smaller time value
	if (fpga_greater) {
		tv_g = &tv_1;
		tv_s = &snd_tv;
	}
	delta_tv = time_minus(*tv_g, *tv_s);
	ret_val = (long long)delta_tv.tv_sec * 1000000 + (long long)delta_tv.tv_usec;
	if (fpga_greater) {
		ret_val = -ret_val;
	}

	D2(sensor_port, cerr << "ALSA time stamp = " << snd_ts.tv_sec << ":" << snd_ts.tv_usec << endl);
	D2(sensor_port, cerr << "delta_tv = " << delta_tv.tv_sec << ":" << delta_tv.tv_usec << endl);

	return ret_val;
}