summaryrefslogtreecommitdiff
path: root/gm_platform/fw/tw_test.c
diff options
context:
space:
mode:
Diffstat (limited to 'gm_platform/fw/tw_test.c')
-rw-r--r--gm_platform/fw/tw_test.c450
1 files changed, 0 insertions, 450 deletions
diff --git a/gm_platform/fw/tw_test.c b/gm_platform/fw/tw_test.c
deleted file mode 100644
index eb41dbe..0000000
--- a/gm_platform/fw/tw_test.c
+++ /dev/null
@@ -1,450 +0,0 @@
-#include <errno.h>
-#include <sys/ioctl.h>
-#include <asm/termbits.h>
-#include <fcntl.h>
-#include <stdbool.h>
-#include <stdlib.h>
-#include <stdio.h>
-#include <string.h>
-#include <unistd.h>
-#include <stdint.h>
-#include <dirent.h>
-#include <sys/types.h>
-#include <assert.h>
-#include <sys/epoll.h>
-#include <time.h>
-
-#include <sqlite3.h>
-
-#include <zlib.h>
-
-int set_interface_attribs (int fd, int baudrate) {
- struct termios2 tio;
- memset (&tio, 0, sizeof(tio));
- if (ioctl (fd, TCGETS2, &tio) != 0) {
- fprintf(stderr, "Could not request termios for given port\n");
- return -1;
- }
-
- /* FIXME set baudrate */
-
- tio.c_cflag = (tio.c_cflag & ~CSIZE) | CS8; /* 8 bit */
- /* disable IGNBRK for mismatched speed tests; otherwise receive break as \000 chars */
- tio.c_iflag &= ~IGNBRK; /* disable break processing */
- tio.c_lflag = 0; /* no signaling chars, no echo, no canonical processing */
- tio.c_oflag = 0; /* no remapping, no delays */
-
- tio.c_iflag &= ~(IXON | IXOFF | IXANY); /* shut off xon/xoff ctrl */
-
- tio.c_cflag |= (CLOCAL | CREAD);/* ignore modem controls, enable reading */
- tio.c_cflag &= ~(PARENB | PARODD); /* no parity */
- tio.c_cflag &= ~CSTOPB;
- tio.c_cflag &= ~CRTSCTS;
-
- tio.c_cflag &= ~(CBAUD | CBAUDEX);
- tio.c_cflag |= BOTHER;
- tio.c_ospeed = baudrate;
- tio.c_cflag &= ~((CBAUD | CBAUDEX) << IBSHIFT);
- tio.c_cflag |= (B0 << IBSHIFT); /* same as output baudrate */
-
- tio.c_cc[VMIN] = 0; /* non-blocking mode */
- tio.c_cc[VTIME] = 10; /* 1000ms seconds read timeout */
-
- if (ioctl (fd, TCSETS2, &tio)) {
- fprintf(stderr, "Could not set serial port attributes: Error %d in tcsetattr (\"%s\")\n", errno, strerror(errno));
- return -1;
- }
- return 0;
-}
-
-ssize_t cobs_decode(char *dst, size_t dstlen, char *src, size_t srclen) {
- size_t p = 1;
- size_t c = (unsigned char)src[0];
- if (c == 0)
- return -5; /* invalid framing. An empty frame would be [...] 00 01 00, not [...] 00 00 */
-
- while (p < srclen && src[p]) {
- char val;
- c--;
-
- if (c == 0) {
- c = (unsigned char)src[p];
- val = 0;
- } else {
- val = src[p];
- }
-
- if (p > dstlen)
- return -4; /* Destination buffer too small */
- dst[p-1] = val;
- p++;
- }
-
- if (p == srclen)
- return -2; /* Invalid framing. The terminating null byte should always be present in the input buffer. */
-
- if (c != 1)
- return -3; /* Invalid framing. The skip counter does not hit the end of the frame. */
-
- return p-1;
-}
-
-int cobs_encode(char *dst, char *src, size_t srclen) {
- if (srclen > 254)
- return -1;
-
- size_t p = 0;
- while (p <= srclen) {
-
- char val;
- if (p != 0 && src[p-1] != 0) {
- val = src[p-1];
-
- } else {
- size_t q = p;
- while (q < srclen && src[q] != 0)
- q++;
- val = (char)q-p+1;
- }
-
-
- *dst++ = val;
- p++;
- }
-
- *dst++ = 0;
-
- return 0;
-}
-
-void print_usage(char *prog) {
- fprintf(stderr, "Usage: %s [-p /dev/serial/some_port] [-b baudrate] dbfile.sqilte3\n", prog);
-}
-
-void hexdump(const void* data, size_t size) {
- char ascii[17];
- size_t i, j;
- ascii[16] = '\0';
- for (i = 0; i < size; ++i) {
- printf("%02X ", ((unsigned char*)data)[i]);
- if (((unsigned char*)data)[i] >= ' ' && ((unsigned char*)data)[i] <= '~') {
- ascii[i % 16] = ((unsigned char*)data)[i];
- } else {
- ascii[i % 16] = '.';
- }
- if ((i+1) % 8 == 0 || i+1 == size) {
- printf(" ");
- if ((i+1) % 16 == 0) {
- printf("| %s \n", ascii);
- } else if (i+1 == size) {
- ascii[(i+1) % 16] = '\0';
- if ((i+1) % 16 <= 8) {
- printf(" ");
- }
- for (j = (i+1) % 16; j < 16; ++j) {
- printf(" ");
- }
- printf("| %s \n", ascii);
- }
- }
- }
-}
-
-int main(int argc, char *argv[]) {
-
- int opt;
- int baudrate = 250000;
- char *endptr = NULL;
- char *port = NULL;
- char *dbfile = NULL;
- while ((opt = getopt(argc, argv, "p:b:")) != -1) {
- switch (opt) {
- case 'p':
- port = optarg;
- break;
- case 'b':
- baudrate = strtol(optarg, &endptr, 10);
- if (errno == ERANGE || endptr == NULL || *endptr != '\0') {
- fprintf(stderr, "Invalid baudrate \"%s\"\n", optarg);
- print_usage(argv[0]);
- }
- break;
- default:
- print_usage(argv[0]);
- exit(EXIT_FAILURE);
- }
- }
-
- if (port == NULL) {
- DIR *le_dir = opendir("/dev/serial/by-id");
- if (le_dir == NULL) {
- fprintf(stderr, "No serial port given and could not find any in /dev/serial\n");
- exit(EXIT_FAILURE);
-
- }
-
- struct dirent *de;
- while ((de = readdir(le_dir))) {
- if (de == NULL) {
- fprintf(stderr, "No serial port given and could not find any in /dev/serial\n");
- exit(EXIT_FAILURE);
- }
-
- if (!strncmp(de->d_name, ".", sizeof(de->d_name)) ||
- !strncmp(de->d_name, "..", sizeof(de->d_name)))
- continue;
-
- if (port != NULL) {
- fprintf(stderr, "No serial port given and found multiple candidates in /dev/serial\n");
- exit(EXIT_FAILURE);
- }
-
- const char *prefix = "/dev/serial/by-id/";
- port = malloc(strlen(prefix) + sizeof(de->d_name) + 1);
- if (port == NULL) {
- fprintf(stderr, "Could not allocate memory\n");
- exit(EXIT_FAILURE);
- }
- strcpy(port, prefix);
- strncat(port, de->d_name, sizeof(de->d_name));
- }
- fprintf(stderr, "No port given, defaulting to %s\n", port);
- closedir(le_dir);
- }
-
- if (optind != argc - 1) {
- fprintf(stderr, "Too few arguments\n");
- print_usage(argv[0]);
- exit(EXIT_FAILURE);
- }
-
- dbfile = argv[optind];
- printf("Using database file %s\n", dbfile);
- fflush(stdout);
-
- int fd = open(port, O_RDWR|O_NOCTTY|O_SYNC);
- if (fd < 0) {
- fprintf(stderr, "Cannot open serial port: %s\n", strerror(errno));
- exit(EXIT_FAILURE);
- }
-
- if (set_interface_attribs (fd, baudrate))
- exit(EXIT_FAILURE);
-
- sqlite3 *db;
- if (sqlite3_open(dbfile, &db) != SQLITE_OK) {
- fprintf(stderr, "Cannot open database: %s\n", sqlite3_errmsg(db));
- sqlite3_close(db);
- exit(EXIT_FAILURE);
- }
-
- char *errmsg;
- if (sqlite3_exec(db,
- "CREATE TABLE IF NOT EXISTS measurements (rx_time INTEGER, tx_seq INTEGER, rx_seq INTEGER, data BLOB);",
- NULL, NULL, &errmsg) != SQLITE_OK) {
- fprintf(stderr, "Error initializing databse: %s\n", errmsg);
- sqlite3_close(db);
- exit(EXIT_FAILURE);
- }
-
- const char *insert_sql = "INSERT INTO measurements VALUES (?, ?, ?, ?)";
- sqlite3_stmt *insert_stmt;
- if (sqlite3_prepare_v2(db, insert_sql, strlen(insert_sql), &insert_stmt, NULL) != SQLITE_OK) {
- fprintf(stderr, "Error compiling SQL: %s\n", sqlite3_errmsg(db));
- sqlite3_close(db);
- exit(EXIT_FAILURE);
- }
-
- char buf [1024];
- int in_sync = 0, wpos = 0;
- struct __attribute__((__packed__)) {
- uint32_t crc;
- uint8_t pid;
- uint8_t _pad;
- uint16_t seq;
- uint16_t data[32];
- } packet;
- struct __attribute__((__packed__)) {
- uint8_t type;
- uint8_t pid;
- } wpacket;
- char wbuf[4];
-
- int epollfd = epoll_create1(0);
- if (epollfd < 0)
- goto epoll_err;
-
- #define MAX_EVENTS 10
- struct epoll_event ev, events[MAX_EVENTS];
- ev.events = EPOLLIN;
- ev.data.fd = fd;
- if (epoll_ctl(epollfd, EPOLL_CTL_ADD, fd, &ev) < 0)
- goto epoll_err;
-
- wpacket.type = 1;
- wpacket.pid = 0;
- cobs_encode(wbuf, (char *)&wpacket, sizeof(wpacket));
- write(fd, wbuf, sizeof(wbuf));
-
- /* FIXME begin debug code */
- for (int i=0; i<32; i++) {
- wpacket.type = 2;
- wpacket.pid = packet.pid;
- cobs_encode(wbuf, (char *)&wpacket, sizeof(wpacket));
- write(fd, wbuf, sizeof(wbuf));
- usleep(20);
- }
- /* FIXME end debug code */
-
- int current_seq = -1;
- uint64_t local_seq = 0;
- while (23) {
- int nfds = epoll_wait(epollfd, events, MAX_EVENTS, -1);
- if (nfds == -1)
- goto epoll_err;
-
- if (nfds == 0)
- continue;
-
- ssize_t n = read(fd, buf+wpos, sizeof(buf)-wpos);
- printf("--- read wpos=%d n=%ld\n", wpos, n);
- hexdump(buf+wpos, n);
- if (n<0) {
- if (errno == EAGAIN || errno == EINTR)
- continue;
-
- fprintf(stderr, "Error reading from port: %s\n", strerror(errno));
- goto loop_err;
- }
- //printf("--- debug: read n=%d bytes at wpos=%d\n", n, wpos);
- //fflush(stdout);
- wpos += n;
-
- while (23) {
- void *first_nul = memchr(buf, 0, wpos) ;
- ssize_t first_nul_offx = first_nul - (void*)buf;
- ssize_t remaining = wpos - first_nul_offx;
-
- if (!in_sync) {
- if (first_nul) {
- memmove(buf, first_nul+1, remaining-1);
- wpos = remaining-1;
- in_sync = 1;
- continue;
-
- } else {
- wpos = 0;
- break;
- }
- }
-
- if (!first_nul)
- break;
-
- printf("--- debug: first_nul=%p (idx=%ld) wpos=%d remaining=%ld\n", first_nul, first_nul_offx, wpos, remaining);
- hexdump(buf, 80);
-
- int rc = cobs_decode((char *)&packet, sizeof(packet), buf, wpos);
- if (rc < 0) {
- printf("Framing error: rc=%d\n", rc);
- goto it_err;
- }
-
- /* Use zlib to calculate CRC32. The STM32 code calculates the CRC byte-wise, so we emulate this here. */
- uint32_t our_crc = 0;
- if (rc > 0) {
- uint8_t buf[4] = {0};
- for (int i=4; i<rc; i++) {
- buf[3] = ((uint8_t *)&packet)[i];
- our_crc = crc32(our_crc, buf, sizeof(buf));
- }
- }
-
- bool error = false;
- /* Check CRC */
- if (our_crc != packet.crc) {
- printf("CRC mismatch: seq=%d packet=%08x computed=%08x\n", packet.pid, packet.crc, our_crc);
- error = true;
- }
-
- /* Check device sequence number */
- int last_seq = current_seq;
- int predicted_seq = (last_seq+1) % 0xffff;
- if (!error)
- current_seq = packet.seq;
- if (last_seq >= 0 && packet.seq != predicted_seq) {
- printf("SEQ mismatch: packet=%d computed=%d\n", packet.seq, predicted_seq);
- error = true;
- }
-
- if (error)
- goto it_err;
-
- /* Write to database */
- struct timespec ts;
- if (clock_gettime(CLOCK_REALTIME, &ts)) {
- fprintf(stderr, "Error getting current wall-clock time: %s\n", strerror(errno));
- goto loop_err;
- }
- uint64_t timestamp = ts.tv_sec*1000 + ts.tv_nsec/1000000;
-
- if (sqlite3_bind_int(insert_stmt, 1, timestamp) != SQLITE_OK)
- goto write_err;
-
- if (sqlite3_bind_int(insert_stmt, 2, packet.seq) != SQLITE_OK)
- goto write_err;
-
- if (sqlite3_bind_int(insert_stmt, 3, local_seq) != SQLITE_OK)
- goto write_err;
-
- if (sqlite3_bind_blob(insert_stmt, 4, packet.data, sizeof(packet.data), SQLITE_STATIC) != SQLITE_OK)
- goto write_err;
-
- while ((rc = sqlite3_step(insert_stmt)) == SQLITE_BUSY)
- ;
- if (rc != SQLITE_DONE)
- goto write_err;
-
- if (sqlite3_reset(insert_stmt) != SQLITE_OK)
- goto write_err;
-
- if (sqlite3_clear_bindings(insert_stmt) != SQLITE_OK)
- goto write_err;
-
- local_seq++;
-
- printf("OK: seq=%d crc=%08x\n", current_seq, packet.crc);
-
- /* send ACK reply */
- wpacket.type = 2;
- wpacket.pid = packet.pid;
- cobs_encode(wbuf, (char *)&wpacket, sizeof(wpacket));
- write(fd, wbuf, sizeof(wbuf));
-
-it_err:
- /* Fixup buffer for next iteration */
- if (remaining-1 > 0) {
- printf(" ---memmove(buf=%p, first_nul+1=%p, remaining-1=%ld);-->\n", buf, first_nul+1, remaining-1);
- memmove(buf, first_nul+1, remaining-1);
- }
- //hexdump(buf, 80);
- fflush(stdout);
- printf("--- continuing wpos=%d->%d\n", wpos, (int)(remaining-1));
- wpos = remaining-1;
- }
- }
-
- return 0;
-
-write_err:
- fprintf(stderr, "Error writing to database: %s\n", sqlite3_errmsg(db));
- sqlite3_close(db);
- return EXIT_FAILURE;
-
-epoll_err:
- fprintf(stderr, "epoll error: %s\n", strerror(errno));
-
-loop_err:
- sqlite3_close(db);
- return EXIT_FAILURE;
-}
generated by cgit (git 2.34.1) at 2025-02-17 10:28:20 +0000