comparison core/PRU.cpp @ 45:579c86316008 newapi

Major API overhaul. Moved to a single data structure for handling render functions. Functionally, generally similar except for scheduling within PRU loop function, which now uses interrupts from the PRU rather than polling. This requires an updated kernel.

 #include "../include/PRU.h"
 #include "../include/prussdrv.h"
 #include "../include/pruss_intc_mapping.h"
 #include "../include/digital_gpio_mapping.h"
 #include "../include/GPIOcontrol.h"
-#include "../include/render.h"
+#include "../include/BeagleRT.h"
 #include "../include/pru_rtaudio_bin.h"
 
 #include <iostream>
 #include <stdlib.h>
 #include <cstdio>

 
 const unsigned int PRU::kPruGPIOTestPin = 60;	// GPIO1(28); P9-12
 const unsigned int PRU::kPruGPIOTestPin2 = 31;	// GPIO0(31); P9-13
 const unsigned int PRU::kPruGPIOTestPin3 = 26;	// GPIO0(26); P8-14
 
-extern int gShouldStop;
+extern bool gShouldStop;
 extern int gRTAudioVerbose;
 
 // Constructor: specify a PRU number (0 or 1)
-PRU::PRU()
-: pru_number(0), running(false), spi_enabled(false), gpio_enabled(false), led_enabled(false),
-  gpio_test_pin_enabled(false), spi_num_channels(0), xenomai_gpio_fd(-1), xenomai_gpio(0)
+PRU::PRU(BeagleRTContext *input_context)
+: context(input_context), pru_number(0), running(false), analog_enabled(false),
+  digital_enabled(false), gpio_enabled(false), led_enabled(false),
+  gpio_test_pin_enabled(false),
+  pru_buffer_comm(0), pru_buffer_spi_dac(0), pru_buffer_spi_adc(0),
+  pru_buffer_digital(0), pru_buffer_audio_dac(0), pru_buffer_audio_adc(0),
+  xenomai_gpio_fd(-1), xenomai_gpio(0)
 {
 
 }
 
 // Destructor

 // If include_test_pin is set, the GPIO output
 // is also prepared for an output which can be
 // viewed on a scope. If include_led is set,
 // user LED 3 on the BBB is taken over by the PRU
 // to indicate activity
-int PRU::prepareGPIO(int use_spi, int use_digital, int include_test_pin, int include_led)
-{
-	if(use_spi) {
+int PRU::prepareGPIO(int include_test_pin, int include_led)
+{
+	if(context->analogFrames != 0) {
 		// Prepare DAC CS/ pin: output, high to begin
 		if(gpio_export(kPruGPIODACSyncPin)) {
 			if(gRTAudioVerbose)
 				cout << "Warning: couldn't export DAC sync pin\n";
 		}

 			if(gRTAudioVerbose)
 				cout << "Couldn't set value on ADC sync pin\n";
 			return -1;
 		}
 
-		spi_enabled = true;
-	}
-
-	if(use_digital){
-		printf("gNumDigitalChannels: %d;\n",gNumDigitalChannels);
-		for(int i=0; i<gNumDigitalChannels; i++){
+		analog_enabled = true;
+	}
+
+	if(context->digitalFrames != 0){
+		for(unsigned int i = 0; i < context->digitalChannels; i++){
 			if(gpio_export(digitalPins[i])) {
 				if(gRTAudioVerbose)
 					cerr << "Warning: couldn't export digital GPIO pin " << digitalPins[i] << "\n"; // this is left as a warning because if the pin has been exported by somebody else, can still be used
 			}
 			if(gpio_set_dir(digitalPins[i], INPUT_PIN)) {
 				if(gRTAudioVerbose)
 					cerr << "Error: Couldn't set direction on digital GPIO pin " << digitalPins[i] << "\n";
 				return -1;
 			}
 		}
-		digital_enabled=true;
+		digital_enabled = true;
 	}
 
 	if(include_test_pin) {
 		// Prepare GPIO test output (for debugging), low to begin
 		if(gpio_export(kPruGPIOTestPin)) {

 // Clean up the GPIO at the end
 void PRU::cleanupGPIO()
 {
 	if(!gpio_enabled)
 		return;
-	if(spi_enabled) {
+	if(analog_enabled) {
 		gpio_unexport(kPruGPIODACSyncPin);
 		gpio_unexport(kPruGPIOADCSyncPin);
 	}
 	if(digital_enabled){
-		for(int i=0; i<gNumDigitalChannels; i++){
+		for(unsigned int i = 0; i < context->digitalChannels; i++){
 			gpio_unexport(digitalPins[i]);
 		}
 	}
 	if(gpio_test_pin_enabled) {
 		gpio_unexport(kPruGPIOTestPin);

 		return 1;
 	}
 
 	pru_number = pru_num;
 
-	/* Set number of SPI ADC / DAC channels to use. This implicitly
-	 * also determines the sample rate relative to the audio clock
-	 * (half audio clock for 8 channels, full audio clock for 4,
-	 * double audio clock for 2)
-	 */
-	spi_num_channels = spi_channels;
-
     /* Initialize structure used by prussdrv_pruintc_intc   */
     /* PRUSS_INTC_INITDATA is found in pruss_intc_mapping.h */
     tpruss_intc_initdata pruss_intc_initdata = PRUSS_INTC_INITDATA;
 
     /* Allocate and initialize memory */
     prussdrv_init();
-    if(prussdrv_open(pru_number == 0 ? PRU_EVTOUT_0 : PRU_EVTOUT_1)) {
+    if(prussdrv_open(PRU_EVTOUT_0)) {
     	rt_printf("Failed to open PRU driver\n");
     	return 1;
     }
 
     /* Map PRU's INTC */
     prussdrv_pruintc_init(&pruss_intc_initdata);
-
-    spi_buffer_frames = frames_per_buffer;
-    audio_buffer_frames = spi_buffer_frames * spi_num_channels / 4;
-    digital_buffer_frames = audio_buffer_frames;
 
     /* Map PRU memory to pointers */
 	prussdrv_map_prumem (PRUSS0_SHARED_DATARAM, (void **)&pruMem);
     pru_buffer_comm = (uint32_t *)&pruMem[PRU_MEM_COMM_OFFSET/sizeof(uint32_t)];
 	pru_buffer_audio_dac = (int16_t *)&pruMem[PRU_MEM_MCASP_OFFSET/sizeof(uint32_t)];
 
 	/* ADC memory starts 2(ch)*2(buffers)*bufsize samples later */
-	pru_buffer_audio_adc = &pru_buffer_audio_dac[4 * audio_buffer_frames];
-
-	if(spi_enabled) {
+	pru_buffer_audio_adc = &pru_buffer_audio_dac[4 * context->audioFrames];
+
+	if(analog_enabled) {
 		prussdrv_map_prumem (pru_number == 0 ? PRUSS0_PRU0_DATARAM : PRUSS0_PRU1_DATARAM, (void **)&pruMem);
 		pru_buffer_spi_dac = (uint16_t *)&pruMem[PRU_MEM_DAC_OFFSET/sizeof(uint32_t)];
 
 		/* ADC memory starts after N(ch)*2(buffers)*bufsize samples */
-		pru_buffer_spi_adc = &pru_buffer_spi_dac[2 * spi_num_channels * spi_buffer_frames];
+		pru_buffer_spi_adc = &pru_buffer_spi_dac[2 * context->analogChannels * context->analogFrames];
 	}
 	else {
 		pru_buffer_spi_dac = pru_buffer_spi_adc = 0;
 	}
 

 		pru_buffer_digital = (uint32_t *)&pruMem[PRU_MEM_DIGITAL_OFFSET/sizeof(uint32_t)];
 	}
 	else {
 		pru_buffer_digital = 0;
 	}
+
     /* Set up flags */
     pru_buffer_comm[PRU_SHOULD_STOP] = 0;
     pru_buffer_comm[PRU_CURRENT_BUFFER] = 0;
-    pru_buffer_comm[PRU_BUFFER_FRAMES] = spi_buffer_frames;
+    pru_buffer_comm[PRU_BUFFER_FRAMES] = context->analogFrames;
     pru_buffer_comm[PRU_SHOULD_SYNC] = 0;
     pru_buffer_comm[PRU_SYNC_ADDRESS] = 0;
     pru_buffer_comm[PRU_SYNC_PIN_MASK] = 0;
     if(led_enabled) {
     	pru_buffer_comm[PRU_LED_ADDRESS] = USERLED3_GPIO_BASE;

     }
     else {
     	pru_buffer_comm[PRU_LED_ADDRESS] = 0;
     	pru_buffer_comm[PRU_LED_PIN_MASK] = 0;
     }
-    if(spi_enabled) {
+    if(analog_enabled) {
     	pru_buffer_comm[PRU_USE_SPI] = 1;
-    	pru_buffer_comm[PRU_SPI_NUM_CHANNELS] = spi_num_channels;
+    	pru_buffer_comm[PRU_SPI_NUM_CHANNELS] = context->analogChannels;
     }
     else {
     	pru_buffer_comm[PRU_USE_SPI] = 0;
     	pru_buffer_comm[PRU_SPI_NUM_CHANNELS] = 0;
     }

     }
 
     /* Clear ADC and DAC memory.*/
     //TODO: this initialisation should only address the memory effectively used by these buffers, i.e.:depend on the number of frames
     //  (otherwise might cause issues if we move memory locations later on)
-    if(spi_enabled) {
+    if(analog_enabled) {
 		for(int i = 0; i < PRU_MEM_DAC_LENGTH / 2; i++)
 			pru_buffer_spi_dac[i] = 0;
 		if(digital_enabled){
 			for(int i = 0; i < PRU_MEM_DIGITAL_OFFSET*2; i++)
 				pru_buffer_digital[i] = 0x0000ffff; // set to all inputs, to avoid unexpected spikes
 		}
     }
 	for(int i = 0; i < PRU_MEM_MCASP_LENGTH / 2; i++)
 		pru_buffer_audio_dac[i] = 0;
-//TODO: maybe the lines below are to be deleted, as we removed the test code from pru_rtaudio.p ?
+
 	/* If using GPIO test pin for Xenomai (for debugging), initialise the pointer now */
 	if(xenomai_test_pin && xenomai_gpio_fd < 0) {
 		xenomai_gpio_fd = open("/dev/mem", O_RDWR);
 		if(xenomai_gpio_fd < 0)
 			rt_printf("Unable to open /dev/mem for GPIO test pin\n");

 
 // Run the code image in the specified file
 int PRU::start(char * const filename)
 {
 	/* Clear any old interrupt */
-	if(pru_number == 0)
-		prussdrv_pru_clear_event(PRU_EVTOUT_0, PRU0_ARM_INTERRUPT);
-	else
-		prussdrv_pru_clear_event(PRU_EVTOUT_1, PRU1_ARM_INTERRUPT);
+	prussdrv_pru_clear_event(PRU_EVTOUT_0, PRU0_ARM_INTERRUPT);
+
 	/* Load and execute binary on PRU */
 	if(filename[0] == '\0') { //if the string is empty, load the embedded code
 		if(gRTAudioVerbose)
 			rt_printf("Using embedded PRU code\n");
 		if(prussdrv_exec_code(pru_number, PRUcode, sizeof(PRUcode))) {

 	}
 
     running = true;
     return 0;
 }
-uint32_t empty[1024]={0x0};
 
 // Main loop to read and write data from/to PRU
-void PRU::loop()
+void PRU::loop(RT_INTR *pru_interrupt, void *userData)
 {
 	// Polling interval is 1/4 of the period
-	RTIME sleepTime = PRU_SAMPLE_INTERVAL_NS * (spi_num_channels / 2) * spi_buffer_frames / 4;
-	float *audioInBuffer, *audioOutBuffer;
-	float *analogInBuffer, *analogOutBuffer, *lastAnalogOutFrame;
+	//RTIME sleepTime = PRU_SAMPLE_INTERVAL_NS * (context->analogChannels / 2) * context->analogFrames / 4;
+
+	RTIME irqTimeout = PRU_SAMPLE_INTERVAL_NS * 1024;	// Timeout for PRU interrupt: about 10ms, much longer than any expected period
+	float *lastAnalogOutFrame;
 	uint32_t *digitalBuffer0, *digitalBuffer1, *lastDigitalBuffer;
-
-	audioInBuffer = (float *)malloc(2 * audio_buffer_frames * sizeof(float));
-	audioOutBuffer = (float *)malloc(2 * audio_buffer_frames * sizeof(float));
-	analogInBuffer = (float *)malloc(spi_num_channels * spi_buffer_frames * sizeof(float));
-	analogOutBuffer = (float *)malloc(spi_num_channels * spi_buffer_frames * sizeof(float));
-	lastAnalogOutFrame = (float *)malloc(spi_num_channels * sizeof(float));
-	digitalBuffer0 = pru_buffer_digital; 
-	digitalBuffer1 = pru_buffer_digital+MEM_DIGITAL_BUFFER1_OFFSET/sizeof(uint32_t);
-    digital_buffer_frames = digital_enabled ? audio_buffer_frames : 0; //TODO: find a more elegant solution for when the digital is disabled e.g.:
-    																	// - embed in the digitalWrite/Read macros a check whether digital is enabled
-    																	// - allocate some memory in ARM just to allow render() to run regardless.
-    																	//     in this case it can be digitalBuffer0 == digitalBuffer1
-	printf("digital_buffer_frames: %d;\n",digital_buffer_frames);
-    lastDigitalBuffer = (uint32_t *)malloc(digital_buffer_frames*sizeof(uint32_t)); //temp buffer to hold previous states
-	if(audioInBuffer == 0 || audioOutBuffer == 0) {
+	uint32_t pru_audio_offset, pru_spi_offset;
+	int result;
+
+	// Allocate audio buffers
+	context->audioIn = (float *)malloc(2 * context->audioFrames * sizeof(float));
+	context->audioOut = (float *)malloc(2 * context->audioFrames * sizeof(float));
+	if(context->audioIn == 0 || context->audioOut == 0) {
 		rt_printf("Error: couldn't allocate audio buffers\n");
 		return;
 	}
-	if(analogInBuffer == 0 || analogOutBuffer == 0 || lastAnalogOutFrame == 0) {
-		rt_printf("Error: couldn't allocate analog buffers\n");
-		return;
-	}
-	if(lastDigitalBuffer == 0) {
-		rt_printf("Error: couldn't allocate digital buffers\n");
-		return;
-	}
-
-	for(unsigned int n=0; n<digital_buffer_frames; n++){ //initialize lastDigitalFrames to all inputs
-		lastDigitalBuffer[n]= 0x0000ffff;
-	}
+
+	// Allocate analog buffers
+	if(analog_enabled) {
+		context->analogIn = (float *)malloc(context->analogChannels * context->analogFrames * sizeof(float));
+		context->analogOut = (float *)malloc(context->analogChannels * context->analogFrames * sizeof(float));
+		lastAnalogOutFrame = (float *)malloc(context->analogChannels * sizeof(float));
+
+		if(context->analogIn == 0 || context->analogOut == 0 || lastAnalogOutFrame == 0) {
+			rt_printf("Error: couldn't allocate analog buffers\n");
+			return;
+		}
+
+		memset(lastAnalogOutFrame, 0, context->analogChannels * sizeof(float));
+	}
+
+	// Allocate digital buffers
+	digitalBuffer0 = pru_buffer_digital; 
+	digitalBuffer1 = pru_buffer_digital + MEM_DIGITAL_BUFFER1_OFFSET / sizeof(uint32_t);
+	if(digital_enabled) {
+		lastDigitalBuffer = (uint32_t *)malloc(context->digitalFrames * sizeof(uint32_t)); //temp buffer to hold previous states
+		if(lastDigitalBuffer == 0) {
+			rt_printf("Error: couldn't allocate digital buffers\n");
+			return;
+		}
+
+		for(unsigned int n = 0; n < context->digitalFrames; n++){
+			// Initialize lastDigitalFrames to all inputs
+			lastDigitalBuffer[n] = 0x0000ffff;
+		}
+	}
+
+	// TESTING
+	uint32_t testCount = 0;
+	// RTIME startTime = rt_timer_read();
+
 	while(!gShouldStop) {
-		// Wait for PRU to move to buffer 1
-		while(pru_buffer_comm[PRU_CURRENT_BUFFER] == 0 && !gShouldStop) {
-			rt_task_sleep(sleepTime);
-		}
+		// Wait for PRU to move to change buffers;
+		// PRU will send an interrupts which we wait for
+		rt_intr_enable(pru_interrupt);
+		while(!gShouldStop) {
+			result = rt_intr_wait(pru_interrupt, irqTimeout);
+			if(result >= 0)
+				break;
+			else if(result == -ETIMEDOUT)
+				rt_printf("Warning: PRU timeout!\n");
+			else {
+				rt_printf("Error: wait for interrupt failed (%d)\n", result);
+				gShouldStop = 1;
+			}
+		}
+
+		// Clear pending PRU interrupt
+		prussdrv_pru_clear_event(PRU_EVTOUT_1, PRU1_ARM_INTERRUPT);
+
 		if(gShouldStop)
 			break;
+
+		// Check which buffer we're on-- will have been set right
+		// before the interrupt was asserted
+		if(pru_buffer_comm[PRU_CURRENT_BUFFER] == 1) {
+			// PRU is on buffer 1. We read and write to buffer 0
+			pru_audio_offset = 0;
+			pru_spi_offset = 0;
+			if(digital_enabled)
+				context->digital = digitalBuffer0;
+		}
+		else {
+			// PRU is on buffer 0. We read and write to buffer 1
+			pru_audio_offset = context->audioFrames * 2;
+			pru_spi_offset = context->analogFrames * context->analogChannels;
+			if(digital_enabled)
+				context->digital = digitalBuffer1;
+		}
+
+		// FIXME: some sort of margin is needed here to prevent the audio
+		// code from completely eating the Linux system
+		testCount++;
+		//rt_task_sleep(sleepTime*4);
+		//rt_task_sleep(sleepTime/4);
 
 		if(xenomai_gpio != 0) {
 			// Set the test pin high
 			xenomai_gpio[GPIO_SETDATAOUT] = TEST_PIN_MASK;
 		}
 
-		// Render from/to buffer 0
-
 		// Convert short (16-bit) samples to float
-		for(unsigned int n = 0; n < 2 * audio_buffer_frames; n++)
-			audioInBuffer[n] = (float)pru_buffer_audio_adc[n] / 32768.0;
-		if(spi_enabled) {
-			for(unsigned int n = 0; n < spi_num_channels * spi_buffer_frames; n++)
-				analogInBuffer[n] = (float)pru_buffer_spi_adc[n] / 65536.0;
-			//initialize the output buffer with the values that were in the last frame of the previous output
-			for(int n = 0; n < spi_num_channels; n++){
-				for(unsigned int j = 0; j < spi_buffer_frames; j++){
-					analogOutBuffer[j*spi_buffer_frames + n] = lastAnalogOutFrame[n];
+		// TODO: NEON
+		for(unsigned int n = 0; n < 2 * context->audioFrames; n++)
+			context->audioIn[n] = (float)pru_buffer_audio_adc[n + pru_audio_offset] / 32768.0;
+
+		if(analog_enabled) {
+			// TODO: NEON
+			for(unsigned int n = 0; n < context->analogChannels * context->analogFrames; n++)
+				context->analogIn[n] = (float)pru_buffer_spi_adc[n + pru_spi_offset] / 65536.0;
+
+			if(context->flags & BEAGLERT_FLAG_ANALOG_OUTPUTS_PERSIST) {
+				// Initialize the output buffer with the values that were in the last frame of the previous output
+				for(unsigned int ch = 0; ch < context->analogChannels; ch++){
+					for(unsigned int n = 0; n < context->analogFrames; n++){
+						context->analogOut[n * context->analogChannels + ch] = lastAnalogOutFrame[ch];
+					}
 				}
 			}
-        //use past digital values to initialize the array properly.
-		//For each frame:
-        //- pins previously set as outputs will keep the output value they had in the last frame of the previous buffer,
-        //- pins previously set as inputs will carry the newly read input value
-            if(digital_enabled){
-                for(unsigned int n = 0; n < digital_buffer_frames; n++){ 
-                    uint16_t inputs=lastDigitalBuffer[n]&0xffff;//half-word, has 1 for inputs and 0 for outputs
-//                    printf("inputs: 0x%x\n",inputs);
-                    uint16_t outputs=~inputs; //half-word has 1 for outputs and 0 for inputs;
-                    digitalBuffer0[n]=(lastDigitalBuffer[digital_buffer_frames-1]&(outputs<<16))| //keep output values set in the last frame of the previous buffer
-                                         (digitalBuffer0[n]&(inputs<<16))   | //inputs from current digitalBuffer0[n];
-                                         (lastDigitalBuffer[n]&(inputs));     //keep pin configuration from previous digitalBuffer1[n]
-//                    digitalBuffer0[n]=digitalBufferTemp[n]; //ignores inputs
-                }
-            }
-			render(spi_buffer_frames, digital_buffer_frames, audio_buffer_frames, audioInBuffer, audioOutBuffer,
-					analogInBuffer, analogOutBuffer, digitalBuffer0);
-			//remember the content of the lastAnalogOutFrame
-			for(int n = 0; n < spi_num_channels; n++){
-				lastAnalogOutFrame[n] = analogOutBuffer[spi_buffer_frames*(spi_buffer_frames-1) + n];
-			}
-			for(unsigned int n = 0; n < spi_num_channels * spi_buffer_frames; n++) {
-				int out = analogOutBuffer[n] * 65536.0;
+			else {
+				// Outputs are 0 unless set otherwise
+				memset(context->analogOut, 0, context->analogChannels * context->analogFrames * sizeof(float));
+			}
+		}
+
+        if(digital_enabled){
+			// Use past digital values to initialize the array properly.
+			// For each frame:
+			// - pins previously set as outputs will keep the output value they had in the last frame of the previous buffer,
+			// - pins previously set as inputs will carry the newly read input value
+
+			for(unsigned int n = 0; n < context->digitalFrames; n++){
+				uint16_t inputs = lastDigitalBuffer[n] & 0xffff; // half-word, has 1 for inputs and 0 for outputs
+
+				uint16_t outputs = ~inputs; // half-word has 1 for outputs and 0 for inputs;
+				context->digital[n] = (lastDigitalBuffer[context->digitalFrames - 1] & (outputs << 16)) | // keep output values set in the last frame of the previous buffer
+									   (context->digital[n] & (inputs << 16))   | // inputs from current context->digital[n];
+									   (lastDigitalBuffer[n] & (inputs));     // keep pin configuration from previous context->digital[n]
+//                    context->digital[n]=digitalBufferTemp[n]; //ignores inputs
+			}
+		}
+
+		// Call user render function
+        // ***********************
+		render(context, userData);
+		// ***********************
+
+		if(analog_enabled) {
+			if(context->flags & BEAGLERT_FLAG_ANALOG_OUTPUTS_PERSIST) {
+				// Remember the content of the lastAnalogOutFrame
+				for(unsigned int ch = 0; ch < context->analogChannels; ch++){
+					lastAnalogOutFrame[ch] = context->analogOut[context->analogChannels * (context->analogFrames - 1) + ch];
+				}
+			}
+
+			// Convert float back to short for SPI output
+			for(unsigned int n = 0; n < context->analogChannels * context->analogFrames; n++) {
+				int out = context->analogOut[n] * 65536.0;
 				if(out < 0) out = 0;
 				else if(out > 65535) out = 65535;
-				pru_buffer_spi_dac[n] = (uint16_t)out;
-			}
-            if(digital_enabled){ // keep track of past digital values
-                for(unsigned int n = 0; n < digital_buffer_frames; n++){ 
-                    lastDigitalBuffer[n]=digitalBuffer0[n];
-                }
-            }
-		}
-		else
-			render(0, 0, audio_buffer_frames, audioInBuffer, audioOutBuffer, 0, 0, 0); // we still pass digitalBuffer, just it is unused
-        // Convert float back to short
-		for(unsigned int n = 0; n < 2 * audio_buffer_frames; n++) {
-			int out = audioOutBuffer[n] * 32768.0;
+				pru_buffer_spi_dac[n + pru_spi_offset] = (uint16_t)out;
+			}
+		}
+
+		if(digital_enabled) { // keep track of past digital values
+			for(unsigned int n = 0; n < context->digitalFrames; n++){
+				lastDigitalBuffer[n] = context->digital[n];
+			}
+		}
+
+        // Convert float back to short for audio
+		// TODO: NEON
+		for(unsigned int n = 0; n < 2 * context->audioFrames; n++) {
+			int out = context->audioOut[n] * 32768.0;
 			if(out < -32768) out = -32768;
 			else if(out > 32767) out = 32767;
-			pru_buffer_audio_dac[n] = (int16_t)out;
+			pru_buffer_audio_dac[n + pru_audio_offset] = (int16_t)out;
 		}
 
 		if(xenomai_gpio != 0) {
 			// Set the test pin high
 			xenomai_gpio[GPIO_CLEARDATAOUT] = TEST_PIN_MASK;
 		}
 
-		// Wait for PRU to move to buffer 0
-		while(pru_buffer_comm[PRU_CURRENT_BUFFER] != 0 && !gShouldStop) {
-			rt_task_sleep(sleepTime);
-		}
-
-		if(gShouldStop)
+		// FIXME: TESTING!!
+		if(testCount > 100000)
 			break;
-
-		if(xenomai_gpio != 0) {
-			// Set the test pin high
-			xenomai_gpio[GPIO_SETDATAOUT] = TEST_PIN_MASK;
-		}
-
-		// Render from/to buffer 1
-
-		// Convert short (16-bit) samples to float
-		for(unsigned int n = 0; n < 2 * audio_buffer_frames; n++)
-			audioInBuffer[n] = (float)pru_buffer_audio_adc[n + audio_buffer_frames * 2] / 32768.0;
-
-		if(spi_enabled) {
-			 //convert input values TODO: move to PRU
-			for(unsigned int n = 0; n < spi_num_channels * spi_buffer_frames; n++){
-				analogInBuffer[n] = (float)pru_buffer_spi_adc[n + spi_buffer_frames * spi_num_channels] / 65536.0;
-			}
-			//initialize the output buffer with the values that were in the last frame of the previous output
-			for(int n = 0; n < spi_num_channels; n++){
-				for(unsigned int j = 0; j < spi_buffer_frames; j++){
-					analogOutBuffer[j*spi_buffer_frames + n] = lastAnalogOutFrame[n];
-				}
-			}
-            if(digital_enabled){
-                for(unsigned int n = 0; n < digital_buffer_frames; n++){
-                    uint16_t inputs=lastDigitalBuffer[n]&0xffff;//half-word, has 1 for inputs and 0 for outputs
-                    uint16_t outputs=~inputs; //half-word has 1 for outputs and one for inputs;
-                    digitalBuffer1[n]=(lastDigitalBuffer[digital_buffer_frames-1]&(outputs<<16))| //keep output values set in the last frame of the  previous buffer
-                                         (digitalBuffer1[n]&(inputs<<16))   | //inputs from current digitalBuffer1[n];
-                                         (lastDigitalBuffer[n]&(inputs));     //keep pin configuration from previous digitalBuffer1[n]
-//                    digitalBuffer1[n]=digitalBufferTemp[n]; //ignores inputs
-                }
-            }
-			render(spi_buffer_frames, digital_buffer_frames, audio_buffer_frames, audioInBuffer, audioOutBuffer,
-					analogInBuffer, analogOutBuffer, digitalBuffer1);
-			//remember the content of the lastAnalogOutFrame
-			for(int n = 0; n < spi_num_channels; n++){
-				lastAnalogOutFrame[n] = analogOutBuffer[spi_buffer_frames*(spi_buffer_frames-1) + n];
-			}
-
-			for(unsigned int n = 0; n < spi_num_channels * spi_buffer_frames; n++) {
-				int out = analogOutBuffer[n] * 65536.0;
-				if(out < 0) out = 0;
-				else if(out > 65535) out = 65535;
-				pru_buffer_spi_dac[n + spi_buffer_frames * spi_num_channels] = (uint16_t)out;
-			}
-            if(digital_enabled){ // keep track of past digital values
-                for(unsigned int n = 0; n < digital_buffer_frames; n++){ 
-                    lastDigitalBuffer[n]=digitalBuffer1[n];
-                }
-            }
-		}
-		else
-			render(0, 0, audio_buffer_frames, audioInBuffer, audioOutBuffer, 0, 0, 0); // we still pass digitalBuffer, just it is unused
-
-		// Convert float back to short
-		for(unsigned int n = 0; n < 2 * audio_buffer_frames; n++) {
-			int out = audioOutBuffer[n] * 32768.0;
-			if(out < -32768) out = -32768;
-			else if(out > 32767) out = 32767;
-			pru_buffer_audio_dac[n + audio_buffer_frames * 2] = (int16_t)out;
-		}
-
-		if(xenomai_gpio != 0) {
-			// Set the test pin high
-			xenomai_gpio[GPIO_CLEARDATAOUT] = TEST_PIN_MASK;
-		}
-	}
+	}
+
+	// Turn off the interrupt for the PRU if it isn't already off
+	rt_intr_disable(pru_interrupt);
+
+	// FIXME: TESTING
+	// RTIME endTime = rt_timer_read();
+	// RTIME diffTime = endTime - startTime;
+	// rt_printf("%d blocks elapsed in %f seconds, %f Hz block rate\n", testCount, ((float)diffTime / 1.0e9), (float)testCount / ((float)diffTime / 1.0e9));
 
 	// Tell PRU to stop
 	pru_buffer_comm[PRU_SHOULD_STOP] = 1;
 
-	free(analogOutBuffer);
-	free(audioInBuffer);
-	free(audioOutBuffer);
-	free(analogInBuffer);
-	free(lastAnalogOutFrame);
-    free(lastDigitalBuffer);
+	// Wait two buffer lengths for the PRU to finish
+	rt_task_sleep(PRU_SAMPLE_INTERVAL_NS * context->analogFrames * 4 * 2);
+
+	// Clean up after ourselves
+	free(context->audioIn);
+	free(context->audioOut);
+
+	if(analog_enabled) {
+		free(context->analogIn);
+		free(context->analogOut);
+		free(lastAnalogOutFrame);
+	}
+
+	if(digital_enabled) {
+		free(lastDigitalBuffer);
+	}
+
+	context->audioIn = context->audioOut = 0;
+	context->analogIn = context->analogOut = 0;
+	context->digital = 0;
 }
 
 // Wait for an interrupt from the PRU indicate it is finished
 void PRU::waitForFinish()
 {
 	if(!running)
 		return;
-    prussdrv_pru_wait_event (pru_number == 0 ? PRU_EVTOUT_0 : PRU_EVTOUT_1);
-	if(pru_number == 0)
-		prussdrv_pru_clear_event(PRU_EVTOUT_0, PRU0_ARM_INTERRUPT);
-	else
-		prussdrv_pru_clear_event(PRU_EVTOUT_1, PRU1_ARM_INTERRUPT);
+    prussdrv_pru_wait_event (PRU_EVTOUT_0);
+	prussdrv_pru_clear_event(PRU_EVTOUT_0, PRU0_ARM_INTERRUPT);
 }
 
 // Turn off the PRU when done
 void PRU::disable()
 {