MustafaMulla29/rp2040-bldc-motor-controller-new

firmware/pd_profile.py

"""Selectable 5/9/15/20 V motor supply; nonvolatile startup remains 9 V for the STUSB4500.

NVM field locations: SparkFun STUSB4500 library (manufacturer of a board using
this IC), and ST's STUSB4500 NVM_Flasher. Preserve all unrelated factory bits.
https://github.com/sparkfun/SparkFun_STUSB4500_Arduino_Library
https://github.com/usb-c/STUSB4500/tree/master/NVM_Flasher
"""
from power_policy import fixed_sink_pdo, DEFAULT_MV, CONTRACT_MA

def profiles(selected_mv=DEFAULT_MV):
    # Mandatory PDO1 is the low-current fallback. PDO2 is the selected motor
    # contract even at 5 V. Inactive PDO3 mirrors PDO2 to avoid a latent profile.
    motor = fixed_sink_pdo(selected_mv, CONTRACT_MA)
    return (fixed_sink_pdo(5000, 500, True), motor, motor)

PDOS = profiles()  # Backwards-compatible name for the 9 V cold-start image.


def nvm_9v_image(original):
    if len(original) != 40:
        raise ValueError("STUSB4500 requires five 8-byte NVM sectors")
    result = bytearray(original)
    # Sector 3 byte 2: 0.5 A PDO1, two active PDOs, USB data capable.
    result[26] = 0x15  # also clear the externally/unconstrained-powered flag
    result[28] = (result[28] & 0xF0) | 7  # 2 A PDO2 (LUT code 7)
    result[29] = (result[29] & 0x0F) | 0x70  # 2 A inactive PDO3
    # 9 V = 180 units of 50 mV. Both flexible voltage fields are 10 bits.
    result[32] &= 0x3F
    result[33] = 45
    result[34] = 180
    result[35] &= 0xFC
    # ST NVM register map: E4[6:5]=10 means POWER_OK2 reports PDO2.
    result[36] = (result[36] & ~0x60) | 0x40
    # E6[4]=1: VBUS_EN_SNK only with an explicit PDO2/PDO3 contract.
    # Despite POWER_ONLY_ABOVE_5V naming, DS12499 Rev 8 Table 6 specifies PDO index. E6[3]=0:
    # request our programmed current. Logic is supplied BEFORE this switch.
    result[38] = (result[38] & ~0x18) | 0x10
    return bytes(result)


def nvm_is_9v(image):
    return len(image) == 40 and bytes(image) == nvm_9v_image(image)


def verify_runtime(bus, address=0x28, selected_mv=DEFAULT_MV):
    expected_pdos = profiles(selected_mv)
    if bus.readfrom_mem(address, 0x70, 1)[0] & 7 != 2:
        raise OSError("PD must have exactly two active profiles")
    # Ignore other capability flags when checking voltage/current/type fields.
    mask = 0xC00FFFFF
    for reg, expected in zip((0x85, 0x89, 0x8D), expected_pdos):
        actual = int.from_bytes(bus.readfrom_mem(address, reg, 4), "little")
        if (actual & mask) != (expected & mask):
            raise OSError("PD voltage/current profile changed; motor prohibited")
    if not int.from_bytes(bus.readfrom_mem(address, 0x85, 4), "little") & (1 << 26):
        raise OSError("PD USB data capability is not configured")


def configure_runtime(bus, address=0x28, selected_mv=DEFAULT_MV):
    """Low-level write; caller MUST disarm and verify motor rail discharged."""
    expected_pdos = profiles(selected_mv)
    # Restrict to mandatory PDO1 while replacing the other profiles.
    old = bus.readfrom_mem(address, 0x70, 1)[0]
    bus.writeto_mem(address, 0x70, bytes(((old & ~7) | 1,)))
    for reg, expected in zip((0x85, 0x89, 0x8D), expected_pdos):
        data = expected.to_bytes(4, "little")
        bus.writeto_mem(address, reg, data)
        if bus.readfrom_mem(address, reg, 4) != data:
            raise OSError("PD profile write did not verify")
    bus.writeto_mem(address, 0x70, bytes(((old & ~7) | 2,)))
    verify_runtime(bus, address, selected_mv)
    bus.writeto_mem(address, 0x51, b"\x0d")
    bus.writeto_mem(address, 0x1A, b"\x26")  # PD Soft_Reset, NOT an NVM reload