audio.c

/*
 * This file is part of the OpenKinect Project. http://www.openkinect.org
 *
 * Copyright (c) 2011 individual OpenKinect contributors. See the CONTRIB file
 * for details.
 *
 * This code is licensed to you under the terms of the Apache License, version
 * 2.0, or, at your option, the terms of the GNU General Public License,
 * version 2.0. See the APACHE20 and GPL2 files for the text of the licenses,
 * or the following URLs:
 * http://www.apache.org/licenses/LICENSE-2.0
 * http://www.gnu.org/licenses/gpl-2.0.txt
 *
 * If you redistribute this file in source form, modified or unmodified, you
 * may:
 *   1) Leave this header intact and distribute it under the same terms,
 *      accompanying it with the APACHE20 and GPL20 files, or
 *   2) Delete the Apache 2.0 clause and accompany it with the GPL2 file, or
 *   3) Delete the GPL v2 clause and accompany it with the APACHE20 file
 * In all cases you must keep the copyright notice intact and include a copy
 * of the CONTRIB file.
 *
 * Binary distributions must follow the binary distribution requirements of
 * either License.
 */
#include "libfreenect.h"
#include "libfreenect_audio.h"
#include "freenect_internal.h"

#include <string.h>
#include <stdlib.h>


static void prepare_iso_out_data(freenect_device* dev, uint8_t* buffer) {
	audio_stream* stream = &dev->audio;
	if (dev->audio_out_cb) {
		// Then pull data from the ring buffer, calling the callback as needed to refill the buffer
	} else {
		memset(buffer, 0, 76);
	}
	((uint16_t*)buffer)[0] = stream->out_window;
	buffer[2] = stream->out_seq;
	if (stream->out_window_parity == 0) {
		if (stream->out_counter_within_window < 4) {
			// madness type 1 - high nibble of buffer[3] should be the seq_in_window-th nibble of timestamp
			buffer[3] = (((stream->out_weird_timestamp >> (stream->out_counter_within_window*4)) & 0x000f) << 4) | 0x05; // I have no idea why we do this.
		} else if (stream->out_counter_within_window < 8) {
			// madness type 2 - same thing mod 4, but we add 23 to weird_timestamp for no clear reason.
			buffer[3] = ((((stream->out_weird_timestamp+23) >> ((stream->out_counter_within_window-4)*4)) & 0x000f) << 4) | 0x05; // I have even less idea why we do this.
		} else {
			buffer[3] = 0x01; // Oh, and then this.
		}
	} else {
		if (stream->out_counter_within_window < 4) {
			// madness type 1
			buffer[3] = (((stream->out_weird_timestamp >> (stream->out_counter_within_window*4)) & 0x000f) << 4) | 0x05; // I have no idea why we do this.
		} else {
			buffer[3] = 0x01;
		}
	}
	// Now, update the values for their next usage:
	stream->out_seq++;
	stream->out_counter_within_window++;
	stream->out_weird_timestamp += (stream->out_window_parity == 1) ? 6 : 5;
	switch(stream->out_seq) {
		case 0x80:
			stream->out_seq = 0;
			// TODO: if we're at the beginning of a new outgoing window, save a timestamp
		case 0x2b:
		case 0x56:
			stream->out_counter_within_window = 0;
			stream->out_window++;
			stream->out_window_parity++;
		default:
			break;
	}
	if (stream->out_window_parity == 3) stream->out_window_parity = 0;
	return;
}

static void iso_out_callback(freenect_device *dev, uint8_t *pkt, int len) {
	prepare_iso_out_data(dev, pkt);
}

static void iso_in_callback(freenect_device *dev, uint8_t *pkt, int len) {
	freenect_context *ctx = dev->parent;
	if (len == 524) { // Cool, this is audio data
		audio_in_block* block = (audio_in_block*)pkt;
		if (block->magic != 0x80000080) {
			FN_ERROR("audio: invalid magic in iso IN packet: %08X\n", block->magic);
			return;
		}
		if (block->window != dev->audio.in_window) {
			FN_SPEW("audio: IN window changed: was %04X now %04X\n", dev->audio.in_window, block->window);
			if (dev->audio_in_cb) {
				dev->audio_in_cb(dev, 256, dev->audio.mic_buffer[0], dev->audio.mic_buffer[1],
				                           dev->audio.mic_buffer[2], dev->audio.mic_buffer[3],
				                           dev->audio.cancelled_buffer, dev->audio.in_unknown);
			}
			int t;
			for(t = 0; t < 10; t++) {
				if (dev->audio.last_seen_window[t] != dev->audio.in_window) {
					FN_SPEW("audio: did not receive data for channel 0x%02x\n", t+1);
				}
			}
			if (block->window - dev->audio.in_window > 3) {
				FN_SPEW("audio: packet loss, dropped %d windows\n", (block->window - dev->audio.in_window - 3) / 3);
			}
			dev->audio.in_window = block->window;
		}
		switch(block->channel) {
			case 1:
				memcpy(dev->audio.cancelled_buffer, &block->samples, 512);
				break;
			case 2:
			case 3:
			case 4:
			case 5:
			case 6:
			case 7:
			case 8:
			case 9:
				if (block->channel & 1) {
					memcpy(&(dev->audio.mic_buffer[(block->channel-2) / 2][128]), &block->samples, 512);
				} else {
					memcpy(dev->audio.mic_buffer[(block->channel-2) / 2], &block->samples, 512);
				}
				break;
			default:
				FN_ERROR("audio: invalid channel in iso IN packet: %d\n", block->channel);
				break;
		}
		dev->audio.last_seen_window[block->channel-1] = block->window;

	} else if (len == 60) { // Then this is the uninterpreted signalling information

	} else if (len != 0) {
		FN_ERROR("audio: received an iso IN packet of strange length: %d\n", len);
	}
}

void freenect_set_audio_in_callback(freenect_device *dev, freenect_audio_in_cb callback) {
	dev->audio_in_cb = callback;
}
void freenect_set_audio_out_callback(freenect_device *dev, freenect_audio_out_cb callback) {
	dev->audio_out_cb = callback;
}

int freenect_start_audio(freenect_device* dev) {
	freenect_context *ctx = dev->parent;
	int res;

	if (dev->audio.running)
		return -1;

	// Allocate buffers
	dev->audio.audio_out_ring = (freenect_sample_51*)malloc(256 * sizeof(freenect_sample_51));
	memset(dev->audio.audio_out_ring, 0, 256 * sizeof(freenect_sample_51));
	dev->audio.cancelled_buffer = (int16_t*)malloc(256*sizeof(int16_t));
	memset(dev->audio.cancelled_buffer, 0, 256*sizeof(int16_t));
	int i;
	for(i = 0; i < 4; i++) {
		dev->audio.mic_buffer[i] = (int32_t*)malloc(256*sizeof(int32_t));
		memset(dev->audio.mic_buffer[i], 0, 256*sizeof(int32_t));
	}
	dev->audio.in_unknown = malloc(48); // amount of data in small transfer

	// Set initial parameter values
	dev->audio.ring_reader_idx = 0;
	dev->audio.ring_writer_idx = 0;
	dev->audio.out_window = 0;
	dev->audio.out_seq = 0;
	dev->audio.out_counter_within_window = 0;
	dev->audio.out_weird_timestamp = 0;
	dev->audio.out_window_parity = 0;
	dev->audio.in_window = 0;
	dev->audio.in_counter = 0;
	for(i = 0; i < 10; i++) {
		dev->audio.last_seen_window[i] = 0;
	}

	// Start isochronous streams
	res = fnusb_start_iso(&dev->usb_audio, &dev->audio_in_isoc, iso_in_callback, 0x82, NUM_XFERS, PKTS_PER_XFER, 524);
	if (res < 0) {
		FN_ERROR("audio: failed to start isochronous IN stream: %d\n", res);
		return res;
	}
	res = fnusb_start_iso(&dev->usb_audio, &dev->audio_out_isoc, iso_out_callback, 0x02, NUM_XFERS, PKTS_PER_XFER, 76);
	if (res < 0) {
		FN_ERROR("audio: failed to start isochronous OUT stream: %d\n", res);
		return res;
	}

	dev->audio.running = 1;
	return 0;
}

int freenect_stop_audio(freenect_device* dev) {
	freenect_context *ctx = dev->parent;
	int res;
	int ret = 0;

	if (!dev->audio.running)
		return -1;

	// Stop isochronous transfers
	// Note: I'm not sure how to make this work if there's partial failure.
	dev->audio.running = 0;
	res = fnusb_stop_iso(&dev->usb_audio, &dev->audio_in_isoc);
	if (res < 0) {
		FN_ERROR("audio: failed to stop isochronous IN stream: %d\n", res);
		return res;
	}
	res = fnusb_stop_iso(&dev->usb_audio, &dev->audio_out_isoc);
	if (res < 0) {
		FN_ERROR("audio: failed to stop isochronous OUT stream: %d\n", res);
		return res;
	}

	// Free buffers
	if (dev->audio.audio_out_ring)
		free(dev->audio.audio_out_ring);
	if (dev->audio.cancelled_buffer)
		free(dev->audio.cancelled_buffer);
	if (dev->audio.in_unknown)
		free(dev->audio.in_unknown);
	
	int i;
	for (i = 0; i < 4; i++) {
		if (dev->audio.mic_buffer[i])
			free(dev->audio.mic_buffer[i]);
		dev->audio.mic_buffer[i] = NULL;
	}

	dev->audio.audio_out_ring = NULL;
	dev->audio.cancelled_buffer = NULL;
	dev->audio.in_unknown = NULL;

	return ret;
}