comparison core/PRU.cpp @ 108:3068421c0737 ultra-staging

Merged default into ultra-staging

 #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 "../include/intervals.h"
 
 #include <iostream>
 #include <stdlib.h>
 #include <cstdio>
 #include <cerrno>

 
 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;
-extern PRU *gPRU;
 
 // Constructor: specify a PRU number (0 or 1)
-PRU::PRU()
-:  renderTimer(100,0,44100.0,"renderTimer"), sleepTimer(100,0,44100.0,"sleepTimer"),
-  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");

 				xenomai_gpio_fd = -1;
 			}
 		}
 	}
 
+	// 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 1;
+	}
+
+	// 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));
+		last_analog_out_frame = (float *)malloc(context->analogChannels * sizeof(float));
+
+		if(context->analogIn == 0 || context->analogOut == 0 || last_analog_out_frame == 0) {
+			rt_printf("Error: couldn't allocate analog buffers\n");
+			return 1;
+		}
+
+		memset(last_analog_out_frame, 0, context->analogChannels * sizeof(float));
+	}
+
+	// Allocate digital buffers
+	digital_buffer0 = pru_buffer_digital;
+	digital_buffer1 = pru_buffer_digital + MEM_DIGITAL_BUFFER1_OFFSET / sizeof(uint32_t);
+	if(digital_enabled) {
+		last_digital_buffer = (uint32_t *)malloc(context->digitalFrames * sizeof(uint32_t)); //temp buffer to hold previous states
+		if(last_digital_buffer == 0) {
+			rt_printf("Error: couldn't allocate digital buffers\n");
+			return 1;
+		}
+
+		for(unsigned int n = 0; n < context->digitalFrames; n++){
+			// Initialize lastDigitalFrames to all inputs
+			last_digital_buffer[n] = 0x0000ffff;
+		}
+	}
+
+	context->digital = digital_buffer0;
+
 	return 0;
 }
 
 // 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)
+{
+#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;
-	float *analogInBuffer, *analogOutBuffer, *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
-    lastDigitalBuffer = (uint32_t *)malloc(digital_buffer_frames*sizeof(uint32_t)); //temp buffer to hold previous states
-	if(audioInBuffer == 0 || audioOutBuffer == 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;
-	}
-
-    if(digital_enabled){
-		for(unsigned int n=0; n<digital_buffer_frames; n++){ //initialize lastDigitalFrames to all inputs
-			lastDigitalBuffer[n]= 0x0000ffff;
-		}
-    }
-	int count=0;
-	sleepTimer.setNumFrames(audio_buffer_frames);
-	renderTimer.setNumFrames(audio_buffer_frames);
+	RTIME sleepTime = PRU_SAMPLE_INTERVAL_NS * (context->analogChannels / 2) * context->analogFrames / 4;
+#endif
+
+	uint32_t pru_audio_offset, pru_spi_offset;
+
+	// Before starting, look at the last state of the analog and digital outputs which might
+	// have been changed by the user during the setup() function. This lets us start with pin
+	// directions and output values at something other than defaults.
+
+	if(analog_enabled) {
+		if(context->flags & BEAGLERT_FLAG_ANALOG_OUTPUTS_PERSIST) {
+			// Remember the content of the last_analog_out_frame
+			for(unsigned int ch = 0; ch < context->analogChannels; ch++){
+				last_analog_out_frame[ch] = context->analogOut[context->analogChannels * (context->analogFrames - 1) + ch];
+			}
+		}
+	}
+
+	if(digital_enabled) {
+		for(unsigned int n = 0; n < context->digitalFrames; n++){
+			last_digital_buffer[n] = context->digital[n];
+		}
+	}
+
+	// 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
-		sleepTimer.start();
-		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);
 		}
-		sleepTimer.split();
+
+		lastPRUBuffer = pru_buffer_comm[PRU_CURRENT_BUFFER];
+#endif
 
 		if(gShouldStop)
 			break;
-		renderTimer.start();
+
+		// 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 = digital_buffer0;
+		}
+		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 = digital_buffer1;
+		}
+
+		// 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_num_channels + 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] = last_analog_out_frame[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 = last_digital_buffer[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] = (last_digital_buffer[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];
+									   (last_digital_buffer[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 last_analog_out_frame
+				for(unsigned int ch = 0; ch < context->analogChannels; ch++){
+					last_analog_out_frame[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++){
+				last_digital_buffer[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;
 		}
-		renderTimer.split();
-		// Wait for PRU to move to buffer 0
-		sleepTimer.start();
-		while(pru_buffer_comm[PRU_CURRENT_BUFFER] != 0 && !gShouldStop) {
-			rt_task_sleep(sleepTime);
-		}
-		sleepTimer.split();
-		if(gShouldStop)
-			break;
-
-		renderTimer.start();
-		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_num_channels + 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;
-		}
-		renderTimer.split();
-		count+=audio_buffer_frames;
-		if((count&32767)==0){
-			scheduleAuxiliaryTask(gPRU->printIntervalsTask);
-		}
-	}
+
+		// 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(analogOutBuffer);
-	free(analogInBuffer);
-	free(audioOutBuffer);
-	free(audioInBuffer);
-	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(last_analog_out_frame);
+	}
+
+	if(digital_enabled) {
+		free(last_digital_buffer);
+	}
+
+	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()
 {