bluenrg-x: added support for BlueNRG-LP device

Extended bluenrg-x flash driver with BlueNRG-LP flash controller.
Changes include:
- register set for the flash controller
- made software structure prone to support more easily future devices
- updated target config file

Change-Id: I2e2dc70db32cf98c62e3a43f2e44a4600a25ac5b
Signed-off-by: luca vinci <luca.vinci@st.com>
Reviewed-on: http://openocd.zylin.com/5343
Tested-by: jenkins
Reviewed-by: Tomas Vanek <vanekt@fbl.cz>

diff --git a/contrib/loaders/flash/bluenrg-x/Makefile b/contrib/loaders/flash/bluenrg-x/Makefile
index 1a5cfc013f420816a77af5b4c3071a267f74cbc3..ce86e481a49affdbe47eb749a1027af92c555796 100644 (file)
--- a/contrib/loaders/flash/bluenrg-x/Makefile
+++ b/contrib/loaders/flash/bluenrg-x/Makefile
@@ -8,15 +8,18 @@ OBJDUMP=$(CROSS_COMPILE)objdump
 
 CFLAGS =  -c -mthumb -mcpu=cortex-m0 -O3 -g
 
 
 CFLAGS =  -c -mthumb -mcpu=cortex-m0 -O3 -g
 

-all: bluenrg-x_write.inc
+all:bluenrg-2_write.inc bluenrg-lp_write.inc

 
 .PHONY: clean
 
 .INTERMEDIATE: bluenrg-x_write.o
 
 
 .PHONY: clean
 
 .INTERMEDIATE: bluenrg-x_write.o
 

-%.o: %.c
+bluenrg-2_write.o: bluenrg-x_write.c

        $(CC) $(CFLAGS) -Wall -Wextra -Wa,-adhln=$*.lst $< -o $@
 
        $(CC) $(CFLAGS) -Wall -Wextra -Wa,-adhln=$*.lst $< -o $@
 

+bluenrg-lp_write.o: bluenrg-x_write.c
+       $(CC) $(CFLAGS) -D BLUENRG_LP -Wall -Wextra -Wa,-adhln=$*.lst $< -o $@
+

 %.bin: %.o
        $(OBJCOPY) -Obinary $< $@
 
 %.bin: %.o
        $(OBJCOPY) -Obinary $< $@
 

diff --git a/contrib/loaders/flash/bluenrg-x/bluenrg-2_write.inc b/contrib/loaders/flash/bluenrg-x/bluenrg-2_write.inc
new file mode 100644 (file)
index 0000000..1ce4c86
--- /dev/null
+++ b/contrib/loaders/flash/bluenrg-x/bluenrg-2_write.inc
@@ -0,0 +1,18 @@
+/* Autogenerated with ../../../../src/helper/bin2char.sh */
+0x05,0x93,0x43,0x68,0x06,0x00,0x09,0x93,0x05,0x9b,0x07,0x91,0x06,0x92,0x34,0x4d,
+0x34,0x4c,0x00,0x2b,0x5d,0xd0,0x72,0x68,0x33,0x68,0x9a,0x42,0xfb,0xd0,0x33,0x68,
+0x00,0x2b,0x56,0xd0,0x72,0x68,0x33,0x68,0x9a,0x42,0x53,0xd9,0x73,0x68,0x07,0x9a,
+0xd3,0x1a,0x0f,0x2b,0xef,0xdd,0x00,0x21,0x2b,0x4a,0x03,0x93,0x11,0x60,0x00,0x2b,
+0x37,0xd0,0x2a,0x4a,0x06,0x9b,0x94,0x46,0x63,0x44,0x18,0x00,0x73,0x68,0x08,0x96,
+0x04,0x93,0x1a,0x00,0x26,0x4b,0x99,0x46,0x26,0x4b,0x98,0x46,0x26,0x4b,0x9c,0x46,
+0x26,0x4b,0x9b,0x46,0x26,0x4b,0x9a,0x46,0x01,0x23,0x91,0x68,0x17,0x68,0x01,0x91,
+0xd1,0x68,0x56,0x68,0x02,0x91,0x3f,0x21,0x29,0x60,0x81,0x03,0x09,0x0c,0x21,0x60,
+0x49,0x46,0x0f,0x60,0x47,0x46,0x3e,0x60,0x66,0x46,0x01,0x99,0x31,0x60,0x5e,0x46,
+0x02,0x99,0x31,0x60,0x51,0x46,0xcc,0x26,0x0e,0x60,0x29,0x68,0x0b,0x42,0xfc,0xd0,
+0x04,0x99,0x03,0x9e,0x10,0x32,0x10,0x30,0x51,0x1a,0x8e,0x42,0xdd,0xd8,0x08,0x9e,
+0x72,0x60,0x03,0x9a,0x06,0x9b,0x94,0x46,0x63,0x44,0x06,0x93,0x07,0x9a,0x73,0x68,
+0x9a,0x42,0x01,0xd8,0x09,0x9b,0x73,0x60,0x05,0x9b,0x03,0x9a,0x9b,0x1a,0x05,0x93,
+0xa1,0xd1,0x00,0xbe,0x33,0x68,0x72,0x68,0x9b,0x1a,0xaa,0xd5,0x9b,0xe7,0xc0,0x46,
+0x10,0x00,0x10,0x40,0x18,0x00,0x10,0x40,0x0c,0x00,0x10,0x40,0x00,0x00,0xfc,0xef,
+0x40,0x00,0x10,0x40,0x44,0x00,0x10,0x40,0x48,0x00,0x10,0x40,0x4c,0x00,0x10,0x40,
+0x00,0x00,0x10,0x40,

diff --git a/contrib/loaders/flash/bluenrg-x/bluenrg-lp_write.inc b/contrib/loaders/flash/bluenrg-x/bluenrg-lp_write.inc
new file mode 100644 (file)
index 0000000..cc9b2a1
--- /dev/null
+++ b/contrib/loaders/flash/bluenrg-x/bluenrg-lp_write.inc
@@ -0,0 +1,18 @@
+/* Autogenerated with ../../../../src/helper/bin2char.sh */
+0x05,0x93,0x43,0x68,0x06,0x00,0x09,0x93,0x05,0x9b,0x07,0x91,0x06,0x92,0x34,0x4d,
+0x34,0x4c,0x00,0x2b,0x5d,0xd0,0x72,0x68,0x33,0x68,0x9a,0x42,0xfb,0xd0,0x33,0x68,
+0x00,0x2b,0x56,0xd0,0x72,0x68,0x33,0x68,0x9a,0x42,0x53,0xd9,0x73,0x68,0x07,0x9a,
+0xd3,0x1a,0x0f,0x2b,0xef,0xdd,0x00,0x21,0x2b,0x4a,0x03,0x93,0x11,0x60,0x00,0x2b,
+0x37,0xd0,0x2a,0x4a,0x06,0x9b,0x94,0x46,0x63,0x44,0x18,0x00,0x73,0x68,0x08,0x96,
+0x04,0x93,0x1a,0x00,0x26,0x4b,0x99,0x46,0x26,0x4b,0x98,0x46,0x26,0x4b,0x9c,0x46,
+0x26,0x4b,0x9b,0x46,0x26,0x4b,0x9a,0x46,0x01,0x23,0x91,0x68,0x17,0x68,0x01,0x91,
+0xd1,0x68,0x56,0x68,0x02,0x91,0x3f,0x21,0x29,0x60,0x81,0x03,0x09,0x0c,0x21,0x60,
+0x49,0x46,0x0f,0x60,0x47,0x46,0x3e,0x60,0x66,0x46,0x01,0x99,0x31,0x60,0x5e,0x46,
+0x02,0x99,0x31,0x60,0x51,0x46,0xcc,0x26,0x0e,0x60,0x29,0x68,0x0b,0x42,0xfc,0xd0,
+0x04,0x99,0x03,0x9e,0x10,0x32,0x10,0x30,0x51,0x1a,0x8e,0x42,0xdd,0xd8,0x08,0x9e,
+0x72,0x60,0x03,0x9a,0x06,0x9b,0x94,0x46,0x63,0x44,0x06,0x93,0x07,0x9a,0x73,0x68,
+0x9a,0x42,0x01,0xd8,0x09,0x9b,0x73,0x60,0x05,0x9b,0x03,0x9a,0x9b,0x1a,0x05,0x93,
+0xa1,0xd1,0x00,0xbe,0x33,0x68,0x72,0x68,0x9b,0x1a,0xaa,0xd5,0x9b,0xe7,0xc0,0x46,
+0x10,0x10,0x00,0x40,0x18,0x10,0x00,0x40,0x0c,0x10,0x00,0x40,0x00,0x00,0xfc,0xef,
+0x40,0x10,0x00,0x40,0x44,0x10,0x00,0x40,0x48,0x10,0x00,0x40,0x4c,0x10,0x00,0x40,
+0x00,0x10,0x00,0x40,

diff --git a/contrib/loaders/flash/bluenrg-x/bluenrg-x_write.c b/contrib/loaders/flash/bluenrg-x/bluenrg-x_write.c
index 3dd17b2fcfadd98ab05e491a56dc179e51472c53..695f9145ae9f60f6a5541d90411072c0dc808700 100644 (file)
--- a/contrib/loaders/flash/bluenrg-x/bluenrg-x_write.c
+++ b/contrib/loaders/flash/bluenrg-x/bluenrg-x_write.c
@@ -14,6 +14,21 @@
 #define ERR_VERIFY_FAILED   7
 
 /* Flash Controller defines ---------------------------------------------------*/
 #define ERR_VERIFY_FAILED   7
 
 /* Flash Controller defines ---------------------------------------------------*/

+#ifdef BLUENRG_LP
+#define FLASH_REG_COMMAND ((volatile uint32_t *)0x40001000)
+#define FLASH_REG_CONFIG  ((volatile uint32_t *)0x40001004)
+#define FLASH_REG_IRQSTAT ((volatile uint32_t *)0x40001008)
+#define FLASH_REG_IRQMASK ((volatile uint32_t *)0x4000100C)
+#define FLASH_REG_IRQRAW  ((volatile uint32_t *)0x40001010)
+#define FLASH_REG_ADDRESS ((volatile uint32_t *)0x40001018)
+#define FLASH_REG_UNLOCKM ((volatile uint32_t *)0x4000101C)
+#define FLASH_REG_UNLOCKL ((volatile uint32_t *)0x40001020)
+#define FLASH_REG_DATA0   ((volatile uint32_t *)0x40001040)
+#define FLASH_REG_DATA1   ((volatile uint32_t *)0x40001044)
+#define FLASH_REG_DATA2   ((volatile uint32_t *)0x40001048)
+#define FLASH_REG_DATA3   ((volatile uint32_t *)0x4000104C)
+#define FLASH_SIZE_REG 0x40001014
+#else

 #define FLASH_REG_COMMAND ((volatile uint32_t *)0x40100000)
 #define FLASH_REG_CONFIG  ((volatile uint32_t *)0x40100004)
 #define FLASH_REG_IRQSTAT ((volatile uint32_t *)0x40100008)
 #define FLASH_REG_COMMAND ((volatile uint32_t *)0x40100000)
 #define FLASH_REG_CONFIG  ((volatile uint32_t *)0x40100004)
 #define FLASH_REG_IRQSTAT ((volatile uint32_t *)0x40100008)


@@ -22,11 +37,12 @@
 #define FLASH_REG_ADDRESS ((volatile uint32_t *)0x40100018)
 #define FLASH_REG_UNLOCKM ((volatile uint32_t *)0x4010001C)
 #define FLASH_REG_UNLOCKL ((volatile uint32_t *)0x40100020)
 #define FLASH_REG_ADDRESS ((volatile uint32_t *)0x40100018)
 #define FLASH_REG_UNLOCKM ((volatile uint32_t *)0x4010001C)
 #define FLASH_REG_UNLOCKL ((volatile uint32_t *)0x40100020)

-#define FLASH_REG_DATA0    ((volatile uint32_t *)0x40100040)
-#define FLASH_REG_DATA1    ((volatile uint32_t *)0x40100044)
-#define FLASH_REG_DATA2    ((volatile uint32_t *)0x40100048)
-#define FLASH_REG_DATA3    ((volatile uint32_t *)0x4010004C)
+#define FLASH_REG_DATA0   ((volatile uint32_t *)0x40100040)
+#define FLASH_REG_DATA1   ((volatile uint32_t *)0x40100044)
+#define FLASH_REG_DATA2   ((volatile uint32_t *)0x40100048)
+#define FLASH_REG_DATA3   ((volatile uint32_t *)0x4010004C)

 #define FLASH_SIZE_REG 0x40100014
 #define FLASH_SIZE_REG 0x40100014

+#endif

 
 #define MFB_MASS_ERASE 0x01
 #define MFB_PAGE_ERASE 0x02
 
 #define MFB_MASS_ERASE 0x01
 #define MFB_PAGE_ERASE 0x02


@@ -70,7 +86,7 @@ static inline __attribute__((always_inline)) uint32_t flashWrite(uint32_t addres
                /* Clear the IRQ flags */
                *FLASH_REG_IRQRAW = 0x0000003F;
                /* Load the flash address to write */
                /* Clear the IRQ flags */
                *FLASH_REG_IRQRAW = 0x0000003F;
                /* Load the flash address to write */

-               *FLASH_REG_ADDRESS = (uint16_t)((address + index) >> 2);
+               *FLASH_REG_ADDRESS = (uint16_t)((address + index - MFB_BOTTOM) >> 2);

                /* Prepare and load the data to flash */
                *FLASH_REG_DATA0 = flash_word[0];
                *FLASH_REG_DATA1 = flash_word[1];
                /* Prepare and load the data to flash */
                *FLASH_REG_DATA0 = flash_word[0];
                *FLASH_REG_DATA1 = flash_word[1];

diff --git a/contrib/loaders/flash/bluenrg-x/bluenrg-x_write.inc b/contrib/loaders/flash/bluenrg-x/bluenrg-x_write.inc
deleted file mode 100644 (file)
index 47f3312..0000000
--- a/contrib/loaders/flash/bluenrg-x/bluenrg-x_write.inc
+++ /dev/null
@@ -1,18 +0,0 @@
-/* Autogenerated with ../../../../src/helper/bin2char.sh */
-0x05,0x93,0x43,0x68,0x05,0x00,0x07,0x93,0x05,0x9b,0x06,0x91,0x03,0x92,0x35,0x4c,
-0x00,0x2b,0x5c,0xd0,0x6a,0x68,0x2b,0x68,0x9a,0x42,0xfb,0xd0,0x2b,0x68,0x00,0x2b,
-0x55,0xd0,0x6a,0x68,0x2b,0x68,0x9a,0x42,0x52,0xd9,0x6b,0x68,0x06,0x9a,0xd3,0x1a,
-0x09,0x93,0x09,0x9b,0x0f,0x2b,0xed,0xdd,0x00,0x21,0x09,0x9b,0x04,0x93,0x1a,0x1e,
-0x29,0x4b,0x19,0x60,0x32,0xd0,0x29,0x4b,0x00,0x20,0x98,0x46,0x28,0x4b,0x6a,0x68,
-0x9c,0x46,0x28,0x4b,0x28,0x4e,0x9b,0x46,0x28,0x4b,0x9a,0x46,0x28,0x4b,0x99,0x46,
-0x01,0x23,0x51,0x68,0x17,0x68,0x00,0x91,0x91,0x68,0x01,0x91,0xd1,0x68,0x02,0x91,
-0x3f,0x21,0x21,0x60,0x03,0x99,0x09,0x18,0x89,0x03,0x09,0x0c,0x31,0x60,0x41,0x46,
-0x0f,0x60,0x67,0x46,0x00,0x99,0x39,0x60,0x5f,0x46,0x01,0x99,0x39,0x60,0x57,0x46,
-0x02,0x99,0x39,0x60,0x49,0x46,0xcc,0x27,0x0f,0x60,0x21,0x68,0x0b,0x42,0xfc,0xd0,
-0x04,0x99,0x10,0x32,0x10,0x30,0x6a,0x60,0x81,0x42,0xda,0xd8,0x03,0x9a,0x09,0x9b,
-0x94,0x46,0x9c,0x44,0x63,0x46,0x06,0x9a,0x03,0x93,0x6b,0x68,0x9a,0x42,0x01,0xd8,
-0x07,0x9b,0x6b,0x60,0x05,0x9a,0x09,0x9b,0xd3,0x1a,0x05,0x93,0xa2,0xd1,0x00,0xbe,
-0x2b,0x68,0x6a,0x68,0x9b,0x1a,0x09,0x93,0x09,0x9b,0x00,0x2b,0xa9,0xda,0x00,0x23,
-0x09,0x93,0xa6,0xe7,0x10,0x00,0x10,0x40,0x0c,0x00,0x10,0x40,0x40,0x00,0x10,0x40,
-0x44,0x00,0x10,0x40,0x48,0x00,0x10,0x40,0x18,0x00,0x10,0x40,0x4c,0x00,0x10,0x40,
-0x00,0x00,0x10,0x40,

diff --git a/doc/openocd.texi b/doc/openocd.texi
index d059cfae280a2c2526b0ff9814981b28d4468517..8bb4df6851fdd0965aa26b29e1de11ff1d6d7a22 100644 (file)
--- a/doc/openocd.texi
+++ b/doc/openocd.texi
@@ -5830,7 +5830,7 @@ The AVR 8-bit microcontrollers from Atmel integrate flash memory.
 @end deffn
 
 @deffn {Flash Driver} bluenrg-x
 @end deffn
 
 @deffn {Flash Driver} bluenrg-x

-STMicroelectronics BlueNRG-1 and BlueNRG-2 Bluetooth low energy wireless system-on-chip. They include ARM Cortex-M0 core and internal flash memory.
+STMicroelectronics BlueNRG-1, BlueNRG-2 and BlueNRG-LP Bluetooth low energy wireless system-on-chip. They include ARM Cortex-M0/M0+ core and internal flash memory.

 The driver automatically recognizes these chips using
 the chip identification registers, and autoconfigures itself.
 
 The driver automatically recognizes these chips using
 the chip identification registers, and autoconfigures itself.
 


@@ -5849,6 +5849,10 @@ flash erase_sector 0 0 79 # It will perform a mass erase on BlueNRG-1
 flash erase_sector 0 0 127 # It will perform a mass erase on BlueNRG-2
 @end example
 
 flash erase_sector 0 0 127 # It will perform a mass erase on BlueNRG-2
 @end example
 

+@example
+flash erase_sector 0 0 127 # It will perform a mass erase on BlueNRG-LP
+@end example
+

 Triggering a mass erase is also useful when users want to disable readout protection.
 @end deffn
 
 Triggering a mass erase is also useful when users want to disable readout protection.
 @end deffn
 

diff --git a/src/flash/nor/bluenrg-x.c b/src/flash/nor/bluenrg-x.c
index f6a249273afe9660aff67b34c0b3915092f55b94..79821168e460bb0907caa72f97f8508a361fc0a2 100644 (file)
--- a/src/flash/nor/bluenrg-x.c
+++ b/src/flash/nor/bluenrg-x.c
@@ -25,16 +25,20 @@
 #include <target/cortex_m.h>
 #include "imp.h"
 
 #include <target/cortex_m.h>
 #include "imp.h"
 

-#define FLASH_SIZE_REG       (0x40100014)
-#define DIE_ID_REG           (0x4090001C)
-#define JTAG_IDCODE_REG      (0x40900028)

 #define BLUENRG2_IDCODE      (0x0200A041)
 #define BLUENRG2_IDCODE      (0x0200A041)

-#define FLASH_BASE           (0x10040000)
-#define FLASH_PAGE_SIZE      (2048)
-#define FLASH_REG_COMMAND    (0x40100000)
-#define FLASH_REG_IRQRAW     (0x40100010)
-#define FLASH_REG_ADDRESS    (0x40100018)
-#define FLASH_REG_DATA       (0x40100040)
+#define BLUENRGLP_IDCODE     (0x0201E041)
+#define BLUENRG2_JTAG_REG       (flash_priv_data_2.jtag_idcode_reg)
+#define BLUENRGLP_JTAG_REG      (flash_priv_data_lp.jtag_idcode_reg)
+
+#define FLASH_SIZE_REG(bluenrgx_info)       (bluenrgx_info->flash_ptr->flash_size_reg)
+#define DIE_ID_REG(bluenrgx_info)           (bluenrgx_info->flash_ptr->die_id_reg)
+#define JTAG_IDCODE_REG(bluenrgx_info)      (bluenrgx_info->flash_ptr->jtag_idcode_reg)
+#define FLASH_BASE(bluenrgx_info)           (bluenrgx_info->flash_ptr->flash_base)
+#define FLASH_PAGE_SIZE(bluenrgx_info)      (bluenrgx_info->flash_ptr->flash_page_size)
+#define FLASH_REG_COMMAND(bluenrgx_info)    (bluenrgx_info->flash_ptr->flash_reg_command)
+#define FLASH_REG_IRQRAW(bluenrgx_info)     (bluenrgx_info->flash_ptr->flash_reg_irqraw)
+#define FLASH_REG_ADDRESS(bluenrgx_info)    (bluenrgx_info->flash_ptr->flash_reg_address)
+#define FLASH_REG_DATA(bluenrgx_info)       (bluenrgx_info->flash_ptr->flash_reg_data)

 #define FLASH_CMD_ERASE_PAGE 0x11
 #define FLASH_CMD_MASSERASE  0x22
 #define FLASH_CMD_WRITE      0x33
 #define FLASH_CMD_ERASE_PAGE 0x11
 #define FLASH_CMD_MASSERASE  0x22
 #define FLASH_CMD_WRITE      0x33


@@ -42,12 +46,91 @@
 #define FLASH_INT_CMDDONE    0x01
 #define FLASH_WORD_LEN       4
 
 #define FLASH_INT_CMDDONE    0x01
 #define FLASH_WORD_LEN       4
 

+/* See contrib/loaders/flash/bluenrg-x/bluenrg-x_write.c for source and
+ * hints how to generate the data!
+ */
+static const uint8_t bluenrgx_flash_write_code_2[] = {
+#include "../../../contrib/loaders/flash/bluenrg-x/bluenrg-2_write.inc"
+                       };
+
+static const uint8_t bluenrgx_flash_write_code_lp[] = {
+#include "../../../contrib/loaders/flash/bluenrg-x/bluenrg-lp_write.inc"
+                       };
+
+struct flash_ctrl_priv_data {
+       uint32_t flash_size_reg;
+       uint32_t die_id_reg;
+       uint32_t jtag_idcode_reg;
+       uint32_t flash_base;
+       uint32_t flash_page_size;
+       uint32_t flash_reg_command;
+       uint32_t flash_reg_irqraw;
+       uint32_t flash_reg_address;
+       uint32_t flash_reg_data;
+       uint32_t jtag_idcode;
+       char *part_name;
+       const uint8_t *flash_write_code;
+       uint32_t flash_write_code_size;
+};
+
+const struct flash_ctrl_priv_data flash_priv_data_1 = {
+       .flash_size_reg = 0x40100014,
+       .die_id_reg = 0x4090001C,
+       .jtag_idcode_reg = 0x40900028,
+       .flash_base = 0x10040000,
+       .flash_page_size = 2048,
+       .flash_reg_command = 0x40100000,
+       .flash_reg_irqraw = 0x40100010,
+       .flash_reg_address = 0x40100018,
+       .flash_reg_data = 0x40100040,
+       .jtag_idcode = 0x00000000,
+       .part_name = "BLUENRG-1",
+       .flash_write_code = bluenrgx_flash_write_code_2,
+       .flash_write_code_size = sizeof(bluenrgx_flash_write_code_2),
+};
+
+const struct flash_ctrl_priv_data flash_priv_data_2 = {
+       .flash_size_reg = 0x40100014,
+       .die_id_reg = 0x4090001C,
+       .jtag_idcode_reg = 0x40900028,
+       .flash_base = 0x10040000,
+       .flash_page_size = 2048,
+       .flash_reg_command = 0x40100000,
+       .flash_reg_irqraw = 0x40100010,
+       .flash_reg_address = 0x40100018,
+       .flash_reg_data = 0x40100040,
+       .jtag_idcode = BLUENRG2_IDCODE,
+       .part_name = "BLUENRG-2",
+       .flash_write_code = bluenrgx_flash_write_code_2,
+       .flash_write_code_size = sizeof(bluenrgx_flash_write_code_2),
+};
+
+const struct flash_ctrl_priv_data flash_priv_data_lp = {
+       .flash_size_reg = 0x40001014,
+       .die_id_reg = 0x40000000,
+       .jtag_idcode_reg = 0x40000004,
+       .flash_base = 0x10040000,
+       .flash_page_size = 2048,
+       .flash_reg_command = 0x40001000,
+       .flash_reg_irqraw = 0x40001010,
+       .flash_reg_address = 0x40001018,
+       .flash_reg_data = 0x40001040,
+       .jtag_idcode = BLUENRGLP_IDCODE,
+       .part_name = "BLUENRG-LP",
+       .flash_write_code = bluenrgx_flash_write_code_lp,
+       .flash_write_code_size = sizeof(bluenrgx_flash_write_code_lp),
+};
+

 struct bluenrgx_flash_bank {
        int probed;
 struct bluenrgx_flash_bank {
        int probed;

-       uint32_t idcode;

        uint32_t die_id;
        uint32_t die_id;

+       const struct flash_ctrl_priv_data *flash_ptr;
+       const uint8_t *flash_write_code;
+       uint32_t flash_write_code_size;

 };
 
 };
 

+const struct flash_ctrl_priv_data *flash_ctrl[] = {&flash_priv_data_1, &flash_priv_data_2, &flash_priv_data_lp};
+

 static int bluenrgx_protect_check(struct flash_bank *bank)
 {
        /* Nothing to do. Protection is only handled in SW. */
 static int bluenrgx_protect_check(struct flash_bank *bank)
 {
        /* Nothing to do. Protection is only handled in SW. */


@@ -103,24 +186,25 @@ static int bluenrgx_erase(struct flash_bank *bank, int first, int last)
        if (mass_erase) {
                command = FLASH_CMD_MASSERASE;
                address = bank->base;
        if (mass_erase) {
                command = FLASH_CMD_MASSERASE;
                address = bank->base;

-               if (target_write_u32(target, FLASH_REG_IRQRAW, 0x3f) != ERROR_OK) {
+               if (target_write_u32(target, FLASH_REG_IRQRAW(bluenrgx_info), 0x3f) != ERROR_OK) {

                        LOG_ERROR("Register write failed");
                        return ERROR_FAIL;
                }
 
                        LOG_ERROR("Register write failed");
                        return ERROR_FAIL;
                }
 

-               if (target_write_u32(target, FLASH_REG_ADDRESS, address >> 2) != ERROR_OK) {
+               if (target_write_u32(target, FLASH_REG_ADDRESS(bluenrgx_info),
+                                                               (address - FLASH_BASE(bluenrgx_info)) >> 2) != ERROR_OK) {

                        LOG_ERROR("Register write failed");
                        return ERROR_FAIL;
                }
 
                        LOG_ERROR("Register write failed");
                        return ERROR_FAIL;
                }
 

-               if (target_write_u32(target, FLASH_REG_COMMAND, command) != ERROR_OK) {
+               if (target_write_u32(target, FLASH_REG_COMMAND(bluenrgx_info), command) != ERROR_OK) {

                        LOG_ERROR("Register write failed");
                        return ERROR_FAIL;
                }
 
                for (int i = 0; i < 100; i++) {
                        uint32_t value;
                        LOG_ERROR("Register write failed");
                        return ERROR_FAIL;
                }
 
                for (int i = 0; i < 100; i++) {
                        uint32_t value;

-                       if (target_read_u32(target, FLASH_REG_IRQRAW, &value)) {
+                       if (target_read_u32(target, FLASH_REG_IRQRAW(bluenrgx_info), &value)) {

                                LOG_ERROR("Register write failed");
                                return ERROR_FAIL;
                        }
                                LOG_ERROR("Register write failed");
                                return ERROR_FAIL;
                        }


@@ -135,26 +219,28 @@ static int bluenrgx_erase(struct flash_bank *bank, int first, int last)
        } else {
                command = FLASH_CMD_ERASE_PAGE;
                for (int i = first; i <= last; i++) {
        } else {
                command = FLASH_CMD_ERASE_PAGE;
                for (int i = first; i <= last; i++) {

-                       address = bank->base+i*FLASH_PAGE_SIZE;
+                       address = bank->base+i*FLASH_PAGE_SIZE(bluenrgx_info);
+                       LOG_DEBUG("address = %08x, index = %d", address, i);

 
 

-                       if (target_write_u32(target, FLASH_REG_IRQRAW, 0x3f) != ERROR_OK) {
+                       if (target_write_u32(target, FLASH_REG_IRQRAW(bluenrgx_info), 0x3f) != ERROR_OK) {

                                LOG_ERROR("Register write failed");
                                return ERROR_FAIL;
                        }
 
                                LOG_ERROR("Register write failed");
                                return ERROR_FAIL;
                        }
 

-                       if (target_write_u32(target, FLASH_REG_ADDRESS, address >> 2) != ERROR_OK) {
+                       if (target_write_u32(target, FLASH_REG_ADDRESS(bluenrgx_info),
+                                                                       (address - FLASH_BASE(bluenrgx_info)) >> 2) != ERROR_OK) {

                                LOG_ERROR("Register write failed");
                                return ERROR_FAIL;
                        }
 
                                LOG_ERROR("Register write failed");
                                return ERROR_FAIL;
                        }
 

-                       if (target_write_u32(target, FLASH_REG_COMMAND, command) != ERROR_OK) {
+                       if (target_write_u32(target, FLASH_REG_COMMAND(bluenrgx_info), command) != ERROR_OK) {

                                LOG_ERROR("Failed");
                                return ERROR_FAIL;
                        }
 
                        for (int j = 0; j < 100; j++) {
                                uint32_t value;
                                LOG_ERROR("Failed");
                                return ERROR_FAIL;
                        }
 
                        for (int j = 0; j < 100; j++) {
                                uint32_t value;

-                               if (target_read_u32(target, FLASH_REG_IRQRAW, &value)) {
+                               if (target_read_u32(target, FLASH_REG_IRQRAW(bluenrgx_info), &value)) {

                                        LOG_ERROR("Register write failed");
                                        return ERROR_FAIL;
                                }
                                        LOG_ERROR("Register write failed");
                                        return ERROR_FAIL;
                                }


@@ -182,11 +268,14 @@ static int bluenrgx_protect(struct flash_bank *bank, int set, int first, int las
                bank->sectors[sector].is_protected = set;
        return ERROR_OK;
 }
                bank->sectors[sector].is_protected = set;
        return ERROR_OK;
 }

-static int bluenrgx_write_word(struct target *target, uint32_t address_base, uint8_t *values, uint32_t count)
+
+static int bluenrgx_write_word(struct flash_bank *bank, uint32_t address_base, uint8_t *values, uint32_t count)

 {
        int retval = ERROR_OK;
 {
        int retval = ERROR_OK;

+       struct target *target =  bank->target;
+       struct bluenrgx_flash_bank *bluenrgx_info = bank->driver_priv;

 
 

-       retval = target_write_u32(target, FLASH_REG_IRQRAW, 0x3f);
+       retval = target_write_u32(target, FLASH_REG_IRQRAW(bluenrgx_info), 0x3f);

        if (retval != ERROR_OK) {
                LOG_ERROR("Register write failed, error code: %d", retval);
                return retval;
        if (retval != ERROR_OK) {
                LOG_ERROR("Register write failed, error code: %d", retval);
                return retval;


@@ -195,19 +284,21 @@ static int bluenrgx_write_word(struct target *target, uint32_t address_base, uin
        for (uint32_t i = 0; i < count; i++) {
                uint32_t address = address_base + i * FLASH_WORD_LEN;
 
        for (uint32_t i = 0; i < count; i++) {
                uint32_t address = address_base + i * FLASH_WORD_LEN;
 

-               retval = target_write_u32(target, FLASH_REG_ADDRESS, address >> 2);
+               retval = target_write_u32(target, FLASH_REG_ADDRESS(bluenrgx_info),
+                                                                       (address - FLASH_BASE(bluenrgx_info)) >> 2);

                if (retval != ERROR_OK) {
                        LOG_ERROR("Register write failed, error code: %d", retval);
                        return retval;
                }
 
                if (retval != ERROR_OK) {
                        LOG_ERROR("Register write failed, error code: %d", retval);
                        return retval;
                }
 

-               retval = target_write_buffer(target, FLASH_REG_DATA, FLASH_WORD_LEN, values + i * FLASH_WORD_LEN);
+               retval = target_write_buffer(target, FLASH_REG_DATA(bluenrgx_info),
+                                                                               FLASH_WORD_LEN, values + i * FLASH_WORD_LEN);

                if (retval != ERROR_OK) {
                        LOG_ERROR("Register write failed, error code: %d", retval);
                        return retval;
                }
 
                if (retval != ERROR_OK) {
                        LOG_ERROR("Register write failed, error code: %d", retval);
                        return retval;
                }
 

-               retval = target_write_u32(target, FLASH_REG_COMMAND, FLASH_CMD_WRITE);
+               retval = target_write_u32(target, FLASH_REG_COMMAND(bluenrgx_info), FLASH_CMD_WRITE);

                if (retval != ERROR_OK) {
                        LOG_ERROR("Register write failed, error code: %d", retval);
                        return retval;
                if (retval != ERROR_OK) {
                        LOG_ERROR("Register write failed, error code: %d", retval);
                        return retval;


@@ -215,7 +306,7 @@ static int bluenrgx_write_word(struct target *target, uint32_t address_base, uin
 
                for (int j = 0; j < 100; j++) {
                        uint32_t reg_value;
 
                for (int j = 0; j < 100; j++) {
                        uint32_t reg_value;

-                       retval = target_read_u32(target, FLASH_REG_IRQRAW, &reg_value);
+                       retval = target_read_u32(target, FLASH_REG_IRQRAW(bluenrgx_info), &reg_value);

 
                        if (retval != ERROR_OK) {
                                LOG_ERROR("Register read failed, error code: %d", retval);
 
                        if (retval != ERROR_OK) {
                                LOG_ERROR("Register read failed, error code: %d", retval);


@@ -234,9 +325,10 @@ static int bluenrgx_write_word(struct target *target, uint32_t address_base, uin
        return retval;
 }
 
        return retval;
 }
 

-static int bluenrgx_write_bytes(struct target *target, uint32_t address_base, uint8_t *buffer, uint32_t count)
+static int bluenrgx_write_bytes(struct flash_bank *bank, uint32_t address_base, uint8_t *buffer, uint32_t count)

 {
        int retval = ERROR_OK;
 {
        int retval = ERROR_OK;

+       struct target *target =  bank->target;

        uint8_t *new_buffer = NULL;
        uint32_t pre_bytes = 0, post_bytes = 0, pre_word, post_word, pre_address, post_address;
 
        uint8_t *new_buffer = NULL;
        uint32_t pre_bytes = 0, post_bytes = 0, pre_word, post_word, pre_address, post_address;
 


@@ -295,7 +387,7 @@ static int bluenrgx_write_bytes(struct target *target, uint32_t address_base, ui
                buffer = new_buffer;
        }
 
                buffer = new_buffer;
        }
 

-       retval = bluenrgx_write_word(target, address_base - pre_bytes, buffer, count/4);
+       retval = bluenrgx_write_word(bank, address_base - pre_bytes, buffer, count/4);

 
        if (new_buffer)
                free(new_buffer);
 
        if (new_buffer)
                free(new_buffer);


@@ -306,6 +398,7 @@ static int bluenrgx_write_bytes(struct target *target, uint32_t address_base, ui
 static int bluenrgx_write(struct flash_bank *bank, const uint8_t *buffer,
                          uint32_t offset, uint32_t count)
 {
 static int bluenrgx_write(struct flash_bank *bank, const uint8_t *buffer,
                          uint32_t offset, uint32_t count)
 {

+       struct bluenrgx_flash_bank *bluenrgx_info = bank->driver_priv;

        struct target *target = bank->target;
        uint32_t buffer_size = 16384 + 8;
        struct working_area *write_algorithm;
        struct target *target = bank->target;
        uint32_t buffer_size = 16384 + 8;
        struct working_area *write_algorithm;


@@ -318,12 +411,9 @@ static int bluenrgx_write(struct flash_bank *bank, const uint8_t *buffer,
        uint32_t pre_size = 0, fast_size = 0, post_size = 0;
        uint32_t pre_offset = 0, fast_offset = 0, post_offset = 0;
 
        uint32_t pre_size = 0, fast_size = 0, post_size = 0;
        uint32_t pre_offset = 0, fast_offset = 0, post_offset = 0;
 

-       /* See contrib/loaders/flash/bluenrg-x/bluenrg-x_write.c for source and
-        * hints how to generate the data!
-        */
-       static const uint8_t bluenrgx_flash_write_code[] = {
-#include "../../../contrib/loaders/flash/bluenrg-x/bluenrg-x_write.inc"
-       };
+       /* check preconditions */
+       if (bluenrgx_info->probed == 0)
+               return ERROR_FLASH_BANK_NOT_PROBED;

 
        if ((offset + count) > bank->size) {
                LOG_ERROR("Requested write past beyond of flash size: (offset+count) = %d, size=%d",
 
        if ((offset + count) > bank->size) {
                LOG_ERROR("Requested write past beyond of flash size: (offset+count) = %d, size=%d",


@@ -350,7 +440,7 @@ static int bluenrgx_write(struct flash_bank *bank, const uint8_t *buffer,
        LOG_DEBUG("post_size = %08x, post_offset=%08x", post_size, post_offset);
 
        /* Program initial chunk not 16 bytes aligned */
        LOG_DEBUG("post_size = %08x, post_offset=%08x", post_size, post_offset);
 
        /* Program initial chunk not 16 bytes aligned */

-       retval = bluenrgx_write_bytes(target, bank->base+pre_offset, (uint8_t *) buffer, pre_size);
+       retval = bluenrgx_write_bytes(bank, bank->base+pre_offset, (uint8_t *) buffer, pre_size);

        if (retval) {
                LOG_ERROR("bluenrgx_write_bytes failed %d", retval);
                return ERROR_FAIL;
        if (retval) {
                LOG_ERROR("bluenrgx_write_bytes failed %d", retval);
                return ERROR_FAIL;


@@ -359,15 +449,15 @@ static int bluenrgx_write(struct flash_bank *bank, const uint8_t *buffer,
        /* Program chunk 16 bytes aligned in fast mode */
        if (fast_size) {
 
        /* Program chunk 16 bytes aligned in fast mode */
        if (fast_size) {
 

-               if (target_alloc_working_area(target, sizeof(bluenrgx_flash_write_code),
-                                             &write_algorithm) != ERROR_OK) {
+               if (target_alloc_working_area(target, bluenrgx_info->flash_write_code_size,
+                                                 &write_algorithm) != ERROR_OK) {

                        LOG_WARNING("no working area available, can't do block memory writes");
                        return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
                }
 
                retval = target_write_buffer(target, write_algorithm->address,
                        LOG_WARNING("no working area available, can't do block memory writes");
                        return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
                }
 
                retval = target_write_buffer(target, write_algorithm->address,

-                                            sizeof(bluenrgx_flash_write_code),
-                                            bluenrgx_flash_write_code);
+                                                bluenrgx_info->flash_write_code_size,
+                                                bluenrgx_info->flash_write_code);

                if (retval != ERROR_OK)
                        return retval;
 
                if (retval != ERROR_OK)
                        return retval;
 


@@ -379,7 +469,7 @@ static int bluenrgx_write(struct flash_bank *bank, const uint8_t *buffer,
 
                /* Stack pointer area */
                if (target_alloc_working_area(target, 64,
 
                /* Stack pointer area */
                if (target_alloc_working_area(target, 64,

-                                             &write_algorithm_sp) != ERROR_OK) {
+                                                 &write_algorithm_sp) != ERROR_OK) {

                        LOG_DEBUG("no working area for write code stack pointer");
                        return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
                }
                        LOG_DEBUG("no working area for write code stack pointer");
                        return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
                }


@@ -458,7 +548,8 @@ static int bluenrgx_write(struct flash_bank *bank, const uint8_t *buffer,
        }
 
        /* Program chunk at end, not addressable by fast burst write algorithm */
        }
 
        /* Program chunk at end, not addressable by fast burst write algorithm */

-       retval = bluenrgx_write_bytes(target, bank->base+post_offset, (uint8_t *) (buffer+pre_size+fast_size), post_size);
+       retval = bluenrgx_write_bytes(bank, bank->base+post_offset,
+                                                                        (uint8_t *) (buffer+pre_size+fast_size), post_size);

        if (retval) {
                LOG_ERROR("bluenrgx_write_bytes failed %d", retval);
                return ERROR_FAIL;
        if (retval) {
                LOG_ERROR("bluenrgx_write_bytes failed %d", retval);
                return ERROR_FAIL;


@@ -471,32 +562,54 @@ static int bluenrgx_probe(struct flash_bank *bank)
        struct bluenrgx_flash_bank *bluenrgx_info = bank->driver_priv;
        uint32_t idcode, size_info, die_id;
        int i;
        struct bluenrgx_flash_bank *bluenrgx_info = bank->driver_priv;
        uint32_t idcode, size_info, die_id;
        int i;

-       int retval = target_read_u32(bank->target, JTAG_IDCODE_REG, &idcode);
+       int retval = target_read_u32(bank->target, BLUENRGLP_JTAG_REG, &idcode);
+

        if (retval != ERROR_OK)
                return retval;
        if (retval != ERROR_OK)
                return retval;

-       retval = target_read_u32(bank->target, FLASH_SIZE_REG, &size_info);
+
+       if (idcode != BLUENRGLP_IDCODE) {
+               retval = target_read_u32(bank->target, BLUENRG2_JTAG_REG, &idcode);
+               if (retval != ERROR_OK)
+                       return retval;
+       }
+
+       /* Default device is BlueNRG-1 */
+       bluenrgx_info->flash_ptr = &flash_priv_data_1;
+       bluenrgx_info->flash_write_code = flash_priv_data_1.flash_write_code;
+       bluenrgx_info->flash_write_code_size = flash_priv_data_1.flash_write_code_size;
+
+       for (i = 0; i < (int)(sizeof(flash_ctrl)/sizeof(*flash_ctrl)); i++) {
+               if (idcode == (*flash_ctrl[i]).jtag_idcode) {
+                       bluenrgx_info->flash_ptr = flash_ctrl[i];
+                       bluenrgx_info->flash_write_code = (*flash_ctrl[i]).flash_write_code;
+                       bluenrgx_info->flash_write_code_size = (*flash_ctrl[i]).flash_write_code_size;
+                       break;
+               }
+       }
+
+       retval = target_read_u32(bank->target, FLASH_SIZE_REG(bluenrgx_info), &size_info);

        if (retval != ERROR_OK)
                return retval;
 
        if (retval != ERROR_OK)
                return retval;
 

-       retval = target_read_u32(bank->target, DIE_ID_REG, &die_id);
+       retval = target_read_u32(bank->target, DIE_ID_REG(bluenrgx_info), &die_id);

        if (retval != ERROR_OK)
                return retval;
 
        if (retval != ERROR_OK)
                return retval;
 

-       bank->size = (size_info + 1) * 4;
-       bank->base = FLASH_BASE;
-       bank->num_sectors = bank->size/FLASH_PAGE_SIZE;
+       bank->size = (size_info + 1) * FLASH_WORD_LEN;
+       bank->base = FLASH_BASE(bluenrgx_info);
+       bank->num_sectors = bank->size/FLASH_PAGE_SIZE(bluenrgx_info);

        bank->sectors = realloc(bank->sectors, sizeof(struct flash_sector) * bank->num_sectors);
 
        for (i = 0; i < bank->num_sectors; i++) {
        bank->sectors = realloc(bank->sectors, sizeof(struct flash_sector) * bank->num_sectors);
 
        for (i = 0; i < bank->num_sectors; i++) {

-               bank->sectors[i].offset = i * FLASH_PAGE_SIZE;
-               bank->sectors[i].size = FLASH_PAGE_SIZE;
+               bank->sectors[i].offset = i * FLASH_PAGE_SIZE(bluenrgx_info);
+               bank->sectors[i].size = FLASH_PAGE_SIZE(bluenrgx_info);

                bank->sectors[i].is_erased = -1;
                bank->sectors[i].is_protected = 0;
        }
 
        bluenrgx_info->probed = 1;
        bluenrgx_info->die_id = die_id;
                bank->sectors[i].is_erased = -1;
                bank->sectors[i].is_protected = 0;
        }
 
        bluenrgx_info->probed = 1;
        bluenrgx_info->die_id = die_id;

-       bluenrgx_info->idcode = idcode;
+

        return ERROR_OK;
 }
 
        return ERROR_OK;
 }
 


@@ -515,7 +628,6 @@ static int bluenrgx_get_info(struct flash_bank *bank, char *buf, int buf_size)
 {
        struct bluenrgx_flash_bank *bluenrgx_info = bank->driver_priv;
        int mask_number, cut_number;
 {
        struct bluenrgx_flash_bank *bluenrgx_info = bank->driver_priv;
        int mask_number, cut_number;

-       char *part_name;

 
        if (!bluenrgx_info->probed) {
                int retval = bluenrgx_probe(bank);
 
        if (!bluenrgx_info->probed) {
                int retval = bluenrgx_probe(bank);


@@ -526,16 +638,11 @@ static int bluenrgx_get_info(struct flash_bank *bank, char *buf, int buf_size)
                }
        }
 
                }
        }
 

-       if (bluenrgx_info->idcode == BLUENRG2_IDCODE)
-               part_name = "BLUENRG-2";
-       else
-               part_name = "BLUENRG-1";
-

        mask_number = (bluenrgx_info->die_id >> 4) & 0xF;
        cut_number = bluenrgx_info->die_id & 0xF;
 
        snprintf(buf, buf_size,
        mask_number = (bluenrgx_info->die_id >> 4) & 0xF;
        cut_number = bluenrgx_info->die_id & 0xF;
 
        snprintf(buf, buf_size,

-                "%s - Rev: %d.%d", part_name, mask_number, cut_number);
+                "%s - Rev: %d.%d", bluenrgx_info->flash_ptr->part_name, mask_number, cut_number);

        return ERROR_OK;
 }
 
        return ERROR_OK;
 }
 

diff --git a/tcl/board/steval-idb011v1.cfg b/tcl/board/steval-idb011v1.cfg
new file mode 100644 (file)
index 0000000..5988c63
--- /dev/null
+++ b/tcl/board/steval-idb011v1.cfg
@@ -0,0 +1,3 @@
+# This is an evaluation board with a single BlueNRG-LP chip.
+set CHIPNAME bluenrg-lp
+source [find target/bluenrg-x.cfg]

diff --git a/tcl/target/bluenrg-x.cfg b/tcl/target/bluenrg-x.cfg
index 109db1708085a5309d886aa26d8c9c2b5fb9ce22..691bbbf83b2744ac0276ce422a9f7295f600f84c 100644 (file)
--- a/tcl/target/bluenrg-x.cfg
+++ b/tcl/target/bluenrg-x.cfg
@@ -1,8 +1,9 @@
 #
 #

-# bluenrg-1/2 devices support only SWD transports.
+# bluenrg-1/2 and bluenrg-lp devices support only SWD transports.

 #
 
 source [find target/swj-dp.tcl]
 #
 
 source [find target/swj-dp.tcl]

+source [find mem_helper.tcl]

 
 if { [info exists CHIPNAME] } {
    set _CHIPNAME $CHIPNAME
 
 if { [info exists CHIPNAME] } {
    set _CHIPNAME $CHIPNAME


@@ -53,22 +54,27 @@ if {![using_hla]} {
 }
 
 $_TARGETNAME configure -event halted {
 }
 
 $_TARGETNAME configure -event halted {

-       global WDOG_VALUE
-       global WDOG_VALUE_SET
-       # Stop watchdog during halt, if enabled
-       mem2array value 32 0x40700008 1
-       set WDOG_VALUE [expr ($value(0))]
-       if [expr ($value(0) & (1 << 1))] {
-               set WDOG_VALUE_SET 1
-               mww 0x40700008 [expr ($value(0) & 0xFFFFFFFD)]
-       }
+    global WDOG_VALUE
+    global WDOG_VALUE_SET
+    set _JTAG_IDCODE [mrw 0x40000004]
+    if {$_JTAG_IDCODE != 0x0201E041} {
+        # Stop watchdog during halt, if enabled. Only Bluenrg-1/2
+        set WDOG_VALUE [mrw 0x40700008]
+        if [expr ($WDOG_VALUE & (1 << 1))] {
+            set WDOG_VALUE_SET 1
+            mww 0x40700008 [expr ($WDOG_VALUE & 0xFFFFFFFD)]
+        }
+    }

 }
 $_TARGETNAME configure -event resumed {
 }
 $_TARGETNAME configure -event resumed {

-       global WDOG_VALUE
-       global WDOG_VALUE_SET
-       if [expr $WDOG_VALUE_SET] {
-               # Restore watchdog enable value after resume
-               mww 0x40700008 $WDOG_VALUE
-               set WDOG_VALUE_SET 0
-       }
+    global WDOG_VALUE
+    global WDOG_VALUE_SET
+    set _JTAG_IDCODE [mrw 0x40000004]
+    if {$_JTAG_IDCODE != 0x0201E041} {
+        if [expr $WDOG_VALUE_SET] {
+            # Restore watchdog enable value after resume. Only Bluenrg-1/2
+            mww 0x40700008 $WDOG_VALUE
+            set WDOG_VALUE_SET 0
+           }
+   }

 }
 }