diff --git a/cfg/sam3s_rpi_sysfs_check.cfg b/cfg/sam3s_rpi_sysfs_check.cfg
new file mode 100644
index 0000000..aedfe05
--- /dev/null
+++ b/cfg/sam3s_rpi_sysfs_check.cfg
@@ -0,0 +1,43 @@
+source [find interface/matrix-creator.cfg]
+transport select jtag 
+adapter_khz 128
+
+
+jtag newtap em358 cpu \
+        -irlen 4\
+        -ircapture 0x1\
+        -irmask 0xf \
+        -expected-id 0x3ba00477
+
+jtag newtap em357 bs \
+        -irlen 4 \
+        -ircapture 0x0e \
+        -irmask 0xf \
+        -expected-id 0x069AA62B
+
+jtag newtap xc6sxl4.fpga fpga \
+	-irlen 6 \
+	-ircapture 0x35 \
+	-irmask 0x3f \
+	-expected-id 0x24000093
+
+jtag newtap sam3n.cpu cpu \
+        -irlen 4\
+        -ircapture 0x1\
+        -irmask 0xf \
+        -expected-id 0x4ba00477
+
+target create sam3n.cpu cortex_m -endian little  -chain-position 3 
+flash bank flash0 at91sam3 0x00400000 0 0 0 sam3n.cpu 
+
+#source at91sam3nXX.cfg
+init
+halt
+wait_halt
+sleep 10
+at91sam3 gpnvm show all
+flash info 0
+flash probe 0                                                                   
+flash verify_bank 0 blob/ch.bin 0
+reset run
+shutdown
diff --git a/sam3-program.bash b/sam3-program.bash
index 7733be8..0eff3ec 100755
--- a/sam3-program.bash
+++ b/sam3-program.bash
@@ -62,7 +62,8 @@ function try_program() {
   echo $RES
   
   sleep 0.5
-  reset_mcu  
+  reset_mcu
+  sleep 1
 }
 
 function enable_program() {
@@ -73,13 +74,17 @@ function enable_program() {
 }
 
 function check_firmware() {
- COMPARE_VERSION=$(diff <(./firmware_info) <(cat mcu_firmware.version)|wc -l)
-
- if [ "$COMPARE_VERSION" == "0" ];then
-  echo 1
- else #failed
-  echo 0 
- fi
+  local result=1
+  local output=$(openocd -f cfg/sam3s_rpi_sysfs_check.cfg 2>&1)
+  if [[ "$output" == *"sam3-gpnvm1: 0"* ]]; then
+    # wrong bootmode
+    result=0
+  fi
+  if [[ "$output" == *"contents differ"* ]]; then
+    # new or no firmware
+    result=0
+  fi
+  echo "$result"
 }
 
 for i in 4 17 18 19 20 22 23 27
@@ -101,9 +106,11 @@ then
   echo "SAM3 MCU was programmed before. Not programming it again."
   exit 0
 fi
+
 enable_program
+
 count=0
-while [  $count -lt 30 ]; do
+while [  $count -lt 5 ]; do
   TEST=$(try_program)
   if [ "$TEST" == "1" ];then
         CHECK=$(check_firmware)