comparison core/PRU.cpp @ 67:472e892c6e41

Merge newapi into default

  */
 
 #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>
 #include <cerrno>
 #include <fcntl.h>
 #include <sys/mman.h>
+#include <unistd.h>
 
 // Xenomai-specific includes
 #include <sys/mman.h>
 #include <native/task.h>
 #include <native/timer.h>

 #define PRU_MEM_MCASP_OFFSET 0x2000  // Offset within PRU-SHARED RAM
 #define PRU_MEM_MCASP_LENGTH 0x2000  // Length of McASP memory, in bytes
 #define PRU_MEM_DAC_OFFSET 0x0     // Offset within PRU0 RAM
 #define PRU_MEM_DAC_LENGTH 0x2000  // Length of ADC+DAC memory, in bytes
 #define PRU_MEM_COMM_OFFSET 0x0    // Offset within PRU-SHARED RAM
-
+#define PRU_MEM_DIGITAL_OFFSET 0x1000 //Offset within PRU-SHARED RAM
+#define MEM_DIGITAL_BUFFER1_OFFSET 0x400 //Start pointer to DIGITAL_BUFFER1, which is 256 words.
+											// 256 is the maximum number of frames allowed
 #define PRU_SHOULD_STOP 	0
 #define PRU_CURRENT_BUFFER  1
 #define PRU_BUFFER_FRAMES   2
 #define PRU_SHOULD_SYNC     3
 #define PRU_SYNC_ADDRESS    4

 #define PRU_LED_ADDRESS		6
 #define PRU_LED_PIN_MASK	7
 #define PRU_FRAME_COUNT		8
 #define PRU_USE_SPI			9
 #define PRU_SPI_NUM_CHANNELS 10
+#define PRU_USE_DIGITAL    11
+
+short int digitalPins[NUM_DIGITALS]={
+		GPIO_NO_BIT_0,
+		GPIO_NO_BIT_1,
+		GPIO_NO_BIT_2,
+		GPIO_NO_BIT_3,
+		GPIO_NO_BIT_4,
+		GPIO_NO_BIT_5,
+		GPIO_NO_BIT_6,
+		GPIO_NO_BIT_7,
+		GPIO_NO_BIT_8,
+		GPIO_NO_BIT_9,
+		GPIO_NO_BIT_10,
+		GPIO_NO_BIT_11,
+		GPIO_NO_BIT_12,
+		GPIO_NO_BIT_13,
+		GPIO_NO_BIT_14,
+		GPIO_NO_BIT_15,
+};
 
 #define PRU_SAMPLE_INTERVAL_NS 11338	// 88200Hz per SPI sample = 11.338us
 
 #define GPIO0_ADDRESS 		0x44E07000
 #define GPIO1_ADDRESS 		0x4804C000

 
 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 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;
+		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;
 	}
 
 	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(unsigned int i = 0; i < context->digitalChannels; i++){
+			gpio_unexport(digitalPins[i]);
+		}
 	}
 	if(gpio_test_pin_enabled) {
 		gpio_unexport(kPruGPIOTestPin);
 		gpio_unexport(kPruGPIOTestPin2);
 		gpio_unexport(kPruGPIOTestPin3);

 		// Set LED back to default eMMC status
 		// TODO: make it go back to its actual value before this program,
 		// rather than the system default
 		led_set_trigger(3, "mmc1");
 	}
-
 	gpio_enabled = gpio_test_pin_enabled = false;
 }
 
 // Initialise and open the PRU
 int PRU::initialise(int pru_num, int frames_per_buffer, int spi_channels, bool xenomai_test_pin)

 		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;
 
     /* 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;
+	}
+
+	if(digital_enabled) {
+		prussdrv_map_prumem (PRUSS0_SHARED_DATARAM, (void **)&pruMem);
+		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 */
-    if(spi_enabled) {
+    if(digital_enabled) {
+    	pru_buffer_comm[PRU_USE_DIGITAL] = 1;
+//TODO: add mask
+    }
+    else {
+    	pru_buffer_comm[PRU_USE_DIGITAL] = 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(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;
 
 	/* If using GPIO test pin for Xenomai (for debugging), initialise the pointer now */

 
 	return 0;
 }
 
 // Run the code image in the specified file
-int PRU::start()
+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);
-
-    /* Load and execute binary on PRU */
-	if(prussdrv_exec_code(pru_number, PRUcode, sizeof(PRUcode))) {
-    	rt_printf("Failed to execute PRU code\n");
-    	return 1;
-    }
+	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))) {
+			rt_printf("Failed to execute PRU code\n");
+			return 1;
+		}
+	} else {
+		if(gRTAudioVerbose)
+			rt_printf("Using PRU code from %s\n",filename);
+		if(prussdrv_exec_program(pru_number, filename)) {
+			rt_printf("Failed to execute PRU code from %s\n", filename);
+			return 1;
+		}
+	}
 
     running = true;
     return 0;
 }
 
 // Main loop to read and write data from/to PRU
-void PRU::loop()
-{
+void PRU::loop(RT_INTR *pru_interrupt, void *userData)
+{
+#ifdef BEAGLERT_USE_XENOMAI_INTERRUPTS
+	RTIME irqTimeout = PRU_SAMPLE_INTERVAL_NS * 1024;	// Timeout for PRU interrupt: about 10ms, much longer than any expected period
+#else
 	// 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;
-
-	audioInBuffer = (float *)malloc(2 * audio_buffer_frames * sizeof(float));
-	audioOutBuffer = (float *)malloc(2 * audio_buffer_frames * sizeof(float));
-
-	if(audioInBuffer == 0 || audioOutBuffer == 0) {
-		rt_printf("Error: couldn't allocated audio buffers\n");
+	RTIME sleepTime = PRU_SAMPLE_INTERVAL_NS * (context->analogChannels / 2) * context->analogFrames / 4;
+#endif
+
+	float *lastAnalogOutFrame;
+	uint32_t *digitalBuffer0, *digitalBuffer1, *lastDigitalBuffer;
+	uint32_t pru_audio_offset, pru_spi_offset;
+
+	// 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;
 	}
 
+	// 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();
+
+#ifdef BEAGLERT_USE_XENOMAI_INTERRUPTS
+	int result;
+#else
+	// Which buffer the PRU was last processing
+	uint32_t lastPRUBuffer = 0;
+#endif
+
 	while(!gShouldStop) {
-		// Wait for PRU to move to buffer 1
-		while(pru_buffer_comm[PRU_CURRENT_BUFFER] == 0 && !gShouldStop) {
+#ifdef BEAGLERT_USE_XENOMAI_INTERRUPTS
+		// 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);
+#else
+		// Poll
+		while(pru_buffer_comm[PRU_CURRENT_BUFFER] == lastPRUBuffer && !gShouldStop) {
 			rt_task_sleep(sleepTime);
 		}
+
+		lastPRUBuffer = pru_buffer_comm[PRU_CURRENT_BUFFER];
+#endif
+
 		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)
-			render(spi_buffer_frames, audio_buffer_frames, audioInBuffer, audioOutBuffer,
-					pru_buffer_spi_adc, pru_buffer_spi_dac);
-		else
-			render(0, audio_buffer_frames, audioInBuffer, audioOutBuffer, 0, 0);
-
-		// Convert float back to short
-		for(unsigned int n = 0; n < 2 * audio_buffer_frames; n++) {
-			int out = audioOutBuffer[n] * 32768.0;
+		// 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];
+					}
+				}
+			}
+			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 + 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;
+		}
+
+		// Increment total number of samples that have elapsed
+		context->audioSampleCount += context->audioFrames;
 
 		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)
-			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)
-			render(spi_buffer_frames, audio_buffer_frames, audioInBuffer, audioOutBuffer,
-					&pru_buffer_spi_adc[spi_buffer_frames * spi_num_channels], &pru_buffer_spi_dac[spi_buffer_frames * spi_num_channels]);
-		else
-			render(0, audio_buffer_frames, audioInBuffer, audioOutBuffer, 0, 0);
-
-		// 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;
-		}
-	}
+		// FIXME: TESTING!!
+		// if(testCount > 100000)
+		//	break;
+	}
+
+#ifdef BEAGLERT_USE_XENOMAI_INTERRUPTS
+	// Turn off the interrupt for the PRU if it isn't already off
+	rt_intr_disable(pru_interrupt);
+#endif
+
+	// 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(audioInBuffer);
-	free(audioOutBuffer);
+	// 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()
 {