diff --git a/demos/STM32/RT-STM32G474RE-NUCLEO64/Makefile b/demos/STM32/RT-STM32G474RE-NUCLEO64/Makefile
index f57a953c9..1baa51b36 100644
--- a/demos/STM32/RT-STM32G474RE-NUCLEO64/Makefile
+++ b/demos/STM32/RT-STM32G474RE-NUCLEO64/Makefile
@@ -113,6 +113,8 @@ include $(CHIBIOS)/tools/mk/autobuild.mk
 include $(CHIBIOS)/os/test/test.mk
 include $(CHIBIOS)/test/rt/rt_test.mk
 include $(CHIBIOS)/test/oslib/oslib_test.mk
+include $(CHIBIOS)/os/hal/lib/streams/streams.mk
+include $(CHIBIOS)/os/various/shell/shell.mk
 
 # Define linker script file here
 LDSCRIPT= $(STARTUPLD)/STM32G474xE.ld
diff --git a/demos/STM32/RT-STM32G474RE-NUCLEO64/main.c b/demos/STM32/RT-STM32G474RE-NUCLEO64/main.c
index bcd695e82..a16e002a7 100644
--- a/demos/STM32/RT-STM32G474RE-NUCLEO64/main.c
+++ b/demos/STM32/RT-STM32G474RE-NUCLEO64/main.c
@@ -14,11 +14,159 @@
     limitations under the License.
 */
 
+#include <string.h>
+
 #include "ch.h"
 #include "hal.h"
 #include "rt_test_root.h"
 #include "oslib_test_root.h"
 
+#include "shell.h"
+#include "chprintf.h"
+
+/*===========================================================================*/
+/* Command line related.                                                     */
+/*===========================================================================*/
+
+#define SHELL_WA_SIZE   THD_WORKING_AREA_SIZE(2048)
+
+/* Can be measured using dd if=/dev/xxxx of=/dev/null bs=512 count=10000.*/
+static void cmd_write(BaseSequentialStream *chp, int argc, char *argv[]) {
+  static uint8_t buf[] =
+      "0123456789abcdef0123456789abcdef0123456789abcdef0123456789abcdef"
+      "0123456789abcdef0123456789abcdef0123456789abcdef0123456789abcdef"
+      "0123456789abcdef0123456789abcdef0123456789abcdef0123456789abcdef"
+      "0123456789abcdef0123456789abcdef0123456789abcdef0123456789abcdef"
+      "0123456789abcdef0123456789abcdef0123456789abcdef0123456789abcdef"
+      "0123456789abcdef0123456789abcdef0123456789abcdef0123456789abcdef"
+      "0123456789abcdef0123456789abcdef0123456789abcdef0123456789abcdef"
+      "0123456789abcdef0123456789abcdef0123456789abcdef0123456789abcdef"
+      "0123456789abcdef0123456789abcdef0123456789abcdef0123456789abcdef"
+      "0123456789abcdef0123456789abcdef0123456789abcdef0123456789abcdef"
+      "0123456789abcdef0123456789abcdef0123456789abcdef0123456789abcdef"
+      "0123456789abcdef0123456789abcdef0123456789abcdef0123456789abcdef"
+      "0123456789abcdef0123456789abcdef0123456789abcdef0123456789abcdef"
+      "0123456789abcdef0123456789abcdef0123456789abcdef0123456789abcdef"
+      "0123456789abcdef0123456789abcdef0123456789abcdef0123456789abcdef"
+      "0123456789abcdef0123456789abcdef0123456789abcdef0123456789abcdef";
+
+  (void)argv;
+  if (argc > 0) {
+    chprintf(chp, "Usage: write\r\n");
+    return;
+  }
+
+  while (chnGetTimeout((BaseChannel *)chp, TIME_IMMEDIATE) == Q_TIMEOUT) {
+#if 1
+    /* Writing in channel mode.*/
+    chnWrite(chp, buf, sizeof buf - 1);
+#else
+    /* Writing in buffer mode.*/
+    (void) obqGetEmptyBufferTimeout(&PORTAB_SDU1.obqueue, TIME_INFINITE);
+    memcpy(PORTAB_SDU1.obqueue.ptr, buf, SERIAL_USB_BUFFERS_SIZE);
+    obqPostFullBuffer(&PORTAB_SDU1.obqueue, SERIAL_USB_BUFFERS_SIZE);
+#endif
+  }
+  chprintf(chp, "\r\n\nstopped\r\n");
+}
+
+#if STM32_CLOCK_DYNAMIC == TRUE
+static void cmd_clock(BaseSequentialStream *chp, int argc, char *argv[]) {
+  bool result;
+  const halclkcfg_t *ccp;
+  static const char usage[] = "Usage: clock [reset|default]\r\n";
+
+  (void)argv;
+  if (argc != 1) {
+    chprintf(chp, usage);
+    return;
+  }
+
+  if (strcmp(argv[0], "reset") == 0) {
+    chprintf(chp, "\r\nSwitching to post-reset clocks: ");
+    ccp = &hal_clkcfg_reset;
+  }
+  else if (strcmp(argv[0], "default") == 0) {
+    chprintf(chp, "\r\nSwitching to default mcuconf.h clocks: ");
+    ccp = &hal_clkcfg_default;
+  }
+  else {
+    chprintf(chp, usage);
+    return;
+  }
+
+  /* Time for the serial TX buffer to flush.*/
+  chThdSleepMilliseconds(100);
+
+  /* Switching clocks.*/
+  result = halClockSwitchMode(ccp);
+
+  /* Reconfiguring the peripherals because clocks frequencies could have
+     changed.*/
+  sioStart(&LPSIOD1, NULL);
+
+  /* Printing result.*/
+  if (result) {
+    chprintf(chp, "failed\r\n");
+  }
+  else {
+    chprintf(chp, "done\r\n");
+  }
+}
+#endif
+
+static void cmd_clocks(BaseSequentialStream *chp, int argc, char *argv[]) {
+  const char *swp;
+
+  (void)argv;
+  if (argc > 0) {
+    chprintf(chp, "Usage: clocks\r\n");
+    return;
+  }
+
+#if STM32_SW == STM32_SW_HSI16
+  swp = "HSI16";
+#elif STM32_SW == STM32_SW_HSE
+  swp = "HSE";
+#elif STM32_SW == STM32_SW_PLLRCLK
+  swp = "PLLR";
+#else
+  #error "invalid STM32_SW value specified"
+#endif
+
+  chprintf(chp, "HSI16:    %10u\r\n", halClockGetPointX(CLK_HSI16));
+  chprintf(chp, "HSI48:    %10u\r\n", halClockGetPointX(CLK_HSI48));
+  chprintf(chp, "HSE:      %10u\r\n", halClockGetPointX(CLK_HSE));
+  chprintf(chp, "SYSCLK:   %10u (%s)\r\n", halClockGetPointX(CLK_SYSCLK), swp);
+  chprintf(chp, "PLLPCLK:  %10u\r\n", halClockGetPointX(CLK_PLLP));
+  chprintf(chp, "PLLRCLK:  %10u\r\n", halClockGetPointX(CLK_PLLR));
+  chprintf(chp, "PLLQCLK:  %10u\r\n", halClockGetPointX(CLK_PLLQ));
+  chprintf(chp, "HCLK:     %10u\r\n", halClockGetPointX(CLK_HCLK));
+  chprintf(chp, "PCLK1:    %10u\r\n", halClockGetPointX(CLK_PCLK1));
+  chprintf(chp, "PCLK1TIM: %10u\r\n", halClockGetPointX(CLK_PCLK1TIM));
+  chprintf(chp, "PCLK2:    %10u\r\n", halClockGetPointX(CLK_PCLK2));
+  chprintf(chp, "PCLK2TIM: %10u\r\n", halClockGetPointX(CLK_PCLK2TIM));
+  chprintf(chp, "MCO:      %10u\r\n", halClockGetPointX(CLK_MCO));
+}
+
+static const ShellCommand commands[] = {
+  {"write", cmd_write},
+#if STM32_CLOCK_DYNAMIC == TRUE
+  {"clock", cmd_clock},
+#endif
+  {"clocks", cmd_clocks},
+  {NULL, NULL}
+};
+
+static const ShellConfig shell_cfg1 = {
+  (BaseSequentialStream *)&LPSIOD1,
+  commands
+};
+
+/*===========================================================================*/
+/* Generic code.                                                             */
+/*===========================================================================*/
+
 /*
  * Green LED blinker thread, times are in milliseconds.
  */
@@ -55,6 +203,11 @@ int main(void) {
    */
   sioStart(&LPSIOD1, NULL);
 
+  /*
+   * Shell manager initialization.
+   */
+  shellInit();
+
   /*
    * Creates the blinker thread.
    */
@@ -65,10 +218,10 @@ int main(void) {
    * sleeping in a loop and check the button state.
    */
   while (true) {
-   if (palReadLine(LINE_BUTTON)) {
-      test_execute((BaseSequentialStream *)&LPSIOD1, &rt_test_suite);
-      test_execute((BaseSequentialStream *)&LPSIOD1, &oslib_test_suite);
-    }
+    thread_t *shelltp = chThdCreateFromHeap(NULL, SHELL_WA_SIZE,
+                                            "shell", NORMALPRIO + 1,
+                                            shellThread, (void *)&shell_cfg1);
+    chThdWait(shelltp);               /* Waiting termination.             */
     chThdSleepMilliseconds(500);
   }
 }
diff --git a/demos/STM32/RT-STM32H563ZI-NUCLEO144/main.c b/demos/STM32/RT-STM32H563ZI-NUCLEO144/main.c
index d4b25dbf4..efd25d131 100644
--- a/demos/STM32/RT-STM32H563ZI-NUCLEO144/main.c
+++ b/demos/STM32/RT-STM32H563ZI-NUCLEO144/main.c
@@ -221,6 +221,11 @@ int main(void) {
    */
   sioStart(&SIOD3, NULL);
 
+  /*
+   * Shell manager initialization.
+   */
+  shellInit();
+
   /*
    * Creates the blinker thread.
    */
@@ -235,12 +240,6 @@ int main(void) {
                                             "shell", NORMALPRIO + 1,
                                             shellThread, (void *)&shell_cfg1);
     chThdWait(shelltp);               /* Waiting termination.             */
-#if 0
-    if (palReadLine(LINE_BUTTON)) {
-      test_execute((BaseSequentialStream *)&SIOD3, &rt_test_suite);
-      test_execute((BaseSequentialStream *)&SIOD3, &oslib_test_suite);
-    }
-#endif
     chThdSleepMilliseconds(500);
   }
 }
diff --git a/os/hal/ports/STM32/STM32G4xx/hal_lld.c b/os/hal/ports/STM32/STM32G4xx/hal_lld.c
index 2fc2104c2..ac4d7b0db 100644
--- a/os/hal/ports/STM32/STM32G4xx/hal_lld.c
+++ b/os/hal/ports/STM32/STM32G4xx/hal_lld.c
@@ -112,34 +112,49 @@ const halclkcfg_t hal_clkcfg_default = {
  * @brief   Dynamic clock points for this device.
  */
 static halfreq_t clock_points[CLK_ARRAY_SIZE] = {
-  [CLK_SYSCLK]      = STM32_SYSCLK,
-  [CLK_PLLPCLK]     = STM32_PLL_P_CLKOUT,
-  [CLK_PLLQCLK]     = STM32_PLL_Q_CLKOUT,
-  [CLK_PLLRCLK]     = STM32_PLL_R_CLKOUT,
-  [CLK_HCLK]        = STM32_HCLK,
-  [CLK_PCLK1]       = STM32_PCLK1,
-  [CLK_PCLK1TIM]    = STM32_TIMP1CLK,
-  [CLK_PCLK2]       = STM32_PCLK2,
-  [CLK_PCLK2TIM]    = STM32_TIMP2CLK,
-  [CLK_MCO]         = STM32_MCOCLK,
+#if STM32_HSI16_ENABLED
+  [CLK_HSI16]           = STM32_HSI16CLK,
+#else
+  [CLK_HSI16]           = 0U,
+#endif
+#if STM32_HSI48_ENABLED
+  [CLK_HSI48]           = STM32_HSI48CLK,
+#else
+  [CLK_HSI48]           = 0U,
+#endif
+#if STM32_HSE_ENABLED
+  [CLK_HSE]             = STM32_HSECLK,
+#else
+  [CLK_HSE]             = 0U,
+#endif
+  [CLK_SYSCLK]          = STM32_SYSCLK,
+  [CLK_PLLP]            = STM32_PLL_P_CLKOUT,
+  [CLK_PLLQ]            = STM32_PLL_Q_CLKOUT,
+  [CLK_PLLR]            = STM32_PLL_R_CLKOUT,
+  [CLK_HCLK]            = STM32_HCLK,
+  [CLK_PCLK1]           = STM32_PCLK1,
+  [CLK_PCLK1TIM]        = STM32_TIMP1CLK,
+  [CLK_PCLK2]           = STM32_PCLK2,
+  [CLK_PCLK2TIM]        = STM32_TIMP2CLK,
+  [CLK_MCO]             = STM32_MCOCLK,
 };
 
 /**
  * @brief   Type of a structure representing system limits.
  */
 typedef struct {
-  halfreq_t     sysclk_max;
-  halfreq_t     pllin_max;
-  halfreq_t     pllin_min;
-  halfreq_t     pllvco_max;
-  halfreq_t     pllvco_min;
-  halfreq_t     pllp_max;
-  halfreq_t     pllp_min;
-  halfreq_t     pllq_max;
-  halfreq_t     pllq_min;
-  halfreq_t     pllr_max;
-  halfreq_t     pllr_min;
-  halfreq_t     flash_thresholds[STM32_WS_THRESHOLDS];
+  halfreq_t             sysclk_max;
+  halfreq_t             pllin_max;
+  halfreq_t             pllin_min;
+  halfreq_t             pllvco_max;
+  halfreq_t             pllvco_min;
+  halfreq_t             pllp_max;
+  halfreq_t             pllp_min;
+  halfreq_t             pllq_max;
+  halfreq_t             pllq_min;
+  halfreq_t             pllr_max;
+  halfreq_t             pllr_min;
+  halfreq_t             flash_thresholds[STM32_WS_THRESHOLDS];
 } system_limits_t;
 
 /**
@@ -301,7 +316,7 @@ static bool hal_lld_clock_check_tree(const halclkcfg_t *ccp) {
                                        2U, 4U, 8U, 16U, 64U, 128U, 256U, 512U};
   static const uint32_t pprediv[16] = {1U, 1U, 1U, 1U, 2U, 4U, 8U, 16U};
   const system_limits_t *slp;
-  halfreq_t hsi16clk = 0U, hseclk = 0U, pllselclk;
+  halfreq_t hsi16clk = 0U, hsi48clk = 0U, hseclk = 0U, pllselclk;
   halfreq_t pllpclk = 0U, pllqclk = 0U, pllrclk = 0U;
   halfreq_t sysclk, hclk, pclk1, pclk2, pclk1tim, pclk2tim, mcoclk;
   uint32_t mcodiv, flashws;
@@ -330,6 +345,11 @@ static bool hal_lld_clock_check_tree(const halclkcfg_t *ccp) {
     hsi16clk = STM32_HSI16CLK;
   }
 
+  /* HSI48 clock after divider.*/
+  if ((ccp->rcc_crrcr & RCC_CRRCR_HSI48ON) != 0U) {
+    hsi48clk = STM32_HSI48CLK;
+  }
+
   /* HSE clock.*/
   if ((ccp->rcc_cr & RCC_CR_HSEON) != 0U) {
     hseclk = STM32_HSECLK;
@@ -495,10 +515,13 @@ static bool hal_lld_clock_check_tree(const halclkcfg_t *ccp) {
   }
 
   /* Writing out results.*/
+  clock_points[CLK_HSI16]    = hsi16clk;
+  clock_points[CLK_HSI48]    = hsi48clk;
+  clock_points[CLK_HSE]      = hseclk;
   clock_points[CLK_SYSCLK]   = sysclk;
-  clock_points[CLK_PLLPCLK]  = pllpclk;
-  clock_points[CLK_PLLQCLK]  = pllqclk;
-  clock_points[CLK_PLLRCLK]  = pllrclk;
+  clock_points[CLK_PLLP]     = pllpclk;
+  clock_points[CLK_PLLQ]     = pllqclk;
+  clock_points[CLK_PLLR]     = pllrclk;
   clock_points[CLK_HCLK]     = hclk;
   clock_points[CLK_PCLK1]    = pclk1;
   clock_points[CLK_PCLK1TIM] = pclk1tim;
diff --git a/os/hal/ports/STM32/STM32G4xx/hal_lld.h b/os/hal/ports/STM32/STM32G4xx/hal_lld.h
index a773f8e03..67e4bb198 100644
--- a/os/hal/ports/STM32/STM32G4xx/hal_lld.h
+++ b/os/hal/ports/STM32/STM32G4xx/hal_lld.h
@@ -89,17 +89,20 @@
  * @name   Clock points names
  * @{
  */
-#define CLK_SYSCLK              0U
-#define CLK_PLLPCLK             1U
-#define CLK_PLLQCLK             2U
-#define CLK_PLLRCLK             3U
-#define CLK_HCLK                4U
-#define CLK_PCLK1               5U
-#define CLK_PCLK1TIM            6U
-#define CLK_PCLK2               7U
-#define CLK_PCLK2TIM            8U
-#define CLK_MCO                 9U
-#define CLK_ARRAY_SIZE          10U
+#define CLK_HSI16               0U
+#define CLK_HSI48               1U
+#define CLK_HSE                 2U
+#define CLK_SYSCLK              3U
+#define CLK_PLLP                4U
+#define CLK_PLLQ                5U
+#define CLK_PLLR                6U
+#define CLK_HCLK                7U
+#define CLK_PCLK1               8U
+#define CLK_PCLK1TIM            9U
+#define CLK_PCLK2               10U
+#define CLK_PCLK2TIM            11U
+#define CLK_MCO                 12U
+#define CLK_ARRAY_SIZE          13U
 /** @} */
 
 /**