diff --git a/Software/VNA_embedded/.cproject b/Software/VNA_embedded/.cproject
index 9bbe132..4dacaaa 100644
--- a/Software/VNA_embedded/.cproject
+++ b/Software/VNA_embedded/.cproject
@@ -29,7 +29,7 @@
             			
             <storageModule moduleId="cdtBuildSystem" version="4.0.0">
                 				
-                <configuration artifactExtension="elf" artifactName="${ProjName}" buildArtefactType="org.eclipse.cdt.build.core.buildArtefactType.exe" buildProperties="org.eclipse.cdt.build.core.buildArtefactType=org.eclipse.cdt.build.core.buildArtefactType.exe,org.eclipse.cdt.build.core.buildType=org.eclipse.cdt.build.core.buildType.debug" cleanCommand="rm -rf" description="" id="fr.ac6.managedbuild.config.gnu.cross.exe.debug.1502405410" name="Debug" optionalBuildProperties="org.eclipse.cdt.docker.launcher.containerbuild.property.volumes=,org.eclipse.cdt.docker.launcher.containerbuild.property.selectedvolumes=" parent="fr.ac6.managedbuild.config.gnu.cross.exe.debug" postannouncebuildStep="Generating hex and Printing size information:" postbuildStep="arm-none-eabi-objcopy -O binary &quot;${BuildArtifactFileBaseName}.elf&quot; &quot;${BuildArtifactFileBaseName}.bin&quot; &amp;&amp; arm-none-eabi-objcopy -O ihex &quot;${BuildArtifactFileBaseName}.elf&quot; &quot;${BuildArtifactFileBaseName}.hex&quot; &amp;&amp; arm-none-eabi-size &quot;${BuildArtifactFileName}&quot;">
+                <configuration artifactExtension="elf" artifactName="${ProjName}" buildArtefactType="org.eclipse.cdt.build.core.buildArtefactType.exe" buildProperties="org.eclipse.cdt.build.core.buildArtefactType=org.eclipse.cdt.build.core.buildArtefactType.exe,org.eclipse.cdt.build.core.buildType=org.eclipse.cdt.build.core.buildType.debug" cleanCommand="rm -rf" description="" id="fr.ac6.managedbuild.config.gnu.cross.exe.debug.1502405410" name="Debug" optionalBuildProperties="org.eclipse.cdt.docker.launcher.containerbuild.property.selectedvolumes=,org.eclipse.cdt.docker.launcher.containerbuild.property.volumes=" parent="fr.ac6.managedbuild.config.gnu.cross.exe.debug" postannouncebuildStep="Generating hex and Printing size information:" postbuildStep="arm-none-eabi-objcopy -O binary &quot;${BuildArtifactFileBaseName}.elf&quot; &quot;${BuildArtifactFileBaseName}.bin&quot; &amp;&amp; arm-none-eabi-objcopy -O ihex &quot;${BuildArtifactFileBaseName}.elf&quot; &quot;${BuildArtifactFileBaseName}.hex&quot; &amp;&amp; arm-none-eabi-size &quot;${BuildArtifactFileName}&quot;">
                     					
                     <folderInfo id="fr.ac6.managedbuild.config.gnu.cross.exe.debug.1502405410." name="/" resourcePath="">
                         						
@@ -185,6 +185,10 @@
                                 								
                                 <option IS_BUILTIN_EMPTY="false" IS_VALUE_EMPTY="false" id="gnu.cpp.compiler.option.preprocessor.def.320332657" name="Defined symbols (-D)" superClass="gnu.cpp.compiler.option.preprocessor.def" useByScannerDiscovery="false" valueType="definedSymbols">
                                     									
+                                    <listOptionValue builtIn="false" value="FW_MAJOR=0"/>
+                                    									
+                                    <listOptionValue builtIn="false" value="FW_MINOR=1"/>
+                                    									
                                     <listOptionValue builtIn="false" value="USE_FULL_LL_DRIVER"/>
                                     									
                                     <listOptionValue builtIn="false" value="HW_REVISION='B'"/>
diff --git a/Software/VNA_embedded/Application/App.cpp b/Software/VNA_embedded/Application/App.cpp
index 3a27dc1..a555dde 100644
--- a/Software/VNA_embedded/Application/App.cpp
+++ b/Software/VNA_embedded/Application/App.cpp
@@ -13,6 +13,9 @@
 #include "FreeRTOS.h"
 #include "task.h"
 #include "Led.hpp"
+#include "Hardware.hpp"
+#include "Manual.hpp"
+#include "Generator.hpp"
 
 #define LOG_LEVEL	LOG_LEVEL_INFO
 #define LOG_MODULE	"App"
@@ -21,17 +24,9 @@
 static Protocol::Datapoint result;
 static Protocol::SweepSettings settings;
 
-static FPGA::SamplingResult statusResult;
-static Protocol::ManualControl manual;
-
 static Protocol::PacketInfo packet;
 static TaskHandle_t handle;
 
-// TODO set proper values
-//#define HW_REVISION			'A'
-#define FW_MAJOR			0
-#define FW_MINOR			01
-
 #if HW_REVISION >= 'B'
 // has MCU controllable flash chip, firmware update supported
 #define HAS_FLASH
@@ -42,7 +37,6 @@ static Flash flash = Flash(&hspi1, FLASH_CS_GPIO_Port, FLASH_CS_Pin);
 
 #define FLAG_USB_PACKET		0x01
 #define FLAG_DATAPOINT		0x02
-#define FLAG_STATUSRESULT	0x04
 
 static void VNACallback(Protocol::Datapoint res) {
 	result = res;
@@ -50,12 +44,6 @@ static void VNACallback(Protocol::Datapoint res) {
 	xTaskNotifyFromISR(handle, FLAG_DATAPOINT, eSetBits, &woken);
 	portYIELD_FROM_ISR(woken);
 }
-static void VNAStatusCallback(FPGA::SamplingResult res) {
-	statusResult = res;
-	BaseType_t woken = false;
-	xTaskNotifyFromISR(handle, FLAG_STATUSRESULT, eSetBits, &woken);
-	portYIELD_FROM_ISR(woken);
-}
 static void USBPacketReceived(Protocol::PacketInfo p) {
 	packet = p;
 	BaseType_t woken = false;
@@ -102,7 +90,7 @@ void App_Start() {
 	EN_6V_GPIO_Port->BSRR = EN_6V_Pin;
 #endif
 
-	if (!VNA::Init()) {
+	if (!HW::Init()) {
 		LOG_CRIT("Initialization failed, unable to start");
 		LED::Error(4);
 	}
@@ -114,7 +102,6 @@ void App_Start() {
 
 	uint32_t lastNewPoint = HAL_GetTick();
 	bool sweepActive = false;
-	Protocol::ReferenceSettings reference;
 
 	LED::Off();
 	while (1) {
@@ -127,84 +114,38 @@ void App_Start() {
 				packet.datapoint = result;
 				Communication::Send(packet);
 				lastNewPoint = HAL_GetTick();
-				if(result.pointNum == settings.points - 1) {
-					// end of sweep
-					VNA::Ref::applySettings(reference);
-					// Compile info packet
-					packet.type = Protocol::PacketType::DeviceInfo;
-					packet.info.FPGA_configured = 1;
-					packet.info.FW_major = FW_MAJOR;
-					packet.info.FW_minor = FW_MINOR;
-					packet.info.HW_Revision = HW_REVISION;
-					VNA::fillDeviceInfo(&packet.info);
-					Communication::Send(packet);
-					FPGA::ResetADCLimits();
-					// Start next sweep
-					FPGA::StartSweep();
-				}
-			}
-			if(notification & FLAG_STATUSRESULT) {
-				Protocol::PacketInfo p;
-				p.type = Protocol::PacketType::Status;
-				memset(&p.status, 0, sizeof(p.status));
-				uint16_t isr_flags = FPGA::GetStatus();
-				if (!(isr_flags & 0x0002)) {
-					p.status.source_locked = 1;
-				}
-				if (!(isr_flags & 0x0001)) {
-					p.status.LO_locked = 1;
-				}
-				auto limits = FPGA::GetADCLimits();
-				FPGA::ResetADCLimits();
-				p.status.port1min = limits.P1min;
-				p.status.port1max = limits.P1max;
-				p.status.port2min = limits.P2min;
-				p.status.port2max = limits.P2max;
-				p.status.refmin = limits.Rmin;
-				p.status.refmax = limits.Rmax;
-				p.status.port1real = (float) statusResult.P1I / manual.Samples;
-				p.status.port1imag = (float) statusResult.P1Q / manual.Samples;
-				p.status.port2real = (float) statusResult.P2I / manual.Samples;
-				p.status.port2imag = (float) statusResult.P2Q / manual.Samples;
-				p.status.refreal = (float) statusResult.RefI / manual.Samples;
-				p.status.refimag = (float) statusResult.RefQ / manual.Samples;
-				VNA::GetTemps(&p.status.temp_source, &p.status.temp_LO);
-				Communication::Send(p);
-				// Trigger next status update
-				FPGA::StartSweep();
 			}
 			if(notification & FLAG_USB_PACKET) {
 				switch(packet.type) {
 				case Protocol::PacketType::SweepSettings:
 					LOG_INFO("New settings received");
 					settings = packet.settings;
-					sweepActive = VNA::ConfigureSweep(settings, VNACallback);
+					sweepActive = VNA::Setup(settings, VNACallback);
 					lastNewPoint = HAL_GetTick();
 					Communication::SendWithoutPayload(Protocol::PacketType::Ack);
 					break;
 				case Protocol::PacketType::ManualControl:
 					sweepActive = false;
-					manual = packet.manual;
-					VNA::ConfigureManual(manual, VNAStatusCallback);
+					Manual::Setup(packet.manual);
 					Communication::SendWithoutPayload(Protocol::PacketType::Ack);
 					break;
 				case Protocol::PacketType::Reference:
-					reference = packet.reference;
+					HW::Ref::set(packet.reference);
 					if(!sweepActive) {
 						// can update right now
-						VNA::Ref::applySettings(reference);
+						HW::Ref::update();
 					}
 					Communication::SendWithoutPayload(Protocol::PacketType::Ack);
 					break;
 				case Protocol::PacketType::Generator:
 					sweepActive = false;
 					LOG_INFO("Updating generator setting");
-					VNA::ConfigureGenerator(packet.generator);
+					Generator::Setup(packet.generator);
 					Communication::SendWithoutPayload(Protocol::PacketType::Ack);
 					break;
 #ifdef HAS_FLASH
 				case Protocol::PacketType::ClearFlash:
-					VNA::SetIdle();
+					HW::SetMode(HW::Mode::Idle);
 					sweepActive = false;
 					LOG_DEBUG("Erasing FLASH in preparation for firmware update...");
 					if(flash.eraseChip()) {
@@ -250,9 +191,9 @@ void App_Start() {
 			LOG_WARN("Timed out waiting for point, last received point was %d (Status 0x%04x)", result.pointNum, FPGA::GetStatus());
 			FPGA::AbortSweep();
 			// restart the current sweep
-			VNA::Init();
-			VNA::Ref::applySettings(reference);
-			VNA::ConfigureSweep(settings, VNACallback);
+			HW::Init();
+			HW::Ref::update();
+			VNA::Setup(settings, VNACallback);
 			sweepActive = true;
 			lastNewPoint = HAL_GetTick();
 		}
diff --git a/Software/VNA_embedded/Application/Communication/Protocol.hpp b/Software/VNA_embedded/Application/Communication/Protocol.hpp
index 05253d8..d5b5a80 100644
--- a/Software/VNA_embedded/Application/Communication/Protocol.hpp
+++ b/Software/VNA_embedded/Application/Communication/Protocol.hpp
@@ -98,6 +98,14 @@ using ManualControl = struct _manualControl {
     uint8_t WindowType :2;
 };
 
+using SpectrumAnalyzerSettings = struct _spectrumAnalyzerSettings {
+	uint64_t f_start;
+	uint64_t f_stop;
+	uint32_t RBW;
+	uint8_t WindowType :2;
+	uint8_t SignalID :1;
+};
+
 
 static constexpr uint16_t FirmwareChunkSize = 256;
 using FirmwarePacket = struct _firmwarePacket {
diff --git a/Software/VNA_embedded/Application/Generator.cpp b/Software/VNA_embedded/Application/Generator.cpp
new file mode 100644
index 0000000..f0d2bd0
--- /dev/null
+++ b/Software/VNA_embedded/Application/Generator.cpp
@@ -0,0 +1,83 @@
+#include "Generator.hpp"
+#include "Manual.hpp"
+#include "Hardware.hpp"
+#include "max2871.hpp"
+#include "Si5351C.hpp"
+
+static constexpr uint32_t BandSwitchFrequency = 25000000;
+
+void Generator::Setup(Protocol::GeneratorSettings g) {
+	if(g.activePort == 0) {
+			// both ports disabled, no need to configure PLLs
+			HW::SetMode(HW::Mode::Idle);
+			return;
+	}
+	Protocol::ManualControl m;
+	// LOs not required
+	m.LO1CE = 0;
+	m.LO1Frequency = 1000000000;
+	m.LO1RFEN = 0;
+	m.LO1RFEN = 0;
+	m.LO2EN = 0;
+	m.LO2Frequency = 60000000;
+	m.Port1EN = 0;
+	m.Port2EN = 0;
+	m.RefEN = 0;
+	m.Samples = 131072;
+	m.WindowType = (int) FPGA::Window::None;
+	// Select correct source
+	if(g.frequency < BandSwitchFrequency) {
+		m.SourceLowEN = 1;
+		m.SourceLowFrequency = g.frequency;
+		m.SourceHighCE = 0;
+		m.SourceHighRFEN = 0;
+		m.SourceHighFrequency = BandSwitchFrequency;
+		m.SourceHighLowpass = (int) FPGA::LowpassFilter::M947;
+		m.SourceHighPower = (int) MAX2871::Power::n4dbm;
+		m.SourceHighband = false;
+	} else {
+		m.SourceLowEN = 0;
+		m.SourceLowFrequency = BandSwitchFrequency;
+		m.SourceHighCE = 1;
+		m.SourceHighRFEN = 1;
+		m.SourceHighFrequency = g.frequency;
+		if(g.frequency < 900000000UL) {
+			m.SourceHighLowpass = (int) FPGA::LowpassFilter::M947;
+		} else if(g.frequency < 1800000000UL) {
+			m.SourceHighLowpass = (int) FPGA::LowpassFilter::M1880;
+		} else if(g.frequency < 3500000000UL) {
+			m.SourceHighLowpass = (int) FPGA::LowpassFilter::M3500;
+		} else {
+			m.SourceHighLowpass = (int) FPGA::LowpassFilter::None;
+		}
+		m.SourceHighband = true;
+	}
+	switch(g.activePort) {
+	case 1:
+		m.AmplifierEN = 1;
+		m.PortSwitch = 0;
+		break;
+	case 2:
+		m.AmplifierEN = 1;
+		m.PortSwitch = 1;
+		break;
+	}
+	// Set level (not very accurate)
+	if(g.cdbm_level > -1000) {
+		// use higher source power (approx 0dbm with no attenuation)
+		m.SourceHighPower = (int) MAX2871::Power::p5dbm;
+		m.SourceLowPower = (int) Si5351C::DriveStrength::mA8;
+	} else {
+		// use lower source power (approx -10dbm with no attenuation)
+		m.SourceHighPower = (int) MAX2871::Power::n4dbm;
+		m.SourceLowPower = (int) Si5351C::DriveStrength::mA2;
+		g.cdbm_level += 1000;
+	}
+	// calculate required attenuation
+	uint16_t attval = -g.cdbm_level / 25;
+	if(attval > 127) {
+		attval = 127;
+	}
+	m.attenuator = attval;
+	Manual::Setup(m);
+}
diff --git a/Software/VNA_embedded/Application/Generator.hpp b/Software/VNA_embedded/Application/Generator.hpp
new file mode 100644
index 0000000..11ab6b2
--- /dev/null
+++ b/Software/VNA_embedded/Application/Generator.hpp
@@ -0,0 +1,10 @@
+#pragma once
+
+#include "Protocol.hpp"
+
+namespace Generator {
+
+// Generator is using the manual mode with some encapsulation for setting up. No further functions required
+void Setup(Protocol::GeneratorSettings g);
+
+}
diff --git a/Software/VNA_embedded/Application/HW_HAL.cpp b/Software/VNA_embedded/Application/HW_HAL.cpp
new file mode 100644
index 0000000..d736b5c
--- /dev/null
+++ b/Software/VNA_embedded/Application/HW_HAL.cpp
@@ -0,0 +1,5 @@
+#include "HW_HAL.hpp"
+
+Si5351C HWHAL::Si5351 = Si5351C(&hi2c2, 26000000);
+MAX2871 HWHAL::Source = MAX2871(&hspi1, FPGA_CS_GPIO_Port, FPGA_CS_Pin, nullptr, 0, nullptr, 0, nullptr, 0, GPIOA, GPIO_PIN_6);
+MAX2871 HWHAL::LO1 = MAX2871(&hspi1, FPGA_CS_GPIO_Port, FPGA_CS_Pin, nullptr, 0, nullptr, 0, nullptr, 0, GPIOA, GPIO_PIN_6);
diff --git a/Software/VNA_embedded/Application/VNA_HAL.hpp b/Software/VNA_embedded/Application/HW_HAL.hpp
similarity index 53%
rename from Software/VNA_embedded/Application/VNA_HAL.hpp
rename to Software/VNA_embedded/Application/HW_HAL.hpp
index e5282d4..0150894 100644
--- a/Software/VNA_embedded/Application/VNA_HAL.hpp
+++ b/Software/VNA_embedded/Application/HW_HAL.hpp
@@ -8,11 +8,11 @@
 extern I2C_HandleTypeDef hi2c2;
 extern SPI_HandleTypeDef hspi1;
 
-namespace VNAHAL {
+namespace HWHAL {
 
-static Si5351C Si5351 = Si5351C(&hi2c2, 26000000);
-static MAX2871 Source = MAX2871(&hspi1, FPGA_CS_GPIO_Port, FPGA_CS_Pin, nullptr, 0, nullptr, 0, nullptr, 0, GPIOA, GPIO_PIN_6);
-static MAX2871 LO1 = MAX2871(&hspi1, FPGA_CS_GPIO_Port, FPGA_CS_Pin, nullptr, 0, nullptr, 0, nullptr, 0, GPIOA, GPIO_PIN_6);
+extern Si5351C Si5351;
+extern MAX2871 Source;
+extern MAX2871 LO1;
 
 // Mapping of the Si5351 channels to PLLs/Mixers
 namespace SiChannel {
diff --git a/Software/VNA_embedded/Application/Hardware.cpp b/Software/VNA_embedded/Application/Hardware.cpp
new file mode 100644
index 0000000..b677eb4
--- /dev/null
+++ b/Software/VNA_embedded/Application/Hardware.cpp
@@ -0,0 +1,293 @@
+#include <HW_HAL.hpp>
+#include "Hardware.hpp"
+#include "Si5351C.hpp"
+#include "max2871.hpp"
+#include "FPGA/FPGA.hpp"
+#include "Exti.hpp"
+#include "VNA.hpp"
+#include "Manual.hpp"
+
+#define LOG_LEVEL	LOG_LEVEL_INFO
+#define LOG_MODULE	"HW"
+#include "Log.h"
+
+static uint32_t extOutFreq = 0;
+static bool extRefInUse = false;
+HW::Mode activeMode;
+
+static constexpr uint32_t IF1 = 60100000;
+static constexpr uint32_t IF2 = 250000;
+static Protocol::ReferenceSettings ref;
+
+using namespace HWHAL;
+
+static void HaltedCallback() {
+	switch(activeMode) {
+	case HW::Mode::VNA:
+		VNA::SweepHalted();
+		break;
+	default:
+		break;
+	}
+}
+static void ReadComplete(FPGA::SamplingResult result) {
+	bool needs_work = false;
+	switch(activeMode) {
+	case HW::Mode::VNA:
+		needs_work = VNA::MeasurementDone(result);
+		break;
+	case HW::Mode::Manual:
+		needs_work = Manual::MeasurementDone(result);
+		break;
+	default:
+		break;
+	}
+	if(needs_work) {
+		HAL_NVIC_SetPendingIRQ(COMP4_IRQn);
+	}
+}
+
+static void FPGA_Interrupt(void*) {
+	FPGA::InitiateSampleRead(ReadComplete);
+}
+
+/* low priority interrupt to handle additional workload after FPGA interrupt */
+extern "C" {
+void COMP4_IRQHandler(void) {
+	switch(activeMode) {
+	case HW::Mode::VNA:
+		VNA::Work();
+		break;
+	case HW::Mode::Manual:
+		Manual::Work();
+		break;
+	default:
+		break;
+	}
+}
+}
+
+bool HW::Init() {
+	LOG_DEBUG("Initializing...");
+	HAL_NVIC_SetPriority(COMP4_IRQn, 15, 0);
+	HAL_NVIC_EnableIRQ(COMP4_IRQn);
+
+	activeMode = Mode::Idle;
+
+	Si5351.Init();
+
+	// Use Si5351 to generate reference frequencies for other PLLs and ADC
+	Si5351.SetPLL(Si5351C::PLL::A, 800000000, Si5351C::PLLSource::XTAL);
+	while(!Si5351.Locked(Si5351C::PLL::A));
+
+	Si5351.SetPLL(Si5351C::PLL::B, 800000000, Si5351C::PLLSource::XTAL);
+	while(!Si5351.Locked(Si5351C::PLL::B));
+
+	extRefInUse = 0;
+	extOutFreq = 0;
+	Si5351.Disable(SiChannel::ReferenceOut);
+
+	// Both MAX2871 get a 100MHz reference
+	Si5351.SetCLK(SiChannel::Source, 100000000, Si5351C::PLL::A, Si5351C::DriveStrength::mA2);
+	Si5351.Enable(SiChannel::Source);
+	Si5351.SetCLK(SiChannel::LO1, 100000000, Si5351C::PLL::A, Si5351C::DriveStrength::mA2);
+	Si5351.Enable(SiChannel::LO1);
+	// 16MHz FPGA clock
+	Si5351.SetCLK(SiChannel::FPGA, 16000000, Si5351C::PLL::A, Si5351C::DriveStrength::mA2);
+	Si5351.Enable(SiChannel::FPGA);
+
+	// Generate second LO with Si5351
+	Si5351.SetCLK(SiChannel::Port1LO2, IF1 - IF2, Si5351C::PLL::B, Si5351C::DriveStrength::mA2);
+	Si5351.Enable(SiChannel::Port1LO2);
+	Si5351.SetCLK(SiChannel::Port2LO2, IF1 - IF2, Si5351C::PLL::B, Si5351C::DriveStrength::mA2);
+	Si5351.Enable(SiChannel::Port2LO2);
+	Si5351.SetCLK(SiChannel::RefLO2, IF1 - IF2, Si5351C::PLL::B, Si5351C::DriveStrength::mA2);
+	Si5351.Enable(SiChannel::RefLO2);
+
+	// PLL reset appears to realign phases of clock signals
+	Si5351.ResetPLL(Si5351C::PLL::B);
+
+	LOG_DEBUG("Si5351 locked");
+
+	// FPGA clock is now present, can initialize
+	if (!FPGA::Init(HaltedCallback)) {
+		LOG_ERR("Aborting due to uninitialized FPGA");
+		return false;
+	}
+
+	// Enable new data and sweep halt interrupt
+	FPGA::EnableInterrupt(FPGA::Interrupt::NewData);
+	FPGA::EnableInterrupt(FPGA::Interrupt::SweepHalted);
+
+	Exti::SetCallback(FPGA_INTR_GPIO_Port, FPGA_INTR_Pin, Exti::EdgeType::Rising, Exti::Pull::Down, FPGA_Interrupt);
+
+	// Initialize PLLs and build VCO maps
+	// enable source synthesizer
+	FPGA::Enable(FPGA::Periphery::SourceChip);
+	FPGA::SetMode(FPGA::Mode::SourcePLL);
+	Source.Init(100000000, false, 1, false);
+	Source.SetPowerOutA(MAX2871::Power::n4dbm);
+	// output B is not used
+	Source.SetPowerOutB(MAX2871::Power::n4dbm, false);
+	if(!Source.BuildVCOMap()) {
+		LOG_WARN("Source VCO map failed");
+	} else {
+		LOG_INFO("Source VCO map complete");
+	}
+	Source.SetFrequency(1000000000);
+	Source.UpdateFrequency();
+	LOG_DEBUG("Source temp: %u", Source.GetTemp());
+	// disable source synthesizer/enable LO synthesizer
+	FPGA::SetMode(FPGA::Mode::FPGA);
+	FPGA::Disable(FPGA::Periphery::SourceChip);
+	FPGA::Enable(FPGA::Periphery::LO1Chip);
+	FPGA::SetMode(FPGA::Mode::LOPLL);
+	LO1.Init(100000000, false, 1, false);
+	LO1.SetPowerOutA(MAX2871::Power::n4dbm);
+	LO1.SetPowerOutB(MAX2871::Power::n4dbm);
+	if(!LO1.BuildVCOMap()) {
+		LOG_WARN("LO1 VCO map failed");
+	} else {
+		LOG_INFO("LO1 VCO map complete");
+	}
+	LO1.SetFrequency(1000000000 + IF1);
+	LO1.UpdateFrequency();
+	LOG_DEBUG("LO temp: %u", LO1.GetTemp());
+
+	FPGA::SetMode(FPGA::Mode::FPGA);
+	// disable both synthesizers
+	FPGA::Disable(FPGA::Periphery::LO1Chip);
+	FPGA::WriteMAX2871Default(Source.GetRegisters());
+
+	LOG_INFO("Initialized");
+	FPGA::Enable(FPGA::Periphery::ReadyLED);
+	return true;
+}
+
+void HW::SetMode(Mode mode) {
+	if(activeMode == mode) {
+		// already the correct mode
+		return;
+	}
+	switch(activeMode) {
+	case Mode::Manual:
+		Manual::Stop();
+		break;
+	case Mode::VNA:
+		VNA::Stop();
+		break;
+	default:
+		break;
+	}
+	if(activeMode == Mode::Manual && mode != Mode::Idle) {
+		// do a full initialization when switching from manual mode to anything else than idle
+		// (making sure that any changes made in manual mode are reverted)
+		HW::Init();
+	}
+	SetIdle();
+	activeMode = mode;
+}
+
+bool HW::GetTemps(uint8_t *source, uint8_t *lo) {
+	FPGA::SetMode(FPGA::Mode::SourcePLL);
+	*source = Source.GetTemp();
+	FPGA::SetMode(FPGA::Mode::LOPLL);
+	*lo = LO1.GetTemp();
+	FPGA::SetMode(FPGA::Mode::FPGA);
+	return true;
+}
+
+void HW::SetIdle() {
+	FPGA::AbortSweep();
+	FPGA::SetMode(FPGA::Mode::FPGA);
+	FPGA::Enable(FPGA::Periphery::SourceChip, false);
+	FPGA::Enable(FPGA::Periphery::SourceRF, false);
+	FPGA::Enable(FPGA::Periphery::LO1Chip, false);
+	FPGA::Enable(FPGA::Periphery::LO1RF, false);
+	FPGA::Enable(FPGA::Periphery::Amplifier, false);
+	FPGA::Enable(FPGA::Periphery::Port1Mixer, false);
+	FPGA::Enable(FPGA::Periphery::Port2Mixer, false);
+	FPGA::Enable(FPGA::Periphery::RefMixer, false);
+}
+
+void HW::fillDeviceInfo(Protocol::DeviceInfo *info) {
+	// read PLL temperatures
+	uint8_t tempSource, tempLO;
+	GetTemps(&tempSource, &tempLO);
+	LOG_INFO("PLL temperatures: %u/%u", tempSource, tempLO);
+	// Read ADC min/max
+	auto limits = FPGA::GetADCLimits();
+	LOG_INFO("ADC limits: P1: %d/%d P2: %d/%d R: %d/%d",
+			limits.P1min, limits.P1max, limits.P2min, limits.P2max,
+			limits.Rmin, limits.Rmax);
+#define ADC_LIMIT 		30000
+	if(limits.P1min < -ADC_LIMIT || limits.P1max > ADC_LIMIT
+			|| limits.P2min < -ADC_LIMIT || limits.P2max > ADC_LIMIT
+			|| limits.Rmin < -ADC_LIMIT || limits.Rmax > ADC_LIMIT) {
+		info->ADC_overload = true;
+	} else {
+		info->ADC_overload = false;
+	}
+	auto status = FPGA::GetStatus();
+	info->LO1_locked = (status & (int) FPGA::Interrupt::LO1Unlock) ? 0 : 1;
+	info->source_locked = (status & (int) FPGA::Interrupt::SourceUnlock) ? 0 : 1;
+	info->extRefAvailable = Ref::available();
+	info->extRefInUse = extRefInUse;
+	info->temperatures.LO1 = tempLO;
+	info->temperatures.source = tempSource;
+	info->temperatures.MCU = 0;
+	FPGA::ResetADCLimits();
+}
+
+bool HW::Ref::available() {
+	return Si5351.ExtCLKAvailable();
+}
+
+void HW::Ref::set(Protocol::ReferenceSettings s) {
+	ref = s;
+}
+
+void HW::Ref::update() {
+	if(extOutFreq != ref.ExtRefOuputFreq) {
+		extOutFreq = ref.ExtRefOuputFreq;
+		if(extOutFreq == 0) {
+			Si5351.Disable(SiChannel::ReferenceOut);
+			LOG_INFO("External reference output disabled");
+		} else {
+			Si5351.SetCLK(SiChannel::ReferenceOut, extOutFreq, Si5351C::PLL::A);
+			Si5351.Enable(SiChannel::ReferenceOut);
+			LOG_INFO("External reference output set to %luHz", extOutFreq);
+		}
+	}
+	bool useExternal = ref.UseExternalRef;
+	if (ref.AutomaticSwitch) {
+		useExternal = Ref::available();
+	}
+	if(useExternal != extRefInUse) {
+		// switch between internal and external reference
+		extRefInUse = useExternal;
+		if(extRefInUse) {
+			if(!Ref::available()) {
+				LOG_WARN("Forced switch to external reference but no signal detected");
+			}
+			Si5351.ConfigureCLKIn(10000000);
+			Si5351.SetPLL(Si5351C::PLL::A, 800000000, Si5351C::PLLSource::CLKIN);
+			Si5351.SetPLL(Si5351C::PLL::B, 800000000, Si5351C::PLLSource::CLKIN);
+			LOG_INFO("Switched to external reference");
+			FPGA::Enable(FPGA::Periphery::ExtRefLED);
+		} else {
+			Si5351.SetPLL(Si5351C::PLL::A, 800000000, Si5351C::PLLSource::XTAL);
+			Si5351.SetPLL(Si5351C::PLL::B, 800000000, Si5351C::PLLSource::XTAL);
+			LOG_INFO("Switched to internal reference");
+			FPGA::Disable(FPGA::Periphery::ExtRefLED);
+		}
+	}
+	constexpr uint32_t lock_timeout = 10;
+	uint32_t start = HAL_GetTick();
+	while(!Si5351.Locked(Si5351C::PLL::A) || !Si5351.Locked(Si5351C::PLL::A)) {
+		if(HAL_GetTick() - start > lock_timeout) {
+			LOG_ERR("Clock distributor PLLs failed to lock");
+			return;
+		}
+	}
+}
diff --git a/Software/VNA_embedded/Application/Hardware.hpp b/Software/VNA_embedded/Application/Hardware.hpp
new file mode 100644
index 0000000..e5cab3a
--- /dev/null
+++ b/Software/VNA_embedded/Application/Hardware.hpp
@@ -0,0 +1,28 @@
+#pragma once
+
+#include <cstdint>
+#include "Protocol.hpp"
+
+namespace HW {
+
+enum class Mode {
+	Idle,
+	Manual,
+	VNA,
+	SA,
+};
+
+bool Init();
+void SetMode(Mode mode);
+void SetIdle();
+
+bool GetTemps(uint8_t *source, uint8_t *lo);
+void fillDeviceInfo(Protocol::DeviceInfo *info);
+namespace Ref {
+	bool available();
+	// reference won't change until update is called
+	void set(Protocol::ReferenceSettings s);
+	void update();
+}
+
+}
diff --git a/Software/VNA_embedded/Application/Manual.cpp b/Software/VNA_embedded/Application/Manual.cpp
new file mode 100644
index 0000000..6d0890e
--- /dev/null
+++ b/Software/VNA_embedded/Application/Manual.cpp
@@ -0,0 +1,128 @@
+#include "Manual.hpp"
+#include "HW_HAL.hpp"
+#include "Hardware.hpp"
+#include "Communication.h"
+#include <cstring>
+
+static bool active = false;
+static uint32_t samples;
+static Protocol::ManualStatus status;
+
+using namespace HWHAL;
+
+void Manual::Setup(Protocol::ManualControl m) {
+	HW::SetMode(HW::Mode::Manual);
+	samples = m.Samples;
+	FPGA::AbortSweep();
+	// Configure lowband source
+	if (m.SourceLowEN) {
+		Si5351.SetCLK(SiChannel::LowbandSource, m.SourceLowFrequency, Si5351C::PLL::B,
+				(Si5351C::DriveStrength) m.SourceLowPower);
+		Si5351.Enable(SiChannel::LowbandSource);
+	} else {
+		Si5351.Disable(SiChannel::LowbandSource);
+	}
+	// Configure highband source
+	Source.SetFrequency(m.SourceHighFrequency);
+	Source.SetPowerOutA((MAX2871::Power) m.SourceHighPower);
+
+	// Configure LO1
+	LO1.SetFrequency(m.LO1Frequency);
+
+	// Configure LO2
+	if(m.LO2EN) {
+		// Generate second LO with Si5351
+		Si5351.SetCLK(SiChannel::Port1LO2, m.LO2Frequency, Si5351C::PLL::B, Si5351C::DriveStrength::mA2);
+		Si5351.Enable(SiChannel::Port1LO2);
+		Si5351.SetCLK(SiChannel::Port2LO2, m.LO2Frequency, Si5351C::PLL::B, Si5351C::DriveStrength::mA2);
+		Si5351.Enable(SiChannel::Port2LO2);
+		Si5351.SetCLK(SiChannel::RefLO2, m.LO2Frequency, Si5351C::PLL::B, Si5351C::DriveStrength::mA2);
+		Si5351.Enable(SiChannel::RefLO2);
+
+		// PLL reset appears to realign phases of clock signals
+		Si5351.ResetPLL(Si5351C::PLL::B);
+	} else {
+		Si5351.Disable(SiChannel::Port1LO2);
+		Si5351.Disable(SiChannel::Port2LO2);
+		Si5351.Disable(SiChannel::RefLO2);
+	}
+
+	FPGA::WriteMAX2871Default(Source.GetRegisters());
+
+	FPGA::SetNumberOfPoints(1);
+	FPGA::SetSamplesPerPoint(m.Samples);
+
+	// Configure single sweep point
+	FPGA::WriteSweepConfig(0, !m.SourceHighband, Source.GetRegisters(),
+			LO1.GetRegisters(), m.attenuator, 0, FPGA::SettlingTime::us20,
+			FPGA::Samples::SPPRegister, 0,
+			(FPGA::LowpassFilter) m.SourceHighLowpass);
+
+	FPGA::SetWindow((FPGA::Window) m.WindowType);
+
+	// Enable/Disable periphery
+	FPGA::Enable(FPGA::Periphery::SourceChip, m.SourceHighCE);
+	FPGA::Enable(FPGA::Periphery::SourceRF, m.SourceHighRFEN);
+	FPGA::Enable(FPGA::Periphery::LO1Chip, m.LO1CE);
+	FPGA::Enable(FPGA::Periphery::LO1RF, m.LO1RFEN);
+	FPGA::Enable(FPGA::Periphery::Amplifier, m.AmplifierEN);
+	FPGA::Enable(FPGA::Periphery::Port1Mixer, m.Port1EN);
+	FPGA::Enable(FPGA::Periphery::Port2Mixer, m.Port2EN);
+	FPGA::Enable(FPGA::Periphery::RefMixer, m.RefEN);
+	FPGA::Enable(FPGA::Periphery::ExcitePort1, m.PortSwitch == 0);
+	FPGA::Enable(FPGA::Periphery::ExcitePort2, m.PortSwitch == 1);
+
+	active = true;
+	FPGA::StartSweep();
+}
+
+bool Manual::MeasurementDone(FPGA::SamplingResult result) {
+	if(!active) {
+		return false;
+	}
+	// save measurement
+	status.port1real = (float) result.P1I / samples;
+	status.port1imag = (float) result.P1Q / samples;
+	status.port2real = (float) result.P2I / samples;
+	status.port2imag = (float) result.P2Q / samples;
+	status.refreal = (float) result.RefI / samples;
+	status.refimag = (float) result.RefQ / samples;
+	return true;
+}
+
+
+void Manual::Work() {
+	if(!active) {
+		return;
+	}
+	Protocol::PacketInfo p;
+	p.type = Protocol::PacketType::Status;
+	uint16_t isr_flags = FPGA::GetStatus();
+	if (!(isr_flags & 0x0002)) {
+		p.status.source_locked = 1;
+	} else {
+		p.status.source_locked = 0;
+	}
+	if (!(isr_flags & 0x0001)) {
+		p.status.LO_locked = 1;
+	} else {
+		p.status.LO_locked = 0;
+	}
+	auto limits = FPGA::GetADCLimits();
+	FPGA::ResetADCLimits();
+	p.status.port1min = limits.P1min;
+	p.status.port1max = limits.P1max;
+	p.status.port2min = limits.P2min;
+	p.status.port2max = limits.P2max;
+	p.status.refmin = limits.Rmin;
+	p.status.refmax = limits.Rmax;
+	HW::GetTemps(&p.status.temp_source, &p.status.temp_LO);
+	Communication::Send(p);
+	// Trigger next status update
+	FPGA::StartSweep();
+}
+
+void Manual::Stop() {
+	active = false;
+}
+
diff --git a/Software/VNA_embedded/Application/Manual.hpp b/Software/VNA_embedded/Application/Manual.hpp
new file mode 100644
index 0000000..ab3f39d
--- /dev/null
+++ b/Software/VNA_embedded/Application/Manual.hpp
@@ -0,0 +1,13 @@
+#pragma once
+
+#include "FPGA/FPGA.hpp"
+#include "Protocol.hpp"
+
+namespace Manual {
+
+void Setup(Protocol::ManualControl m);
+bool MeasurementDone(FPGA::SamplingResult result);
+void Work();
+void Stop();
+
+}
diff --git a/Software/VNA_embedded/Application/VNA.cpp b/Software/VNA_embedded/Application/VNA.cpp
index a013816..6690e6a 100644
--- a/Software/VNA_embedded/Application/VNA.cpp
+++ b/Software/VNA_embedded/Application/VNA.cpp
@@ -1,3 +1,4 @@
+#include <HW_HAL.hpp>
 #include <VNA.hpp>
 #include "Si5351C.hpp"
 #include "max2871.hpp"
@@ -6,7 +7,8 @@
 #include "FPGA/FPGA.hpp"
 #include <complex>
 #include "Exti.hpp"
-#include "VNA_HAL.hpp"
+#include "Hardware.hpp"
+#include "Communication.h"
 
 #define LOG_LEVEL	LOG_LEVEL_INFO
 #define LOG_MODULE	"VNA"
@@ -17,12 +19,11 @@ static constexpr uint32_t IF1_alternate = 57000000;
 static constexpr uint32_t IF2 = 250000;
 
 static VNA::SweepCallback sweepCallback;
-static VNA::StatusCallback statusCallback;
 static Protocol::SweepSettings settings;
 static uint16_t pointCnt;
 static bool excitingPort1;
 static Protocol::Datapoint data;
-static bool manualMode = false;
+static bool active = false;
 
 using IFTableEntry = struct {
 	uint16_t pointCnt;
@@ -36,208 +37,21 @@ static uint16_t IFTableIndexCnt = 0;
 
 static constexpr uint32_t BandSwitchFrequency = 25000000;
 
-static uint32_t extOutFreq = 0;
-static bool extRefInUse = false;
+using namespace HWHAL;
 
-using namespace VNAHAL;
-
-static void HaltedCallback() {
-	LOG_DEBUG("Halted before point %d", pointCnt);
-	// Check if IF table has entry at this point
-//	if (IFTable[IFTableIndexCnt].pointCnt == pointCnt) {
-//		LOG_DEBUG("Shifting IF to %lu at point %u",
-//				IFTable[IFTableIndexCnt].IF1, pointCnt);
-//		Si5351.WriteRawCLKConfig(1, IFTable[IFTableIndexCnt].clkconfig);
-//		Si5351.WriteRawCLKConfig(4, IFTable[IFTableIndexCnt].clkconfig);
-//		Si5351.WriteRawCLKConfig(5, IFTable[IFTableIndexCnt].clkconfig);
-//		Si5351.ResetPLL(Si5351C::PLL::B);
-//		IFTableIndexCnt++;
-//	}
-	uint64_t frequency = settings.f_start
-			+ (settings.f_stop - settings.f_start) * pointCnt
-					/ (settings.points - 1);
-	if (frequency < BandSwitchFrequency) {
-		// need the Si5351 as Source
-		Si5351.SetCLK(SiChannel::LowbandSource, frequency, Si5351C::PLL::B,
-				Si5351C::DriveStrength::mA2);
-		if (pointCnt == 0) {
-			// First point in sweep, enable CLK
-			Si5351.Enable(SiChannel::LowbandSource);
-			FPGA::Disable(FPGA::Periphery::SourceRF);
-		}
-	} else {
-		// first sweep point in highband is also halted, disable lowband source
-		Si5351.Disable(SiChannel::LowbandSource);
-		FPGA::Enable(FPGA::Periphery::SourceRF);
-	}
-
-	FPGA::ResumeHaltedSweep();
-}
-
-static void ReadComplete(FPGA::SamplingResult result) {
-	if(!manualMode) {
-		// normal sweep mode
-		auto port1_raw = std::complex<float>(result.P1I, result.P1Q);
-		auto port2_raw = std::complex<float>(result.P2I, result.P2Q);
-		auto ref = std::complex<float>(result.RefI, result.RefQ);
-		auto port1 = port1_raw / ref;
-		auto port2 = port2_raw / ref;
-		data.pointNum = pointCnt;
-		data.frequency = settings.f_start + (settings.f_stop - settings.f_start) * pointCnt / (settings.points - 1);
-		if(excitingPort1) {
-			data.real_S11 = port1.real();
-			data.imag_S11 = port1.imag();
-			data.real_S21 = port2.real();
-			data.imag_S21 = port2.imag();
-		} else {
-			data.real_S12 = port1.real();
-			data.imag_S12 = port1.imag();
-			data.real_S22 = port2.real();
-			data.imag_S22 = port2.imag();
-		}
-		// figure out whether this sweep point is complete and which port gets excited next
-		bool pointComplete = false;
-		if(settings.excitePort1 == 1 && settings.excitePort2 == 1) {
-			// point is complete when port 2 was active
-			pointComplete = !excitingPort1;
-			// next measurement will be from other port
-			excitingPort1 = !excitingPort1;
-		} else {
-			// only one port active, point is complete after every measurement
-			pointComplete = true;
-		}
-		if(pointComplete) {
-			if (sweepCallback) {
-				sweepCallback(data);
-			}
-			pointCnt++;
-			if (pointCnt >= settings.points) {
-				// reached end of sweep, start again
-				pointCnt = 0;
-				IFTableIndexCnt = 0;
-			}
-		}
-	} else {
-		// Manual control mode, simply pass on raw result
-		if(statusCallback) {
-			statusCallback(result);
-		}
-	}
-}
-
-static void FPGA_Interrupt(void*) {
-	FPGA::InitiateSampleRead(ReadComplete);
-}
-
-bool VNA::Init() {
-	LOG_DEBUG("Initializing...");
-
-	manualMode = false;
-
-	Si5351.Init();
-
-	// Use Si5351 to generate reference frequencies for other PLLs and ADC
-	Si5351.SetPLL(Si5351C::PLL::A, 800000000, Si5351C::PLLSource::XTAL);
-	while(!Si5351.Locked(Si5351C::PLL::A));
-
-	Si5351.SetPLL(Si5351C::PLL::B, 800000000, Si5351C::PLLSource::XTAL);
-	while(!Si5351.Locked(Si5351C::PLL::B));
-
-	extRefInUse = 0;
-	extOutFreq = 0;
-	Si5351.Disable(SiChannel::ReferenceOut);
-
-	// Both MAX2871 get a 100MHz reference
-	Si5351.SetCLK(SiChannel::Source, 100000000, Si5351C::PLL::A, Si5351C::DriveStrength::mA2);
-	Si5351.Enable(SiChannel::Source);
-	Si5351.SetCLK(SiChannel::LO1, 100000000, Si5351C::PLL::A, Si5351C::DriveStrength::mA2);
-	Si5351.Enable(SiChannel::LO1);
-	// 16MHz FPGA clock
-	Si5351.SetCLK(SiChannel::FPGA, 16000000, Si5351C::PLL::A, Si5351C::DriveStrength::mA2);
-	Si5351.Enable(SiChannel::FPGA);
-
-	// Generate second LO with Si5351
-	Si5351.SetCLK(SiChannel::Port1LO2, IF1 - IF2, Si5351C::PLL::B, Si5351C::DriveStrength::mA2);
-	Si5351.Enable(SiChannel::Port1LO2);
-	Si5351.SetCLK(SiChannel::Port2LO2, IF1 - IF2, Si5351C::PLL::B, Si5351C::DriveStrength::mA2);
-	Si5351.Enable(SiChannel::Port2LO2);
-	Si5351.SetCLK(SiChannel::RefLO2, IF1 - IF2, Si5351C::PLL::B, Si5351C::DriveStrength::mA2);
-	Si5351.Enable(SiChannel::RefLO2);
-
-	// PLL reset appears to realign phases of clock signals
-	Si5351.ResetPLL(Si5351C::PLL::B);
-
-	LOG_DEBUG("Si5351 locked");
-
-	// FPGA clock is now present, can initialize
-	if (!FPGA::Init(HaltedCallback)) {
-		LOG_ERR("Aborting due to uninitialized FPGA");
-		return false;
-	}
-
-	// Enable new data and sweep halt interrupt
-	FPGA::EnableInterrupt(FPGA::Interrupt::NewData);
-	FPGA::EnableInterrupt(FPGA::Interrupt::SweepHalted);
-
-	Exti::SetCallback(FPGA_INTR_GPIO_Port, FPGA_INTR_Pin, Exti::EdgeType::Rising, Exti::Pull::Down, FPGA_Interrupt);
-
-	// Initialize PLLs and build VCO maps
-	// enable source synthesizer
-	FPGA::Enable(FPGA::Periphery::SourceChip);
-	FPGA::SetMode(FPGA::Mode::SourcePLL);
-	Source.Init(100000000, false, 1, false);
-	Source.SetPowerOutA(MAX2871::Power::n4dbm);
-	// output B is not used
-	Source.SetPowerOutB(MAX2871::Power::n4dbm, false);
-	if(!Source.BuildVCOMap()) {
-		LOG_WARN("Source VCO map failed");
-	} else {
-		LOG_INFO("Source VCO map complete");
-	}
-	Source.SetFrequency(1000000000);
-	Source.UpdateFrequency();
-	LOG_DEBUG("Source temp: %u", Source.GetTemp());
-	// disable source synthesizer/enable LO synthesizer
-	FPGA::SetMode(FPGA::Mode::FPGA);
-	FPGA::Disable(FPGA::Periphery::SourceChip);
-	FPGA::Enable(FPGA::Periphery::LO1Chip);
-	FPGA::SetMode(FPGA::Mode::LOPLL);
-	LO1.Init(100000000, false, 1, false);
-	LO1.SetPowerOutA(MAX2871::Power::n4dbm);
-	LO1.SetPowerOutB(MAX2871::Power::n4dbm);
-	if(!LO1.BuildVCOMap()) {
-		LOG_WARN("LO1 VCO map failed");
-	} else {
-		LOG_INFO("LO1 VCO map complete");
-	}
-	LO1.SetFrequency(1000000000 + IF1);
-	LO1.UpdateFrequency();
-	LOG_DEBUG("LO temp: %u", LO1.GetTemp());
-
-	FPGA::SetMode(FPGA::Mode::FPGA);
-	// disable both synthesizers
-	FPGA::Disable(FPGA::Periphery::LO1Chip);
-	FPGA::WriteMAX2871Default(Source.GetRegisters());
-
-	LOG_INFO("Initialized");
-	FPGA::Enable(FPGA::Periphery::ReadyLED);
-	return true;
-}
-
-bool VNA::ConfigureSweep(Protocol::SweepSettings s, SweepCallback cb) {
-	if (manualMode) {
-		// was used in manual mode last, do full initialization before starting sweep
-		VNA::Init();
-	}
+bool VNA::Setup(Protocol::SweepSettings s, SweepCallback cb) {
+	HW::SetMode(HW::Mode::VNA);
 	if(s.excitePort1 == 0 && s.excitePort2 == 0) {
-		// both ports disabled, set to idle
-		SetIdle();
+		// both ports disabled, nothing to do
+		HW::SetIdle();
+		active = false;
 		return false;
 	}
 	sweepCallback = cb;
 	settings = s;
 	// Abort possible active sweep first
 	FPGA::AbortSweep();
+	FPGA::SetMode(FPGA::Mode::FPGA);
 	uint16_t points = settings.points <= FPGA::MaxPoints ? settings.points : FPGA::MaxPoints;
 	// Configure sweep
 	FPGA::SetNumberOfPoints(points);
@@ -264,7 +78,7 @@ bool VNA::ConfigureSweep(Protocol::SweepSettings s, SweepCallback cb) {
 
 	// Transfer PLL configuration to FPGA
 	for (uint16_t i = 0; i < points; i++) {
-		uint64_t freq = s.f_start + (s.f_stop - s.f_start) * i / (s.points - 1);
+		uint64_t freq = s.f_start + (s.f_stop - s.f_start) * i / (points - 1);
 		// SetFrequency only manipulates the register content in RAM, no SPI communication is done.
 		// No mode-switch of FPGA necessary here.
 
@@ -355,251 +169,116 @@ bool VNA::ConfigureSweep(Protocol::SweepSettings s, SweepCallback cb) {
 	// starting port depends on whether port 1 is active in sweep
 	excitingPort1 = s.excitePort1;
 	IFTableIndexCnt = 0;
+	active = true;
 	// Start the sweep
 	FPGA::StartSweep();
 	return true;
 }
 
-bool VNA::ConfigureManual(Protocol::ManualControl m, StatusCallback cb) {
-	manualMode = true;
-	statusCallback = cb;
-	FPGA::AbortSweep();
-	// Configure lowband source
-	if (m.SourceLowEN) {
-		Si5351.SetCLK(SiChannel::LowbandSource, m.SourceLowFrequency, Si5351C::PLL::B,
-				(Si5351C::DriveStrength) m.SourceLowPower);
-		Si5351.Enable(SiChannel::LowbandSource);
-	} else {
-		Si5351.Disable(SiChannel::LowbandSource);
+bool VNA::MeasurementDone(FPGA::SamplingResult result) {
+	if(!active) {
+		return false;
 	}
-	// Configure highband source
-	Source.SetFrequency(m.SourceHighFrequency);
-	Source.SetPowerOutA((MAX2871::Power) m.SourceHighPower);
-
-	// Configure LO1
-	LO1.SetFrequency(m.LO1Frequency);
-
-	// Configure LO2
-	if(m.LO2EN) {
-		// Generate second LO with Si5351
-		Si5351.SetCLK(SiChannel::Port1LO2, m.LO2Frequency, Si5351C::PLL::B, Si5351C::DriveStrength::mA2);
-		Si5351.Enable(SiChannel::Port1LO2);
-		Si5351.SetCLK(SiChannel::Port2LO2, m.LO2Frequency, Si5351C::PLL::B, Si5351C::DriveStrength::mA2);
-		Si5351.Enable(SiChannel::Port2LO2);
-		Si5351.SetCLK(SiChannel::RefLO2, m.LO2Frequency, Si5351C::PLL::B, Si5351C::DriveStrength::mA2);
-		Si5351.Enable(SiChannel::RefLO2);
-
-		// PLL reset appears to realign phases of clock signals
-		Si5351.ResetPLL(Si5351C::PLL::B);
+	// normal sweep mode
+	auto port1_raw = std::complex<float>(result.P1I, result.P1Q);
+	auto port2_raw = std::complex<float>(result.P2I, result.P2Q);
+	auto ref = std::complex<float>(result.RefI, result.RefQ);
+	auto port1 = port1_raw / ref;
+	auto port2 = port2_raw / ref;
+	data.pointNum = pointCnt;
+	data.frequency = settings.f_start + (settings.f_stop - settings.f_start) * pointCnt / (settings.points - 1);
+	if(excitingPort1) {
+		data.real_S11 = port1.real();
+		data.imag_S11 = port1.imag();
+		data.real_S21 = port2.real();
+		data.imag_S21 = port2.imag();
 	} else {
-		Si5351.Disable(SiChannel::Port1LO2);
-		Si5351.Disable(SiChannel::Port2LO2);
-		Si5351.Disable(SiChannel::RefLO2);
+		data.real_S12 = port1.real();
+		data.imag_S12 = port1.imag();
+		data.real_S22 = port2.real();
+		data.imag_S22 = port2.imag();
 	}
-
-	FPGA::WriteMAX2871Default(Source.GetRegisters());
-
-	FPGA::SetNumberOfPoints(1);
-	FPGA::SetSamplesPerPoint(m.Samples);
-
-	// Configure single sweep point
-	FPGA::WriteSweepConfig(0, !m.SourceHighband, Source.GetRegisters(),
-			LO1.GetRegisters(), m.attenuator, 0, FPGA::SettlingTime::us20,
-			FPGA::Samples::SPPRegister, 0,
-			(FPGA::LowpassFilter) m.SourceHighLowpass);
-
-	FPGA::SetWindow((FPGA::Window) m.WindowType);
-
-	// Enable/Disable periphery
-	FPGA::Enable(FPGA::Periphery::SourceChip, m.SourceHighCE);
-	FPGA::Enable(FPGA::Periphery::SourceRF, m.SourceHighRFEN);
-	FPGA::Enable(FPGA::Periphery::LO1Chip, m.LO1CE);
-	FPGA::Enable(FPGA::Periphery::LO1RF, m.LO1RFEN);
-	FPGA::Enable(FPGA::Periphery::Amplifier, m.AmplifierEN);
-	FPGA::Enable(FPGA::Periphery::Port1Mixer, m.Port1EN);
-	FPGA::Enable(FPGA::Periphery::Port2Mixer, m.Port2EN);
-	FPGA::Enable(FPGA::Periphery::RefMixer, m.RefEN);
-	FPGA::Enable(FPGA::Periphery::ExcitePort1, m.PortSwitch == 0);
-	FPGA::Enable(FPGA::Periphery::ExcitePort2, m.PortSwitch == 1);
-
-	FPGA::StartSweep();
-	return true;
-}
-
-bool VNA::GetTemps(uint8_t *source, uint8_t *lo) {
-	FPGA::SetMode(FPGA::Mode::SourcePLL);
-	*source = Source.GetTemp();
-	FPGA::SetMode(FPGA::Mode::LOPLL);
-	*lo = LO1.GetTemp();
-	FPGA::SetMode(FPGA::Mode::FPGA);
-	return true;
-}
-
-void VNA::fillDeviceInfo(Protocol::DeviceInfo *info) {
-	// read PLL temperatures
-	uint8_t tempSource, tempLO;
-	VNA::GetTemps(&tempSource, &tempLO);
-	LOG_INFO("PLL temperatures: %u/%u", tempSource, tempLO);
-	// Read ADC min/max
-	auto limits = FPGA::GetADCLimits();
-	LOG_INFO("ADC limits: P1: %d/%d P2: %d/%d R: %d/%d",
-			limits.P1min, limits.P1max, limits.P2min, limits.P2max,
-			limits.Rmin, limits.Rmax);
-#define ADC_LIMIT 		30000
-	// Set VNA related member of info struct
-	if(limits.P1min < -ADC_LIMIT || limits.P1max > ADC_LIMIT
-			|| limits.P2min < -ADC_LIMIT || limits.P2max > ADC_LIMIT
-			|| limits.Rmin < -ADC_LIMIT || limits.Rmax > ADC_LIMIT) {
-		info->ADC_overload = true;
+	// figure out whether this sweep point is complete and which port gets excited next
+	bool pointComplete = false;
+	if(settings.excitePort1 == 1 && settings.excitePort2 == 1) {
+		// point is complete when port 2 was active
+		pointComplete = !excitingPort1;
+		// next measurement will be from other port
+		excitingPort1 = !excitingPort1;
 	} else {
-		info->ADC_overload = false;
+		// only one port active, point is complete after every measurement
+		pointComplete = true;
 	}
-	auto status = FPGA::GetStatus();
-	info->LO1_locked = (status & (int) FPGA::Interrupt::LO1Unlock) ? 0 : 1;
-	info->source_locked = (status & (int) FPGA::Interrupt::SourceUnlock) ? 0 : 1;
-	info->extRefAvailable = Ref::available();
-	info->extRefInUse = extRefInUse;
-	info->temperatures.LO1 = tempLO;
-	info->temperatures.source = tempSource;
-	info->temperatures.MCU = 0;
-	FPGA::ResetADCLimits();
-}
-
-bool VNA::Ref::available() {
-	return Si5351.ExtCLKAvailable();
-}
-
-bool VNA::Ref::applySettings(Protocol::ReferenceSettings s) {
-	if(extOutFreq != s.ExtRefOuputFreq) {
-		extOutFreq = s.ExtRefOuputFreq;
-		if(extOutFreq == 0) {
-			Si5351.Disable(SiChannel::ReferenceOut);
-			LOG_INFO("External reference output disabled");
-		} else {
-			Si5351.SetCLK(SiChannel::ReferenceOut, extOutFreq, Si5351C::PLL::A);
-			Si5351.Enable(SiChannel::ReferenceOut);
-			LOG_INFO("External reference output set to %luHz", extOutFreq);
+	if(pointComplete) {
+		if (sweepCallback) {
+			sweepCallback(data);
 		}
-	}
-	bool useExternal = s.UseExternalRef;
-	if (s.AutomaticSwitch) {
-		useExternal = Ref::available();
-	}
-	if(useExternal != extRefInUse) {
-		// switch between internal and external reference
-		extRefInUse = useExternal;
-		if(extRefInUse) {
-			if(!Ref::available()) {
-				LOG_WARN("Forced switch to external reference but no signal detected");
-			}
-			Si5351.ConfigureCLKIn(10000000);
-			Si5351.SetPLL(Si5351C::PLL::A, 800000000, Si5351C::PLLSource::CLKIN);
-			Si5351.SetPLL(Si5351C::PLL::B, 800000000, Si5351C::PLLSource::CLKIN);
-			LOG_INFO("Switched to external reference");
-			FPGA::Enable(FPGA::Periphery::ExtRefLED);
-		} else {
-			Si5351.SetPLL(Si5351C::PLL::A, 800000000, Si5351C::PLLSource::XTAL);
-			Si5351.SetPLL(Si5351C::PLL::B, 800000000, Si5351C::PLLSource::XTAL);
-			LOG_INFO("Switched to internal reference");
-			FPGA::Disable(FPGA::Periphery::ExtRefLED);
-		}
-	}
-	constexpr uint32_t lock_timeout = 10;
-	uint32_t start = HAL_GetTick();
-	while(!Si5351.Locked(Si5351C::PLL::A) || !Si5351.Locked(Si5351C::PLL::A)) {
-		if(HAL_GetTick() - start > lock_timeout) {
-			LOG_ERR("Clock distributor PLLs failed to lock");
-			return false;
+		pointCnt++;
+		if (pointCnt >= settings.points) {
+			// reached end of sweep, start again
+			pointCnt = 0;
+			IFTableIndexCnt = 0;
+			// request to trigger work function
+			return true;
 		}
 	}
 	return false;
 }
 
-bool VNA::ConfigureGenerator(Protocol::GeneratorSettings g) {
-	if(g.activePort == 0) {
-		// both ports disabled, no need to configure PLLs
-		SetIdle();
-		return true;
-	}
-	Protocol::ManualControl m;
-	// LOs not required
-	m.LO1CE = 0;
-	m.LO1Frequency = 1000000000;
-	m.LO1RFEN = 0;
-	m.LO1RFEN = 0;
-	m.LO2EN = 0;
-	m.LO2Frequency = 60000000;
-	m.Port1EN = 0;
-	m.Port2EN = 0;
-	m.RefEN = 0;
-	m.Samples = 131072;
-	m.WindowType = (int) FPGA::Window::None;
-	// Select correct source
-	if(g.frequency < BandSwitchFrequency) {
-		m.SourceLowEN = 1;
-		m.SourceLowFrequency = g.frequency;
-		m.SourceHighCE = 0;
-		m.SourceHighRFEN = 0;
-		m.SourceHighFrequency = BandSwitchFrequency;
-		m.SourceHighLowpass = (int) FPGA::LowpassFilter::M947;
-		m.SourceHighPower = (int) MAX2871::Power::n4dbm;
-		m.SourceHighband = false;
-	} else {
-		m.SourceLowEN = 0;
-		m.SourceLowFrequency = BandSwitchFrequency;
-		m.SourceHighCE = 1;
-		m.SourceHighRFEN = 1;
-		m.SourceHighFrequency = g.frequency;
-		if(g.frequency < 900000000UL) {
-			m.SourceHighLowpass = (int) FPGA::LowpassFilter::M947;
-		} else if(g.frequency < 1800000000UL) {
-			m.SourceHighLowpass = (int) FPGA::LowpassFilter::M1880;
-		} else if(g.frequency < 3500000000UL) {
-			m.SourceHighLowpass = (int) FPGA::LowpassFilter::M3500;
-		} else {
-			m.SourceHighLowpass = (int) FPGA::LowpassFilter::None;
-		}
-		m.SourceHighband = true;
-	}
-	switch(g.activePort) {
-	case 1:
-		m.AmplifierEN = 1;
-		m.PortSwitch = 0;
-		break;
-	case 2:
-		m.AmplifierEN = 1;
-		m.PortSwitch = 1;
-		break;
-	}
-	// Set level (not very accurate)
-	if(g.cdbm_level > -1000) {
-		// use higher source power (approx 0dbm with no attenuation)
-		m.SourceHighPower = (int) MAX2871::Power::p5dbm;
-		m.SourceLowPower = (int) Si5351C::DriveStrength::mA8;
-	} else {
-		// use lower source power (approx -10dbm with no attenuation)
-		m.SourceHighPower = (int) MAX2871::Power::n4dbm;
-		m.SourceLowPower = (int) Si5351C::DriveStrength::mA2;
-		g.cdbm_level += 1000;
-	}
-	// calculate required attenuation
-	uint16_t attval = -g.cdbm_level / 25;
-	if(attval > 127) {
-		attval = 127;
-	}
-	m.attenuator = attval;
-	return ConfigureManual(m, nullptr);
+void VNA::Work() {
+	// end of sweep
+	HW::Ref::update();
+	// Compile info packet
+	Protocol::PacketInfo packet;
+	packet.type = Protocol::PacketType::DeviceInfo;
+	packet.info.FPGA_configured = 1;
+	packet.info.FW_major = FW_MAJOR;
+	packet.info.FW_minor = FW_MINOR;
+	packet.info.HW_Revision = HW_REVISION;
+	HW::fillDeviceInfo(&packet.info);
+	Communication::Send(packet);
+	FPGA::ResetADCLimits();
+	// Start next sweep
+	FPGA::StartSweep();
 }
 
-void VNA::SetIdle() {
-	FPGA::AbortSweep();
-	FPGA::SetMode(FPGA::Mode::FPGA);
-	FPGA::Enable(FPGA::Periphery::SourceChip, false);
-	FPGA::Enable(FPGA::Periphery::SourceRF, false);
-	FPGA::Enable(FPGA::Periphery::LO1Chip, false);
-	FPGA::Enable(FPGA::Periphery::LO1RF, false);
-	FPGA::Enable(FPGA::Periphery::Amplifier, false);
-	FPGA::Enable(FPGA::Periphery::Port1Mixer, false);
-	FPGA::Enable(FPGA::Periphery::Port2Mixer, false);
-	FPGA::Enable(FPGA::Periphery::RefMixer, false);
+void VNA::SweepHalted() {
+	if(!active) {
+		return;
+	}
+	LOG_DEBUG("Halted before point %d", pointCnt);
+	// Check if IF table has entry at this point
+//	if (IFTable[IFTableIndexCnt].pointCnt == pointCnt) {
+//		LOG_DEBUG("Shifting IF to %lu at point %u",
+//				IFTable[IFTableIndexCnt].IF1, pointCnt);
+//		Si5351.WriteRawCLKConfig(1, IFTable[IFTableIndexCnt].clkconfig);
+//		Si5351.WriteRawCLKConfig(4, IFTable[IFTableIndexCnt].clkconfig);
+//		Si5351.WriteRawCLKConfig(5, IFTable[IFTableIndexCnt].clkconfig);
+//		Si5351.ResetPLL(Si5351C::PLL::B);
+//		IFTableIndexCnt++;
+//	}
+	uint64_t frequency = settings.f_start
+			+ (settings.f_stop - settings.f_start) * pointCnt
+					/ (settings.points - 1);
+	if (frequency < BandSwitchFrequency) {
+		// need the Si5351 as Source
+		Si5351.SetCLK(SiChannel::LowbandSource, frequency, Si5351C::PLL::B,
+				Si5351C::DriveStrength::mA2);
+		if (pointCnt == 0) {
+			// First point in sweep, enable CLK
+			Si5351.Enable(SiChannel::LowbandSource);
+			FPGA::Disable(FPGA::Periphery::SourceRF);
+		}
+	} else {
+		// first sweep point in highband is also halted, disable lowband source
+		Si5351.Disable(SiChannel::LowbandSource);
+		FPGA::Enable(FPGA::Periphery::SourceRF);
+	}
+
+	FPGA::ResumeHaltedSweep();
+}
+
+void VNA::Stop() {
+	active = false;
+	FPGA::AbortSweep();
 }
diff --git a/Software/VNA_embedded/Application/VNA.hpp b/Software/VNA_embedded/Application/VNA.hpp
index d337c1b..9b1ba57 100644
--- a/Software/VNA_embedded/Application/VNA.hpp
+++ b/Software/VNA_embedded/Application/VNA.hpp
@@ -7,22 +7,12 @@
 namespace VNA {
 
 using SweepCallback = void(*)(Protocol::Datapoint);
-using StatusCallback = void(*)(FPGA::SamplingResult);
 
-bool Init();
-// returns whether the sweep is actually started
-bool ConfigureSweep(Protocol::SweepSettings s, SweepCallback cb);
-bool ConfigureManual(Protocol::ManualControl m, StatusCallback cb);
-bool ConfigureGenerator(Protocol::GeneratorSettings g);
-void SetIdle();
-
-// Only call the following function when the sweep is inactive
-bool GetTemps(uint8_t *source, uint8_t *lo);
-void fillDeviceInfo(Protocol::DeviceInfo *info);
-namespace Ref {
-	bool available();
-	bool applySettings(Protocol::ReferenceSettings s);
-}
+bool Setup(Protocol::SweepSettings s, SweepCallback cb);
+bool MeasurementDone(FPGA::SamplingResult result);
+void Work();
+void SweepHalted();
+void Stop();
 
 }