# Copyright (c) 2020 P&E Microcomputer Systems, Inc
# All rights reserved.
# Visit us at www.pemicro.com
#
# Redistribution and use in source and binary forms, with or without modification,
# are permitted provided that the following conditions are met:
#
# o Redistributions of source code must retain the above copyright notice, this list
#   of conditions and the following disclaimer.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
# ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
# (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
# ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
# SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

# Version 1.00

'''
 Authenticate device secured in Challenge/Response mode
 (Python 3.5 or higher required)
 This script requires an ADKP to be registered before running. See register_adkp.py.
 This script requires one parameter:
 -hardwareid: the debug hardware IP address, name, serial number, or port name (e.g. -hardwareid=10.0.4.17)
 This script has one optional parameter:
 -user_pwd: if a smart card is being used and is password protected, the password must be provided (e.g. -user_pwd=pwd123)
'''

from pemicroprobewrapper import pemicroProbeWrapper
from pemicrounitacmp import pemicroUnitAcmp
from ctypes import *
import sys
import os


''' Constants '''

VOLKANO_DLL = 'volkano/volkano.dll'


''' Functions: '''

# Load DLL Library
dll_path = os.path.dirname(os.path.abspath(__file__)) + os.path.sep + VOLKANO_DLL
libc = cdll.LoadLibrary(dll_path)

# Initialize DLL external functions
libc.vlk_init.argtypes = [c_char_p]
libc.vlk_init.restype = c_ulong
libc.vlk_get_response.argtypes = [POINTER(c_ubyte),c_ulonglong,POINTER(c_ubyte),c_ulonglong,POINTER(c_ubyte)]
libc.vlk_get_response.restype = c_ulong

# Create internal functions
def volkanoInit(pwd):
    cwd = os.getcwd()
    os.chdir('volkano')
    return libc.vlk_init(c_char_p(pwd.encode('utf-8')))
    os.chdir(cwd)

def computeResponse(challenge, uid):
    retValue = 0
    
    uid = uid.ljust(16,'0')
    uidList = [0]*8
    for i in range(0,8):
        uidList[i] = int(uid[i*2:i*2+2], 16)
    uidBuffer = bytearray(uidList)
    uidSize = c_ulonglong(8)
    
    challenge = challenge.rjust(64, '0')
    chalList = [0]*32
    for i in range(0,32):
        chalList[i] = int(challenge[i*2:i*2+2], 16)
    chalBuffer = bytearray(chalList)
    chalSize = c_ulonglong(32)
    
    respBuffer = bytearray(32)
    
    retValue = libc.vlk_get_response((c_ubyte*8).from_buffer(uidBuffer), uidSize, (c_ubyte*32).from_buffer(chalBuffer), chalSize, (c_ubyte*32).from_buffer(respBuffer))

    return (retValue, respBuffer)


''' Get parameter (e.g. -hardwareid=10.0.4.17) '''

print('')
print('Authenticating S32K3xx in Challenge/Response Mode.')
print('')

hardwareid=''
user_pwd=''
for i in range(1, len(sys.argv)):
    if sys.argv[i].split('=')[0] == "-hardwareid":
        if len(sys.argv[i].split('=')) > 1: # if hardware ID is given
            hardwareid = sys.argv[i].split('=')[1]

    elif sys.argv[i].split('=')[0] == "-user_pwd":
        if len(sys.argv[i].split('=')) > 1: # if user password ID is given
            user_pwd = sys.argv[i].split('=')[1]

if hardwareid=='':
    print('')
    print('Error - Please specify the debug hardware IP address, name, serial number, or generic port name (such as USB1).')
    print('        Note that the underlying class does not require this (it will assume USB1), but this test file requires it.')
    print('')
    print('Examples of specifying the HW Identifier :')
    print('')
    print('             python pemicrounitacmptest.py -hardwareid=USB1                ; Opens First PEmicro HW detected on USB bus')
    print('             python pemicrounitacmptest.py -hardwareid=PEM592731           ; Opens PEmicro HW with Serial Number PEM592731')
    print('             python pemicrounitacmptest.py -hardwareid=10.0.4.17           ; Opens PEmicro HW with IP 10.0.4.17')
    print('             python pemicrounitacmptest.py -hardwareid=MyCyclone           ; Opens PEmicro HW with assigned Name MyCyclone')
    print('             python pemicrounitacmptest.py -hardwareid=ETHERNET1           ; Opens First PEmicro HW detected on Ethernet')
    print('')
    print('Current List of autodetected PEmicro Hardware :')
    print('')
    pemicroUnitAcmp().listPortsDescription()
    sys.exit(1)


''' Power up DAP main system domain and debug domain. '''

myprobe = pemicroProbeWrapper(0x07,hardwareid)

if not myprobe.connectedToDebugHardware:
    print ('Error ... Unable to connect to PEMicro HW',hardwareid)
    sys.exit(1)
else:
    print ('Connecting to debug hardware ... ok.')    

if not myprobe.connectedToTarget:
    print ('Error ... Unable to establish communications with target MCU')
    sys.exit(1)
else:
    print ('Requesting Debug Module Power On via SWD ... ok.')


''' Initialize Volkano DLL '''

retResult = volkanoInit(user_pwd)
if retResult != 0:
    print ('Initializing ... ERROR !!!!!!!!!!!!!!!!!!!!! (Error code: ', retResult,')')
    sys.exit(1)
else:
    print ('Initializing ... ok.')


''' Read the challenge from KEYCHALn registers. The offset for a KEYCHALn register is 0x10 + n*0x4. '''

# Test that we can talk to the SDA AP by reading its ID value (should read 0x001C0040)
targetRead = myprobe.read_reg(0xFC)
if not ((targetRead==0x001C0040) and (myprobe.pemicroProbeLibrary.lastSwdStatus()==0x04)):
    print ('Reading SDA AP ID Register ... ERROR !!!!!!!!!!!!!!!!!!!!! . (', hex(targetRead),')')
    sys.exit(1)
else:
    print ('Reading SDA AP ID Register ... ok.')

# Read KEYCHALn values
keystr = ['','','','','','','','']
for n in range(8):
    keyval = myprobe.read_reg(0x10+n*4)
    keystr[n] = hex(keyval).replace('0x','')
print ('Reading SDA AP KEYCHALn registers ... ok.')


# Change the endianness of each word read from KEYCHALn registers
challenge = ''
for n in range(8):
    challenge = challenge + keystr[n][6:8] + keystr[n][4:6] + keystr[n][2:4] + keystr[n][0:2]


''' Compute the response with Volkano '''

# Get UID value
uid0 = myprobe.read_reg(0x70)
uid0_str = hex(uid0)
uid1 = myprobe.read_reg(0x74)
uid1_str = hex(uid1)
uid_int = (uid1 << 32) | uid0
uid = hex(uid_int).replace('0x','')
print ('Reading SDA AP UID0 and UID1 ... ok.')

# Compute response
(retResult, retMessages) = computeResponse(challenge, uid)
if retResult != 0:
    print ('Computing response ... ERROR !!!!!!!!!!!!!!!!!!!!! (Error code: ', retResult,')')
    sys.exit(1)
else:
    print ('Computing response ... ok.')
response = retMessages.hex()


''' Write the response to the KEYRESPn registers. The offset for a KEYRESPn register is 0x40 + n*0x4. '''

# Make sure response contains 64 characters
response = response.rjust(64, '0')

for n in range(8):
    # Adjust endianness of response
    index = n*8
    keystr = response[index+6:index+8] + response[index+4:index+6] + response[index+2:index+4] + response[index+0:index+2]
    keyval = int(keystr, 16)
    
    # Write to KEYRESPn register
    myprobe.write_reg(0x40+n*4,keyval)
    targetRead = myprobe.read_reg(0x40+n*4)
    
    if (not ((targetRead==keyval) and (myprobe.pemicroProbeLibrary.lastSwdStatus()==0x04))):
        print ('Reading/Writing SDA AP KEYRESP'+str(n)+' ... ERROR !!!!!!!!!!!!!!!!!!!!! . (', hex(targetRead),')')
        sys.exit(1)
    else:
        print ('Reading/Writing SDA AP KEYRESP'+str(n)+' ... ok.')


''' Trigger the authentication by setting HSEAUTHREQ (bit 0) in AUTHCTL register (offset 0x4). '''

myprobe.write_reg(0x04,0x00000001) #set HSEAUTHREQ (0)
print ('Writing SDA AP AUTHCTL.HSEAUTHREQ ... ok.')


''' Set GDBGEN (bit 4) and CDBGEN (bit 28) in DBGENCTRL register (offset 0x80 in SDA memory map). Setting these bits ensure that the cores can be debugged. '''

myprobe.write_reg(0x80,0x10000010) #set GDBGEN (4), CDBGEN (28)
print ('Writing SDA AP DBGENCTRL.GDBGEN and DBGENCTRL.CDBGEN ... ok.')



# Test that we can talk to the SDA AP again by reading its ID value (should read 0x001C0040). If a wrong password is provided, this test will fail. 
targetRead = myprobe.read_reg(0xFC)
if not ((targetRead==0x001C0040) and (myprobe.pemicroProbeLibrary.lastSwdStatus()==0x04)):
    print ('Reading SDA AP ID Register ... ERROR !!!!!!!!!!!!!!!!!!!!! . (', hex(targetRead),')')
    sys.exit(1)
else:
    print ('Reading SDA AP ID Register ... ok.')

''' AUTHSTTS SDA register (offset 0) shows the authentication status. If the authentication was successful, APPDBGEN (bit 30) will be set. Otherwise, it remains cleared. On successful authentication, the debugger can access the application cores. '''
AUTHSTTS = myprobe.read_reg(0x0)
if ((AUTHSTTS & 0x60000000) != 0x60000000):
    print ('Checking Assertion of SDA AP AUTHSTTS.APPDBGEN ... ERROR !!!!!!!!!!!!!!!!!!!!! . (', hex(AUTHSTTS),')')
    sys.exit(1)
else:
    print ('Checking Assertion of SDA AP AUTHSTTS.APPDBGEN ... ok.')


myprobe.close()
del myprobe
print ('Success.') 
sys.exit(0)
