From 8842493f31b8b2a452f9bc04ea35ffde30e74be9 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Kuba=20Szczodrzy=C5=84ski?= <kuba@szczodrzynski.pl>
Date: Fri, 22 Apr 2022 19:39:55 +0200
Subject: [PATCH] Fix ambz-arduino compilation errors

---
 arduino/realtek-ambz/cores/arduino/Arduino.h  |   5 -
 .../cores/arduino/LOGUARTClass.cpp            |  47 ++--
 .../realtek-ambz/cores/arduino/LOGUARTClass.h |   5 -
 .../cores/arduino/PowerManagement.cpp         |   6 +-
 .../realtek-ambz/cores/arduino/bitband_io.c   |  62 -----
 .../realtek-ambz/cores/arduino/bitband_io.h   | 161 ------------
 arduino/realtek-ambz/cores/arduino/main.cpp   |  27 +-
 .../realtek-ambz/cores/arduino/rtl_sys.cpp    |  14 +-
 .../realtek-ambz/cores/arduino/uvc/uvc_drv.c  | 241 ------------------
 .../realtek-ambz/cores/arduino/uvc/uvc_drv.h  |  53 ----
 .../cores/arduino/wiring_analog.c             |  63 ++---
 .../cores/arduino/wiring_digital.c            |   3 +-
 12 files changed, 50 insertions(+), 637 deletions(-)
 delete mode 100644 arduino/realtek-ambz/cores/arduino/bitband_io.c
 delete mode 100644 arduino/realtek-ambz/cores/arduino/bitband_io.h
 delete mode 100644 arduino/realtek-ambz/cores/arduino/uvc/uvc_drv.c
 delete mode 100644 arduino/realtek-ambz/cores/arduino/uvc/uvc_drv.h

diff --git a/arduino/realtek-ambz/cores/arduino/Arduino.h b/arduino/realtek-ambz/cores/arduino/Arduino.h
index 8dd13e9..a44358f 100644
--- a/arduino/realtek-ambz/cores/arduino/Arduino.h
+++ b/arduino/realtek-ambz/cores/arduino/Arduino.h
@@ -57,11 +57,6 @@ extern void wait_us(int us);
 
 extern void yield(void);
 
-extern uint32_t DiagPrintf(const char *fmt, ...);
-
-extern int rtl_printf(const char *fmt, ...);
-extern int rtl_sprintf(char* str, const char* fmt, ...);
-
 #ifndef printf
 #define printf		rtl_printf
 #endif
diff --git a/arduino/realtek-ambz/cores/arduino/LOGUARTClass.cpp b/arduino/realtek-ambz/cores/arduino/LOGUARTClass.cpp
index ef4204f..a7ce8bb 100644
--- a/arduino/realtek-ambz/cores/arduino/LOGUARTClass.cpp
+++ b/arduino/realtek-ambz/cores/arduino/LOGUARTClass.cpp
@@ -29,10 +29,9 @@
 extern "C" {
 #endif
 
-#include "log_uart_api.h"
-#include "hal_irqn.h"
-
-log_uart_t log_uart_obj;
+#include "osdep_service.h"
+#include "rtl8710b.h"
+extern int LOGUART_SetBaud(uint32_t BaudRate); // from fixups/log_uart.c
 
 #ifdef __cplusplus
 }
@@ -40,17 +39,6 @@ log_uart_t log_uart_obj;
 
 RingBuffer rx_buffer0;
 
-void arduino_loguart_irq_handler(uint32_t id, LOG_UART_INT_ID event)
-{
-    char c;
-    RingBuffer *pRxBuffer = (RingBuffer *)id;
-
-    if (event == IIR_RX_RDY || IIR_CHAR_TIMEOUT) {
-        c = log_uart_getc(&log_uart_obj);
-        pRxBuffer->store_char(c);
-    }
-}
-
 LOGUARTClass::LOGUARTClass(int dwIrq, RingBuffer* pRx_buffer )
 {
     _rx_buffer = pRx_buffer;
@@ -65,7 +53,7 @@ LOGUARTClass::LOGUARTClass(int dwIrq, RingBuffer* pRx_buffer )
 // Public Methods //////////////////////////////////////////////////////////////
 
 
-void LOGUARTClass::IrqHandler( void )
+void IrqHandler( void )
 {
 
     uint8_t     data = 0;
@@ -75,8 +63,8 @@ void LOGUARTClass::IrqHandler( void )
     DiagSetIsrEnReg(0);
 
     data = DiagGetChar(PullMode);
-	if ( data > 0 ) 
-		_rx_buffer->store_char(data);
+	if ( data > 0 )
+		rx_buffer0.store_char(data);
 
     DiagSetIsrEnReg(IrqEn);
 
@@ -85,24 +73,19 @@ void LOGUARTClass::IrqHandler( void )
 
 void LOGUARTClass::begin( const uint32_t dwBaudRate )
 {
-#if LOG_UART_MODIFIABLE_BAUD_RATE
-    /* log uart initialize in 38400 baud rate.
-     * If we change baud rate here, Serail Monitor would not detect this change and show nothing on screen.
-     */
-    log_uart_init(&log_uart_obj, dwBaudRate, 8, ParityNone, 1);
-#else
-    log_uart_init(&log_uart_obj, 38400, 8, ParityNone, 1);
-#endif
-    log_uart_irq_set(&log_uart_obj, IIR_RX_RDY, 1);
-    log_uart_irq_handler(&log_uart_obj, arduino_loguart_irq_handler, (uint32_t)_rx_buffer);
+    DiagPrintf("LOGUARTClass::begin\r\n");
+    DIAG_UartReInit((IRQ_FUN) IrqHandler);
+    DiagPrintf("DIAG_UartReInit ok\r\n");
+    NVIC_SetPriority(UART_LOG_IRQ, 10);
+    DiagPrintf("NVIC_SetPriority ok\r\n");
+    LOGUART_SetBaud(dwBaudRate);
+    DiagPrintf("LOGUART_SetBaud ok\r\n");
 }
 
 void LOGUARTClass::end( void )
 {
     // clear any received data
     _rx_buffer->_iHead = _rx_buffer->_iTail ;
-
-    log_uart_free(&log_uart_obj);
 }
 
 int LOGUARTClass::available( void )
@@ -134,7 +117,7 @@ int LOGUARTClass::read( void )
 
 void LOGUARTClass::flush( void )
 {
-// TODO: 
+// TODO:
 // while ( serial_writable(&(this->sobj)) != 1 );
 /*
   // Wait for transmission to complete
@@ -145,7 +128,7 @@ void LOGUARTClass::flush( void )
 
 size_t LOGUARTClass::write( const uint8_t uc_data )
 {
-    log_uart_putc(&log_uart_obj, uc_data);
+    DiagPutChar(uc_data);
   	return 1;
 }
 
diff --git a/arduino/realtek-ambz/cores/arduino/LOGUARTClass.h b/arduino/realtek-ambz/cores/arduino/LOGUARTClass.h
index 02d56a7..7c03aef 100644
--- a/arduino/realtek-ambz/cores/arduino/LOGUARTClass.h
+++ b/arduino/realtek-ambz/cores/arduino/LOGUARTClass.h
@@ -35,16 +35,11 @@ class LOGUARTClass : public HardwareSerial
     void flush(void);
     size_t write(const uint8_t c);
 
-
-    void IrqHandler(void);
-
     using Print::write; // pull in write(str) and write(buf, size) from Print
 
     operator bool() { return true; }; // UART always active
 
   protected:
-    void init(const uint32_t dwBaudRate, const uint32_t config);
-
     RingBuffer *_rx_buffer;
 
     int _dwIrq;
diff --git a/arduino/realtek-ambz/cores/arduino/PowerManagement.cpp b/arduino/realtek-ambz/cores/arduino/PowerManagement.cpp
index c2abe0c..e98d497 100644
--- a/arduino/realtek-ambz/cores/arduino/PowerManagement.cpp
+++ b/arduino/realtek-ambz/cores/arduino/PowerManagement.cpp
@@ -5,6 +5,7 @@ extern "C" {
 
 #include "freertos_pmu.h"
 #include "sys_api.h"
+#include "sleep_ex_api.h"
 
 #include "wiring_digital.h"
 
@@ -23,10 +24,6 @@ extern "C" {
 
 bool PowerManagementClass::reservePLL = true;
 
-void PowerManagementClass::setPllReserved(bool reserve) {
-    pmu_set_pll_reserved(reserve);
-}
-
 void PowerManagementClass::sleep(uint32_t bitflg) {
     if (!safeLock()) {
         pmu_release_wakelock(bitflg);
@@ -63,4 +60,3 @@ void PowerManagementClass::softReset() {
 }
 
 PowerManagementClass PowerManagement;
-
diff --git a/arduino/realtek-ambz/cores/arduino/bitband_io.c b/arduino/realtek-ambz/cores/arduino/bitband_io.c
deleted file mode 100644
index 9aaecdb..0000000
--- a/arduino/realtek-ambz/cores/arduino/bitband_io.c
+++ /dev/null
@@ -1,62 +0,0 @@
-
-#include "bitband_io.h"
-
-volatile uint8_t * BitBandAddr(void *addr, uint8_t bit) {
-	return (volatile uint8_t *)(BITBAND_ADDR((u32)addr, bit));
-}
-
-volatile uint8_t * BitBandPeriAddr(void *addr, uint8_t bit) {
-	return (volatile uint8_t *)(BITBAND_PERI((u32)addr, bit));
-}
-
-volatile uint8_t * GetOutPinBitBandAddr(PinName pin) {
-	uint32_t paddr = NULL;
-	uint32_t ippin = HAL_GPIO_GetIPPinName_8195a(pin);
-	if(ippin < 0xff) {
-		// paddr = 0x42000000 + (0x40001000 + 0x0c * (ippin >> 5) - 0x40000000) * 32 + ((ippin & 0x1f) * 4);
-		paddr =  BitBandPeriAddr((void *)(GPIO_REG_BASE + GPIO_PORTB_DR * (ippin >> 5)),  ippin & 0x1f);
-	}
-	return paddr;
-}
-
-volatile uint8_t * GetInPinBitBandAddr(PinName pin) {
-	volatile uint8_t * paddr = NULL;
-	uint32_t ippin = HAL_GPIO_GetIPPinName_8195a(pin);
-	if(ippin < 0xff) {
-		// paddr = 0x42000000 + (0x40001000 + 0x0c * (ippin >> 5) - 0x40000000) * 32 + ((ippin & 0x1f) * 4);
-		paddr = BitBandPeriAddr((void *)(GPIO_REG_BASE + GPIO_EXT_PORTA + (ippin >> 5) * 4),  ippin & 0x1f);
-	}
-	return paddr;
-}
-
-volatile uint8_t * HardSetPin(PinName pin, HAL_GPIO_PIN_MODE pmode, uint8_t val)
-{
-	volatile uint8_t *paddr = NULL;
-	uint32_t ippin = HAL_GPIO_GetIPPinName_8195a(pin);
-	if(ippin < 0xff) {
-		if(_pHAL_Gpio_Adapter == NULL) {
-			extern HAL_GPIO_ADAPTER gBoot_Gpio_Adapter;
-			_pHAL_Gpio_Adapter = &gBoot_Gpio_Adapter;
-		}
-		if(_pHAL_Gpio_Adapter->Gpio_Func_En == 0) GPIO_FuncOn_8195a();
-		wait_us(100);
-		// delayMicroseconds(100);
-		// paddr = 0x42000000 + (0x40001000 + 0x0c * (ippin >> 5) - 0x40000000) * 32 + ((ippin & 0x1f) * 4);
-#if CONFIG_DEBUG_LOG > 3		
-		GpioFunctionChk(ippin, ENABLE);
-#endif		
-    	GPIO_PullCtrl_8195a(ippin, HAL_GPIO_HIGHZ); // Make the pin pull control default as High-Z
-		paddr = BitBandPeriAddr((void *)(GPIO_REG_BASE + GPIO_PORTB_DR * (ippin >> 5)),  ippin & 0x1f);
-		*paddr = val;						// data register
-		HAL_GPIO_PIN gpio;
-		gpio.pin_name = ippin;
-		gpio.pin_mode = pmode;
-		HAL_GPIO_Init_8195a(&gpio);
-		*paddr = val;						// data register
-//		paddr[(GPIO_PORTB_DDR - GPIO_PORTB_DR) * 32] = pmode == DOUT_PUSH_PULL;	// data direction
-//		GPIO_PullCtrl_8195a(ippin, pmode);					// set GPIO_PULL_CTRLx
-//		paddr[(GPIO_PORTB_CTRL - GPIO_PORTB_DR) * 32] = 0;		// data source control, we should keep it as default: data source from software
-	}
-	return paddr;
-}
-
diff --git a/arduino/realtek-ambz/cores/arduino/bitband_io.h b/arduino/realtek-ambz/cores/arduino/bitband_io.h
deleted file mode 100644
index 57c34ef..0000000
--- a/arduino/realtek-ambz/cores/arduino/bitband_io.h
+++ /dev/null
@@ -1,161 +0,0 @@
-#ifndef _BITBAND_IO_H_
-#define _BITBAND_IO_H_
-
-#ifdef __cplusplus
-extern "C"{
-#endif // __cplusplus
-
-#include "PinNames.h"
-#include "hal_platform.h"
-#include "hal_api.h"
-#include "hal_gpio.h"
-#include "rtl8195a_gpio.h"
-
-#define BITBAND_SRAM_REF  0x10000000
-#define BITBAND_SRAM_BASE 0x12000000
-#define BITBAND_SRAM(a,b) (BITBAND_SRAM_BASE + (a-BITBAND_SRAM_REF)*32 + (b*4)) // Convert SRAM address
-
-/*
- * in hal_platform.h
-#define BITBAND_REG_BASE		0x40001000
- */
-
-/*
- * in rtl8195a_gpio.h
- *
-#define BITBAND_PORTA_DR           0x00        // data register
-#define BITBAND_PORTA_DDR          0x04        // data direction
-#define BITBAND_PORTA_CTRL         0x08        // data source control, we should keep it as default: data source from software
-
-#define BITBAND_PORTB_DR           0x0c        // data register
-#define BITBAND_PORTB_DDR          0x10        // data direction
-#define BITBAND_PORTB_CTRL         0x14        // data source control, we should keep it as default: data source from software
-
-#define BITBAND_PORTC_DR           0x18        // data register
-#define BITBAND_PORTC_DDR          0x1c        // data direction
-#define BITBAND_PORTC_CTRL         0x20        // data source control, we should keep it as default: data source from software
-
-#define BITBAND_EXT_PORTA          0x50        // GPIO IN read or OUT read back
-#define BITBAND_EXT_PORTB          0x54        // GPIO IN read or OUT read back
-#define BITBAND_EXT_PORTC          0x58        // GPIO IN read or OUT read back
-*/
-
-#define BITBAND_PERI_REF	0x40000000
-#define BITBAND_PERI_BASE	0x42000000
-#define BITBAND_PERI(a,b)	(BITBAND_PERI_BASE + (a-BITBAND_PERI_REF)*32 + (b*4))	// Convert PERI address
-#define ucBITBAND_PERI(a,b)	*((volatile unsigned char *)BITBAND_PERI(a,b))
-#define uiBITBAND_PERI(a,b) *((volatile unsigned int *)BITBAND_PERI(a,b))
-
-#define BITBAND_A0 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTA_DR,0)	//Port = 0, bit = 0, A0
-#define BITBAND_A1 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTA_DR,1)	//Port = 0, bit = 1, A1
-#define BITBAND_A2 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTB_DR,0)	//Port = 1, bit = 0, A2
-#define BITBAND_A3 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTB_DR,1)	//Port = 1, bit = 1, A3
-#define BITBAND_A4 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTB_DR,2)	//Port = 1, bit = 2, A4
-#define BITBAND_A5 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTB_DR,3)	//Port = 1, bit = 3, A5
-#define BITBAND_A6 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTB_DR,4)	//Port = 1, bit = 4, A6
-#define BITBAND_A7 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTB_DR,5)	//Port = 1, bit = 5, A7
-
-#define BITBAND_B0 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTB_DR,6)	//Port = 1, bit = 6, B0
-#define BITBAND_B1 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTB_DR,7)	//Port = 1, bit = 7, B1
-#define BITBAND_B2 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTB_DR,8)	//Port = 1, bit = 8, B2
-#define BITBAND_B3 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTA_DR,2)	//Port = 0, bit = 2, B3
-#define BITBAND_B4 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTA_DR,3)	//Port = 0, bit = 3, B4
-#define BITBAND_B5 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTB_DR,9)	//Port = 1, bit = 9, B5
-#define BITBAND_B6 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTA_DR,4)	//Port = 0, bit = 4, B6
-#define BITBAND_B7 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTA_DR,5)	//Port = 0, bit = 5, B7
-
-#define BITBAND_C0 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTB_DR,10)	//Port = 1, bit = 10, C0
-#define BITBAND_C1 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTA_DR,6)	//Port = 0, bit = 6,  C1
-#define BITBAND_C2 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTB_DR,11)	//Port = 1, bit = 11, C2
-#define BITBAND_C3 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTA_DR,7)	//Port = 0, bit = 7,  C3
-#define BITBAND_C4 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTA_DR,8)	//Port = 0, bit = 8,  C4
-#define BITBAND_C5 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTA_DR,9)	//Port = 0, bit = 9,  C5
-#define BITBAND_C6 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTA_DR,10)	//Port = 0, bit = 10, C6
-#define BITBAND_C7 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTA_DR,11)	//Port = 0, bit = 11, C7
-#define BITBAND_C8 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTA_DR,12)	//Port = 0, bit = 12, C8
-#define BITBAND_C9 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTA_DR,13)	//Port = 0, bit = 13, C9
-
-#define BITBAND_D0 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTB_DR,12)  //Port = 1, bit = 12, D0
-#define BITBAND_D1 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTA_DR,14)  //Port = 0, bit = 14, D1
-#define BITBAND_D2 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTB_DR,13)  //Port = 1, bit = 13, D2
-#define BITBAND_D3 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTA_DR,15)  //Port = 0, bit = 15, D3
-#define BITBAND_D4 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTA_DR,16)  //Port = 0, bit = 16, D4
-#define BITBAND_D5 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTA_DR,17)  //Port = 0, bit = 17, D5
-#define BITBAND_D6 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTA_DR,18)  //Port = 0, bit = 18, D6
-#define BITBAND_D7 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTA_DR,19)  //Port = 0, bit = 19, D7
-#define BITBAND_D8 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTB_DR,14)  //Port = 1, bit = 14, D8
-#define BITBAND_D9 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTA_DR,20)  //Port = 0, bit = 20, D9
-
-#define BITBAND_E0 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTC_DR,15)  //Port = 2, bit = 15, E0
-#define BITBAND_E1 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTA_DR,21)  //Port = 0, bit = 21, E1
-#define BITBAND_E2 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTA_DR,22)  //Port = 0, bit = 22, E2
-#define BITBAND_E3 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTA_DR,23)  //Port = 0, bit = 23, E3
-#define BITBAND_E4 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTB_DR,16)  //Port = 1, bit = 16, E4
-#define BITBAND_E5 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTA_DR,24)  //Port = 0, bit = 24, E5
-#define BITBAND_E6 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTA_DR,25)  //Port = 0, bit = 25, E6
-#define BITBAND_E7 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTA_DR,26)  //Port = 0, bit = 26, E7
-#define BITBAND_E8 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTA_DR,27)  //Port = 0, bit = 27, E8
-#define BITBAND_E9 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTB_DR,17)  //Port = 1, bit = 17, E9
-#define BITBAND_E10 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTB_DR,18) //Port = 1, bit = 17, E10
-
-#define BITBAND_F0 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTB_DR,19)  //Port = 1, bit = 19, F0
-#define BITBAND_F1 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTB_DR,20)  //Port = 1, bit = 20, F1
-#define BITBAND_F2 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTB_DR,21)  //Port = 1, bit = 21, F2
-#define BITBAND_F3 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTB_DR,22)  //Port = 1, bit = 22, F3
-#define BITBAND_F4 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTB_DR,23)  //Port = 1, bit = 23, F4
-#define BITBAND_F5 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTB_DR,24)  //Port = 1, bit = 24, F5
-
-#define BITBAND_G0 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTB_DR,25)  //Port = 1, bit = 25, G0
-#define BITBAND_G1 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTB_DR,26)  //Port = 1, bit = 26, G1
-#define BITBAND_G2 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTB_DR,27)  //Port = 1, bit = 27, G2
-#define BITBAND_G3 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTA_DR,28)  //Port = 0, bit = 28, G3
-#define BITBAND_G4 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTB_DR,28)  //Port = 1, bit = 28, G4
-#define BITBAND_G5 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTB_DR,29)  //Port = 1, bit = 29, G5
-#define BITBAND_G6 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTB_DR,30)  //Port = 1, bit = 30, G6
-#define BITBAND_G7 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTB_DR,31)  //Port = 1, bit = 31, G7
-
-#define BITBAND_H0 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTC_DR,0)   //Port = 2, bit = 0,  H0
-#define BITBAND_H1 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTA_DR,29)  //Port = 0, bit = 29, H1
-#define BITBAND_H2 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTC_DR,1)   //Port = 2, bit = 1,  H2
-#define BITBAND_H3 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTA_DR,30)  //Port = 0, bit = 30, H3
-#define BITBAND_H4 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTC_DR,2)   //Port = 2, bit = 2,  H4
-#define BITBAND_H5 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTA_DR,31)  //Port = 0, bit = 31, H5
-#define BITBAND_H6 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTC_DR,3)   //Port = 2, bit = 3,  H6
-#define BITBAND_H7 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTA_DR,4)   //Port = 2, bit = 4,  H7
-
-#define BITBAND_I0 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTC_DR,5)   //Port = 2, bit = 5,  I0
-#define BITBAND_I1 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTC_DR,6)   //Port = 2, bit = 6,  I1
-#define BITBAND_I2 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTC_DR,7)   //Port = 2, bit = 7,  I2
-#define BITBAND_I3 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTC_DR,8)   //Port = 2, bit = 8,  I3
-#define BITBAND_I4 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTC_DR,9)   //Port = 2, bit = 9,  I4
-#define BITBAND_I5 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTC_DR,10)  //Port = 2, bit = 10, I5
-#define BITBAND_I6 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTC_DR,11)  //Port = 2, bit = 11, I6
-#define BITBAND_I7 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTC_DR,12)  //Port = 2, bit = 12, I7
-
-#define BITBAND_J0 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTC_DR,13)  //Port = 2, bit = 13, J0
-#define BITBAND_J1 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTC_DR,14)  //Port = 2, bit = 14, J1
-#define BITBAND_J2 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTC_DR,15)  //Port = 2, bit = 15, J2
-#define BITBAND_J3 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTC_DR,16)  //Port = 2, bit = 16, J3
-#define BITBAND_J4 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTC_DR,17)  //Port = 2, bit = 17, J4
-#define BITBAND_J5 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTC_DR,18)  //Port = 2, bit = 18, J5
-#define BITBAND_J6 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTC_DR,19)  //Port = 2, bit = 19, J6
-
-#define BITBAND_K0 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTC_DR,20)  //Port = 2, bit = 20, K0
-#define BITBAND_K1 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTC_DR,21)  //Port = 2, bit = 21, K1
-#define BITBAND_K2 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTC_DR,22)  //Port = 2, bit = 22, K2
-#define BITBAND_K3 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTC_DR,23)  //Port = 2, bit = 23, K3
-#define BITBAND_K4 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTC_DR,24)  //Port = 2, bit = 24, K4
-#define BITBAND_K5 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTC_DR,25)  //Port = 2, bit = 25, K5
-#define BITBAND_K6 ucBITBAND_PERI(GPIO_REG_BASE+GPIO_PORTC_DR,26)  //Port = 2, bit = 26, K6
-
-volatile uint8_t * BitBandAddr(void *addr, uint8_t bit);
-volatile uint8_t * BitBandPeriAddr(void *addr, uint8_t bit);
-volatile uint8_t * GetOutPinBitBandAddr(PinName pin);
-volatile uint8_t * GetInPinBitBandAddr(PinName pin);
-volatile uint8_t * HardSetPin(PinName pin, HAL_GPIO_PIN_MODE pmode, uint8_t val);
-
-#ifdef __cplusplus
-} // extern "C"
-#endif // __cplusplus
-
-#endif // _BITBAND_IO_H_
diff --git a/arduino/realtek-ambz/cores/arduino/main.cpp b/arduino/realtek-ambz/cores/arduino/main.cpp
index 4334cfb..e0e676d 100644
--- a/arduino/realtek-ambz/cores/arduino/main.cpp
+++ b/arduino/realtek-ambz/cores/arduino/main.cpp
@@ -24,6 +24,7 @@
 #ifdef __cplusplus
 extern "C" {
 #endif
+#include "rtl8710b.h"
 extern void HalCpuClkConfig(u8  CpuType);
 extern void SystemCoreClockUpdate(void);
 extern void En32KCalibration(void);
@@ -31,7 +32,6 @@ extern void En32KCalibration(void);
 #include "FreeRTOS.h"
 #include "task.h"
 #include "diag.h"
-#include "rtl8195a.h"
 extern int tcm_heap_freeSpace(void);
 extern void console_init(void);
 
@@ -65,30 +65,6 @@ void Init_Rand(void)
 	_rand_first = 1;
 }
 
-/*
- * \brief Set CPU CLK 166MHz
- * clk : 0 - 166666666 Hz, 1 - 83333333 Hz, 2 - 41666666 Hz, 3 - 20833333 Hz, 4 - 10416666 Hz, 5 - 4000000 Hz
- *       6 - 200000000 Hz, 7 - 10000000 Hz, 8 - 50000000 Hz, 9 - 25000000 Hz, 10 - 12500000 Hz, 11 - 4000000 Hz
- * baud: 38400,...
- */
-void Init_CPU_CLK_UART(int clkn, int baud)
-{
-//	if(HalGetCpuClk() < 166000000)
-		if(clkn > 5) {
-	 		HalCpuClkConfig(clkn - 6);
-	 		*((int *)0x40000074) |= (1<<17);
-		}
-		else
-		{
-	 		HalCpuClkConfig(clkn);
-	 		*((int *)0x40000074) &= (~(1<<17));
-		}
-		HAL_LOG_UART_ADAPTER pUartAdapter;
-		pUartAdapter.BaudRate = baud;
-		HalLogUartSetBaudRate(&pUartAdapter);
-		SystemCoreClockUpdate();
-		En32KCalibration();
-}
 /*
  * \brief handle sketch
  */
@@ -143,4 +119,3 @@ int main( void )
 #ifdef __cplusplus
 }
 #endif
-
diff --git a/arduino/realtek-ambz/cores/arduino/rtl_sys.cpp b/arduino/realtek-ambz/cores/arduino/rtl_sys.cpp
index 199309b..b9bbadb 100644
--- a/arduino/realtek-ambz/cores/arduino/rtl_sys.cpp
+++ b/arduino/realtek-ambz/cores/arduino/rtl_sys.cpp
@@ -79,12 +79,17 @@ void sys_info(void) {
 				HalGetCpuClk(), xPortGetFreeHeapSize(), tcm_heap_freeSpace());
 }				
 
+flash_t flashobj;
+bool fspic_isinit = false;
+
 unsigned int GetFlashSize(void)
 {
 	unsigned int FlashSize;
 	if(!fspic_isinit) flash_get_status(&flashobj);
-	if(flashobj.SpicInitPara.id[3] >= 0x14 && flashobj.SpicInitPara.id[0] <= 0x19) {
-		FlashSize = 1<<(flashobj.SpicInitPara.id[2]); // Flash size in bytes
+	fspic_isinit = true;
+	uint8_t* flash_id = *(uint8_t**)(&flashobj.SpicInitPara.FLASH_Id);
+	if(flash_id[3] >= 0x14 && flash_id[0] <= 0x19) {
+		FlashSize = 1<<(flash_id[2]); // Flash size in bytes
 	}
 	else FlashSize = 1024*1024;  // 1 mbytes
 	return FlashSize;
@@ -93,7 +98,9 @@ unsigned int GetFlashSize(void)
 unsigned int GetFlashId(void)
 {
 	if(!fspic_isinit) flash_get_status(&flashobj);
-	return (flashobj.SpicInitPara.id[0]<<16) | (flashobj.SpicInitPara.id[1]<<8) | flashobj.SpicInitPara.id[2];
+	fspic_isinit = true;
+	uint8_t* flash_id = *(uint8_t**)(&flashobj.SpicInitPara.FLASH_Id);
+	return (flash_id[0]<<16) | (flash_id[1]<<8) | flash_id[2];
 }
 
 
@@ -230,4 +237,3 @@ void __throw_out_of_range(const char* str)
 }
 
 */    
-
diff --git a/arduino/realtek-ambz/cores/arduino/uvc/uvc_drv.c b/arduino/realtek-ambz/cores/arduino/uvc/uvc_drv.c
deleted file mode 100644
index d071aee..0000000
--- a/arduino/realtek-ambz/cores/arduino/uvc/uvc_drv.c
+++ /dev/null
@@ -1,241 +0,0 @@
-/*
- * uvc_drv.c
- *
- *  Created on: 25/09/2017.
- *      Author: pvvx
- */
-
-
-#include <platform_opts.h>
-#include "rtl8195a.h"
-#include "FreeRTOS.h"
-#include "task.h"
-#include "rtl8195a/rtl_libc.h"
-#include "section_config.h"
-
-#include <platform/platform_stdlib.h>
-#include <lwip/sockets.h>
-#include "sockets.h"
-#include "lwip/netif.h"
-
-#include "cmsis_os.h"
-
-#include "freertos_pmu.h"
-
-#include "os_support.h"
-#include "timer_api.h"
-
-#include "videodev2.h"
-#include "uvcvideo.h"
-#include "v4l2_driver.h"
-#include "mjpeg/mjpeg.h"
-#include "rtsp/rtsp_api.h"
-#include "dwc_otg_driver.h"
-#include "v4l2_intf.h"
-
-#include "uvc_drv.h"
-
-//----------------------------------------------
-#undef info_printf
-#define info_printf(fmt, ...) rtl_printf(fmt, ##__VA_ARGS__)
-#undef err_printf
-#define err_printf(fmt, ...) rtl_printf(fmt, ##__VA_ARGS__)
-//----------------------------------------------
-extern struct usb_device * usb_get_dev_index(int index);
-extern void usb_hub_reset(void);
-extern void usb_stop(void);
-//----------------------------------------------
-char uvc_is_start; //
-vfrm_buf_t vfrmb; // управляющая структура
-//---start_uvc()-------------------------------------------
-int start_uvc(void) {
-	int result = -1;
-	if (uvc_is_start == 0) {
-		do {
-			info_printf("USB init...\n");
-			_usb_init();
-			result = wait_usb_ready();
-			if (result < 0) {
-				err_printf("\r\nFail to init usb driver!\n");
-				break;
-			}
-			struct usb_device * usb_dev = usb_get_dev_index(1);
-			if (usb_dev)
-				printf("USB ID %04X:%04X\n", usb_dev->descriptor.idProduct,
-						usb_dev->descriptor.idVendor);
-			info_printf("UVC stream init...\n");
-			result = uvc_stream_init();
-			if (result < 0) {
-				err_printf("Fail!\n");
-				break;
-			}
-			uvc_is_start = 1;
-		} while (0);
-	} else  if (uvc_is_start == 2) {
-		do {
-			info_printf("USB hub reset...\n");
-			usb_hub_reset(); // ?
-			result = wait_usb_ready();
-			if (result < 0) {
-				err_printf("\r\nFail to init usb driver!\n");
-				break;
-			}
-			result = 2;
-		} while(0);
-	} else 	{
-		result = 1;
-	}
-	return result;
-}
-//---stop_uvc()-------------------------------------------
-void stop_uvc(void) {
-	//TODO:  как выгрузить всё полностью? _usb_deinit() = { return; } only!
-	info_printf("USB Stop.\n");
-	usb_stop();
-	uvc_is_start = 2;
-}
-//---get_video_frame_thrd()-------------------------------------------
-void get_video_frame_thrd(void *parm)
-{
-	int ret;
-	vfrm_buf_t * pvfrm = (vfrm_buf_t *) parm;
-	struct uvc_buf_context buf;
-
-	pvfrm->run = 1;
-	if (uvc_stream_on() >= 0) {
-		while (uvc_is_stream_on() && !pvfrm->off) {
-			memset(&buf, 0, 0x10u);
-			vTaskDelay(1);
-			ret = uvc_dqbuf(&buf);
-//			DiagPrintf("dqbuf(%d)\r\n", ret);
-			if (ret < 0) {
-				if (ret == -61)
-					break;
-				uvc_stream_free();
-			}
-//			DiagPrintf("buf.index(%d)\r\n", buf.index);
-			if (buf.index == -1)
-				continue;
-			pvfrm->frame_count++;
-#if CONFIG_DEBUG_LOG > 2
-			if(pvfrm->pbuf_size != buf.len) HalSerialPutcRtl8195a('@');
-			else HalSerialPutcRtl8195a('#');
-#endif
-			if (pvfrm->get && pvfrm->pbuf) {
-				if(pvfrm->get == 1 || pvfrm->pbuf_size == buf.len) {
-					memcpy(pvfrm->pbuf, buf.data, buf.len);
-					pvfrm->frame_size = buf.len;
-					pvfrm->copy_count++;
-					pvfrm->get = 0;
-					pvfrm->ok = 1;
-//					DiagPrintf("\r\nFrameBuf[%d] at %p[%d]\r\n", buf.index, buf.data, buf.len);
-				}
-			}
-			ret = uvc_qbuf(&buf);
-//			DiagPrintf("qbuf(%d)\r\n", ret);
-			if (ret >= 0)
-				continue;
-			uvc_stream_free();
-		};
-	};
-	pvfrm->run = 0;
-#if CONFIG_DEBUG_LOG > 2
-	DiagPrintf("\r\nTask 'get-vfrm' close.\r\n");
-#endif
-	vTaskDelete(NULL);
-}
-
-//---FreeBufCameraFrame()-------------------------------------------
-void FreeBufCameraFrame(void) {
-	vfrmb.get = 0;
-	if(vfrmb.pbuf != NULL) {
-		free(vfrmb.pbuf);
-		vfrmb.pbuf = NULL;
-	}
-}
-//---StartCamera()-------------------------------------------
-unsigned int SetCameraParam(int fmt_type, int width, int height, int frame_rate, int compression_ratio) {
-	FreeBufCameraFrame();
-	switch(fmt_type) {
-		case 0:
-			vfrmb.uvc_ctx.fmt_type = V4L2_PIX_FMT_H264;
-			break;
-		case 1:
-			vfrmb.uvc_ctx.fmt_type = V4L2_PIX_FMT_MJPEG;
-			break;
-		default:
-			vfrmb.uvc_ctx.fmt_type = V4L2_PIX_FMT_YUYV;
-			break;
-	}
-	vfrmb.uvc_ctx.width = width;
-	vfrmb.uvc_ctx.height = height;
-	vfrmb.uvc_ctx.frame_rate = frame_rate;
-	vfrmb.uvc_ctx.compression_ratio = compression_ratio;
-	vfrmb.pbuf_size = vfrmb.uvc_ctx.width * vfrmb.uvc_ctx.height;
-	if(vfrmb.uvc_ctx.fmt_type == V4L2_PIX_FMT_YUYV) vfrmb.pbuf_size <<= 1;
-	return vfrmb.pbuf_size;
-}
-//---StartCamera()-------------------------------------------
-int StartCamera(void) {
-	if (vfrmb.run && vfrmb.pbuf_size) return 1;
-	vfrmb.off = 0;
-	vfrmb.run = 0;
-	vfrmb.get = 0;
-	vfrmb.ok = 0;
-	vfrmb.copy_count = 0;
-	vfrmb.frame_count = 0;
-	if (start_uvc() >= 0) {
-		info_printf("\r\nStart camera...\n");
-		if (v4l_set_param(vfrmb.uvc_ctx.fmt_type, &vfrmb.uvc_ctx.width, &vfrmb.uvc_ctx.height, &vfrmb.uvc_ctx.frame_rate, &vfrmb.uvc_ctx.compression_ratio) < 0) {
-			err_printf("Not set param!\r\n");
-			return 0;
-		}
-		if(xTaskCreate((TaskFunction_t) get_video_frame_thrd, "get-vfrm", 1024, &vfrmb, tskIDLE_PRIORITY + 2, NULL) != pdPASS) { // +4 //  + PRIORITIE_OFFSET
-			err_printf("Not start Task 'get-vfrm'!\r\n");
-			return 0;
-		}
-		vTaskDelay(2);
-	} else return 0;
-	return 1;
-}
-//---GetCameraFrame()-------------------------------------------
-unsigned int GetCameraFrame(void *pbuf) {
-	if (vfrmb.run != 0) {
-		if(pbuf == NULL) {
-			if(vfrmb.pbuf == NULL) {
-				vfrmb.pbuf = malloc(vfrmb.pbuf_size);
-				if(vfrmb.pbuf == NULL) {
-#if CONFIG_DEBUG_LOG > 2
-					err_printf("Not alloc frame buf!\r\n");
-#endif
-					return 0;
-				}
-			}
-		} else vfrmb.pbuf = (char *)pbuf;
-		TickType_t tt = xTaskGetTickCount();
-		vfrmb.get = 0;	vfrmb.ok = 0;
-		if(vfrmb.uvc_ctx.fmt_type == V4L2_PIX_FMT_YUYV) vfrmb.get = 2;
-		else vfrmb.get = 1;
-		vTaskDelay(10);
-		while(vfrmb.get) {
-#if CONFIG_DEBUG_LOG > 2
-			HalSerialPutcRtl8195a('.') ;
-#endif
-			if(xTaskGetTickCount() - tt > 5000) {
-				err_printf("USB timeout!\r\n");
-				vfrmb.off = 1;
-				return 0;
-			}
-			vTaskDelay(10);
-		}
-		if(!vfrmb.get && vfrmb.ok && vfrmb.frame_size) {
-			return vfrmb.frame_size;
-		}
-	}
-#if CONFIG_DEBUG_LOG > 2
-	else err_printf("Camera not run!\r\n");
-#endif
-	return 0;
-}
-
-
diff --git a/arduino/realtek-ambz/cores/arduino/uvc/uvc_drv.h b/arduino/realtek-ambz/cores/arduino/uvc/uvc_drv.h
deleted file mode 100644
index 31d4443..0000000
--- a/arduino/realtek-ambz/cores/arduino/uvc/uvc_drv.h
+++ /dev/null
@@ -1,53 +0,0 @@
-/*
- *  Get video frame YUYV/MGPEG (usb uvc)
- *
- *  Created on: 22/09/2017
- *      Author: pvvx
- */
-#ifndef _USB_UVC_DRV_H_
-#define _USB_UVC_DRV_H_
-
-#ifdef __cplusplus
-
-#define VFRMT_H264   0
-#define VFRMT_MJPEG  1
-#define VFRMT_YUYV   2
-
-extern "C" { 
-
-#else
-
-#include "uvc_intf.h"
-#include "uapi_videodev2.h"
-
-typedef struct {
-	volatile char off; // !=0 - завершить  get_video_frame_thrd
-	volatile char run; // !=0 - get_video_frame_thrd запущен
-	volatile char get; // =1 - запрос копирования фрейма который влезет в буфер, = 2 - копирования фрейма только с размером pbuf_size
-	volatile char ok;  //
-	char *pbuf;
-	unsigned int pbuf_size;
-	unsigned int frame_size;
-	unsigned int copy_count;
-	unsigned int frame_count;
-	struct uvc_context uvc_ctx;	
-} vfrm_buf_t;
-
-extern vfrm_buf_t vfrmb; // управляющая структура драйвера get video frame
-
-#endif
-
-
-
-//extern void stop_uvc(void);
-//extern int start_uvc(void);
-void FreeBufCameraFrame(void);
-unsigned int SetCameraParam(int fmt_type, int width, int height, int frame_rate, int compression_ratio);
-int StartCamera(void);
-unsigned int GetCameraFrame(void *pbuf);
-
-#ifdef __cplusplus
-} // extern "C"
-#endif
-
-#endif // _USB_UVC_DRV_H_
diff --git a/arduino/realtek-ambz/cores/arduino/wiring_analog.c b/arduino/realtek-ambz/cores/arduino/wiring_analog.c
index b941522..1faba48 100644
--- a/arduino/realtek-ambz/cores/arduino/wiring_analog.c
+++ b/arduino/realtek-ambz/cores/arduino/wiring_analog.c
@@ -43,13 +43,6 @@ bool g_adc_enabled[] = {
     false, false, false
 };
 
-/* DAC */
-dac_t dac0;
-
-bool g_dac_enabled[] = {
-    false
-};
-
 extern void *gpio_pin_struct[];
 
 //
@@ -151,39 +144,27 @@ void analogWrite(uint32_t ulPin, uint32_t ulValue)
 {
     pwmout_t *obj;
 
-    if (ulPin == DAC0)
-    {
-        if (g_dac_enabled[0] == false) {
-            analogout_init(&dac0, DA_0);
-            g_dac_enabled[0] = true;
-        }
-        ulValue %= (1<<_writeResolution);
-        analogout_write(&dac0, ulValue * 1.0 / (1<<_writeResolution) );       
-    }
-    else
-    {
-        if ((g_APinDescription[ulPin].ulPinAttribute & PIO_PWM) == PIO_PWM) {
-            /* Handle */
-            if ( g_APinDescription[ulPin].ulPinType != PIO_PWM )
-            {
-                if ( (g_APinDescription[ulPin].ulPinType == PIO_GPIO) || (g_APinDescription[ulPin].ulPinType == PIO_GPIO_IRQ) ) {
-                    pinRemoveMode(ulPin);
-                }
-                gpio_pin_struct[ulPin] = malloc ( sizeof(pwmout_t) );
-                pwmout_t *obj = (pwmout_t *)gpio_pin_struct[ulPin];
-                pwmout_init( obj, g_APinDescription[ulPin].pinname );
-                pwmout_period_us( obj, _writePeriod);
-                pwmout_write( obj, ulValue * 1.0 / (1<<_writeResolution));
-                g_APinDescription[ulPin].ulPinType = PIO_PWM;
-                g_APinDescription[ulPin].ulPinMode = PWM_MODE_ENABLED;
-            } else {
-                pwmout_t *obj = (pwmout_t *)gpio_pin_struct[ulPin];
-                pwmout_period_us( obj, _writePeriod);
-                pwmout_write( obj, ulValue * 1.0 / (1<<_writeResolution));
-                if ( g_APinDescription[ulPin].ulPinMode == PWM_MODE_DISABLED ) {
-                    HAL_Pwm_Enable( &obj->pwm_hal_adp );
-                }
+    if ((g_APinDescription[ulPin].ulPinAttribute & PIO_PWM) == PIO_PWM) {
+        /* Handle */
+        if ( g_APinDescription[ulPin].ulPinType != PIO_PWM )
+        {
+            if ( (g_APinDescription[ulPin].ulPinType == PIO_GPIO) || (g_APinDescription[ulPin].ulPinType == PIO_GPIO_IRQ) ) {
+                pinRemoveMode(ulPin);
             }
+            gpio_pin_struct[ulPin] = malloc ( sizeof(pwmout_t) );
+            pwmout_t *obj = (pwmout_t *)gpio_pin_struct[ulPin];
+            pwmout_init( obj, g_APinDescription[ulPin].pinname );
+            pwmout_period_us( obj, _writePeriod);
+            pwmout_write( obj, ulValue * 1.0 / (1<<_writeResolution));
+            g_APinDescription[ulPin].ulPinType = PIO_PWM;
+            g_APinDescription[ulPin].ulPinMode = PWM_MODE_ENABLED;
+        } else {
+            pwmout_t *obj = (pwmout_t *)gpio_pin_struct[ulPin];
+            pwmout_period_us( obj, _writePeriod);
+            pwmout_write( obj, ulValue * 1.0 / (1<<_writeResolution));
+            /* if ( g_APinDescription[ulPin].ulPinMode == PWM_MODE_DISABLED ) {
+                HAL_Pwm_Enable( &obj->pwm_hal_adp );
+            } */
         }
     }
 }
@@ -232,9 +213,9 @@ void _tone(uint32_t ulPin, unsigned int frequency, unsigned long duration)
         pwmout_t *obj = (pwmout_t *)gpio_pin_struct[ulPin];
         pwmout_period( obj, 1.0/frequency );
         pwmout_pulsewidth( obj, 1.0/(frequency * 2));
-        if (g_APinDescription[ulPin].ulPinMode == PWM_MODE_DISABLED) {
+        /* if (g_APinDescription[ulPin].ulPinMode == PWM_MODE_DISABLED) {
             HAL_Pwm_Enable( &obj->pwm_hal_adp );
-        }
+        } */
     }
 
     if (duration > 0) {
diff --git a/arduino/realtek-ambz/cores/arduino/wiring_digital.c b/arduino/realtek-ambz/cores/arduino/wiring_digital.c
index 9904b01..2d5489f 100644
--- a/arduino/realtek-ambz/cores/arduino/wiring_digital.c
+++ b/arduino/realtek-ambz/cores/arduino/wiring_digital.c
@@ -241,7 +241,7 @@ void pinRemoveMode(uint32_t ulPin) {
     if ( g_APinDescription[ulPin].ulPinType == PIO_PWM ) {
         // The PWM pin can only be disabled
         pwmout_t *obj = (pwmout_t *)gpio_pin_struct[ulPin];
-        HAL_Pwm_Disable( &obj->pwm_hal_adp );
+        // HAL_Pwm_Disable( &obj->pwm_hal_adp );
         g_APinDescription[ulPin].ulPinMode = PWM_MODE_DISABLED;
     }
     if ( g_APinDescription[ulPin].ulPinType == PIO_GPIO ) {
@@ -263,4 +263,3 @@ void pinRemoveMode(uint32_t ulPin) {
 #ifdef __cplusplus
 }
 #endif
-