firmware coadHEADmaster

10 files changed, 3564 insertions, 0 deletions

diff --git a/firmware/rf test/Core/Src/fonts.c b/firmware/rf test/Core/Src/fonts.c
new file mode 100644
index 0000000..35ff8e4
--- /dev/null
+++ b/firmware/rf test/Core/Src/fonts.c
@@ -0,0 +1,22 @@
+#include "fonts.h"
+
+uint8_t jetbrains_mono_quarter_fb[1598] = {255,255,255,231,231,215,215,219,219,131,189,189,61,255,255,255,255,255,255,255,193,189,189,189,193,157,189,189,189,193,255,255,255,255,255,255,255,195,187,189,253,253,253,253,189,187,195,255,255,255,255,255,255,255,193,157,189,189,189,189,189,189,157,193,255,255,255,255,255,255,255,129,249,249,249,129,249,249,249,249,129,255,255,255,255,255,255,255,129,253,253,253,129,249,249,249,249,249,255,255,255,255,255,255,255,195,187,253,253,141,189,189,189,187,195,255,255,255,255,255,255,255,189,189,189,189,129,189,189,189,189,189,255,255,255,255,255,255,255,131,239,239,239,239,239,239,239,239,131,255,255,255,255,255,255,255,191,191,191,191,191,191,191,189,157,195,255,255,255,255,255,255,255,61,189,221,205,225,205,221,157,189,61,255,255,255,255,255,255,255,251,251,251,251,251,251,251,251,251,3,255,255,255,255,255,255,255,185,153,149,165,165,173,189,189,189,189,255,255,255,255,255,255,255,185,185,181,181,173,173,173,157,157,157,255,255,255,255,255,255,255,195,187,189,189,189,189,189,189,187,195,255,255,255,255,255,255,255,193,189,61,61,189,129,253,253,253,253,255,255,255,255,255,255,255,195,185,189,189,189,189,189,189,185,195,223,159,191,255,255,255,255,193,189,189,189,193,205,221,221,189,61,255,255,255,255,255,255,255,195,187,189,249,227,159,191,189,185,195,255,255,255,255,255,255,255,1,239,239,239,239,239,239,239,239,239,255,255,255,255,255,255,255,189,189,189,189,189,189,189,189,187,195,255,255,255,255,255,255,255,61,189,185,187,219,219,215,215,231,231,255,255,255,255,255,255,255,110,101,101,101,85,149,149,153,153,155,255,255,255,255,255,255,255,61,187,219,215,231,231,215,219,185,61,255,255,255,255,255,255,255,61,189,155,219,215,231,239,239,239,239,255,255,255,255,255,255,255,129,191,223,207,239,247,247,251,249,129,255,255,255,255,255,255,255,255,255,195,185,191,131,185,189,153,163,255,255,255,255,255,255,255,253,253,197,185,189,189,189,189,185,197,255,255,255,255,255,255,255,255,255,195,187,189,253,253,189,187,195,255,255,255,255,255,255,255,191,191,163,155,189,189,189,189,155,163,255,255,255,255,255,255,255,255,255,195,187,189,129,253,253,187,195,255,255,255,255,255,255,255,143,247,247,129,247,247,247,247,247,247,255,255,255,255,255,255,255,255,255,163,155,189,189,189,189,155,163,191,191,195,255,255,255,255,253,253,197,153,185,189,189,189,189,189,255,255,255,255,255,255,207,207,255,227,239,239,239,239,239,239,1,255,255,255,255,255,255,159,159,255,193,223,223,223,223,223,223,223,223,207,225,255,255,255,255,249,249,57,153,217,225,201,217,185,57,255,255,255,255,255,255,255,240,247,247,247,247,247,247,247,247,15,255,255,255,255,255,255,255,255,255,145,173,45,45,45,45,45,45,255,255,255,255,255,255,255,255,255,197,153,185,189,189,189,189,189,255,255,255,255,255,255,255,255,255,195,187,189,189,189,189,187,195,255,255,255,255,255,255,255,255,255,197,185,189,189,189,189,185,197,253,253,253,255,255,255,255,255,255,163,155,189,189,189,189,155,163,191,191,191,255,255,255,255,255,255,195,187,187,187,251,251,251,251,255,255,255,255,255,255,255,255,255,195,185,253,227,159,191,185,195,255,255,255,255,255,255,255,247,247,129,247,247,247,247,247,247,143,255,255,255,255,255,255,255,255,255,189,189,189,189,189,189,155,195,255,255,255,255,255,255,255,255,255,61,185,187,219,219,215,231,231,255,255,255,255,255,255,255,255,255,108,101,101,165,149,149,153,155,255,255,255,255,255,255,255,255,255,185,219,215,231,231,211,155,61,255,255,255,255,255,255,255,255,255,61,185,187,219,211,215,231,239,231,247,247,255,255,255,255,255,255,129,159,223,239,247,243,251,129,255,255,255,255,255,255,255,231,235,237,239,239,239,239,239,239,1,255,255,255,255,255,255,255,195,185,189,191,191,223,239,247,251,129,255,255,255,255,255,255,255,129,223,239,199,159,191,191,189,153,195,255,255,255,255,255,255,255,223,239,231,247,187,185,189,129,191,191,255,255,255,255,255,255,255,129,253,249,193,159,191,191,189,153,195,255,255,255,255,255,255,255,207,247,247,195,187,189,189,189,185,195,255,255,255,255,255,255,255,1,189,189,159,223,223,239,239,231,247,255,255,255,255,255,255,255,195,185,189,187,199,153,189,189,185,195,255,255,255,255,255,255,255,195,185,189,189,185,131,223,207,239,247,255,255,255,255,255,255,255,195,185,189,189,165,189,189,189,185,195,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,231,231,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,239,231,231,247,255,255,255,255,239,239,239,239,239,239,239,255,255,231,255,255,255,255,255,255,255,195,185,125,77,45,117,117,117,109,141,253,249,199,255,239,239,199,139,173,233,235,195,143,175,173,173,195,239,239,255,255,255,255,255,113,182,214,209,239,23,103,107,109,28,255,255,255,255,255,255,239,231,215,219,187,189,255,255,255,255,255,255,255,255,255,255,255,255,195,219,187,251,241,109,156,156,141,97,255,255,255,255,255,191,207,231,247,247,247,247,246,247,247,247,238,207,191,255,255,255,251,243,239,239,223,223,223,223,223,207,239,239,243,251,255,255,255,159,239,239,239,239,239,241,239,239,239,239,239,143,255,255,255,255,241,231,247,247,247,247,143,247,247,247,247,231,241,255,255,255,255,199,247,247,247,247,247,247,247,247,247,247,247,199,255,255,255,255,227,239,239,239,239,239,239,239,239,239,239,239,227,255,255,255,255,255,255,239,239,239,239,255,255,255,255,255,255,255,255,255,255,255,255,255,215,215,215,215,255,255,255,255,255,255,255,255,255,255,255,255,255,195,159,191,159,199,247,247,255,255,231,255,255,255,255,255,255,255,255,127,63,207,243,251,231,143,63,255,255,255,255,255,255,255,255,255,255,252,243,207,223,231,241,252,255,255,255,255,255,255,255,255,255,255,231,231,255,255,255,255,231,231,255,255,255,255,255,255,255,255,255,231,231,255,255,255,255,255,239,231,247,247,255,255,253,249,251,251,247,247,231,239,239,207,223,223,191,191,255,255,255,191,191,159,223,223,239,239,239,247,247,243,251,251,253,255,255,255,255,255,255,255,255,255,255,195,255,255,255,255,255,255,255,255,255,255,255,255,255,239,239,239,129,239,239,239,255,255,255,255,255,255,255,255,255,255,255,129,255,255,129,255,255,255,255,255,255,255,255,255,255,255,255,255,179,173,141,255,255,255,255,255,255,255,255,255,255,255,199,219,219,199,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,1,255,255,255,255,255,255,255,239,239,45,131,231,211,155,255,255,255,255,255,255,255,239,239,239,239,239,239,239,239,239,239,239,239,239,239,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255};
+uint8_t jetbrains_mono_quarter_lu[128] = {93,93,93,93,93,93,93,93,93,93,93,93,93,93,93,93,93,93,93,93,93,93,93,93,93,93,93,93,93,93,93,93,93,64,77,93,66,67,69,76,70,71,91,86,63,85,62,84,61,52,53,54,55,56,57,58,59,60,81,82,79,87,80,78,65,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24,25,74,83,75,68,90,89,26,27,28,29,30,31,32,33,34,35,36,37,38,39,40,41,42,43,44,45,46,47,48,49,50,51,72,92,73,88,93};
+
+
+bitmap_font_t** fonts;
+bitmap_font_t jetbrains_mono_quarter;
+
+void set_up_fonts() {
+	fonts = malloc(sizeof(bitmap_font_t *)*NUMBER_OF_FONTS);
+	fonts[0] = &jetbrains_mono_quarter;
+
+    jetbrains_mono_quarter.char_w = 8;
+    jetbrains_mono_quarter.char_h = 17;
+    jetbrains_mono_quarter.bytes_per_row = 1;
+    jetbrains_mono_quarter.bytes_per_char = 17;
+    jetbrains_mono_quarter.font_bytes = malloc(sizeof(uint8_t)*1598);
+    jetbrains_mono_quarter.lookup = malloc(sizeof(uint8_t)*128);
+    memcpy(jetbrains_mono_quarter.font_bytes, jetbrains_mono_quarter_fb, 1598);
+    memcpy(jetbrains_mono_quarter.lookup, jetbrains_mono_quarter_lu, 128);
+}
diff --git a/firmware/rf test/Core/Src/lis2dw12.c b/firmware/rf test/Core/Src/lis2dw12.c
new file mode 100644
index 0000000..15a8df4
--- /dev/null
+++ b/firmware/rf test/Core/Src/lis2dw12.c
@@ -0,0 +1,8 @@
+/*
+ * lis2dw12.c
+ *
+ *  Created on: Dec 30, 2025
+ *      Author: Anson Bridges
+ */
+#include "lis2dw12.h"
+
diff --git a/firmware/rf test/Core/Src/main.c b/firmware/rf test/Core/Src/main.c
new file mode 100644
index 0000000..6f4ae4e
--- /dev/null
+++ b/firmware/rf test/Core/Src/main.c
@@ -0,0 +1,612 @@
+/* USER CODE BEGIN Header */

+/**

+  ******************************************************************************

+  * @file           : main.c

+  * @brief          : Main program body

+  ******************************************************************************

+  * @attention

+  *

+  * Copyright (c) 2025 STMicroelectronics.

+  * All rights reserved.

+  *

+  * This software is licensed under terms that can be found in the LICENSE file

+  * in the root directory of this software component.

+  * If no LICENSE file comes with this software, it is provided AS-IS.

+  *

+  ******************************************************************************

+  */

+/* USER CODE END Header */

+/* Includes ------------------------------------------------------------------*/

+#include "main.h"

+

+/* Private includes ----------------------------------------------------------*/

+/* USER CODE BEGIN Includes */

+#include "sharpmem.h"

+//#include "w25q_mem.h"

+#include <stdio.h>

+/* USER CODE END Includes */

+

+/* Private typedef -----------------------------------------------------------*/

+/* USER CODE BEGIN PTD */

+

+/* USER CODE END PTD */

+

+/* Private define ------------------------------------------------------------*/

+/* USER CODE BEGIN PD */

+uint8_t testimg[1360] = {255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,7,192,1,252,240,0,248,31,248,199,3,0,0,7,112,224,143,223,255,255,7,192,1,60,192,0,248,31,224,199,3,0,0,7,112,128,143,223,255,255,255,254,253,159,255,223,255,223,231,215,127,255,251,247,127,191,47,223,255,255,255,254,253,223,255,223,255,223,207,155,127,255,251,247,127,63,111,223,255,255,255,254,253,223,255,223,255,223,207,187,127,255,251,247,127,191,111,222,255,255,255,254,1,156,255,223,255,223,239,187,127,255,251,7,112,159,239,222,255,255,255,254,1,60,248,223,255,223,231,125,127,255,251,7,112,192,239,220,255,255,255,254,253,255,225,223,255,31,240,125,127,255,251,247,127,224,239,221,255,255,255,254,253,255,207,223,255,31,252,0,127,255,251,247,127,207,239,219,255,255,255,254,253,255,159,223,255,223,255,0,126,255,251,247,127,223,239,219,255,255,255,254,253,255,159,223,255,223,255,254,126,255,251,247,127,191,239,215,255,255,255,254,253,255,207,223,255,223,127,254,124,255,251,247,127,63,239,199,255,255,255,254,1,28,224,223,255,223,127,255,125,255,251,7,112,127,238,207,255,255,255,254,1,124,248,223,255,223,127,255,125,255,251,7,112,255,236,207,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,31,255,255,255,255,249,255,255,255,255,255,63,255,255,255,255,255,255,255,255,207,255,255,255,255,249,255,255,255,255,255,63,255,255,255,255,255,255,255,255,231,255,255,255,243,249,255,255,255,255,255,255,255,63,255,255,255,255,255,255,231,255,255,255,243,249,255,255,255,255,255,255,255,63,255,255,255,255,255,255,131,143,159,249,129,153,63,252,63,124,56,51,103,30,248,255,255,255,255,255,231,115,30,248,243,57,206,249,207,31,55,48,231,56,255,255,255,255,255,255,231,243,156,255,243,121,206,243,199,159,63,63,231,57,255,255,255,255,255,255,231,241,156,255,243,121,6,240,135,143,63,63,231,57,255,255,255,255,255,255,231,241,156,255,243,121,198,255,15,140,63,63,231,57,255,255,255,255,255,255,231,241,156,255,243,121,198,255,127,136,63,63,231,57,255,255,255,255,255,255,231,243,156,255,243,121,206,255,255,153,63,63,231,57,255,255,255,255,255,255,231,115,158,255,227,121,142,247,247,28,55,63,231,60,254,255,255,255,255,255,231,143,159,255,143,121,62,248,15,127,56,63,39,254,248,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,231,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,231,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,231,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255};

+SharpMemDisplay_t lcd;

+RTC_TimeTypeDef sTime;

+RTC_AlarmTypeDef sAlarm;

+RTC_DateTypeDef sDate;

+/* USER CODE END PD */

+

+/* Private macro -------------------------------------------------------------*/

+/* USER CODE BEGIN PM */

+int _write(int file, char *ptr, int len) {

+	int i = 0;

+	for(i = 0; i < len; i++)

+		ITM_SendChar((*ptr++));

+	return len;

+}

+/* USER CODE END PM */

+

+/* Private variables ---------------------------------------------------------*/

+I2C_HandleTypeDef hi2c1;

+

+IPCC_HandleTypeDef hipcc;

+

+RTC_HandleTypeDef hrtc;

+

+SPI_HandleTypeDef hspi1;

+

+/* USER CODE BEGIN PV */

+

+/* USER CODE END PV */

+

+/* Private function prototypes -----------------------------------------------*/

+void SystemClock_Config(void);

+void PeriphCommonClock_Config(void);

+static void MX_GPIO_Init(void);

+static void MX_RTC_Init(void);

+static void MX_SPI1_Init(void);

+static void MX_I2C1_Init(void);

+static void MX_IPCC_Init(void);

+static void MX_RF_Init(void);

+/* USER CODE BEGIN PFP */

+

+/* USER CODE END PFP */

+

+/* Private user code ---------------------------------------------------------*/

+/* USER CODE BEGIN 0 */

+

+/* USER CODE END 0 */

+

+/**

+  * @brief  The application entry point.

+  * @retval int

+  */

+int main(void)

+{

+

+  /* USER CODE BEGIN 1 */

+

+  /* USER CODE END 1 */

+

+  /* MCU Configuration--------------------------------------------------------*/

+

+  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */

+  HAL_Init();

+

+  /* USER CODE BEGIN Init */

+

+  /* USER CODE END Init */

+

+  /* Configure the system clock */

+  SystemClock_Config();

+

+  /* Configure the peripherals common clocks */

+  PeriphCommonClock_Config();

+

+  /* IPCC initialisation */

+  MX_IPCC_Init();

+

+  /* USER CODE BEGIN SysInit */

+

+  /* USER CODE END SysInit */

+

+  /* Initialize all configured peripherals */

+  MX_GPIO_Init();

+  MX_RTC_Init();

+  MX_SPI1_Init();

+  MX_I2C1_Init();

+  MX_RF_Init();

+  /* USER CODE BEGIN 2 */

+  set_up_fonts();

+  SharpMem_Init();

+  uint8_t started = 0;

+  //SHARPMEM_write(&lcd, "Boot", 0, 0,0, true, false);

+  //SHARPMEM_refresh_display(&lcd);

+

+  HAL_Delay(5000);

+  /* USER CODE END 2 */

+

+  /* Infinite loop */

+  /* USER CODE BEGIN WHILE */

+

+

+

+  while (1)

+  {

+    /* USER CODE END WHILE */

+

+    /* USER CODE BEGIN 3 */

+    if (!started){

+    	// disable pullup resistors causing current draw

+    	/*GPIO_InitTypeDef GPIO_InitStruct = {0};

+

+		GPIO_InitStruct.Pin = GPIO_PIN_6|GPIO_PIN_7;

+		GPIO_InitStruct.Mode = GPIO_MODE_AF_OD;

+		GPIO_InitStruct.Pull = GPIO_NOPULL;

+		GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;

+		GPIO_InitStruct.Alternate = GPIO_AF4_I2C1;

+		HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);*/

+

+		started = 1;

+	}

+    HAL_RTC_GetTime(&hrtc, &sTime, RTC_FORMAT_BIN);

+    HAL_RTC_GetDate(&hrtc, &sDate, RTC_FORMAT_BIN);

+    char timestr[15];

+    sprintf(timestr, "%i:%i:%i", sTime.Hours, sTime.Minutes, sTime.Seconds);

+    started++;

+

+    SHARPMEM_clear_display_buffer(&lcd);

+    SHARPMEM_write(&lcd, timestr, 0, 30, 20, true, false);

+    SHARPMEM_refresh_display(&lcd);

+    //HAL_Delay(900);

+

+    HAL_SuspendTick();

+

+    __HAL_PWR_CLEAR_FLAG(PWR_FLAG_WU);

+    HAL_PWREx_EnterSTOP2Mode(PWR_STOPENTRY_WFI);

+    HAL_RTCEx_DeactivateWakeUpTimer(&hrtc);

+    __HAL_PWR_CLEAR_FLAG(PWR_FLAG_WU);

+    SystemClock_Config();

+    HAL_ResumeTick();

+

+	/*HAL_SuspendTick();

+	__HAL_RTC_ALARM_CLEAR_FLAG(&hrtc, RTC_FLAG_ALRAF);

+	__HAL_PWR_CLEAR_FLAG(PWR_FLAG_WU);

+	HAL_NVIC_SetPriority(RTC_Alarm_IRQn, 0, 0);

+	HAL_NVIC_EnableIRQ(RTC_Alarm_IRQn);

+	//Custom_STM_App_Update_Char(0, (uint8_t *)(12));

+

+	sAlarm.AlarmTime.Seconds = (sTime.Seconds+10) % 60;

+	sAlarm.AlarmTime.TimeFormat = RTC_HOURFORMAT12_AM;

+	sAlarm.AlarmDateWeekDay = RTC_WEEKDAY_FRIDAY;

+	sAlarm.AlarmMask = RTC_ALARMMASK_DATEWEEKDAY | RTC_ALARMMASK_HOURS;

+	sAlarm.AlarmTime.DayLightSaving = RTC_DAYLIGHTSAVING_NONE;

+	sAlarm.AlarmTime.StoreOperation = RTC_STOREOPERATION_RESET;

+	sAlarm.AlarmSubSecondMask = RTC_ALARMSUBSECONDMASK_ALL;

+	sAlarm.AlarmDateWeekDaySel = RTC_ALARMDATEWEEKDAYSEL_DATE;

+	sAlarm.Alarm = RTC_ALARM_A;

+	if (HAL_RTC_SetAlarm_IT(&hrtc, &sAlarm, RTC_FORMAT_BIN) != HAL_OK)

+	{

+		Error_Handler();

+	}

+

+	 if(   (LL_PWR_IsActiveFlag_C1SB() == 0)

+		 || (LL_PWR_IsActiveFlag_C2SB() == 0)

+		)

+	  {

+		LL_C2_PWR_SetPowerMode(LL_PWR_MODE_SHUTDOWN);

+	  }

+

+	HAL_PWREx_EnterSTOP2Mode(PWR_STOPENTRY_WFI);

+	HAL_ResumeTick();

+

+	SystemClock_Config();*/

+

+    //SHARPMEM_write(&lcd, "TEST PROGRAM", 0, 0,0, true, false);

+

+    //printf("lcd params: %d, %d\n", i % lcd.width, i / lcd.width);

+    //memcpy(lcd._buffer, testimg, 1360);

+

+    //SHARPMEM_clear_display(&lcd);

+

+  }

+  /* USER CODE END 3 */

+}

+

+/**

+  * @brief System Clock Configuration

+  * @retval None

+  */

+void SystemClock_Config(void)

+{

+  RCC_OscInitTypeDef RCC_OscInitStruct = {0};

+  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

+

+  /** Configure LSE Drive Capability

+  */

+  HAL_PWR_EnableBkUpAccess();

+  __HAL_RCC_LSEDRIVE_CONFIG(RCC_LSEDRIVE_MEDIUMHIGH);

+

+  /** Configure the main internal regulator output voltage

+  */

+  __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);

+

+  /** Initializes the RCC Oscillators according to the specified parameters

+  * in the RCC_OscInitTypeDef structure.

+  */

+  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI|RCC_OSCILLATORTYPE_HSE

+                              |RCC_OSCILLATORTYPE_LSE;

+  RCC_OscInitStruct.HSEState = RCC_HSE_ON;

+  RCC_OscInitStruct.LSEState = RCC_LSE_ON;

+  RCC_OscInitStruct.HSIState = RCC_HSI_ON;

+  RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;

+  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_NONE;

+  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)

+  {

+    Error_Handler();

+  }

+

+  /** Configure the SYSCLKSource, HCLK, PCLK1 and PCLK2 clocks dividers

+  */

+  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK4|RCC_CLOCKTYPE_HCLK2

+                              |RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK

+                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;

+  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_HSE;

+  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV16;

+  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1;

+  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

+  RCC_ClkInitStruct.AHBCLK2Divider = RCC_SYSCLK_DIV1;

+  RCC_ClkInitStruct.AHBCLK4Divider = RCC_SYSCLK_DIV1;

+

+  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_1) != HAL_OK)

+  {

+    Error_Handler();

+  }

+}

+

+/**

+  * @brief Peripherals Common Clock Configuration

+  * @retval None

+  */

+void PeriphCommonClock_Config(void)

+{

+  RCC_PeriphCLKInitTypeDef PeriphClkInitStruct = {0};

+

+  /** Initializes the peripherals clock

+  */

+  PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_SMPS|RCC_PERIPHCLK_RFWAKEUP;

+  PeriphClkInitStruct.RFWakeUpClockSelection = RCC_RFWKPCLKSOURCE_LSE;

+  PeriphClkInitStruct.SmpsClockSelection = RCC_SMPSCLKSOURCE_HSE;

+  PeriphClkInitStruct.SmpsDivSelection = RCC_SMPSCLKDIV_RANGE1;

+

+  if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK)

+  {

+    Error_Handler();

+  }

+  /* USER CODE BEGIN Smps */

+

+  /* USER CODE END Smps */

+}

+

+/**

+  * @brief I2C1 Initialization Function

+  * @param None

+  * @retval None

+  */

+static void MX_I2C1_Init(void)

+{

+

+  /* USER CODE BEGIN I2C1_Init 0 */

+

+  /* USER CODE END I2C1_Init 0 */

+

+  /* USER CODE BEGIN I2C1_Init 1 */

+

+  /* USER CODE END I2C1_Init 1 */

+  hi2c1.Instance = I2C1;

+  hi2c1.Init.Timing = 0x00000508;

+  hi2c1.Init.OwnAddress1 = 0;

+  hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;

+  hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;

+  hi2c1.Init.OwnAddress2 = 0;

+  hi2c1.Init.OwnAddress2Masks = I2C_OA2_NOMASK;

+  hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;

+  hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;

+  if (HAL_I2C_Init(&hi2c1) != HAL_OK)

+  {

+    Error_Handler();

+  }

+

+  /** Configure Analogue filter

+  */

+  if (HAL_I2CEx_ConfigAnalogFilter(&hi2c1, I2C_ANALOGFILTER_ENABLE) != HAL_OK)

+  {

+    Error_Handler();

+  }

+

+  /** Configure Digital filter

+  */

+  if (HAL_I2CEx_ConfigDigitalFilter(&hi2c1, 0) != HAL_OK)

+  {

+    Error_Handler();

+  }

+  /* USER CODE BEGIN I2C1_Init 2 */

+

+  /* USER CODE END I2C1_Init 2 */

+

+}

+

+/**

+  * @brief IPCC Initialization Function

+  * @param None

+  * @retval None

+  */

+static void MX_IPCC_Init(void)

+{

+

+  /* USER CODE BEGIN IPCC_Init 0 */

+

+  /* USER CODE END IPCC_Init 0 */

+

+  /* USER CODE BEGIN IPCC_Init 1 */

+

+  /* USER CODE END IPCC_Init 1 */

+  hipcc.Instance = IPCC;

+  if (HAL_IPCC_Init(&hipcc) != HAL_OK)

+  {

+    Error_Handler();

+  }

+  /* USER CODE BEGIN IPCC_Init 2 */

+

+  /* USER CODE END IPCC_Init 2 */

+

+}

+

+/**

+  * @brief RF Initialization Function

+  * @param None

+  * @retval None

+  */

+static void MX_RF_Init(void)

+{

+

+  /* USER CODE BEGIN RF_Init 0 */

+

+  /* USER CODE END RF_Init 0 */

+

+  /* USER CODE BEGIN RF_Init 1 */

+

+  /* USER CODE END RF_Init 1 */

+  /* USER CODE BEGIN RF_Init 2 */

+

+  /* USER CODE END RF_Init 2 */

+

+}

+

+/**

+  * @brief RTC Initialization Function

+  * @param None

+  * @retval None

+  */

+static void MX_RTC_Init(void)

+{

+

+  /* USER CODE BEGIN RTC_Init 0 */

+

+  /* USER CODE END RTC_Init 0 */

+

+  RTC_TimeTypeDef sTime = {0};

+  RTC_DateTypeDef sDate = {0};

+

+  /* USER CODE BEGIN RTC_Init 1 */

+

+  /* USER CODE END RTC_Init 1 */

+

+  /** Initialize RTC Only

+  */

+  hrtc.Instance = RTC;

+  hrtc.Init.HourFormat = RTC_HOURFORMAT_24;

+  hrtc.Init.AsynchPrediv = 127;

+  hrtc.Init.SynchPrediv = 255;

+  hrtc.Init.OutPut = RTC_OUTPUT_DISABLE;

+  hrtc.Init.OutPutPolarity = RTC_OUTPUT_POLARITY_HIGH;

+  hrtc.Init.OutPutType = RTC_OUTPUT_TYPE_OPENDRAIN;

+  hrtc.Init.OutPutRemap = RTC_OUTPUT_REMAP_NONE;

+  if (HAL_RTC_Init(&hrtc) != HAL_OK)

+  {

+    Error_Handler();

+  }

+

+  /* USER CODE BEGIN Check_RTC_BKUP */

+

+  /* USER CODE END Check_RTC_BKUP */

+

+  /** Initialize RTC and set the Time and Date

+  */

+  sTime.Hours = 12;

+  sTime.Minutes = 50;

+  sTime.Seconds = 30;

+  sTime.SubSeconds = 0x0;

+  sTime.DayLightSaving = RTC_DAYLIGHTSAVING_NONE;

+  sTime.StoreOperation = RTC_STOREOPERATION_RESET;

+  if (HAL_RTC_SetTime(&hrtc, &sTime, RTC_FORMAT_BIN) != HAL_OK)

+  {

+    Error_Handler();

+  }

+  sDate.WeekDay = RTC_WEEKDAY_MONDAY;

+  sDate.Month = RTC_MONTH_JANUARY;

+  sDate.Date = 1;

+  sDate.Year = 0;

+

+  if (HAL_RTC_SetDate(&hrtc, &sDate, RTC_FORMAT_BIN) != HAL_OK)

+  {

+    Error_Handler();

+  }

+

+  /** Enable the WakeUp

+  */

+  if (HAL_RTCEx_SetWakeUpTimer_IT(&hrtc, 0, RTC_WAKEUPCLOCK_RTCCLK_DIV16) != HAL_OK)

+  {

+    Error_Handler();

+  }

+  /* USER CODE BEGIN RTC_Init 2 */

+

+  /* USER CODE END RTC_Init 2 */

+

+}

+

+/**

+  * @brief SPI1 Initialization Function

+  * @param None

+  * @retval None

+  */

+static void MX_SPI1_Init(void)

+{

+

+  /* USER CODE BEGIN SPI1_Init 0 */

+

+  /* USER CODE END SPI1_Init 0 */

+

+  /* USER CODE BEGIN SPI1_Init 1 */

+

+  /* USER CODE END SPI1_Init 1 */

+  /* SPI1 parameter configuration*/

+  hspi1.Instance = SPI1;

+  hspi1.Init.Mode = SPI_MODE_MASTER;

+  hspi1.Init.Direction = SPI_DIRECTION_2LINES;

+  hspi1.Init.DataSize = SPI_DATASIZE_8BIT;

+  hspi1.Init.CLKPolarity = SPI_POLARITY_LOW;

+  hspi1.Init.CLKPhase = SPI_PHASE_1EDGE;

+  hspi1.Init.NSS = SPI_NSS_SOFT;

+  hspi1.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_2;

+  hspi1.Init.FirstBit = SPI_FIRSTBIT_LSB;

+  hspi1.Init.TIMode = SPI_TIMODE_DISABLE;

+  hspi1.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;

+  hspi1.Init.CRCPolynomial = 7;

+  hspi1.Init.CRCLength = SPI_CRC_LENGTH_DATASIZE;

+  hspi1.Init.NSSPMode = SPI_NSS_PULSE_DISABLE;

+  if (HAL_SPI_Init(&hspi1) != HAL_OK)

+  {

+    Error_Handler();

+  }

+  /* USER CODE BEGIN SPI1_Init 2 */

+

+  /* USER CODE END SPI1_Init 2 */

+

+}

+

+/**

+  * @brief GPIO Initialization Function

+  * @param None

+  * @retval None

+  */

+static void MX_GPIO_Init(void)

+{

+  GPIO_InitTypeDef GPIO_InitStruct = {0};

+  /* USER CODE BEGIN MX_GPIO_Init_1 */

+

+  /* USER CODE END MX_GPIO_Init_1 */

+

+  /* GPIO Ports Clock Enable */

+  __HAL_RCC_GPIOC_CLK_ENABLE();

+  __HAL_RCC_GPIOA_CLK_ENABLE();

+  __HAL_RCC_GPIOB_CLK_ENABLE();

+

+  /*Configure GPIO pin Output Level */

+  HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4|GPIO_PIN_6, GPIO_PIN_RESET);

+

+  /*Configure GPIO pin Output Level */

+  HAL_GPIO_WritePin(GPIOA, GPIO_PIN_10, GPIO_PIN_SET);

+

+  /*Configure GPIO pin Output Level */

+  HAL_GPIO_WritePin(GPIOB, GPIO_PIN_4|GPIO_PIN_5, GPIO_PIN_SET);

+

+  /*Configure GPIO pin : PA0 */

+  GPIO_InitStruct.Pin = GPIO_PIN_0;

+  GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING;

+  GPIO_InitStruct.Pull = GPIO_PULLDOWN;

+  HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);

+

+  /*Configure GPIO pins : PA4 PA6 PA10 */

+  GPIO_InitStruct.Pin = GPIO_PIN_4|GPIO_PIN_6|GPIO_PIN_10;

+  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;

+  GPIO_InitStruct.Pull = GPIO_NOPULL;

+  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;

+  HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);

+

+  /*Configure GPIO pins : PA8 PA11 PA15 */

+  GPIO_InitStruct.Pin = GPIO_PIN_8|GPIO_PIN_11|GPIO_PIN_15;

+  GPIO_InitStruct.Mode = GPIO_MODE_INPUT;

+  GPIO_InitStruct.Pull = GPIO_PULLUP;

+  HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);

+

+  /*Configure GPIO pin : PB0 */

+  GPIO_InitStruct.Pin = GPIO_PIN_0;

+  GPIO_InitStruct.Mode = GPIO_MODE_INPUT;

+  GPIO_InitStruct.Pull = GPIO_PULLUP;

+  HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);

+

+  /*Configure GPIO pins : PB4 PB5 */

+  GPIO_InitStruct.Pin = GPIO_PIN_4|GPIO_PIN_5;

+  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_OD;

+  GPIO_InitStruct.Pull = GPIO_PULLUP;

+  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;

+  HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);

+

+  /* USER CODE BEGIN MX_GPIO_Init_2 */

+

+  /* USER CODE END MX_GPIO_Init_2 */

+}

+

+/* USER CODE BEGIN 4 */

+

+void SharpMem_Init() {

+	lcd.width = 160;

+	lcd.height = 68;

+	lcd.spidev = &hspi1;

+	lcd.cs_pin_bank = GPIOA;

+	lcd.cs_pin = GPIO_PIN_4;

+	lcd.lcdmode_pin_bank = GPIOA;

+	lcd.lcdmode_pin = GPIO_PIN_6;

+

+	lcd._sharpmem_vcom = SHARPMEM_BIT_VCOM;

+	lcd._buffer = malloc(sizeof(uint8_t)*(lcd.width * lcd.height / 8));

+	SHARPMEM_clear_display(&lcd);

+}

+

+/* USER CODE END 4 */

+

+/**

+  * @brief  This function is executed in case of error occurrence.

+  * @retval None

+  */

+void Error_Handler(void)

+{

+  /* USER CODE BEGIN Error_Handler_Debug */

+  /* User can add his own implementation to report the HAL error return state */

+  __disable_irq();

+  while (1)

+  {

+  }

+  /* USER CODE END Error_Handler_Debug */

+}

+#ifdef USE_FULL_ASSERT

+/**

+  * @brief  Reports the name of the source file and the source line number

+  *         where the assert_param error has occurred.

+  * @param  file: pointer to the source file name

+  * @param  line: assert_param error line source number

+  * @retval None

+  */

+void assert_failed(uint8_t *file, uint32_t line)

+{

+  /* USER CODE BEGIN 6 */

+  /* User can add his own implementation to report the file name and line number,

+     ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */

+  /* USER CODE END 6 */

+}

+#endif /* USE_FULL_ASSERT */

diff --git a/firmware/rf test/Core/Src/sharpmem.c b/firmware/rf test/Core/Src/sharpmem.c
new file mode 100644
index 0000000..0117921
--- /dev/null
+++ b/firmware/rf test/Core/Src/sharpmem.c
@@ -0,0 +1,160 @@
+#include "sharpmem.h"
+#include "fonts.h"
+
+void SHARPMEM_TOGGLEVCOM(SharpMemDisplay_t *display) {
+    display->_sharpmem_vcom = display->_sharpmem_vcom ? 0x00 : SHARPMEM_BIT_VCOM;  
+}
+
+void SHARPMEM_draw_pixel(SharpMemDisplay_t *display, uint16_t x, uint16_t y, bool black) {
+    if ((x >= display->width) || (y >= display->height))
+        return;
+
+    if (black) {
+        display->_buffer[(y * display->width + x) / 8] |= (0x1 << (x & 7)); //potentially expensive when run many times, use lookup from adafruit lib
+    } else {
+        display->_buffer[(y * display->width + x) / 8] &= ~(0x1 << (x & 7));
+    }
+}
+
+void SHARPMEM_draw_line(SharpMemDisplay_t *display, uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2, bool black, uint8_t thickness) {
+
+}
+
+void SHARPMEM_draw_circle(SharpMemDisplay_t *display, uint16_t x, uint16_t y, uint8_t r, bool filled) {
+
+}
+
+void SHARPMEM_draw_rect(SharpMemDisplay_t *display, uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2, bool filled) {
+
+}
+
+void SHARPMEM_write(SharpMemDisplay_t *display, char *text, uint8_t font_index, uint16_t x0, uint16_t y0, bool inverse, bool force_bg) {
+	/*uint8_t mask = 0xFF;
+	uint8_t a1, a2, b;
+	if(font_index > NUMBER_OF_FONTS) return;
+	bitmap_font_t *font = fonts[font_index];
+
+	for (uint8_t ci = 0; ci < strlen(text); ci++){
+		uint16_t base_index = font->lookup[(uint8_t)text[ci]];
+		uint8_t offset = (x0 + (ci)*font->char_w) % 8;
+
+		for (uint8_t y = 0; y < font->char_h; y++) {
+			if(y+y0 > display->height) return;
+
+			uint8_t start_byte = (x0 + ci*font->char_w)/8 + (y0+y)*display->width/8;
+
+			if((x0 + ci*font->char_w % 8))
+				memcpy(display->_buffer + ((y+y0)*(display->width)/8) + x0/8 + ci*font->char_w/8 , font->font_bytes + base_index*font->bytes_per_char + y*font->bytes_per_row , font->bytes_per_row);
+			else {
+				uint8_t charbase = base_index*font->bytes_per_char + y*font->bytes_per_row;
+				bool single_ended_last_byte = (offset + font->char_w - 1) / 8 > (bytes_per_row-1);
+				uint8_t buffer_bytes = font->bytes_per_row + ( single_ended_last_byte ? 1 : 0 );
+				for(uint8_t b_i = 0; b_i < buffer_bytes; b_i++) {
+					b = display->_buffer[start_byte + b_i];
+					a2 = font->font_bytes[ charbase + b_i ] >> offset;
+					if(b_i == 0) {
+						b = ((mask >> offset) & a2) + ( (mask << (8-offset)) & b );
+					} else if (b_i != (buffer_bytes-1 + !single_ended_last_byte)) {
+						a1 = font->font_bytes[ charbase + b_i - 1 ] << (8-offset);
+						if (!single_ended_last_byte) {
+							uint8_t remainder = (offset + font->char_w) % 8;
+							b = ( (a1 + a2) & (mask << (8-remainder)) ) + ((mask >> remainder) & b);
+						}
+						else b = a1 + a2;
+					} else {
+						a1 = font->font_bytes[ charbase + b_i - 1 ] << (8-offset);
+						uint8_t remainder = (offset + font->char_w) % 8;
+						b = ((mask << (8-remainder)) & a1) + ( (mask >> remainder) & b );
+					}
+					display->_buffer[start_byte + b_i] = b;
+				}
+				start_byte += font->bytes_per_char;
+				end_byte += font->bytes_per_char;
+			}
+		}
+	}*/
+
+	// potentially inefficient but simplest algorithm involving reading and writing every single pixel
+	if(font_index > NUMBER_OF_FONTS) return;
+	bitmap_font_t *font = fonts[font_index];
+
+	for (uint8_t ci = 0; ci < strlen(text); ci++){
+		uint16_t base_index = font->lookup[(uint8_t)text[ci]];
+
+		for (uint16_t y = 0; y < font->char_h; y++) {
+			uint16_t charbase = base_index*font->bytes_per_char + y*font->bytes_per_row;
+			for (uint16_t x = 0; x < font->char_w; x++) {
+				bool white = ( (font->font_bytes[charbase + x/8] & (0x1 << (x%8)) ) == 0 );
+				if (!force_bg && white) continue;
+				if (inverse) white = !white;
+				SHARPMEM_draw_pixel(display, x0+ci*font->char_w+x, y0+y, !white);
+			}
+		}
+	}
+}
+
+uint8_t SHARPMEM_get_pixel(SharpMemDisplay_t *display, uint16_t x, uint16_t y) {
+	if ((x >= display->width) || (y >= display->height))
+	   return 1; //1 = empty
+
+}
+
+void SHARPMEM_clear_display(SharpMemDisplay_t *display) {
+	HAL_GPIO_WritePin(display->lcdmode_pin_bank, display->lcdmode_pin, GPIO_PIN_RESET); // set lcdmode pin to low for manual vcom control
+
+	memset(display->_buffer, 0xff, (display->width * display->height) / 8);
+
+    // Send the clear screen command rather than doing a HW refresh (quicker)
+    
+    uint8_t clear_data[2] = {(uint8_t)(display->_sharpmem_vcom | SHARPMEM_BIT_CLEAR),
+                            0x00};
+    
+    HAL_GPIO_WritePin(display->cs_pin_bank, display->cs_pin, GPIO_PIN_SET);
+    HAL_SPI_Transmit(display->spidev, clear_data, 2, 16);
+    HAL_GPIO_WritePin(display->cs_pin_bank, display->cs_pin, GPIO_PIN_RESET);
+
+    SHARPMEM_TOGGLEVCOM(display);
+    HAL_GPIO_WritePin(display->lcdmode_pin_bank, display->lcdmode_pin, GPIO_PIN_SET); // set lcdmode pin to high for 1Hz external vcom signal
+}
+
+void SHARPMEM_refresh_display(SharpMemDisplay_t *display) {
+	HAL_GPIO_WritePin(display->lcdmode_pin_bank, display->lcdmode_pin, GPIO_PIN_RESET); // set lcdmode pin to low for manual vcom control
+    uint16_t i, currentline;
+
+    //spidev->beginTransaction();
+    // Send the write command
+    HAL_GPIO_WritePin(display->cs_pin_bank, display->cs_pin, GPIO_PIN_SET);
+    uint8_t write_data[1] = {0x00 | SHARPMEM_BIT_WRITECMD};
+    HAL_SPI_Transmit(display->spidev, write_data, 1, 100);
+    SHARPMEM_TOGGLEVCOM(display);
+
+    uint8_t bytes_per_line = display->width / 8;
+    uint16_t totalbytes = (display->width * display->height) / 8;
+
+    for (i = 0; i < totalbytes; i += bytes_per_line) {
+        uint8_t line[bytes_per_line + 2];
+
+        // Send address byte
+        currentline = ((i + 1) / (display->width / 8)) + 1;
+        line[0] = currentline;
+        // copy over this line
+        memcpy(line + 1, display->_buffer + i, bytes_per_line);
+        // Send end of line
+        line[bytes_per_line + 1] = 0x00;
+        // send it!
+        HAL_SPI_Transmit(display->spidev, line, bytes_per_line + 2, 16);
+    }
+
+    // Send another trailing 8 bits for the last line
+    HAL_SPI_Transmit(display->spidev, (0x00), 1, 100);
+    HAL_GPIO_WritePin(display->cs_pin_bank, display->cs_pin, GPIO_PIN_RESET);
+    HAL_GPIO_WritePin(display->lcdmode_pin_bank, display->lcdmode_pin, GPIO_PIN_SET); // set lcdmode pin to high for 1Hz external vcom signal
+}
+
+void SHARPMEM_clear_display_buffer(SharpMemDisplay_t *display) {
+	memset(display->_buffer, 0xff, (display->width * display->height) / 8);
+}
+
+uint8_t *SHARPMEM_get_buffer(SharpMemDisplay_t *display) {
+
+}
diff --git a/firmware/rf test/Core/Src/stm32wbxx_hal_msp.c b/firmware/rf test/Core/Src/stm32wbxx_hal_msp.c
new file mode 100644
index 0000000..cb3fa8a
--- /dev/null
+++ b/firmware/rf test/Core/Src/stm32wbxx_hal_msp.c
@@ -0,0 +1,348 @@
+/* USER CODE BEGIN Header */

+/**

+  ******************************************************************************

+  * @file         stm32wbxx_hal_msp.c

+  * @brief        This file provides code for the MSP Initialization

+  *               and de-Initialization codes.

+  ******************************************************************************

+  * @attention

+  *

+  * Copyright (c) 2025 STMicroelectronics.

+  * All rights reserved.

+  *

+  * This software is licensed under terms that can be found in the LICENSE file

+  * in the root directory of this software component.

+  * If no LICENSE file comes with this software, it is provided AS-IS.

+  *

+  ******************************************************************************

+  */

+/* USER CODE END Header */

+

+/* Includes ------------------------------------------------------------------*/

+#include "main.h"

+/* USER CODE BEGIN Includes */

+

+/* USER CODE END Includes */

+

+/* Private typedef -----------------------------------------------------------*/

+/* USER CODE BEGIN TD */

+

+/* USER CODE END TD */

+

+/* Private define ------------------------------------------------------------*/

+/* USER CODE BEGIN Define */

+

+/* USER CODE END Define */

+

+/* Private macro -------------------------------------------------------------*/

+/* USER CODE BEGIN Macro */

+

+/* USER CODE END Macro */

+

+/* Private variables ---------------------------------------------------------*/

+/* USER CODE BEGIN PV */

+

+/* USER CODE END PV */

+

+/* Private function prototypes -----------------------------------------------*/

+/* USER CODE BEGIN PFP */

+

+/* USER CODE END PFP */

+

+/* External functions --------------------------------------------------------*/

+/* USER CODE BEGIN ExternalFunctions */

+

+/* USER CODE END ExternalFunctions */

+

+/* USER CODE BEGIN 0 */

+

+/* USER CODE END 0 */

+/**

+  * Initializes the Global MSP.

+  */

+void HAL_MspInit(void)

+{

+

+  /* USER CODE BEGIN MspInit 0 */

+

+  /* USER CODE END MspInit 0 */

+

+  __HAL_RCC_HSEM_CLK_ENABLE();

+

+  /* System interrupt init*/

+

+  /* Peripheral interrupt init */

+  /* HSEM_IRQn interrupt configuration */

+  HAL_NVIC_SetPriority(HSEM_IRQn, 0, 0);

+  HAL_NVIC_EnableIRQ(HSEM_IRQn);

+

+  /* USER CODE BEGIN MspInit 1 */

+

+  /* USER CODE END MspInit 1 */

+}

+

+/**

+  * @brief I2C MSP Initialization

+  * This function configures the hardware resources used in this example

+  * @param hi2c: I2C handle pointer

+  * @retval None

+  */

+void HAL_I2C_MspInit(I2C_HandleTypeDef* hi2c)

+{

+  GPIO_InitTypeDef GPIO_InitStruct = {0};

+  RCC_PeriphCLKInitTypeDef PeriphClkInitStruct = {0};

+  if(hi2c->Instance==I2C1)

+  {

+    /* USER CODE BEGIN I2C1_MspInit 0 */

+

+    /* USER CODE END I2C1_MspInit 0 */

+

+  /** Initializes the peripherals clock

+  */

+    PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_I2C1;

+    PeriphClkInitStruct.I2c1ClockSelection = RCC_I2C1CLKSOURCE_PCLK1;

+    if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK)

+    {

+      Error_Handler();

+    }

+

+    __HAL_RCC_GPIOB_CLK_ENABLE();

+    /**I2C1 GPIO Configuration

+    PB6     ------> I2C1_SCL

+    PB7     ------> I2C1_SDA

+    */

+    GPIO_InitStruct.Pin = GPIO_PIN_6|GPIO_PIN_7;

+    GPIO_InitStruct.Mode = GPIO_MODE_AF_OD;

+    GPIO_InitStruct.Pull = GPIO_NOPULL;

+    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;

+    GPIO_InitStruct.Alternate = GPIO_AF4_I2C1;

+    HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);

+

+    /* Peripheral clock enable */

+    __HAL_RCC_I2C1_CLK_ENABLE();

+    /* USER CODE BEGIN I2C1_MspInit 1 */

+

+    /* USER CODE END I2C1_MspInit 1 */

+

+  }

+

+}

+

+/**

+  * @brief I2C MSP De-Initialization

+  * This function freeze the hardware resources used in this example

+  * @param hi2c: I2C handle pointer

+  * @retval None

+  */

+void HAL_I2C_MspDeInit(I2C_HandleTypeDef* hi2c)

+{

+  if(hi2c->Instance==I2C1)

+  {

+    /* USER CODE BEGIN I2C1_MspDeInit 0 */

+

+    /* USER CODE END I2C1_MspDeInit 0 */

+    /* Peripheral clock disable */

+    __HAL_RCC_I2C1_CLK_DISABLE();

+

+    /**I2C1 GPIO Configuration

+    PB6     ------> I2C1_SCL

+    PB7     ------> I2C1_SDA

+    */

+    HAL_GPIO_DeInit(GPIOB, GPIO_PIN_6);

+

+    HAL_GPIO_DeInit(GPIOB, GPIO_PIN_7);

+

+    /* USER CODE BEGIN I2C1_MspDeInit 1 */

+

+    /* USER CODE END I2C1_MspDeInit 1 */

+  }

+

+}

+

+/**

+  * @brief IPCC MSP Initialization

+  * This function configures the hardware resources used in this example

+  * @param hipcc: IPCC handle pointer

+  * @retval None

+  */

+void HAL_IPCC_MspInit(IPCC_HandleTypeDef* hipcc)

+{

+  if(hipcc->Instance==IPCC)

+  {

+    /* USER CODE BEGIN IPCC_MspInit 0 */

+

+    /* USER CODE END IPCC_MspInit 0 */

+    /* Peripheral clock enable */

+    __HAL_RCC_IPCC_CLK_ENABLE();

+    /* IPCC interrupt Init */

+    HAL_NVIC_SetPriority(IPCC_C1_RX_IRQn, 0, 0);

+    HAL_NVIC_EnableIRQ(IPCC_C1_RX_IRQn);

+    HAL_NVIC_SetPriority(IPCC_C1_TX_IRQn, 0, 0);

+    HAL_NVIC_EnableIRQ(IPCC_C1_TX_IRQn);

+    /* USER CODE BEGIN IPCC_MspInit 1 */

+

+    /* USER CODE END IPCC_MspInit 1 */

+

+  }

+

+}

+

+/**

+  * @brief IPCC MSP De-Initialization

+  * This function freeze the hardware resources used in this example

+  * @param hipcc: IPCC handle pointer

+  * @retval None

+  */

+void HAL_IPCC_MspDeInit(IPCC_HandleTypeDef* hipcc)

+{

+  if(hipcc->Instance==IPCC)

+  {

+    /* USER CODE BEGIN IPCC_MspDeInit 0 */

+

+    /* USER CODE END IPCC_MspDeInit 0 */

+    /* Peripheral clock disable */

+    __HAL_RCC_IPCC_CLK_DISABLE();

+

+    /* IPCC interrupt DeInit */

+    HAL_NVIC_DisableIRQ(IPCC_C1_RX_IRQn);

+    HAL_NVIC_DisableIRQ(IPCC_C1_TX_IRQn);

+    /* USER CODE BEGIN IPCC_MspDeInit 1 */

+

+    /* USER CODE END IPCC_MspDeInit 1 */

+  }

+

+}

+

+/**

+  * @brief RTC MSP Initialization

+  * This function configures the hardware resources used in this example

+  * @param hrtc: RTC handle pointer

+  * @retval None

+  */

+void HAL_RTC_MspInit(RTC_HandleTypeDef* hrtc)

+{

+  RCC_PeriphCLKInitTypeDef PeriphClkInitStruct = {0};

+  if(hrtc->Instance==RTC)

+  {

+    /* USER CODE BEGIN RTC_MspInit 0 */

+

+    /* USER CODE END RTC_MspInit 0 */

+

+  /** Initializes the peripherals clock

+  */

+    PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_RTC;

+    PeriphClkInitStruct.RTCClockSelection = RCC_RTCCLKSOURCE_LSE;

+    if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK)

+    {

+      Error_Handler();

+    }

+

+    /* Peripheral clock enable */

+    __HAL_RCC_RTC_ENABLE();

+    __HAL_RCC_RTCAPB_CLK_ENABLE();

+    /* RTC interrupt Init */

+    HAL_NVIC_SetPriority(RTC_WKUP_IRQn, 0, 0);

+    HAL_NVIC_EnableIRQ(RTC_WKUP_IRQn);

+    /* USER CODE BEGIN RTC_MspInit 1 */

+

+    /* USER CODE END RTC_MspInit 1 */

+

+  }

+

+}

+

+/**

+  * @brief RTC MSP De-Initialization

+  * This function freeze the hardware resources used in this example

+  * @param hrtc: RTC handle pointer

+  * @retval None

+  */

+void HAL_RTC_MspDeInit(RTC_HandleTypeDef* hrtc)

+{

+  if(hrtc->Instance==RTC)

+  {

+    /* USER CODE BEGIN RTC_MspDeInit 0 */

+

+    /* USER CODE END RTC_MspDeInit 0 */

+    /* Peripheral clock disable */

+    __HAL_RCC_RTC_DISABLE();

+    __HAL_RCC_RTCAPB_CLK_DISABLE();

+

+    /* RTC interrupt DeInit */

+    HAL_NVIC_DisableIRQ(RTC_WKUP_IRQn);

+    /* USER CODE BEGIN RTC_MspDeInit 1 */

+

+    /* USER CODE END RTC_MspDeInit 1 */

+  }

+

+}

+

+/**

+  * @brief SPI MSP Initialization

+  * This function configures the hardware resources used in this example

+  * @param hspi: SPI handle pointer

+  * @retval None

+  */

+void HAL_SPI_MspInit(SPI_HandleTypeDef* hspi)

+{

+  GPIO_InitTypeDef GPIO_InitStruct = {0};

+  if(hspi->Instance==SPI1)

+  {

+    /* USER CODE BEGIN SPI1_MspInit 0 */

+

+    /* USER CODE END SPI1_MspInit 0 */

+    /* Peripheral clock enable */

+    __HAL_RCC_SPI1_CLK_ENABLE();

+

+    __HAL_RCC_GPIOA_CLK_ENABLE();

+    /**SPI1 GPIO Configuration

+    PA5     ------> SPI1_SCK

+    PA7     ------> SPI1_MOSI

+    */

+    GPIO_InitStruct.Pin = GPIO_PIN_5|GPIO_PIN_7;

+    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;

+    GPIO_InitStruct.Pull = GPIO_NOPULL;

+    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;

+    GPIO_InitStruct.Alternate = GPIO_AF5_SPI1;

+    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);

+

+    /* USER CODE BEGIN SPI1_MspInit 1 */

+

+    /* USER CODE END SPI1_MspInit 1 */

+

+  }

+

+}

+

+/**

+  * @brief SPI MSP De-Initialization

+  * This function freeze the hardware resources used in this example

+  * @param hspi: SPI handle pointer

+  * @retval None

+  */

+void HAL_SPI_MspDeInit(SPI_HandleTypeDef* hspi)

+{

+  if(hspi->Instance==SPI1)

+  {

+    /* USER CODE BEGIN SPI1_MspDeInit 0 */

+

+    /* USER CODE END SPI1_MspDeInit 0 */

+    /* Peripheral clock disable */

+    __HAL_RCC_SPI1_CLK_DISABLE();

+

+    /**SPI1 GPIO Configuration

+    PA5     ------> SPI1_SCK

+    PA7     ------> SPI1_MOSI

+    */

+    HAL_GPIO_DeInit(GPIOA, GPIO_PIN_5|GPIO_PIN_7);

+

+    /* USER CODE BEGIN SPI1_MspDeInit 1 */

+

+    /* USER CODE END SPI1_MspDeInit 1 */

+  }

+

+}

+

+/* USER CODE BEGIN 1 */

+

+/* USER CODE END 1 */

diff --git a/firmware/rf test/Core/Src/stm32wbxx_it.c b/firmware/rf test/Core/Src/stm32wbxx_it.c
new file mode 100644
index 0000000..2759953
--- /dev/null
+++ b/firmware/rf test/Core/Src/stm32wbxx_it.c
@@ -0,0 +1,260 @@
+/* USER CODE BEGIN Header */

+/**

+  ******************************************************************************

+  * @file    stm32wbxx_it.c

+  * @brief   Interrupt Service Routines.

+  ******************************************************************************

+  * @attention

+  *

+  * Copyright (c) 2025 STMicroelectronics.

+  * All rights reserved.

+  *

+  * This software is licensed under terms that can be found in the LICENSE file

+  * in the root directory of this software component.

+  * If no LICENSE file comes with this software, it is provided AS-IS.

+  *

+  ******************************************************************************

+  */

+/* USER CODE END Header */

+

+/* Includes ------------------------------------------------------------------*/

+#include "main.h"

+#include "stm32wbxx_it.h"

+/* Private includes ----------------------------------------------------------*/

+/* USER CODE BEGIN Includes */

+/* USER CODE END Includes */

+

+/* Private typedef -----------------------------------------------------------*/

+/* USER CODE BEGIN TD */

+

+/* USER CODE END TD */

+

+/* Private define ------------------------------------------------------------*/

+/* USER CODE BEGIN PD */

+

+/* USER CODE END PD */

+

+/* Private macro -------------------------------------------------------------*/

+/* USER CODE BEGIN PM */

+

+/* USER CODE END PM */

+

+/* Private variables ---------------------------------------------------------*/

+/* USER CODE BEGIN PV */

+

+/* USER CODE END PV */

+

+/* Private function prototypes -----------------------------------------------*/

+/* USER CODE BEGIN PFP */

+

+/* USER CODE END PFP */

+

+/* Private user code ---------------------------------------------------------*/

+/* USER CODE BEGIN 0 */

+

+/* USER CODE END 0 */

+

+/* External variables --------------------------------------------------------*/

+extern IPCC_HandleTypeDef hipcc;

+extern RTC_HandleTypeDef hrtc;

+/* USER CODE BEGIN EV */

+

+/* USER CODE END EV */

+

+/******************************************************************************/

+/*           Cortex Processor Interruption and Exception Handlers          */

+/******************************************************************************/

+/**

+  * @brief This function handles Non maskable interrupt.

+  */

+void NMI_Handler(void)

+{

+  /* USER CODE BEGIN NonMaskableInt_IRQn 0 */

+

+  /* USER CODE END NonMaskableInt_IRQn 0 */

+  /* USER CODE BEGIN NonMaskableInt_IRQn 1 */

+   while (1)

+  {

+  }

+  /* USER CODE END NonMaskableInt_IRQn 1 */

+}

+

+/**

+  * @brief This function handles Hard fault interrupt.

+  */

+void HardFault_Handler(void)

+{

+  /* USER CODE BEGIN HardFault_IRQn 0 */

+

+  /* USER CODE END HardFault_IRQn 0 */

+  while (1)

+  {

+    /* USER CODE BEGIN W1_HardFault_IRQn 0 */

+    /* USER CODE END W1_HardFault_IRQn 0 */

+  }

+}

+

+/**

+  * @brief This function handles Memory management fault.

+  */

+void MemManage_Handler(void)

+{

+  /* USER CODE BEGIN MemoryManagement_IRQn 0 */

+

+  /* USER CODE END MemoryManagement_IRQn 0 */

+  while (1)

+  {

+    /* USER CODE BEGIN W1_MemoryManagement_IRQn 0 */

+    /* USER CODE END W1_MemoryManagement_IRQn 0 */

+  }

+}

+

+/**

+  * @brief This function handles Prefetch fault, memory access fault.

+  */

+void BusFault_Handler(void)

+{

+  /* USER CODE BEGIN BusFault_IRQn 0 */

+

+  /* USER CODE END BusFault_IRQn 0 */

+  while (1)

+  {

+    /* USER CODE BEGIN W1_BusFault_IRQn 0 */

+    /* USER CODE END W1_BusFault_IRQn 0 */

+  }

+}

+

+/**

+  * @brief This function handles Undefined instruction or illegal state.

+  */

+void UsageFault_Handler(void)

+{

+  /* USER CODE BEGIN UsageFault_IRQn 0 */

+

+  /* USER CODE END UsageFault_IRQn 0 */

+  while (1)

+  {

+    /* USER CODE BEGIN W1_UsageFault_IRQn 0 */

+    /* USER CODE END W1_UsageFault_IRQn 0 */

+  }

+}

+

+/**

+  * @brief This function handles System service call via SWI instruction.

+  */

+void SVC_Handler(void)

+{

+  /* USER CODE BEGIN SVCall_IRQn 0 */

+

+  /* USER CODE END SVCall_IRQn 0 */

+  /* USER CODE BEGIN SVCall_IRQn 1 */

+

+  /* USER CODE END SVCall_IRQn 1 */

+}

+

+/**

+  * @brief This function handles Debug monitor.

+  */

+void DebugMon_Handler(void)

+{

+  /* USER CODE BEGIN DebugMonitor_IRQn 0 */

+

+  /* USER CODE END DebugMonitor_IRQn 0 */

+  /* USER CODE BEGIN DebugMonitor_IRQn 1 */

+

+  /* USER CODE END DebugMonitor_IRQn 1 */

+}

+

+/**

+  * @brief This function handles Pendable request for system service.

+  */

+void PendSV_Handler(void)

+{

+  /* USER CODE BEGIN PendSV_IRQn 0 */

+

+  /* USER CODE END PendSV_IRQn 0 */

+  /* USER CODE BEGIN PendSV_IRQn 1 */

+

+  /* USER CODE END PendSV_IRQn 1 */

+}

+

+/**

+  * @brief This function handles System tick timer.

+  */

+void SysTick_Handler(void)

+{

+  /* USER CODE BEGIN SysTick_IRQn 0 */

+

+  /* USER CODE END SysTick_IRQn 0 */

+  HAL_IncTick();

+  /* USER CODE BEGIN SysTick_IRQn 1 */

+

+  /* USER CODE END SysTick_IRQn 1 */

+}

+

+/******************************************************************************/

+/* STM32WBxx Peripheral Interrupt Handlers                                    */

+/* Add here the Interrupt Handlers for the used peripherals.                  */

+/* For the available peripheral interrupt handler names,                      */

+/* please refer to the startup file (startup_stm32wbxx.s).                    */

+/******************************************************************************/

+

+/**

+  * @brief This function handles RTC wake-up interrupt through EXTI line 19.

+  */

+void RTC_WKUP_IRQHandler(void)

+{

+  /* USER CODE BEGIN RTC_WKUP_IRQn 0 */

+

+  /* USER CODE END RTC_WKUP_IRQn 0 */

+  HAL_RTCEx_WakeUpTimerIRQHandler(&hrtc);

+  /* USER CODE BEGIN RTC_WKUP_IRQn 1 */

+

+  /* USER CODE END RTC_WKUP_IRQn 1 */

+}

+

+/**

+  * @brief This function handles IPCC RX occupied interrupt.

+  */

+void IPCC_C1_RX_IRQHandler(void)

+{

+  /* USER CODE BEGIN IPCC_C1_RX_IRQn 0 */

+

+  /* USER CODE END IPCC_C1_RX_IRQn 0 */

+  HAL_IPCC_RX_IRQHandler(&hipcc);

+  /* USER CODE BEGIN IPCC_C1_RX_IRQn 1 */

+

+  /* USER CODE END IPCC_C1_RX_IRQn 1 */

+}

+

+/**

+  * @brief This function handles IPCC TX free interrupt.

+  */

+void IPCC_C1_TX_IRQHandler(void)

+{

+  /* USER CODE BEGIN IPCC_C1_TX_IRQn 0 */

+

+  /* USER CODE END IPCC_C1_TX_IRQn 0 */

+  HAL_IPCC_TX_IRQHandler(&hipcc);

+  /* USER CODE BEGIN IPCC_C1_TX_IRQn 1 */

+

+  /* USER CODE END IPCC_C1_TX_IRQn 1 */

+}

+

+/**

+  * @brief This function handles HSEM global interrupt.

+  */

+void HSEM_IRQHandler(void)

+{

+  /* USER CODE BEGIN HSEM_IRQn 0 */

+

+  /* USER CODE END HSEM_IRQn 0 */

+  HAL_HSEM_IRQHandler();

+  /* USER CODE BEGIN HSEM_IRQn 1 */

+

+  /* USER CODE END HSEM_IRQn 1 */

+}

+

+/* USER CODE BEGIN 1 */

+

+/* USER CODE END 1 */

diff --git a/firmware/rf test/Core/Src/syscalls.c b/firmware/rf test/Core/Src/syscalls.c
new file mode 100644
index 0000000..8884b5a
--- /dev/null
+++ b/firmware/rf test/Core/Src/syscalls.c
@@ -0,0 +1,176 @@
+/**
+ ******************************************************************************
+ * @file      syscalls.c
+ * @author    Auto-generated by STM32CubeIDE
+ * @brief     STM32CubeIDE Minimal System calls file
+ *
+ *            For more information about which c-functions
+ *            need which of these lowlevel functions
+ *            please consult the Newlib libc-manual
+ ******************************************************************************
+ * @attention
+ *
+ * Copyright (c) 2020-2025 STMicroelectronics.
+ * All rights reserved.
+ *
+ * This software is licensed under terms that can be found in the LICENSE file
+ * in the root directory of this software component.
+ * If no LICENSE file comes with this software, it is provided AS-IS.
+ *
+ ******************************************************************************
+ */
+
+/* Includes */
+#include <sys/stat.h>
+#include <stdlib.h>
+#include <errno.h>
+#include <stdio.h>
+#include <signal.h>
+#include <time.h>
+#include <sys/time.h>
+#include <sys/times.h>
+
+
+/* Variables */
+extern int __io_putchar(int ch) __attribute__((weak));
+extern int __io_getchar(void) __attribute__((weak));
+
+
+char *__env[1] = { 0 };
+char **environ = __env;
+
+
+/* Functions */
+void initialise_monitor_handles()
+{
+}
+
+int _getpid(void)
+{
+  return 1;
+}
+
+int _kill(int pid, int sig)
+{
+  (void)pid;
+  (void)sig;
+  errno = EINVAL;
+  return -1;
+}
+
+void _exit (int status)
+{
+  _kill(status, -1);
+  while (1) {}    /* Make sure we hang here */
+}
+
+__attribute__((weak)) int _read(int file, char *ptr, int len)
+{
+  (void)file;
+  int DataIdx;
+
+  for (DataIdx = 0; DataIdx < len; DataIdx++)
+  {
+    *ptr++ = __io_getchar();
+  }
+
+  return len;
+}
+
+__attribute__((weak)) int _write(int file, char *ptr, int len)
+{
+  (void)file;
+  int DataIdx;
+
+  for (DataIdx = 0; DataIdx < len; DataIdx++)
+  {
+    __io_putchar(*ptr++);
+  }
+  return len;
+}
+
+int _close(int file)
+{
+  (void)file;
+  return -1;
+}
+
+
+int _fstat(int file, struct stat *st)
+{
+  (void)file;
+  st->st_mode = S_IFCHR;
+  return 0;
+}
+
+int _isatty(int file)
+{
+  (void)file;
+  return 1;
+}
+
+int _lseek(int file, int ptr, int dir)
+{
+  (void)file;
+  (void)ptr;
+  (void)dir;
+  return 0;
+}
+
+int _open(char *path, int flags, ...)
+{
+  (void)path;
+  (void)flags;
+  /* Pretend like we always fail */
+  return -1;
+}
+
+int _wait(int *status)
+{
+  (void)status;
+  errno = ECHILD;
+  return -1;
+}
+
+int _unlink(char *name)
+{
+  (void)name;
+  errno = ENOENT;
+  return -1;
+}
+
+int _times(struct tms *buf)
+{
+  (void)buf;
+  return -1;
+}
+
+int _stat(char *file, struct stat *st)
+{
+  (void)file;
+  st->st_mode = S_IFCHR;
+  return 0;
+}
+
+int _link(char *old, char *new)
+{
+  (void)old;
+  (void)new;
+  errno = EMLINK;
+  return -1;
+}
+
+int _fork(void)
+{
+  errno = EAGAIN;
+  return -1;
+}
+
+int _execve(char *name, char **argv, char **env)
+{
+  (void)name;
+  (void)argv;
+  (void)env;
+  errno = ENOMEM;
+  return -1;
+}
diff --git a/firmware/rf test/Core/Src/sysmem.c b/firmware/rf test/Core/Src/sysmem.c
new file mode 100644
index 0000000..5d9f7e6
--- /dev/null
+++ b/firmware/rf test/Core/Src/sysmem.c
@@ -0,0 +1,79 @@
+/**
+ ******************************************************************************
+ * @file      sysmem.c
+ * @author    Generated by STM32CubeIDE
+ * @brief     STM32CubeIDE System Memory calls file
+ *
+ *            For more information about which C functions
+ *            need which of these lowlevel functions
+ *            please consult the newlib libc manual
+ ******************************************************************************
+ * @attention
+ *
+ * Copyright (c) 2025 STMicroelectronics.
+ * All rights reserved.
+ *
+ * This software is licensed under terms that can be found in the LICENSE file
+ * in the root directory of this software component.
+ * If no LICENSE file comes with this software, it is provided AS-IS.
+ *
+ ******************************************************************************
+ */
+
+/* Includes */
+#include <errno.h>
+#include <stdint.h>
+
+/**
+ * Pointer to the current high watermark of the heap usage
+ */
+static uint8_t *__sbrk_heap_end = NULL;
+
+/**
+ * @brief _sbrk() allocates memory to the newlib heap and is used by malloc
+ *        and others from the C library
+ *
+ * @verbatim
+ * ############################################################################
+ * #  .data  #  .bss  #       newlib heap       #          MSP stack          #
+ * #         #        #                         # Reserved by _Min_Stack_Size #
+ * ############################################################################
+ * ^-- RAM start      ^-- _end                             _estack, RAM end --^
+ * @endverbatim
+ *
+ * This implementation starts allocating at the '_end' linker symbol
+ * The '_Min_Stack_Size' linker symbol reserves a memory for the MSP stack
+ * The implementation considers '_estack' linker symbol to be RAM end
+ * NOTE: If the MSP stack, at any point during execution, grows larger than the
+ * reserved size, please increase the '_Min_Stack_Size'.
+ *
+ * @param incr Memory size
+ * @return Pointer to allocated memory
+ */
+void *_sbrk(ptrdiff_t incr)
+{
+  extern uint8_t _end; /* Symbol defined in the linker script */
+  extern uint8_t _estack; /* Symbol defined in the linker script */
+  extern uint32_t _Min_Stack_Size; /* Symbol defined in the linker script */
+  const uint32_t stack_limit = (uint32_t)&_estack - (uint32_t)&_Min_Stack_Size;
+  const uint8_t *max_heap = (uint8_t *)stack_limit;
+  uint8_t *prev_heap_end;
+
+  /* Initialize heap end at first call */
+  if (NULL == __sbrk_heap_end)
+  {
+    __sbrk_heap_end = &_end;
+  }
+
+  /* Protect heap from growing into the reserved MSP stack */
+  if (__sbrk_heap_end + incr > max_heap)
+  {
+    errno = ENOMEM;
+    return (void *)-1;
+  }
+
+  prev_heap_end = __sbrk_heap_end;
+  __sbrk_heap_end += incr;
+
+  return (void *)prev_heap_end;
+}
diff --git a/firmware/rf test/Core/Src/system_stm32wbxx.c b/firmware/rf test/Core/Src/system_stm32wbxx.c
new file mode 100644
index 0000000..57f6a4e
--- /dev/null
+++ b/firmware/rf test/Core/Src/system_stm32wbxx.c
@@ -0,0 +1,378 @@
+/**
+  ******************************************************************************
+  * @file    system_stm32wbxx.c
+  * @author  MCD Application Team
+  * @brief   CMSIS Cortex Device Peripheral Access Layer System Source File
+  *
+  *   This file provides two functions and one global variable to be called from
+  *   user application:
+  *      - SystemInit(): This function is called at startup just after reset and
+  *                      before branch to main program. This call is made inside
+  *                      the "startup_stm32wbxx.s" file.
+  *
+  *      - SystemCoreClock variable: Contains the core clock (HCLK), it can be used
+  *                                  by the user application to setup the SysTick
+  *                                  timer or configure other parameters.
+  *
+  *      - SystemCoreClockUpdate(): Updates the variable SystemCoreClock and must
+  *                                 be called whenever the core clock is changed
+  *                                 during program execution.
+  *
+  *   After each device reset the MSI (4 MHz) is used as system clock source.
+  *   Then SystemInit() function is called, in "startup_stm32wbxx.s" file, to
+  *   configure the system clock before to branch to main program.
+  *
+  *   This file configures the system clock as follows:
+  *=============================================================================
+  *-----------------------------------------------------------------------------
+  *        System Clock source                    | MSI
+  *-----------------------------------------------------------------------------
+  *        SYSCLK(Hz)                             | 4000000
+  *-----------------------------------------------------------------------------
+  *        HCLK(Hz)                               | 4000000
+  *-----------------------------------------------------------------------------
+  *        AHB Prescaler                          | 1
+  *-----------------------------------------------------------------------------
+  *        APB1 Prescaler                         | 1
+  *-----------------------------------------------------------------------------
+  *        APB2 Prescaler                         | 1
+  *-----------------------------------------------------------------------------
+  *        PLL_M                                  | 1
+  *-----------------------------------------------------------------------------
+  *        PLL_N                                  | 8
+  *-----------------------------------------------------------------------------
+  *        PLL_P                                  | 7
+  *-----------------------------------------------------------------------------
+  *        PLL_Q                                  | 2
+  *-----------------------------------------------------------------------------
+  *        PLL_R                                  | 2
+  *-----------------------------------------------------------------------------
+  *        PLLSAI1_P                              | NA
+  *-----------------------------------------------------------------------------
+  *        PLLSAI1_Q                              | NA
+  *-----------------------------------------------------------------------------
+  *        PLLSAI1_R                              | NA
+  *-----------------------------------------------------------------------------
+  *        Require 48MHz for USB OTG FS,          | Disabled
+  *        SDIO and RNG clock                     |
+  *-----------------------------------------------------------------------------
+  *=============================================================================
+  ******************************************************************************
+  * @attention
+  *
+  * Copyright (c) 2019-2021 STMicroelectronics.
+  * All rights reserved.
+  *
+  * This software is licensed under terms that can be found in the LICENSE file
+  * in the root directory of this software component.
+  * If no LICENSE file comes with this software, it is provided AS-IS.
+  *
+  ******************************************************************************
+  */
+
+/** @addtogroup CMSIS
+  * @{
+  */
+
+/** @addtogroup stm32WBxx_system
+  * @{
+  */
+
+/** @addtogroup stm32WBxx_System_Private_Includes
+  * @{
+  */
+
+#include "stm32wbxx.h"
+
+#if !defined  (HSE_VALUE)
+#define HSE_VALUE    (32000000UL) /*!< Value of the External oscillator in Hz */
+#endif /* HSE_VALUE */
+
+#if !defined  (MSI_VALUE)
+#define MSI_VALUE    (4000000UL) /*!< Value of the Internal oscillator in Hz*/
+#endif /* MSI_VALUE */
+
+#if !defined  (HSI_VALUE)
+#define HSI_VALUE    (16000000UL) /*!< Value of the Internal oscillator in Hz*/
+#endif /* HSI_VALUE */
+
+#if !defined  (LSI_VALUE)
+#define LSI_VALUE  (32000UL)       /*!< Value of LSI in Hz*/
+#endif /* LSI_VALUE */
+
+#if !defined  (LSE_VALUE)
+#if defined(STM32WB5Mxx)
+  #define LSE_VALUE    32774U     /*!< Value of the LSE oscillator in Hz */
+#else
+  #define LSE_VALUE    32768U     /*!< Value of the LSE oscillator in Hz */
+#endif /* STM32WB5Mxx */
+#endif /* LSE_VALUE */
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32WBxx_System_Private_TypesDefinitions
+  * @{
+  */
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32WBxx_System_Private_Defines
+  * @{
+  */
+
+/* Note: Following vector table addresses must be defined in line with linker
+         configuration. */
+/*!< Uncomment the following line if you need to relocate CPU1 CM4 and/or CPU2
+     CM0+ vector table anywhere in Sram or Flash. Else vector table will be kept
+     at address 0x00 which correspond to automatic remap of boot address selected */
+/* #define USER_VECT_TAB_ADDRESS */
+#if defined(USER_VECT_TAB_ADDRESS)
+/*!< Uncomment this line for user vector table remap in Sram else user remap
+     will be done in Flash. */
+/* #define VECT_TAB_SRAM */
+#if defined(VECT_TAB_SRAM)
+#define VECT_TAB_BASE_ADDRESS   SRAM1_BASE      /*!< Vector Table base address field.
+                                                     This value must be a multiple of 0x200. */
+#if !defined(VECT_TAB_OFFSET)
+#define VECT_TAB_OFFSET         0x00000000U     /*!< Vector Table offset field.
+                                                     This value must be a multiple of 0x200. */
+#endif /* VECT_TAB_OFFSET */
+
+#else
+#define VECT_TAB_BASE_ADDRESS   FLASH_BASE      /*!< Vector Table base address field.
+                                                     This value must be a multiple of 0x200. */
+#if !defined(VECT_TAB_OFFSET)
+#define VECT_TAB_OFFSET         0x00000000U     /*!< Vector Table offset field.
+                                                     This value must be a multiple of 0x200. */
+#endif /* VECT_TAB_OFFSET */
+
+#endif /* VECT_TAB_SRAM */
+#endif /* USER_VECT_TAB_ADDRESS */
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32WBxx_System_Private_Macros
+  * @{
+  */
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32WBxx_System_Private_Variables
+  * @{
+  */
+/* The SystemCoreClock variable is updated in three ways:
+    1) by calling CMSIS function SystemCoreClockUpdate()
+    2) by calling HAL API function HAL_RCC_GetHCLKFreq()
+    3) each time HAL_RCC_ClockConfig() is called to configure the system clock frequency
+       Note: If you use this function to configure the system clock; then there
+             is no need to call the 2 first functions listed above, since SystemCoreClock
+             variable is updated automatically.
+*/
+uint32_t SystemCoreClock = 4000000UL ; /*CPU1: M4 on MSI clock after startup (4MHz)*/
+
+const uint32_t AHBPrescTable[16UL] = {1UL, 3UL, 5UL, 1UL, 1UL, 6UL, 10UL, 32UL, 2UL, 4UL, 8UL, 16UL, 64UL, 128UL, 256UL, 512UL};
+
+const uint32_t APBPrescTable[8UL]  = {0UL, 0UL, 0UL, 0UL, 1UL, 2UL, 3UL, 4UL};
+
+const uint32_t MSIRangeTable[16UL] = {100000UL, 200000UL, 400000UL, 800000UL, 1000000UL, 2000000UL, \
+                                      4000000UL, 8000000UL, 16000000UL, 24000000UL, 32000000UL, 48000000UL, 0UL, 0UL, 0UL, 0UL
+                                     }; /* 0UL values are incorrect cases */
+
+#if defined(STM32WB55xx) || defined(STM32WB5Mxx) || defined(STM32WB35xx) || defined (STM32WB15xx) || defined (STM32WB1Mxx)
+const uint32_t SmpsPrescalerTable[4UL][6UL] = {{1UL, 3UL, 2UL, 2UL, 1UL, 2UL}, \
+  {2UL, 6UL, 4UL, 3UL, 2UL, 4UL}, \
+  {4UL, 12UL, 8UL, 6UL, 4UL, 8UL}, \
+  {4UL, 12UL, 8UL, 6UL, 4UL, 8UL}
+};
+#endif /* STM32WB55xx || STM32WB5Mxx || STM32WB35xx || STM32WB15xx || STM32WB1Mxx */
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32WBxx_System_Private_FunctionPrototypes
+  * @{
+  */
+
+/**
+  * @}
+  */
+
+/** @addtogroup STM32WBxx_System_Private_Functions
+  * @{
+  */
+
+/**
+  * @brief  Setup the microcontroller system.
+  * @param  None
+  * @retval None
+  */
+void SystemInit(void)
+{
+#if defined(USER_VECT_TAB_ADDRESS)
+  /* Configure the Vector Table location add offset address ------------------*/
+  SCB->VTOR = VECT_TAB_BASE_ADDRESS | VECT_TAB_OFFSET;
+#endif /* USER_VECT_TAB_ADDRESS */
+
+  /* FPU settings ------------------------------------------------------------*/
+#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
+  SCB->CPACR |= ((3UL << (10UL * 2UL)) | (3UL << (11UL * 2UL))); /* set CP10 and CP11 Full Access */
+#endif /* FPU */
+
+  /* Reset the RCC clock configuration to the default reset state ------------*/
+  /* Set MSION bit */
+  RCC->CR |= RCC_CR_MSION;
+
+  /* Reset CFGR register */
+  RCC->CFGR = 0x00070000U;
+
+  /* Reset PLLSAI1ON, PLLON, HSECSSON, HSEON, HSION, and MSIPLLON bits */
+  RCC->CR &= (uint32_t)0xFAF6FEFBU;
+
+  /*!< Reset LSI1 and LSI2 bits */
+  RCC->CSR &= (uint32_t)0xFFFFFFFAU;
+
+  /*!< Reset HSI48ON  bit */
+  RCC->CRRCR &= (uint32_t)0xFFFFFFFEU;
+
+  /* Reset PLLCFGR register */
+  RCC->PLLCFGR = 0x22041000U;
+
+#if defined(STM32WB55xx) || defined(STM32WB5Mxx)
+  /* Reset PLLSAI1CFGR register */
+  RCC->PLLSAI1CFGR = 0x22041000U;
+#endif /* STM32WB55xx || STM32WB5Mxx */
+
+  /* Reset HSEBYP bit */
+  RCC->CR &= 0xFFFBFFFFU;
+
+  /* Disable all interrupts */
+  RCC->CIER = 0x00000000;
+}
+
+/**
+  * @brief  Update SystemCoreClock variable according to Clock Register Values.
+  *         The SystemCoreClock variable contains the core clock (HCLK), it can
+  *         be used by the user application to setup the SysTick timer or configure
+  *         other parameters.
+  *
+  * @note   Each time the core clock (HCLK) changes, this function must be called
+  *         to update SystemCoreClock variable value. Otherwise, any configuration
+  *         based on this variable will be incorrect.
+  *
+  * @note   - The system frequency computed by this function is not the real
+  *           frequency in the chip. It is calculated based on the predefined
+  *           constant and the selected clock source:
+  *
+  *           - If SYSCLK source is MSI, SystemCoreClock will contain the MSI_VALUE(*)
+  *
+  *           - If SYSCLK source is HSI, SystemCoreClock will contain the HSI_VALUE(**)
+  *
+  *           - If SYSCLK source is HSE, SystemCoreClock will contain the HSE_VALUE(***)
+  *
+  *           - If SYSCLK source is PLL, SystemCoreClock will contain the HSE_VALUE(***)
+  *             or HSI_VALUE(*) or MSI_VALUE(*) multiplied/divided by the PLL factors.
+  *
+  *         (*) MSI_VALUE is a constant defined in stm32wbxx_hal.h file (default value
+  *             4 MHz) but the real value may vary depending on the variations
+  *             in voltage and temperature.
+  *
+  *         (**) HSI_VALUE is a constant defined in stm32wbxx_hal_conf.h file (default value
+  *              16 MHz) but the real value may vary depending on the variations
+  *              in voltage and temperature.
+  *
+  *         (***) HSE_VALUE is a constant defined in stm32wbxx_hal_conf.h file (default value
+  *              32 MHz), user has to ensure that HSE_VALUE is same as the real
+  *              frequency of the crystal used. Otherwise, this function may
+  *              have wrong result.
+  *
+  *         - The result of this function could be not correct when using fractional
+  *           value for HSE crystal.
+  *
+  * @param  None
+  * @retval None
+  */
+void SystemCoreClockUpdate(void)
+{
+  uint32_t tmp, msirange, pllvco, pllr, pllsource, pllm;
+
+  /* Get MSI Range frequency--------------------------------------------------*/
+
+  /*MSI frequency range in Hz*/
+  msirange = MSIRangeTable[(RCC->CR & RCC_CR_MSIRANGE) >> RCC_CR_MSIRANGE_Pos];
+
+  /* Get SYSCLK source -------------------------------------------------------*/
+  switch (RCC->CFGR & RCC_CFGR_SWS)
+  {
+    case 0x00: /* MSI used as system clock source */
+      SystemCoreClock = msirange;
+      break;
+
+    case 0x04: /* HSI used as system clock source */
+      /* HSI used as system clock source */
+      SystemCoreClock = HSI_VALUE;
+      break;
+
+    case 0x08:  /* HSE used as system clock source */
+      SystemCoreClock = HSE_VALUE;
+      break;
+
+    case 0x0C: /* PLL used as system clock  source */
+      /* PLL_VCO = (HSE_VALUE or HSI_VALUE or MSI_VALUE/ PLLM) * PLLN
+         SYSCLK = PLL_VCO / PLLR
+         */
+      pllsource = (RCC->PLLCFGR & RCC_PLLCFGR_PLLSRC);
+      pllm = ((RCC->PLLCFGR & RCC_PLLCFGR_PLLM) >> RCC_PLLCFGR_PLLM_Pos) + 1UL ;
+
+      if (pllsource == 0x02UL) /* HSI used as PLL clock source */
+      {
+        pllvco = (HSI_VALUE / pllm);
+      }
+      else if (pllsource == 0x03UL) /* HSE used as PLL clock source */
+      {
+        pllvco = (HSE_VALUE / pllm);
+      }
+      else /* MSI used as PLL clock source */
+      {
+        pllvco = (msirange / pllm);
+      }
+
+      pllvco = pllvco * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> RCC_PLLCFGR_PLLN_Pos);
+      pllr = (((RCC->PLLCFGR & RCC_PLLCFGR_PLLR) >> RCC_PLLCFGR_PLLR_Pos) + 1UL);
+
+      SystemCoreClock = pllvco / pllr;
+      break;
+
+    default:
+      SystemCoreClock = msirange;
+      break;
+  }
+
+  /* Compute HCLK clock frequency --------------------------------------------*/
+  /* Get HCLK1 prescaler */
+  tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> RCC_CFGR_HPRE_Pos)];
+
+  /* HCLK clock frequency */
+  SystemCoreClock = SystemCoreClock / tmp;
+}
+
+
+/**
+  * @}
+  */
+
+/**
+  * @}
+  */
+
+/**
+  * @}
+  */
diff --git a/firmware/rf test/Core/Src/w25q_mem.rrr b/firmware/rf test/Core/Src/w25q_mem.rrr
new file mode 100644
index 0000000..5f2581b
--- /dev/null
+++ b/firmware/rf test/Core/Src/w25q_mem.rrr
@@ -0,0 +1,1521 @@
+/**
+ *******************************************
+ * @file    w25q_mem.c
+ * @author  Dmitriy Semenov / Crazy_Geeks
+ * @version 0.1b
+ * @date    12-August-2021
+ * @brief   Source file for W25Qxxx lib
+ * @note    https://github.com/Crazy-Geeks/STM32-W25Q-QSPI
+ *******************************************
+ *
+ * @note https://ru.mouser.com/datasheet/2/949/w25q256jv_spi_revg_08032017-1489574.pdf
+ * @note https://www.st.com/resource/en/application_note/DM00227538-.pdf
+ */
+
+/**
+ * @addtogroup W25Q_Driver
+ * @{
+ */
+
+#include "w25q_mem.h"
+
+/**
+ * @addtogroup W25Q_Exp Exported types
+ * @brief External fields and data
+ * @{
+ */
+extern QSPI_HandleTypeDef hqspi;	///< Quad SPI HAL Instance
+/// @}
+
+/**
+ * @addtogroup W25Q_PrivFi Private fields
+ * @brief Private variables and defines
+ * @{
+ */
+#define w25q_delay(x) HAL_Delay(x) 	///< Delay define to provide future support of RTOS
+W25Q_STATUS_REG w25q_status; 		///< Internal status structure instance
+
+/// @}
+
+/**
+ * @addtogroup W25Q_PrivFu Private Methods
+ * @brief Internal lib's functions
+ * @{
+ */
+W25Q_STATE W25Q_WriteEnable(bool enable); 	///< Toggle WOL bit
+W25Q_STATE W25Q_EnableQSPI(bool enable);	///< Toggle QE bit
+W25Q_STATE W25Q_Enter4ByteMode(bool enable); 	///< Toggle ADS bit
+W25Q_STATE W25Q_SetExtendedAddr(u8_t Addr);  	///< Set addr in 3-byte mode
+W25Q_STATE W25Q_GetExtendedAddr(u8_t *outAddr); ///< Get addr in 3-byte mode
+
+static inline u32_t page_to_addr(u32_t pageNum, u8_t pageShift); ///< Translate page addr to byte addr
+/// @}
+
+/**
+ * @addtogroup W25Q_Pub Public methods
+ * @brief Public Methods
+ * @{
+ */
+
+/**
+ * @addtogroup W25Q_Init Init methods
+ * @brief Initalization
+ * @{
+ */
+
+/**
+ * @brief W25Q Init function
+ *
+ * @param none
+ * @return W25Q_STATE enum
+ */
+W25Q_STATE W25Q_Init(void) {
+	W25Q_STATE state;		// temp status variable
+
+	// read id
+	u8_t id = 0;
+	state = W25Q_ReadID(&id);
+	if (state != W25Q_OK)
+		return state;
+	// u can check id here
+
+	// read chip's state to private lib's struct
+	state = W25Q_ReadStatusStruct(NULL);
+	if (state != W25Q_OK)
+		return state;
+
+#if MEM_FLASH_SIZE > 128 // if 4-byte mode
+
+	/* If power-default 4-byte
+	 mode disabled */
+	if (!w25q_status.ADP) {
+		u8_t buf_reg = 0;
+		state = W25Q_ReadStatusReg(&buf_reg, 3);
+		if (state != W25Q_OK)
+			return state;
+		buf_reg |= 0b10; 	// set ADP bit
+		state = W25Q_WriteStatusReg(buf_reg, 3);
+		if (state != W25Q_OK)
+			return state;
+	}
+
+	/* If current 4-byte
+	 mode disabled */
+	if (!w25q_status.ADS) {
+		state = W25Q_Enter4ByteMode(1);
+		if (state != W25Q_OK)
+			return state;
+	}
+#endif
+
+	/* If Quad-SPI mode disabled */
+	if (!w25q_status.QE) {
+		u8_t buf_reg = 0;
+		state = W25Q_ReadStatusReg(&buf_reg, 2);
+		if (state != W25Q_OK)
+			return state;
+		buf_reg |= 0b10;
+		state = W25Q_WriteStatusReg(buf_reg, 2);
+		if (state != W25Q_OK)
+			return state;
+	}
+
+	// make another read
+	state = W25Q_ReadStatusStruct(NULL);
+	// return communication status
+	return state;
+}
+
+/**
+ * @}
+ * @addtogroup W25Q_Reg Register Functions
+ * @brief Operations with Status Registers
+ * @{
+ */
+
+/**
+ * @brief W25Q Enable Volatile SR
+ * Makes status register volatile (temporary)
+ *
+ * @attention Func in development
+ * @param none
+ * @return W25Q_STATE enum
+ */
+W25Q_STATE W25Q_EnableVolatileSR(void) {
+	return W25Q_PARAM_ERR;
+}
+
+/**
+ * @brief W25Q Read Status Register
+ * Read one status register
+ *
+ * @param[out] reg_data 1 byte
+ * @param[in] reg_num Desired register 1..3
+ * @return W25Q_STATE enum
+ */
+W25Q_STATE W25Q_ReadStatusReg(u8_t *reg_data, u8_t reg_num) {
+	QSPI_CommandTypeDef com;
+
+	com.InstructionMode = QSPI_INSTRUCTION_1_LINE; // QSPI_INSTRUCTION_...
+
+	if (reg_num == 1)
+		com.Instruction = W25Q_READ_SR1;
+	else if (reg_num == 2)
+		com.Instruction = W25Q_READ_SR2;
+	else if (reg_num == 3)
+		com.Instruction = W25Q_READ_SR3;
+	else
+		return W25Q_PARAM_ERR;
+
+	com.AddressMode = QSPI_ADDRESS_NONE;
+	com.AddressSize = QSPI_ADDRESS_NONE;
+	com.Address = 0x0U;
+
+	com.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
+	com.AlternateBytes = QSPI_ALTERNATE_BYTES_NONE;
+	com.AlternateBytesSize = QSPI_ALTERNATE_BYTES_NONE;
+
+	com.DummyCycles = 0;
+	com.DataMode = QSPI_DATA_1_LINE;
+	com.NbData = 1;
+
+	com.DdrMode = QSPI_DDR_MODE_DISABLE;
+	//com.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
+	com.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;
+
+	if (HAL_QSPI_Command(&hqspi, &com, HAL_QSPI_TIMEOUT_DEFAULT_VALUE)
+			!= HAL_OK) {
+		return W25Q_SPI_ERR;
+	}
+	if (HAL_QSPI_Receive(&hqspi, reg_data, HAL_QSPI_TIMEOUT_DEFAULT_VALUE)
+			!= HAL_OK) {
+		return W25Q_SPI_ERR;
+	}
+
+	return W25Q_OK;
+}
+
+/**
+ * @brief W25Q Write Status Register
+ * Write one status register
+ *
+ * @param[in] reg_data 1 byte
+ * @param[in] reg_num Desired register 1..3
+ * @return W25Q_STATE enum
+ */
+W25Q_STATE W25Q_WriteStatusReg(u8_t reg_data, u8_t reg_num) {
+	while (W25Q_IsBusy() == W25Q_BUSY)
+		w25q_delay(1);
+
+	W25Q_STATE state = W25Q_WriteEnable(1);
+	if (state != W25Q_OK)
+		return state;
+
+	QSPI_CommandTypeDef com;
+
+	com.InstructionMode = QSPI_INSTRUCTION_1_LINE; // QSPI_INSTRUCTION_...
+
+	if (reg_num == 1)
+		com.Instruction = W25Q_WRITE_SR1;
+	else if (reg_num == 2)
+		com.Instruction = W25Q_WRITE_SR2;
+	else if (reg_num == 3)
+		com.Instruction = W25Q_WRITE_SR3;
+	else
+		return W25Q_PARAM_ERR;
+
+	com.AddressMode = QSPI_ADDRESS_NONE;
+	com.AddressSize = QSPI_ADDRESS_NONE;
+	com.Address = 0x0U;
+
+	com.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
+	com.AlternateBytes = QSPI_ALTERNATE_BYTES_NONE;
+	com.AlternateBytesSize = QSPI_ALTERNATE_BYTES_NONE;
+
+	com.DummyCycles = 0;
+	com.DataMode = QSPI_DATA_1_LINE;
+	com.NbData = 1;
+
+	com.DdrMode = QSPI_DDR_MODE_DISABLE;
+	//com.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
+	com.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;
+
+	if (HAL_QSPI_Command(&hqspi, &com, HAL_QSPI_TIMEOUT_DEFAULT_VALUE)
+			!= HAL_OK) {
+		return W25Q_SPI_ERR;
+	}
+	if (HAL_QSPI_Transmit(&hqspi, &reg_data, HAL_QSPI_TIMEOUT_DEFAULT_VALUE)
+			!= HAL_OK) {
+		return W25Q_SPI_ERR;
+	}
+
+	return W25Q_OK;
+}
+
+/**
+ * @brief W25Q Read Status Registers
+ * Read all status registers to struct
+ *
+ * @param[out] status W25Q_STATUS_REG Pointer
+ * @return W25Q_STATE enum
+ */
+W25Q_STATE W25Q_ReadStatusStruct(W25Q_STATUS_REG *status) {
+	// buffer enum-variable
+	W25Q_STATE state;
+
+	// buffer register variables
+	u8_t SRs[3] = { 0, };
+
+	// first portion
+	state = W25Q_ReadStatusReg(&SRs[0], 1);
+	if (state != W25Q_OK)
+		return state;
+
+	// second portion
+	state = W25Q_ReadStatusReg(&SRs[1], 2);
+	if (state != W25Q_OK)
+		return state;
+
+	// third portion
+	state = W25Q_ReadStatusReg(&SRs[2], 3);
+	if (state != W25Q_OK)
+		return state;
+	if(status){
+		status->BUSY = w25q_status.BUSY = SRs[0] & 0b1;
+		status->WEL = w25q_status.WEL = (SRs[0] >> 1) & 0b1;
+		status->QE = w25q_status.QE = (SRs[1] >> 1) & 0b1;
+		status->SUS = w25q_status.SUS = (SRs[1] >> 7) & 0b1;
+		status->ADS = w25q_status.ADS = SRs[2] & 0b1;
+		status->ADP = w25q_status.ADP = (SRs[2] >> 1) & 0b1;
+		status->SLEEP = w25q_status.SLEEP; // возможно нужно вынести в начало (тестить)
+	}
+
+	return state;
+}
+
+/**
+ * @brief W25Q Check Busy flag
+ * Fast checking Busy flag
+ *
+ * @param none
+ * @return W25Q_STATE enum (W25Q_OK / W25Q_BUSY)
+ */
+W25Q_STATE W25Q_IsBusy(void) {
+	W25Q_STATE state;
+	u8_t sr = 0;
+
+	state = W25Q_ReadStatusReg(&sr, 1);
+	if (state != W25Q_OK)
+		return state;
+
+	w25q_status.BUSY = sr & 0b1;
+
+	return w25q_status.BUSY ? W25Q_BUSY : W25Q_OK;
+}
+
+/**
+ * @}
+ * @addtogroup W25Q_Read Read Functions
+ * @brief Read operations - single data type variables or raw 8-bit blocks
+ * @{
+ */
+
+/**
+ * @brief W25Q Read single Signed Byte
+ * Read signed 8-bit byte variable
+ *
+ * @param[out] buf Data to be read (single)
+ * @param[in] pageShift Byte shift inside page (0..255)
+ * @param[in] pageNum Page number (0..PAGE_COUNT-1)
+ * @return W25Q_STATE enum
+ */
+W25Q_STATE W25Q_ReadSByte(i8_t *buf, u8_t pageShift, u32_t pageNum) {
+	if (pageNum >= PAGE_COUNT)
+		return W25Q_PARAM_ERR;
+	u32_t rawAddr = page_to_addr(pageNum, pageShift);
+	u8_t data;
+	W25Q_STATE state = W25Q_ReadRaw(&data, 1, rawAddr);
+	if (state != W25Q_OK)
+		return state;
+	memcpy(buf, &data, 1);
+	return W25Q_OK;
+}
+
+/**
+ * @brief W25Q Read single Unsigned Byte
+ * Read unsigned 8-bit byte variable
+ *
+ * @param[out] buf Data to be read (single)
+ * @param[in] pageShift Byte shift inside page (0..255)
+ * @param[in] pageNum Page number (0..PAGE_COUNT-1)
+ * @return W25Q_STATE enum
+ */
+W25Q_STATE W25Q_ReadByte(u8_t *buf, u8_t pageShift, u32_t pageNum) {
+	if (pageNum >= PAGE_COUNT)
+		return W25Q_PARAM_ERR;
+	u32_t rawAddr = page_to_addr(pageNum, pageShift);
+	u8_t data;
+	W25Q_STATE state = W25Q_ReadRaw(&data, 1, rawAddr);
+	if (state != W25Q_OK)
+		return state;
+	buf[0] = data;
+	return W25Q_OK;
+}
+
+/**
+ * @brief W25Q Read single Signed Word
+ * Read signed 16-bit word variable
+ *
+ * @param[out] buf Data to be read (single)
+ * @param[in] pageShift Byte shift inside page (0..254)
+ * @param[in] pageNum Page number (0..PAGE_COUNT-1)
+ * @return W25Q_STATE enum
+ */
+W25Q_STATE W25Q_ReadSWord(i16_t *buf, u8_t pageShift, u32_t pageNum) {
+	if (pageNum >= PAGE_COUNT || pageShift > 256 - 2)
+		return W25Q_PARAM_ERR;
+	u32_t rawAddr = page_to_addr(pageNum, pageShift);
+	u8_t data[2];
+	W25Q_STATE state = W25Q_ReadRaw(data, 2, rawAddr);
+	if (state != W25Q_OK)
+		return state;
+	memcpy(buf, data, 2);
+	return W25Q_OK;
+}
+
+/**
+ * @brief W25Q Read single Unsigned Word
+ * Read unsigned 16-bit word variable
+ *
+ * @param[out] buf Data to be read (single)
+ * @param[in] pageShift Byte shift inside page (0..254)
+ * @param[in] pageNum Page number (0..PAGE_COUNT-1)
+ * @return W25Q_STATE enum
+ */
+W25Q_STATE W25Q_ReadWord(u16_t *buf, u8_t pageShift, u32_t pageNum) {
+	if (pageNum >= PAGE_COUNT || pageShift > 256 - 2)
+		return W25Q_PARAM_ERR;
+	u32_t rawAddr = page_to_addr(pageNum, pageShift);
+	u8_t data[2];
+	W25Q_STATE state = W25Q_ReadRaw(data, 2, rawAddr);
+	if (state != W25Q_OK)
+		return state;
+	memcpy(buf, data, 2);
+	return W25Q_OK;
+}
+
+/**
+ * @brief W25Q Read single Signed Long
+ * Read signed 32-bit long variable
+ *
+ * @param[out] buf Data to be read (single)
+ * @param[in] pageShift Byte shift inside page (0..252)
+ * @param[in] pageNum Page number (0..PAGE_COUNT-1)
+ * @return W25Q_STATE enum
+ */
+W25Q_STATE W25Q_ReadSLong(i32_t *buf, u8_t pageShift, u32_t pageNum) {
+	if (pageNum >= PAGE_COUNT || pageShift > 256 - 4)
+		return W25Q_PARAM_ERR;
+	u32_t rawAddr = page_to_addr(pageNum, pageShift);
+	u8_t data[4];
+	W25Q_STATE state = W25Q_ReadRaw(data, 4, rawAddr);
+	if (state != W25Q_OK)
+		return state;
+	memcpy(buf, data, 4);
+	return W25Q_OK;
+}
+
+/**
+ * @brief W25Q Read single Signed Long
+ * Read signed 32-bit long variable
+ *
+ * @param[out] buf Data to be read (single)
+ * @param[in] pageShift Byte shift inside page (0..252)
+ * @param[in] pageNum Page number (0..PAGE_COUNT-1)
+ * @return W25Q_STATE enum
+ */
+W25Q_STATE W25Q_ReadLong(u32_t *buf, u8_t pageShift, u32_t pageNum) {
+	if (pageNum >= PAGE_COUNT || pageShift > 256 - 4)
+		return W25Q_PARAM_ERR;
+	u32_t rawAddr = page_to_addr(pageNum, pageShift);
+	u8_t data[4];
+	W25Q_STATE state = W25Q_ReadRaw(data, 4, rawAddr);
+	if (state != W25Q_OK)
+		return state;
+	memcpy(buf, data, 4);
+	return W25Q_OK;
+}
+
+/**
+ * @brief W25Q Read any 8-bit data
+ * Read any 8-bit data from preffered page place
+ *
+ * @note Use memcpy to decode data
+ * @param[out] buf Pointer to data to be read (single or array)
+ * @param[in] len Length of data (1..256)
+ * @param[in] pageShift Byte shift inside page (0..256 - len)
+ * @param[in] pageNum Page number (0..PAGE_COUNT-1)
+ * @return W25Q_STATE enum
+ */
+W25Q_STATE W25Q_ReadData(u8_t *buf, u16_t len, u8_t pageShift, u32_t pageNum) {
+	if (pageNum >= PAGE_COUNT || len == 0 || len > 256 || pageShift > 256 - len)
+		return W25Q_PARAM_ERR;
+	u32_t rawAddr = page_to_addr(pageNum, pageShift);
+	return W25Q_ReadRaw(buf, len, rawAddr);
+}
+
+/**
+ * @brief W25Q Read any 8-bit data from raw addr
+ * Read any 8-bit data from preffered chip address
+ *
+ * @note Address is in [byte] size
+ * @note Be carefull with page overrun
+ * @param[out] buf Pointer to data to be written (single or array)
+ * @param[in] data_len Length of data (1..256)
+ * @param[in] rawAddr Start address of chip's cell
+ * @return W25Q_STATE enum
+ */
+W25Q_STATE W25Q_ReadRaw(u8_t *buf, u16_t data_len, u32_t rawAddr) {
+	if (data_len > 256 || data_len == 0)
+		return W25Q_PARAM_ERR;
+
+	while (W25Q_IsBusy() == W25Q_BUSY)
+		w25q_delay(1);
+
+	QSPI_CommandTypeDef com;
+
+	com.InstructionMode = QSPI_INSTRUCTION_1_LINE; // QSPI_INSTRUCTION_...
+#if MEM_FLASH_SIZE > 128U
+	com.Instruction = W25Q_FAST_READ_QUAD_IO_4B;	 // Command
+	com.AddressSize = QSPI_ADDRESS_32_BITS;
+#else
+		com.Instruction = W25Q_FAST_READ;	 // Command
+		com.AddressSize = QSPI_ADDRESS_24_BITS;
+	#endif
+	com.AddressMode = QSPI_ADDRESS_1_LINE;
+
+	com.Address = rawAddr;
+
+	com.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
+	com.AlternateBytes = QSPI_ALTERNATE_BYTES_NONE;
+	com.AlternateBytesSize = QSPI_ALTERNATE_BYTES_NONE;
+
+	com.DummyCycles = 8;
+	com.DataMode = QSPI_DATA_1_LINE;
+	com.NbData = data_len;
+
+	com.DdrMode = QSPI_DDR_MODE_DISABLE;
+	//com.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
+	com.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;
+
+	if (HAL_QSPI_Command(&hqspi, &com, HAL_QSPI_TIMEOUT_DEFAULT_VALUE)
+			!= HAL_OK)
+		return W25Q_SPI_ERR;
+
+	if (HAL_QSPI_Receive(&hqspi, buf, HAL_QSPI_TIMEOUT_DEFAULT_VALUE) != HAL_OK)
+		return W25Q_SPI_ERR;
+
+	return W25Q_OK;
+}
+
+/**
+ * @brief W25Q Read any 8-bit data from raw addr
+ * Read any 8-bit data from preffered chip address by SINGLE SPI
+ *
+ * @note Works only with SINGLE SPI Line
+ * @param[out] buf Pointer to data array
+ * @param[in] len Length of array
+ * @param[in] Addr Address to data
+ * @return W25Q_STATE enum
+ */
+W25Q_STATE W25Q_SingleRead(u8_t *buf, u32_t len, u32_t Addr) {
+	QSPI_CommandTypeDef com;
+
+	com.InstructionMode = QSPI_INSTRUCTION_1_LINE; // QSPI_INSTRUCTION_...
+#if MEM_FLASH_SIZE > 128U
+	com.Instruction = W25Q_READ_DATA_4B;	 // Command
+	com.AddressSize = QSPI_ADDRESS_32_BITS;
+#else
+	com.Instruction = W25Q_READ_DATA;	 // Command
+	com.AddressSize = QSPI_ADDRESS_24_BITS;
+#endif
+	com.AddressMode = QSPI_ADDRESS_1_LINE;
+
+	com.Address = Addr;
+
+	com.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
+	com.AlternateBytes = QSPI_ALTERNATE_BYTES_NONE;
+	com.AlternateBytesSize = QSPI_ALTERNATE_BYTES_NONE;
+
+	com.DummyCycles = 0;
+	com.DataMode = QSPI_DATA_1_LINE;
+	com.NbData = len;
+
+	com.DdrMode = QSPI_DDR_MODE_DISABLE;
+	//com.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
+	com.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;
+
+	if (HAL_QSPI_Command(&hqspi, &com, HAL_QSPI_TIMEOUT_DEFAULT_VALUE)
+			!= HAL_OK)
+		return W25Q_SPI_ERR;
+
+	if (HAL_QSPI_Receive(&hqspi, buf, HAL_QSPI_TIMEOUT_DEFAULT_VALUE) != HAL_OK)
+		return W25Q_SPI_ERR;
+
+	return W25Q_OK;
+}
+
+/**
+ * @}
+ * @addtogroup W25Q_Write Write functions
+ * @brief Program/write operations - single data type variables or raw 8-bit blocks
+ * @{
+ */
+
+/**
+ * @brief W25Q Burst Wrap settings
+ *
+ * @attention Func in development
+ *
+ * @param[in] WrapSize Wrap size: 8/16/32/64 / 0 - disable
+ * @return W25Q_STATE enum
+ */
+W25Q_STATE W25Q_SetBurstWrap(u8_t WrapSize) {
+	return W25Q_PARAM_ERR;
+}
+
+/**
+ * @brief W25Q Program single Signed Byte
+ * Program signed 8-bit byte variable
+ *
+ * @param[in] buf Data to be written (single)
+ * @param[in] pageShift Byte shift inside page (0..255)
+ * @param[in] pageNum Page number (0..PAGE_COUNT-1)
+ * @return W25Q_STATE enum
+ */
+W25Q_STATE W25Q_ProgramSByte(i8_t buf, u8_t pageShift, u32_t pageNum) {
+	if (pageNum >= PAGE_COUNT)
+		return W25Q_PARAM_ERR;
+	u32_t rawAddr = page_to_addr(pageNum, pageShift);
+	u8_t data;
+	memcpy(&data, &buf, 1);
+	return W25Q_ProgramRaw(&data, 1, rawAddr);
+}
+
+/**
+ * @brief W25Q Program single Unsigned Byte
+ * Program unsigned 8-bit byte vairable
+ *
+ * @param[in] buf Data to be written (single)
+ * @param[in] pageShift Byte shift inside page (0..255)
+ * @param[in] pageNum Page number (0..PAGE_COUNT-1)
+ * @return W25Q_STATE enum
+ */
+W25Q_STATE W25Q_ProgramByte(u8_t buf, u8_t pageShift, u32_t pageNum) {
+	if (pageNum >= PAGE_COUNT)
+		return W25Q_PARAM_ERR;
+	u32_t rawAddr = page_to_addr(pageNum, pageShift);
+	u8_t data;
+	memcpy(&data, &buf, 1);
+	return W25Q_ProgramRaw(&data, 1, rawAddr);
+}
+
+/**
+ * @brief W25Q Program single Signed Word
+ * Program signed 16-bit word vairable
+ *
+ * @param[in] buf Data to be written (single)
+ * @param[in] pageShift Byte shift inside page (0..254)
+ * @param[in] pageNum Page number (0..PAGE_COUNT-1)
+ * @return W25Q_STATE enum
+ */
+W25Q_STATE W25Q_ProgramSWord(i16_t buf, u8_t pageShift, u32_t pageNum) {
+	if (pageNum >= PAGE_COUNT || pageShift > 256 - 2)
+		return W25Q_PARAM_ERR;
+	u32_t rawAddr = page_to_addr(pageNum, pageShift);
+	u8_t data[2];
+	memcpy(data, &buf, 2);
+	return W25Q_ProgramRaw(data, 2, rawAddr);
+}
+
+/**
+ * @brief W25Q Program single Unsigned Word
+ * Program unsigned 16-bit word vairable
+ *
+ * @param[in] buf Data to be written (single)
+ * @param[in] pageShift Byte shift inside page (0..254)
+ * @param[in] pageNum Page number (0..PAGE_COUNT-1)
+ * @return W25Q_STATE enum
+ */
+W25Q_STATE W25Q_ProgramWord(u16_t buf, u8_t pageShift, u32_t pageNum) {
+	if (pageNum >= PAGE_COUNT || pageShift > 256 - 2)
+		return W25Q_PARAM_ERR;
+	u32_t rawAddr = page_to_addr(pageNum, pageShift);
+	u8_t data[2];
+	memcpy(data, &buf, 2);
+	return W25Q_ProgramRaw(data, 2, rawAddr);
+}
+
+/**
+ * @brief W25Q Program single Signed Long
+ * Program signed 32-bit long vairable
+ *
+ * @param[in] buf Data to be written (single)
+ * @param[in] pageShift Byte shift inside page (0..252)
+ * @param[in] pageNum Page number (0..PAGE_COUNT-1)
+ * @return W25Q_STATE enum
+ */
+W25Q_STATE W25Q_ProgramSLong(i32_t buf, u8_t pageShift, u32_t pageNum) {
+	if (pageNum >= PAGE_COUNT || pageShift > 256 - 4)
+		return W25Q_PARAM_ERR;
+	u32_t rawAddr = page_to_addr(pageNum, pageShift);
+	u8_t data[4];
+	memcpy(data, &buf, 4);
+	return W25Q_ProgramRaw(data, 4, rawAddr);
+}
+
+/**
+ * @brief W25Q Program single Unigned Long
+ * Program unsigned 32-bit long vairable
+ *
+ * @param[in] buf Data to be written (single)
+ * @param[in] pageShift Byte shift inside page (0..252)
+ * @param[in] pageNum Page number (0..PAGE_COUNT-1)
+ * @return W25Q_STATE enum
+ */
+W25Q_STATE W25Q_ProgramLong(u32_t buf, u8_t pageShift, u32_t pageNum) {
+	if (pageNum >= PAGE_COUNT || pageShift > 256 - 4)
+		return W25Q_PARAM_ERR;
+	u32_t rawAddr = page_to_addr(pageNum, pageShift);
+	u8_t data[4];
+	memcpy(data, &buf, 4);
+	return W25Q_ProgramRaw(data, 4, rawAddr);
+}
+
+/**
+ * @brief W25Q Program any 8-bit data
+ * Program any 8-bit data to preffered page place
+ *
+ * @note Use memcpy to prepare data
+ * @param[in] buf Pointer to data to be written (single or array)
+ * @param[in] len Length of data (1..256)
+ * @param[in] pageShift Byte shift inside page (0..256 - len)
+ * @param[in] pageNum Page number (0..PAGE_COUNT-1)
+ * @return W25Q_STATE enum
+ */
+W25Q_STATE W25Q_ProgramData(u8_t *buf, u16_t len, u8_t pageShift, u32_t pageNum) {
+	if (pageNum >= PAGE_COUNT || len == 0 || len > 256 || pageShift > 256 - len)
+		return W25Q_PARAM_ERR;
+	u32_t rawAddr = page_to_addr(pageNum, pageShift);
+	return W25Q_ProgramRaw(buf, len, rawAddr);
+}
+
+/**
+ * @brief W25Q Program any 8-bit data to raw addr
+ * Program any 8-bit data to preffered chip address
+ *
+ * @note Address is in [byte] size
+ * @note Be carefull with page overrun
+ * @param[in] buf Pointer to data to be written (single or array)
+ * @param[in] data_len Length of data (1..256)
+ * @param[in] rawAddr Start address of chip's cell
+ * @return W25Q_STATE enum
+ */
+W25Q_STATE W25Q_ProgramRaw(u8_t *buf, u16_t data_len, u32_t rawAddr) {
+	if (data_len > 256 || data_len == 0)
+		return W25Q_PARAM_ERR;
+
+	while (W25Q_IsBusy() == W25Q_BUSY)
+		w25q_delay(1);
+
+	W25Q_STATE state = W25Q_WriteEnable(1);
+	if (state != W25Q_OK)
+		return state;
+
+	QSPI_CommandTypeDef com;
+
+	com.InstructionMode = QSPI_INSTRUCTION_1_LINE; // QSPI_INSTRUCTION_...
+#if MEM_FLASH_SIZE > 128U
+	com.Instruction = W25Q_PAGE_PROGRAM_QUAD_INP_4B;	 // Command
+	com.AddressSize = QSPI_ADDRESS_32_BITS;
+#else
+		com.Instruction = W25Q_PAGE_PROGRAM;	 // Command
+		com.AddressSize = QSPI_ADDRESS_24_BITS;
+	#endif
+	com.AddressMode = QSPI_ADDRESS_1_LINE;
+
+	com.Address = rawAddr;
+
+	com.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
+	com.AlternateBytes = QSPI_ALTERNATE_BYTES_NONE;
+	com.AlternateBytesSize = QSPI_ALTERNATE_BYTES_NONE;
+
+	com.DummyCycles = 0;
+	com.DataMode = QSPI_DATA_1_LINE;
+	com.NbData = data_len;
+
+	com.DdrMode = QSPI_DDR_MODE_DISABLE;
+	//com.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
+	com.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;
+
+	if (HAL_QSPI_Command(&hqspi, &com, HAL_QSPI_TIMEOUT_DEFAULT_VALUE)
+			!= HAL_OK)
+		return W25Q_SPI_ERR;
+
+	if (HAL_QSPI_Transmit(&hqspi, buf, HAL_QSPI_TIMEOUT_DEFAULT_VALUE)
+			!= HAL_OK)
+		return W25Q_SPI_ERR;
+
+	while (W25Q_IsBusy() == W25Q_BUSY)
+		w25q_delay(1);
+
+	return W25Q_OK;
+}
+
+/**
+ * @}
+ * @addtogroup W25Q_Erase Erase functions
+ * @brief Erase sector, blocks or whole chip
+ * @{
+ */
+
+/**
+ * @brief W25Q Sector erase (4KB)
+ * Minimal size operation to erase data
+ *
+ * @note Should be executed before writing
+ * @param[in] SectAddr Sector start address
+ * @return W25Q_STATE enum
+ */
+W25Q_STATE W25Q_EraseSector(u32_t SectAddr) {
+	if (SectAddr >= SECTOR_COUNT)
+		return W25Q_PARAM_ERR;
+
+	while (W25Q_IsBusy() == W25Q_BUSY)
+		w25q_delay(1);
+
+	u32_t rawAddr = SectAddr * MEM_SECTOR_SIZE * 1024U;
+
+	W25Q_STATE state = W25Q_WriteEnable(1);
+	if (state != W25Q_OK)
+		return state;
+
+	QSPI_CommandTypeDef com;
+
+	com.InstructionMode = QSPI_INSTRUCTION_1_LINE; // QSPI_INSTRUCTION_...
+#if MEM_FLASH_SIZE > 128U
+	com.Instruction = W25Q_SECTOR_ERASE_4B;	 // Command
+	com.AddressSize = QSPI_ADDRESS_32_BITS;
+#else
+	com.Instruction = W25Q_SECTOR_ERASE;	 // Command
+	com.AddressSize = QSPI_ADDRESS_24_BITS;
+#endif
+	com.AddressMode = QSPI_ADDRESS_1_LINE;
+
+	com.Address = rawAddr;
+
+	com.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
+	com.AlternateBytes = QSPI_ALTERNATE_BYTES_NONE;
+	com.AlternateBytesSize = QSPI_ALTERNATE_BYTES_NONE;
+
+	com.DummyCycles = 0;
+	com.DataMode = QSPI_DATA_NONE;
+	com.NbData = 0;
+
+	com.DdrMode = QSPI_DDR_MODE_DISABLE;
+	//com.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
+	com.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;
+
+	if (HAL_QSPI_Command(&hqspi, &com, HAL_QSPI_TIMEOUT_DEFAULT_VALUE)
+			!= HAL_OK)
+		return W25Q_SPI_ERR;
+
+	while (W25Q_IsBusy() == W25Q_BUSY)
+		w25q_delay(1);
+
+	return W25Q_OK;
+}
+
+/**
+ * @brief W25Q Block erase (32/64 KB)
+ * Func to erase big block
+ *
+ * @note Should be executed before writing
+ * @param[in] BlockAddr Block start address
+ * @param[in] size Size of block: 32KB or 64KB
+ * @return W25Q_STATE enum
+ */
+W25Q_STATE W25Q_EraseBlock(u32_t BlockAddr, u8_t size) {
+	if (size != 32 && size != 64)
+		return W25Q_PARAM_ERR;
+	if ((size == 64 && BlockAddr >= BLOCK_COUNT)
+			|| (size == 32 && BlockAddr >= BLOCK_COUNT * 2))
+		return W25Q_PARAM_ERR;
+
+	while (W25Q_IsBusy() == W25Q_BUSY)
+		w25q_delay(1);
+
+	u32_t rawAddr = BlockAddr * MEM_SECTOR_SIZE * 1024U * 16;
+	if (size == 32)
+		rawAddr /= 2;
+
+	W25Q_STATE state = W25Q_WriteEnable(1);
+	if (state != W25Q_OK)
+		return state;
+
+	QSPI_CommandTypeDef com;
+
+	com.InstructionMode = QSPI_INSTRUCTION_1_LINE; // QSPI_INSTRUCTION_...
+
+	if (size == 32) {
+		com.Instruction = W25Q_32KB_BLOCK_ERASE;	 // Command
+#if MEM_FLASH_SIZE > 128U
+		com.AddressSize = QSPI_ADDRESS_32_BITS;
+#else
+	com.AddressSize = QSPI_ADDRESS_24_BITS;
+#endif
+	} else if (size == 64) {
+#if MEM_FLASH_SIZE > 128U
+		com.Instruction = W25Q_64KB_BLOCK_ERASE_4B;	 // Command
+		com.AddressSize = QSPI_ADDRESS_32_BITS;
+#else
+	com.Instruction = W25Q_64KB_BLOCK_ERASE;	 // Command
+	com.AddressSize = QSPI_ADDRESS_24_BITS;
+#endif
+	}
+
+	com.AddressMode = QSPI_ADDRESS_1_LINE;
+
+	com.Address = rawAddr;
+
+	com.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
+	com.AlternateBytes = QSPI_ALTERNATE_BYTES_NONE;
+	com.AlternateBytesSize = QSPI_ALTERNATE_BYTES_NONE;
+
+	com.DummyCycles = 0;
+	com.DataMode = QSPI_DATA_NONE;
+	com.NbData = 0;
+
+	com.DdrMode = QSPI_DDR_MODE_DISABLE;
+	//com.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
+	com.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;
+
+	if (HAL_QSPI_Command(&hqspi, &com, HAL_QSPI_TIMEOUT_DEFAULT_VALUE)
+			!= HAL_OK)
+		return W25Q_SPI_ERR;
+
+	while (W25Q_IsBusy() == W25Q_BUSY)
+		w25q_delay(1);
+
+	return W25Q_OK;
+}
+
+/**
+ * @brief W25Q Chip erase
+ * Func to erase all the data on chip
+ *
+ * @note Should be executed before writing
+ * @param none
+ * @return W25Q_STATE enum
+ */
+W25Q_STATE W25Q_EraseChip(void) {
+	while (W25Q_IsBusy() == W25Q_BUSY)
+		w25q_delay(1);
+
+	W25Q_STATE state = W25Q_WriteEnable(1);
+	if (state != W25Q_OK)
+		return state;
+
+	QSPI_CommandTypeDef com;
+
+	com.InstructionMode = QSPI_INSTRUCTION_1_LINE; // QSPI_INSTRUCTION_...
+	com.Instruction = W25Q_CHIP_ERASE;	 // Command
+
+	com.AddressSize = QSPI_ADDRESS_NONE;
+	com.AddressMode = QSPI_ADDRESS_NONE;
+	com.Address = 0;
+
+	com.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
+	com.AlternateBytes = QSPI_ALTERNATE_BYTES_NONE;
+	com.AlternateBytesSize = QSPI_ALTERNATE_BYTES_NONE;
+
+	com.DummyCycles = 0;
+	com.DataMode = QSPI_DATA_NONE;
+	com.NbData = 0;
+
+	com.DdrMode = QSPI_DDR_MODE_DISABLE;
+	//com.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
+	com.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;
+
+	if (HAL_QSPI_Command(&hqspi, &com, HAL_QSPI_TIMEOUT_DEFAULT_VALUE)
+			!= HAL_OK)
+		return W25Q_SPI_ERR;
+
+	while (W25Q_IsBusy() == W25Q_BUSY)
+		w25q_delay(1);
+
+	return W25Q_OK;
+}
+
+/**
+ * @}
+ * @addtogroup W25Q_SUS Suspend functions
+ * @brief Pause/resume operations
+ * @{
+ */
+
+/**
+ * @brief W25Q Programm/Erase Suspend
+ * Pause programm or suspend operatiom
+ *
+ * @note SUS == 0 && BUSY == 1, otherwise ignored
+ * @note Power loose during suspend state may corrupt data
+ * @param none
+ * @return W25Q_STATE enum
+ */
+W25Q_STATE W25Q_ProgSuspend(void) {
+	if (W25Q_BUSY != W25Q_IsBusy())
+		return W25Q_CHIP_IGNORE;
+
+	QSPI_CommandTypeDef com;
+
+	com.InstructionMode = QSPI_INSTRUCTION_1_LINE; // QSPI_INSTRUCTION_...
+	com.Instruction = W25Q_ERASEPROG_SUSPEND;	 // Command
+
+	com.AddressSize = QSPI_ADDRESS_NONE;
+	com.AddressMode = QSPI_ADDRESS_NONE;
+	com.Address = 0;
+
+	com.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
+	com.AlternateBytes = QSPI_ALTERNATE_BYTES_NONE;
+	com.AlternateBytesSize = QSPI_ALTERNATE_BYTES_NONE;
+
+	com.DummyCycles = 0;
+	com.DataMode = QSPI_DATA_NONE;
+	com.NbData = 0;
+
+	com.DdrMode = QSPI_DDR_MODE_DISABLE;
+	//com.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
+	com.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;
+
+	if (HAL_QSPI_Command(&hqspi, &com, HAL_QSPI_TIMEOUT_DEFAULT_VALUE)
+			!= HAL_OK)
+		return W25Q_SPI_ERR;
+
+	return W25Q_OK;
+}
+
+/**
+ * @brief W25Q Programm/Erase Resume
+ * Resume suspended state
+ *
+ * @note SUS == 1, otherwise ignored
+ * @param none
+ * @return W25Q_STATE enum
+ */
+W25Q_STATE W25Q_ProgResume(void) {
+
+	W25Q_STATE state = W25Q_ReadStatusStruct(NULL);
+	if (state != W25Q_OK)
+		return state;
+
+	if (w25q_status.SUS != 1)
+		return W25Q_CHIP_IGNORE;
+
+	QSPI_CommandTypeDef com;
+
+	com.InstructionMode = QSPI_INSTRUCTION_1_LINE; // QSPI_INSTRUCTION_...
+	com.Instruction = W25Q_ERASEPROG_RESUME;	 // Command
+
+	com.AddressSize = QSPI_ADDRESS_NONE;
+	com.AddressMode = QSPI_ADDRESS_NONE;
+	com.Address = 0;
+
+	com.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
+	com.AlternateBytes = QSPI_ALTERNATE_BYTES_NONE;
+	com.AlternateBytesSize = QSPI_ALTERNATE_BYTES_NONE;
+
+	com.DummyCycles = 0;
+	com.DataMode = QSPI_DATA_NONE;
+	com.NbData = 0;
+
+	com.DdrMode = QSPI_DDR_MODE_DISABLE;
+	//com.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
+	com.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;
+
+	if (HAL_QSPI_Command(&hqspi, &com, HAL_QSPI_TIMEOUT_DEFAULT_VALUE)
+			!= HAL_OK)
+		return W25Q_SPI_ERR;
+
+	return W25Q_OK;
+}
+
+/**
+ * @}
+ * @addtogroup W25Q_Sleep Sleep functions
+ * @brief Баю-бай, ток засыпай
+ * @{
+ */
+
+/**
+ * @brief W25Q Sleep / Power Down
+ * Set chip to low-power state
+ *
+ * @note Use WakeUP or ReadID
+ * @param none
+ * @return W25Q_STATE enum
+ */
+W25Q_STATE W25Q_Sleep(void) {
+	QSPI_CommandTypeDef com;
+
+	com.InstructionMode = QSPI_INSTRUCTION_1_LINE; // QSPI_INSTRUCTION_...
+	com.Instruction = W25Q_POWERDOWN;	 // Command
+
+	com.AddressMode = QSPI_ADDRESS_NONE;
+	com.AddressSize = QSPI_ADDRESS_NONE;
+	com.Address = 0x0U;
+
+	com.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
+	com.AlternateBytes = QSPI_ALTERNATE_BYTES_NONE;
+	com.AlternateBytesSize = QSPI_ALTERNATE_BYTES_NONE;
+
+	com.DummyCycles = 0;
+	com.DataMode = QSPI_DATA_NONE;
+	com.NbData = 0;
+
+	com.DdrMode = QSPI_DDR_MODE_DISABLE;
+	//com.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
+	com.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;
+
+	if (HAL_QSPI_Command(&hqspi, &com, HAL_QSPI_TIMEOUT_DEFAULT_VALUE)
+			!= HAL_OK) {
+		return W25Q_SPI_ERR;
+	}
+	w25q_delay(1); // Give a little time to sleep
+
+	w25q_status.SLEEP = 1;
+
+	return W25Q_OK;
+}
+
+/**
+ * @brief W25Q WakeUP
+ * Wake UP function
+ *
+ * @param none
+ * @return W25Q_STATE enum
+ */
+W25Q_STATE W25Q_WakeUP(void) {
+	QSPI_CommandTypeDef com;
+
+	com.InstructionMode = QSPI_INSTRUCTION_1_LINE; // QSPI_INSTRUCTION_...
+	com.Instruction = W25Q_POWERUP;	 // Command
+
+	com.AddressMode = QSPI_ADDRESS_NONE;
+	com.AddressSize = QSPI_ADDRESS_NONE;
+	com.Address = 0x0U;
+
+	com.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
+	com.AlternateBytes = QSPI_ALTERNATE_BYTES_NONE;
+	com.AlternateBytesSize = QSPI_ALTERNATE_BYTES_NONE;
+
+	com.DummyCycles = 0;
+	com.DataMode = QSPI_DATA_NONE;
+	com.NbData = 0;
+
+	com.DdrMode = QSPI_DDR_MODE_DISABLE;
+	//com.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
+	com.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;
+
+	if (HAL_QSPI_Command(&hqspi, &com, HAL_QSPI_TIMEOUT_DEFAULT_VALUE)
+			!= HAL_OK) {
+		return W25Q_SPI_ERR;
+	}
+	w25q_delay(1); // Give a little time to wake
+
+	return W25Q_OK;
+}
+
+/**
+ * @}
+ * @addtogroup W25Q_ID ID functions
+ * @brief Who am I? Хто я?
+ * @{
+ */
+
+/**
+ * @brief W25Q Read ID
+ * Function for reading chip ID
+ *
+ * @param[out] buf Pointer to output data (1 byte)
+ * @return W25Q_STATE enum
+ */
+W25Q_STATE W25Q_ReadID(u8_t *buf) {
+	QSPI_CommandTypeDef com;
+
+	com.InstructionMode = QSPI_INSTRUCTION_1_LINE; // QSPI_INSTRUCTION_...
+	com.Instruction = W25Q_DEVID;	 // Command
+
+	com.AddressMode = QSPI_ADDRESS_1_LINE;
+	com.AddressSize = QSPI_ADDRESS_24_BITS;
+	com.Address = 0x0U;
+
+	com.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
+	com.AlternateBytes = QSPI_ALTERNATE_BYTES_NONE;
+	com.AlternateBytesSize = QSPI_ALTERNATE_BYTES_NONE;
+
+	com.DummyCycles = 0;
+	com.DataMode = QSPI_DATA_1_LINE;
+	com.NbData = 1;
+
+	com.DdrMode = QSPI_DDR_MODE_DISABLE;
+	//com.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
+	com.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;
+
+	if (HAL_QSPI_Command(&hqspi, &com, HAL_QSPI_TIMEOUT_DEFAULT_VALUE)
+			!= HAL_OK) {
+		return W25Q_SPI_ERR;
+	}
+	if (HAL_QSPI_Receive(&hqspi, buf, HAL_QSPI_TIMEOUT_DEFAULT_VALUE)
+			!= HAL_OK) {
+		return W25Q_SPI_ERR;
+	}
+	return W25Q_OK;
+}
+
+/**
+ * @brief W25Q Read chip Full ID
+ * Read Manufacturer ID + Device ID
+ *
+ * @attention Func in development
+ * @param[out] buf Pointer to data from ID register
+ * @return W25Q_STATE enum
+ */
+W25Q_STATE W25Q_ReadFullID(u8_t *buf) {
+	return W25Q_PARAM_ERR;
+}
+
+/**
+ * @brief W25Q Read chip UID
+ * Read Unique ID
+ *
+ * @attention Func in development
+ * @param[out] buf Pointer to data from ID register
+ * @return W25Q_STATE enum
+ */
+W25Q_STATE W25Q_ReadUID(u8_t *buf) {
+	return W25Q_PARAM_ERR;
+}
+
+/**
+ * @brief W25Q Read JEDEC ID
+ * Read ID by JEDEC standards
+ *
+ * @attention Func in development
+ * @param[out] buf Pointer to data from ID register
+ * @return W25Q_STATE enum
+ */
+W25Q_STATE W25Q_ReadJEDECID(u8_t *buf) {
+	return W25Q_PARAM_ERR;
+}
+
+/**
+ * @brief W25Q Read SFDP Register
+ * Read device descriptor by SFDP standard
+ *
+ * @attention Func in development
+ * @param[out] buf Pointer to data from ID register
+ * @return W25Q_STATE enum
+ */
+W25Q_STATE W25Q_ReadSFDPRegister(u8_t *buf) {
+	return W25Q_PARAM_ERR;
+}
+
+/**
+ * @}
+ * @addtogroup W25Q_Secure Security register functions
+ * @brief Shhh, security
+ * @{
+ */
+
+/**
+ * @brief W25Q Erase Security Registers
+ * Clean security registers (one or all)
+ *
+ * @attention Func in development
+ * @param[in] numReg Number of security register (1..3 / 0-all)
+ * @return W25Q_STATE enum
+ */
+W25Q_STATE W25Q_EraseSecurityRegisters(u8_t numReg) {
+	return W25Q_PARAM_ERR;
+}
+
+/**
+ * @brief W25Q Program Security Register
+ * Write data to security reg
+ *
+ * @attention Func in development
+ * @param[in] buf Pointer to 8-bit data bufer
+ * @param[in] numReg Number of security register (1..3)
+ * @param[in] byteAddr Byte addr in register (0..255)
+ * @return W25Q_STATE enum
+ */
+W25Q_STATE W25Q_ProgSecurityRegisters(u8_t *buf, u8_t numReg, u8_t byteAddr) {
+	return W25Q_PARAM_ERR;
+}
+
+/**
+ * @brief W25Q Read Security Register
+ * Read data from security reg
+ *
+ * @attention Func in development
+ * @param[out] buf Pointer to 8-bit data bufer
+ * @param[in] numReg Number of security register (1..3)
+ * @param[in] byteAddr Byte addr in register (0..255)
+ * @return W25Q_STATE enum
+ */
+W25Q_STATE W25Q_ReadSecurityRegisters(u8_t *buf, u8_t numReg, u8_t byteAddr) {
+	return W25Q_PARAM_ERR;
+}
+
+/**
+ * @}
+ * @addtogroup W25Q_Protect Read-only protection functions
+ * @brief No writing! Protect block or whole chip
+ * @{
+ */
+
+/**
+ * @brief W25Q 4K Block lock
+ * Set read-only status to 4K block
+ *
+ * @attention Func in development
+ * @param[in] Addr Block address
+ * @param[in] enable 1-Enable/0-Disable
+ * @return W25Q_STATE enum
+ */
+W25Q_STATE W25Q_BlockReadOnly(u32_t Addr, bool enable) {
+	return W25Q_PARAM_ERR;
+}
+
+/**
+ * @brief W25Q 4K Block lock CHECK
+ * Check read-only status from 4K block
+ *
+ * @attention Func in development
+ * @param[out] state Block read-only status (1-locked/0-unlocked)
+ * @param[in] Addr Block address
+ * @return W25Q_STATE enum
+ */
+W25Q_STATE W25Q_BlockReadOnlyCheck(bool *state, u32_t Addr) {
+	return W25Q_PARAM_ERR;
+}
+
+/**
+ * @brief W25Q Global read-only lock
+ * Set read-only status to the whole chip
+ *
+ * @attention Func in development
+ * @param[in] enable 1-enable/0-disable
+ * @return W25Q_STATE enum
+ */
+W25Q_STATE W25Q_GlobalReadOnly(bool enable){
+	return W25Q_PARAM_ERR;
+}
+
+/**
+ * @}
+ * @addtogroup W25Q_RST Reset functions
+ * @brief Reboot the chip
+ * @{
+ */
+
+/**
+ * @brief W25Q Software Reset
+ * Reset by register (not by external GPIO pin)
+ *
+ * @param[in] force Enable/disable (0/1) force reset - wait for BUSY and SUSpend
+ * @return W25Q_STATE enum
+ */
+W25Q_STATE W25Q_SwReset(bool force) {
+
+	W25Q_STATE state;	// temp status reg
+
+	state = W25Q_ReadStatusStruct(NULL); // read settings
+	if (state != W25Q_OK)
+		return state;
+
+	if (!force || w25q_status.BUSY || w25q_status.SUS) // if busy or suspend
+		return W25Q_CHIP_ERR;
+
+	if (force) {
+		while (W25Q_IsBusy() == W25Q_BUSY)
+			w25q_delay(1);
+		if (w25q_status.SUS)
+			W25Q_ProgResume();
+	}
+
+	QSPI_CommandTypeDef com;
+
+	com.InstructionMode = QSPI_INSTRUCTION_1_LINE; // QSPI_INSTRUCTION_...
+	com.Instruction = W25Q_ENABLE_RST;	 // Command
+
+	com.AddressMode = QSPI_ADDRESS_NONE;
+	com.AddressSize = QSPI_ADDRESS_NONE;
+	com.Address = 0x0U;
+
+	com.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
+	com.AlternateBytes = QSPI_ALTERNATE_BYTES_NONE;
+	com.AlternateBytesSize = QSPI_ALTERNATE_BYTES_NONE;
+
+	com.DummyCycles = 0;
+	com.DataMode = QSPI_DATA_NONE;
+	com.NbData = 0;
+
+	com.DdrMode = QSPI_DDR_MODE_DISABLE;
+	//com.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
+	com.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;
+
+	if (HAL_QSPI_Command(&hqspi, &com, HAL_QSPI_TIMEOUT_DEFAULT_VALUE)
+			!= HAL_OK) {
+		return W25Q_SPI_ERR;
+	}
+	w25q_delay(1); // Give a little time to prepare
+
+	com.Instruction = W25Q_RESET;
+
+	if (HAL_QSPI_Command(&hqspi, &com, HAL_QSPI_TIMEOUT_DEFAULT_VALUE)
+			!= HAL_OK) {
+		return W25Q_SPI_ERR;
+	}
+	w25q_delay(5); // Give a little time to reset
+
+	state = W25Q_Init();
+
+	return state;
+}
+/// @}
+// addgroup{
+/// @}
+// Public {
+
+/**
+ * @brief W25Q Toggle WEL bit
+ * Toggle write enable latch
+ *
+ * @param[in] enable 1-enable write/0-disable write
+ * @return W25Q_STATE enum
+ */
+W25Q_STATE W25Q_WriteEnable(bool enable) {
+	QSPI_CommandTypeDef com;
+
+	com.InstructionMode = QSPI_INSTRUCTION_1_LINE; // QSPI_INSTRUCTION_...
+	com.Instruction = enable ? W25Q_WRITE_ENABLE : W25Q_WRITE_DISABLE;
+
+	com.AddressMode = QSPI_ADDRESS_NONE;
+	com.AddressSize = QSPI_ADDRESS_NONE;
+	com.Address = 0x0U;
+
+	com.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
+	com.AlternateBytes = QSPI_ALTERNATE_BYTES_NONE;
+	com.AlternateBytesSize = QSPI_ALTERNATE_BYTES_NONE;
+
+	com.DummyCycles = 0;
+	com.DataMode = QSPI_DATA_NONE;
+	com.NbData = 0;
+
+	com.DdrMode = QSPI_DDR_MODE_DISABLE;
+	//com.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
+	com.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;
+
+	if (HAL_QSPI_Command(&hqspi, &com, HAL_QSPI_TIMEOUT_DEFAULT_VALUE)
+			!= HAL_OK) {
+		return W25Q_SPI_ERR;
+	}
+	w25q_delay(1); // Give a little time to sleep
+
+	w25q_status.WEL = 1;
+
+	return W25Q_OK;
+}
+
+/**
+ * @brief W25Q Toggle 4-byte mode
+ *
+ * @note Affects only ADS bit
+ * @param[in] enable 1-enable/0-disable
+ * @return W25Q_STATE enum
+ */
+W25Q_STATE W25Q_Enter4ByteMode(bool enable) {
+
+	while (W25Q_IsBusy() == W25Q_BUSY)
+		w25q_delay(1);
+
+	QSPI_CommandTypeDef com;
+
+	com.InstructionMode = QSPI_INSTRUCTION_1_LINE; // QSPI_INSTRUCTION_...
+	com.Instruction = enable ? W25Q_ENABLE_4B_MODE : W25Q_DISABLE_4B_MODE;
+
+	com.AddressMode = QSPI_ADDRESS_NONE;
+	com.AddressSize = QSPI_ADDRESS_NONE;
+	com.Address = 0x0U;
+
+	com.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
+	com.AlternateBytes = QSPI_ALTERNATE_BYTES_NONE;
+	com.AlternateBytesSize = QSPI_ALTERNATE_BYTES_NONE;
+
+	com.DummyCycles = 0;
+	com.DataMode = QSPI_DATA_NONE;
+	com.NbData = 0;
+
+	com.DdrMode = QSPI_DDR_MODE_DISABLE;
+	//com.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
+	com.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;
+
+	if (HAL_QSPI_Command(&hqspi, &com, HAL_QSPI_TIMEOUT_DEFAULT_VALUE)
+			!= HAL_OK) {
+		return W25Q_SPI_ERR;
+	}
+
+	w25q_delay(1); // Give a little time to set command
+
+	return W25Q_ReadStatusStruct(NULL);
+}
+
+/**
+ * @brief W25Q Set extended byte
+ * Extended byte in 3-byte mode
+ *
+ * @attention Func in development
+ * @param[in] Addr 4th byte of addr
+ * @return W25Q_STATE enum
+ */
+W25Q_STATE W25Q_SetExtendedAddr(u8_t Addr) {
+	return W25Q_PARAM_ERR;
+}
+
+/**
+ * @brief W25Q Get extended byte
+ * Read extended byte in 3-byte mode
+ *
+ * @attention Func in development
+ * @param[out] Addr 4th byte of addr
+ * @return W25Q_STATE enum
+ */
+W25Q_STATE W25Q_GetExtendedAddr(u8_t *outAddr) {
+	return W25Q_PARAM_ERR;
+}
+
+/**
+ * @brief Page to address
+ * Translate page to byte-address
+ *
+ * @param[in] pageNum Number of page
+ * @param[in] pageShift Byte to shift inside page
+ * @return byte-address
+ */
+u32_t page_to_addr(u32_t pageNum, u8_t pageShift) {
+	return pageNum * MEM_PAGE_SIZE + pageShift;
+}
+
+///@}