layoutx/plc_sim.c

/* PLC simulator: connects to the PLC as an S7comm client and every
 * <interval_ms> writes the next state pattern into the app's read area,
 * cycling through 6 states:
 *   0: all bits 0                          -> default layout
 *   1: byte0 = 0b00000011 (M0.0|M0.1)      -> layout 0
 *   2: byte0 = 0b00001100 (M0.2|M0.3)      -> layout 1
 *   3: byte0 = 0b00110000 (M0.4|M0.5)      -> layout 2
 *   4: byte0 = 0b00001111 (two groups)     -> layout 3
 *   5: byte0 = 0b00111111 (three groups)   -> layout 4
 *
 * Usage: plc_sim [-c channels.conf] [-t interval_ms] [-f first_state]
 */

#include <arpa/inet.h>
#include <errno.h>
#include <fcntl.h>
#include <poll.h>
#include <signal.h>
#include <stdarg.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <unistd.h>

#define S7_PDU_REQ    0x01
#define S7_PDU_ACKD   0x03
#define COTP_CR       0xE0
#define COTP_CC       0xD0
#define COTP_DT       0xF0
#define TS_BYTE       0x02

#define SOCK_TIMEOUT_MS 3000
#define MAX_CFG_STR    256
#define MAX_BLOCK      64

static volatile int g_running = 1;
static uint16_t g_seq = 0;
static char g_err[256];

typedef struct {
    char ip[MAX_CFG_STR];
    int  port;
    int  area;              /* 0x81 I, 0x82 Q, 0x83 M, 0x84 DB */
    int  db;
    int  start;
    int  len;
    int  tsap_local;
    int  tsap_remote;
} SimCfg;

static const char *area_names[] = { "?", "I", "Q", "M", "DB" };

#define MAX_RULES 8
#define MAX_RULE_BITS 16

/* One bit position (byte.bit relative to read_start) taken from [plc_rules]. */
typedef struct {
    int byte;
    int bit;
} RuleBit;

typedef struct {
    int layout;            /* rule id -> layout index */
    RuleBit bits[MAX_RULE_BITS];
    int nbits;
    int has_bits;          /* non-zero if the rule carries explicit bits */
} PlcRuleCfg;

static PlcRuleCfg g_rules[MAX_RULES];
static int g_num_rules = 0;

/* Build the write block (cfg.len bytes) for a given rule state.
 * state 0 -> all bits cleared (default layout).
 * n>=1   -> set the bits listed in g_rules[state-1] (fall back to byte0). */
static void state_to_block(uint8_t *data, int len, int state, int nrules) {
    memset(data, 0, (size_t)len * sizeof(data[0]));
    if (state <= 0) return;
    int idx = state - 1;
    if (idx >= nrules || idx < 0) return;
    PlcRuleCfg *R = &g_rules[idx];
    if (!R->has_bits) {
        /* no [plc_rules] for this state: fall back to byte0 low bits */
        if (len > 0) data[0] = (uint8_t)(0x03u << (2 * (idx % 3)));
        return;
    }
    for (int i = 0; i < R->nbits; i++) {
        int by = R->bits[i].byte;
        int bi = R->bits[i].bit;
        if (by < 0 || by >= len || bi < 0 || bi > 7) continue;
        data[by] |= (uint8_t)(1u << bi);
    }
}

/* Render the set bits of a rule state as `prefix byte.bit` / `db.byte.bit`,
 * matching the config grammar, for display only. */
static int format_state_addr(char *buf, size_t buflen,
                             const SimCfg *cfg, const uint8_t *data, int len) {
    const char *prefix = "?";
    switch (cfg->area) {
        case 0x81: prefix = "I";  break;
        case 0x82: prefix = "Q";  break;
        case 0x83: prefix = "M";  break;
        case 0x84: prefix = "DB"; break;
    }
    int first = 1;
    size_t o = 0;
    for (int i = 0; i < len * 8 && o < buflen; i++) {
        int byte = i / 8;
        int bit = i % 8;
        if (!(data[byte] & (1u << bit))) continue;
        int pos = cfg->start + byte;
        int n;
        if (cfg->area == 0x84)
            n = snprintf(buf + o, buflen - o, "%s%d.%d.%d",
                         first ? "" : ",", cfg->db, pos, bit);
        else
            n = snprintf(buf + o, buflen - o, "%s%s%d.%d",
                         first ? "" : ",", prefix, pos, bit);
        if (n < 0 || (size_t)n >= buflen - o) break;
        o += (size_t)n;
        first = 0;
    }
    if (o == 0 && buflen > 0) { buf[0] = '\0'; }
    return (int)o;
}

static void sig_handler(int sig) {
    (void)sig;
    g_running = 0;
}

static void set_err(const char *fmt, ...) {
    va_list ap;
    va_start(ap, fmt);
    vsnprintf(g_err, sizeof(g_err), fmt, ap);
    va_end(ap);
}

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) {
            if (r < 0 && (errno == EAGAIN || errno == EWOULDBLOCK))
                set_err("recv timeout");
            else
                set_err("recv failed: %s", strerror(errno));
            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) {
            if (r < 0 && (errno == EAGAIN || errno == EWOULDBLOCK))
                set_err("send timeout");
            else
                set_err("send failed: %s", strerror(errno));
            return -1;
        }
        sent += (size_t)r;
    }
    return 0;
}

static int send_s7(int fd, const uint8_t *s7, int s7len) {
    uint8_t frame[4 + 3 + 1024];
    if (3 + s7len > (int)sizeof(frame) - 4) {
        set_err("s7 frame too large");
        return -1;
    }
    int total = 4 + 3 + s7len;
    frame[0] = 3; frame[1] = 0;
    frame[2] = (uint8_t)(total >> 8);
    frame[3] = (uint8_t)(total & 0xFF);
    frame[4] = 2;          /* COTP DT header length */
    frame[5] = COTP_DT;
    frame[6] = 0x80;       /* EOT + sequence 0 */
    memcpy(frame + 7, s7, s7len);
    return send_all(fd, frame, total);
}

static int recv_s7(int fd, uint8_t *out, int maxout, int *out_len) {
    uint8_t hdr[4];
    if (recv_exact(fd, hdr, 4) < 0) return -1;
    if (hdr[0] != 3) { set_err("bad TPKT version %d", hdr[0]); return -1; }
    int total = ((int)hdr[2] << 8) | hdr[3];
    if (total < 7 || total > 4 + 1024) {
        set_err("bad TPKT length %d", total);
        return -1;
    }
    uint8_t payload[1024];
    if (recv_exact(fd, payload, total - 4) < 0) return -1;
    if (payload[0] != 2 || payload[1] != COTP_DT) {
        set_err("expected COTP DT, got %02x %02x", payload[0], payload[1]);
        return -1;
    }
    int s7len = total - 4 - 3;
    if (s7len < 0 || s7len > maxout) {
        set_err("bad s7 length %d", s7len);
        return -1;
    }
    memcpy(out, payload + 3, s7len);
    *out_len = s7len;
    return 0;
}

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

/* ---- COTP Connection Request ---- */
static int cotp_connect(int fd, const SimCfg *cfg) {
    uint8_t pdu[22];
    int n = 0;
    pdu[n++] = 0x11;
    pdu[n++] = COTP_CR;
    pdu[n++] = 0x00; pdu[n++] = 0x00;
    pdu[n++] = 0x00; pdu[n++] = 0x01;
    pdu[n++] = 0x00;
    pdu[n++] = 0xC1; pdu[n++] = 0x02;
    pdu[n++] = (uint8_t)((cfg->tsap_local >> 8) & 0xFF);
    pdu[n++] = (uint8_t)(cfg->tsap_local & 0xFF);
    pdu[n++] = 0xC2; pdu[n++] = 0x02;
    pdu[n++] = (uint8_t)((cfg->tsap_remote >> 8) & 0xFF);
    pdu[n++] = (uint8_t)(cfg->tsap_remote & 0xFF);
    pdu[n++] = 0xC0; pdu[n++] = 0x01; pdu[n++] = 0x0A;

    uint8_t frame[4 + n];
    int total = 4 + n;
    frame[0] = 3; frame[1] = 0;
    frame[2] = (uint8_t)(total >> 8);
    frame[3] = (uint8_t)(total & 0xFF);
    memcpy(frame + 4, pdu, n);
    if (send_all(fd, frame, total) < 0) return -1;

    uint8_t resp[64];
    if (recv_exact(fd, resp, 4) < 0) return -1;
    if (resp[0] != 3) { set_err("CC bad TPKT version"); return -1; }
    int rtot = ((int)resp[2] << 8) | resp[3];
    if (rtot < 7) { set_err("CC too short"); return -1; }
    if (recv_exact(fd, resp + 4, rtot - 4) < 0) return -1;
    if (resp[5] != COTP_CC) { set_err("expected COTP CC"); return -1; }
    return 0;
}

/* ---- S7 Setup Communication ---- */
static int s7_setup(int fd) {
    uint8_t req[18];
    req[0] = 0x32; req[1] = S7_PDU_REQ;
    req[2] = 0x00; req[3] = 0x00;
    put_u16(req + 4, ++g_seq);
    put_u16(req + 6, 0x0008);
    put_u16(req + 8, 0x0000);
    req[10] = 0xF0; req[11] = 0x00;
    put_u16(req + 12, 0x0001);
    put_u16(req + 14, 0x0001);
    put_u16(req + 16, 0x01E0);
    if (send_s7(fd, req, sizeof(req)) < 0) return -1;

    uint8_t resp[1024];
    int rlen;
    if (recv_s7(fd, resp, sizeof(resp), &rlen) < 0) return -1;
    return 0;
}

/* ---- S7 Write Var ---- */
static int s7_write(int fd, const SimCfg *cfg, const uint8_t *data, int len) {
    uint8_t req[10 + 14 + 4 + MAX_BLOCK];
    int o = 0;
    req[o++] = 0x32; req[o++] = S7_PDU_REQ;
    req[o++] = 0x00; req[o++] = 0x00;
    put_u16(req + o, ++g_seq); o += 2;      /* PDU ref */
    put_u16(req + o, 0x000E); o += 2;       /* param len = 14 */
    put_u16(req + o, 0x0004 + len); o += 2; /* data len */

    req[o++] = 0x05; req[o++] = 0x01;       /* WRITE VAR, 1 item */
    req[o++] = 0x12; req[o++] = 0x0A;       /* spec, addr spec len */
    req[o++] = 0x10;                        /* syntax S7-ANY */
    req[o++] = TS_BYTE;                     /* transport size BYTE */
    put_u16(req + o, (uint16_t)len); o += 2;
    put_u16(req + o, (uint16_t)cfg->db); o += 2;
    req[o++] = (uint8_t)cfg->area;
    uint32_t addr = (uint32_t)cfg->start * 8u;
    req[o++] = (uint8_t)(addr >> 16);
    req[o++] = (uint8_t)(addr >> 8);
    req[o++] = (uint8_t)(addr & 0xFF);

    req[o++] = 0x00;                        /* return code (ignored) */
    req[o++] = TS_BYTE;
    put_u16(req + o, (uint16_t)(len * 8)); o += 2;
    memcpy(req + o, data, len); o += len;

    if (send_s7(fd, req, o) < 0) return -1;

    uint8_t resp[1024];
    int rlen;
    if (recv_s7(fd, resp, sizeof(resp), &rlen) < 0) return -1;
    if (rlen < 14 || resp[0] != 0x32 || resp[1] != S7_PDU_ACKD) {
        set_err("bad write response header");
        return -1;
    }
    if (resp[10] != 0 || resp[11] != 0) {
        set_err("s7 error class %02x code %02x", resp[10], resp[11]);
        return -1;
    }
    /* param: 0x05 0x01 0xFF  (0xFF = item success) */
    if (rlen > 14 && resp[14] != 0xFF) {
        set_err("write item rc=0x%02x", resp[14]);
        return -1;
    }
    return 0;
}

static int tcp_connect_handshake(const SimCfg *cfg) {
    int fd = socket(AF_INET, SOCK_STREAM, 0);
    if (fd < 0) { set_err("socket: %s", strerror(errno)); return -1; }

    struct sockaddr_in sa;
    memset(&sa, 0, sizeof(sa));
    sa.sin_family = AF_INET;
    sa.sin_port = htons((uint16_t)cfg->port);
    if (inet_pton(AF_INET, cfg->ip, &sa.sin_addr) != 1) {
        set_err("bad ip %s", cfg->ip);
        close(fd);
        return -1;
    }

    int flags = fcntl(fd, F_GETFL, 0);
    fcntl(fd, F_SETFL, flags | O_NONBLOCK);
    int rc = connect(fd, (struct sockaddr *)&sa, sizeof(sa));
    if (rc < 0 && errno != EINPROGRESS) {
        set_err("tcp connect: %s", strerror(errno));
        close(fd);
        return -1;
    }
    if (rc < 0) {
        struct pollfd pfd = { .fd = fd, .events = POLLOUT };
        int pr = poll(&pfd, 1, SOCK_TIMEOUT_MS);
        if (pr <= 0) {
            set_err(pr == 0 ? "tcp connect timeout" : "tcp connect: %s",
                    strerror(errno));
            close(fd);
            return -1;
        }
        int soerr = 0;
        socklen_t sl = sizeof(soerr);
        if (getsockopt(fd, SOL_SOCKET, SO_ERROR, &soerr, &sl) < 0 || soerr != 0) {
            set_err("tcp connect refused: %s", strerror(soerr));
            close(fd);
            return -1;
        }
    }
    fcntl(fd, F_SETFL, flags);

    struct timeval tv;
    tv.tv_sec = SOCK_TIMEOUT_MS / 1000;
    tv.tv_usec = (SOCK_TIMEOUT_MS % 1000) * 1000;
    setsockopt(fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv));
    setsockopt(fd, SOL_SOCKET, SO_SNDTIMEO, &tv, sizeof(tv));
    int one = 1;
    setsockopt(fd, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one));

    if (cotp_connect(fd, cfg) < 0) { close(fd); return -1; }
    if (s7_setup(fd) < 0) {
        /* Setup is non-fatal. */
    }
    return fd;
}

/* Read ip/port/area/db/read_start/read_len/tsap from the [plc] section and the
 * layout-switch bit rules from the [plc_rules] section of a channels.conf-style
 * file. Rules use `byte.bit` (relative to read_start), e.g. `6.6,6.7`.
 * Unknown keys are ignored. */
static int parse_config(const char *path, SimCfg *cfg) {
    FILE *fp = fopen(path, "r");
    if (!fp) { set_err("cannot open %s", path); return -1; }
    int section = 0;
    int rules_seen = 0;
    char line[512];
    while (fgets(line, sizeof(line), fp)) {
        char *s = line;
        while (*s == ' ' || *s == '\t') s++;
        if (*s == '\n' || *s == '\r' || *s == '#' || *s == '\0') continue;
        if (s[0] == '[') {
            if (strncmp(s, "[plc]", 5) == 0) section = 5;
            else if (strncmp(s, "[plc_rules]", 11) == 0) section = 7;
            else section = 0;
            continue;
        }
        if (section == 5) {
            char key[64], val[256];
            if (sscanf(s, "%63s = %255[^\n\r]", key, val) < 2) continue;
            char *c = strchr(val, ';');
            if (!c) c = strchr(val, '#');
            if (c) *c = '\0';
            int vlen = (int)strlen(val);
            while (vlen > 0 && (val[vlen-1] == ' ' || val[vlen-1] == '\t'))
                val[--vlen] = '\0';
            if (strcmp(key, "ip") == 0) {
                strncpy(cfg->ip, val, sizeof(cfg->ip) - 1);
            } else if (strcmp(key, "port") == 0) {
                cfg->port = atoi(val);
            } else if (strcmp(key, "area") == 0) {
                if (strcasecmp(val, "i") == 0) cfg->area = 0x81;
                else if (strcasecmp(val, "q") == 0) cfg->area = 0x82;
                else if (strcasecmp(val, "m") == 0) cfg->area = 0x83;
                else if (strcasecmp(val, "db") == 0) cfg->area = 0x84;
            } else if (strcmp(key, "db") == 0) {
                cfg->db = atoi(val);
            } else if (strcmp(key, "read_start") == 0) {
                cfg->start = atoi(val);
            } else if (strcmp(key, "read_len") == 0) {
                cfg->len = atoi(val);
            } else if (strcmp(key, "tsap_local") == 0) {
                cfg->tsap_local = (int)strtol(val, NULL, 0);
            } else if (strcmp(key, "tsap_remote") == 0) {
                cfg->tsap_remote = (int)strtol(val, NULL, 0);
            }
        } else if (section == 7) {
            /* e.g. `0 : 6.6,6.7  ; 8.4,8.5`  (byte.bit, ';' = another group) */
            int layout;
            char *colon = strchr(s, ':');
            if (!colon) continue;
            if (sscanf(s, "%d", &layout) != 1) continue;
            if (layout < 0 || layout >= MAX_RULES) continue;
            PlcRuleCfg *R = &g_rules[layout];
            memset(R, 0, sizeof(*R));
            R->layout = layout;
            char *tok = colon + 1;
            while (tok && *tok && R->nbits < MAX_RULE_BITS) {
                while (*tok == ' ' || *tok == '\t' || *tok == ',' || *tok == ';')
                    tok++;
                if (*tok == ':' || *tok == '\0' ||
                    *tok == '\n' || *tok == '\r' || *tok == '#') break;
                int by, bi = 0;
                if (sscanf(tok, "%d.%d", &by, &bi) == 2) {
                    /* ok */
                } else if (sscanf(tok, "%d", &by) == 1) {
                    bi = 0;
                } else {
                    break;
                }
                R->bits[R->nbits].byte = by;
                R->bits[R->nbits].bit = bi;
                R->nbits++;
                R->has_bits = 1;
                while (*tok && *tok != ',' && *tok != ';' && *tok != ':')
                    tok++;
            }
            if (R->nbits > 0 && layout + 1 > g_num_rules)
                g_num_rules = layout + 1;
            rules_seen = 1;
        }
    }
    fclose(fp);
    if (cfg->len <= 0 || cfg->len > MAX_BLOCK) cfg->len = 1;
    if (!rules_seen) {
        /* no [plc_rules]: fall back to a fixed 6-state cycle on byte0 */
        g_num_rules = 5;
    }
    return 0;
}

static void usage(const char *prog) {
    fprintf(stderr,
        "Usage: %s [-c channels.conf] [-t interval_ms] [-f first_state]\n"
        "  -c   config file with [plc] and [plc_rules] sections (default: channels.conf)\n"
        "  -t   interval between state switches in ms (default: 5000)\n"
        "  -f   first state index 0..N (default: 0)\n",
        prog);
}

int main(int argc, char **argv) {
    SimCfg cfg;
    memset(&cfg, 0, sizeof(cfg));
    strncpy(cfg.ip, "192.168.1.90", sizeof(cfg.ip) - 1);
    cfg.port = 102;
    cfg.area = 0x81;          /* I area, matches channels.conf */
    cfg.db = 1;
    cfg.start = 0;
    cfg.len = 1;
    cfg.tsap_local = 0x0100;
    cfg.tsap_remote = 0x0102;

    const char *config_path = "channels.conf";
    int interval_ms = 5000;
    int state = 0;

    int opt;
    while ((opt = getopt(argc, argv, "c:t:f:h")) != -1) {
        switch (opt) {
        case 'c': config_path = optarg; break;
        case 't': interval_ms = atoi(optarg); break;
        case 'f': state = atoi(optarg); break;
        case 'h': default: usage(argv[0]); return 1;
        }
    }
    if (interval_ms <= 0) interval_ms = 5000;
    if (state < 0) state = 0;

    if (parse_config(config_path, &cfg) < 0)
        fprintf(stderr, "WARN: %s\n", g_err);
    if (state >= g_num_rules + 1) state = 0;

    signal(SIGINT, sig_handler);
    signal(SIGTERM, sig_handler);

    printf("PLC simulator: %s:%d area=%s db=%d start=%d len=%d "
           "interval=%dms rules=%d\n",
           cfg.ip, cfg.port,
           cfg.area >= 0x81 && cfg.area <= 0x84 ? area_names[cfg.area - 0x80] : "?",
           cfg.db, cfg.start, cfg.len, interval_ms, g_num_rules);

    int fd = -1;
    while (g_running) {
        if (fd < 0) {
            fd = tcp_connect_handshake(&cfg);
            if (fd < 0) {
                printf("connect failed (%s), retrying in 2s...\n", g_err);
                for (int i = 0; i < 40 && g_running; i++) usleep(50000);
                continue;
            }
            printf("connected to %s:%d\n", cfg.ip, cfg.port);
        }

        uint8_t data[MAX_BLOCK];
        char write_desc[256];
        state_to_block(data, cfg.len, state, g_num_rules);
        format_state_addr(write_desc, sizeof(write_desc), &cfg, data, cfg.len);

        if (s7_write(fd, &cfg, data, cfg.len) < 0) {
            printf("write failed (%s), reconnecting...\n", g_err);
            close(fd);
            fd = -1;
            continue;
        }
        printf("state %d: %s (byte0=0x%02X)\n", state, write_desc, data[0]);

        state = (state + 1) % (g_num_rules + 1);

        for (int i = 0; i < interval_ms / 50 && g_running; i++)
            usleep(50000);
    }

    if (fd >= 0) close(fd);
    printf("done\n");
    return 0;
}