// An UDS device/bootloader simulation


// SVN $Date: 2022-08-11 13:44:19 +0200 (Do, 11. Aug 2022) $
// SVN $Rev: 41652 $
// SVN $Author: ged $

//
// this script partially simulates a very basic UDS devices
// only device addressing is supported
//

// configuration settings
const deviceCanID =  0x6a5
const toolCanID   =  0x680

// we use extended addressing with 1st data byte as part of an address
const deviceCanAddrByte = 0
const toolCanAddrByte = 0x25



////////////////////////////////////////////////////////////////////////////////////
var cnt = 0;
var blocksize1 = 0x0f;
var blocksize2 = 0xff;
var activeSession = 1;
var timerId = 0;
var securityOpen = false
var aKey = [0, 0, 0, 0]

// primary service handler to dispatch to different services
////////////////////////////////////////////////////////////////////////////////////
function udsServiceHandler (data) {


    // restart timeout timer with every message
    util.deleteTimer(timerId);
    timerId = util.after(14000, "handleTimeout()");

    srv = data[0];
	switch (srv) {
        case 0x10:
            util.print("UDS Service: " + util.formatString("0x%x", data[0]))
            diagnosticSessionControl(data);
            break;

        case 0x11:
            util.print("UDS Service: " + util.formatString("0x%x", data[0]))
            ecuResetHandler(data);
			break;

        case 0x14:
            util.print("UDS Service: " + util.formatString("0x%x", data[0]))
            clearDTCHandler(data);
            break;

        case 0x22:
            util.print("UDS Service: " + util.formatString("0x%x", data[0]))
           readDataByIdentifierHandler(data);
           break;

        case 0x23:
            util.print("UDS Service: " + util.formatString("0x%x", data[0]))
           readMemoryByAddressHandler(data);
           break;

        case 0x27:
            util.print("UDS Service: " + util.formatString("0x%x", data[0]))
            securityAccessHandler(data);
            break;

        case 0x28:
            util.print("UDS Service: " + util.formatString("0x%x", data[0]))
            communicationControl(data);
            break;

        case 0x2e:
           util.print("UDS Service: " + util.formatString("0x%x", data[0]))
           writeDataByIdentifierHandler(data);
           break;

        case 0x2f:
           util.print("UDS Service: " + util.formatString("0x%x", data[0]))
           ioControlByIdentifierHandler(data);
           break;

        case 0x31:
            util.print("UDS Service: " + util.formatString("0x%x", data[0]))
            routineControl(data);
            break;

        case 0x34:
            util.print("UDS Service: " + util.formatString("0x%x", data[0]))
            requestDownloadHandler(data);
            break;

        case 0x36:
            util.print("UDS Service: " + util.formatString("0x%x", data[0]))
            transferDataHandler(data);
            break;

        case 0x37:
            util.print("UDS Service: " + util.formatString("0x%x", data[0]))
            requestTransferExit(data);
            break;

        case 0x3D:
            util.print("UDS Service: " + util.formatString("0x%x", data[0]))
            writeMemoryByAddressHandler(data);
            break;

        case 0x3e:
            testerPresent(data);
            break;

        case 0x85:
            util.print("UDS Service: " + util.formatString("0x%x", data[0]))
            controlDTCSetting(data);
            break;

		default:
            util.print("Unsupported UDS service: " + util.formatString("0x%x",srv))
		break;
	}
}

function handleTimeout() {
    util.print("Timeout: Return to default session.");
    activeSession = 1;
    securityOpen = false;
}

function testerPresent(data)
{
    if ((data[1] & 0x80) === 0x80) {
        //util.print("  Positive response suppressed.");
    } else {
        util.print("TesterPresent received");
        // send some response
        resp = new ByteArray(2);
        resp[0] = 0x7e // response for testerPresent
        resp[1] = data[1] & 0x7f;
        isotp.startTransfer(resp, false);
    }
}

function routineControl(data)
{
    // just return something  with response srv code
    util.print("routine control");

    // if erase mememory was requested, we send 2 NegResp with rcrcp
    // to indicate that it takes longer
    sid = data[1]
    rid = (data[2] << 8) + (data[3])
    if ((sid == 0x1) && ((rid == 0xff01) || (rid == 0xff00))) {
        if (securityOpen === true) {
            resp = new ByteArray(3);
            resp[0] = 0x7f;
            resp[1] = 0x31;
            resp[2] = 0x78;
            isotp.startTransfer(resp, false);
            util.print(" neg resp RCRRP");

            util.msleep(50);

            resp = new ByteArray(5);
            resp[0] = 0x71;
            resp[1] = data[1];
            resp[2] = data[2];
            resp[3] = data[3];
            resp[4] = 0;
            util.msleep(1);
            isotp.startTransfer(resp, false);
            util.print(" pos resp");
        } else {
            resp = new ByteArray(3);
            resp[0] = 0x7f;
            resp[1] = 0x31;
            resp[2] = 0x33; // SAD .. security access denied
            util.msleep(1);
            isotp.startTransfer(resp, false);
            util.print(" neg resp SAD");
        }
    } else if  ((sid == 0x1) && (rid == 0xff02)) {
        // simulate an error here
        resp = new ByteArray(4);
        resp[0] = 0x7f;
        resp[1] = 0x31;
        resp[2] = 0x78;
        isotp.startTransfer(resp, false);
        util.print(" neg resp RCRRP");

        util.msleep(50);

        isotp.startTransfer(resp, false);
        util.print(" neg resp RCRRP");
        util.msleep(50);

        isotp.startTransfer(resp, false);
        util.print(" neg resp RCRRP");
        util.msleep(50);

    } else  {
        data[0] = 0x71;
        isotp.startTransfer(data, false);
        util.msleep(1);
    }
}

function requestTransferExit(data)
{
    // just return something  with response srv code
    util.print("requestTransferExit");

    data[0] = 0x77;

    isotp.startTransfer(data, false);
    util.msleep(1);
}

function controlDTCSetting(data)
{
    // just return something  with response srv code
    util.print("control DTC Setting");

    data[0] = 0xC5;

    util.msleep(10);
    isotp.startTransfer(data, false);
}

function communicationControl(data)
{
    // just return something  with response srv code
    util.print("Communication Control");

    data[0] = 0x68;

    // reduce blocksize to 512(4) for specific o
    //blocksize1 = 0x02;

    util.msleep(10);
    isotp.startTransfer(data, false);
}

function clearDTCHandler(data)
{
    // just return something  with response srv code
    util.print("Communication Control");

    data[0] = 0x54;


    util.msleep(10);
    isotp.startTransfer(data, false);

}

function transferDataHandler(data)
{
    // just return something block sequence counter
    util.print("transferData received: bsc = " + data[1]);

    resp = new ByteArray(2);
    resp[0] = 0x76 // resp
    resp[1] = data[1];

    isotp.startTransfer(resp, false);
    util.msleep(1);

}


function requestDownloadHandler(data)
{
    // just return something

    util.print("requestDownload: dataFormat = " + util.formatString("%x",data[1]) + " addressLength = "   + util.formatString("%x", data[2]));
    sizeLen = (data[2] & 0xf0) >> 4;
    addrLen = (data[2] & 0x0f);

    var Out = "Addr =";
    for (i = 0; i < addrLen; i++) {
        Out = Out + " " + util.formatString("%02x", data[3 + i])
    }
    util.print(Out);

    Out = "Size =";
    for (i = 0; i < sizeLen; i++) {
        Out = Out + " " + util.formatString("%02x", data[3 + addrLen + i])
    }
    util.print(Out);


    resp = new ByteArray(6);
    resp[0] = 0x74 // resp
    resp[1] = 0x40;
    resp[2] = 0x00;
    resp[3] = 0x00;
    resp[4] = blocksize1;
    resp[5] = blocksize2;

    util.msleep(100);
    isotp.startTransfer(resp, false);
}

function securityAccessHandler(data)
{
    // just return something
    util.print("security Access received: subfunction = " + data[1]);
    util.print(aKey[0]);
    util.print(aKey[1]);

    util.print(data[1]);
    util.print(data[2]);
    util.print(data[3]);

    var size = 0;
    if ((data[1] === 0x01) || (data[1] === 0x11)) {
        // request seed
        size = 8;
        resp = new ByteArray(10);
        resp[0] = 0x67; //
        resp[1] = data[1];

    } else {
        // check key
        if ((aKey[0] === data[2]) &&
            (aKey[1] === data[3])) {

            // key okay
            securityOpen = true;
            resp[0] = 0x67;
            resp[1] = data[1];

        } else {
            securityOpen = false;

            resp = new ByteArray(3);
            resp[0] = 0x7f; // neg. Resp
            resp[1] = 0x27; // neg. Resp
            resp[2] = 0x35; // invalid key

            util.msleep(50);
            isotp.startTransfer(resp, false);
            return;
        }
    }

    if ((data[1] & 0x80) === 0x80) {
        util.print("  Positive response suppressed.");
        cnt++;
        cnt++;
        cnt++;
        return;
    }

    isotp.startTransfer(resp, false);
    util.msleep(50);
}

function ioControlByIdentifierHandler(data)
{
    // just return something
    util.print("IOControlByIdentifier received. ID =  " + util.formatString("0x%02x%02x", data[1], data[2]))

    if ((data[2] !== 0x00)) {
        resp = new ByteArray(4);
        resp[0] = 0x6f // resp
        resp[1] = data[1] // ID
        resp[2] = data[2]
        util.msleep(20);
        isotp.startTransfer(resp, false);
    } else {
        // send some  Negative response
        resp = new ByteArray(3);
        resp[0] = 0x7f // resp
        resp[1] = 0x2e // Write by
        resp[2] = 0x31 // out of reange

        util.msleep(20);
        isotp.startTransfer(resp, false);

        util.print("  neg. resp. sent.");
    }
}

function writeDataByIdentifierHandler(data)
{
    // just return something
    util.print("WriteDataByIdentifier received. ID =  " + util.formatString("0x%02x%02x", data[1], data[2]))

    if ((data[2] !== 0x00)) {
        resp = new ByteArray(3);
        resp[0] = 0x6e // resp
        resp[1] = data[1] // ID
        resp[2] = data[2]
        util.msleep(20);
        isotp.startTransfer(resp, false);
    } else {
        // send some  Negative response
        resp = new ByteArray(3);
        resp[0] = 0x7f // resp
        resp[1] = 0x2e // Write by
        resp[2] = 0x31 // out of reange

        util.msleep(20);
        isotp.startTransfer(resp, false);

        util.print("  neg. resp. sent.");
    }
}

function writeMemoryByAddressHandler(data)
{
    // just return something
    util.print("WriteMemoryByAddressHandler");

    rsp = new ByteArray(4);
    rsp[0] = 0x7d // resp
    rsp[1] = 0x65;
    rsp[2] = 0x65;
    rsp[3] = 0x65;

    util.msleep(10);
    isotp.startTransfer(rsp, false);
}

function readMemoryByAddressHandler(data)
{
    // just return something
    util.print("ReadMemoryByAddressHandler");

    rsp = new ByteArray(120);
    rsp[0] = 0x63 // resp
    rsp[1] = 0x65;
    rsp[2] = 0x65;
    rsp[3] = 0x65;
    rsp[4] = 0x6d;
    rsp[5] = 0x6f;
    rsp[6] = 0x74;
    rsp[7] = 0x61;
    rsp[8] = 0x73;
    rsp[9] = 0x20;
    rsp[10] = 0x42;
    rsp[11] = 0x6f;
    rsp[12] = 0x6f;
    rsp[13] = 0x74;
    rsp[14] = 0x6c;
    rsp[99] = 0x6c;
    rsp[110] = 0x6c;
    rsp[120] = 0x6c;

    util.msleep(10);
    isotp.startTransfer(rsp, false);
}


function readDataByIdentifierHandler(data)
{
    // just return something
    util.print("ReadDataByIdentifier received. ID =  " + util.formatString("0x%02x%02x", data[1], data[2]))

    size = 4;
    if (data[1] < 0xf0) {
        size = 113;
    }
    resp = new ByteArray(size);
    resp[0] = 0x62 // resp
    resp[1] = data[1] // ID
    resp[2] = data[2]
    resp[3] = 0x4
    // return current session
    if ((data[1] === 0xf1) && (data[2] === 0x86)) {
        resp[3] = activeSession;
    }

    if (data[1] < 0xf0) {
        resp[4] = 0x2
        resp[5] = 0x3
        resp[6] = 0x4
        resp[7] = cnt;
        resp[8] = 0xfe
        resp[9] = 0xA0
        cnt++;
    }
    // set blocksize to different value if id ended in 0c
    if (data[2] === 0x0c) {
        //blocksize1 = 0x08;
    }

    // handle special ID 0xf180
    if ((data[1] === 0xf1) && (data[2] === 0x80)) {
        rsp = new ByteArray(125);
        rsp[0] = 0x62 // resp
        rsp[1] = data[1] // ID
        rsp[2] = data[2]
        rsp[3] = 0x65;
        rsp[4] = 0x6d;
        rsp[5] = 0x6f;
        rsp[6] = 0x74;
        rsp[7] = 0x61;
        rsp[8] = 0x73;
        rsp[9] = 0x20;
        rsp[10] = 0x42;
        rsp[11] = 0x6f;
        rsp[12] = 0x6f;
        rsp[13] = 0x74;
        rsp[14] = 0x6c;
        rsp[99] = 0x6c;
        rsp[110] = 0x6c;
        rsp[120] = 0x6c;

        util.msleep(10);
        isotp.startTransfer(rsp, false);
    } else if ((data[1] === 0xf1) && (data[2] === 0x81)) {
        rsp = new ByteArray(25);
        rsp[0] = 0x62 // resp
        rsp[1] = data[1] // ID
        rsp[2] = data[2]
        rsp[3] = 0x65;
        rsp[4] = 0x6d;
        rsp[5] = 0x6f;
        rsp[6] = 0x74;
        rsp[7] = 0x61;
        rsp[8] = 0x73;
        rsp[9] = 0x20;
        rsp[10] = 0x42;
        rsp[11] = 0x6f;
        rsp[12] = 0x6f;
        rsp[13] = 0x74;
        rsp[14] = 0x6c;
        util.msleep(10);
        isotp.startTransfer(rsp, false);
    } else {
        isotp.startTransfer(resp, false);
        util.msleep(10);
    }
}

function diagnosticSessionControl(data)
{
    util.print("diagnosticSessionControl received. SessionType =  " + util.formatString("%x", data[1]))

    if ((data[1] & 0x80) === 0x80) {
        util.print("  Positive response suppressed.");
        activeSession = data[1];
    } else {
        // send some response for settion types 0..4
        resp = new ByteArray(2);
        resp[0] = 0x50;
        resp[1] = data[1];
        isotp.startTransfer(resp, false);
    }
}

function ecuResetHandler(data) {
    util.print("ECUReset received. ResetType =  " + util.formatString("%x", data[1]))
    if ((data[1] & 0x80) === 0x80) {
        util.print("  Positive response suppressed.");
    } else {
        // send some response
        var d0 = 0x2; // single frame
        var d1 = 0x51; // Response ECU Reset
        util.msleep(10);

        // send some response
        resp = new ByteArray(2);
        resp[0] = 0x51;
        resp[1] = data[1] & 0x7f;
        isotp.startTransfer(resp, false);
        util.print("  Response sent.");
    }
}


// start scripting here
util.clear()
util.print("Start 2nd UDS simulation: " + deviceCanID + " " + toolCanID + "(no functional adddressing)")
util.setTitle("UDS server simulator: with extended ISO-TP addressing");

isotp.setCanIDs(deviceCanID, toolCanID)
isotp.setExtendedAddressing(true, deviceCanAddrByte, true, toolCanAddrByte);

util.print("Wait for next UDS command:")
do {
    ret = isotp.waitForTransfer(43411);
    if (ret === "OK Data") {
        data = isotp.getResponseData();
        udsServiceHandler(data)

        if (data[0] !== 0x3e) {
            util.print("Wait for next UDS command:")
        }

    }
} while (ret === "OK Data")
util.print(ret);
util.print("\n");
util.print("End of script due to time out.");
