Merge pull request #144 from czietz/window_loss

Compensate IFFT window / zero-padding loss in TD
2025-12-06 03:31:59 +01:00 · 2020-11-12 06:02:21 +09:00 · 2020-10-25 13:34:19 +01:00 · 2020-06-20 08:08:21 +09:00 · 2020-06-16 08:22:18 +09:00 · 2020-06-12 05:50:55 +09:00
26 changed files with 8415 additions and 7400 deletions
--- a/.clang-format
+++ b/.clang-format
@ -0,0 +1,156 @@
+---
+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
@ -0,0 +1,31 @@
+{
+    "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,3 +1,4 @@
 {
-    "C_Cpp.errorSquiggles": "Disabled"
+    "C_Cpp.errorSquiggles": "Disabled",
+    "cpplint.filters": ["-build/include_subdir", "-build/include_order", "-readability/casting", "-whitespace/comments"]
 }
--- a/.vscode/tasks.json
+++ b/.vscode/tasks.json
@ -2,19 +2,19 @@
    "version": "2.0.0",
    "tasks": [
        {
-            "label": "make",
+            "label": "build",
            "type": "shell",
            "command": "make",
-            "group": "build"
+            "group": {
+                "kind": "build",
+                "isDefault": true
+            }
        },
        {
            "label": "flash",
            "type": "shell",
            "command": "make dfu flash",
-            "group": {
-                "kind": "build",
-                "isDefault": true
-            }
+            "group": "build"
        }
    ]
 }
--- a/Font5x7.c
+++ b/Font5x7.c
--- a/23
+++ b/23
@ -81,6 +81,10 @@ endif
 # Project, sources and paths
 #

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

@ -100,9 +104,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
@ -118,12 +122,9 @@ 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 =
@ -153,8 +154,7 @@ ASMSRC = $(STARTUPASM) $(PORTASM) $(OSALASM)

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

 #
 # 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
+AS   = $(TRGT)gcc -x assembler-with-cpp -ggdb
 AR   = $(TRGT)ar
 OD   = $(TRGT)objdump
 SZ   = $(TRGT)size
@ -229,9 +229,4 @@ flash: build/ch.bin
 	dfu-util -d 0483:df11 -a 0 -s 0x08000000:leave -D build/ch.bin

 dfu:
-	-@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
-
+	-printf "reset dfu\r" >$(DEVICE) && sleep 1
--- a/NANOVNA_STM32_F072/board.h
+++ b/NANOVNA_STM32_F072/board.h
@ -17,10 +17,6 @@
 #ifndef _BOARD_H_
 #define _BOARD_H_

-/*
- * Setup for the Strawberry Linux STbee
- */
-
 /*
 * Board identifier.
 */
@ -130,8 +126,8 @@
                                     PIN_MODE_ALTERNATE(GPIOA_MCO) | \
                                     PIN_MODE_INPUT(9U) |           \
                                     PIN_MODE_OUTPUT(GPIOA_USB_DISC) | \
-                                     PIN_MODE_INPUT(GPIOA_USB_DM) |  \
-                                     PIN_MODE_INPUT(GPIOA_USB_DP) |  \
+                                     PIN_MODE_ALTERNATE(GPIOA_USB_DM) |  \
+                                     PIN_MODE_ALTERNATE(GPIOA_USB_DP) |  \
                                     PIN_MODE_ALTERNATE(GPIOA_JTMS) |    \
                                     PIN_MODE_ALTERNATE(GPIOA_JTCK) |    \
                                     PIN_MODE_OUTPUT(GPIOA_LCD_RESET))
@ -161,7 +157,7 @@
                                     PIN_OSPEED_2M(7) |          \
                                     PIN_OSPEED_100M(GPIOA_MCO) | \
                                     PIN_OSPEED_100M(9) |          \
-                                     PIN_OSPEED_100M(10) |         \
+                                     PIN_OSPEED_100M(GPIOA_USB_DISC) |         \
                                     PIN_OSPEED_100M(GPIOA_USB_DM) |     \
                                     PIN_OSPEED_100M(GPIOA_USB_DP) |     \
                                     PIN_OSPEED_100M(GPIOA_JTMS) |         \
@ -177,7 +173,7 @@
                                     PIN_PUPDR_FLOATING(7) |         \
                                     PIN_PUPDR_PULLUP(GPIOA_MCO) | \
                                     PIN_PUPDR_PULLUP(9) |         \
-                                     PIN_PUPDR_PULLUP(GPIOA_USB_DISC) | \
+                                     PIN_PUPDR_FLOATING(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 -C /usr/local 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
    $ 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](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
+* [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)

 ## Reference

@ -105,6 +105,7 @@ 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

@ -114,3 +115,4 @@ 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,80 +28,82 @@
 #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 */
-  ADC1->IER = 0;
+  VNA_ADC->IER = 0;

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

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

-uint16_t adc_single_read(ADC_TypeDef *adc, uint32_t chsel)
+uint16_t adc_single_read(uint32_t chsel)
 {
  /* ADC setup */
-  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;
+  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 conversion start.*/
-  adc->CR |= ADC_CR_ADSTART;
+  VNA_ADC->CR |= ADC_CR_ADSTART;

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

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

-int16_t adc_vbat_read(ADC_TypeDef *adc)
+int16_t adc_vbat_read(void)
 {
+// 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))
-	float vbat = 0;
-	float vrefint = 0;
-
+  adc_stop();
  ADC->CCR |= ADC_CCR_VREFEN | ADC_CCR_VBATEN;
  // VREFINT == ADC_IN17
-	vrefint = adc_single_read(adc, ADC_CHSELR_CHSEL17);
+  uint32_t vrefint = adc_single_read(ADC_CHSELR_CHSEL17);
  // VBAT == ADC_IN18
  // VBATEN enables resiter devider circuit. It consume vbat power.
-	vbat = adc_single_read(adc, ADC_CHSELR_CHSEL18);
+  uint32_t vbat = adc_single_read(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)));
+  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 + VBAT_DIODE_VF;
-
+  return vbat_raw + config.vbat_offset;
 }

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

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

  /* ADC setup, if it is defined a callback for the analog watch dog then it
     is enabled.*/
-  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;
+  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 configuration and start.*/
-  adc->CFGR1  = cfgr1;
+  VNA_ADC->CFGR1  = cfgr1;

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

-void adc_stop(ADC_TypeDef *adc)
+void adc_stop(void)
 {
-  if (adc->CR & ADC_CR_ADEN) {
-    if (adc->CR & ADC_CR_ADSTART) {
-      adc->CR |= ADC_CR_ADSTP;
-      while (adc->CR & ADC_CR_ADSTP)
+  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)
        ;
    }

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

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

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

-  adc_interrupt(ADC1);
+  adc_interrupt();

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

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

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

 /**
 * @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        TRUE
+#define CH_CFG_USE_MUTEXES_RECURSIVE        FALSE

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

 /**
 * @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           TRUE
+#define CH_CFG_USE_EVENTS_TIMEOUT           FALSE

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

 /**
 * @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 TRUE
+#define SHELL_CMD_INFO_ENABLED FALSE
 #define SHELL_CMD_ECHO_ENABLED FALSE
 #define SHELL_CMD_SYSTIME_ENABLED FALSE
 #define SHELL_CMD_MEM_ENABLED FALSE
-#define SHELL_CMD_THREADS_ENABLED TRUE
+#define SHELL_CMD_THREADS_ENABLED FALSE
 #define SHELL_CMD_TEST_ENABLED FALSE


--- a/chprintf.c
+++ b/chprintf.c
@ -0,0 +1,594 @@
+/*
+    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 }
 };

-int32_t acc_samp_s;
-int32_t acc_samp_c;
-int32_t acc_ref_s;
-int32_t acc_ref_c;
+float acc_samp_s;
+float acc_samp_c;
+float acc_ref_s;
+float 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 * 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);
+				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);
 				array[l][real] = array[j][real] - tpre;
 				array[l][imag] = array[j][imag] - tpim;
 				array[j][real] += tpre;
--- a/flash.c
+++ b/flash.c
@ -64,7 +64,6 @@ void flash_unlock(void)
  FLASH->KEYR = 0xCDEF89AB;
 }

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


 #define FLASH_PAGESIZE 0x800

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

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

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

  flash_unlock();

@ -114,7 +112,7 @@ config_recall(void)

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

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

-#define SAVEAREA_MAX 5
-
-const uint32_t saveareas[] =
-  { 0x08018800, 0x0801a000, 0x0801b800, 0x0801d000, 0x0801e800 };
+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 };

 int16_t lastsaveid = 0;

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

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

  flash_unlock();

@ -175,15 +174,15 @@ caldata_recall(int id)
  void *dst = &current_props;

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

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

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

  /* active configuration points to save data on flash memory */
  active_props = src;
@ -191,8 +190,10 @@ 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 *
@ -205,12 +206,12 @@ caldata_ref(int id)

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

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

 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                 TRUE
+#define HAL_USE_RTC                 FALSE
 #endif

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

 /**
--- a/ili9341.c
+++ b/ili9341.c
@ -21,6 +21,120 @@
 #include "hal.h"
 #include "nanovna.h"

+uint16_t spi_buffer[SPI_BUFFER_SIZE];
+// Default foreground & background colors
+uint16_t foreground_color = 0;
+uint16_t background_color = 0;
+
+// Display width and height definition
+#define ILI9341_WIDTH     320
+#define ILI9341_HEIGHT    240
+
+// Display commands list
+#define ILI9341_NOP                        0x00
+#define ILI9341_SOFTWARE_RESET             0x01
+#define ILI9341_READ_IDENTIFICATION        0x04
+#define ILI9341_READ_STATUS                0x09
+#define ILI9341_READ_POWER_MODE            0x0A
+#define ILI9341_READ_MADCTL                0x0B
+#define ILI9341_READ_PIXEL_FORMAT          0x0C
+#define ILI9341_READ_IMAGE_FORMAT          0x0D
+#define ILI9341_READ_SIGNAL_MODE           0x0E
+#define ILI9341_READ_SELF_DIAGNOSTIC       0x0F
+#define ILI9341_SLEEP_IN                   0x10
+#define ILI9341_SLEEP_OUT                  0x11
+#define ILI9341_PARTIAL_MODE_ON            0x12
+#define ILI9341_NORMAL_DISPLAY_MODE_ON     0x13
+#define ILI9341_INVERSION_OFF              0x20
+#define ILI9341_INVERSION_ON               0x21
+#define ILI9341_GAMMA_SET                  0x26
+#define ILI9341_DISPLAY_OFF                0x28
+#define ILI9341_DISPLAY_ON                 0x29
+#define ILI9341_COLUMN_ADDRESS_SET         0x2A
+#define ILI9341_PAGE_ADDRESS_SET           0x2B
+#define ILI9341_MEMORY_WRITE               0x2C
+#define ILI9341_COLOR_SET                  0x2D
+#define ILI9341_MEMORY_READ                0x2E
+#define ILI9341_PARTIAL_AREA               0x30
+#define ILI9341_VERTICAL_SCROLLING_DEF     0x33
+#define ILI9341_TEARING_LINE_OFF           0x34
+#define ILI9341_TEARING_LINE_ON            0x35
+#define ILI9341_MEMORY_ACCESS_CONTROL      0x36
+#define ILI9341_VERTICAL_SCROLLING         0x37
+#define ILI9341_IDLE_MODE_OFF              0x38
+#define ILI9341_IDLE_MODE_ON               0x39
+#define ILI9341_PIXEL_FORMAT_SET           0x3A
+#define ILI9341_WRITE_MEMORY_CONTINUE      0x3C
+#define ILI9341_READ_MEMORY_CONTINUE       0x3E
+#define ILI9341_SET_TEAR_SCANLINE          0x44
+#define ILI9341_GET_SCANLINE               0x45
+#define ILI9341_WRITE_BRIGHTNESS           0x51
+#define ILI9341_READ_BRIGHTNESS            0x52
+#define ILI9341_WRITE_CTRL_DISPLAY         0x53
+#define ILI9341_READ_CTRL_DISPLAY          0x54
+#define ILI9341_WRITE_CA_BRIGHTNESS        0x55
+#define ILI9341_READ_CA_BRIGHTNESS         0x56
+#define ILI9341_WRITE_CA_MIN_BRIGHTNESS    0x5E
+#define ILI9341_READ_CA_MIN_BRIGHTNESS     0x5F
+#define ILI9341_READ_ID1                   0xDA
+#define ILI9341_READ_ID2                   0xDB
+#define ILI9341_READ_ID3                   0xDC
+#define ILI9341_RGB_INTERFACE_CONTROL      0xB0
+#define ILI9341_FRAME_RATE_CONTROL_1       0xB1
+#define ILI9341_FRAME_RATE_CONTROL_2       0xB2
+#define ILI9341_FRAME_RATE_CONTROL_3       0xB3
+#define ILI9341_DISPLAY_INVERSION_CONTROL  0xB4
+#define ILI9341_BLANKING_PORCH_CONTROL     0xB5
+#define ILI9341_DISPLAY_FUNCTION_CONTROL   0xB6
+#define ILI9341_ENTRY_MODE_SET             0xB7
+#define ILI9341_BACKLIGHT_CONTROL_1        0xB8
+#define ILI9341_BACKLIGHT_CONTROL_2        0xB9
+#define ILI9341_BACKLIGHT_CONTROL_3        0xBA
+#define ILI9341_BACKLIGHT_CONTROL_4        0xBB
+#define ILI9341_BACKLIGHT_CONTROL_5        0xBC
+#define ILI9341_BACKLIGHT_CONTROL_7        0xBE
+#define ILI9341_BACKLIGHT_CONTROL_8        0xBF
+#define ILI9341_POWER_CONTROL_1            0xC0
+#define ILI9341_POWER_CONTROL_2            0xC1
+#define ILI9341_VCOM_CONTROL_1             0xC5
+#define ILI9341_VCOM_CONTROL_2             0xC7
+#define ILI9341_POWERA                     0xCB
+#define ILI9341_POWERB                     0xCF
+#define ILI9341_NV_MEMORY_WRITE            0xD0
+#define ILI9341_NV_PROTECTION_KEY          0xD1
+#define ILI9341_NV_STATUS_READ             0xD2
+#define ILI9341_READ_ID4                   0xD3
+#define ILI9341_POSITIVE_GAMMA_CORRECTION  0xE0
+#define ILI9341_NEGATIVE_GAMMA_CORRECTION  0xE1
+#define ILI9341_DIGITAL_GAMMA_CONTROL_1    0xE2
+#define ILI9341_DIGITAL_GAMMA_CONTROL_2    0xE3
+#define ILI9341_DTCA                       0xE8
+#define ILI9341_DTCB                       0xEA
+#define ILI9341_POWER_SEQ                  0xED
+#define ILI9341_3GAMMA_EN                  0xF2
+#define ILI9341_INTERFACE_CONTROL          0xF6
+#define ILI9341_PUMP_RATIO_CONTROL         0xF7
+
+//
+// ILI9341_MEMORY_ACCESS_CONTROL registers
+//
+#define ILI9341_MADCTL_MY  0x80
+#define ILI9341_MADCTL_MX  0x40
+#define ILI9341_MADCTL_MV  0x20
+#define ILI9341_MADCTL_ML  0x10
+#define ILI9341_MADCTL_BGR 0x08
+#define ILI9341_MADCTL_MH  0x04
+#define ILI9341_MADCTL_RGB 0x00
+
+#define DISPLAY_ROTATION_270   (ILI9341_MADCTL_MX | ILI9341_MADCTL_BGR)
+#define DISPLAY_ROTATION_90    (ILI9341_MADCTL_MY | ILI9341_MADCTL_BGR)
+#define DISPLAY_ROTATION_0     (ILI9341_MADCTL_MV | ILI9341_MADCTL_BGR)
+#define DISPLAY_ROTATION_180   (ILI9341_MADCTL_MX | ILI9341_MADCTL_MY  \
+                              | ILI9341_MADCTL_MV | ILI9341_MADCTL_BGR)
+
+//
+// Pin macros
+//
 #define RESET_ASSERT  palClearPad(GPIOA, 15)
 #define RESET_NEGATE  palSetPad(GPIOA, 15)
 #define CS_LOW        palClearPad(GPIOB, 6)
@ -28,380 +142,512 @@
 #define DC_CMD        palClearPad(GPIOB, 7)
 #define DC_DATA       palSetPad(GPIOB, 7)

-uint16_t spi_buffer[1024];
+//*****************************************************************************
+//********************************** SPI bus **********************************
+//*****************************************************************************
+// STM32 SPI transfer mode:
+// in 8 bit mode:
+// if you write *(uint8_t*)(&SPI1->DR)  = (uint8_t) data, then data send as << data
+// if you write *(uint16_t*)(&SPI1->DR) =(uint16_t) data, then data send as << dataLoByte, after send dataHiByte
+// in 16 bit mode
+// if you write *(uint16_t*)(&SPI1->DR) =(uint16_t) data, then data send as << data

-void
-ssp_wait(void)
+// SPI init in 8 bit mode
+#define SPI_CR2_8BIT  0x0700
+#define SPI_CR2_16BIT 0x0F00
+
+// SPI bus activity macros
+
+// The RXNE flag is set depending on the FRXTH bit value in the SPIx_CR2 register:
+// • If FRXTH is set, RXNE goes high and stays high until the RXFIFO level is greater or equal to 1/4 (8-bit).
+#define SPI_RX_IS_NOT_EMPTY  (SPI1->SR&SPI_SR_RXNE)
+#define SPI_RX_IS_EMPTY     (((SPI1->SR&SPI_SR_RXNE) == 0))
+
+// The TXE flag is set when transmission TXFIFO has enough space to store data to send.
+// 0: Tx buffer not empty, bit is cleared automatically when the TXFIFO level becomes greater than 1/2
+// 1: Tx buffer empty, flag goes high and stays high until the TXFIFO level is lower or equal to 1/2 of the FIFO depth
+#define SPI_TX_IS_NOT_EMPTY  (((SPI1->SR&(SPI_SR_TXE)) == 0))
+#define SPI_TX_IS_EMPTY     (SPI1->SR&SPI_SR_TXE)
+
+// When BSY is set, it indicates that a data transfer is in progress on the SPI (the SPI bus is busy).
+#define SPI_IS_BUSY     (SPI1->SR & SPI_SR_BSY)
+
+// SPI send data macros
+#define SPI_WRITE_8BIT(data)  *(__IO uint8_t*)(&SPI1->DR) = (uint8_t) data
+#define SPI_WRITE_16BIT(data) SPI1->DR = data
+
+// SPI read data macros
+#define SPI_READ_DATA        SPI1->DR
+
+#ifdef __USE_DISPLAY_DMA__
+static const stm32_dma_stream_t *dmatx =
+    STM32_DMA_STREAM(STM32_SPI_SPI1_TX_DMA_STREAM);
+static uint32_t txdmamode =
+    STM32_DMA_CR_CHSEL(SPI1_TX_DMA_CHANNEL)         // Select SPI1 Tx DMA
+    | STM32_DMA_CR_PL(STM32_SPI_SPI1_DMA_PRIORITY)  // Set priority
+    | STM32_DMA_CR_DIR_M2P                          // Memory to Spi
+    | STM32_DMA_CR_DMEIE                            //
+    | STM32_DMA_CR_TEIE;
+
+static void spi_lld_serve_tx_interrupt(SPIDriver *spip, uint32_t flags)
 {
-  while (SPI1->SR & SPI_SR_BSY)
-    ;
-}
-
-void
-ssp_wait_slot(void)
-{
-  while ((SPI1->SR & 0x1800) == 0x1800)
-    ;
-}
-
-void
-ssp_senddata(uint8_t x)
-{
-  *(uint8_t*)(&SPI1->DR) = x;
-  while (SPI1->SR & SPI_SR_BSY)
-    ;
-}
-
-uint8_t
-ssp_sendrecvdata(uint8_t x)
-{
-    while (!(SPI1->SR & SPI_SR_TXE));
-    // clear OVR
-    while (SPI1->SR & SPI_SR_RXNE) (void)SPI1->DR;
-
-    *(uint8_t*)(&SPI1->DR) = x;
-    while (!(SPI1->SR & SPI_SR_RXNE));
-    return SPI1->DR;
-}
-
-void
-ssp_senddata16(uint16_t x)
-{
-  ssp_wait_slot();
-  SPI1->DR = x;
-  //while (SPI1->SR & SPI_SR_BSY)
-  //  ;
-}
-
-void
-ssp_databit8(void)
-{
-  SPI1->CR2 = (SPI1->CR2 & 0xf0ff) | 0x0700;
-//LPC_SSP1->CR0 = (LPC_SSP1->CR0 & 0xf0) | SSP_DATABIT_8;
-}
-
-void
-ssp_databit16(void)
-{
-  SPI1->CR2 = (SPI1->CR2 & 0xf0ff) | 0x0f00;
-  //LPC_SSP1->CR0 = (LPC_SSP1->CR0 & 0xf0) | SSP_DATABIT_16;
-}
-
-
-const stm32_dma_stream_t  *dmatx;
-uint32_t txdmamode;
-
-static void spi_lld_serve_tx_interrupt(SPIDriver *spip, uint32_t flags) {
  (void)spip;
  (void)flags;
 }

-void
-spi_init(void)
+static const stm32_dma_stream_t  *dmarx = STM32_DMA_STREAM(STM32_SPI_SPI1_RX_DMA_STREAM);
+static uint32_t rxdmamode = STM32_DMA_CR_CHSEL(SPI1_RX_DMA_CHANNEL)
+                        | STM32_DMA_CR_PL(STM32_SPI_SPI1_DMA_PRIORITY)
+                        | STM32_DMA_CR_DIR_P2M
+                        | STM32_DMA_CR_TCIE
+                        | STM32_DMA_CR_DMEIE
+                        | STM32_DMA_CR_TEIE;
+
+static void spi_lld_serve_rx_interrupt(SPIDriver *spip, uint32_t flags)
 {
-  rccEnableSPI1(FALSE);
-
-  dmatx     = STM32_DMA_STREAM(STM32_SPI_SPI1_TX_DMA_STREAM);
-  txdmamode = STM32_DMA_CR_CHSEL(SPI1_TX_DMA_CHANNEL) |
-    STM32_DMA_CR_PL(STM32_SPI_SPI1_DMA_PRIORITY) |
-    STM32_DMA_CR_DIR_M2P |
-    STM32_DMA_CR_DMEIE |
-    STM32_DMA_CR_TEIE |
-    STM32_DMA_CR_PSIZE_HWORD |
-    STM32_DMA_CR_MSIZE_HWORD;
-  dmaStreamAllocate(dmatx,
-                    STM32_SPI_SPI1_IRQ_PRIORITY,
-                    (stm32_dmaisr_t)spi_lld_serve_tx_interrupt,
-                    NULL);
-  dmaStreamSetPeripheral(dmatx, &SPI1->DR);
-
-  SPI1->CR1 = 0;
-  SPI1->CR1 = SPI_CR1_MSTR | SPI_CR1_SSM | SPI_CR1_SSI;// | SPI_CR1_BR_1;
-  SPI1->CR2 = 0x0700 | SPI_CR2_TXDMAEN | SPI_CR2_FRXTH;
-  SPI1->CR1 |= SPI_CR1_SPE;  
+  (void)spip;
+  (void)flags;
 }

-void
-send_command(uint8_t cmd, int len, const uint8_t *data)
+static void dmaStreamFlush(uint32_t len)
+{
+  while (len) {
+    // DMA data transfer limited by 65535
+    uint16_t tx_size = len > 65535 ? 65535 : len;
+    dmaStreamSetTransactionSize(dmatx, tx_size);
+    dmaStreamEnable(dmatx);
+    len -= tx_size;
+    dmaWaitCompletion(dmatx);
+  }
+}
+#endif
+
+static void spi_init(void)
+{
+  rccEnableSPI1(FALSE);
+  SPI1->CR1 = 0;
+  SPI1->CR1 = SPI_CR1_MSTR       // SPI is MASTER
+            | SPI_CR1_SSM        // Software slave management (The external NSS pin is free for other application uses)
+            | SPI_CR1_SSI;       // Internal slave select (This bit has an effect only when the SSM bit is set. Allow use NSS pin as I/O)
+//          | SPI_CR1_BR_1;      // Baud rate control
+
+  SPI1->CR2 = SPI_CR2_8BIT       // SPI data size, set to 8 bit
+            | SPI_CR2_FRXTH;     // SPI_SR_RXNE generated every 8 bit data
+//          | SPI_CR2_SSOE;      //
+
+#ifdef __USE_DISPLAY_DMA__
+  // Tx DMA init
+  dmaStreamAllocate(dmatx, STM32_SPI_SPI1_IRQ_PRIORITY, (stm32_dmaisr_t)spi_lld_serve_tx_interrupt, NULL);
+  dmaStreamSetPeripheral(dmatx, &SPI1->DR);
+  // Rx DMA init
+  dmaStreamAllocate(dmarx, STM32_SPI_SPI1_IRQ_PRIORITY, (stm32_dmaisr_t)spi_lld_serve_rx_interrupt, NULL);
+  dmaStreamSetPeripheral(dmarx, &SPI1->DR);
+  // Enable DMA on SPI
+  SPI1->CR2|= SPI_CR2_TXDMAEN    // Tx DMA enable
+           |  SPI_CR2_RXDMAEN;   // Rx DMA enable
+#endif
+  SPI1->CR1|= SPI_CR1_SPE;       //SPI enable
+}
+
+// Disable inline for this function
+static void __attribute__ ((noinline)) send_command(uint8_t cmd, uint8_t len, const uint8_t *data)
 {
  CS_LOW;
+  //while (SPI_TX_IS_NOT_EMPTY);
  DC_CMD;
-    ssp_databit8();
-	ssp_senddata(cmd);
+  SPI_WRITE_8BIT(cmd);
+  // Need wait transfer complete and set data bit
+  while (SPI_IS_BUSY)
+    ;
+  // Send command data (if need)
  DC_DATA;
  while (len-- > 0) {
-      ssp_senddata(*data++);
+    while (SPI_TX_IS_NOT_EMPTY)
+      ;
+    SPI_WRITE_8BIT(*data++);
  }
  //CS_HIGH;
 }

-void
-send_command16(uint8_t cmd, int data)
-{
-	CS_LOW;
-	DC_CMD;
-    ssp_databit8();
-	ssp_senddata(cmd);
-	DC_DATA;
-    ssp_databit16();
-	ssp_senddata16(data);
-	CS_HIGH;
-}
-
-const uint8_t ili9341_init_seq[] = {
+static const uint8_t ili9341_init_seq[] = {
  // cmd, len, data...,
+  // SW reset
+  ILI9341_SOFTWARE_RESET, 0,
+  // display off
+  ILI9341_DISPLAY_OFF, 0,
  // Power control B
-		0xCF, 3, 0x00, 0x83, 0x30,
+  ILI9341_POWERB, 3, 0x00, 0x83, 0x30,
  // Power on sequence control
-		0xED, 4, 0x64, 0x03, 0x12, 0x81,
-		//0xED, 4, 0x55, 0x01, 0x23, 0x01,
+  ILI9341_POWER_SEQ, 4, 0x64, 0x03, 0x12, 0x81,
+  //ILI9341_POWER_SEQ, 4, 0x55, 0x01, 0x23, 0x01,
  // Driver timing control A
-		0xE8, 3, 0x85, 0x01, 0x79,
-		//0xE8, 3, 0x84, 0x11, 0x7a,
+  ILI9341_DTCA, 3, 0x85, 0x01, 0x79,
+  //ILI9341_DTCA, 3, 0x84, 0x11, 0x7a,
  // Power control A
-		0xCB, 5, 0x39, 0x2C, 0x00, 0x34, 0x02,
+  ILI9341_POWERA, 5, 0x39, 0x2C, 0x00, 0x34, 0x02,
  // Pump ratio control
-		0xF7, 1, 0x20,
+  ILI9341_PUMP_RATIO_CONTROL, 1, 0x20,
  // Driver timing control B
-		0xEA, 2, 0x00, 0x00,
+  ILI9341_DTCB, 2, 0x00, 0x00,
  // POWER_CONTROL_1
-		0xC0, 1, 0x26,
+  ILI9341_POWER_CONTROL_1, 1, 0x26,
  // POWER_CONTROL_2
-		0xC1, 1, 0x11,
+  ILI9341_POWER_CONTROL_2, 1, 0x11,
  // VCOM_CONTROL_1
-		0xC5, 2, 0x35, 0x3E,
+  ILI9341_VCOM_CONTROL_1, 2, 0x35, 0x3E,
  // VCOM_CONTROL_2
-		0xC7, 1, 0xBE,
+  ILI9341_VCOM_CONTROL_2, 1, 0xBE,
  // MEMORY_ACCESS_CONTROL
-		//0x36, 1, 0x48, // portlait
-		0x36, 1, 0x28, // landscape
+  //ILI9341_MEMORY_ACCESS_CONTROL, 1, 0x48, // portlait
+  ILI9341_MEMORY_ACCESS_CONTROL, 1, DISPLAY_ROTATION_0, // landscape
  // COLMOD_PIXEL_FORMAT_SET : 16 bit pixel
-		0x3A, 1, 0x55,
+  ILI9341_PIXEL_FORMAT_SET, 1, 0x55,
  // Frame Rate
-		0xB1, 2, 0x00, 0x1B,
+  ILI9341_FRAME_RATE_CONTROL_1, 2, 0x00, 0x1B,
  // Gamma Function Disable
-		0xF2, 1, 0x08,
+  ILI9341_3GAMMA_EN, 1, 0x08,
  // gamma set for curve 01/2/04/08
-		0x26, 1, 0x01,
+  ILI9341_GAMMA_SET, 1, 0x01,
  // positive gamma correction
-		0xE0, 15, 0x1F,  0x1A,  0x18,  0x0A,  0x0F,  0x06,  0x45,  0x87,  0x32,  0x0A,  0x07,  0x02,  0x07, 0x05,  0x00,
+//ILI9341_POSITIVE_GAMMA_CORRECTION, 15, 0x1F,  0x1A,  0x18,  0x0A,  0x0F,  0x06,  0x45,  0x87,  0x32,  0x0A,  0x07,  0x02,  0x07, 0x05,  0x00,
  // negativ gamma correction
-		0xE1, 15, 0x00,  0x25,  0x27,  0x05,  0x10,  0x09,  0x3A,  0x78,  0x4D,  0x05,  0x18,  0x0D,  0x38, 0x3A,  0x1F,
-
+//ILI9341_NEGATIVE_GAMMA_CORRECTION, 15, 0x00,  0x25,  0x27,  0x05,  0x10,  0x09,  0x3A,  0x78,  0x4D,  0x05,  0x18,  0x0D,  0x38, 0x3A,  0x1F,
  // Column Address Set
-	    0x2A, 4, 0x00, 0x00, 0x01, 0x3f, // width 320
+//ILI9341_COLUMN_ADDRESS_SET, 4, 0x00, 0x00, 0x01, 0x3f, // width 320
  // Page Address Set
-	    0x2B, 4, 0x00, 0x00, 0x00, 0xef, // height 240
-
+//ILI9341_PAGE_ADDRESS_SET, 4, 0x00, 0x00, 0x00, 0xef,   // height 240
  // entry mode
-		0xB7, 1, 0x06,
+  ILI9341_ENTRY_MODE_SET, 1, 0x06,
  // display function control
-		0xB6, 4, 0x0A, 0x82, 0x27, 0x00,
-
+  ILI9341_DISPLAY_FUNCTION_CONTROL, 4, 0x0A, 0x82, 0x27, 0x00,
+  // Interface Control (set WEMODE=0)
+  ILI9341_INTERFACE_CONTROL, 3, 0x00, 0x00, 0x00,
  // control display
-		//0x53, 1, 0x0c,
+  //ILI9341_WRITE_CTRL_DISPLAY, 1, 0x0c,
  // diaplay brightness
-		//0x51, 1, 0xff,
-
+  //ILI9341_WRITE_BRIGHTNESS, 1, 0xff,
  // sleep out
-		0x11, 0,
+  ILI9341_SLEEP_OUT, 0,
+  // display on
+  ILI9341_DISPLAY_ON, 0,
  0 // sentinel
 };

-void
-ili9341_init(void)
+void ili9341_init(void)
 {
  spi_init();
-
  DC_DATA;
  RESET_ASSERT;
  chThdSleepMilliseconds(10);
  RESET_NEGATE;
-
-  send_command(0x01, 0, NULL); // SW reset
-  chThdSleepMilliseconds(5);
-  send_command(0x28, 0, NULL); // display off
-
  const uint8_t *p;
  for (p = ili9341_init_seq; *p; ) {
    send_command(p[0], p[1], &p[2]);
    p += 2 + p[1];
    chThdSleepMilliseconds(5);
  }
-
-  chThdSleepMilliseconds(100);
-  send_command(0x29, 0, NULL); // display on
 }

-void ili9341_pixel(int x, int y, int color)
-{
-	uint8_t xx[4] = { x >> 8, x, (x+1) >> 8, (x+1) };
-	uint8_t yy[4] = { y >> 8, y, (y+1) >> 8, (y+1) };
-	uint8_t cc[2] = { color >> 8, color };
-	send_command(0x2A, 4, xx);
-    send_command(0x2B, 4, yy);
-    send_command(0x2C, 2, cc);
-    //send_command16(0x2C, color);
-}
-
-
-
+#ifndef __USE_DISPLAY_DMA__
 void ili9341_fill(int x, int y, int w, int h, int color)
 {
-	uint8_t xx[4] = { x >> 8, x, (x+w-1) >> 8, (x+w-1) };
-	uint8_t yy[4] = { y >> 8, y, (y+h-1) >> 8, (y+h-1) };
-    int len = w * h;
-	send_command(0x2A, 4, xx);
-    send_command(0x2B, 4, yy);
-    send_command(0x2C, 0, NULL);
-    while (len-- > 0) 
-      ssp_senddata16(color);
+//uint8_t xx[4] = { x >> 8, x, (x+w-1) >> 8, (x+w-1) };
+//uint8_t yy[4] = { y >> 8, y, (y+h-1) >> 8, (y+h-1) };
+  uint32_t xx = __REV16(x | ((x + w - 1) << 16));
+  uint32_t yy = __REV16(y | ((y + h - 1) << 16));
+  send_command(ILI9341_COLUMN_ADDRESS_SET, 4, (uint8_t*)&xx);
+  send_command(ILI9341_PAGE_ADDRESS_SET, 4, (uint8_t*)&yy);
+  send_command(ILI9341_MEMORY_WRITE, 0, NULL);
+  int32_t len = w * h;
+  while (len-- > 0) {
+    while (SPI_TX_IS_NOT_EMPTY)
+      ;
+  SPI_WRITE_16BIT(color);
+  }
 }

-#if 0
 void ili9341_bulk(int x, int y, int w, int h)
 {
-	uint8_t xx[4] = { x >> 8, x, (x+w-1) >> 8, (x+w-1) };
-	uint8_t yy[4] = { y >> 8, y, (y+h-1) >> 8, (y+h-1) };
+  // uint8_t xx[4] = { x >> 8, x, (x+w-1) >> 8, (x+w-1) };
+  // uint8_t yy[4] = { y >> 8, y, (y+h-1) >> 8, (y+h-1) };
  uint16_t *buf = spi_buffer;
-    int len = w * h;
-	send_command(0x2A, 4, xx);
-	send_command(0x2B, 4, yy);
-	send_command(0x2C, 0, NULL);
-    while (len-- > 0) 
-      ssp_senddata16(*buf++);
+  uint32_t xx = __REV16(x | ((x + w - 1) << 16));
+  uint32_t yy = __REV16(y | ((y + h - 1) << 16));
+  send_command(ILI9341_COLUMN_ADDRESS_SET, 4, (uint8_t *)&xx);
+  send_command(ILI9341_PAGE_ADDRESS_SET, 4, (uint8_t*)&yy);
+  send_command(ILI9341_MEMORY_WRITE, 0, NULL);
+  int32_t len = w * h;
+  while (len-- > 0) {
+    while (SPI_TX_IS_NOT_EMPTY)
+      ;
+    SPI_WRITE_16BIT(*buf++);
+  }
+}
+
+static uint8_t ssp_sendrecvdata(void)
+{
+  // Start RX clock (by sending data)
+  SPI_WRITE_8BIT(0);
+  while (SPI_RX_IS_EMPTY && SPI_IS_BUSY)
+    ;
+  return SPI_READ_DATA;
+}
+
+void ili9341_read_memory(int x, int y, int w, int h, int len, uint16_t *out)
+{
+  // uint8_t xx[4] = { x >> 8, x, (x+w-1) >> 8, (x+w-1) };
+  // uint8_t yy[4] = { y >> 8, y, (y+h-1) >> 8, (y+h-1) };
+  uint32_t xx = __REV16(x | ((x + w - 1) << 16));
+  uint32_t yy = __REV16(y | ((y + h - 1) << 16));
+  send_command(ILI9341_COLUMN_ADDRESS_SET, 4, (uint8_t *)&xx);
+  send_command(ILI9341_PAGE_ADDRESS_SET, 4, (uint8_t*)&yy);
+  send_command(ILI9341_MEMORY_READ, 0, NULL);
+
+  // Skip data from rx buffer
+  while (SPI_RX_IS_NOT_EMPTY)
+    (void) SPI_READ_DATA;
+  // require 8bit dummy clock
+  ssp_sendrecvdata();
+  while (len-- > 0) {
+    // read data is always 18bit
+    uint8_t r = ssp_sendrecvdata();
+    uint8_t g = ssp_sendrecvdata();
+    uint8_t b = ssp_sendrecvdata();
+    *out++ = RGB565(r, g, b);
+  }
+  CS_HIGH;
 }
 #else
+//
+// Use DMA for send data
+//
+
+// Fill region by some color
+void ili9341_fill(int x, int y, int w, int h, int color)
+{
+  uint32_t xx = __REV16(x | ((x + w - 1) << 16));
+  uint32_t yy = __REV16(y | ((y + h - 1) << 16));
+  send_command(ILI9341_COLUMN_ADDRESS_SET, 4, (uint8_t *)&xx);
+  send_command(ILI9341_PAGE_ADDRESS_SET, 4, (uint8_t *)&yy);
+  send_command(ILI9341_MEMORY_WRITE, 0, NULL);
+
+  dmaStreamSetMemory0(dmatx, &color);
+  dmaStreamSetMode(dmatx, txdmamode | STM32_DMA_CR_PSIZE_HWORD | STM32_DMA_CR_MSIZE_HWORD);
+  dmaStreamFlush(w * h);
+}
+
+void ili9341_bulk_8bit(int x, int y, int w, int h, uint16_t *palette)
+{
+  uint32_t xx = __REV16(x | ((x + w - 1) << 16));
+  uint32_t yy = __REV16(y | ((y + h - 1) << 16));
+  send_command(ILI9341_COLUMN_ADDRESS_SET, 4, (uint8_t *)&xx);
+  send_command(ILI9341_PAGE_ADDRESS_SET, 4, (uint8_t *)&yy);
+  send_command(ILI9341_MEMORY_WRITE, 0, NULL);
+
+  uint8_t *buf = (uint8_t *)spi_buffer;
+  int32_t len = w * h;
+  while (len-- > 0) {
+    uint16_t color = palette[*buf++];
+    while (SPI_TX_IS_NOT_EMPTY)
+      ;
+    SPI_WRITE_16BIT(color);
+  }
+}
+
+// Copy spi_buffer to region
 void ili9341_bulk(int x, int y, int w, int h)
 {
-	uint8_t xx[4] = { x >> 8, x, (x+w-1) >> 8, (x+w-1) };
-	uint8_t yy[4] = { y >> 8, y, (y+h-1) >> 8, (y+h-1) };
-    int len = w * h;
-
-	send_command(0x2A, 4, xx);
-	send_command(0x2B, 4, yy);
-	send_command(0x2C, 0, NULL);
+  uint32_t xx = __REV16(x | ((x + w - 1) << 16));
+  uint32_t yy = __REV16(y | ((y + h - 1) << 16));
+  send_command(ILI9341_COLUMN_ADDRESS_SET, 4, (uint8_t *)&xx);
+  send_command(ILI9341_PAGE_ADDRESS_SET, 4, (uint8_t *)&yy);
+  send_command(ILI9341_MEMORY_WRITE, 0, NULL);

+  // Init Tx DMA mem->spi, set size, mode (spi and mem data size is 16 bit)
  dmaStreamSetMemory0(dmatx, spi_buffer);
-    dmaStreamSetTransactionSize(dmatx, len);
-    dmaStreamSetMode(dmatx, txdmamode | STM32_DMA_CR_MINC);
+  dmaStreamSetMode(dmatx, txdmamode | STM32_DMA_CR_PSIZE_HWORD |
+                              STM32_DMA_CR_MSIZE_HWORD | STM32_DMA_CR_MINC);
+  dmaStreamFlush(w * h);
+}
+
+// Copy screen data to buffer
+// Warning!!! buffer size must be greater then 3*len + 1 bytes
+void ili9341_read_memory(int x, int y, int w, int h, int len, uint16_t *out)
+{
+  uint8_t dummy_tx = 0;
+  uint8_t *rgbbuf = (uint8_t *)out;
+  uint16_t data_size = len * 3 + 1;
+  uint32_t xx = __REV16(x | ((x + w - 1) << 16));
+  uint32_t yy = __REV16(y | ((y + h - 1) << 16));
+  send_command(ILI9341_COLUMN_ADDRESS_SET, 4, (uint8_t *)&xx);
+  send_command(ILI9341_PAGE_ADDRESS_SET, 4, (uint8_t *)&yy);
+  send_command(ILI9341_MEMORY_READ, 0, NULL);
+  // Skip SPI rx buffer
+  while (SPI_RX_IS_NOT_EMPTY) (void)SPI_READ_DATA;
+  // Init Rx DMA buffer, size, mode (spi and mem data size is 8 bit)
+  dmaStreamSetMemory0(dmarx, rgbbuf);
+  dmaStreamSetTransactionSize(dmarx, data_size);
+  dmaStreamSetMode(dmarx, rxdmamode | STM32_DMA_CR_PSIZE_BYTE | STM32_DMA_CR_MSIZE_BYTE |
+                              STM32_DMA_CR_MINC);
+  // Init dummy Tx DMA (for rx clock), size, mode (spi and mem data size is 8 bit)
+  dmaStreamSetMemory0(dmatx, &dummy_tx);
+  dmaStreamSetTransactionSize(dmatx, data_size);
+  dmaStreamSetMode(dmatx, txdmamode | STM32_DMA_CR_PSIZE_BYTE | STM32_DMA_CR_MSIZE_BYTE);
+
+  // Start DMA exchange
  dmaStreamEnable(dmatx);
+  dmaStreamEnable(dmarx);
+  // Wait DMA completion
  dmaWaitCompletion(dmatx);
+  dmaWaitCompletion(dmarx);
+  CS_HIGH;
+
+  // Parce recived data
+  // Skip dummy 8-bit read
+  rgbbuf++;
+  while (len-- > 0) {
+    uint8_t r, g, b;
+    // read data is always 18bit
+    r = rgbbuf[0];
+    g = rgbbuf[1];
+    b = rgbbuf[2];
+    *out++ = RGB565(r, g, b);
+    rgbbuf += 3;
+  }
 }
 #endif

-void
-ili9341_read_memory_raw(uint8_t cmd, int len, uint16_t* out)
+void ili9341_clear_screen(void) 
 {
-    uint8_t r, g, b;
-    send_command(cmd, 0, NULL);
-    ssp_databit8();
-
-    // consume old data
-    while (!(SPI1->SR & SPI_SR_TXE));
-    // clear OVR
-    while (SPI1->SR & SPI_SR_RXNE) r = SPI1->DR;
-
-    // require 8bit dummy clock
-    r = ssp_sendrecvdata(0);
-
-    while (len-- > 0) {
-        // read data is always 18bit
-        r = ssp_sendrecvdata(0);
-        g = ssp_sendrecvdata(0);
-        b = ssp_sendrecvdata(0);
-        *out++ = ((r & 0xF8) << 8) | ((g & 0xFC) << 3) | (b >> 3);
+  ili9341_fill(0, 0, ILI9341_WIDTH, ILI9341_HEIGHT, background_color);
 }

-    CS_HIGH;
-}
-
-void
-ili9341_read_memory(int x, int y, int w, int h, int len, uint16_t *out)
+void ili9341_set_foreground(uint16_t fg)
 {
-    uint8_t xx[4] = { x >> 8, x, (x+w-1) >> 8, (x+w-1) };
-    uint8_t yy[4] = { y >> 8, y, (y+h-1) >> 8, (y+h-1) };
-
-    send_command(0x2A, 4, xx);
-    send_command(0x2B, 4, yy);
-
-    ili9341_read_memory_raw(0x2E, len, out);
+  foreground_color = fg;
 }

-void
-ili9341_read_memory_continue(int len, uint16_t* out)
+void ili9341_set_background(uint16_t bg)
 {
-    ili9341_read_memory_raw(0x3E, len, out);
+  background_color = bg; 
 }

-void
-ili9341_drawchar_5x7(uint8_t ch, int x, int y, uint16_t fg, uint16_t bg)
+void ili9341_set_rotation(uint8_t r)
+{
+  //  static const uint8_t rotation_const[]={DISPLAY_ROTATION_0, DISPLAY_ROTATION_90,
+  //  DISPLAY_ROTATION_180, DISPLAY_ROTATION_270};
+  send_command(ILI9341_MEMORY_ACCESS_CONTROL, 1, &r);
+}
+
+void blit8BitWidthBitmap(uint16_t x, uint16_t y, uint16_t width, uint16_t height,
+                         const uint8_t *bitmap) 
 {
  uint16_t *buf = spi_buffer;
-  uint8_t bits;
-  int c, r;
-  for(c = 0; c < 7; c++) {
-    bits = x5x7_bits[(ch * 7) + c];
-    for (r = 0; r < 5; r++) {
-      *buf++ = (0x80 & bits) ? fg : bg;
+  for (uint16_t c = 0; c < height; c++) {
+    uint8_t bits = *bitmap++;
+    for (uint16_t r = 0; r < width; r++) {
+      *buf++ = (0x80 & bits) ? foreground_color : background_color;
      bits <<= 1;
    }
  }
-  ili9341_bulk(x, y, 5, 7);
+  ili9341_bulk(x, y, width, height);
 }

-void
-ili9341_drawstring_5x7(const char *str, int x, int y, uint16_t fg, uint16_t bg)
-{
-  while (*str) {
-    ili9341_drawchar_5x7(*str, x, y, fg, bg);
-    x += 5;
-    str++;
-  }
-}
-
-void
-ili9341_drawstring_5x7_inv(const char *str, int x, int y, uint16_t fg, uint16_t bg, bool invert)
-{
-  if (invert)
-    ili9341_drawstring_5x7(str, x, y, bg, fg);
-  else
-    ili9341_drawstring_5x7(str, x, y, fg, bg);
-}
-
-void
-ili9341_drawchar_size(uint8_t ch, int x, int y, uint16_t fg, uint16_t bg, uint8_t size)
+static void blit16BitWidthBitmap(uint16_t x, uint16_t y, uint16_t width, uint16_t height,
+                                 const uint16_t *bitmap) 
 {
  uint16_t *buf = spi_buffer;
-  uint8_t bits;
-  int c, r;
-  for(c = 0; c < 7*size; c++) {
-    bits = x5x7_bits[(ch * 7) + (c / size)];
-    for (r = 0; r < 5*size; r++) {
-      *buf++ = (0x80 & bits) ? fg : bg;
-      if (r % size == (size-1)) {
+  for (uint16_t c = 0; c < height; c++) {
+    uint16_t bits = *bitmap++;
+    for (uint16_t r = 0; r < width; r++) {
+      *buf++ = (0x8000 & bits) ? foreground_color : background_color;
      bits <<= 1;
    }
  }
-  }
-  ili9341_bulk(x, y, 5*size, 7*size);
+  ili9341_bulk(x, y, width, height);
 }

-void
-ili9341_drawstring_size(const char *str, int x, int y, uint16_t fg, uint16_t bg, uint8_t size)
+void ili9341_drawchar(uint8_t ch, int x, int y)
+{
+  blit8BitWidthBitmap(x, y, FONT_GET_WIDTH(ch), FONT_GET_HEIGHT, FONT_GET_DATA(ch));
+}
+
+void ili9341_drawstring(const char *str, int x, int y)
 {
  while (*str) {
-    ili9341_drawchar_size(*str, x, y, fg, bg, size);
-    x += 5 * size;
-    str++;
+    uint8_t ch = *str++;
+    const uint8_t *char_buf = FONT_GET_DATA(ch);
+    uint16_t w = FONT_GET_WIDTH(ch);
+    blit8BitWidthBitmap(x, y, w, FONT_GET_HEIGHT, char_buf);
+    x += w;
  }
 }

-#define SWAP(x,y) do { int z=x; x = y; y = z; } while(0)
-
-void
-ili9341_line(int x0, int y0, int x1, int y1, uint16_t fg)
+void ili9341_drawstringV(const char *str, int x, int y)
 {
+  ili9341_set_rotation(DISPLAY_ROTATION_270);
+  ili9341_drawstring(str, ILI9341_HEIGHT-y, x);
+  ili9341_set_rotation(DISPLAY_ROTATION_0);
+}
+
+int ili9341_drawchar_size(uint8_t ch, int x, int y, uint8_t size)
+{
+  uint16_t *buf = spi_buffer;
+  const uint8_t *char_buf = FONT_GET_DATA(ch);
+  uint16_t w = FONT_GET_WIDTH(ch);
+  for (int c = 0; c < FONT_GET_HEIGHT; c++, char_buf++) {
+    for (int i = 0; i < size; i++) {
+      uint8_t bits = *char_buf;
+      for (int r = 0; r < w; r++, bits <<= 1)
+        for (int j = 0; j < size; j++)
+          *buf++ = (0x80 & bits) ? foreground_color : background_color;
+    }
+  }
+  ili9341_bulk(x, y, w * size, FONT_GET_HEIGHT * size);
+  return w*size;
+}
+
+void ili9341_drawfont(uint8_t ch, int x, int y)
+{
+  blit16BitWidthBitmap(x, y, NUM_FONT_GET_WIDTH, NUM_FONT_GET_HEIGHT,
+                       NUM_FONT_GET_DATA(ch));
+}
+
+void ili9341_drawstring_size(const char *str, int x, int y, uint8_t size)
+{
+  while (*str)
+    x += ili9341_drawchar_size(*str++, x, y, size);
+}
+#if 0
+static void ili9341_pixel(int x, int y, uint16_t color)
+{
+  uint32_t xx = __REV16(x|((x)<<16));
+  uint32_t yy = __REV16(y|((y)<<16));
+  send_command(ILI9341_COLUMN_ADDRESS_SET, 4, (uint8_t*)&xx);
+  send_command(ILI9341_PAGE_ADDRESS_SET, 4, (uint8_t*)&yy);
+  send_command(ILI9341_MEMORY_WRITE, 2, &color);
+}
+#endif
+
+#define SWAP(x, y) { int z = x; x = y; y = z; }
+
+void ili9341_line(int x0, int y0, int x1, int y1)
+{
+#if 0
+  // modifed Bresenham's line algorithm, see https://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm
+  int dx = x1 - x0, sx = 1; if (dx < 0) {dx = -dx; sx = -1;}
+  int dy = y1 - y0, sy = 1; if (dy < 0) {dy = -dy; sy = -1;}
+  int err = (dx > dy ? dx : -dy) / 2;
+  while (1) {
+    ili9341_pixel(x0, y0, DEFAULT_FG_COLOR);
+    if (x0 == x1 && y0 == y1)
+      break;
+    int e2 = err;
+    if (e2 > -dx) { err -= dy; x0 += sx; }
+    if (e2 <  dy) { err += dx; y0 += sy; }
+  }
+#endif
+
  if (x0 > x1) {
    SWAP(x0, x1);
    SWAP(y0, y1);
@ -426,48 +672,21 @@ ili9341_line(int x0, int y0, int x1, int y1, uint16_t fg)
      }
    }
    if (dy > 0)
-      ili9341_fill(x0, y0, dx, dy, fg);
+      ili9341_fill(x0, y0, dx, dy, foreground_color);
    else
-      ili9341_fill(x0, y0+dy, dx, -dy, fg);
+      ili9341_fill(x0, y0+dy, dx, -dy, foreground_color);
    x0 += dx;
    y0 += dy;
  }
 }

-
-const font_t NF20x22 = { 20, 22, 1, 3*22, (const uint8_t *)numfont20x22 };
-
-void
-ili9341_drawfont(uint8_t ch, const font_t *font, int x, int y, uint16_t fg, uint16_t bg)
-{
-	uint16_t *buf = spi_buffer;
-	const uint8_t *bitmap = &font->bitmap[font->slide * ch];
-	int c, r;
-
-	for (c = 0; c < font->height; c++) {
-    uint8_t bits = *bitmap++;
-    uint8_t m = 0x80;
-    for (r = 0; r < font->width; r++) {
-      *buf++ = (bits & m) ? fg : bg;
-      m >>= 1;
-
-      if (m == 0) {
-        bits = *bitmap++;
-        m = 0x80;
-      }
-    }
-  }
-    ili9341_bulk(x, y, font->width, font->height);
-}
-
 #if 0
-const uint16_t colormap[] = {
-  RGB565(255,0,0), RGB565(0,255,0), RGB565(0,0,255),
-  RGB565(255,255,0), RGB565(0,255,255), RGB565(255,0,255)
+static const uint16_t colormap[] = {
+  RGBHEX(0x00ff00), RGBHEX(0x0000ff), RGBHEX(0xff0000),
+  RGBHEX(0x00ffff), RGBHEX(0xff00ff), RGBHEX(0xffff00)
 };

-void
-ili9341_test(int mode)
+void ili9341_test(int mode)
 {
  int x, y;
  int i;
@ -476,7 +695,7 @@ ili9341_test(int mode)
 #if 1
    ili9341_fill(0, 0, 320, 240, 0);
    for (y = 0; y < 240; y++) {
-      ili9341_fill(0, y, 320, 1, RGB565(y, (y + 120) % 256, 240-y));
+      ili9341_fill(0, y, 320, 1, RGB(240-y, y, (y + 120) % 256));
    }
    break;
    case 1:
@ -495,7 +714,7 @@ ili9341_test(int mode)
 #if 1
    case 3:
      for (i = 0; i < 10; i++)
-      ili9341_drawfont(i, &NF20x22, i*20, 120, colormap[i%6], 0x0000);
+        ili9341_drawfont(i, i*20, 120);
    break;
 #endif
 #if 0
@ -504,10 +723,10 @@ ili9341_test(int mode)
    break;
 #endif
    case 4:
-    ili9341_line(0, 0, 15, 100, 0xffff);
-    ili9341_line(0, 0, 100, 100, 0xffff);
-    ili9341_line(0, 15, 100, 0, 0xffff);
-    ili9341_line(0, 100, 100, 0, 0xffff);
+      ili9341_line(0, 0, 15, 100);
+      ili9341_line(0, 0, 100, 100);
+      ili9341_line(0, 15, 100, 0);
+      ili9341_line(0, 100, 100, 0);
    break;
  }
 }
--- a/main.c
+++ b/main.c
--- a/mcuconf.h
+++ b/mcuconf.h
@ -58,7 +58,8 @@
 #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_HSI
+#define STM32_I2C1SW                        STM32_I2C1SW_SYSCLK
 #define STM32_USART1SW                      STM32_USART1SW_PCLK
 #define STM32_RTCSEL                        STM32_RTCSEL_LSI

@ -117,7 +118,9 @@
 #define STM32_I2C_BUSY_TIMEOUT              50
 #define STM32_I2C_I2C1_IRQ_PRIORITY         3
 #define STM32_I2C_I2C2_IRQ_PRIORITY         3
-#define STM32_I2C_USE_DMA                   TRUE
+
+// I2C1 rx operation use DMA3, some as SPI1 DMA Tx used by LCD
+#define STM32_I2C_USE_DMA                   FALSE
 #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,11 +19,26 @@
 */
 #include "ch.h"

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

-extern float measured[2][101][2];
+// 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];

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

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

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

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

 void toggle_sweep(void);
+void load_default_properties(void);

-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;
+#define SWEEP_ENABLE  0x01
+#define SWEEP_ONCE    0x02
+extern int8_t sweep_mode;
+extern const char *info_about[];

 /*
 * dsp.c
@ -111,9 +122,6 @@ 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
 */
@ -122,42 +130,74 @@ 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
+
+// Offset of plot area
+#define OFFSETX 10
 #define OFFSETY  0
-#define WIDTH 291
-#define HEIGHT 233

+// 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 (WIDTH + CELLOFFSETX*2)
+#define AREA_WIDTH_NORMAL  (CELLOFFSETX + WIDTH  + 1 + 4)
+#define AREA_HEIGHT_NORMAL (              HEIGHT + 1)

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

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

 // font
-
 extern const uint8_t x5x7_bits [];
-extern const uint8_t numfont20x22[][22 * 3];
+#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 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

-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
+#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
 };
+// 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
@ -169,16 +209,20 @@ enum {
 // Electrical Delay
 // Phase

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

-typedef struct {
+#define FREQ_MODE_START_STOP    0x0
+#define FREQ_MODE_CENTER_SPAN   0x1
+#define FREQ_MODE_DOTTED_GRID   0x2
+
+typedef struct config {
  int32_t magic;
  uint16_t dac_value;
  uint16_t grid_color;
@ -186,15 +230,14 @@ typedef struct {
  uint16_t menu_active_color;
  uint16_t trace_color[TRACES_MAX];
  int16_t  touch_cal[4];
-  int8_t default_loadcal;
  uint32_t harmonic_freq_threshold;
-  int32_t checksum;
+  uint16_t vbat_offset;
+  uint8_t _reserved[22];
+  uint32_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);
@ -202,21 +245,23 @@ 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

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

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

 void plot_init(void);
 void update_grid(void);
@ -225,96 +270,128 @@ 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, int update_info);
-void trace_get_info(int t, char *buf, int len);
-void plot_into_index(float measured[2][101][2]);
+void redraw_marker(int marker);
+void plot_into_index(float measured[2][POINTS_COUNT][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);
-int marker_search(int mode);
+void set_marker_search(int mode);
+int marker_search(void);
 int marker_search_left(int from);
 int marker_search_right(int from);

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

 /*
 * ili9341.c
 */
-#define RGB565(b,r,g)     ( (((b)<<8)&0xfc00) | (((r)<<2)&0x03e0) | (((g)>>3)&0x001f) )
+// 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) )

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

-extern const font_t NF20x22;
+#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 uint16_t spi_buffer[1024];
+extern uint16_t foreground_color;
+extern uint16_t background_color;
+
+extern uint16_t spi_buffer[SPI_BUFFER_SIZE];

 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_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_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_read_memory(int x, int y, int w, int h, int len, uint16_t* out);
-void ili9341_read_memory_continue(int len, uint16_t* out);
-
+void ili9341_line(int x0, int y0, int x1, int y1);
+void show_version(void);
+void show_logo(void);

 /*
 * 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 {
-  int32_t magic;
-  int32_t _frequency0; // start or center
-  int32_t _frequency1; // stop or span
-  int16_t _sweep_points;
+typedef struct properties {
+  uint32_t magic;
+  uint32_t _frequency0;
+  uint32_t _frequency1;
+  uint16_t _sweep_points;
  uint16_t _cal_status;

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

-  int32_t checksum;
+  float _velocity_factor; // %
+  int8_t _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;

+//sizeof(properties_t) == 0x1200
+
 #define CONFIG_MAGIC 0x434f4e45 /* 'CONF' */

 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
@ -328,6 +405,13 @@ extern int8_t previous_marker;
 #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);
@ -341,29 +425,42 @@ 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 {
-  LM_MARKER, LM_SEARCH, LM_CENTER, LM_SPAN
+enum lever_mode {
+  LM_MARKER, LM_SEARCH, LM_CENTER, LM_SPAN, LM_EDELAY
 };

-typedef struct {
+// marker smith value format
+enum marker_smithvalue {
+  MS_LIN, MS_LOG, MS_REIM, MS_RX, MS_RLC
+};
+
+typedef struct uistat {
  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);
@ -379,25 +476,22 @@ void enter_dfu(void);
 */

 void adc_init(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);
+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);

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

-// 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;
-}
-
+// 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)
 /*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,5 +1,6 @@
 /*
 * 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
@ -21,40 +22,100 @@
 #include "nanovna.h"
 #include "si5351.h"

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

-static void
-si5351_write(uint8_t reg, uint8_t dat)
+// 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)
 {
-  int addr = SI5351_I2C_ADDR>>1;
-  uint8_t buf[] = { reg, dat };
-  i2cAcquireBus(&I2CD1);
-  (void)i2cMasterTransmitTimeout(&I2CD1, addr, buf, 2, NULL, 0, 1000);
-  i2cReleaseBus(&I2CD1);
+  return current_freq;
+}
+
+void si5351_set_frequency_offset(int32_t offset)
+{
+  current_offset = offset;
+  current_freq = 0; // reset freq, for
 }

 static void
 si5351_bulk_write(const uint8_t *buf, int len)
 {
-  int addr = SI5351_I2C_ADDR>>1;
  i2cAcquireBus(&I2CD1);
-  (void)i2cMasterTransmitTimeout(&I2CD1, addr, buf, len, NULL, 0, 1000);
+  (void)i2cMasterTransmitTimeout(&I2CD1, SI5351_I2C_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_26_PLL_A, /*P3*/0, 1, /*P1*/0, 14, 0, /*P3/P2*/0, 0, 0,
+  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,
  // RESET PLL
-  2, SI5351_REG_177_PLL_RESET, SI5351_PLL_RESET_A | SI5351_PLL_RESET_B,
+  2, SI5351_REG_177_PLL_RESET, SI5351_PLL_RESET_A | SI5351_PLL_RESET_B | 0x0C, //
  // 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,
-  2, SI5351_REG_3_OUTPUT_ENABLE_CONTROL, 0,
+#endif
+  2, SI5351_REG_3_OUTPUT_ENABLE_CONTROL, ~(SI5351_CLK0_EN|SI5351_CLK1_EN|SI5351_CLK2_EN),
  0 // sentinel
 };

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

-void si5351_disable_output(void)
-{
-  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, 0x00);
-}
-
-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
+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
 };
-  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 + floor(128*(num/denom)) - 512
-   * P2 register is a 20-bit value using the following formula:
-   * 	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
-   */
-
-  /* Set the main PLL config registers */
-  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;
-  }
-
-  /* The datasheet is a nightmare of typos and inconsistencies here! */
-  uint8_t reg[9];
-  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);
-}
-
-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[] = {
+static const uint8_t msreg_base[] = {
  SI5351_REG_42_MULTISYNTH0,
  SI5351_REG_50_MULTISYNTH1,
  SI5351_REG_58_MULTISYNTH2,
 };
-  const uint8_t clkctrl[] = {
+static 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;
+// 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;
+}
+
+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;
+}
+
+// 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)
+{
+  /* 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
+   * P2 register is a 20-bit value using the following formula:
+   *    P2[19:0] = (128 * num) % denom
+   * P3 register is a 20-bit value using the following formula:
+   *    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;
+    P3 = denom;
+  }
+  // Pll MSN(A|B) registers Datasheet
+  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]
+  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
+{
  /* 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 + floor(128*(b/c)) - 512
+   *   P1[17:0] = 128 * a + int((128*b)/c) - 512
   * P2 register is a 20-bit value using the following formula:
-   * 	P2[19:0] = 128 * b - c * floor(128*(b/c))
+   *   P2[19:0] = (128 * b) % c
   * P3 register is a 20-bit value using the following formula:
   *   P3[19:0] = c
   */
  /* Set the main PLL config registers */
-  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);
+  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;
    }
-
+  }
  /* Set the MSx config registers */
  uint8_t reg[9];
-  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);
+  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]
  si5351_bulk_write(reg, 9);

  /* Configure the clk control and enable the output */
-  dat = drive_strength | SI5351_CLK_INPUT_MULTISYNTH_N;
-  if (pllSource == SI5351_PLL_B)
-    dat |= SI5351_CLK_PLL_SELECT_B;
+  uint8_t dat = chctrl | SI5351_CLK_INPUT_MULTISYNTH_N;
  if (num == 0)
    dat |= SI5351_CLK_INTEGER_MODE;
-  si5351_write(clkctrl[output], dat);
+
+#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
 }

-static uint32_t
-gcd(uint32_t x, uint32_t y)
+// Find better approximate values for n/d
+#define MAX_DENOMINATOR ((1 << 20) - 1)
+static inline void approximate_fraction(uint32_t *n, uint32_t *d)
 {
-  uint32_t z;
-  while (y != 0) {
-    z = x % y;
-    x = y;
-    y = z;
+  // 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;
+    }
+    *n = p1;
+    *d = q1;
  }
-  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)
+// 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)
 {
-    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;
+  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);
 }
-    si5351_setupMultisynth(channel, pll, div, num, denom, rdiv, drive_strength);
+
+// 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);
 }

 void
-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)
+si5351_set_frequency(int channel, uint32_t 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);
+    si5351_set_frequency_fixedpll(channel, SI5351_PLL_B, PLLFREQ, freq, SI5351_R_DIV_1, drive_strength, 1);
  } else if (freq < 150000000) {
-    si5351_set_frequency_fixeddiv(channel, SI5351_PLL_B, freq, 6, drive_strength);
+    si5351_set_frequency_fixeddiv(channel, SI5351_PLL_B, freq, 6, drive_strength, 1);
  } else {
-    si5351_set_frequency_fixeddiv(channel, SI5351_PLL_B, freq, 4, drive_strength);
+    si5351_set_frequency_fixeddiv(channel, SI5351_PLL_B, freq, 4, drive_strength, 1);
  }
 }
-
-
-int current_band = -1;
-
-#define DELAY_NORMAL 3
-#define DELAY_BANDCHANGE 1
-#define DELAY_LOWBAND 1
+#endif

 /*
+ * 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_with_offset(uint32_t freq, int offset, uint8_t drive_strength)
+si5351_set_frequency(uint32_t freq, uint8_t drive_strength)
 {
-  int band;
+  uint8_t band;
  int delay = DELAY_NORMAL;
-  uint32_t ofreq = freq + offset;
+  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 rdiv = SI5351_R_DIV_1;
-  if (freq >= config.harmonic_freq_threshold * 3) {
-    freq /= 5;
-    ofreq /= 7;
+  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;
  } else if (freq >= config.harmonic_freq_threshold) {
-    freq /= 3;
-    ofreq /= 5;
-  }
-  if (freq <= 100000000) {
-    band = 0;
-  } else if (freq < 150000000) {
-    band = 1;
-  } else {
-    band = 2;
-  }
-  if (freq <= 500000) {
+     mul = 3;
+    omul = 5;
+  } else if (freq <= 500000U) {
    rdiv = SI5351_R_DIV_64;
-  } else if (freq <= 4000000) {
+     freq<<= 6;
+    ofreq<<= 6;
+  } else if (freq <= 4000000U) {
    rdiv = SI5351_R_DIV_8;
+     freq<<= 3;
+    ofreq<<= 3;
  }
-
-#if 1
-  if (current_band != band)
-    si5351_disable_output();
-#endif
-
+  band = si5351_get_band(freq / mul);
  switch (band) {
-  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);
+      // 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;
      }
-
-    // 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);
+      // 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:
-    // 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;
-  }
-
+    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_reset_pll();
-#if 1
-    si5351_enable_output();
-#endif
-    delay += DELAY_BANDCHANGE;
+        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;
      }
-  if (band == 0)
-    delay += DELAY_LOWBAND;
-
+      // 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;
+  }
+  if (current_band != band) {
+    si5351_reset_pll(SI5351_PLL_RESET_A|SI5351_PLL_RESET_B);
    current_band = band;
+  }
  return delay;
 }
--- a/si5351.h
+++ b/si5351.h
@ -17,12 +17,39 @@
 * 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_MULTISYNTH_DIV_4  4
-#define SI5351_MULTISYNTH_DIV_6  6
-#define SI5351_MULTISYNTH_DIV_8  8
+#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)
+
+// 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_PLL_A            26
+#define SI5351_REG_PLL_B            34
+
+#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)
@ -31,35 +58,6 @@
 #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)
-
-#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_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_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_177_PLL_RESET    177
 #define SI5351_PLL_RESET_B          (1<<7)
@ -70,20 +68,10 @@
 #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_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);
+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);
--- a/tlv320aic3204.c
+++ b/tlv320aic3204.c
@ -25,74 +25,78 @@

 #define wait_ms(ms)     chThdSleepMilliseconds(ms)

-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 */
+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 */
 #ifdef REFCLK_8000KHZ
  /* 8.000MHz*10.7520 = 86.016MHz, 86.016MHz/(2*7*128) = 48kHz */
-  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,
+  0x05, 0x91, /* Power up PLL, P=1,R=1 */
+  0x06, 0x0a, /* J=10 */
+  0x07, 29,   /* D=7520 = (29<<8) + 96 */
+  0x08, 96,
 #endif
-  0 // sentinel
+// 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 */
 };

-// 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_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 */
 };

-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_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 */
 };

-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 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 */
 };

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

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

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

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

 void tlv320aic3204_select(int channel)
 {
-    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);
+  tlv320aic3204_config(channel ? conf_data_ch1_select : conf_data_ch3_select, sizeof(conf_data_ch3_select)/2);
 }

 void tlv320aic3204_set_gain(int lgain, int rgain)
 {
  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
+    0x00, 0x01, /* Select Page 1 */
+    0x3b, lgain, /* Unmute Left MICPGA, set gain */
+    0x3c, rgain, /* Unmute Right MICPGA, set gain */
  };
-    tlv320aic3204_config(data);
+  tlv320aic3204_config(data, sizeof(data)/2);
 }
--- a/ui.c
+++ b/ui.c