# Copyright 2018-2021, 2023-2025 NXP

# SPDX-License-Identifier: BSD-3-Clause

import gdb
from errors import Result, Error, ErrorCode, Ok, traced_call
from loggers import islogger

"""
/* Initialization sequence */
#define ccs_init_action_start                       0x00000000
#define ccs_init_action_pause                       0x00000001
#define ccs_init_action_write                       0x00000002
#define ccs_init_action_set                         0x00000003
#define ccs_init_action_check                       0x00000004
#define ccs_init_action_apsel                       0x00000005
#define ccs_init_action_write_jtag                  0x00000006
#define ccs_init_action_check_jtag                  0x00000007
#define ccs_init_action_write_mdm                   0x00000008
#define ccs_init_action_check_mdm                   0x00000009
#define ccs_init_action_jtag_pin                    0x0000000A
#define ccs_init_action_reset_tap                   0x0000000B
#define ccs_init_action_power_dap                   0x0000000C
#define	ccs_init_action_check_reg_safe              0x0000000D
#define	ccs_init_action_write_sda                   0x0000000E
#define ccs_init_action_fill                        0x0000000F
#define ccs_init_action_write_reg                   0x00000010
#define ccs_init_action_check_reg                   0x00000011
#define ccs_init_action_write_without_error_check   0x00000012
#define ccs_init_action_enable_debug                0x00000013
#define ccs_init_action_core_mode_config			0x00000014
#define ccs_init_action_core_status_config			0x00000015
#define ccs_init_action_write_and_check_mdm			0x00000016
#define ccs_init_action_put_label                   0x00000017
#define ccs_init_action_branch_equal,				0x00000018
#define ccs_init_action_branch_not_equal,			0x00000019
#define ccs_init_action_send_tms,                   0x0000001A
#define ccs_init_action_custom_core,                0x0000001B
#define ccs_init_action_custom_subcore,             0x0000001C
#define ccs_init_action_custom_set_ap,              0x0000001D
#define ccs_init_action_last_command				0x0000001E
"""
# When this variable is se tot True, every init action command will be displayed in GDB console.
EnableConsoleLoggingOutput = False


@traced_call
def sequence_start():
    if EnableConsoleLoggingOutput:
        print(
            "monitor ctx id :ccs set prop ccs_mc_me_str  0  0           0  0           0           0"
        )
    islogger.log("sequence_start")

    return _run(
        "monitor ctx id :ccs set prop ccs_mc_me_str  0  0           0  0           0           0"
    )


@traced_call
def pause(timeout_ms):
    init_str = "monitor ctx id :ccs set prop ccs_mc_me_str  1  "
    init_str = init_str + timeout_ms + "  0  0           0           0"
    if EnableConsoleLoggingOutput:
        print(init_str)

    islogger.log("pause", timeout_ms=timeout_ms)
    return _run(init_str)


@traced_call
def write_reg(addr, value, size="4"):
    (vhi, vlow) = _val2hilow(value)
    init_str = "monitor ctx id :ccs set prop ccs_mc_me_str  2  "
    init_str = init_str + addr + "  "
    init_str = init_str + size + "  "
    init_str = init_str + vhi + "  "
    init_str = init_str + vlow + "   0"
    if EnableConsoleLoggingOutput:
        print(init_str)
    islogger.log("write_reg", addr=addr, value=value, size=size)
    return _run(init_str)


@traced_call
def write_reg32(addr, value):
    init_str = "monitor ctx id :ccs set prop ccs_mc_me_str  2  "
    init_str = init_str + addr + "  "
    init_str = init_str + "4" + "  "
    init_str = init_str + value + "  0           0  "
    if EnableConsoleLoggingOutput:
        print(init_str)
    islogger.log("write_reg32", addr=addr, value=value)
    return _run(init_str)


@traced_call
def write_reg64(addr, vhigh, vlow):
    init_str = "monitor ctx id :ccs set prop ccs_mc_me_str  2  "
    init_str = init_str + addr + "  "
    init_str = init_str + "8" + "  "
    init_str = init_str + vhigh + "  "
    init_str = init_str + vlow + "  0  "
    if EnableConsoleLoggingOutput:
        print(init_str)
    islogger.log("write_reg64", addr=addr, vhigh=vhigh, vlow=vlow)
    return _run(init_str)


@traced_call
def set_reg(addr, mask, value, size="4"):
    init_str = "monitor ctx id :ccs set prop ccs_mc_me_str  3  "
    init_str = init_str + addr + "  "
    init_str = init_str + size + "  "
    init_str = init_str + mask + "  "
    init_str = init_str + value + "  0"
    if EnableConsoleLoggingOutput:
        print(init_str)
    islogger.log("set_reg", addr=addr, mask=mask, 
                 value=value, size=size)
    return _run(init_str)


@traced_call
def check_reg(addr, mask, value, size="4", timeout="1"):
    init_str = "monitor ctx id :ccs set prop ccs_mc_me_str  4  "
    init_str = init_str + addr + "  "
    init_str = init_str + size + "  "
    init_str = init_str + mask + "  "
    init_str = init_str + value + "  "
    init_str = init_str + timeout
    if EnableConsoleLoggingOutput:
        print(init_str)

    islogger.log("check_reg", addr=addr, mask=mask, 
                 value=value, size=size, timeout=timeout)

    return _run(init_str)


@traced_call
def ahb_ap_select(dap_master_apsel_value):
    init_str = "monitor ctx id :ccs set prop ccs_mc_me_str  5  "
    init_str = init_str + dap_master_apsel_value + "  0  0           0           0"
    if EnableConsoleLoggingOutput:
        print(init_str)
    islogger.log("ahb_ap_select", dap_master_apsel_value=dap_master_apsel_value)
    return _run(init_str)


@traced_call
def write_jtag(reg_num, value_0_31, value_32_63="0", size="4"):
    init_str = "monitor ctx id :ccs set prop ccs_mc_me_str  6  "
    init_str = init_str + reg_num + "           "
    init_str = init_str + size + "  "
    init_str = init_str + value_0_31 + "  "
    init_str = init_str + value_32_63 + "  0"
    if EnableConsoleLoggingOutput:
        print(init_str)
    islogger.log("write_jtag", reg_num=reg_num, value_0_31=value_0_31, 
                 value_32_63=value_32_63, size=size)

    return _run(init_str)


@traced_call
def check_jtag(jtag_id_register, mask, value, size="4", timeout="1"):
    init_str = "monitor ctx id :ccs set prop ccs_mc_me_str  7  "
    init_str = init_str + jtag_id_register + "           "
    init_str = init_str + size + "  "
    init_str = init_str + mask + "           "
    init_str = init_str + value + "           "
    init_str = init_str + timeout
    if EnableConsoleLoggingOutput:
        print(init_str)
    islogger.log("check_jtag", jtag_id_register=jtag_id_register, mask=mask, 
                 value=value, size=size, timeout=timeout)

    return _run(init_str)


@traced_call
def write_mdm(reg_number, value):
    init_str = "monitor ctx id :ccs set prop ccs_mc_me_str  8  "
    init_str = init_str + reg_number + "            0 "
    init_str = init_str + value + "   0           0"
    if EnableConsoleLoggingOutput:
        print(init_str)
    islogger.log("write_mdm", reg_number=reg_number, value=value)
    return _run(init_str)


@traced_call
def check_mdm(jtag_id_register, mask, value, timeout="1"):
    init_str = "monitor ctx id :ccs set prop ccs_mc_me_str  9  "
    init_str = init_str + jtag_id_register + "           0  "
    init_str = init_str + mask + "  "
    init_str = init_str + value + "  "
    init_str = init_str + timeout
    if EnableConsoleLoggingOutput:
        print(init_str)
    islogger.log("check_mdm", jtag_id_register=jtag_id_register, 
                 mask=mask, value=value, timeout=timeout)
    return _run(init_str)


@traced_call
def set_jtag_pin0(value):
    init_str = "monitor ctx id :ccs set prop ccs_mc_me_str  10  "
    init_str = init_str + value + "           0  0           0           0"
    if EnableConsoleLoggingOutput:
        print(init_str)
    islogger.log("set_jtag_pin0", value=value)
    return _run(init_str)


@traced_call
def reset_tap():
    if EnableConsoleLoggingOutput:
        print(
            "monitor ctx id :ccs set prop ccs_mc_me_str 11  0           0  0           0           0"
        )
    islogger.log("reset_tap")
    return _run(
        "monitor ctx id :ccs set prop ccs_mc_me_str 11  0           0  0           0           0"
    )


@traced_call
def power_up_dap():
    if EnableConsoleLoggingOutput:
        print(
            "monitor ctx id :ccs set prop ccs_mc_me_str 12  0           0  0           0           0"
        )
    islogger.log("power_up_dap")
    return _run(
        "monitor ctx id :ccs set prop ccs_mc_me_str 12  0           0  0           0           0"
    )


@traced_call
def check_reg_safe(addr, mask, value, size="4", timeout="100"):
    init_str = "monitor ctx id :ccs set prop ccs_mc_me_str  13  "
    init_str = init_str + addr + "  "
    init_str = init_str + size + "  "
    init_str = init_str + mask + "  "
    init_str = init_str + value + "  "
    init_str = init_str + timeout
    if EnableConsoleLoggingOutput:
        print(init_str)
    islogger.log("check_reg_safe", addr=addr, mask=mask,
                  value=value, size=size, timeout=timeout)
    return _run(init_str)


@traced_call
def write_sda(addr, value):
    init_str = "monitor ctx id :ccs set prop ccs_mc_me_str  14  "
    init_str = init_str + addr + "             0 "
    init_str = init_str + value + "   0           0"
    if EnableConsoleLoggingOutput:
        print(init_str)
    islogger.log("write_sda", addr=addr, value=value)
    return _run(init_str)


@traced_call
def fill_mem(addr, size, count, vlow, vhigh):
    init_str = "monitor ctx id :ccs set prop ccs_mc_me_str  15  "
    init_str = init_str + addr + "  "
    init_str = init_str + size + "  "
    init_str = init_str + count + "  "
    init_str = init_str + vlow + "  "
    init_str = init_str + vhigh + "  "
    if EnableConsoleLoggingOutput:
        print(init_str)
    islogger.log("fill_mem", addr=addr, size=size,
                  count=count, vlow=vlow, vhigh=vhigh)
    return _run(init_str)


@traced_call
def write_register(reg_number, value):
    init_str = "monitor ctx id :ccs set prop ccs_mc_me_str  16  "
    init_str = init_str + reg_number + "            0 "
    init_str = init_str + value + "   0           0"
    if EnableConsoleLoggingOutput:
        print(init_str)
    islogger.log("write_register", reg_number=reg_number, value=value)
    return _run(init_str)


@traced_call
def check_register(jtag_id_register, mask, value, timeout="1"):
    init_str = "monitor ctx id :ccs set prop ccs_mc_me_str  17  "
    init_str = init_str + jtag_id_register + "           0  "
    init_str = init_str + mask + "  "
    init_str = init_str + value + "  "
    init_str = init_str + timeout
    if EnableConsoleLoggingOutput:
        print(init_str)
    islogger.log("check_register", jtag_id_register=jtag_id_register,
                  mask=mask, value=value, timeout=timeout)
    return _run(init_str)


@traced_call
def write_reg_without_error_check(addr, value, size="4"):
    if size == "4" or size == "2" or size == "1":
        init_str = "monitor ctx id :ccs set prop ccs_mc_me_str 18  "
        init_str = init_str + addr + "  "
        init_str = init_str + size + "  "
        init_str = init_str + value + "  0           0  "
    else:
        (vhi, vlow) = _val2hilow(value)
        init_str = "monitor ctx id :ccs set prop ccs_mc_me_str 18  "
        init_str = init_str + addr + "  "
        init_str = init_str + size + "  "
        init_str = init_str + vhi + "  "
        init_str = init_str + vlow + "   0"
    if EnableConsoleLoggingOutput:
        print(init_str)
    islogger.log("write_reg_without_error_check", addr=addr,
                  value=value, size=size)
    return _run(init_str)


@traced_call
def init_action_enable_debug():
    if EnableConsoleLoggingOutput:
        print(
            "monitor ctx id :ccs set prop ccs_mc_me_str 19  0           0  0           0           0"
        )
    islogger.log("init_action_enable_debug")
    return _run(
        "monitor ctx id :ccs set prop ccs_mc_me_str 19  0           0  0           0           0"
    )


@traced_call
def core_mode_config(setting, mode):
    init_str = "monitor ctx id :ccs set prop ccs_mc_me_str  20  "
    init_str = init_str + setting + "           "
    init_str = init_str + mode + "  "
    init_str = init_str + "0           0           0"
    if EnableConsoleLoggingOutput:
        print(init_str)
    islogger.log("core_mode_config", setting=setting, mode=mode)
    return _run(init_str)


@traced_call
def core_status_config(state, apsel, addr):
    init_str = "monitor ctx id :ccs set prop ccs_mc_me_str  21  "
    init_str = init_str + state + "           "
    init_str = init_str + apsel + "  "
    init_str = init_str + addr + "           "
    init_str = init_str + "0           0"
    if EnableConsoleLoggingOutput:
        print(init_str)
    islogger.log("core_status_config", state=state, apsel=apsel, addr=addr)
    return _run(init_str)


@traced_call
def write_and_check_mdm(reg_number, mask, value, timeout="1"):
    init_str = "monitor ctx id :ccs set prop ccs_mc_me_str  22  "
    init_str = init_str + reg_number + "           0  "
    init_str = init_str + mask + "  "
    init_str = init_str + value + "  "
    init_str = init_str + timeout
    if EnableConsoleLoggingOutput:
        print(init_str)
    islogger.log("write_and_check_mdm", reg_number=reg_number, 
                 mask=mask, value=value, timeout=timeout)
    return _run(init_str)


@traced_call
def put_label(label):
    init_str = "monitor ctx id :ccs set prop ccs_mc_me_str  23  "
    init_str = init_str + label + "  "
    init_str = init_str + "0  0           0           0"
    if EnableConsoleLoggingOutput:
        print(init_str)
    islogger.log("put_label", label=label)
    return _run(init_str)


@traced_call
def branch_equal(check, addr, mask, value, size, timeout, label):
    init_str = "monitor ctx id :ccs set prop ccs_mc_me_str  24  "
    init_str = init_str + addr + "  "
    init_str = init_str + size + "  "
    init_str = init_str + mask + "  "
    init_str = init_str + value + "  "
    init_str = init_str + timeout + "  "
    if check == "mem":
        init_str = init_str + "4  "
    elif check == "reg":
        init_str = init_str + "17  "
    else:
        return Result(
            Error(
                ErrorCode.CoreInitializationFailed,
                "Invalid branch check! Check: mem/reg",
            )
        )
    init_str = init_str + label
    if EnableConsoleLoggingOutput:
        print(init_str)
    islogger.log("branch_equal", check=check, addr=addr, mask=mask, 
                 value=value, size=size, timeout=timeout, label=label)
    return _run(init_str)


@traced_call
def branch_not_equal(check, addr, mask, value, size, timeout, label):
    init_str = "monitor ctx id :ccs set prop ccs_mc_me_str  25  "
    init_str = init_str + addr + "  "
    init_str = init_str + size + "  "
    init_str = init_str + mask + "  "
    init_str = init_str + value + "  "
    init_str = init_str + timeout + "  "
    if check == "mem":
        init_str = init_str + "4  "
    elif check == "reg":
        init_str = init_str + "17  "
    else:
        return Result(
            Error(
                ErrorCode.CoreInitializationFailed,
                "Invalid branch check! Check: mem/reg",
            )
        )
    init_str = init_str + label
    if EnableConsoleLoggingOutput:
        print(init_str)
    islogger.log("branch_not_equal", check=check, addr=addr, mask=mask, 
                 value=value, size=size, timeout=timeout, label=label)
    return _run(init_str)

@traced_call
def custom_core_mculink(coreid, ap, base_addr, redcoreid):
    init_str = "monitor ctx id :ccs set prop ccs_mc_me_str  27  "
    init_str = init_str + coreid + "  "
    init_str = init_str + ap + "  "
    init_str = init_str + base_addr + "  "
    init_str = init_str + redcoreid + "  0"
    if EnableConsoleLoggingOutput:
        print(init_str)
    islogger.log("custom_core_mculink", coreid=coreid, ap=ap, base_addr=base_addr,
                  redcoreid=redcoreid)
    return _run(init_str)

@traced_call
def custom_core_s32dbg(coreid, debug_ap, memory_ap, base_addr):
    init_str = "monitor ctx id :ccs set prop ccs_mc_me_str  27  "
    init_str = init_str + coreid + "  "
    init_str = init_str + debug_ap + "  "
    init_str = init_str + memory_ap + "  "
    init_str = init_str + base_addr + "  0"
    if EnableConsoleLoggingOutput:
        print(init_str)
    islogger.log("custom_core_s32dbg", coreid=coreid, debug_ap=debug_ap,
                  memory_ap=memory_ap, base_addr=base_addr)
    return _run(init_str)

@traced_call
def custom_subcore_s32dbg(coreid, memory_ap, base_addr):
    init_str = "monitor ctx id :ccs set prop ccs_mc_me_str  28  "
    init_str = init_str + coreid + "  "
    init_str = init_str + memory_ap + "  "
    init_str = init_str + base_addr + "  0  0"
    if EnableConsoleLoggingOutput:
        print(init_str)
    islogger.log("custom_subcore_s32dbg", coreid=coreid, memory_ap=memory_ap, base_addr=base_addr)
    return _run(init_str)

@traced_call
def custom_set_ap(apsel, value):
    init_str = "monitor ctx id :ccs set prop ccs_mc_me_str  29  "
    init_str = init_str + apsel + "  "
    init_str = init_str + value + "  0  0  0"
    if EnableConsoleLoggingOutput:
        print(init_str)
    islogger.log("custom_set_ap", apsel=apsel, value=value)
    return _run(init_str)

@traced_call
def _val2hilow(v):
    val_hex = int(v, 16)
    vhi = val_hex >> 32
    vlow = val_hex & 0x00000000FFFFFFFF
    (vhi, vlow) = (vlow, 0) if vhi == 0 else (vhi, vlow)
    vhi = "0x%X" % vhi
    vlow = "0x%X" % vlow
    return vhi, vlow


def _run(cmd):
    try:
        gdb.execute(cmd)
    except BaseException as e:
        Result(Error(ErrorCode.UndefinedError, str(e))).unwrap()
    return Result(Ok(None))
