// Creepy UDS simulation


// SVN $Date: 2023-10-05 09:58:39 +0200 (Do, 05. Okt 2023) $
// SVN $Rev: 49933 $
// SVN $Author: ged $

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

// configuration settings
const EXTENDED_ID =  0x20000000 // use 29 bit IDs
//const toolCanID     =  0x18da97f9 | EXTENDED_ID;
//const deviceCanID   =  0x18daf997 | EXTENDED_ID;
const deviceCanID =  0x7e8
const toolCanID   =  0x7e0

const deviceCanAddrByte = 30
const toolCanAddrByte = 10



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

var tmpCnt = 0;

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


    // restart timeout timer with every message
    util.deleteTimer(timerId);
    timerId = util.after(10000, "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 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]))
           readMemoryByAddress(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 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 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");
    rsp = new ByteArray(2);
    rsp[0] = 0x68 // resp
    rsp[1] = data[1];

    util.msleep(10);
    isotp.startTransfer(rsp, 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]);

    var size = 0;
    if (data[1] === 0x01) {
        // request seed
        size = 6;

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

            // key okay
            securityOpen = true;

        } else {
            securityOpen = false;

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

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

        }


        size = 2;
    }

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

    resp = new ByteArray(size);
    resp[0] = 0x67 // resp
    resp[1] = data[1];
    if (size > 2) {
        resp[2] = 0xff-cnt;
        resp[3] = 0x22;
        resp[4] = cnt;
        resp[5] = (cnt * 7) & 0xfe;

        if (securityOpen === true) {
            resp[2] = 0;
            resp[3] = 0;
            resp[4] = 0;
            resp[5] = 0;
        }

        aKey[0] = resp[2]
        aKey[1] = resp[3]
        aKey[2] = resp[4]
        aKey[3] = resp[5]
    }
    cnt++;

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

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
        // send Negative Response
        var d0 = 0x4; // single frame
        var d1 = 0x7f; // Negative Response
        var d2 = 0x2e; // diagnosticSessionControl()
        var d3 = 0x31; // out of range
        if (deviceCanID > 0x1fffFFFF) {
            can.sendExtendedFrame(deviceCanID & 0x1fffFFFF, 8, d0, d1, d2, d3, 0x55, 0x55, 0x55, 0x55)
        } else {
            can.sendBaseFrame(deviceCanID & 0x1fffFFFF, 8, d0, d1, d2, d3, 0x55, 0x55, 0x55, 0x55)
        }
        util.print("  neg. resp. sent.");
    }
}

function readMemoryByAddress(data)
{
    // just return something
    util.print("ReadMemorybyAddress recieved.");

    rsp = new ByteArray(101);
    rsp[0] = 0x63 // resp
    rsp[1] = 0x1;
    rsp[2] = 0x2;
    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[98] = 0x98;
    rsp[99] = 0x99;
    rsp[100] = 0xff;
    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;
    }


    // id the ID is 0x1234, we send a RCRRP message
    if ((data[1] === 0x12) && (data[2] === 0x34)) {
        temp = new ByteArray(3);
        temp[0] = 0x7f // NR
        temp[1] = 0x22 // Service
        temp[2] = 0x78 // RCRRP
        isotp.startTransfer(temp, false);
        util.msleep(400);
        isotp.startTransfer(temp, false);
        util.msleep(400);
        isotp.startTransfer(temp, false);
        util.msleep(400);
        isotp.startTransfer(temp, false);
        util.msleep(400);
        isotp.startTransfer(temp, false);
        util.msleep(400);
        isotp.startTransfer(temp, false);
    }


    // handle special ID 0xf180
    if ((data[1] === 0xf1) && (data[2] === 0x80)) {

        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;
        rsp[15] = 0x42;
        rsp[16] = 0x6f;
        rsp[17] = 0x6f;
        rsp[18] = 0x74;
        rsp[19] = 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
        if (data[1] < 4) {
            activeSession = data[1];
            if (activeSession === 1) {
                d0 = 0x4; // single frame
                d1 = 0x50; // Response diagnosticSessionControl()
                util.msleep(20);
                if (deviceCanID > 0x1fffFFff) {
                    can.sendExtendedFrame(deviceCanID & 0x1fffFFff, 8, d0, d1, data[1], data[2], 0x55, 0x55, 0x55, 0x55)
                } else {
                    can.sendBaseFrame(deviceCanID & 0x1fffFFff, 8, d0, d1, data[1], data[1], 0x55, 0x55, 0x55, 0x55)
                }
            } else {
                resp = new ByteArray(6);
                resp[0] = 0x50 // resp
                resp[1] = data[1] // ID
                resp[2] = 0
                resp[3] = 0x64 * 2
                resp[4] = 0x1;
                resp[5] = 0xf4;
                util.msleep(10);
                isotp.startTransfer(resp, false);
                util.print("  Response sent.");
            }
        } else {
            // send Negative Response
            d0 = 0x4; // single frame
            d1 = 0x7f; // Negative Response
            d2 = 0x10; // diagnosticSessionControl()
            d3 = 0x12; // sub function not supported
            util.msleep(20);
            if (deviceCanID > 0x1fffFFFF) {
                can.sendExtendedFrame(deviceCanID & 0x1fffFFff, 8, d0, d1, d2, d3, 0x55, 0x55, 0x55, 0x55)
            } else {
                can.sendBaseFrame(deviceCanID & 0x1fffFFff, 8, d0, d1, d2, d3, 0x55, 0x55, 0x55, 0x55)
            }
            util.print("  neg. resp. sent.");
        }
    }
}

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.");
    }

    //blocksize1 = 0x04;
}


// start scripting here
util.clear()
util.print("Start basic UDS simulation: " + deviceCanID + " " + toolCanID + "(no functional adddressing)")
util.setTitle("UDS server simulator");
//settings.setBackgroundColor(240,230,230,100);

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

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.");
