test working
This commit is contained in:
jaseg
parent
e6e49ee481
commit
b778151734
2 changed files with 489 additions and 0 deletions
431
gm_platform/fw/tw_test.c
Normal file
431
gm_platform/fw/tw_test.c
Normal file
|
|
@ -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;
|
||||
}
|
||||
58
gm_platform/fw/tw_test.py
Normal file
58
gm_platform/fw/tw_test.py
Normal file
|
|
@ -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()}')
|
||||
Loading…
Add table
Add a link
Reference in a new issue