diff options
| -rw-r--r-- | gm_platform/fw/tw_test.c | 431 | ||||
| -rw-r--r-- | gm_platform/fw/tw_test.py | 58 |
2 files changed, 489 insertions, 0 deletions
diff --git a/gm_platform/fw/tw_test.c b/gm_platform/fw/tw_test.c new file mode 100644 index 0000000..60c6c67 --- /dev/null +++ b/gm_platform/fw/tw_test.c @@ -0,0 +1,431 @@ +#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; + + int current_seq = -1; + wpacket.type = 1; + wpacket.pid = 0; + cobs_encode(wbuf, (char *)&wpacket, sizeof(wpacket)); + write(fd, wbuf, sizeof(wbuf)); + 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); + 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) ; + + if (!in_sync) { + if (first_nul) { + ssize_t first_nul_offx = first_nul - (void*)buf; + ssize_t remaining = wpos - first_nul_offx; + memmove(buf, first_nul+1, remaining-1); + wpos = remaining-1; + in_sync = 1; + continue; + + } else { + wpos = 0; + break; + } + } + + if (!first_nul) + break; + + 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)); + } + } + + /* Check CRC */ + if (our_crc != packet.crc) { + printf("CRC mismatch: seq=%d packet=%08x computed=%08x\n", packet.pid, packet.crc, our_crc); + goto it_err; + } + + /* Check device sequence number */ + int last_seq = current_seq; + int predicted_seq = (last_seq+1) % 0xffff; + current_seq = packet.seq; + if (last_seq >= 0 && packet.seq != predicted_seq) { + printf("SEQ mismatch: packet=%d computed=%d\n", packet.seq, predicted_seq); + 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); + +it_err: + /* FIXME don't send acks in case of error */ + /* send ACK reply */ + wpacket.type = 2; + wpacket.pid = packet.pid; + cobs_encode(wbuf, (char *)&wpacket, sizeof(wpacket)); + write(fd, wbuf, sizeof(wbuf)); + + /* Fixup buffer for next iteration */ + ssize_t first_nul_offx = first_nul - (void*)buf; + ssize_t remaining = wpos - first_nul_offx; + printf("--- debug: first_nul=%p (idx=%d) wpos=%d remaining=%d\n", first_nul, first_nul_offx, wpos, remaining); + hexdump(buf, 80); + printf(" ---memmove(buf=%p, first_nul+1=%p, remaining-1=%d);-->\n", buf, first_nul+1, remaining-1); + memmove(buf, first_nul+1, remaining-1); + hexdump(buf, 80); + fflush(stdout); + 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; +} diff --git a/gm_platform/fw/tw_test.py b/gm_platform/fw/tw_test.py new file mode 100644 index 0000000..0380f95 --- /dev/null +++ b/gm_platform/fw/tw_test.py @@ -0,0 +1,58 @@ +#!/usr/bin/env python3 + +from time import time +from binascii import hexlify +import enum +import struct +import zlib +import sys + +import serial +from cobs import cobs + + +class CtrlPacketTypes(enum.Enum): + RESET = 1 + ACK = 2 + RETRANSMIT = 3 + +def unpack_head(fmt, data): + split = struct.calcsize(fmt) + return *struct.unpack(fmt, data[:split]), data[split:] + +def ctrl_packet(ptype, pid=0): + return cobs.encode(struct.pack('BB', ptype.value, pid)) + b'\0' + +ctrl_reset = lambda: ctrl_packet(CtrlPacketTypes.RESET) +ctrl_ack = lambda pid: ctrl_packet(CtrlPacketTypes.ACK, pid) +ctrl_retransmit = lambda pid: ctrl_packet(CtrlPacketTypes.RETRANSMIT, pid) + + +ser = serial.Serial('/dev/serial/by-id/usb-Silicon_Labs_CP2102_USB_to_UART_Bridge_Controller_0001-if00-port0', 250000, timeout=1.0) +ser.write(b'foobar'*32) +sys.exit(0) + +log = [] +ser.flushInput() +ser.write(ctrl_reset()) +ser.flushOutput() +for _ in range(100): + #ser.write(cobs.encode(b'\x01\xff') + b'\0') + data = ser.read_until(b'\0') + if not data or data[-1] != 0x00: + #print(f'{time():>7.3f} Timeout: resetting') + #ser.write(cobs.encode(b'\x01\xff') + b'\0') # reset + continue + + crc32, payload = unpack_head('I', cobs.decode(data[:-1])) + pid, seq, data = unpack_head('xBH', payload) + ser.write(ctrl_ack(pid)) + ser.flushOutput() + + # Calculate byte-wise CRC32 + #our_crc = zlib.crc32(bytes(b for x in payload for b in (0, 0, 0, x))) + our_crc = 0 + #log.append((time(), seq, crc32, our_crc, pid, data)) + +for time, seq, crc32, our_crc, pid, data in log: + print(f'{time:>7.3f} {seq:05d} {crc32:08x} {our_crc:08x} {pid} {hexlify(data).decode()}') |