andrewm@0: /*
andrewm@0:  *  RTAudio.cpp
andrewm@0:  *
andrewm@0:  *  Central control code for hard real-time audio on BeagleBone Black
andrewm@0:  *  using PRU and Xenomai Linux extensions. This code began as part
andrewm@0:  *  of the Hackable Instruments project (EPSRC) at Queen Mary University
andrewm@0:  *  of London, 2013-14.
andrewm@0:  *
andrewm@0:  *  (c) 2014 Victor Zappi and Andrew McPherson
andrewm@0:  *  Queen Mary University of London
andrewm@0:  */
andrewm@0: 
andrewm@0: 
andrewm@0: #include <stdio.h>
andrewm@0: #include <stdlib.h>
andrewm@0: #include <string.h>
andrewm@0: #include <strings.h>
andrewm@0: #include <math.h>
andrewm@0: #include <iostream>
andrewm@0: #include <assert.h>
andrewm@0: #include <vector>
andrewm@0: 
andrewm@0: // Xenomai-specific includes
andrewm@0: #include <sys/mman.h>
andrewm@0: #include <native/task.h>
andrewm@0: #include <native/timer.h>
andrewm@0: #include <rtdk.h>
andrewm@0: 
andrewm@0: #include "../include/RTAudio.h"
andrewm@0: #include "../include/PRU.h"
andrewm@0: #include "../include/I2c_Codec.h"
andrewm@0: #include "../include/render.h"
andrewm@0: #include "../include/GPIOcontrol.h"
andrewm@0: 
andrewm@0: using namespace std;
andrewm@0: 
andrewm@0: // Data structure to keep track of auxiliary tasks we
andrewm@0: // can schedule
andrewm@0: typedef struct {
andrewm@0: 	RT_TASK task;
andrewm@0: 	void (*function)(void);
andrewm@0: 	char *name;
andrewm@0: 	int priority;
andrewm@0: } InternalAuxiliaryTask;
andrewm@0: 
andrewm@0: const char gRTAudioThreadName[] = "beaglert-audio";
andrewm@0: const char gRTCalculationThreadNameMedium[] = "dbox-calculation-medium";
andrewm@0: const char gRTCalculationThreadNameLow[] = "dbox-calculation-low";
andrewm@0: 
andrewm@0: // Real-time tasks and objects
andrewm@0: RT_TASK gRTAudioThread;
andrewm@0: PRU *gPRU = 0;
andrewm@0: I2c_Codec *gAudioCodec = 0;
andrewm@0: 
andrewm@0: vector<InternalAuxiliaryTask*> gAuxTasks;
andrewm@0: 
andrewm@0: // Flag which tells the audio task to stop
andrewm@0: bool gShouldStop = false;
andrewm@0: 
andrewm@0: // general settings
andrewm@0: int gRTAudioVerbose = 0;   						// Verbosity level for debugging
andrewm@0: char gPRUFilename[256]	 = "pru_rtaudio.bin"; 	// path to PRU binary file
andrewm@0: int gAmplifierMutePin = -1;
andrewm@0: 
andrewm@0: 
andrewm@0: // initAudio() prepares the infrastructure for running PRU-based real-time
andrewm@0: // audio, but does not actually start the calculations.
andrewm@0: // periodSize indicates the number of _sensor_ frames per period: the audio period size
andrewm@0: // is twice this value. In total, the audio latency in frames will be 4*periodSize,
andrewm@0: // plus any latency inherent in the ADCs and DACs themselves.
andrewm@0: // useMatrix indicates whether to use the ADC and DAC or just the audio codec.
andrewm@0: // userData is an opaque pointer which will be passed through to the initialise_render()
andrewm@0: // function for application-specific use
andrewm@0: //
andrewm@0: // Returns 0 on success.
andrewm@0: 
andrewm@0: int initAudio(int periodSize, int useMatrix,
andrewm@0: 			  void *userData,
andrewm@0: 			  int codecI2CAddress, int ampMutePin)
andrewm@0: {
andrewm@0: 	rt_print_auto_init(1);
andrewm@0: 	if(gRTAudioVerbose == 1)
andrewm@0: 		rt_printf("Running with Xenomai\n");
andrewm@0: 
andrewm@0: 	if(gRTAudioVerbose == 1)
andrewm@0: 		cout << "---------------->Init Audio Thread" << endl;
andrewm@0: 
andrewm@0: 	// Prepare GPIO pins for amplifier mute and status LED
andrewm@0: 	if(ampMutePin >= 0) {
andrewm@0: 		gAmplifierMutePin = ampMutePin;
andrewm@0: 
andrewm@0: 		if(gpio_export(ampMutePin)) {
andrewm@0: 			if(gRTAudioVerbose)
andrewm@0: 				cout << "Warning: couldn't export amplifier mute pin\n";
andrewm@0: 		}
andrewm@0: 		if(gpio_set_dir(ampMutePin, OUTPUT_PIN)) {
andrewm@0: 			if(gRTAudioVerbose)
andrewm@0: 				cout << "Couldn't set direction on amplifier mute pin\n";
andrewm@0: 			return -1;
andrewm@0: 		}
andrewm@0: 		if(gpio_set_value(ampMutePin, LOW)) {
andrewm@0: 			if(gRTAudioVerbose)
andrewm@0: 				cout << "Couldn't set value on amplifier mute pin\n";
andrewm@0: 			return -1;
andrewm@0: 		}
andrewm@0: 	}
andrewm@0: 
andrewm@0: 	// Use PRU for audio
andrewm@0: 	gPRU = new PRU();
andrewm@0: 	gAudioCodec = new I2c_Codec();
andrewm@0: 
andrewm@0: 	if(gPRU->prepareGPIO(useMatrix, 1, 1)) {
andrewm@0: 		cout << "Error: unable to prepare GPIO for PRU audio\n";
andrewm@0: 		return 1;
andrewm@0: 	}
andrewm@0: 	if(gPRU->initialise(0, periodSize, true)) {
andrewm@0: 		cout << "Error: unable to initialise PRU\n";
andrewm@0: 		return 1;
andrewm@0: 	}
andrewm@0: 	if(gAudioCodec->initI2C_RW(2, codecI2CAddress, -1)) {
andrewm@0: 		cout << "Unable to open codec I2C\n";
andrewm@0: 		return 1;
andrewm@0: 	}
andrewm@0: 	if(gAudioCodec->initCodec()) {
andrewm@0: 		cout << "Error: unable to initialise audio codec\n";
andrewm@0: 		return 1;
andrewm@0: 	}
andrewm@0: 	gAudioCodec->setDACVolume(0);	// Set the DAC volume to full-scale
andrewm@0: 	gAudioCodec->setHPVolume(-12);	// Headphones 6dB down
andrewm@0: 	gAudioCodec->setADCVolume(-12);	// Set the ADC volume to 6dB down
andrewm@0: 
andrewm@0: 	if(!initialise_render(2, useMatrix ? periodSize : 0, periodSize * 2, 22050.0, 44100.0, userData)) {
andrewm@0: 		cout << "Couldn't initialise audio rendering\n";
andrewm@0: 		return 1;
andrewm@0: 	}
andrewm@0: 
andrewm@0: 	return 0;
andrewm@0: }
andrewm@0: 
andrewm@0: // audioLoop() is the main function which starts the PRU audio code
andrewm@0: // and then transfers control to the PRU object. The PRU object in
andrewm@0: // turn will call the audio render() callback function every time
andrewm@0: // there is new data to process.
andrewm@0: 
andrewm@0: void audioLoop(void *)
andrewm@0: {
andrewm@0: 	if(gRTAudioVerbose==1)
andrewm@0: 		rt_printf("_________________Audio Thread!\n");
andrewm@0: 
andrewm@0: 	// PRU audio
andrewm@0: 	assert(gAudioCodec != 0 && gPRU != 0);
andrewm@0: 
andrewm@0: 	if(gAudioCodec->startAudio(0)) {
andrewm@0: 		rt_printf("Error: unable to start I2C audio codec\n");
andrewm@0: 		gShouldStop = 1;
andrewm@0: 	}
andrewm@0: 	else {
andrewm@0: 		if(gPRU->start(gPRUFilename)) {
andrewm@0: 			rt_printf("Error: unable to start PRU from file %s\n", gPRUFilename);
andrewm@0: 			gShouldStop = 1;
andrewm@0: 		}
andrewm@0: 		else {
andrewm@0: 			// All systems go. Run the loop; it will end when gShouldStop is set to 1
andrewm@0: 			// First unmute the amplifier
andrewm@0: 			if(gpio_set_value(gAmplifierMutePin, HIGH)) {
andrewm@0: 				if(gRTAudioVerbose)
andrewm@0: 					rt_printf("Warning: couldn't set value (high) on amplifier mute pin\n");
andrewm@0: 			}
andrewm@0: 
andrewm@0: 			gPRU->loop();
andrewm@0: 
andrewm@0: 			// Now clean up
andrewm@0: 			// gPRU->waitForFinish();
andrewm@0: 			gPRU->disable();
andrewm@0: 			gAudioCodec->stopAudio();
andrewm@0: 			gPRU->cleanupGPIO();
andrewm@0: 		}
andrewm@0: 	}
andrewm@0: 
andrewm@0: 	if(gRTAudioVerbose == 1)
andrewm@0: 		rt_printf("audio thread ended\n");
andrewm@0: }
andrewm@0: 
andrewm@0: // Create a calculation loop which can run independently of the audio, at a different
andrewm@0: // (equal or lower) priority. Audio priority is 99; priority should be generally be less than this.
andrewm@0: // Returns an (opaque) pointer to the created task on success; 0 on failure
andrewm@0: AuxiliaryTask createAuxiliaryTaskLoop(void (*functionToCall)(void), int priority, const char *name)
andrewm@0: {
andrewm@0: 	InternalAuxiliaryTask *newTask = (InternalAuxiliaryTask*)malloc(sizeof(InternalAuxiliaryTask));
andrewm@0: 
andrewm@0: 	// Attempt to create the task
andrewm@0: 	if(rt_task_create(&(newTask->task), name, 0, priority, T_JOINABLE | T_FPU)) {
andrewm@0: 		  cout << "Error: unable to create auxiliary task " << name << endl;
andrewm@0: 		  free(newTask);
andrewm@0: 		  return 0;
andrewm@0: 	}
andrewm@0: 
andrewm@0: 	// Populate the rest of the data structure and store it in the vector
andrewm@0: 	newTask->function = functionToCall;
andrewm@0: 	newTask->name = strdup(name);
andrewm@0: 	newTask->priority = priority;
andrewm@0: 
andrewm@0: 	gAuxTasks.push_back(newTask);
andrewm@0: 
andrewm@0: 	return (AuxiliaryTask)newTask;
andrewm@0: }
andrewm@0: 
andrewm@0: // Schedule a previously created auxiliary task. It will run when the priority rules next
andrewm@0: // allow it to be scheduled.
andrewm@0: void scheduleAuxiliaryTask(AuxiliaryTask task)
andrewm@0: {
andrewm@0: 	InternalAuxiliaryTask *taskToSchedule = (InternalAuxiliaryTask *)task;
andrewm@0: 
andrewm@0: 	rt_task_resume(&taskToSchedule->task);
andrewm@0: }
andrewm@0: 
andrewm@0: // Calculation loop that can be used for other tasks running at a lower
andrewm@0: // priority than the audio thread. Simple wrapper for Xenomai calls.
andrewm@0: // Treat the argument as containing the task structure
andrewm@0: void auxiliaryTaskLoop(void *taskStruct)
andrewm@0: {
andrewm@0: 	// Get function to call from the argument
andrewm@0: 	void (*auxiliary_function)(void) = ((InternalAuxiliaryTask *)taskStruct)->function;
andrewm@0: 	const char *name = ((InternalAuxiliaryTask *)taskStruct)->name;
andrewm@0: 
andrewm@0: 	// Wait for a notification
andrewm@0: 	rt_task_suspend(NULL);
andrewm@0: 
andrewm@0: 	while(!gShouldStop) {
andrewm@0: 		// Then run the calculations
andrewm@0: 		auxiliary_function();
andrewm@0: 
andrewm@0: 		// Wait for a notification
andrewm@0: 		rt_task_suspend(NULL);
andrewm@0: 	}
andrewm@0: 
andrewm@0: 	if(gRTAudioVerbose == 1)
andrewm@0: 		rt_printf("auxiliary task %s ended\n", name);
andrewm@0: }
andrewm@0: 
andrewm@0: // startAudio() should be called only after initAudio() successfully completes.
andrewm@0: // It launches the real-time Xenomai task which runs the audio loop. Returns 0
andrewm@0: // on success.
andrewm@0: 
andrewm@0: int startAudio()
andrewm@0: {
andrewm@0: 	// Create audio thread with the highest priority
andrewm@0: 	if(rt_task_create(&gRTAudioThread, gRTAudioThreadName, 0, 99, T_JOINABLE | T_FPU)) {
andrewm@0: 		  cout << "Error: unable to create Xenomai audio thread" << endl;
andrewm@0: 		  return -1;
andrewm@0: 	}
andrewm@0: 
andrewm@0: 	// Start all RT threads
andrewm@0: 	if(rt_task_start(&gRTAudioThread, &audioLoop, 0)) {
andrewm@0: 		  cout << "Error: unable to start Xenomai audio thread" << endl;
andrewm@0: 		  return -1;
andrewm@0: 	}
andrewm@0: 
andrewm@0: 	// The user may have created other tasks. Start those also.
andrewm@0: 	vector<InternalAuxiliaryTask*>::iterator it;
andrewm@0: 	for(it = gAuxTasks.begin(); it != gAuxTasks.end(); it++) {
andrewm@0: 		InternalAuxiliaryTask *taskStruct = *it;
andrewm@0: 
andrewm@0: 		if(rt_task_start(&(taskStruct->task), &auxiliaryTaskLoop, taskStruct)) {
andrewm@0: 			cerr << "Error: unable to start Xenomai task " << taskStruct->name << endl;
andrewm@0: 			return -1;
andrewm@0: 		}
andrewm@0: 	}
andrewm@0: 
andrewm@0: 	return 0;
andrewm@0: }
andrewm@0: 
andrewm@0: // Stop the PRU-based audio from running and wait
andrewm@0: // for the tasks to complete before returning.
andrewm@0: 
andrewm@0: void stopAudio()
andrewm@0: {
andrewm@0: 	// Tell audio thread to stop (if this hasn't been done already)
andrewm@0: 	gShouldStop = true;
andrewm@0: 
andrewm@0: 	// Now wait for threads to respond and actually stop...
andrewm@0: 	rt_task_join(&gRTAudioThread);
andrewm@0: 
andrewm@0: 	// Stop all the auxiliary threads too
andrewm@0: 	vector<InternalAuxiliaryTask*>::iterator it;
andrewm@0: 	for(it = gAuxTasks.begin(); it != gAuxTasks.end(); it++) {
andrewm@0: 		InternalAuxiliaryTask *taskStruct = *it;
andrewm@0: 
andrewm@0: 		// Wake up each thread and join it
andrewm@0: 		rt_task_resume(&(taskStruct->task));
andrewm@0: 		rt_task_join(&(taskStruct->task));
andrewm@0: 	}
andrewm@0: }
andrewm@0: 
andrewm@0: // Free any resources associated with PRU real-time audio
andrewm@0: void cleanupAudio()
andrewm@0: {
andrewm@0: 	cleanup_render();
andrewm@0: 
andrewm@0: 	// Clean up the auxiliary tasks
andrewm@0: 	vector<InternalAuxiliaryTask*>::iterator it;
andrewm@0: 	for(it = gAuxTasks.begin(); it != gAuxTasks.end(); it++) {
andrewm@0: 		InternalAuxiliaryTask *taskStruct = *it;
andrewm@0: 
andrewm@0: 		// Free the name string and the struct itself
andrewm@0: 		free(taskStruct->name);
andrewm@0: 		free(taskStruct);
andrewm@0: 	}
andrewm@0: 	gAuxTasks.clear();
andrewm@0: 
andrewm@0: 	if(gPRU != 0)
andrewm@0: 		delete gPRU;
andrewm@0: 	if(gAudioCodec != 0)
andrewm@0: 		delete gAudioCodec;
andrewm@0: 
andrewm@0: 	if(gAmplifierMutePin >= 0)
andrewm@0: 		gpio_unexport(gAmplifierMutePin);
andrewm@0: 	gAmplifierMutePin = -1;
andrewm@0: }
andrewm@0: 
andrewm@0: // Set the verbosity level
andrewm@0: void setVerboseLevel(int level)
andrewm@0: {
andrewm@0: 	gRTAudioVerbose = level;
andrewm@0: }