layoutx/test_plc.c

/*
 * test_plc.c - local mock S7 server + client round-trip self test.
 *
 * Verifies byte-exact S7comm frames (COTP CR, setup, read request/response)
 * against the python-snap7 reference implementation layout.
 */

#include <arpa/inet.h>
#include <errno.h>
#include <pthread.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/socket.h>
#include <unistd.h>

#include "plc.h"

#define TEST_PORT 12022

#define COTP_CR 0xE0
#define COTP_CC 0xD0
#define COTP_DT 0xF0

static int g_server_fail = 0;
static int g_server_done = 0;

static int recv_exact(int fd, void *buf, size_t len) {
    size_t got = 0;
    while (got < len) {
        ssize_t r = recv(fd, (uint8_t *)buf + got, len - got, 0);
        if (r <= 0) return -1;
        got += (size_t)r;
    }
    return 0;
}

static int send_all(int fd, const void *buf, size_t len) {
    size_t sent = 0;
    while (sent < len) {
        ssize_t r = send(fd, (const uint8_t *)buf + sent, len - sent, MSG_NOSIGNAL);
        if (r <= 0) return -1;
        sent += (size_t)r;
    }
    return 0;
}

static void put_u16(uint8_t *p, uint16_t v) {
    p[0] = (uint8_t)(v >> 8);
    p[1] = (uint8_t)(v & 0xFF);
}

static uint16_t get_u16(const uint8_t *p) {
    return (uint16_t)(((uint16_t)p[0] << 8) | p[1]);
}

/* Read one TPKT frame; returns COTP/S7 payload after the 3-byte DT header. */
static int read_frame(int fd, uint8_t *out, int maxout, int *out_len) {
    uint8_t hdr[4];
    if (recv_exact(fd, hdr, 4) < 0) return -1;
    int total = ((int)hdr[2] << 8) | hdr[3];
    if (total < 7 || total > 1200) return -1;
    uint8_t payload[1200];
    if (recv_exact(fd, payload, total - 4) < 0) return -1;
    if (payload[0] != 2 || payload[1] != COTP_DT) return -1;
    int n = total - 4 - 3;
    if (n > maxout) return -1;
    memcpy(out, payload + 3, n);
    *out_len = n;
    return 0;
}

static void send_s7(int fd, const uint8_t *s7, int len) {
    uint8_t frame[4 + 3 + 1200];
    int total = 4 + 3 + len;
    frame[0] = 3; frame[1] = 0;
    frame[2] = (uint8_t)(total >> 8);
    frame[3] = (uint8_t)(total & 0xFF);
    frame[4] = 2; frame[5] = COTP_DT; frame[6] = 0x80;
    memcpy(frame + 7, s7, len);
    send_all(fd, frame, total);
}

static void fail(const char *msg, const uint8_t *got, int glen,
                 const uint8_t *exp, int elen) {
    g_server_fail = 1;
    fprintf(stderr, "MOCK FAIL: %s (got %d bytes, want %d)\n", msg, glen, elen);
    fprintf(stderr, "  got: ");
    for (int i = 0; i < glen; i++) fprintf(stderr, "%02x ", got[i]);
    fprintf(stderr, "\n  exp: ");
    for (int i = 0; i < elen; i++) fprintf(stderr, "%02x ", exp[i]);
    fprintf(stderr, "\n");
}

static void serve_connection(int cfd) {
    /* --- COTP Connection Request --- */
    uint8_t hdr[4], payload[1200];
    if (recv_exact(cfd, hdr, 4) < 0) goto close;
    int total = ((int)hdr[2] << 8) | hdr[3];
    if (recv_exact(cfd, payload, total - 4) < 0) goto close;
    {
        uint8_t exp[18] = {
            0x11, COTP_CR, 0x00, 0x00, 0x00, 0x01, 0x00,
            0xC1, 0x02, 0x01, 0x00,          /* calling TSAP 0x0100 */
            0xC2, 0x02, 0x01, 0x02,          /* called TSAP 0x0102 */
            0xC0, 0x01, 0x0A,
        };
        if (total - 4 != 18 || memcmp(payload, exp, sizeof(exp)) != 0)
            fail("COTP CR", payload, total - 4, exp, sizeof(exp));
    }
    /* --- COTP Connection Confirm --- */
    {
        uint8_t cc[10] = {0x09, COTP_CC, 0x00, 0x01, 0x00, 0x01, 0x00,
                          0xC0, 0x01, 0x0A};
        uint8_t frame[4 + 10];
        int t = 4 + 10;
        frame[0] = 3; frame[1] = 0;
        frame[2] = (uint8_t)(t >> 8); frame[3] = (uint8_t)(t & 0xFF);
        memcpy(frame + 4, cc, 10);
        send_all(cfd, frame, t);
    }

    /* --- S7 Setup Communication --- */
    {
        uint8_t data[1200];
        int n;
        if (read_frame(cfd, data, sizeof(data), &n) < 0) goto close;
        uint8_t exp[18] = {
            0x32, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x08, 0x00, 0x00,
            0xF0, 0x00, 0x00, 0x01, 0x00, 0x01, 0x01, 0xE0,
        };
        exp[4] = data[4];
        exp[5] = data[5];
        if (n != 18 || memcmp(data, exp, sizeof(exp)) != 0)
            fail("S7 setup", data, n, exp, sizeof(exp));
        uint8_t resp[20] = {
            0x32, 0x03, 0x00, 0x00, 0x00, 0x01, 0x00, 0x08, 0x00, 0x00,
            0x00, 0x00,
            0xF0, 0x00, 0x00, 0x01, 0x00, 0x01, 0x01, 0xE0,
        };
        send_s7(cfd, resp, sizeof(resp));
    }

    /* --- Read requests (loop) --- */
    {
        const uint8_t *data_pattern = (const uint8_t *)"\x10\x11\x12\x13";
        int read_len = 4;
        uint8_t data[1200];
        int n;
        while (recv_exact(cfd, hdr, 4) >= 0) {
            total = ((int)hdr[2] << 8) | hdr[3];
            if (recv_exact(cfd, data, total - 4) < 0) break;
            if (data[0] != 2 || data[1] != COTP_DT) break;
            n = total - 4 - 3;
            uint8_t *s7 = data + 3;
            uint8_t exp[24] = {
                0x32, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0E, 0x00, 0x00,
                0x04, 0x01, 0x12, 0x0A, 0x10, 0x02, /* READ VAR + spec */
                0x00, 0x04,                         /* count = 4 bytes */
                0x00, 0x00,                         /* db = 0 */
                0x83,                               /* area = M */
                0x00, 0x00, 0x10,                   /* addr = 2*8 */
            };
            exp[4] = s7[4];
            exp[5] = s7[5];
            if (n != 24 || memcmp(s7, exp, sizeof(exp)) != 0)
                fail("S7 read request", s7, n, exp, sizeof(exp));

            uint16_t seq = get_u16(s7 + 4);
            uint8_t resp[12 + 2 + 4 + read_len];
            int r = 0;
            resp[r++] = 0x32; resp[r++] = 0x03;
            resp[r++] = 0x00; resp[r++] = 0x00;
            put_u16(resp + r, seq); r += 2;
            put_u16(resp + r, 0x0002); r += 2;      /* param len */
            put_u16(resp + r, 0x0004 + read_len); r += 2; /* data len */
            resp[r++] = 0x00; resp[r++] = 0x00;     /* error class/code */
            resp[r++] = 0x04; resp[r++] = 0x01;     /* read response */
            resp[r++] = 0xFF;                        /* return code OK */
            resp[r++] = 0x04;                        /* transport size */
            put_u16(resp + r, (uint16_t)(read_len * 8)); r += 2;
            memcpy(resp + r, data_pattern, read_len); r += read_len;
            send_s7(cfd, resp, r);
        }
    }

close:
    close(cfd);
    g_server_done = 1;
}

static void *mock_server(void *arg) {
    int persistent = (arg != NULL);
    int lfd = socket(AF_INET, SOCK_STREAM, 0);
    if (lfd < 0) { g_server_fail = 1; g_server_done = 1; return NULL; }
    int one = 1;
    setsockopt(lfd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
    struct sockaddr_in sa;
    memset(&sa, 0, sizeof(sa));
    sa.sin_family = AF_INET;
    sa.sin_addr.s_addr = htonl(INADDR_LOOPBACK);
    sa.sin_port = htons(TEST_PORT);
    if (bind(lfd, (struct sockaddr *)&sa, sizeof(sa)) < 0 ||
        listen(lfd, 4) < 0) {
        perror("bind/listen");
        g_server_fail = 1;
        g_server_done = 1;
        return NULL;
    }

    if (persistent) {
        for (;;) {
            int cfd = accept(lfd, NULL, NULL);
            if (cfd < 0) {
                if (errno == EINTR) continue;
                break;
            }
            serve_connection(cfd);
        }
    } else {
        int cfd = accept(lfd, NULL, NULL);
        if (cfd >= 0) serve_connection(cfd);
    }
    close(lfd);
    return NULL;
}

int main(int argc, char **argv) {
    /* Standalone mock PLC server mode: ./test_plc server
     * Handy for on-device testing against the real rtsp_multi_view. */
    if (argc > 1 && strcmp(argv[1], "server") == 0) {
        mock_server((void *)1);
        return 0;
    }

    pthread_t server;
    pthread_create(&server, NULL, mock_server, NULL);

    PlcConfig cfg;
    memset(&cfg, 0, sizeof(cfg));
    strcpy(cfg.ip, "127.0.0.1");
    cfg.port = TEST_PORT;
    cfg.area = PLC_AREA_M;
    cfg.read_start = 2;
    cfg.read_len = 4;
    cfg.poll_ms = 50;
    cfg.reconnect_ms = 200;
    cfg.tsap_local = 0x0100;
    cfg.tsap_remote = 0x0102;

    int failures = 0;
    if (plc_init(&cfg) < 0) {
        fprintf(stderr, "plc_init failed\n");
        return 1;
    }

    /* Wait for connection */
    int ok = 0;
    for (int i = 0; i < 100; i++) {
        if (plc_is_connected()) { ok = 1; break; }
        usleep(30000);
    }
    if (!ok) {
        fprintf(stderr, "FAIL: never connected (%s)\n", plc_last_error());
        failures++;
    } else {
        uint8_t b;
        if (plc_get_byte(0, &b) == 0 && b == 0x10) {
            fprintf(stderr, "PASS: byte0 = 0x%02x\n", b);
        } else {
            fprintf(stderr, "FAIL: byte0 = %02x\n", b);
            failures++;
        }
        if (plc_get_byte(3, &b) == 0 && b == 0x13) {
            fprintf(stderr, "PASS: byte3 = 0x%02x\n", b);
        } else {
            fprintf(stderr, "FAIL: byte3 = %02x\n", b);
            failures++;
        }
        /* 0x10 = bit4 set, bit0 clear */
        if (plc_get_bit(0, 4) == 1 && plc_get_bit(0, 0) == 0) {
            fprintf(stderr, "PASS: bit4=1 bit0=0\n");
        } else {
            fprintf(stderr, "FAIL: bit checks %d %d\n",
                    plc_get_bit(0, 4), plc_get_bit(0, 0));
            failures++;
        }
        /* out-of-range access must fail gracefully */
        if (plc_get_bit(7, 0) == -1 && plc_get_byte(9, &b) == -1) {
            fprintf(stderr, "PASS: out-of-range rejected\n");
        } else {
            fprintf(stderr, "FAIL: out-of-range not rejected\n");
            failures++;
        }
    }

    plc_shutdown();
    pthread_join(server, NULL);

    if (g_server_fail) {
        fprintf(stderr, "FAIL: mock server reported frame mismatches\n");
        failures++;
    } else {
        fprintf(stderr, "PASS: all S7 frames byte-exact\n");
    }

    if (failures == 0) {
        fprintf(stderr, "ALL TESTS PASSED\n");
        return 0;
    }
    fprintf(stderr, "%d FAILURE(S)\n", failures);
    return 1;
}