diff --git a/meson.build b/meson.build
index 777cce66..96e1bcb8 100644
--- a/meson.build
+++ b/meson.build
@@ -504,7 +504,8 @@ cs7000_src = ['platform/drivers/NVM/nvmem_CS7000.c',
              'platform/drivers/GPIO/gpio_shiftReg.c',
              'platform/drivers/SPI/spi_custom.c',
              'platform/drivers/SPI/spi_bitbang.c',
-             'platform/drivers/GPS/GPS_CS7000.cpp',
+             'platform/drivers/GPS/gps_stm32.cpp',
+             'platform/drivers/GPS/nmea_rbuf.c',
              'platform/targets/CS7000/hwconfig.c',
              'platform/targets/CS7000/platform.c']
 
diff --git a/platform/drivers/GPS/GPS_CS7000.cpp b/platform/drivers/GPS/GPS_CS7000.cpp
deleted file mode 100644
index d2a6a8ee..00000000
--- a/platform/drivers/GPS/GPS_CS7000.cpp
+++ /dev/null
@@ -1,175 +0,0 @@
-/***************************************************************************
- *   Copyright (C) 2024 - 2025 by Silvano Seva IU2KWO                      *
- *                                                                         *
- *   This program is free software; you can redistribute it and/or modify  *
- *   it under the terms of the GNU General Public License as published by  *
- *   the Free Software Foundation; either version 3 of the License, or     *
- *   (at your option) any later version.                                   *
- *                                                                         *
- *   This program is distributed in the hope that it will be useful,       *
- *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
- *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
- *   GNU General Public License for more details.                          *
- *                                                                         *
- *   You should have received a copy of the GNU General Public License     *
- *   along with this program; if not, see <http://www.gnu.org/licenses/>   *
- ***************************************************************************/
-
-#include <kernel/scheduler/scheduler.h>
-#include <interfaces/delays.h>
-#include <peripherals/gpio.h>
-#include <peripherals/gps.h>
-#include <hwconfig.h>
-#include <string.h>
-#include <miosix.h>
-#include <rcc.h>
-
-static size_t bufPos = 0;
-static size_t maxPos = 0;
-static char   *dataBuf;
-static bool   receiving = false;
-
-using namespace miosix;
-static Thread *gpsWaiting = 0;
-
-void __attribute__((used)) GpsUsartImpl()
-{
-    if(USART6->SR & USART_SR_RXNE)
-    {
-        char value = USART6->DR;
-
-        if((receiving == false) && (value == '$') && (bufPos == 0))
-        {
-            receiving = true;
-        }
-
-        if(receiving)
-        {
-            if(bufPos == maxPos)
-            {
-                receiving = false;
-            }
-
-            char prevChar   = dataBuf[bufPos - 1];
-            dataBuf[bufPos] = value;
-            bufPos += 1;
-
-            if((prevChar == '\r') && (value == '\n'))
-            {
-                receiving = false;
-                bufPos -= 1;
-            }
-        }
-
-        if((receiving == false) && (bufPos != 0))
-        {
-            // NMEA sentence received, turn off serial port
-            USART6->CR1 &= ~USART_CR1_UE;
-
-            if(gpsWaiting)
-            {
-                gpsWaiting->IRQwakeup();
-                if(gpsWaiting->IRQgetPriority()>
-                    Thread::IRQgetCurrentThread()->IRQgetPriority())
-                        Scheduler::IRQfindNextThread();
-                gpsWaiting = 0;
-            }
-        }
-    }
-
-    USART6->SR = 0;
-}
-
-void __attribute__((naked)) USART6_IRQHandler()
-{
-    saveContext();
-    asm volatile("bl _Z12GpsUsartImplv");
-    restoreContext();
-}
-
-
-void gps_init(const uint16_t baud)
-{
-    gpio_setMode(GPS_RXD, ALTERNATE | ALTERNATE_FUNC(8));
-
-    RCC->APB2ENR |= RCC_APB2ENR_USART6EN;
-    __DSB();
-
-    uint32_t quot = rcc_getBusClock(PERIPH_BUS_APB2);
-    quot = (2 * quot) / baud;
-
-    USART6->BRR  = (quot / 2) + (quot & 1);
-    USART6->CR3 |= USART_CR3_ONEBIT;
-    USART6->CR1  = USART_CR1_RE
-                 | USART_CR1_RXNEIE;
-
-    NVIC_SetPriority(USART6_IRQn, 14);
-}
-
-void gps_terminate()
-{
-    gps_disable();
-
-    RCC->APB2ENR &= ~RCC_APB2ENR_USART6EN;
-}
-
-void gps_enable()
-{
-    // Enable IRQ
-    NVIC_ClearPendingIRQ(USART6_IRQn);
-    NVIC_EnableIRQ(USART6_IRQn);
-}
-
-void gps_disable()
-{
-    USART6->CR1 &= ~USART_CR1_UE;
-    NVIC_DisableIRQ(USART6_IRQn);
-
-    receiving = false;
-    bufPos    = 0;
-}
-
-bool gps_detect(uint16_t timeout)
-{
-    (void) timeout;
-
-    return true;
-}
-
-int gps_getNmeaSentence(char *buf, const size_t maxLength)
-{
-    memset(buf, 0x00, maxLength);
-    bufPos  = 0;
-    maxPos  = maxLength;
-    dataBuf = buf;
-
-    // Enable serial port
-    USART6->CR1 |= USART_CR1_UE;
-
-    return 0;
-}
-
-bool gps_nmeaSentenceReady()
-{
-    return (receiving == false) && (bufPos > 0);
-}
-
-void gps_waitForNmeaSentence()
-{
-    /*
-     * Put the calling thread in waiting status until a complete sentence is ready.
-     */
-    {
-        FastInterruptDisableLock dLock;
-        gpsWaiting = Thread::IRQgetCurrentThread();
-        do
-        {
-            Thread::IRQwait();
-            {
-                FastInterruptEnableLock eLock(dLock);
-                Thread::yield();
-            }
-        }
-        while(gpsWaiting);
-    }
-}
diff --git a/platform/targets/CS7000/hwconfig.c b/platform/targets/CS7000/hwconfig.c
index 5f2cb564..43c24117 100644
--- a/platform/targets/CS7000/hwconfig.c
+++ b/platform/targets/CS7000/hwconfig.c
@@ -20,10 +20,12 @@
 #include <spi_custom.h>
 #include <adc_stm32.h>
 #include <spi_stm32.h>
+#include <gps_stm32.h>
 #include <SKY72310.h>
 #include <hwconfig.h>
 #include <pthread.h>
 #include <AK2365A.h>
+#include <gps.h>
 
 static const struct spiConfig spiFlashCfg =
 {
@@ -113,3 +115,10 @@ const struct sky73210 pll =
     .cs     = { PLL_CS },
     .refClk = 16800000
 };
+
+const struct gpsDevice gps =
+{
+    .enable = gpsStm32_enable,
+    .disable = gpsStm32_disable,
+    .getSentence = gpsStm32_getNmeaSentence
+};
diff --git a/platform/targets/CS7000/hwconfig.h b/platform/targets/CS7000/hwconfig.h
index c774bc53..c8cf724c 100644
--- a/platform/targets/CS7000/hwconfig.h
+++ b/platform/targets/CS7000/hwconfig.h
@@ -50,6 +50,7 @@ extern const struct spiDevice c6000_spi;
 extern const struct gpioDev extGpio;
 extern const struct ak2365a detector;
 extern const struct sky73210 pll;
+extern const struct gpsDevice gps;
 
 /* Screen dimensions */
 #define CONFIG_SCREEN_WIDTH 160
@@ -70,6 +71,8 @@ extern const struct sky73210 pll;
 
 /* Device has a GPS chip */
 #define CONFIG_GPS
+#define CONFIG_GPS_STM32_USART6
+#define CONFIG_NMEA_RBUF_SIZE 128
 
 #ifdef __cplusplus
 }
diff --git a/platform/targets/CS7000/platform.c b/platform/targets/CS7000/platform.c
index e770c231..32b21716 100644
--- a/platform/targets/CS7000/platform.c
+++ b/platform/targets/CS7000/platform.c
@@ -22,9 +22,11 @@
 #include <gpio_shiftReg.h>
 #include <spi_bitbang.h>
 #include <adc_stm32.h>
+#include <gps_stm32.h>
 #include <Cx000_dac.h>
 #include <hwconfig.h>
 #include <string.h>
+#include <gps.h>
 
 static const hwInfo_t hwInfo =
 {
@@ -65,6 +67,7 @@ void platform_init()
 void platform_terminate()
 {
     adcStm32_terminate(&adc1);
+    gpsStm32_terminate();
 
     #ifndef RUNNING_TESTSUITE
     gpioDev_clear(MAIN_PWR_SW);
@@ -201,3 +204,11 @@ const hwInfo_t *platform_getHwInfo()
 {
     return &hwInfo;
 }
+
+const struct gpsDevice *platform_initGps()
+{
+    gpio_setMode(GPS_RXD, ALTERNATE | ALTERNATE_FUNC(8));
+    gpsStm32_init(9600);
+
+    return &gps;
+}