diff --git a/firmware/ice40-riscv/icE1usb/Makefile b/firmware/ice40-riscv/icE1usb/Makefile
index a6e9138..25c65d1 100644
--- a/firmware/ice40-riscv/icE1usb/Makefile
+++ b/firmware/ice40-riscv/icE1usb/Makefile
@@ -49,6 +49,7 @@
 	config.h \
 	e1.h \
 	gps.h \
+	gpsdo.h \
 	ice1usb_proto.h \
 	misc.h \
 	usb_desc_ids.h \
@@ -62,6 +63,7 @@
 	e1.c \
 	fw_app.c \
 	gps.c \
+	gpsdo.c \
 	misc.c \
 	usb_desc_app.c \
 	usb_dev.c \
diff --git a/firmware/ice40-riscv/icE1usb/fw_app.c b/firmware/ice40-riscv/icE1usb/fw_app.c
index 78da8b6..c1fe295 100644
--- a/firmware/ice40-riscv/icE1usb/fw_app.c
+++ b/firmware/ice40-riscv/icE1usb/fw_app.c
@@ -15,6 +15,7 @@
 #include "console.h"
 #include "e1.h"
 #include "gps.h"
+#include "gpsdo.h"
 #include "led.h"
 #include "misc.h"
 #include "mini-printf.h"
@@ -94,12 +95,13 @@
 	pdm_set(PDM_E1_RX1,  true, 128 + d, false);
 #endif
 
-	/* Setup clock tuning */
-	pdm_set(PDM_CLK_HI, true, 2048, false);
-	pdm_set(PDM_CLK_LO, false,   0, false);
-
 	/* GPS init */
 	gps_init();
+#if defined(BOARD_ICE1USB_PROTO_ICEBREAKER) || defined(BOARD_ICE1USB_PROTO_BITSY)
+	gpsdo_init(VCTXO_TAITIEN_VT40);
+#else
+	gpsdo_init(VCTXO_SITIME_SIT3808_E);
+#endif
 
 	/* Enable USB directly */
 	usb_init(&app_stack_desc);
@@ -170,6 +172,7 @@
 
 		/* GPS poll */
 		gps_poll();
+		gpsdo_poll();
 		usb_gps_poll();
 	}
 }
diff --git a/firmware/ice40-riscv/icE1usb/gpsdo.c b/firmware/ice40-riscv/icE1usb/gpsdo.c
new file mode 100644
index 0000000..ca9b806
--- /dev/null
+++ b/firmware/ice40-riscv/icE1usb/gpsdo.c
@@ -0,0 +1,254 @@
+/*
+ * gpsdo.c
+ *
+ * Copyright (C) 2019-2022  Sylvain Munaut <tnt@246tNt.com>
+ * SPDX-License-Identifier: GPL-3.0-or-later
+ */
+
+#include <stdint.h>
+#include <string.h>
+
+#include "console.h"
+#include "gps.h"
+#include "gpsdo.h"
+#include "misc.h"
+
+
+struct {
+	/* Current tuning */
+	struct {
+		uint16_t coarse;
+		uint16_t fine;
+	} tune;
+
+	/* Measurements */
+	struct {
+		uint32_t tick_prev;	/* Previous tick value */
+		uint32_t last;		/* Last measurement */
+		int      skip;		/* Number of measurement left to discard */
+		int      invalid;	/* Number of consecutive invalid measurements */
+	} meas;
+
+	/* FSM */
+	enum {
+		STATE_DISABLED,		/* Forced to manual */
+		STATE_HOLD_OVER,	/* GPS invalid data, hold */
+		STATE_TUNE_COARSE,	/* Adjust coarse tuning until we're +- 3 Hz */
+		STATE_TUNE_FINE,	/* Fine tracking */
+	} state;
+
+	/* Coarse tuning */
+	struct {
+		int vctxo_sensitivity;
+		int step;
+	} coarse;
+
+	/* Fine tracking */
+	struct {
+		int acc;
+		int div;
+	} fine;
+
+} g_gpsdo;
+
+
+/* VCXTO sensitivity vs 'coarse' count for fast initial acquisition */
+	/*
+	 * Note that the spec if often a guaranteed minimum range and goes
+	 * from ~0.1V to 3.2V instead of 0-3.3V so actual sensitivity is
+	 * higher than the "theoritical value". We boost it by ~ 10% here.
+	 */
+static const int
+vctxo_sensitivity[] = {
+	/* +-  50 ppm pull range => ~ 0.75 Hz / hi-count (set to 0.85) */
+	[VCTXO_TAITIEN_VT40]     = 300,
+
+	/* +- 100 ppm pull range => ~ 1.50 Hz / hi-count (set to 1.6) */
+	[VCTXO_SITIME_SIT3808_E] = 160,
+};
+
+/* Tuning params */
+#define TARGET		30720000
+#define MAX_DEV_VALID	1000
+#define MAX_DEV_FINE	3
+#define MAX_INVALID	5
+
+
+static void
+_gpsdo_coarse_start(void)
+{
+	/* Set the state */
+	g_gpsdo.state = STATE_TUNE_COARSE;
+
+	/* Skip a few measurements to be safe */
+	g_gpsdo.meas.skip = 3;
+
+	/* Reset coarse tuning state */
+	g_gpsdo.coarse.step = 0;
+
+	/* Put the fine adjust back in the middle point */
+	g_gpsdo.tune.coarse += ((int)g_gpsdo.tune.fine - 2048) >> 6;
+	g_gpsdo.tune.fine    = 2048;
+
+	pdm_set(PDM_CLK_HI, true, g_gpsdo.tune.coarse, false);
+	pdm_set(PDM_CLK_LO, true, g_gpsdo.tune.fine,   false);
+}
+
+static void
+_gpsdo_fine_start(void)
+{
+	/* Set the state */
+	g_gpsdo.state = STATE_TUNE_FINE;
+
+	/* Reset the long term error tracking */
+	g_gpsdo.fine.acc = 0;
+	g_gpsdo.fine.div = 0;
+}
+
+static void
+_gpsdo_coarse_tune(uint32_t tick_diff)
+{
+	int freq_diff = (int)tick_diff - TARGET;
+
+	/* Is the measurement good enough to switch to fine ? */
+	if ((freq_diff > -MAX_DEV_FINE) && (freq_diff < MAX_DEV_FINE)) {
+		_gpsdo_fine_start();
+		return;
+	}
+
+	/* Estimate correction and apply it */
+	g_gpsdo.tune.coarse -= (freq_diff * g_gpsdo.coarse.vctxo_sensitivity) >> 8;
+	pdm_set(PDM_CLK_HI, true, g_gpsdo.tune.coarse, false);
+
+	/* Skip next measurement */
+	g_gpsdo.meas.skip = 1;
+
+	/* Debug */
+#ifdef GPSDO_DEBUG
+	printf("[+] GPSDO Coarse: err=%d tune=%d\n", freq_diff, g_gpsdo.tune.coarse);
+#endif
+}
+
+static void
+_gpsdo_fine_track(uint32_t tick_diff)
+{
+	int freq_diff = (int)tick_diff - TARGET;
+	uint16_t tune;
+
+	/* Did we deviate too much ? */
+	if ((freq_diff < -2*MAX_DEV_FINE) && (freq_diff > 2*MAX_DEV_FINE)) {
+		_gpsdo_coarse_start();
+		return;
+	}
+
+	/* Accumulate long term error */
+	g_gpsdo.fine.acc += freq_diff;
+
+	/* Update fine tuning value */
+	if (((g_gpsdo.fine.acc >= 0) && (freq_diff > 0)) ||
+	    ((g_gpsdo.fine.acc <= 0) && (freq_diff < 0)))
+	{
+		    g_gpsdo.tune.fine -= freq_diff;
+	}
+
+	if (g_gpsdo.fine.acc) {
+		if (++g_gpsdo.fine.div > 10) {
+			g_gpsdo.fine.div = 0;
+			if (g_gpsdo.fine.acc > 0)
+				g_gpsdo.tune.fine--;
+			else if (g_gpsdo.fine.acc < 0)
+				g_gpsdo.tune.fine++;
+		}
+	} else {
+		g_gpsdo.fine.div = 0;
+	}
+
+	/* Apply value with a bias from long term accumulator */
+	tune = g_gpsdo.tune.fine - (g_gpsdo.fine.acc / 2);
+	pdm_set(PDM_CLK_LO, true, tune, false);
+
+	/* Debug */
+#ifdef GPSDO_DEBUG
+	printf("[+] GPSDO Fine: err=%d acc=%d tune=%d\n", freq_diff, g_gpsdo.fine.acc, g_gpsdo.tune.fine);
+#endif
+}
+
+
+void
+gpsdo_poll(void)
+{
+	uint32_t tick_now, tick_diff;
+	bool valid;
+
+	/* Get current tick and check if there was a PPS and estimate frequency */
+	tick_now = time_pps_read();
+
+	if (tick_now == g_gpsdo.meas.tick_prev)
+		return;
+
+	g_gpsdo.meas.last = tick_diff = tick_now - g_gpsdo.meas.tick_prev;
+	g_gpsdo.meas.tick_prev = tick_now;
+
+	/* If we're currently discarding samples, skip it */
+	if (g_gpsdo.meas.skip) {
+		g_gpsdo.meas.skip--;
+		return;
+	}
+
+	/* If we're disabled, nothing else to do */
+	if (g_gpsdo.state == STATE_DISABLED)
+		return;
+
+	/* Check GPS state */
+	if (!gps_has_valid_fix()) {
+		/* No GPS fix, go to hold-over */
+		g_gpsdo.state = STATE_HOLD_OVER;
+		return;
+	}
+
+	/* Check measurement plausibility */
+	valid = (tick_diff > (TARGET - MAX_DEV_VALID)) && (tick_diff < (TARGET + MAX_DEV_VALID));
+
+	if (valid) {
+		/* If we're in hold-over, switch to active */
+		if (g_gpsdo.state == STATE_HOLD_OVER) {
+			_gpsdo_coarse_start();
+			return;
+		}
+	} else {
+		/* Count invalid measurements and if too many of
+		 * them, we go back to hold-over */
+		if (++g_gpsdo.meas.invalid >= MAX_INVALID) {
+			g_gpsdo.state = STATE_HOLD_OVER;
+			return;
+		}
+	}
+
+	g_gpsdo.meas.invalid = 0;
+
+	/* If we reach here, we have a valid fix, valid measurement and
+	 * we're not in hold-over or disabled. Feed the correct loop */
+	if (g_gpsdo.state == STATE_TUNE_COARSE)
+		_gpsdo_coarse_tune(tick_diff);
+	else if (g_gpsdo.state == STATE_TUNE_FINE)
+		_gpsdo_fine_track(tick_diff);
+}
+
+
+void
+gpsdo_init(enum gpsdo_vctxo_model vctxo)
+{
+	/* State */
+	memset(&g_gpsdo, 0x00, sizeof(g_gpsdo));
+
+	/* Default tune to middle range */
+	g_gpsdo.tune.coarse = 2048;
+	g_gpsdo.tune.fine   = 2048;
+
+	pdm_set(PDM_CLK_HI, true, g_gpsdo.tune.coarse, false);
+	pdm_set(PDM_CLK_LO, true, g_gpsdo.tune.fine,   false);
+
+	/* Initial state and config */
+	g_gpsdo.state = STATE_HOLD_OVER;
+	g_gpsdo.coarse.vctxo_sensitivity = vctxo_sensitivity[vctxo];
+}
diff --git a/firmware/ice40-riscv/icE1usb/gpsdo.h b/firmware/ice40-riscv/icE1usb/gpsdo.h
new file mode 100644
index 0000000..181e4b9
--- /dev/null
+++ b/firmware/ice40-riscv/icE1usb/gpsdo.h
@@ -0,0 +1,16 @@
+/*
+ * gpsdo.h
+ *
+ * Copyright (C) 2019-2022  Sylvain Munaut <tnt@246tNt.com>
+ * SPDX-License-Identifier: GPL-3.0-or-later
+ */
+
+#pragma once
+
+enum gpsdo_vctxo_model {
+	VCTXO_TAITIEN_VT40 = 0,		/* VTEUALJANF-30.720000 */
+	VCTXO_SITIME_SIT3808_E = 1,	/* SIT3808AI-D2-33EE-30.720000T */
+};
+
+void gpsdo_poll(void);
+void gpsdo_init(enum gpsdo_vctxo_model vctxo);