2025-12-06 03:31:59 +01:00
26 changed files with 7391 additions and 8406 deletions
--- a/.clang-format
+++ b/.clang-format
@ -1,156 +0,0 @@
---
-Language:        Cpp
-# BasedOnStyle:  Google
-AccessModifierOffset: -1
-AlignAfterOpenBracket: Align
-AlignConsecutiveAssignments: false
-AlignConsecutiveDeclarations: false
-AlignEscapedNewlines: Left
-AlignOperands:   true
-AlignTrailingComments: true
-AllowAllArgumentsOnNextLine: true
-AllowAllConstructorInitializersOnNextLine: true
-AllowAllParametersOfDeclarationOnNextLine: true
-AllowShortBlocksOnASingleLine: false
-AllowShortCaseLabelsOnASingleLine: false
-AllowShortFunctionsOnASingleLine: All
-AllowShortLambdasOnASingleLine: All
-AllowShortIfStatementsOnASingleLine: WithoutElse
-AllowShortLoopsOnASingleLine: true
-AlwaysBreakAfterDefinitionReturnType: None
-AlwaysBreakAfterReturnType: None
-AlwaysBreakBeforeMultilineStrings: true
-AlwaysBreakTemplateDeclarations: Yes
-BinPackArguments: true
-BinPackParameters: true
-BraceWrapping:   
-  AfterCaseLabel:  false
-  AfterClass:      true
-  AfterControlStatement: false
-  AfterEnum:       false
-  AfterFunction:   true
-  AfterNamespace:  false
-  AfterObjCDeclaration: false
-  AfterStruct:     false
-  AfterUnion:      false
-  AfterExternBlock: false
-  BeforeCatch:     false
-  BeforeElse:      false
-  IndentBraces:    false
-  SplitEmptyFunction: true
-  SplitEmptyRecord: true
-  SplitEmptyNamespace: true
-BreakBeforeBinaryOperators: None
-BreakBeforeBraces: Attach
-BreakBeforeInheritanceComma: false
-BreakInheritanceList: BeforeColon
-BreakBeforeTernaryOperators: true
-BreakConstructorInitializersBeforeComma: false
-BreakConstructorInitializers: BeforeColon
-BreakAfterJavaFieldAnnotations: false
-BreakStringLiterals: true
-ColumnLimit:     90
-CommentPragmas:  '^ IWYU pragma:'
-CompactNamespaces: false
-ConstructorInitializerAllOnOneLineOrOnePerLine: true
-ConstructorInitializerIndentWidth: 4
-ContinuationIndentWidth: 4
-Cpp11BracedListStyle: true
-DerivePointerAlignment: true
-DisableFormat:   false
-ExperimentalAutoDetectBinPacking: false
-FixNamespaceComments: true
-ForEachMacros:   
-  - foreach
-  - Q_FOREACH
-  - BOOST_FOREACH
-IncludeBlocks:   Regroup
-IncludeCategories: 
-  - Regex:           '^<ext/.*\.h>'
-    Priority:        2
-  - Regex:           '^<.*\.h>'
-    Priority:        1
-  - Regex:           '^<.*'
-    Priority:        2
-  - Regex:           '.*'
-    Priority:        3
-IncludeIsMainRegex: '([-_](test|unittest))?$'
-IndentCaseLabels: true
-IndentPPDirectives: None
-IndentWidth:     2
-IndentWrappedFunctionNames: false
-JavaScriptQuotes: Leave
-JavaScriptWrapImports: true
-KeepEmptyLinesAtTheStartOfBlocks: false
-MacroBlockBegin: ''
-MacroBlockEnd:   ''
-MaxEmptyLinesToKeep: 1
-NamespaceIndentation: None
-ObjCBinPackProtocolList: Never
-ObjCBlockIndentWidth: 2
-ObjCSpaceAfterProperty: false
-ObjCSpaceBeforeProtocolList: true
-PenaltyBreakAssignment: 2
-PenaltyBreakBeforeFirstCallParameter: 1
-PenaltyBreakComment: 300
-PenaltyBreakFirstLessLess: 120
-PenaltyBreakString: 1000
-PenaltyBreakTemplateDeclaration: 10
-PenaltyExcessCharacter: 1000000
-PenaltyReturnTypeOnItsOwnLine: 200
-PointerAlignment: Left
-RawStringFormats: 
-  - Language:        Cpp
-    Delimiters:      
-      - cc
-      - CC
-      - cpp
-      - Cpp
-      - CPP
-      - 'c++'
-      - 'C++'
-    CanonicalDelimiter: ''
-    BasedOnStyle:    google
-  - Language:        TextProto
-    Delimiters:      
-      - pb
-      - PB
-      - proto
-      - PROTO
-    EnclosingFunctions: 
-      - EqualsProto
-      - EquivToProto
-      - PARSE_PARTIAL_TEXT_PROTO
-      - PARSE_TEST_PROTO
-      - PARSE_TEXT_PROTO
-      - ParseTextOrDie
-      - ParseTextProtoOrDie
-    CanonicalDelimiter: ''
-    BasedOnStyle:    google
-ReflowComments:  true
-SortIncludes:    true
-SortUsingDeclarations: true
-SpaceAfterCStyleCast: false
-SpaceAfterLogicalNot: false
-SpaceAfterTemplateKeyword: true
-SpaceBeforeAssignmentOperators: true
-SpaceBeforeCpp11BracedList: false
-SpaceBeforeCtorInitializerColon: true
-SpaceBeforeInheritanceColon: true
-SpaceBeforeParens: ControlStatements
-SpaceBeforeRangeBasedForLoopColon: true
-SpaceInEmptyParentheses: false
-SpacesBeforeTrailingComments: 2
-SpacesInAngles:  false
-SpacesInContainerLiterals: true
-SpacesInCStyleCastParentheses: false
-SpacesInParentheses: false
-SpacesInSquareBrackets: false
-Standard:        Auto
-StatementMacros: 
-  - Q_UNUSED
-  - QT_REQUIRE_VERSION
-TabWidth:        4
-UseTab:          Never
-...
-
--- a/.vscode/launch.json
+++ b/.vscode/launch.json
@ -1,31 +0,0 @@
-{
-    "version": "0.2.0",
-    "configurations": [
-        {
-            "name": "OpenOCD-Debug",
-            "type": "cortex-debug",
-            "request": "launch",
-            "servertype": "openocd",
-            "executable": "build/ch.elf",
-            "configFiles": [
-                "interface/stlink.cfg",
-                "target/stm32f0x.cfg"
-            ],
-            "cwd": "${workspaceRoot}",
-            "svdFile": "STM32F0x2.svd",
-            "device": "stm32f0x",
-            "preLaunchTask": "build",
-        },
-        {
-            "name": "STLink-Debug",
-            "type": "cortex-debug",
-            "request": "launch",
-            "servertype": "stutil",
-            "executable": "build/ch.elf",
-            "cwd": "${workspaceRoot}",
-            "svdFile": "STM32F0x2.svd",
-            "device": "stm32f0x",
-            "preLaunchTask": "build",
-        }
-    ]
-}
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@ -1,4 +1,3 @@
 {
-    "C_Cpp.errorSquiggles": "Disabled",
-    "cpplint.filters": ["-build/include_subdir", "-build/include_order", "-readability/casting", "-whitespace/comments"]
+    "C_Cpp.errorSquiggles": "Disabled"
 }
--- a/.vscode/tasks.json
+++ b/.vscode/tasks.json
@ -2,19 +2,19 @@
    "version": "2.0.0",
    "tasks": [
        {
-            "label": "build",
+            "label": "make",
            "type": "shell",
            "command": "make",
-            "group": {
-                "kind": "build",
-                "isDefault": true
-            }
+            "group": "build"
        },
        {
            "label": "flash",
            "type": "shell",
            "command": "make dfu flash",
-            "group": "build"
+            "group": {
+                "kind": "build",
+                "isDefault": true
+            }
        }
    ]
 }
--- a/Font5x7.c
+++ b/Font5x7.c
--- a/23
+++ b/23
@ -81,10 +81,6 @@ endif
 # Project, sources and paths
 #

-# Dvice node to flash
-DEVICE = /dev/cu.usbmodem401
-#DEVICE = /dev/ttyACM0
-
 # Define project name here
 PROJECT = ch

@ -104,9 +100,9 @@ include $(CHIBIOS)/os/hal/osal/rt/osal.mk
 include $(CHIBIOS)/os/rt/rt.mk
 include $(CHIBIOS)/os/common/ports/ARMCMx/compilers/GCC/mk/port_v6m.mk
 # Other files (optional).
-#include $(CHIBIOS)/test/rt/test.mk
+include $(CHIBIOS)/test/rt/test.mk
 include $(CHIBIOS)/os/hal/lib/streams/streams.mk
-#include $(CHIBIOS)/os/various/shell/shell.mk
+include $(CHIBIOS)/os/various/shell/shell.mk

 # Define linker script file here
 #LDSCRIPT= $(STARTUPLD)/STM32F072xB.ld
@ -122,9 +118,12 @@ CSRC = $(STARTUPSRC) \
       $(PLATFORMSRC) \
       $(BOARDSRC) \
       $(STREAMSSRC) \
+       $(SHELLSRC) \
       usbcfg.c \
       main.c si5351.c tlv320aic3204.c dsp.c plot.c ui.c ili9341.c numfont20x22.c Font5x7.c flash.c adc.c

+#       $(TESTSRC) \
+
 # C++ sources that can be compiled in ARM or THUMB mode depending on the global
 # setting.
 CPPSRC =
@ -154,7 +153,8 @@ ASMSRC = $(STARTUPASM) $(PORTASM) $(OSALASM)

 INCDIR = $(STARTUPINC) $(KERNINC) $(PORTINC) $(OSALINC) \
         $(HALINC) $(PLATFORMINC) $(BOARDINC)  \
-         $(STREAMSINC)
+         $(STREAMSINC) $(SHELLINC)
+# $(TESTINC)

 #
 # Project, sources and paths
@ -176,7 +176,7 @@ CPPC = $(TRGT)g++
 LD   = $(TRGT)gcc
 #LD   = $(TRGT)g++
 CP   = $(TRGT)objcopy
-AS   = $(TRGT)gcc -x assembler-with-cpp -ggdb
+AS   = $(TRGT)gcc -x assembler-with-cpp
 AR   = $(TRGT)ar
 OD   = $(TRGT)objdump
 SZ   = $(TRGT)size
@ -229,4 +229,9 @@ flash: build/ch.bin
 	dfu-util -d 0483:df11 -a 0 -s 0x08000000:leave -D build/ch.bin

 dfu:
-	-printf "reset dfu\r" >$(DEVICE) && sleep 1
+	-@printf "reset dfu\r" >/dev/cu.usbmodem401
+
+TAGS: Makefile
+	@etags *.[ch] NANOVNA_STM32_F072/*.[ch] $(shell find ChibiOS/os/hal/ports/STM32/STM32F0xx ChibiOS/os -name \*.\[ch\] -print) 
+	@ls -l TAGS
+
--- a/NANOVNA_STM32_F072/board.h
+++ b/NANOVNA_STM32_F072/board.h
@ -17,6 +17,10 @@
 #ifndef _BOARD_H_
 #define _BOARD_H_

+/*
+ * Setup for the Strawberry Linux STbee
+ */
+
 /*
 * Board identifier.
 */
@ -126,8 +130,8 @@
                                     PIN_MODE_ALTERNATE(GPIOA_MCO) | \
                                     PIN_MODE_INPUT(9U) |           \
                                     PIN_MODE_OUTPUT(GPIOA_USB_DISC) | \
-                                     PIN_MODE_ALTERNATE(GPIOA_USB_DM) |  \
-                                     PIN_MODE_ALTERNATE(GPIOA_USB_DP) |  \
+                                     PIN_MODE_INPUT(GPIOA_USB_DM) |  \
+                                     PIN_MODE_INPUT(GPIOA_USB_DP) |  \
                                     PIN_MODE_ALTERNATE(GPIOA_JTMS) |    \
                                     PIN_MODE_ALTERNATE(GPIOA_JTCK) |    \
                                     PIN_MODE_OUTPUT(GPIOA_LCD_RESET))
@ -157,7 +161,7 @@
                                     PIN_OSPEED_2M(7) |          \
                                     PIN_OSPEED_100M(GPIOA_MCO) | \
                                     PIN_OSPEED_100M(9) |          \
-                                     PIN_OSPEED_100M(GPIOA_USB_DISC) |         \
+                                     PIN_OSPEED_100M(10) |         \
                                     PIN_OSPEED_100M(GPIOA_USB_DM) |     \
                                     PIN_OSPEED_100M(GPIOA_USB_DP) |     \
                                     PIN_OSPEED_100M(GPIOA_JTMS) |         \
@ -173,7 +177,7 @@
                                     PIN_PUPDR_FLOATING(7) |         \
                                     PIN_PUPDR_PULLUP(GPIOA_MCO) | \
                                     PIN_PUPDR_PULLUP(9) |         \
-                                     PIN_PUPDR_FLOATING(GPIOA_USB_DISC) | \
+                                     PIN_PUPDR_PULLUP(GPIOA_USB_DISC) | \
                                     PIN_PUPDR_FLOATING(GPIOA_USB_DM) | \
                                     PIN_PUPDR_FLOATING(GPIOA_USB_DP) | \
                                     PIN_PUPDR_PULLDOWN(GPIOA_JTMS) |   \
--- a/README.md
+++ b/README.md
@ -36,7 +36,7 @@ Install cross tools and firmware updating tool.
 Download arm cross tools from [here](https://developer.arm.com/tools-and-software/open-source-software/developer-tools/gnu-toolchain/gnu-rm/downloads).

    $ wget https://developer.arm.com/-/media/Files/downloads/gnu-rm/8-2018q4/gcc-arm-none-eabi-8-2018-q4-major-linux.tar.bz2
-    $ sudo tar xfj gcc-arm-none-eabi-8-2018-q4-major-linux.tar.bz2 -C /usr/local
+    $ sudo tar xfj -C /usr/local gcc-arm-none-eabi-8-2018-q4-major-linux.tar.bz2
    $ PATH=/usr/local/gcc-arm-none-eabi-8-2018-q4-major/bin:$PATH
    $ sudo apt install -y dfu-util

@ -89,7 +89,7 @@ There are seveal numbers of great companion PC tools from third-party.

 ## Documentation

-* [NanoVNA User Guide(ja)](https://cho45.github.io/NanoVNA-manual/) by cho45. [(en:google translate)](https://translate.google.com/translate?sl=ja&tl=en&u=https%3A%2F%2Fcho45.github.io%2FNanoVNA-manual%2F)
+* [NanoVNA User Guide](https://cho45.github.io/NanoVNA-manual/) [(google translate)](https://translate.google.com/translate?sl=ja&tl=en&u=https%3A%2F%2Fcho45.github.io%2FNanoVNA-manual%2F) by cho45

 ## Reference

@ -105,7 +105,6 @@ Hardware design material is disclosed to prevent bad quality clone. Please let m
 ## Authorized Distributor

 * [Nooelec](https://www.nooelec.com/store/nanovna-bundle.html)
-* Switch Science(ja) [NanoVNA-H](https://www.switch-science.com/catalog/6405/) [NanoVNA-H4](https://www.switch-science.com/catalog/6406/)

 ## Credit

@ -115,4 +114,3 @@ Hardware design material is disclosed to prevent bad quality clone. Please let m

 * [@hugen79](https://github.com/hugen79)
 * [@cho45](https://github.com/cho45)
-* [@DiSlord](https://github.com/DiSlord/)
--- a/adc.c
+++ b/adc.c
@ -28,82 +28,80 @@
 #define ADC_SMPR_SMP_239P5      7U  /**< @brief 252 cycles conversion time. */ 
 #define ADC_CFGR1_RES_12BIT             (0U << 3U)

-#define VNA_ADC     ADC1
-
 void adc_init(void)
 {
  rccEnableADC1(FALSE);

  /* Ensure flag states */
-  VNA_ADC->IER = 0;
+  ADC1->IER = 0;

  /* Calibration procedure.*/
  ADC->CCR = 0;
-  if (VNA_ADC->CR & ADC_CR_ADEN) {
-    VNA_ADC->CR |= ~ADC_CR_ADDIS; /* Disable ADC */
+  if (ADC1->CR & ADC_CR_ADEN) {
+      ADC1->CR |= ~ADC_CR_ADDIS; /* Disable ADC */
  }
-  while (VNA_ADC->CR & ADC_CR_ADEN)
+  while (ADC1->CR & ADC_CR_ADEN)
    ;
-  VNA_ADC->CFGR1 &= ~ADC_CFGR1_DMAEN;
-  VNA_ADC->CR |= ADC_CR_ADCAL;
-  while (VNA_ADC->CR & ADC_CR_ADCAL)
+  ADC1->CFGR1 &= ~ADC_CFGR1_DMAEN;
+  ADC1->CR |= ADC_CR_ADCAL;
+  while (ADC1->CR & ADC_CR_ADCAL)
    ;

-  if (VNA_ADC->ISR & ADC_ISR_ADRDY) {
-    VNA_ADC->ISR |= ADC_ISR_ADRDY; /* clear ADRDY */
+  if (ADC1->ISR & ADC_ISR_ADRDY) {
+      ADC1->ISR |= ADC_ISR_ADRDY; /* clear ADRDY */
  }
  /* Enable ADC */
-  VNA_ADC->CR |= ADC_CR_ADEN;
-  while (!(VNA_ADC->ISR & ADC_ISR_ADRDY))
+  ADC1->CR |= ADC_CR_ADEN;
+  while (!(ADC1->ISR & ADC_ISR_ADRDY))
    ;
 }

-uint16_t adc_single_read(uint32_t chsel)
+uint16_t adc_single_read(ADC_TypeDef *adc, uint32_t chsel)
 {
  /* ADC setup */
-  VNA_ADC->ISR    = VNA_ADC->ISR;
-  VNA_ADC->IER    = 0;
-  VNA_ADC->TR     = ADC_TR(0, 0);
-  VNA_ADC->SMPR   = ADC_SMPR_SMP_239P5;
-  VNA_ADC->CFGR1  = ADC_CFGR1_RES_12BIT;
-  VNA_ADC->CHSELR = chsel;
+  adc->ISR    = adc->ISR;
+  adc->IER    = 0;
+  adc->TR     = ADC_TR(0, 0);
+  adc->SMPR   = ADC_SMPR_SMP_239P5;
+  adc->CFGR1  = ADC_CFGR1_RES_12BIT;
+  adc->CHSELR = chsel;

  /* ADC conversion start.*/
-  VNA_ADC->CR |= ADC_CR_ADSTART;
+  adc->CR |= ADC_CR_ADSTART;

-  while (VNA_ADC->CR & ADC_CR_ADSTART)
+  while (adc->CR & ADC_CR_ADSTART)
    ;

-  return VNA_ADC->DR;
+  return adc->DR;
 }

-int16_t adc_vbat_read(void)
+int16_t adc_vbat_read(ADC_TypeDef *adc)
 {
-// 13.9 Temperature sensor and internal reference voltage
-// VREFINT_CAL calibrated on 3.3V, need get value in mV
 #define ADC_FULL_SCALE 3300
+#define VBAT_DIODE_VF 500
 #define VREFINT_CAL (*((uint16_t*)0x1FFFF7BA))
-  adc_stop();
-  ADC->CCR |= ADC_CCR_VREFEN | ADC_CCR_VBATEN;
-  // VREFINT == ADC_IN17
-  uint32_t vrefint = adc_single_read(ADC_CHSELR_CHSEL17);
-  // VBAT == ADC_IN18
-  // VBATEN enables resiter devider circuit. It consume vbat power.
-  uint32_t vbat = adc_single_read(ADC_CHSELR_CHSEL18);
-  ADC->CCR &= ~(ADC_CCR_VREFEN | ADC_CCR_VBATEN);
-  touch_start_watchdog();
-  // vbat_raw = (3300 * 2 * vbat / 4095) * (VREFINT_CAL / vrefint)
-  // uint16_t vbat_raw = (ADC_FULL_SCALE * VREFINT_CAL * (float)vbat * 2 / (vrefint * ((1<<12)-1)));
-  // For speed divide not on 4095, divide on 4096, get little error, but no matter
-  uint16_t vbat_raw = ((ADC_FULL_SCALE * 2 * vbat)>>12) * VREFINT_CAL / vrefint;
-  if (vbat_raw < 100) {
-    // maybe D2 is not installed
-    return -1;
-  }
-  return vbat_raw + config.vbat_offset;
+	float vbat = 0;
+	float vrefint = 0;
+
+	ADC->CCR |= ADC_CCR_VREFEN | ADC_CCR_VBATEN;
+	// VREFINT == ADC_IN17
+	vrefint = adc_single_read(adc, ADC_CHSELR_CHSEL17);
+	// VBAT == ADC_IN18
+	// VBATEN enables resiter devider circuit. It consume vbat power.
+	vbat = adc_single_read(adc, ADC_CHSELR_CHSEL18);
+	ADC->CCR &= ~(ADC_CCR_VREFEN | ADC_CCR_VBATEN);
+
+	uint16_t vbat_raw = (ADC_FULL_SCALE * VREFINT_CAL * vbat * 2 / (vrefint * ((1<<12)-1)));
+	if (vbat_raw < 100) {
+		// maybe D2 is not installed
+		return -1;
+	}
+	
+	return vbat_raw + VBAT_DIODE_VF;
+
 }

-void adc_start_analog_watchdogd(uint32_t chsel)
+void adc_start_analog_watchdogd(ADC_TypeDef *adc, uint32_t chsel)
 {
  uint32_t cfgr1;

@ -113,38 +111,38 @@ void adc_start_analog_watchdogd(uint32_t chsel)

  /* ADC setup, if it is defined a callback for the analog watch dog then it
     is enabled.*/
-  VNA_ADC->ISR    = VNA_ADC->ISR;
-  VNA_ADC->IER    = ADC_IER_AWDIE;
-  VNA_ADC->TR     = ADC_TR(0, TOUCH_THRESHOLD);
-  VNA_ADC->SMPR   = ADC_SMPR_SMP_1P5;
-  VNA_ADC->CHSELR = chsel;
+  adc->ISR    = adc->ISR;
+  adc->IER    = ADC_IER_AWDIE;
+  adc->TR     = ADC_TR(0, TOUCH_THRESHOLD);
+  adc->SMPR   = ADC_SMPR_SMP_1P5;
+  adc->CHSELR = chsel;

  /* ADC configuration and start.*/
-  VNA_ADC->CFGR1  = cfgr1;
+  adc->CFGR1  = cfgr1;

  /* ADC conversion start.*/
-  VNA_ADC->CR |= ADC_CR_ADSTART;
+  adc->CR |= ADC_CR_ADSTART;
 }

-void adc_stop(void)
+void adc_stop(ADC_TypeDef *adc)
 {
-  if (VNA_ADC->CR & ADC_CR_ADEN) {
-    if (VNA_ADC->CR & ADC_CR_ADSTART) {
-      VNA_ADC->CR |= ADC_CR_ADSTP;
-      while (VNA_ADC->CR & ADC_CR_ADSTP)
+  if (adc->CR & ADC_CR_ADEN) {
+    if (adc->CR & ADC_CR_ADSTART) {
+      adc->CR |= ADC_CR_ADSTP;
+      while (adc->CR & ADC_CR_ADSTP)
        ;
    }

-    /*    VNA_ADC->CR |= ADC_CR_ADDIS;
-    while (VNA_ADC->CR & ADC_CR_ADDIS)
+    /*    adc->CR |= ADC_CR_ADDIS;
+    while (adc->CR & ADC_CR_ADDIS)
    ;*/
  }
 }

-void adc_interrupt(void)
+void adc_interrupt(ADC_TypeDef *adc)
 {
-  uint32_t isr = VNA_ADC->ISR;
-  VNA_ADC->ISR = isr;
+  uint32_t isr = adc->ISR;
+  adc->ISR = isr;

  if (isr & ADC_ISR_OVR) {
    /* ADC overflow condition, this could happen only if the DMA is unable
@ -161,7 +159,7 @@ OSAL_IRQ_HANDLER(STM32_ADC1_HANDLER)
 {
  OSAL_IRQ_PROLOGUE();

-  adc_interrupt();
+  adc_interrupt(ADC1);

  OSAL_IRQ_EPILOGUE();
 }
--- a/chconf.h
+++ b/chconf.h
@ -147,7 +147,7 @@
 *
 * @note    The default is @p TRUE.
 */
-#define CH_CFG_USE_REGISTRY                 FALSE
+#define CH_CFG_USE_REGISTRY                 TRUE

 /**
 * @brief   Threads synchronization APIs.
@ -156,7 +156,7 @@
 *
 * @note    The default is @p TRUE.
 */
-#define CH_CFG_USE_WAITEXIT                 FALSE
+#define CH_CFG_USE_WAITEXIT                 TRUE

 /**
 * @brief   Semaphores APIs.
@ -183,7 +183,7 @@
 *
 * @note    The default is @p TRUE.
 */
-#define CH_CFG_USE_MUTEXES                  FALSE
+#define CH_CFG_USE_MUTEXES                  TRUE

 /**
 * @brief   Enables recursive behavior on mutexes.
@ -193,7 +193,7 @@
 * @note    The default is @p FALSE.
 * @note    Requires @p CH_CFG_USE_MUTEXES.
 */
-#define CH_CFG_USE_MUTEXES_RECURSIVE        FALSE
+#define CH_CFG_USE_MUTEXES_RECURSIVE        TRUE

 /**
 * @brief   Conditional Variables APIs.
@ -221,7 +221,7 @@
 *
 * @note    The default is @p TRUE.
 */
-#define CH_CFG_USE_EVENTS                   FALSE
+#define CH_CFG_USE_EVENTS                   TRUE

 /**
 * @brief   Events Flags APIs with timeout.
@ -231,7 +231,7 @@
 * @note    The default is @p TRUE.
 * @note    Requires @p CH_CFG_USE_EVENTS.
 */
-#define CH_CFG_USE_EVENTS_TIMEOUT           FALSE
+#define CH_CFG_USE_EVENTS_TIMEOUT           TRUE

 /**
 * @brief   Synchronous Messages APIs.
@ -269,7 +269,7 @@
 *
 * @note    The default is @p TRUE.
 */
-#define CH_CFG_USE_QUEUES                   FALSE
+#define CH_CFG_USE_QUEUES                   TRUE

 /**
 * @brief   Core Memory Manager APIs.
@ -530,11 +530,11 @@
 * ChibiOS/os/various/shell/shell_cmd.c
 */
 #define SHELL_CMD_EXIT_ENABLED TRUE
-#define SHELL_CMD_INFO_ENABLED FALSE
+#define SHELL_CMD_INFO_ENABLED TRUE
 #define SHELL_CMD_ECHO_ENABLED FALSE
 #define SHELL_CMD_SYSTIME_ENABLED FALSE
 #define SHELL_CMD_MEM_ENABLED FALSE
-#define SHELL_CMD_THREADS_ENABLED FALSE
+#define SHELL_CMD_THREADS_ENABLED TRUE
 #define SHELL_CMD_TEST_ENABLED FALSE


--- a/chprintf.c
+++ b/chprintf.c
@ -1,594 +0,0 @@
-/*
-    ChibiOS - Copyright (C) 2006..2016 Giovanni Di Sirio
-
-    Licensed under the Apache License, Version 2.0 (the "License");
-    you may not use this file except in compliance with the License.
-    You may obtain a copy of the License at
-
-        http://www.apache.org/licenses/LICENSE-2.0
-
-    Unless required by applicable law or agreed to in writing, software
-    distributed under the License is distributed on an "AS IS" BASIS,
-    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-    See the License for the specific language governing permissions and
-    limitations under the License.
-*/
-
-/*
-   Concepts and parts of this file have been contributed by Fabio Utzig,
-   chvprintf() added by Brent Roman.
- */
-
-/**
- * @file    chprintf.c
- * @brief   Mini printf-like functionality.
- *
- * @addtogroup chprintf
- * @{
- */
-
-#include "hal.h"
-#include "chprintf.h"
-//#include "memstreams.h"
-#include <math.h>
-
-// Enable [flags], support:
-// ' ' Prepends a space for positive signed-numeric types. positive = ' ', negative = '-'. This flag is ignored if the + flag exists.
-//#define CHPRINTF_USE_SPACE_FLAG
-
-// Force putting trailing zeros on float value
-#define CHPRINTF_FORCE_TRAILING_ZEROS
-
-#define MAX_FILLER 11
-#define FLOAT_PRECISION 9
-
-#pragma pack(push, 2)
-
-static const uint32_t pow10[FLOAT_PRECISION+1] = {
-    1, 10, 100, 1000, 10000, 100000, 1000000, 10000000, 100000000, 1000000000
-};
-// Prefixes for values bigger then 1000.0
-//                            1  1e3, 1e6, 1e9, 1e12, 1e15, 1e18, 1e21, 1e24
-static char bigPrefix[] = {' ', 'k', 'M', 'G', 'T', 'P', 'E', 'Z', 'Y', 0};
-// Prefixes for values less   then 1.0
-//                          1e-3, 1e-6, 1e-9, 1e-12, 1e-15, 1e-18, 1e-21, 1e-24
-static char smallPrefix[] = {'m', 0x1d, 'n', 'p', 'f', 'a', 'z', 'y', 0};
-
-#pragma pack(pop)
-
-static char *long_to_string_with_divisor(char *p,
-                                         uint32_t num,
-                                         uint32_t radix,
-                                         uint32_t precision) {
-  char *q = p + MAX_FILLER;
-  char *b = q;
-  // convert to string from end buffer to begin
-  do {
-    uint8_t c = num % radix;
-    num /= radix;
-    *--q = c + ((c > 9) ? ('A'-10) : '0');
-  }while((precision && --precision) || num);
-  // copy string at begin
-  int i = (int)(b - q);
-  do
-    *p++ = *q++;
-  while (--i);
-  return p;
-}
-
-// default prescision = 13
-// g.mmm kkk hhh
-#define MAX_FREQ_PRESCISION 13
-#define FREQ_PSET           1
-#define FREQ_NO_SPACE       2
-#define FREQ_PREFIX_SPACE   4
-
-static char *
-ulong_freq(char *p, uint32_t freq, uint32_t precision)
-{
-  uint8_t flag = FREQ_PSET;
-  if (precision == 0)
-    flag|=FREQ_PREFIX_SPACE;
-  if (precision == 0 || precision > MAX_FREQ_PRESCISION)
-    precision = MAX_FREQ_PRESCISION;
-  char *q = p + MAX_FREQ_PRESCISION;
-  char *b = q;
-  // Prefix counter
-  uint32_t s = 0;
-  // Set format (every 3 digits add ' ' up to GHz)
-  uint32_t format = 0b00100100100;
-  do {
-#if 0
-    uint8_t c = freq % 10;
-    freq/= 10;
-#else
-    // Fast and compact division uint32_t on 10, using shifts, result:
-    // c = freq % 10
-    // freq = freq / 10;
-    uint32_t c = freq;
-    freq >>= 1;
-    freq += freq >> 1;
-    freq += freq >> 4;
-    freq += freq >> 8;
-    freq += freq >> 16;  // freq = 858993459*freq/1073741824 = freq *
-                         // 0,799999999813735485076904296875
-    freq >>= 3;  // freq/=8; freq = freq * 0,09999999997671693563461303710938
-    c -= freq * 10;  // freq*10 = (freq*4+freq)*2 = ((freq<<2)+freq)<<1
-    while (c >= 10) {
-      freq++;
-      c -= 10;
-    }
-#endif
-    *--q = c + '0';
-    if (freq == 0) break;
-    // Add spaces, calculate prefix
-    if (format & 1) {
-      *--q = ' ';
-      s++;
-    }
-    format >>= 1;
-  } while (1);
-  s = bigPrefix[s];
-
-  // Get string size
-  uint32_t i = (b - q);
-  // Limit string size, max size is - precision
-  if (precision && i > precision) {
-    i = precision;
-    flag |= FREQ_NO_SPACE;
-  }
-  // copy string
-  // Replace first ' ' by '.', remove ' ' if size too big
-  do {
-    char c = *q++;
-    // replace first ' ' on '.'
-    if (c == ' ') {
-      if (flag & FREQ_PSET) {
-        c = '.';
-        flag &= ~FREQ_PSET;
-      } else if (flag & FREQ_NO_SPACE)
-        c = *q++;
-    }
-    *p++ = c;
-  } while (--i);
-  // Put pref (amd space before it if need)
-  if (flag & FREQ_PREFIX_SPACE && s != ' ') 
-    *p++ = ' ';
-  *p++ = s;
-  return p;
-}
-
-#if CHPRINTF_USE_FLOAT
-static char *ftoa(char *p, float num, uint32_t precision) {
-  // Check precision limit
-  if (precision > FLOAT_PRECISION)
-    precision = FLOAT_PRECISION;
-  uint32_t multi = pow10[precision];
-  uint32_t l = num;
-  // Round value
-  uint32_t k = ((num-l)*multi+0.5);
-  // Fix rounding error if get
-  if (k>=multi){k-=multi;l++;}
-  p = long_to_string_with_divisor(p, l, 10, 0);
-  if (precision) {
-    *p++ = '.';
-    p=long_to_string_with_divisor(p, k, 10, precision);
-#ifndef CHPRINTF_FORCE_TRAILING_ZEROS
-    // remove zeros at end
-    while (p[-1]=='0') p--;
-    if (p[-1]=='.') p--;
-#endif
-  }
-  return p;
-}
-
-static char *ftoaS(char *p, float num, uint32_t precision) {
-  char prefix=0;
-  char *ptr;
-  if (num > 1000.0){
-    for (ptr = bigPrefix+1; *ptr && num > 1000.0; num/=1000, ptr++)
-      ;
-    prefix = ptr[-1];
-  }
-  else if (num < 1){
-    for (ptr = smallPrefix; *ptr && num < 1.0; num*=1000, ptr++)
-      ;
-    prefix = num > 1e-3 ? ptr[-1] : 0;
-  }
-  // Auto set prescision
-  uint32_t l = num;
-  if (l < 10)
-    precision+=2;
-  else if (l < 100)
-    precision+=1;
-  p=ftoa(p, num, precision);
-  if (prefix)
-    *p++ = prefix;
-  return p;
-}
-#endif
-
-/**
- * @brief   System formatted output function.
- * @details This function implements a minimal @p vprintf()-like functionality
- *          with output on a @p BaseSequentialStream.
- *          The general parameters format is: %[-][width|*][.precision|*][l|L]p.
- *          The following parameter types (p) are supported:
- *          - <b>x</b> hexadecimal integer.
- *          - <b>X</b> hexadecimal long.
- *          - <b>o</b> octal integer.
- *          - <b>O</b> octal long.
- *          - <b>d</b> decimal signed integer.
- *          - <b>D</b> decimal signed long.
- *          - <b>u</b> decimal unsigned integer.
- *          - <b>U</b> decimal unsigned long.
- *          - <b>c</b> character.
- *          - <b>s</b> string.
- *          .
- *
- * @param[in] chp       pointer to a @p BaseSequentialStream implementing object
- * @param[in] fmt       formatting string
- * @param[in] ap        list of parameters
- * @return              The number of bytes that would have been
- *                      written to @p chp if no stream error occurs
- *
- * @api
- */
-#define IS_LONG             1
-#define LEFT_ALIGN          2
-#define POSITIVE            4
-#define NEGATIVE            8
-#define PAD_ZERO            16
-#define PLUS_SPACE          32
-#define DEFAULT_PRESCISION  64
-
-int chvprintf(BaseSequentialStream *chp, const char *fmt, va_list ap) {
-  char *p, *s, c, filler=' ';
-  int precision, width;
-  int n = 0;
-  uint32_t  state;
-  union {
-      uint32_t u;
-      int32_t  l;
-      float    f;
-  }value;
-#if CHPRINTF_USE_FLOAT
-  char tmpbuf[2*MAX_FILLER + 1];
-#else
-  char tmpbuf[MAX_FREQ_PRESCISION + 1];
-#endif
-
-  while (true) {
-    c = *fmt++;
-    if (c == 0)
-      return n;
-    if (c != '%') {
-      streamPut(chp, (uint8_t)c);
-      n++;
-      continue;
-    }
-    // Parse %[flags][width][.precision][length]type
-    p = tmpbuf;
-    s = tmpbuf;
-    state = 0;
-    width = 0;
-    precision = 0;
-
-    // Get [flags], support:
-    // '-' Left-align the output of this placeholder. (The default is to right-align the output.)
-    // '+' Prepends a  plus for positive signed-numeric types. positive = '+', negative = '-'.
-    // ' ' Prepends a space for positive signed-numeric types. positive = ' ', negative = '-'. This flag is ignored if the + flag exists.
-    // '0' When the 'width' option is specified, prepends zeros for numeric types. (The default prepends spaces.)
-    while (true){
-      if (*fmt == '-')
-        state|=LEFT_ALIGN;
-      else if (*fmt == '+')
-        state|=POSITIVE;
-      else if (*fmt == '0')
-        state|=PAD_ZERO;
-#ifdef CHPRINTF_USE_SPACE_FLAG
-      else if (*fmt == ' ')
-        state|=PLUS_SPACE;
-#endif
-      else
-        break;
-      fmt++;
-    }
-    // Get [width] - The Width field specifies a minimum number of characters to output
-    // if set *, get width as argument
-    while (true) {
-      c = *fmt++;
-      if (c >= '0' && c <= '9')
-        c -= '0';
-      else if (c == '*')
-        c = va_arg(ap, int);
-      else
-        break;
-      width = width * 10 + c;
-    }
-    // Get [.precision]
-    if (c == '.') {
-      while (true) {
-        c = *fmt++;
-        if (c >= '0' && c <= '9')
-          c -= '0';
-        else if (c == '*')
-          c = va_arg(ap, int);
-        else
-           break;
-        precision = precision * 10 + c;
-      }
-    }
-    else
-      state|=DEFAULT_PRESCISION;
-    //Get [length]
-    /*
-    if (c == 'l' || c == 'L') {
-      state|=IS_LONG;
-      if (*fmt)
-        c = *fmt++;
-    }
-    else if ((c >= 'A') && (c <= 'Z'))
-        state|=IS_LONG;
-    */
-    // Parse type
-    switch (c) {
-    case 'c':
-      state&=~PAD_ZERO;
-      *p++ = va_arg(ap, int);
-      break;
-    case 's':
-      state&=~PAD_ZERO;
-      if ((s = va_arg(ap, char *)) == 0)
-        s = "(null)";
-      if (state&DEFAULT_PRESCISION)
-        precision = 32767;
-      for (p = s; *p && (--precision >= 0); p++)
-        ;
-      break;
-    case 'D':
-    case 'd':
-    case 'I':
-    case 'i':/*
-      if (state & IS_LONG)
-        value.l = va_arg(ap, long);
-      else*/
-        value.l = va_arg(ap, uint32_t);
-      if (value.l < 0) {
-        state|=NEGATIVE;
-        *p++ = '-';
-        value.l = -value.l;
-      }
-      else if (state & POSITIVE)
-        *p++ = '+';
-#ifdef CHPRINTF_USE_SPACE_FLAG
-      else if (state & PLUS_SPACE)
-        *p++ = ' ';
-#endif
-      p = long_to_string_with_divisor(p, value.l, 10, 0);
-      break;
-    case 'q':
-      value.u = va_arg(ap, uint32_t);
-      p=ulong_freq(p, value.u, precision);
-      break;
-#if CHPRINTF_USE_FLOAT
-    case 'F':
-    case 'f':
-      value.f = va_arg(ap, double);
-      if (value.f < 0) {
-        state|=NEGATIVE;
-        *p++ = '-';
-        value.f = -value.f;
-      }
-      else if (state & POSITIVE)
-        *p++ = '+';
-#ifdef CHPRINTF_USE_SPACE_FLAG
-      else if (state & PLUS_SPACE)
-        *p++ = ' ';
-#endif
-      if (value.f == INFINITY){
-        *p++ = 0x19;
-        break;
-      }
-      p = (c=='F') ? ftoaS(p, value.f, precision) : ftoa(p, value.f, state&DEFAULT_PRESCISION ? FLOAT_PRECISION : precision);
-      break;
-#endif
-    case 'X':
-    case 'x':
-      c = 16;
-      goto unsigned_common;
-    case 'U':
-    case 'u':
-      c = 10;
-      goto unsigned_common;
-    case 'O':
-    case 'o':
-      c = 8;
-unsigned_common:/*
-      if (state & IS_LONG)
-        value.u = va_arg(ap, unsigned long);
-      else*/
-        value.u = va_arg(ap, uint32_t);
-      p = long_to_string_with_divisor(p, value.u, c, 0);
-      break;
-    default:
-      *p++ = c;
-      break;
-    }
-    // Now need print buffer s[{sign}XXXXXXXXXXXX]p and align it on width
-    // Check filler width (if buffer less then width) and prepare filler if need fill
-    if ((width -=(int)(p - s)) < 0)
-      width = 0;
-    else
-      filler = (state&PAD_ZERO) ? '0' : ' ';
-    // if left align, put sign digit, and fill
-    // [{sign}ffffffXXXXXXXXXXXX]
-    if (!(state&LEFT_ALIGN)) {
-      // Put '+' or '-' or ' ' first if need
-      if ((state&(NEGATIVE|POSITIVE|PLUS_SPACE)) && (state&PAD_ZERO)) {
-        streamPut(chp, (uint8_t)*s++);
-        n++;
-      }
-      // fill from left
-      while (width){
-        streamPut(chp, (uint8_t)filler);
-        n++;
-        width--;
-      }
-    }
-    // put data
-    while (s < p) {
-      streamPut(chp, (uint8_t)*s++);
-      n++;
-    }
-    // Put filler from right (if need)
-    while (width) {
-      streamPut(chp, (uint8_t)filler);
-      n++;
-      width--;
-    }
-  }
-}
-
-/**
- * @brief   System formatted output function.
- * @details This function implements a minimal @p printf() like functionality
- *          with output on a @p BaseSequentialStream.
- *          The general parameters format is: %[-][width|*][.precision|*][l|L]p.
- *          The following parameter types (p) are supported:
- *          - <b>x</b> hexadecimal integer.
- *          - <b>X</b> hexadecimal long.
- *          - <b>o</b> octal integer.
- *          - <b>O</b> octal long.
- *          - <b>d</b> decimal signed integer.
- *          - <b>D</b> decimal signed long.
- *          - <b>u</b> decimal unsigned integer.
- *          - <b>U</b> decimal unsigned long.
- *          - <b>c</b> character.
- *          - <b>s</b> string.
- *          .
- *
- * @param[in] chp       pointer to a @p BaseSequentialStream implementing object
- * @param[in] fmt       formatting string
- *
- * @api
- */
-#if 0
-int chprintf(BaseSequentialStream *chp, const char *fmt, ...) {
-  va_list ap;
-  int formatted_bytes;
-
-  va_start(ap, fmt);
-  formatted_bytes = chvprintf(chp, fmt, ap);
-  va_end(ap);
-
-  return formatted_bytes;
-}
-#endif
-/**
- * @brief   System formatted output function.
- * @details This function implements a minimal @p vprintf()-like functionality
- *          with output on a @p BaseSequentialStream.
- *          The general parameters format is: %[-][width|*][.precision|*][l|L]p.
- *          The following parameter types (p) are supported:
- *          - <b>x</b> hexadecimal integer.
- *          - <b>X</b> hexadecimal long.
- *          - <b>o</b> octal integer.
- *          - <b>O</b> octal long.
- *          - <b>d</b> decimal signed integer.
- *          - <b>D</b> decimal signed long.
- *          - <b>u</b> decimal unsigned integer.
- *          - <b>U</b> decimal unsigned long.
- *          - <b>c</b> character.
- *          - <b>s</b> string.
- *          .
- * @post    @p str is NUL-terminated, unless @p size is 0.
- *
- * @param[in] str       pointer to a buffer
- * @param[in] size      maximum size of the buffer
- * @param[in] fmt       formatting string
- * @return              The number of characters (excluding the
- *                      terminating NUL byte) that would have been
- *                      stored in @p str if there was room.
- *
- * @api
- */
-#if 0
-int chsnprintf(char *str, size_t size, const char *fmt, ...) {
-  va_list ap;
-  MemoryStream ms;
-  BaseSequentialStream *chp;
-  size_t size_wo_nul;
-  int retval;
-
-  if (size > 0)
-    size_wo_nul = size - 1;
-  else
-    size_wo_nul = 0;
-
-  /* Memory stream object to be used as a string writer, reserving one
-     byte for the final zero.*/
-  msObjectInit(&ms, (uint8_t *)str, size_wo_nul, 0);
-
-  /* Performing the print operation using the common code.*/
-  chp = (BaseSequentialStream *)(void *)&ms;
-  va_start(ap, fmt);
-  retval = chvprintf(chp, fmt, ap);
-  va_end(ap);
-
-  /* Terminate with a zero, unless size==0.*/
-  if (ms.eos < size)
-      str[ms.eos] = 0;
-
-  /* Return number of bytes that would have been written.*/
-  return retval;
-}
-#endif
-
-//
-// Small memory stream object, only put function
-//
-struct printStreamVMT {
-  _base_sequential_stream_methods
-};
-
-typedef struct {
-  const struct printStreamVMT *vmt;
-  uint8_t  *buffer;
-  uint16_t size;
-} printStream;
-
-static msg_t put(void *ip, uint8_t b) {
-  printStream *ps = ip;
-  if (ps->size > 1){
-   *(ps->buffer++) = b;
-   ps->size--;
-  }
-  return MSG_OK;
-}
-
-static const struct printStreamVMT vmt = {NULL, NULL, put, NULL};
-void printObjectInit(printStream *ps, int size, uint8_t *buffer){
-  ps->vmt    = &vmt;
-  ps->buffer = buffer;
-  ps->size   = size;
-}
-// Simple print in buffer function
-int plot_printf(char *str, int size, const char *fmt, ...) {
-  va_list ap;
-  printStream ps;
-  int retval;
-  if (size <= 0) return 0;
-  // Init small memory stream for print
-  printObjectInit(&ps, size, (uint8_t *)str);
-  // Performing the print operation using the common code.
-  va_start(ap, fmt);
-  retval = chvprintf((BaseSequentialStream *)(void *)&ps, fmt, ap);
-  va_end(ap);
-  *(ps.buffer)=0;
-  if (retval > size-1) retval = size-1;
-  // Return number of bytes that would have been written.
-  return retval;
-}
-
-/** @} */
--- a/dsp.c
+++ b/dsp.c
@ -41,10 +41,10 @@ const int16_t sincos_tbl[48][2] = {
  {-24636, -21605 }, {-32698,  -2143 }, {-27246,  18205 }, {-10533,  31029 }
 };

-float acc_samp_s;
-float acc_samp_c;
-float acc_ref_s;
-float acc_ref_c;
+int32_t acc_samp_s;
+int32_t acc_samp_c;
+int32_t acc_ref_s;
+int32_t acc_ref_c;

 void
 dsp_process(int16_t *capture, size_t length)
@ -79,10 +79,10 @@ dsp_process(int16_t *capture, size_t length)
    ref_c = __SMLATT(sr, sc, ref_c);
 #endif
  }
-  acc_samp_s += samp_s;
-  acc_samp_c += samp_c;
-  acc_ref_s += ref_s;
-  acc_ref_c += ref_c;
+  acc_samp_s = samp_s;
+  acc_samp_c = samp_c;
+  acc_ref_s = ref_s;
+  acc_ref_c = ref_c;
 }

 void
--- a/fft.h
+++ b/fft.h
@ -68,8 +68,8 @@ static void fft256(float array[][2], const uint8_t dir) {
 			uint16_t j, k;
 			for (j = i, k = 0; j < i + halfsize; j++, k += tablestep) {
 				uint16_t l = j + halfsize;
-				float tpre =  array[l][real] * cos(2 * VNA_PI * k / 256) + array[l][imag] * sin(2 * VNA_PI * k / 256);
-				float tpim = -array[l][real] * sin(2 * VNA_PI * k / 256) + array[l][imag] * cos(2 * VNA_PI * k / 256);
+				float tpre =  array[l][real] * cos(2 * M_PI * k / 256) + array[l][imag] * sin(2 * M_PI * k / 256);
+				float tpim = -array[l][real] * sin(2 * M_PI * k / 256) + array[l][imag] * cos(2 * M_PI * k / 256);
 				array[l][real] = array[j][real] - tpre;
 				array[l][imag] = array[j][imag] - tpim;
 				array[j][real] += tpre;
--- a/flash.c
+++ b/flash.c
@ -32,12 +32,12 @@ static int flash_wait_for_last_operation(void)

 static void flash_erase_page0(uint32_t page_address)
 {
-  flash_wait_for_last_operation();
-  FLASH->CR |= FLASH_CR_PER;
-  FLASH->AR = page_address;
-  FLASH->CR |= FLASH_CR_STRT;
-  flash_wait_for_last_operation();
-  FLASH->CR &= ~FLASH_CR_PER;
+	flash_wait_for_last_operation();
+	FLASH->CR |= FLASH_CR_PER;
+	FLASH->AR = page_address;
+	FLASH->CR |= FLASH_CR_STRT;
+	flash_wait_for_last_operation();
+	FLASH->CR &= ~FLASH_CR_PER;
 }

 int flash_erase_page(uint32_t page_address)
@ -50,11 +50,11 @@ int flash_erase_page(uint32_t page_address)

 void flash_program_half_word(uint32_t address, uint16_t data)
 {
-  flash_wait_for_last_operation();
-  FLASH->CR |= FLASH_CR_PG;
-  *(__IO uint16_t*)address = data;
-  flash_wait_for_last_operation();
-  FLASH->CR &= ~FLASH_CR_PG;
+	flash_wait_for_last_operation();
+	FLASH->CR |= FLASH_CR_PG;
+    *(__IO uint16_t*)address = data;
+	flash_wait_for_last_operation();
+	FLASH->CR &= ~FLASH_CR_PG;
 }

 void flash_unlock(void)
@ -64,6 +64,7 @@ void flash_unlock(void)
  FLASH->KEYR = 0xCDEF89AB;
 }

+
 static uint32_t
 checksum(const void *start, size_t len)
 {
@ -71,14 +72,14 @@ checksum(const void *start, size_t len)
  uint32_t *tail = (uint32_t*)(start + len);
  uint32_t value = 0;
  while (p < tail)
-    value = __ROR(value, 31) + *p++;
+    value ^= *p++;
  return value;
 }


 #define FLASH_PAGESIZE 0x800

-const uint32_t save_config_area = SAVE_CONFIG_ADDR;
+const uint32_t save_config_area = 0x08018000;

 int
 config_save(void)
@ -88,7 +89,8 @@ config_save(void)
  int count = sizeof(config_t) / sizeof(uint16_t);

  config.magic = CONFIG_MAGIC;
-  config.checksum = checksum(&config, sizeof config - sizeof config.checksum);
+  config.checksum = 0;
+  config.checksum = checksum(&config, sizeof config);

  flash_unlock();

@ -96,7 +98,7 @@ config_save(void)
  flash_erase_page((uint32_t)dst);

  /* write to flahs */
-  while (count-- > 0) {
+  while(count-- > 0) {
    flash_program_half_word((uint32_t)dst, *src++);
    dst++;
  }
@ -112,7 +114,7 @@ config_recall(void)

  if (src->magic != CONFIG_MAGIC)
    return -1;
-  if (checksum(src, sizeof *src - sizeof src->checksum) != src->checksum)
+  if (checksum(src, sizeof(config_t)) != 0)
    return -1;

  /* duplicated saved data onto sram to be able to modify marker/trace */
@ -120,15 +122,14 @@ config_recall(void)
  return 0;
 }

-const uint32_t saveareas[SAVEAREA_MAX] = {
-  SAVE_PROP_CONFIG_0_ADDR,
-  SAVE_PROP_CONFIG_1_ADDR,
-  SAVE_PROP_CONFIG_2_ADDR,
-  SAVE_PROP_CONFIG_3_ADDR,
-  SAVE_PROP_CONFIG_4_ADDR };
+#define SAVEAREA_MAX 5
+
+const uint32_t saveareas[] =
+  { 0x08018800, 0x0801a000, 0x0801b800, 0x0801d000, 0x0801e800 };

 int16_t lastsaveid = 0;

+
 int
 caldata_save(int id)
 {
@ -141,8 +142,8 @@ caldata_save(int id)
  dst = (uint16_t*)saveareas[id];

  current_props.magic = CONFIG_MAGIC;
-  current_props.checksum = checksum(
-      &current_props, sizeof current_props - sizeof current_props.checksum);
+  current_props.checksum = 0;
+  current_props.checksum = checksum(&current_props, sizeof current_props);

  flash_unlock();

@ -155,7 +156,7 @@ caldata_save(int id)
  }

  /* write to flahs */
-  while (count-- > 0) {
+  while(count-- > 0) {
    flash_program_half_word((uint32_t)dst, *src++);
    dst++;
  }
@ -174,15 +175,15 @@ caldata_recall(int id)
  void *dst = &current_props;

  if (id < 0 || id >= SAVEAREA_MAX)
-    goto load_default;
+    return -1;

  // point to saved area on the flash memory
  src = (properties_t*)saveareas[id];

  if (src->magic != CONFIG_MAGIC)
-    goto load_default;
-  if (checksum(src, sizeof *src - sizeof src->checksum) != src->checksum)
-    goto load_default;
+    return -1;
+  if (checksum(src, sizeof(properties_t)) != 0)
+    return -1;

  /* active configuration points to save data on flash memory */
  active_props = src;
@ -190,10 +191,8 @@ caldata_recall(int id)

  /* duplicated saved data onto sram to be able to modify marker/trace */
  memcpy(dst, src, sizeof(properties_t));
+
  return 0;
-load_default:
-  load_default_properties();
-  return -1;
 }

 const properties_t *
@ -206,12 +205,12 @@ caldata_ref(int id)

  if (src->magic != CONFIG_MAGIC)
    return NULL;
-  if (checksum(src, sizeof *src - sizeof src->checksum) != src->checksum)
+  if (checksum(src, sizeof(properties_t)) != 0)
    return NULL;
  return src;
 }

-const uint32_t save_config_prop_area_size = SAVE_CONFIG_AREA_SIZE;
+const uint32_t save_config_prop_area_size = 0x8000;

 void
 clear_all_config_prop_data(void)
--- a/halconf.h
+++ b/halconf.h
@ -118,7 +118,7 @@
 * @brief   Enables the RTC subsystem.
 */
 #if !defined(HAL_USE_RTC) || defined(__DOXYGEN__)
-#define HAL_USE_RTC                 FALSE
+#define HAL_USE_RTC                 TRUE
 #endif

 /**
@ -132,7 +132,7 @@
 * @brief   Enables the SERIAL subsystem.
 */
 #if !defined(HAL_USE_SERIAL) || defined(__DOXYGEN__)
-#define HAL_USE_SERIAL              FALSE
+#define HAL_USE_SERIAL              TRUE
 #endif

 /**
--- a/ili9341.c
+++ b/ili9341.c
--- a/main.c
+++ b/main.c
--- a/mcuconf.h
+++ b/mcuconf.h
@ -58,8 +58,7 @@
 #define STM32_ADCSW                         STM32_ADCSW_HSI14
 #define STM32_USBSW                         STM32_USBSW_HSI48
 #define STM32_CECSW                         STM32_CECSW_HSI
-//#define STM32_I2C1SW                        STM32_I2C1SW_HSI
-#define STM32_I2C1SW                        STM32_I2C1SW_SYSCLK
+#define STM32_I2C1SW                        STM32_I2C1SW_HSI
 #define STM32_USART1SW                      STM32_USART1SW_PCLK
 #define STM32_RTCSEL                        STM32_RTCSEL_LSI

@ -96,7 +95,7 @@
 #define STM32_EXT_EXTI17_IRQ_PRIORITY       3
 #define STM32_EXT_EXTI21_22_IRQ_PRIORITY    3

-#define STM32_DISABLE_EXTI2122_HANDLER      TRUE
+#define STM32_DISABLE_EXTI2122_HANDLER 		TRUE

 /*
 * GPT driver system settings.
@ -118,9 +117,7 @@
 #define STM32_I2C_BUSY_TIMEOUT              50
 #define STM32_I2C_I2C1_IRQ_PRIORITY         3
 #define STM32_I2C_I2C2_IRQ_PRIORITY         3
-
-// I2C1 rx operation use DMA3, some as SPI1 DMA Tx used by LCD
-#define STM32_I2C_USE_DMA                   FALSE
+#define STM32_I2C_USE_DMA                   TRUE
 #define STM32_I2C_I2C1_DMA_PRIORITY         1
 #define STM32_I2C_I2C2_DMA_PRIORITY         1
 #define STM32_I2C_I2C1_RX_DMA_STREAM        STM32_DMA_STREAM_ID(1, 3)
--- a/nanovna.h
+++ b/nanovna.h
@ -19,26 +19,11 @@
 */
 #include "ch.h"

-// Need enable HAL_USE_SPI in halconf.h
-#define __USE_DISPLAY_DMA__
-
 /*
 * main.c
 */

-// Minimum frequency set
-#define START_MIN                50000
-// Maximum frequency set
-#define STOP_MAX                 2700000000U
-// Frequency offset (sin_cos table in dsp.c generated for this offset, if change need create new table)
-#define FREQUENCY_OFFSET         5000
-// Speed of light const
-#define SPEED_OF_LIGHT           299792458
-// pi const
-#define VNA_PI                   3.14159265358979323846
-
-#define POINTS_COUNT 101
-extern float measured[2][POINTS_COUNT][2];
+extern float measured[2][101][2];

 #define CAL_LOAD 0
 #define CAL_OPEN 1
@ -82,23 +67,27 @@ extern float measured[2][POINTS_COUNT][2];
 void cal_collect(int type);
 void cal_done(void);

-#define MAX_FREQ_TYPE 5
-enum stimulus_type {
-  ST_START=0, ST_STOP, ST_CENTER, ST_SPAN, ST_CW
+enum {
+  ST_START, ST_STOP, ST_CENTER, ST_SPAN, ST_CW
 };

-void set_sweep_frequency(int type, uint32_t frequency);
+void set_sweep_frequency(int type, int32_t frequency);
 uint32_t get_sweep_frequency(int type);

-double my_atof(const char *p);
+float my_atof(const char *p);

 void toggle_sweep(void);
-void load_default_properties(void);

-#define SWEEP_ENABLE  0x01
-#define SWEEP_ONCE    0x02
-extern int8_t sweep_mode;
-extern const char *info_about[];
+extern int8_t sweep_enabled;
+
+/*
+ * ui.c
+ */
+extern void ui_init(void);
+extern void ui_process(void);
+
+enum { OP_NONE = 0, OP_LEVER, OP_TOUCH, OP_FREQCHANGE };
+extern uint8_t operation_requested;

 /*
 * dsp.c
@ -122,6 +111,9 @@ void calculate_gamma(float *gamma);
 void fetch_amplitude(float *gamma);
 void fetch_amplitude_ref(float *gamma);

+int si5351_set_frequency_with_offset(uint32_t freq, int offset, uint8_t drive_strength);
+
+
 /*
 * tlv320aic3204.c
 */
@ -130,74 +122,42 @@ extern void tlv320aic3204_init(void);
 extern void tlv320aic3204_set_gain(int lgain, int rgain);
 extern void tlv320aic3204_select(int channel);

+
 /*
 * plot.c
 */
+#define OFFSETX 15
+#define OFFSETY 0
+#define WIDTH 291
+#define HEIGHT 233

-// Offset of plot area
-#define OFFSETX 10
-#define OFFSETY  0
-
-// WIDTH better be n*(POINTS_COUNT-1)
-#define WIDTH  300
-// HEIGHT = 8*GRIDY
-#define HEIGHT 232
-
-//#define NGRIDY 10
-#define NGRIDY 8
-
-#define FREQUENCIES_XPOS1 OFFSETX
-#define FREQUENCIES_XPOS2 200
-#define FREQUENCIES_YPOS  (240-7)
-
-// GRIDX calculated depends from frequency span
-//#define GRIDY 29
-#define GRIDY (HEIGHT / NGRIDY)
-
-//
 #define CELLOFFSETX 5
-#define AREA_WIDTH_NORMAL  (CELLOFFSETX + WIDTH  + 1 + 4)
-#define AREA_HEIGHT_NORMAL (              HEIGHT + 1)
+#define AREA_WIDTH_NORMAL (WIDTH + CELLOFFSETX*2)

-// Smith/polar chart
-#define P_CENTER_X (CELLOFFSETX + WIDTH/2)
-#define P_CENTER_Y (HEIGHT/2)
-#define P_RADIUS   (HEIGHT/2)
+extern int area_width;
+extern int area_height;

-extern int16_t area_width;
-extern int16_t area_height;
+#define GRIDY 29

 // font
+
 extern const uint8_t x5x7_bits [];
-#define FONT_GET_DATA(ch)   (&x5x7_bits[ch*7])
-#define FONT_GET_WIDTH(ch)  (8-(x5x7_bits[ch*7]&7))
-#define FONT_MAX_WIDTH      7
-#define FONT_GET_HEIGHT     7
+extern const uint8_t numfont20x22[][22 * 3];

-extern const uint16_t numfont16x22[];
-#define NUM_FONT_GET_DATA(ch)   (&numfont16x22[ch*22])
-#define NUM_FONT_GET_WIDTH      16
-#define NUM_FONT_GET_HEIGHT     22
-
-#define S_DELTA "\004"
-#define S_DEGREE "\037"
-#define S_SARROW "\030"
-#define S_INFINITY "\031"
-#define S_LARROW "\032"
-#define S_RARROW "\033"
 #define S_PI    "\034"
 #define S_MICRO "\035"
 #define S_OHM   "\036"
+#define S_DEGREE "\037"
+#define S_LARROW "\032"
+#define S_RARROW "\033"
+
 // trace 

 #define TRACES_MAX 4

-#define MAX_TRACE_TYPE 12
-enum trace_type {
-  TRC_LOGMAG=0, TRC_PHASE, TRC_DELAY, TRC_SMITH, TRC_POLAR, TRC_LINEAR, TRC_SWR, TRC_REAL, TRC_IMAG, TRC_R, TRC_X, TRC_Q, TRC_OFF
+enum {
+  TRC_LOGMAG, TRC_PHASE, TRC_DELAY, TRC_SMITH, TRC_POLAR, TRC_LINEAR, TRC_SWR, TRC_REAL, TRC_IMAG, TRC_R, TRC_X, TRC_OFF
 };
-// Mask for define rectangular plot
-#define RECTANGULAR_GRID_MASK ((1<<TRC_LOGMAG)|(1<<TRC_PHASE)|(1<<TRC_DELAY)|(1<<TRC_LINEAR)|(1<<TRC_SWR)|(1<<TRC_REAL)|(1<<TRC_IMAG)|(1<<TRC_R)|(1<<TRC_X)|(1<<TRC_Q))

 // LOGMAG: SCALE, REFPOS, REFVAL
 // PHASE: SCALE, REFPOS, REFVAL
@ -209,35 +169,32 @@ enum trace_type {
 // Electrical Delay
 // Phase

-typedef struct trace {
+typedef struct {
  uint8_t enabled;
  uint8_t type;
  uint8_t channel;
-  uint8_t reserved;
+  uint8_t polar;
  float scale;
  float refpos;
 } trace_t;

-#define FREQ_MODE_START_STOP    0x0
-#define FREQ_MODE_CENTER_SPAN   0x1
-#define FREQ_MODE_DOTTED_GRID   0x2
-
-typedef struct config {
+typedef struct {
  int32_t magic;
  uint16_t dac_value;
  uint16_t grid_color;
  uint16_t menu_normal_color;
  uint16_t menu_active_color;
  uint16_t trace_color[TRACES_MAX];
-  int16_t  touch_cal[4];
+  int16_t touch_cal[4];
+  int8_t default_loadcal;
  uint32_t harmonic_freq_threshold;
-  uint16_t vbat_offset;
-  uint8_t _reserved[22];
-  uint32_t checksum;
+  int32_t checksum;
 } config_t;

 extern config_t config;

+//extern trace_t trace[TRACES_MAX];
+
 void set_trace_type(int t, int type);
 void set_trace_channel(int t, int channel);
 void set_trace_scale(int t, float scale);
@ -245,23 +202,21 @@ void set_trace_refpos(int t, float refpos);
 float get_trace_scale(int t);
 float get_trace_refpos(int t);
 const char *get_trace_typename(int t);
+void draw_battery_status(void);

 void set_electrical_delay(float picoseconds);
 float get_electrical_delay(void);
-float groupdelay_from_array(int i, float array[POINTS_COUNT][2]);

 // marker

-#define MARKERS_MAX 4
-
-typedef struct marker {
+typedef struct {
  int8_t enabled;
  int16_t index;
  uint32_t frequency;
 } marker_t;

-extern int8_t previous_marker;
-extern int8_t marker_tracking;
+//extern marker_t markers[4];
+//extern int active_marker;

 void plot_init(void);
 void update_grid(void);
@ -270,121 +225,87 @@ void redraw_frame(void);
 //void redraw_all(void);
 void request_to_draw_cells_behind_menu(void);
 void request_to_draw_cells_behind_numeric_input(void);
-void redraw_marker(int marker);
-void plot_into_index(float measured[2][POINTS_COUNT][2]);
+void redraw_marker(int marker, int update_info);
+void trace_get_info(int t, char *buf, int len);
+void plot_into_index(float measured[2][101][2]);
 void force_set_markmap(void);
 void draw_frequencies(void);
 void draw_all(bool flush);

 void draw_cal_status(void);

-//void markmap_all_markers(void);
+void markmap_all_markers(void);

 void marker_position(int m, int t, int *x, int *y);
 int search_nearest_index(int x, int y, int t);
-void set_marker_search(int mode);
-int marker_search(void);
+int marker_search(int mode);
 int marker_search_left(int from);
 int marker_search_right(int from);

-// _request flag for update screen
+extern uint16_t redraw_request;
+
 #define REDRAW_CELLS      (1<<0)
 #define REDRAW_FREQUENCY  (1<<1)
 #define REDRAW_CAL_STATUS (1<<2)
 #define REDRAW_MARKER     (1<<3)
-#define REDRAW_BATTERY    (1<<4)
-#define REDRAW_AREA       (1<<5)
-extern volatile uint8_t redraw_request;
+
+extern int16_t vbat;

 /*
 * ili9341.c
 */
-// SPI bus revert byte order
-//gggBBBbb RRRrrGGG
-#define RGB565(r,g,b)  ( (((g)&0x1c)<<11) | (((b)&0xf8)<<5) | ((r)&0xf8) | (((g)&0xe0)>>5) )
-#define RGBHEX(hex) ( (((hex)&0x001c00)<<3) | (((hex)&0x0000f8)<<5) | (((hex)&0xf80000)>>16) | (((hex)&0x00e000)>>13) )
+#define RGB565(b,r,g)     ( (((b)<<8)&0xfc00) | (((r)<<2)&0x03e0) | (((g)>>3)&0x001f) )

-// Define size of screen buffer in pixels (one pixel 16bit size)
-#define SPI_BUFFER_SIZE             2048
+typedef struct {
+	uint16_t width;
+	uint16_t height;
+	uint16_t scaley;
+	uint16_t slide;
+	const uint8_t *bitmap;
+} font_t;

-#define DEFAULT_FG_COLOR            RGB565(255,255,255)
-#define DEFAULT_BG_COLOR            RGB565(  0,  0,  0)
-#define DEFAULT_GRID_COLOR          RGB565(128,128,128)
-#define DEFAULT_MENU_COLOR          RGB565(255,255,255)
-#define DEFAULT_MENU_TEXT_COLOR     RGB565(  0,  0,  0)
-#define DEFAULT_MENU_ACTIVE_COLOR   RGB565(180,255,180)
-#define DEFAULT_TRACE_1_COLOR       RGB565(255,255,  0)
-#define DEFAULT_TRACE_2_COLOR       RGB565(  0,255,255)
-#define DEFAULT_TRACE_3_COLOR       RGB565(  0,255,  0)
-#define DEFAULT_TRACE_4_COLOR       RGB565(255,  0,255)
-#define DEFAULT_NORMAL_BAT_COLOR    RGB565( 31,227,  0)
-#define DEFAULT_LOW_BAT_COLOR       RGB565(255,  0,  0)
-#define DEFAULT_SPEC_INPUT_COLOR    RGB565(128,255,128);
+extern const font_t NF20x22;

-extern uint16_t foreground_color;
-extern uint16_t background_color;
-
-extern uint16_t spi_buffer[SPI_BUFFER_SIZE];
+extern uint16_t spi_buffer[1024];

 void ili9341_init(void);
 void ili9341_test(int mode);
 void ili9341_bulk(int x, int y, int w, int h);
 void ili9341_fill(int x, int y, int w, int h, int color);
-void ili9341_set_foreground(uint16_t fg);
-void ili9341_set_background(uint16_t fg);
-void ili9341_clear_screen(void);
-void blit8BitWidthBitmap(uint16_t x, uint16_t y, uint16_t width, uint16_t height, const uint8_t *bitmap);
-void ili9341_drawchar(uint8_t ch, int x, int y);
-void ili9341_drawstring(const char *str, int x, int y);
-void ili9341_drawstringV(const char *str, int x, int y);
-int  ili9341_drawchar_size(uint8_t ch, int x, int y, uint8_t size);
-void ili9341_drawstring_size(const char *str, int x, int y, uint8_t size);
-void ili9341_drawfont(uint8_t ch, int x, int y);
+void ili9341_drawchar_5x7(uint8_t ch, int x, int y, uint16_t fg, uint16_t bg);
+void ili9341_drawstring_5x7(const char *str, int x, int y, uint16_t fg, uint16_t bg);
+void ili9341_drawstring_5x7_inv(const char *str, int x, int y, uint16_t fg, uint16_t bg, bool inv);
+void ili9341_drawchar_size(uint8_t ch, int x, int y, uint16_t fg, uint16_t bg, uint8_t size);
+void ili9341_drawstring_size(const char *str, int x, int y, uint16_t fg, uint16_t bg, uint8_t size);
+void ili9341_drawfont(uint8_t ch, const font_t *font, int x, int y, uint16_t fg, uint16_t bg);
 void ili9341_read_memory(int x, int y, int w, int h, int len, uint16_t* out);
-void ili9341_line(int x0, int y0, int x1, int y1);
-void show_version(void);
-void show_logo(void);
+void ili9341_read_memory_continue(int len, uint16_t* out);
+

 /*
 * flash.c
 */
 #define SAVEAREA_MAX 5
-// Begin addr                   0x08018000
-#define SAVE_CONFIG_AREA_SIZE   0x00008000
-// config save area
-#define SAVE_CONFIG_ADDR        0x08018000
-// properties_t save area
-#define SAVE_PROP_CONFIG_0_ADDR 0x08018800
-#define SAVE_PROP_CONFIG_1_ADDR 0x0801a000
-#define SAVE_PROP_CONFIG_2_ADDR 0x0801b800
-#define SAVE_PROP_CONFIG_3_ADDR 0x0801d000
-#define SAVE_PROP_CONFIG_4_ADDR 0x0801e800

-typedef struct properties {
-  uint32_t magic;
-  uint32_t _frequency0;
-  uint32_t _frequency1;
-  uint16_t _sweep_points;
+typedef struct {
+  int32_t magic;
+  int32_t _frequency0; // start or center
+  int32_t _frequency1; // stop or span
+  int16_t _sweep_points;
  uint16_t _cal_status;

-  uint32_t _frequencies[POINTS_COUNT];
-  float _cal_data[5][POINTS_COUNT][2];
+  uint32_t _frequencies[101];
+  float _cal_data[5][101][2];
  float _electrical_delay; // picoseconds
  
  trace_t _trace[TRACES_MAX];
-  marker_t _markers[MARKERS_MAX];
-
-  float _velocity_factor; // %
-  int8_t _active_marker;
+  marker_t _markers[4];
+  int _active_marker;
  uint8_t _domain_mode; /* 0bxxxxxffm : where ff: TD_FUNC m: DOMAIN_MODE */
-  uint8_t _marker_smith_format;
-  uint8_t _bandwidth;
-  int8_t _freq_mode;
-  uint8_t _reserved[49];
-  uint32_t checksum;
-} properties_t;
+  uint8_t _velocity_factor; // %

-//sizeof(properties_t) == 0x1200
+  int32_t checksum;
+} properties_t;

 #define CONFIG_MAGIC 0x434f4e45 /* 'CONF' */

@ -392,6 +313,8 @@ extern int16_t lastsaveid;
 extern properties_t *active_props;
 extern properties_t current_props;

+extern int8_t previous_marker;
+
 #define frequency0 current_props._frequency0
 #define frequency1 current_props._frequency1
 #define sweep_points current_props._sweep_points
@ -405,13 +328,6 @@ extern properties_t current_props;
 #define active_marker current_props._active_marker
 #define domain_mode current_props._domain_mode
 #define velocity_factor current_props._velocity_factor
-#define marker_smith_format current_props._marker_smith_format
-#define bandwidth current_props._bandwidth
-#define freq_mode current_props._freq_mode
-
-#define FREQ_IS_STARTSTOP() (!(freq_mode & FREQ_MODE_CENTER_SPAN))
-#define FREQ_IS_CENTERSPAN() (freq_mode & FREQ_MODE_CENTER_SPAN)
-#define FREQ_IS_CW() (frequency0 == frequency1)

 int caldata_save(int id);
 int caldata_recall(int id);
@ -425,42 +341,29 @@ void clear_all_config_prop_data(void);
 /*
 * ui.c
 */
-extern void ui_init(void);
-extern void ui_process(void);
-
-// Irq operation process set
-#define OP_NONE       0x00
-#define OP_LEVER      0x01
-#define OP_TOUCH      0x02
-//#define OP_FREQCHANGE 0x04
-extern volatile uint8_t operation_requested;

 // lever_mode
-enum lever_mode {
-  LM_MARKER, LM_SEARCH, LM_CENTER, LM_SPAN, LM_EDELAY
+enum {
+  LM_MARKER, LM_SEARCH, LM_CENTER, LM_SPAN
 };

-// marker smith value format
-enum marker_smithvalue {
-  MS_LIN, MS_LOG, MS_REIM, MS_RX, MS_RLC
-};
-
-typedef struct uistat {
+typedef struct {
  int8_t digit; /* 0~5 */
  int8_t digit_mode;
  int8_t current_trace; /* 0..3 */
  uint32_t value; // for editing at numeric input area
-//  uint32_t previous_value;
+  uint32_t previous_value;
  uint8_t lever_mode;
-  uint8_t marker_delta;
-  uint8_t marker_tracking;
 } uistat_t;

 extern uistat_t uistat;
+  
 void ui_init(void);
 void ui_show(void);
 void ui_hide(void);

+extern uint8_t operation_requested;
+
 void touch_start_watchdog(void);
 void touch_position(int *x, int *y);
 void handle_touch_interrupt(void);
@ -476,22 +379,25 @@ void enter_dfu(void);
 */

 void adc_init(void);
-uint16_t adc_single_read(uint32_t chsel);
-void adc_start_analog_watchdogd(uint32_t chsel);
-void adc_stop(void);
-void adc_interrupt(void);
-int16_t adc_vbat_read(void);
+uint16_t adc_single_read(ADC_TypeDef *adc, uint32_t chsel);
+void adc_start_analog_watchdogd(ADC_TypeDef *adc, uint32_t chsel);
+void adc_stop(ADC_TypeDef *adc);
+void adc_interrupt(ADC_TypeDef *adc);
+int16_t adc_vbat_read(ADC_TypeDef *adc);

 /*
 * misclinous
 */
-int plot_printf(char *str, int, const char *fmt, ...);
 #define PULSE do { palClearPad(GPIOC, GPIOC_LED); palSetPad(GPIOC, GPIOC_LED);} while(0)

-// Speed profile definition
-#define START_PROFILE   systime_t time = chVTGetSystemTimeX();
-#define STOP_PROFILE    {char string_buf[12];plot_printf(string_buf, sizeof string_buf, "T:%06d", chVTGetSystemTimeX() - time);ili9341_drawstringV(string_buf, 1, 60);}
-// Macros for convert define value to string
-#define STR1(x)  #x
-#define define_to_STR(x)  STR1(x)
+// convert vbat [mV] to battery indicator
+static inline uint8_t vbat2bati(int16_t vbat)
+{
+	if (vbat < 3200) return 0;
+	if (vbat < 3450) return 25;
+	if (vbat < 3700) return 50;
+	if (vbat < 4100) return 75;
+	return 100;
+}
+
 /*EOF*/
--- a/numfont20x22.c
+++ b/numfont20x22.c
--- a/plot.c
+++ b/plot.c
--- a/python/NanoVNA-example.ipynb
+++ b/python/NanoVNA-example.ipynb
--- a/si5351.c
+++ b/si5351.c
@ -1,6 +1,5 @@
 /*
 * Copyright (c) 2014-2015, TAKAHASHI Tomohiro (TTRFTECH) edy555@gmail.com
- * Modified by DiSlord dislordlive@gmail.com
 * All rights reserved.
 *
 * This is free software; you can redistribute it and/or modify
@ -22,100 +21,40 @@
 #include "nanovna.h"
 #include "si5351.h"

-// Enable cache for SI5351 CLKX_CONTROL register, little speedup exchange
-#define USE_CLK_CONTROL_CACHE        TRUE
+#define SI5351_I2C_ADDR   	(0x60<<1)

-// XTAL frequency on si5351
-#define XTALFREQ 26000000U
-// MCLK (processor clock if set, audio codec) frequency clock
-#define CLK2_FREQUENCY 8000000U
-
-// Fixed PLL mode multiplier (used in band 1)
-#define PLL_N 32
-
-// I2C address on bus (only 0x60 for Si5351A in 10-Pin MSOP)
-#define SI5351_I2C_ADDR     0x60
-
-static uint8_t  current_band   = 0;
-static uint32_t current_freq   = 0;
-static int32_t  current_offset = FREQUENCY_OFFSET;
-
-// Minimum value is 2, freq change apply at next dsp measure, and need skip it
-#define DELAY_NORMAL       2
-// Delay for bands (depend set band 1 more fast (can change before next dsp buffer ready, need wait additional interval)
-#define DELAY_BAND_1       3
-#define DELAY_BAND_2       2
-// Band changes need set delay after reset PLL
-#define DELAY_BANDCHANGE_1 3
-#define DELAY_BANDCHANGE_2 3
-// Delay after set new PLL values, and send reset (on band 1 unstable if less then 900, on 4000-5000 no amplitude spike on change)
-#define DELAY_RESET_PLL    5000
-
-uint32_t si5351_get_frequency(void)
+static void
+si5351_write(uint8_t reg, uint8_t dat)
 {
-  return current_freq;
-}
-
-void si5351_set_frequency_offset(int32_t offset)
-{
-  current_offset = offset;
-  current_freq = 0; // reset freq, for
+  int addr = SI5351_I2C_ADDR>>1;
+  uint8_t buf[] = { reg, dat };
+  i2cAcquireBus(&I2CD1);
+  (void)i2cMasterTransmitTimeout(&I2CD1, addr, buf, 2, NULL, 0, 1000);
+  i2cReleaseBus(&I2CD1);
 }

 static void
 si5351_bulk_write(const uint8_t *buf, int len)
 {
+  int addr = SI5351_I2C_ADDR>>1;
  i2cAcquireBus(&I2CD1);
-  (void)i2cMasterTransmitTimeout(&I2CD1, SI5351_I2C_ADDR, buf, len, NULL, 0, 1000);
+  (void)i2cMasterTransmitTimeout(&I2CD1, addr, buf, len, NULL, 0, 1000);
  i2cReleaseBus(&I2CD1);
 }

-#if 0
-static bool si5351_bulk_read(uint8_t reg, uint8_t* buf, int len)
-{
-  i2cAcquireBus(&I2CD1);
-  msg_t mr = i2cMasterTransmitTimeout(&I2CD1, SI5351_I2C_ADDR, &reg, 1, buf, len, 1000);
-  i2cReleaseBus(&I2CD1);
-  return mr == MSG_OK;
-}
-
-static void si5351_wait_pll_lock(void)
-{
-  uint8_t status;
-  int count = 100;
-  do{
-    status=0xFF;
-    si5351_bulk_read(0, &status, 1);
-    if ((status & 0x60) == 0) // PLLA and PLLB locked
-      return;
-  }while (--count);
-}
-#endif
-
-static inline void
-si5351_write(uint8_t reg, uint8_t dat)
-{
-  uint8_t buf[] = { reg, dat };
-  si5351_bulk_write(buf, 2);
-}
-
 // register addr, length, data, ...
 const uint8_t si5351_configs[] = {
  2, SI5351_REG_3_OUTPUT_ENABLE_CONTROL, 0xff,
  4, SI5351_REG_16_CLK0_CONTROL, SI5351_CLK_POWERDOWN, SI5351_CLK_POWERDOWN, SI5351_CLK_POWERDOWN,
  2, SI5351_REG_183_CRYSTAL_LOAD, SI5351_CRYSTAL_LOAD_8PF,
-// All of this init code run late on sweep
-#if 0
  // setup PLL (26MHz * 32 = 832MHz, 32/2-2=14)
-  9, SI5351_REG_PLL_A, /*P3*/0, 1, /*P1*/0, 14, 0, /*P3/P2*/0, 0, 0,
-  9, SI5351_REG_PLL_B, /*P3*/0, 1, /*P1*/0, 14, 0, /*P3/P2*/0, 0, 0,
+  9, SI5351_REG_26_PLL_A, /*P3*/0, 1, /*P1*/0, 14, 0, /*P3/P2*/0, 0, 0,
  // RESET PLL
-  2, SI5351_REG_177_PLL_RESET, SI5351_PLL_RESET_A | SI5351_PLL_RESET_B | 0x0C, //
+  2, SI5351_REG_177_PLL_RESET, SI5351_PLL_RESET_A | SI5351_PLL_RESET_B,
  // setup multisynth (832MHz / 104 = 8MHz, 104/2-2=50)
  9, SI5351_REG_58_MULTISYNTH2, /*P3*/0, 1, /*P1*/0, 50, 0, /*P2|P3*/0, 0, 0,
  2, SI5351_REG_18_CLK2_CONTROL, SI5351_CLK_DRIVE_STRENGTH_2MA | SI5351_CLK_INPUT_MULTISYNTH_N | SI5351_CLK_INTEGER_MODE,
-#endif
-  2, SI5351_REG_3_OUTPUT_ENABLE_CONTROL, ~(SI5351_CLK0_EN|SI5351_CLK1_EN|SI5351_CLK2_EN),
+  2, SI5351_REG_3_OUTPUT_ENABLE_CONTROL, 0,
  0 // sentinel
 };

@ -130,344 +69,335 @@ si5351_init(void)
  }
 }

-static const uint8_t disable_output[] = {
-  SI5351_REG_16_CLK0_CONTROL,
-  SI5351_CLK_POWERDOWN,  // CLK 0
-  SI5351_CLK_POWERDOWN,  // CLK 1
-  SI5351_CLK_POWERDOWN   // CLK 2
-};
-
-/* Get the appropriate starting point for the PLL registers */
-static const uint8_t msreg_base[] = {
-  SI5351_REG_42_MULTISYNTH0,
-  SI5351_REG_50_MULTISYNTH1,
-  SI5351_REG_58_MULTISYNTH2,
-};
-static const uint8_t clkctrl[] = {
-  SI5351_REG_16_CLK0_CONTROL,
-  SI5351_REG_17_CLK1_CONTROL,
-  SI5351_REG_18_CLK2_CONTROL
-};
-
-// Reset PLL need then band changes
-static void si5351_reset_pll(uint8_t mask)
-{
-  // Writing a 1<<5 will reset PLLA, 1<<7 reset PLLB, this is a self clearing bits.
-  // !!! Need delay before reset PLL for apply PLL freq changes before
-  chThdSleepMicroseconds(DELAY_RESET_PLL);
-  si5351_write(SI5351_REG_177_PLL_RESET, mask | 0x0C);
-}
-
 void si5351_disable_output(void)
 {
-  si5351_write(SI5351_REG_3_OUTPUT_ENABLE_CONTROL, 0xFF);
-  si5351_bulk_write(disable_output, sizeof(disable_output));
-  current_band = 0;
+  uint8_t reg[4];
+  si5351_write(SI5351_REG_3_OUTPUT_ENABLE_CONTROL, 0xff);
+  reg[0] = SI5351_REG_16_CLK0_CONTROL;
+  reg[1] = SI5351_CLK_POWERDOWN;
+  reg[2] = SI5351_CLK_POWERDOWN;
+  reg[3] = SI5351_CLK_POWERDOWN;
+  si5351_bulk_write(reg, 4);
 }

 void si5351_enable_output(void)
 {
-  si5351_write(SI5351_REG_3_OUTPUT_ENABLE_CONTROL, ~(SI5351_CLK0_EN|SI5351_CLK1_EN|SI5351_CLK2_EN));
-//si5351_reset_pll(SI5351_PLL_RESET_A | SI5351_PLL_RESET_B);
-  current_freq = 0;
-  current_band = 0;
+  si5351_write(SI5351_REG_3_OUTPUT_ENABLE_CONTROL, 0x00);
 }

-// Set PLL freq = XTALFREQ * (mult + num/denom)
-static void si5351_setupPLL(uint8_t   pllSource,  /* SI5351_REG_PLL_A or SI5351_REG_PLL_B */
-                            uint32_t  mult,
-                            uint32_t  num,
-                            uint32_t  denom)
+void si5351_reset_pll(void)
 {
+  //si5351_write(SI5351_REG_177_PLL_RESET, SI5351_PLL_RESET_A | SI5351_PLL_RESET_B);
+  si5351_write(SI5351_REG_177_PLL_RESET, 0xAC);
+}
+
+void si5351_setupPLL(uint8_t pll, /* SI5351_PLL_A or SI5351_PLL_B */
+                     uint8_t     mult,
+                     uint32_t    num,
+                     uint32_t    denom)
+{
+  /* Get the appropriate starting point for the PLL registers */
+  const uint8_t pllreg_base[] = {
+    SI5351_REG_26_PLL_A,
+    SI5351_REG_34_PLL_B
+  };
+  uint32_t P1;
+  uint32_t P2;
+  uint32_t P3;
+
  /* Feedback Multisynth Divider Equation
   * where: a = mult, b = num and c = denom
   * P1 register is an 18-bit value using following formula:
-   *    P1[17:0] = 128 * mult + int((128*num)/denom) - 512
+   * 	P1[17:0] = 128 * mult + floor(128*(num/denom)) - 512
   * P2 register is a 20-bit value using the following formula:
-   *    P2[19:0] = (128 * num) % denom
+   * 	P2[19:0] = 128 * num - denom * floor(128*(num/denom))
   * P3 register is a 20-bit value using the following formula:
-   *    P3[19:0] = denom
+   * 	P3[19:0] = denom
   */
+
  /* Set the main PLL config registers */
-  mult <<= 7;
-  num <<= 7;
-  uint32_t P1 = mult - 512;  // Integer mode
-  uint32_t P2 = 0;
-  uint32_t P3 = 1;
-  if (num) {                 // Fractional mode
-    P1+= num / denom;
-    P2 = num % denom;
+  if (num == 0)
+  {
+    /* Integer mode */
+    P1 = 128 * mult - 512;
+    P2 = 0;
+    P3 = 1;
+  }
+  else
+  {
+    /* Fractional mode */
+    //P1 = (uint32_t)(128 * mult + floor(128 * ((float)num/(float)denom)) - 512);
+    P1 = 128 * mult + ((128 * num) / denom) - 512;
+    //P2 = (uint32_t)(128 * num - denom * floor(128 * ((float)num/(float)denom)));
+    P2 = 128 * num - denom * ((128 * num) / denom);
    P3 = denom;
  }
-  // Pll MSN(A|B) registers Datasheet
+
+  /* The datasheet is a nightmare of typos and inconsistencies here! */
  uint8_t reg[9];
-  reg[0] = pllSource;                                       // SI5351_REG_PLL_A or SI5351_REG_PLL_B
-  reg[1] = (P3 & 0x0FF00) >> 8;                             // MSN_P3[15: 8]
-  reg[2] = (P3 & 0x000FF);                                  // MSN_P3[ 7: 0]
-  reg[3] = (P1 & 0x30000) >> 16;                            // MSN_P1[17:16]
-  reg[4] = (P1 & 0x0FF00) >> 8;                             // MSN_P1[15: 8]
-  reg[5] = (P1 & 0x000FF);                                  // MSN_P1[ 7: 0]
-  reg[6] = ((P3 & 0xF0000) >> 12) | ((P2 & 0xF0000) >> 16); // MSN_P3[19:16] | MSN_P2[19:16]
-  reg[7] = (P2 & 0x0FF00) >> 8;                             // MSN_P2[15: 8]
-  reg[8] = (P2 & 0x000FF);                                  // MSN_P2[ 7: 0]
+  reg[0] = pllreg_base[pll];
+  reg[1] = (P3 & 0x0000FF00) >> 8;
+  reg[2] = (P3 & 0x000000FF);
+  reg[3] = (P1 & 0x00030000) >> 16;
+  reg[4] = (P1 & 0x0000FF00) >> 8;
+  reg[5] = (P1 & 0x000000FF);
+  reg[6] = ((P3 & 0x000F0000) >> 12) | ((P2 & 0x000F0000) >> 16);
+  reg[7] = (P2 & 0x0000FF00) >> 8;
+  reg[8] = (P2 & 0x000000FF);
  si5351_bulk_write(reg, 9);
 }

-// Set Multisynth divider = (div + num/denom) * rdiv
-static void
-si5351_setupMultisynth(uint8_t   channel,
-                       uint32_t  div,    // 4,6,8, 8+ ~ 900
-                       uint32_t  num,
-                       uint32_t  denom,
-                       uint32_t  rdiv,   // SI5351_R_DIV_1~128
-                       uint8_t   chctrl) // SI5351_REG_16_CLKX_CONTROL settings
+void 
+si5351_setupMultisynth(uint8_t     output,
+                       uint8_t	   pllSource,
+                       uint32_t    div, // 4,6,8, 8+ ~ 900
+                       uint32_t    num,
+                       uint32_t    denom,
+                       uint32_t    rdiv, // SI5351_R_DIV_1~128
+                       uint8_t     drive_strength)
 {
+  /* Get the appropriate starting point for the PLL registers */
+  const uint8_t msreg_base[] = {
+    SI5351_REG_42_MULTISYNTH0,
+    SI5351_REG_50_MULTISYNTH1,
+    SI5351_REG_58_MULTISYNTH2,
+  };
+  const uint8_t clkctrl[] = {
+    SI5351_REG_16_CLK0_CONTROL,
+    SI5351_REG_17_CLK1_CONTROL,
+    SI5351_REG_18_CLK2_CONTROL
+  };
+  uint8_t dat;
+
+  uint32_t P1;
+  uint32_t P2;
+  uint32_t P3;
+  uint32_t div4 = 0;
+
  /* Output Multisynth Divider Equations
   * where: a = div, b = num and c = denom
   * P1 register is an 18-bit value using following formula:
-   *   P1[17:0] = 128 * a + int((128*b)/c) - 512
+   * 	P1[17:0] = 128 * a + floor(128*(b/c)) - 512
   * P2 register is a 20-bit value using the following formula:
-   *   P2[19:0] = (128 * b) % c
+   * 	P2[19:0] = 128 * b - c * floor(128*(b/c))
   * P3 register is a 20-bit value using the following formula:
-   *   P3[19:0] = c
+   * 	P3[19:0] = c
   */
  /* Set the main PLL config registers */
-  uint32_t P1 = 0;
-  uint32_t P2 = 0;
-  uint32_t P3 = 1;
-  if (div == 4)
-    rdiv|= SI5351_DIVBY4;
-  else {
-    num<<=7;
-    div<<=7;
-    P1 = div - 512; // Integer mode
-    if (num) {       // Fractional mode
-      P1+= num / denom;
-      P2 = num % denom;
-      P3 = denom;
-    }
+  if (div == 4) {
+    div4 = SI5351_DIVBY4;
+    P1 = P2 = 0;
+    P3 = 1;
+  } else if (num == 0) {
+    /* Integer mode */
+    P1 = 128 * div - 512;
+    P2 = 0;
+    P3 = 1;
+  } else {
+    /* Fractional mode */
+    P1 = 128 * div + ((128 * num) / denom) - 512;
+    P2 = 128 * num - denom * ((128 * num) / denom);
+    P3 = denom;
  }
+
  /* Set the MSx config registers */
  uint8_t reg[9];
-  reg[0] = msreg_base[channel];                       // SI5351_REG_42_MULTISYNTH0, SI5351_REG_50_MULTISYNTH1, SI5351_REG_58_MULTISYNTH2
-  reg[1] = (P3 & 0x0FF00)>>8;                         // MSx_P3[15: 8]
-  reg[2] = (P3 & 0x000FF);                            // MSx_P3[ 7: 0]
-  reg[3] = ((P1 & 0x30000)>>16)| rdiv;                // Rx_DIV[2:0] | MSx_DIVBY4[1:0] | MSx_P1[17:16]
-  reg[4] = (P1 & 0x0FF00)>> 8;                        // MSx_P1[15: 8]
-  reg[5] = (P1 & 0x000FF);                            // MSx_P1[ 7: 0]
-  reg[6] = ((P3 & 0xF0000)>>12)|((P2 & 0xF0000)>>16); // MSx_P3[19:16] | MSx_P2[19:16]
-  reg[7] = (P2 & 0x0FF00)>>8;                         // MSx_P2[15: 8]
-  reg[8] = (P2 & 0x000FF);                            // MSx_P2[ 7: 0]
+  reg[0] = msreg_base[output];
+  reg[1] = (P3 & 0x0000FF00) >> 8;
+  reg[2] = (P3 & 0x000000FF);
+  reg[3] = ((P1 & 0x00030000) >> 16) | div4 | rdiv;
+  reg[4] = (P1 & 0x0000FF00) >> 8;
+  reg[5] = (P1 & 0x000000FF);
+  reg[6] = ((P3 & 0x000F0000) >> 12) | ((P2 & 0x000F0000) >> 16);
+  reg[7] = (P2 & 0x0000FF00) >> 8;
+  reg[8] = (P2 & 0x000000FF);
  si5351_bulk_write(reg, 9);

  /* Configure the clk control and enable the output */
-  uint8_t dat = chctrl | SI5351_CLK_INPUT_MULTISYNTH_N;
+  dat = drive_strength | SI5351_CLK_INPUT_MULTISYNTH_N;
+  if (pllSource == SI5351_PLL_B)
+    dat |= SI5351_CLK_PLL_SELECT_B;
  if (num == 0)
    dat |= SI5351_CLK_INTEGER_MODE;
-
-#if USE_CLK_CONTROL_CACHE == TRUE
-  // Use cache for this reg, not update if not change
-  static uint8_t clk_cache[3];
-  if (clk_cache[channel]!=dat) {
-    si5351_write(clkctrl[channel], dat);
-    clk_cache[channel]=dat;
-  }
-#else
-  si5351_write(clkctrl[channel], dat);
-#endif
+  si5351_write(clkctrl[output], dat);
 }

-// Find better approximate values for n/d
-#define MAX_DENOMINATOR ((1 << 20) - 1)
-static inline void approximate_fraction(uint32_t *n, uint32_t *d)
+static uint32_t
+gcd(uint32_t x, uint32_t y)
 {
-  // cf. https://github.com/python/cpython/blob/master/Lib/fractions.py#L227
-  uint32_t denom = *d;
-  if (denom > MAX_DENOMINATOR) {
-    uint32_t num = *n;
-    uint32_t p0 = 0, q0 = 1, p1 = 1, q1 = 0;
-    while (denom != 0) {
-      uint32_t a = num / denom;
-      uint32_t b = num % denom;
-      uint32_t q2 = q0 + a*q1;
-      if (q2 > MAX_DENOMINATOR)
-        break;
-      uint32_t p2 = p0 + a*p1;
-      p0 = p1; q0 = q1; p1 = p2; q1 = q2;
-      num = denom; denom = b;
+  uint32_t z;
+  while (y != 0) {
+    z = x % y;
+    x = y;
+    y = z;
+  }
+  return x;
+}
+
+#define XTALFREQ 26000000L
+#define PLL_N 32
+#define PLLFREQ (XTALFREQ * PLL_N)
+
+void
+si5351_set_frequency_fixedpll(int channel, int pll, int pllfreq, int freq,
+                              uint32_t rdiv, uint8_t drive_strength)
+{
+    int32_t div = pllfreq / freq; // range: 8 ~ 1800
+    int32_t num = pllfreq - freq * div;
+    int32_t denom = freq;
+    //int32_t k = freq / (1<<20) + 1;
+    int32_t k = gcd(num, denom);
+    num /= k;
+    denom /= k;
+    while (denom >= (1<<20)) {
+      num >>= 1;
+      denom >>= 1;
    }
-    *n = p1;
-    *d = q1;
-  }
-}
-
-// Setup Multisynth divider for get correct output freq if fixed PLL = pllfreq
-static void
-si5351_set_frequency_fixedpll(uint8_t channel, uint64_t pllfreq, uint32_t freq, uint32_t rdiv, uint8_t chctrl)
-{
-  uint32_t denom = freq;
-  uint32_t div = pllfreq / denom; // range: 8 ~ 1800
-  uint32_t num = pllfreq % denom;
-  approximate_fraction(&num, &denom);
-  si5351_setupMultisynth(channel, div, num, denom, rdiv, chctrl);
-}
-
-// Setup PLL freq if Multisynth divider fixed = div (need get output =  freq/mul)
-static void
-si5351_setupPLL_freq(uint32_t pllSource, uint32_t freq, uint32_t div, uint32_t mul)
-{
-  uint32_t denom = XTALFREQ * mul;
-  uint64_t pllfreq = (uint64_t)freq * div;
-  uint32_t multi = pllfreq / denom;
-  uint32_t num   = pllfreq % denom;
-  approximate_fraction(&num, &denom);
-  si5351_setupPLL(pllSource, multi, num, denom);
-}
-
-#if 0
-static void
-si5351_set_frequency_fixeddiv(uint8_t channel, uint32_t pll, uint32_t freq, uint32_t div,
-                              uint8_t chctrl, uint32_t mul)
-{
-  si5351_setupPLL_freq(pll, freq, div, mul);
-  si5351_setupMultisynth(channel, div, 0, 1, SI5351_R_DIV_1, chctrl);
+    si5351_setupMultisynth(channel, pll, div, num, denom, rdiv, drive_strength);
 }

 void
-si5351_set_frequency(int channel, uint32_t freq, uint8_t drive_strength)
+si5351_set_frequency_fixeddiv(int channel, int pll, int freq, int div,
+                              uint8_t     drive_strength)
+{
+    int32_t pllfreq = freq * div;
+    int32_t multi = pllfreq / XTALFREQ;
+    int32_t num = pllfreq - multi * XTALFREQ;
+    int32_t denom = XTALFREQ;
+    int32_t k = gcd(num, denom);
+    num /= k;
+    denom /= k;
+    while (denom >= (1<<20)) {
+      num >>= 1;
+      denom >>= 1;
+    }
+    si5351_setupPLL(pll, multi, num, denom);
+    si5351_setupMultisynth(channel, pll, div, 0, 1, SI5351_R_DIV_1, drive_strength);
+}
+
+/* 
+ * 1~100MHz fixed PLL 900MHz, fractional divider
+ * 100~150MHz fractional PLL 600-900MHz, fixed divider 6
+ * 150~200MHz fractional PLL 600-900MHz, fixed divider 4
+ */
+void
+si5351_set_frequency(int channel, int freq, uint8_t drive_strength)
 {
  if (freq <= 100000000) {
    si5351_setupPLL(SI5351_PLL_B, 32, 0, 1);
-    si5351_set_frequency_fixedpll(channel, SI5351_PLL_B, PLLFREQ, freq, SI5351_R_DIV_1, drive_strength, 1);
+    si5351_set_frequency_fixedpll(channel, SI5351_PLL_B, PLLFREQ, freq, SI5351_R_DIV_1, drive_strength);
  } else if (freq < 150000000) {
-    si5351_set_frequency_fixeddiv(channel, SI5351_PLL_B, freq, 6, drive_strength, 1);
+    si5351_set_frequency_fixeddiv(channel, SI5351_PLL_B, freq, 6, drive_strength);
  } else {
-    si5351_set_frequency_fixeddiv(channel, SI5351_PLL_B, freq, 4, drive_strength, 1);
+    si5351_set_frequency_fixeddiv(channel, SI5351_PLL_B, freq, 4, drive_strength);
  }
 }
-#endif
+
+
+int current_band = -1;
+
+#define DELAY_NORMAL 3
+#define DELAY_BANDCHANGE 1
+#define DELAY_LOWBAND 1

 /*
- * Frequency generation divide on 3 band
- *  Band 1
- *   1~100MHz      fixed PLL = XTALFREQ * PLL_N, fractional divider
- *  Band 2
- * 100~150MHz fractional PLL = 600- 900MHz, fixed divider 'fdiv = 6'
- *  Band 3
- * 150~300MHz fractional PLL = 600-1200MHz, fixed divider 'fdiv = 4'
- *
- * For FREQ_HARMONICS = 300MHz - band range is:
- *  +-----------------------------------------------------------------------------------------------------------------------+
- *  |     Band 1      |   Band 2     |    Band 3    |   Band 2     |                       Band 3                           |
- *  +-----------------------------------------------------------------------------------------------------------------------+
- *  |            Direct mode  x1 :  x1              |           x3 : x5         |    x5-x7    |    x7-x9     |    x9-x11    |
- *  +-----------------------------------------------------------------------------------------------------------------------+
- *  | 50kHz - 100MHz  | 100 - 150MHz | 150 - 300MHz |  300-450MHz  | 450-900MHz | 900-1500MHz | 1500-2100MHz | 2100-2700MHz |
- *  +-----------------------------------------------------------------------------------------------------------------------+
- *  |              f = 50kHz-300MHz                 | f=100-150    | f=150-300  | f=150-300   | f=214-300    | f=233-300    |
- *  |             of = 50kHz-300MHz                 |of= 60- 90    |of= 90-180  |of=128-215   |of=166-234    |of=190-246    |
- *  +-----------------------------------------------------------------------------------------------------------------------+
- */
-static inline uint8_t
-si5351_get_band(uint32_t freq)
-{
-  if (freq < 100000000U) return 1;
-  if (freq < 150000000U) return 2;
-  return 3;
-}
-
-/*
- * Maximum supported frequency = FREQ_HARMONICS * 9U
 * configure output as follows:
 * CLK0: frequency + offset
 * CLK1: frequency
 * CLK2: fixed 8MHz
 */
+#define CLK2_FREQUENCY 8000000L
 int
-si5351_set_frequency(uint32_t freq, uint8_t drive_strength)
+si5351_set_frequency_with_offset(uint32_t freq, int offset, uint8_t drive_strength)
 {
-  uint8_t band;
+  int band;
  int delay = DELAY_NORMAL;
-  if (freq == current_freq)
-    return delay;
-  else if (current_freq > freq)  // Reset band on sweep begin (if set range 150-600, fix error then 600 MHz band 2 or 3 go back)
-    current_band = 0;
-  current_freq = freq;
-  uint32_t ofreq = freq + current_offset;
-  uint32_t mul = 1, omul = 1;
+  uint32_t ofreq = freq + offset;
  uint32_t rdiv = SI5351_R_DIV_1;
-  uint32_t fdiv;
-  // Fix possible incorrect input
-  drive_strength&=SI5351_CLK_DRIVE_STRENGTH_MASK;
-
-  if (freq >= config.harmonic_freq_threshold * 7U) {
-     mul =  9;
-    omul = 11;
-  } else if (freq >= config.harmonic_freq_threshold * 5U) {
-     mul = 7;
-    omul = 9;
-  } else if (freq >= config.harmonic_freq_threshold * 3U) {
-     mul = 5;
-    omul = 7;
+  if (freq >= config.harmonic_freq_threshold * 3) {
+    freq /= 5;
+    ofreq /= 7;
  } else if (freq >= config.harmonic_freq_threshold) {
-     mul = 3;
-    omul = 5;
-  } else if (freq <= 500000U) {
+    freq /= 3;
+    ofreq /= 5;
+  }
+  if (freq <= 100000000) {
+    band = 0;
+  } else if (freq < 150000000) {
+    band = 1;
+  } else {
+    band = 2;
+  }
+  if (freq <= 500000) {
    rdiv = SI5351_R_DIV_64;
-     freq<<= 6;
-    ofreq<<= 6;
-  } else if (freq <= 4000000U) {
+  } else if (freq <= 4000000) {
    rdiv = SI5351_R_DIV_8;
-     freq<<= 3;
-    ofreq<<= 3;
  }
-  band = si5351_get_band(freq / mul);
+
+#if 1
+  if (current_band != band)
+    si5351_disable_output();
+#endif
+
  switch (band) {
-    case 1:
-      // Setup CH0 and CH1 constant PLLA freq at band change, and set CH2 freq =
-      // CLK2_FREQUENCY
-      if (current_band != 1) {
-        si5351_setupPLL(SI5351_REG_PLL_A, PLL_N, 0, 1);
-        si5351_set_frequency_fixedpll(
-            2, XTALFREQ * PLL_N, CLK2_FREQUENCY, SI5351_R_DIV_1,
-            SI5351_CLK_DRIVE_STRENGTH_2MA | SI5351_CLK_PLL_SELECT_A);
-        delay = DELAY_BANDCHANGE_1;
-      } else {
-        delay = DELAY_BAND_1;
-      }
-      // Calculate and set CH0 and CH1 divider
-      si5351_set_frequency_fixedpll(0, (uint64_t)omul * XTALFREQ * PLL_N, ofreq, rdiv,
-                                    drive_strength | SI5351_CLK_PLL_SELECT_A);
-      si5351_set_frequency_fixedpll(1, (uint64_t)mul * XTALFREQ * PLL_N, freq, rdiv,
-                                    drive_strength | SI5351_CLK_PLL_SELECT_A);
-      break;
-    case 2:  // fdiv = 6
-    case 3:  // fdiv = 4;
-      fdiv = (band == 2) ? 6 : 4;
-      // Setup CH0 and CH1 constant fdiv divider at change
-      if (current_band != band) {
-        si5351_setupMultisynth(0, fdiv, 0, 1, SI5351_R_DIV_1,
-                               drive_strength | SI5351_CLK_PLL_SELECT_A);
-        si5351_setupMultisynth(1, fdiv, 0, 1, SI5351_R_DIV_1,
-                               drive_strength | SI5351_CLK_PLL_SELECT_B);
-        delay = DELAY_BANDCHANGE_2;
-      } else {
-        delay = DELAY_BAND_2;
-      }
-      // Calculate and set CH0 and CH1 PLL freq
-      si5351_setupPLL_freq(SI5351_REG_PLL_A, ofreq, fdiv,
-                           omul);  // set PLLA freq = (ofreq/omul)*fdiv
-      si5351_setupPLL_freq(SI5351_REG_PLL_B, freq, fdiv,
-                           mul);  // set PLLB freq = ( freq/ mul)*fdiv
-      // Calculate CH2 freq = CLK2_FREQUENCY, depend from calculated before CH1 PLLB = (freq/mul)*fdiv
-      si5351_set_frequency_fixedpll(
-          2, (uint64_t)freq * fdiv, CLK2_FREQUENCY * mul, SI5351_R_DIV_1,
-          SI5351_CLK_DRIVE_STRENGTH_2MA | SI5351_CLK_PLL_SELECT_B);
-      break;
+  case 0:
+    // fractional divider mode. only PLL A is used.
+    if (current_band == 1 || current_band == 2)
+      si5351_setupPLL(SI5351_PLL_A, 32, 0, 1);
+    // Set PLL twice on changing from band 2
+    if (current_band == 2) 
+      si5351_setupPLL(SI5351_PLL_A, 32, 0, 1);
+
+    if (rdiv == SI5351_R_DIV_8) {
+      freq *= 8;
+      ofreq *= 8;
+    } else if (rdiv == SI5351_R_DIV_64) {
+      freq *= 64;
+      ofreq *= 64;
+    }
+
+    si5351_set_frequency_fixedpll(0, SI5351_PLL_A, PLLFREQ, ofreq,
+                                  rdiv, drive_strength);
+    si5351_set_frequency_fixedpll(1, SI5351_PLL_A, PLLFREQ, freq,
+                                  rdiv, drive_strength);
+    //if (current_band != 0)
+      si5351_set_frequency_fixedpll(2, SI5351_PLL_A, PLLFREQ, CLK2_FREQUENCY,
+                                    SI5351_R_DIV_1, SI5351_CLK_DRIVE_STRENGTH_2MA);
+    break;
+
+  case 1:
+    // Set PLL twice on changing from band 2
+    if (current_band == 2) {
+      si5351_set_frequency_fixeddiv(0, SI5351_PLL_A, ofreq, 6, drive_strength);
+      si5351_set_frequency_fixeddiv(1, SI5351_PLL_B, freq, 6, drive_strength);
+    }
+
+    // div by 6 mode. both PLL A and B are dedicated for CLK0, CLK1
+    si5351_set_frequency_fixeddiv(0, SI5351_PLL_A, ofreq, 6, drive_strength);
+    si5351_set_frequency_fixeddiv(1, SI5351_PLL_B, freq, 6, drive_strength);
+    si5351_set_frequency_fixedpll(2, SI5351_PLL_B, freq * 6, CLK2_FREQUENCY,
+                                  SI5351_R_DIV_1, SI5351_CLK_DRIVE_STRENGTH_2MA);
+    break;
+
+  case 2:
+    // div by 4 mode. both PLL A and B are dedicated for CLK0, CLK1
+    si5351_set_frequency_fixeddiv(0, SI5351_PLL_A, ofreq, 4, drive_strength);
+    si5351_set_frequency_fixeddiv(1, SI5351_PLL_B, freq, 4, drive_strength);
+    si5351_set_frequency_fixedpll(2, SI5351_PLL_B, freq * 4, CLK2_FREQUENCY,
+                                  SI5351_R_DIV_1, SI5351_CLK_DRIVE_STRENGTH_2MA);
+    break;
  }
+
  if (current_band != band) {
-    si5351_reset_pll(SI5351_PLL_RESET_A|SI5351_PLL_RESET_B);
-    current_band = band;
+    si5351_reset_pll();
+#if 1
+    si5351_enable_output();
+#endif
+    delay += DELAY_BANDCHANGE;
  }
+  if (band == 0)
+    delay += DELAY_LOWBAND;
+
+  current_band = band;
  return delay;
 }
--- a/si5351.h
+++ b/si5351.h
@ -17,61 +17,73 @@
 * the Free Software Foundation, Inc., 51 Franklin Street,
 * Boston, MA 02110-1301, USA.
 */
+#define SI5351_PLL_A	0
+#define SI5351_PLL_B	1

-#define SI5351_REG_3_OUTPUT_ENABLE_CONTROL  3
-#define SI5351_CLK0_EN     (1<<0)
-#define SI5351_CLK1_EN     (1<<1)
-#define SI5351_CLK2_EN     (1<<2)
+#define SI5351_MULTISYNTH_DIV_4  4
+#define SI5351_MULTISYNTH_DIV_6  6
+#define SI5351_MULTISYNTH_DIV_8  8
+#define SI5351_R_DIV_1		(0<<4)
+#define SI5351_R_DIV_2		(1<<4)
+#define SI5351_R_DIV_4		(2<<4)
+#define SI5351_R_DIV_8		(3<<4)
+#define SI5351_R_DIV_16		(4<<4)
+#define SI5351_R_DIV_32		(5<<4)
+#define SI5351_R_DIV_64		(6<<4)
+#define SI5351_R_DIV_128	(7<<4)
+#define SI5351_DIVBY4 		(3<<2)

-// Reg 16-18 CLKX_CONTROL
-#define SI5351_REG_16_CLK0_CONTROL  16
-#define SI5351_REG_17_CLK1_CONTROL  17
-#define SI5351_REG_18_CLK2_CONTROL  18
-#define SI5351_CLK_POWERDOWN                (1<<7)
-#define SI5351_CLK_INTEGER_MODE             (1<<6)
-#define SI5351_CLK_PLL_SELECT_A             (0<<5)
-#define SI5351_CLK_PLL_SELECT_B             (1<<5)
-#define SI5351_CLK_INVERT                   (1<<4)
-#define SI5351_CLK_INPUT_MASK               (3<<2)
-#define SI5351_CLK_INPUT_XTAL               (0<<2)
-#define SI5351_CLK_INPUT_CLKIN              (1<<2)
-#define SI5351_CLK_INPUT_MULTISYNTH_0_4     (2<<2)
-#define SI5351_CLK_INPUT_MULTISYNTH_N       (3<<2)
-#define SI5351_CLK_DRIVE_STRENGTH_MASK      (3<<0)
-#define SI5351_CLK_DRIVE_STRENGTH_2MA       (0<<0)
-#define SI5351_CLK_DRIVE_STRENGTH_4MA       (1<<0)
-#define SI5351_CLK_DRIVE_STRENGTH_6MA       (2<<0)
-#define SI5351_CLK_DRIVE_STRENGTH_8MA       (3<<0)
+#define SI5351_REG_3_OUTPUT_ENABLE_CONTROL 3
+#define SI5351_REG_16_CLK0_CONTROL	16
+#define SI5351_REG_17_CLK1_CONTROL	17
+#define SI5351_REG_18_CLK2_CONTROL	18
+#define SI5351_REG_26_PLL_A 		26
+#define SI5351_REG_34_PLL_B 		34
+#define SI5351_REG_42_MULTISYNTH0	42
+#define SI5351_REG_50_MULTISYNTH1	50
+#define SI5351_REG_58_MULTISYNTH2	58

-#define SI5351_REG_PLL_A            26
-#define SI5351_REG_PLL_B            34
+#define SI5351_CLK_POWERDOWN			    (1<<7)
+#define SI5351_CLK_INTEGER_MODE		     	(1<<6)
+#define SI5351_CLK_PLL_SELECT_B		     	(1<<5)
+#define SI5351_CLK_INVERT				    (1<<4)

-#define SI5351_REG_42_MULTISYNTH0   42
-#define SI5351_REG_50_MULTISYNTH1   50
-#define SI5351_REG_58_MULTISYNTH2   58
-#define SI5351_DIVBY4       (3<<2)
-#define SI5351_R_DIV_1      (0<<4)
-#define SI5351_R_DIV_2      (1<<4)
-#define SI5351_R_DIV_4      (2<<4)
-#define SI5351_R_DIV_8      (3<<4)
-#define SI5351_R_DIV_16     (4<<4)
-#define SI5351_R_DIV_32     (5<<4)
-#define SI5351_R_DIV_64     (6<<4)
-#define SI5351_R_DIV_128    (7<<4)
+#define SI5351_CLK_INPUT_MASK			    (3<<2)
+#define SI5351_CLK_INPUT_XTAL			    (0<<2)
+#define SI5351_CLK_INPUT_CLKIN			    (1<<2)
+#define SI5351_CLK_INPUT_MULTISYNTH_0_4 	(2<<2)
+#define SI5351_CLK_INPUT_MULTISYNTH_N	 	(3<<2)

-#define SI5351_REG_177_PLL_RESET    177
-#define SI5351_PLL_RESET_B          (1<<7)
-#define SI5351_PLL_RESET_A          (1<<5)
+#define SI5351_CLK_DRIVE_STRENGTH_MASK		(3<<0)
+#define SI5351_CLK_DRIVE_STRENGTH_2MA		(0<<0)
+#define SI5351_CLK_DRIVE_STRENGTH_4MA		(1<<0)
+#define SI5351_CLK_DRIVE_STRENGTH_6MA	 	(2<<0)
+#define SI5351_CLK_DRIVE_STRENGTH_8MA		(3<<0)
+
+
+#define SI5351_REG_177_PLL_RESET 	177
+#define SI5351_PLL_RESET_B 			(1<<7)
+#define SI5351_PLL_RESET_A 			(1<<5)

 #define SI5351_REG_183_CRYSTAL_LOAD 183
-#define SI5351_CRYSTAL_LOAD_6PF     (1<<6)
-#define SI5351_CRYSTAL_LOAD_8PF     (2<<6)
-#define SI5351_CRYSTAL_LOAD_10PF    (3<<6)
+#define SI5351_CRYSTAL_LOAD_6PF  	(1<<6)
+#define SI5351_CRYSTAL_LOAD_8PF  	(2<<6)
+#define SI5351_CRYSTAL_LOAD_10PF  	(3<<6)
+
+#define SI5351_CRYSTAL_FREQ_25MHZ 	25000000

 void si5351_init(void);
-void si5351_disable_output(void);
-void si5351_enable_output(void);

-void si5351_set_frequency_offset(int32_t offset);
-int  si5351_set_frequency(uint32_t freq, uint8_t drive_strength);
-uint32_t si5351_get_frequency(void);
+void si5351_setupPLL(uint8_t pll, /* SI5351_PLL_A or SI5351_PLL_B */
+                     uint8_t     mult,
+                     uint32_t    num,
+                     uint32_t    denom);
+void si5351_setupMultisynth(uint8_t     output,
+                            uint8_t	   pllSource,
+                            uint32_t    div,
+                            uint32_t    num,
+                            uint32_t    denom,
+                            uint32_t    rdiv,
+                            uint8_t drive_strength);
+
+void si5351_set_frequency(int channel, int freq, uint8_t drive_strength);
--- a/tlv320aic3204.c
+++ b/tlv320aic3204.c
@ -25,78 +25,74 @@

 #define wait_ms(ms)     chThdSleepMilliseconds(ms)

-static const uint8_t conf_data[] = {
-// reg, data,
-// PLL clock config
-  0x00, 0x00, /* Initialize to Page 0 */
-  0x01, 0x01, /* Initialize the device through software reset */
-  0x04, 0x43, /* PLL Clock High, MCLK, PLL */
+static const uint8_t conf_data_pll[] = {
+  // len, ( reg, data ), 
+  2, 0x00, 0x00, /* Initialize to Page 0 */
+  2, 0x01, 0x01, /* Initialize the device through software reset */
+  2, 0x04, 0x43, /* PLL Clock High, MCLK, PLL */
 #ifdef REFCLK_8000KHZ
  /* 8.000MHz*10.7520 = 86.016MHz, 86.016MHz/(2*7*128) = 48kHz */
-  0x05, 0x91, /* Power up PLL, P=1,R=1 */
-  0x06, 0x0a, /* J=10 */
-  0x07, 29,   /* D=7520 = (29<<8) + 96 */
-  0x08, 96,
+  2, 0x05, 0x91, /* Power up PLL, P=1,R=1 */
+  2, 0x06, 0x0a, /* J=10 */
+  2, 0x07, 29,    /* D=7520 = (29<<8) + 96 */
+  2, 0x08, 96,
 #endif
-// Clock config, default fs=48kHz
-  0x0b, 0x82, /* Power up the NDAC divider with value 2 */
-  0x0c, 0x87, /* Power up the MDAC divider with value 7 */
-  0x0d, 0x00, /* Program the OSR of DAC to 128 */
-  0x0e, 0x80,
-  0x3c, 0x08, /* Set the DAC Mode to PRB_P8 */
-  //0x3c, 25, /* Set the DAC Mode to PRB_P25 */
-  0x1b, 0x0c, /* Set the BCLK,WCLK as output */
-  0x1e, 0x80 + 28, /* Enable the BCLKN divider with value 28 */
-  0x25, 0xee, /* DAC power up */
-
-  0x12, 0x82, /* Power up the NADC divider with value 2 */
-  0x13, 0x87, /* Power up the MADC divider with value 7 */
-  0x14, 0x80, /* Program the OSR of ADC to 128 */
-  0x3d, 0x01, /* Select ADC PRB_R1 */
-// Data routing
-  0x00, 0x01, /* Select Page 1 */
-  0x01, 0x08, /* Disable Internal Crude AVdd in presence of external AVdd supply or before powering up internal AVdd LDO*/
-  0x02, 0x01, /* Enable Master Analog Power Control */
-  0x7b, 0x01, /* Set the REF charging time to 40ms */
-  0x14, 0x25, /* HP soft stepping settings for optimal pop performance at power up Rpop used is 6k with N = 6 and soft step = 20usec. This should work with 47uF coupling capacitor. Can try N=5,6 or 7 time constants as well. Trade-off delay vs “pop” sound. */
-  0x0a, 0x33, /* Set the Input Common Mode to 0.9V and Output Common Mode for Headphone to 1.65V */
-
-  0x3d, 0x00, /* Select ADC PTM_R4 */
-  0x47, 0x32, /* Set MicPGA startup delay to 3.1ms */
-  0x7b, 0x01, /* Set the REF charging time to 40ms */
-  0x34, 0x10, /* Route IN2L to LEFT_P with 10K */
-  0x36, 0x10, /* Route IN2R to LEFT_N with 10K */
-//0x37, 0x04, /* Route IN3R to RIGHT_P with 10K */
-//0x39, 0x04, /* Route IN3L to RIGHT_N with 10K */
-//0x3b, 0x00, /* Unmute Left MICPGA, Gain selection of 32dB to make channel gain 0dB */
-//0x3c, 0x00, /* Unmute Right MICPGA, Gain selection of 32dB to make channel gain 0dB */
+  0 // sentinel
 };

-static const uint8_t conf_data_unmute[] = {
-// reg, data,
-  0x00, 0x00, /* Select Page 0 */
-  0x51, 0xc0, /* Power up Left and Right ADC Channels */
-  0x52, 0x00, /* Unmute Left and Right ADC Digital Volume Control */
+// default fs=48kHz
+static const uint8_t conf_data_clk[] = {
+  2, 0x0b, 0x82, /* Power up the NDAC divider with value 2 */
+  2, 0x0c, 0x87, /* Power up the MDAC divider with value 7 */
+  2, 0x0d, 0x00, /* Program the OSR of DAC to 128 */
+  2, 0x0e, 0x80,
+  2, 0x3c, 0x08, /* Set the DAC Mode to PRB_P8 */
+  //2, 0x3c, 25, /* Set the DAC Mode to PRB_P25 */
+  2, 0x1b, 0x0c, /* Set the BCLK,WCLK as output */    
+  2, 0x1e, 0x80 + 28, /* Enable the BCLKN divider with value 28 */
+  2, 0x25, 0xee, /* DAC power up */
+
+  2, 0x12, 0x82, /* Power up the NADC divider with value 2 */
+  2, 0x13, 0x87, /* Power up the MADC divider with value 7 */
+  2, 0x14, 0x80, /* Program the OSR of ADC to 128 */
+  2, 0x3d, 0x01, /* Select ADC PRB_R1 */
+  0 // sentinel
 };

-static const uint8_t conf_data_ch3_select[] = {
-// reg, data,
-  0x00, 0x01, /* Select Page 1 */
-  0x37, 0x04, /* Route IN3R to RIGHT_P with input impedance of 10K */
-  0x39, 0x04, /* Route IN3L to RIGHT_N with input impedance of 10K */
+static const uint8_t conf_data_routing[] = {
+  2, 0x00, 0x01, /* Select Page 1 */
+  2, 0x01, 0x08, /* Disable Internal Crude AVdd in presence of external AVdd supply or before powering up internal AVdd LDO*/
+  2, 0x02, 0x01, /* Enable Master Analog Power Control */
+  2, 0x7b, 0x01, /* Set the REF charging time to 40ms */
+  2, 0x14, 0x25, /* HP soft stepping settings for optimal pop performance at power up Rpop used is 6k with N = 6 and soft step = 20usec. This should work with 47uF coupling capacitor. Can try N=5,6 or 7 time constants as well. Trade-off delay vs “pop” sound. */
+  2, 0x0a, 0x33, /* Set the Input Common Mode to 0.9V and Output Common Mode for Headphone to 1.65V */
+
+  2, 0x3d, 0x00, /* Select ADC PTM_R4 */
+  2, 0x47, 0x32, /* Set MicPGA startup delay to 3.1ms */
+  2, 0x7b, 0x01, /* Set the REF charging time to 40ms */
+  2, 0x34, 0x10, /* Route IN2L to LEFT_P with 10K */
+  2, 0x36, 0x10, /* Route IN2R to LEFT_N with 10K */
+  2, 0x37, 0x04, /* Route IN3R to RIGHT_P with 10K */
+  2, 0x39, 0x04, /* Route IN3L to RIGHT_N with 10K */
+  2, 0x3b, 0, /* Unmute Left MICPGA, Gain selection of 32dB to make channel gain 0dB */
+  2, 0x3c, 0, /* Unmute Right MICPGA, Gain selection of 32dB to make channel gain 0dB */
+  0 // sentinel
 };

-static const uint8_t conf_data_ch1_select[] = {
-// reg, data,
-  0x00, 0x01, /* Select Page 1 */
-  0x37, 0x40, /* Route IN1R to RIGHT_P with input impedance of 10K */
-  0x39, 0x10, /* Route IN1L to RIGHT_N with input impedance of 10K */
+const uint8_t conf_data_unmute[] = {
+  2, 0x00, 0x00, /* Select Page 0 */
+  2, 0x51, 0xc0, /* Power up Left and Right ADC Channels */
+  2, 0x52, 0x00, /* Unmute Left and Right ADC Digital Volume Control */    
+  0 // sentinel
 };

-static inline void
+static void
 tlv320aic3204_bulk_write(const uint8_t *buf, int len)
 {
-  (void)i2cMasterTransmitTimeout(&I2CD1, AIC3204_ADDR, buf, len, NULL, 0, 1000);
+  int addr = AIC3204_ADDR;
+  i2cAcquireBus(&I2CD1);
+  (void)i2cMasterTransmitTimeout(&I2CD1, addr, buf, len, NULL, 0, 1000);
+  i2cReleaseBus(&I2CD1);
 }

 #if 0
@ -113,32 +109,49 @@ tlv320aic3204_read(uint8_t d0)
 #endif

 static void
-tlv320aic3204_config(const uint8_t *data, int len)
+tlv320aic3204_config(const uint8_t *data)
 {
-  i2cAcquireBus(&I2CD1);
-  for (; len--; data += 2)
-    tlv320aic3204_bulk_write(data, 2);
-  i2cReleaseBus(&I2CD1);
+  const uint8_t *p = data;
+  while (*p) {
+    uint8_t len = *p++;
+    tlv320aic3204_bulk_write(p, len);
+    p += len;
+  }
 }

 void tlv320aic3204_init(void)
 {
-  tlv320aic3204_config(conf_data, sizeof(conf_data)/2);
+  tlv320aic3204_config(conf_data_pll);
+  tlv320aic3204_config(conf_data_clk);
+  tlv320aic3204_config(conf_data_routing);
  wait_ms(40);
-  tlv320aic3204_config(conf_data_unmute, sizeof(conf_data_unmute)/2);
+  tlv320aic3204_config(conf_data_unmute);
 }

 void tlv320aic3204_select(int channel)
 {
-  tlv320aic3204_config(channel ? conf_data_ch1_select : conf_data_ch3_select, sizeof(conf_data_ch3_select)/2);
+    const uint8_t ch3[] = {
+        2, 0x00, 0x01, /* Select Page 1 */
+        2, 0x37, 0x04, /* Route IN3R to RIGHT_P with input impedance of 10K */
+        2, 0x39, 0x04, /* Route IN3L to RIGHT_N with input impedance of 10K */    
+        0 // sentinel
+    };
+    const uint8_t ch1[] = {
+        2, 0x00, 0x01, /* Select Page 1 */
+        2, 0x37, 0x40, /* Route IN1R to RIGHT_P with input impedance of 10K */
+        2, 0x39, 0x10, /* Route IN1L to RIGHT_N with input impedance of 10K */    
+        0 // sentinel
+    };
+    tlv320aic3204_config(channel ? ch1 : ch3);
 }

 void tlv320aic3204_set_gain(int lgain, int rgain)
 {
-  uint8_t data[] = {
-    0x00, 0x01, /* Select Page 1 */
-    0x3b, lgain, /* Unmute Left MICPGA, set gain */
-    0x3c, rgain, /* Unmute Right MICPGA, set gain */
-  };
-  tlv320aic3204_config(data, sizeof(data)/2);
+    uint8_t data[] = {
+        2, 0x00, 0x01, /* Select Page 1 */
+        2, 0x3b, lgain, /* Unmute Left MICPGA, set gain */
+        2, 0x3c, rgain, /* Unmute Right MICPGA, set gain */    
+        0 // sentinel
+    };
+    tlv320aic3204_config(data);
 }
--- a/ui.c
+++ b/ui.c