# 
# Return hex value of register
# 
proc get_reg_value { data } {
    set reg_str [string trimright $data "\n " ]
    lassign [split $reg_str ":"] reg out
    set hex 0x[string trim $out]

    return $hex
}

#
# Unlock any flash by register
#
proc _unlock_flash_by_reg { TARGET REG } {
    $TARGET mww $REG 0x45670123
    $TARGET mww $REG 0xCDEF89AB
}

#
# Unlock flash (FLASH_KEYR - FPEC key register)
#
proc _unlock_flash { TARGET } {
    set _FLASH_KEYR_ADDR    0x40022004

    _unlock_flash_by_reg $TARGET $_FLASH_KEYR_ADDR
}

#
# Unlock the User Option Bytes
#
proc _unlock_user_ob { TARGET } {
    set _FLASH_OBKEYR_ADDR  0x40022008

    _unlock_flash_by_reg $TARGET $_FLASH_OBKEYR_ADDR
}

set _TARGET $_TARGETNAME.0

set FLASH_CTLR_ADDR    0x40022010
set FLASH_STATR_ADDR   0x4002200C
set FLASH_OBR_ADDR	   0x4002201C
set RDPR_ADDR		   0x1FFFF800

# Status Register (FLASH_STATR)
set BSY_BIT_MASK	[expr {1 << 0}]
set EOP_BIT_MASK	[expr {1 << 5}]

# Control Register (FLASH_CTLR)
set LOCK_BIT_MASK	[expr {1 << 7}]
set OBWRE_BIT_MASK	[expr {1 << 9}]

# Selection word register
set RDPRT_BIT_MASK	[expr {1 << 1}]

set RDPR_MASK_LOCK	0x5aa5

# ms
set TIMEOUT_STEP	10
set TIMEOUT			500
#
# Read Protection Release
# 32.6.2 User Option Bytes Programming
#
proc unlock { } {
	set op "UNLOCK"
	
    global _TARGET
    
    global FLASH_CTLR_ADDR    
	global FLASH_STATR_ADDR
	global FLASH_OBR_ADDR   
	global RDPR_ADDR			

	# Status Register (FLASH_STATR)
	global BSY_BIT_MASK	
	global EOP_BIT_MASK

	# Control Register (FLASH_CTLR)
	set OBPG_BIT_MASK	[expr {1 << 4}]
	global LOCK_BIT_MASK	
	global OBWRE_BIT_MASK
	global RDPRT_BIT_MASK	

	global RDPR_MASK_LOCK

	# ms
	global TIMEOUT_STEP	
	global TIMEOUT			

	# Read protection status.
	# 0: Current read protection of flash is invalid;
    set RDPRT_byte [ get_reg_value [ $_TARGET mdb $FLASH_OBR_ADDR ] ]
    if { [ expr {$RDPRT_byte & $RDPRT_BIT_MASK} ] == 0 } {
        echo "** \[$op\] Memory is unlocked already **"
        return
    }
    
	# Erase the entire User Option Bytes area

	# 1)
	# Check the LOCK bit in the FLASH_CTLR register.
	# If it is 1, you need to perform the "Release Flash Memory Lock" operation.

	# Lock. When this bit is '1', it means that FPEC and FLASH_CTLR
	# are locked and cannot be written. After detecting the correct unlock sequence, the
	# hardware will clear this bit to ‘0’.
	# After an unsuccessful unlock operation, this bit will not change until the next system reset.

	set flash_ctrl_data [ get_reg_value [ $_TARGET mdb $FLASH_CTLR_ADDR ] ]
	if { [ expr {$flash_ctrl_data & $LOCK_BIT_MASK} ] == $LOCK_BIT_MASK } {
		echo "** \[$op\] FLASH_CTLR are locked. Need Release Flash Memory. Releasing ... **"		
		_unlock_flash $_TARGET
	} 
	
	# 2)
	# Check the BSY bit in the FLASH_STATR register to ensure that
	# there is no programming operation in progress.

	set timeout $TIMEOUT
	while { 1 } {
		set flash_statr_data [ get_reg_value [ $_TARGET mdb $FLASH_STATR_ADDR ] ]
		
		if { [ expr {$flash_statr_data & $BSY_BIT_MASK} ] != $BSY_BIT_MASK } {
		    break
		}

		if {$timeout == $TIMEOUT} {
			echo "** \[$op\] Flash memory operation is in the process. Waiting ... **"
		} elseif {$timeout == 0} {
			echo "\[$op\] !Err 1: Fail flash processing **"
			return 
		}

		incr timeout -$TIMEOUT_STEP
		after $TIMEOUT_STEP
	}

	# 3)
	# Check the OBWRE bit in the FLASH_CTLR register.
	# If it is 0, you need to perform the "User Option Bytes Unlock" operation.

	set flash_ctrl_data [ get_reg_value [ $_TARGET mdw $FLASH_CTLR_ADDR ] ]
	if { [ expr {$flash_ctrl_data & $OBWRE_BIT_MASK} ] == 0 } {
		echo "** \[$op\] User option bytes is locked **"
		_unlock_user_ob $_TARGET
	}

	# 4)
	# Set the OBPG bit in the FLASH_CTLR register to `1`.
	
	set flash_ctrl_data [ get_reg_value [ $_TARGET mdh $FLASH_CTLR_ADDR ] ]
    $_TARGET mwh $FLASH_CTLR_ADDR [expr {$flash_ctrl_data | $OBPG_BIT_MASK}]

	# 5)
	# Write the half word (2 bytes) to be programmed to the designated address.
	
    # Set correct RDPR code 0xA5 to the User Option Bytes
    $_TARGET mwh $RDPR_ADDR $RDPR_MASK_LOCK

	# 6)
	# When the BSY bit changes to '0' or the EOP bit in the FLASH_STATR register to be '1',
	# it indicates the end of programming. Clear the EOP bit to 0.
	set timeout $TIMEOUT
    while { 1 } {
    	set flash_statr_data [ get_reg_value [ $_TARGET mdb $FLASH_STATR_ADDR ] ]
    	
    	if {[ expr {$flash_statr_data & $BSY_BIT_MASK} ] != $BSY_BIT_MASK || [expr {$flash_statr_data & $EOP_BIT_MASK}] == $EOP_BIT_MASK} {
    		# Clear the EOP bit to 0.
			$_TARGET mww $FLASH_STATR_ADDR [expr {$flash_statr_data & !$EOP_BIT_MASK}]
			break
    	} 

		if {$timeout == $TIMEOUT} {
			echo "** \[$op\] Flash memory operation is in the process. Waiting ... **"
		} elseif {$timeout == 0} {
			echo "\[$op\] !Err 2: Fail flash processing **"
			return 
		}
		
		incr timeout -$TIMEOUT_STEP
		after $TIMEOUT_STEP
	}

	# 7)
	# Read the programming address data for verification.
	
    # State of RDPR byte in the User Option Bytes
    # RDPR_ADDR should be `5aa5` already
    set result [ get_reg_value [ $_TARGET mdh $RDPR_ADDR ] ]
    if { $result == $RDPR_MASK_LOCK} {
    	echo "** \[$op\] Memory unlocked **"
    } else {
    	echo "\[$op\] !Err 3: Fail memory unlocking **"
		return 
    }
}

proc lock { } {
	set op "LOCK"
	
	global _TARGET

    global FLASH_CTLR_ADDR    
	global FLASH_STATR_ADDR
	global FLASH_OBR_ADDR
	global RDPR_ADDR			

	# Status Register (FLASH_STATR)
	global BSY_BIT_MASK	
	global EOP_BIT_MASK

	# Control Register (FLASH_CTLR)
	set OBER_BIT_MASK	[expr {1 << 5}]
	set STRT_BIT_MASK	[expr {1 << 6}]
	global LOCK_BIT_MASK	
	global OBWRE_BIT_MASK
	global RDPRT_BIT_MASK

	global RDPR_MASK_LOCK
	set RDPR_MASK_UNLOCK		0xe339

	# ms
	global TIMEOUT_STEP	
	global TIMEOUT		


	# Read protection status.
	# 1: Current read protection of flash is valid.
    set RDPRT_byte [ get_reg_value [ $_TARGET mdb $FLASH_OBR_ADDR ] ]
    if { [ expr {$RDPRT_byte & $RDPRT_BIT_MASK} ] == $RDPRT_BIT_MASK } {
        echo "** \[$op\] Memory is locked already **"
        return
    }

    # 1)
	# Check the LOCK bit in the FLASH_CTLR register.
	# If it is 1, you need to perform the "Release Flash Memory Lock" operation.

	set flash_ctrl_data [ get_reg_value [ $_TARGET mdb $FLASH_CTLR_ADDR ] ]
	if { [ expr {$flash_ctrl_data & $LOCK_BIT_MASK} ] == $LOCK_BIT_MASK } {
		echo "** \[$op\] FLASH_CTLR are locked. Need Release Flash Memory. Releasing ... **"
		_unlock_flash $_TARGET
	}	

    # 2)
	# Check the BSY bit in the FLASH_STATR register to ensure that
	# there is no programming operation in progress.

	set timeout $TIMEOUT
	while { 1 } {
		set flash_statr_data [ get_reg_value [ $_TARGET mdh $FLASH_STATR_ADDR ] ]
		
		if { [ expr {$flash_statr_data & $BSY_BIT_MASK} ] != $BSY_BIT_MASK } {
		    break
		}

		if {$timeout == $TIMEOUT} {
			echo "** \[$op\] Flash memory operation is in the process. Waiting ... **"
		} elseif {$timeout == 0} {
			echo "\[$op\] !Err 1: Fail flash processing **"
			return 
		}

		incr timeout -$TIMEOUT_STEP
		after $TIMEOUT_STEP
	}

	# 3)
	# Check the OBWRE bit in the FLASH_CTLR register.
	# If it is 0, you need to perform the "User Option Bytes Unlock" operation.

	set flash_ctrl_data [ get_reg_value [ $_TARGET mdh $FLASH_CTLR_ADDR ] ]
	if { [ expr {$flash_ctrl_data & $OBWRE_BIT_MASK} ] == 0 } {
		echo "** \[$op\] User option bytes is locked. Need to unlock. Unlocking ... **"
		_unlock_user_ob $_TARGET
	}

	# 4)
	# Set the OBER bit in the FLASH_CTLR register to ‘1’,
	# then set the STRT bit in the FLASH_CTLR register to ‘1’,
	# to enable the user option bytes erasure.
	
    # Write OBER bit to 1 in FLASH_CTLR
    $_TARGET mwb $FLASH_CTLR_ADDR $OBER_BIT_MASK

    # Write STRT bit to 1 in FLASH_CTLR
    $_TARGET mwb $FLASH_CTLR_ADDR $STRT_BIT_MASK

    # 5)
	# When the BSY bit changes to '0' or the EOP bit in the FLASH_STATR register to be '1',
	# it indicates the end of programming. Clear the EOP bit to 0.
	set timeout $TIMEOUT
    while { 1 } {
    	set flash_statr_data [ get_reg_value [ $_TARGET mdb $FLASH_STATR_ADDR ] ]
    	
    	if {[ expr {$flash_statr_data & $BSY_BIT_MASK} ] != $BSY_BIT_MASK || [expr {$flash_statr_data & $EOP_BIT_MASK}] == $EOP_BIT_MASK} {
    		# Clear the EOP bit to 0.
			$_TARGET mww $FLASH_STATR_ADDR [expr {$flash_statr_data & !$EOP_BIT_MASK}]
			break
    	} 

		if {$timeout == $TIMEOUT} {
			echo "** \[$op\] Flash memory operation is in the process. Waiting ... **"
		} elseif {$timeout == 0} {
			echo "\[$op\] !Err 2: Fail flash processing **"
			return 
		}
		
		incr timeout -$TIMEOUT_STEP
		after $TIMEOUT_STEP
	}

	# 6)
	# Read the erasure address data for verification.
	
    # State of RDPR byte in the User Option Bytes
    # 0x1ffff800: e339e339
    set result [ get_reg_value [ $_TARGET mdh $RDPR_ADDR ] ]
    if {$result == $RDPR_MASK_UNLOCK} {
    	echo "** \[$op\] Flash succefully locked **"
    } else {
    	echo "\[$op\] !Err 3: Fail flash locking **"
    }

    # Clear the OBER bit to 0.
	set flash_statr_data [ get_reg_value [ $_TARGET mdw $FLASH_CTLR_ADDR ] ]
	$_TARGET mww $FLASH_CTLR_ADDR [expr {$flash_statr_data & !$OBER_BIT_MASK}]
}