commit 3d15b4afbd0c51b6a7fa63d19e9d253ce9e26bf6
parent 0c863e887026f2e62d052ce7dfd0291aad116ef0
Author: Brian Swetland <swetland@frotz.net>
Date: Thu, 5 May 2022 16:26:32 -0700
support litex bootloader serial download protocol
- any number of files may be specified with -bfilename@hexaddr
- in response to the serial boot challenge, they will be downloaded in order
- after downloading, the bootloader will be instructed to jump to the *first* item
- rearranged sconsole.c a bit
- pushed apple compatibility (no clue if we still need this) and
swv decoder stuff into their own files
Diffstat:
| A | apple.c | | | 66 | ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| M | sconsole.c | | | 562 | ++++++++++++++++++++++++++++++++++++------------------------------------------- |
| A | swv.c | | | 147 | +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
3 files changed, 467 insertions(+), 308 deletions(-)
diff --git a/apple.c b/apple.c
@@ -0,0 +1,66 @@
+// Copyright 2005-2022, Brian Swetland <swetland@frotz.net>
+// Licensed under the Apache License, Version 2.0
+
+#include <IOKit/serial/ioss.h>
+#define B4000000 4000000
+#define B3500000 3500000
+#define B3000000 3000000
+#define B2500000 2500000
+#define B2000000 2000000
+#define B1500000 1500000
+#define B1152000 1152000
+#define B1000000 1000000
+#define B921600 921600
+
+/* Darwin's poll is broken. Implement a replacement using select */
+int select_poll(struct pollfd *fds, int nfds, int timeout)
+{
+ fd_set rfds;
+ fd_set wfds;
+ fd_set efds;
+ int max_fd = 0;
+ int retval;
+ struct timeval tv;
+ int i;
+
+ FD_ZERO(&rfds);
+ FD_ZERO(&wfds);
+ FD_ZERO(&efds);
+
+ for (i = 0; i < nfds; i++) {
+ if (fds[i].events == POLLIN)
+ FD_SET(fds[i].fd, &rfds);
+ if (fds[i].events == POLLOUT)
+ FD_SET(fds[i].fd, &wfds);
+ FD_SET(fds[i].fd, &efds);
+
+ if (fds[i].fd > max_fd)
+ max_fd = fds[i].fd;
+
+ fds[i].revents = 0;
+ }
+
+ retval = select(max_fd + 1, &rfds, &wfds, &efds, NULL);
+
+ if (errno == EAGAIN) {
+ return 0;
+ }
+
+ if (retval < 0) {
+ return -1;
+ }
+
+ for (i = 0; i < nfds; i++ ) {
+ if (FD_ISSET(fds[i].fd, &rfds))
+ fds[i].revents |= POLLIN;
+ if (FD_ISSET(fds[i].fd, &wfds))
+ fds[i].revents |= POLLOUT;
+ if (FD_ISSET(fds[i].fd, &efds))
+ fds[i].revents |= POLLERR;
+ }
+
+ return 1;
+}
+
+#define poll select_poll
+
diff --git a/sconsole.c b/sconsole.c
@@ -1,38 +1,14 @@
-/* sconsole - cheap serial console (for xterm, etc)
- *
- * Copyright (c) 2005-2015
- * Brian Swetland.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- * 1. Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in the
- * documentation and/or other materials provided with the distribution.
- * 3. Neither the name of the authors nor the names of its contributors
- * may be used to endorse or promote products derived from this software
- * without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND
- * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
- * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
- * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
- * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
- * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
- * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
- * SUCH DAMAGE.
- */
+// Copyright 2005-2022, Brian Swetland <swetland@frotz.net>
+// Licensed under the Apache License, Version 2.0
+//
+// sconsole - cheap serial console (for xterm, etc)
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <errno.h>
+#include <fcntl.h>
#include <termios.h>
#include <signal.h>
@@ -44,23 +20,41 @@
#include <sys/fcntl.h>
#include <sys/types.h>
+#include "litex/sfl.h"
+#include "litex/crc16.c"
+
#ifdef __APPLE__
-#include <IOKit/serial/ioss.h>
-#define B4000000 4000000
-#define B3500000 3500000
-#define B3000000 3000000
-#define B2500000 2500000
-#define B2000000 2000000
-#define B1500000 1500000
-#define B1152000 1152000
-#define B1000000 1000000
-#define B921600 921600
+#include "apple.h"
#endif
+void *load_file(const char *fn, size_t *_sz) {
+ int fd;
+ off_t sz;
+ void *data = NULL;
+ fd = open(fn, O_RDONLY);
+ if (fd < 0) goto fail;
+ sz = lseek(fd, 0, SEEK_END);
+ if (sz < 0) goto fail;
+ if (lseek(fd, 0, SEEK_SET)) goto fail;
+ if ((data = malloc(sz + 4)) == NULL) goto fail;
+ if (read(fd, data, sz) != sz) goto fail;
+ *_sz = sz;
+ return data;
+fail:
+ if (data) free(data);
+ if (fd >= 0) close(fd);
+ return NULL;
+}
+
+typedef struct bootinfo {
+ struct bootinfo* next;
+ const char* name;
+ unsigned addr;
+} bootinfo_t;
+
static struct termios tio_save;
-static void stdin_raw_init(void)
-{
+static void stdin_raw_init(void) {
struct termios tio;
if (tcgetattr(0, &tio))
@@ -79,22 +73,19 @@ static void stdin_raw_init(void)
tcflush(0, TCIFLUSH);
}
-static void stdin_raw_restore(void)
-{
+static void stdin_raw_restore(void) {
tcsetattr(0, TCSANOW, &tio_save);
tcflush(0, TCIFLUSH);
}
-void oops(int x)
-{
+void oops(int x) {
char *msg = "\n[ killed by signal ]\n";
write(2, msg, strlen(msg));
stdin_raw_restore();
exit(1);
}
-int text2speed(const char *s)
-{
+int text2speed(const char *s) {
char *e;
unsigned n = strtoul(s, &e, 10);
switch (*e) {
@@ -128,8 +119,7 @@ int text2speed(const char *s)
}
}
-int openserial(const char *device, int speed)
-{
+int openserial(const char *device, int speed) {
struct termios tio;
int fd;
int fl;
@@ -193,61 +183,6 @@ static unsigned char valid[256];
#define STATE_PREFIX 1
#define STATE_COMMAND 2
-#ifdef __APPLE__
-/* Darwin's poll is broken. Implement a replacement using select */
-int select_poll(struct pollfd *fds, int nfds, int timeout)
-{
- fd_set rfds;
- fd_set wfds;
- fd_set efds;
- int max_fd = 0;
- int retval;
- struct timeval tv;
- int i;
-
- FD_ZERO(&rfds);
- FD_ZERO(&wfds);
- FD_ZERO(&efds);
-
- for (i = 0; i < nfds; i++) {
- if (fds[i].events == POLLIN)
- FD_SET(fds[i].fd, &rfds);
- if (fds[i].events == POLLOUT)
- FD_SET(fds[i].fd, &wfds);
- FD_SET(fds[i].fd, &efds);
-
- if (fds[i].fd > max_fd)
- max_fd = fds[i].fd;
-
- fds[i].revents = 0;
- }
-
- retval = select(max_fd + 1, &rfds, &wfds, &efds, NULL);
-
- if (errno == EAGAIN) {
- return 0;
- }
-
- if (retval < 0) {
- return -1;
- }
-
- for (i = 0; i < nfds; i++ ) {
- if (FD_ISSET(fds[i].fd, &rfds))
- fds[i].revents |= POLLIN;
- if (FD_ISSET(fds[i].fd, &wfds))
- fds[i].revents |= POLLOUT;
- if (FD_ISSET(fds[i].fd, &efds))
- fds[i].revents |= POLLERR;
- }
-
- return 1;
-}
-
-#define poll select_poll
-
-#endif /* __APPLE__ */
-
void usage(void) {
fprintf(stderr,
"usage: sconsole [ <flag> ]* [ <device> [ <speed> ] ]\n"
@@ -256,157 +191,208 @@ void usage(void) {
" -l log to console.log (or -lsomeotherlogfile)\n"
" -c convert NL to CR on transmit\n"
" -x display characters in hex\n"
+ " -b serialboot file at addr (-bfilename@1000)\n"
"\n"
"default device /dev/ttyUSB and speed 115200\n"
);
}
-typedef enum {
- SWV_SYNC,
- SWV_1X1,
- SWV_1X2,
- SWV_1X4,
- SWV_2X2,
- SWV_2X4,
- SWV_3X4,
- SWV_4X4,
- SWV_PROTO,
- SWV_IDLE,
-} swv_state_t;
-
-typedef struct {
- swv_state_t state;
- unsigned zcount;
- unsigned ccount;
- unsigned id;
- unsigned val;
- unsigned char data[8];
-} swv_t;
-
-static swv_t swv = {
- .state = SWV_SYNC,
- .zcount = 0
-};
-
-void handle_swv_src(unsigned id, unsigned val, unsigned n) {
- printf("SRC %s %02x %08x\n", (id & 0x100) ? "HW" : "SW", id & 0xFF, val);
+int sfd = -1;
+
+int logfd = -1;
+int hexmode = 0;
+int decode_swv = 0;
+
+unsigned bootmatch = 0;
+bootinfo_t* bootlist = NULL;
+bootinfo_t* bootlast = NULL; // end of list
+
+bootinfo_t* bootitem = NULL;
+unsigned char* bootdata;
+unsigned char* bootptr;
+size_t bootsize = 0;
+unsigned bootaddr = 0;
+unsigned bootfail = 0;
+
+struct sfl_frame frame;
+
+void send_frame(unsigned cmd, unsigned param, void* data, unsigned len) {
+ frame.cmd = cmd;
+ if (param == SFL_CMD_ABORT) {
+ frame.payload_length = 0;
+ } else {
+ frame.payload_length = len + 4;
+ frame.payload[0] = param >> 24;
+ frame.payload[1] = param >> 16;
+ frame.payload[2] = param >> 8;
+ frame.payload[3] = param;
+ memcpy(frame.payload + 4, data, len);
+ }
+ unsigned crc = crc16(&frame.cmd, frame.payload_length + 1);
+ frame.crc[0] = crc >> 8;
+ frame.crc[1] = crc;
+ write(sfd, &frame, frame.payload_length + 4);
+ bootfail = 0;
+}
+
+void resend_frame(void) {
+ write(sfd, &frame, frame.payload_length + 4);
+ bootfail++;
}
-void handle_swv_proto(unsigned char *data, unsigned len) {
- switch (len) {
- case 1:
- printf("PRO %02x\n", data[0]);
- break;
- case 2:
- printf("PRO %02x %02x\n", data[0], data[1]);
- break;
- case 3:
- printf("PRO %02x %02x %02x\n",
- data[0], data[1], data[2]);
- break;
- case 4:
- printf("PRO %02x %02x %02x %02x\n",
- data[0], data[1], data[2], data[3]);
- break;
- case 5:
- printf("PRO %02x %02x %02x %02x %02x\n",
- data[0], data[1], data[2], data[3], data[4]);;
- break;
- case 6:
- printf("PRO %02x %02x %02x %02x %02x %02x\n",
- data[0], data[1], data[2], data[3],
- data[4], data[5]);
- break;
- case 7:
- printf("PRO %02x %02x %02x %02x %02x %02x %02x\n",
- data[0], data[1], data[2], data[3],
- data[4], data[5], data[6]);
- break;
+int boot_start(bootinfo_t* item);
+
+int boot_next(int x) {
+ if ((x > ' ') && (x < 127)) {
+ fprintf(stderr, "%c", x);
+ } else {
+ fprintf(stderr, ".");
+ }
+ switch (x) {
+ case SFL_ACK_SUCCESS: {
+ if (bootsize == 0) {
+ free(bootdata);
+ if (bootitem->next) {
+ return boot_start(bootitem->next);
+ }
+ fprintf(stderr, "\n[ jump to 0x%08x ]\n", bootlist->addr);
+ send_frame(SFL_CMD_JUMP, bootlist->addr, NULL, 0);
+ bootitem = NULL;
+ return -1;
+ }
+ unsigned xfer = bootsize > 64 ? 64 : bootsize;
+ send_frame(SFL_CMD_LOAD, bootaddr, bootptr, xfer);
+ bootptr += xfer;
+ bootaddr += xfer;
+ bootsize -= xfer;
+ return 250;
+ }
+ case -1: // timeout
+ case SFL_ACK_CRCERROR:
+ case SFL_ACK_UNKNOWN:
+ case SFL_ACK_ERROR:
+ if (bootfail == 10) {
+ fprintf(stderr, "\n[ serial boot failed ]\n");
+ free(bootdata);
+ bootitem = NULL;
+ send_frame(SFL_CMD_ABORT, 0, NULL, 0);
+ return 250;
+ }
+ resend_frame();
+ }
+}
+
+int boot_start(bootinfo_t* item) {
+ bootitem = item;
+ bootdata = load_file(bootitem->name, &bootsize);
+ bootaddr = bootitem->addr;
+ if (bootdata == NULL) {
+ fprintf(stderr, "\n[ could not load '%s' ]\n", bootitem->name);
+ bootitem = NULL;
+ send_frame(SFL_CMD_ABORT, 0, NULL, 0);
+ return -1;
+ } else {
+ fprintf(stderr, "\n[ sending '%s' to 0x%08x ]\n",
+ bootitem->name, bootitem->addr);
+ bootptr = bootdata;
+ return boot_next(SFL_ACK_SUCCESS);
}
}
-void handle_swv(swv_t *swv, unsigned x) {
- //printf("%02x ", x); fflush(stdout);
- // any sequence ending in 00 00 00 00 00 80 is a re-sync
- if (x == 0) {
- swv->zcount++;
+int serial_rx(int x) {
+ if (bootitem) {
+ return boot_next(x);
+ }
+ if (bootlist) {
+ // if we can serialboot, listen for magic to start
+ if (SFL_MAGIC_REQ[bootmatch] == x) {
+ bootmatch++;
+ if (bootmatch == SFL_MAGIC_LEN) {
+ fprintf(stderr, "\n[ serial boot requested ]\n");
+ bootmatch = 0;
+ write(sfd, SFL_MAGIC_ACK, SFL_MAGIC_LEN);
+ return boot_start(bootlist);
+ }
+ } else {
+ bootmatch = 0;
+ }
+ }
+ if (hexmode) {
+ char hex[4];
+ sprintf(hex, "%02x ", x);
+ write(1, hex, 3);
+ if (logfd != -1) {
+ write(logfd, &x, 1);
+ }
+#if WITH_SWV
+ } else if (decode_swv) {
+ handle_swv(&swv, x);
+#endif
} else {
- if ((swv->zcount >= 5) && (x == 0x80)) {
- swv->state = SWV_IDLE;
- swv->zcount = 0;
- printf("SYNC\n");
- return;
+ unsigned char c = x;
+ if (!valid[x]) {
+ c = '.';
+ }
+ if (valid[x] != -1) {
+ write(1, &c, 1);
+ if (logfd != -1) {
+ write(logfd, &c, 1);
+ }
}
- swv->zcount = 0;
}
+ return -1;
+}
+
+unsigned char ESC = 27;
+static int escape = 0;
+int map_nl_to_cr = 0;
- switch (swv->state) {
- case SWV_IDLE:
- if (x & 7) {
- // AAAAAHSS source packet
- swv->id = (x >> 3) | ((x & 4) << 6);
- swv->val = 0;
- swv->state = (x & 3);
- } else if (x != 0) {
- // CXXXXX00 protocol packet
- swv->data[0] = x;
- if (x & 0x80) {
- swv->ccount = 1;
- swv->state = SWV_PROTO;
+void console_rx(int x) {
+ switch (escape) {
+ case 0:
+ if (x == 27) {
+ escape = 1;
+ } else {
+ if ((x == '\n') && map_nl_to_cr) {
+ x = '\r';
+ write(sfd, &x, 1);
} else {
- handle_swv_proto(swv->data, 1);
+ write(sfd, &x, 1);
}
- } else {
- // 00 packets are for sync, ignore
}
break;
- case SWV_PROTO:
- swv->data[swv->ccount++] = x;
- // protocol packets end at 7 total bytes or a byte with bit7 clear
- if ((swv->ccount == 7) || (!(x & 0x80))) {
- handle_swv_proto(swv->data, swv->ccount);
- swv->state = SWV_IDLE;
+ case 1:
+ if (x == 27) {
+ escape = 2;
+ fprintf(stderr, "\n[ (b)reak? e(x)it? ]\n");
+ } else {
+ escape = 0;
+ write(sfd, &ESC, 1);
+ write(sfd, &x, 1);
}
break;
- case SWV_1X1:
- handle_swv_src(swv->id, x, 1);
- swv->state = SWV_IDLE;
- break;
- case SWV_1X2:
- swv->val = x;
- swv->state = SWV_2X2;
- break;
- case SWV_2X2:
- handle_swv_src(swv->id, swv->val | (x << 8), 2);
- swv->state = SWV_IDLE;
- break;
- case SWV_1X4:
- swv->val = x;
- swv->state = SWV_2X4;
- break;
- case SWV_2X4:
- swv->val |= (x << 8);
- swv->state = SWV_3X4;
- break;
- case SWV_3X4:
- swv->val |= (x << 16);
- swv->state = SWV_4X4;
- break;
- case SWV_4X4:
- handle_swv_src(swv->id, swv->val | (x << 24), 4);
- swv->state = SWV_IDLE;
- break;
- case SWV_SYNC:
+ case 2:
+ escape = 0;
+ switch (x) {
+ case 27:
+ write(sfd, &x, 1);
+ break;
+ case 'b':
+ fprintf(stderr, "[ break ]\n");
+ tcsendbreak(sfd, 0);
+ break;
+ case 'x':
+ fprintf(stderr, "[ exit ]\n");
+ stdin_raw_restore();
+ exit(0);
+ default:
+ fprintf(stderr, "[ huh? ]\n");
break;
- default:
- // impossible
- printf("fatal error, bad state %d\n", swv->state);
- exit(1);
+ }
}
}
-int main(int argc, char *argv[])
-{
+int main(int argc, char *argv[]) {
struct pollfd fds[2];
int speed = B115200;
#ifdef __APPLE__
@@ -415,13 +401,8 @@ int main(int argc, char *argv[])
const char *device = "/dev/ttyUSB0";
#endif
const char *logfile = "console.log";
- int fd, n;
- int map_nl_to_cr = 0;
- int escape = 0;
- int logfd = -1;
- int hexmode = 0;
- unsigned char ESC = 27;
- int decode_swv = 0;
+ int timeout;
+ int n, r;
for (n = ' '; n < 127; n++)
valid[n] = 1;
@@ -434,6 +415,25 @@ int main(int argc, char *argv[])
while ((argc > 1) && (argv[1][0] == '-')) {
switch (argv[1][1]) {
+ case 'b': {
+ bootinfo_t *item = malloc(sizeof(bootinfo_t));
+ item->name = &argv[1][2];
+ item->next = NULL;
+ char *tmp = strchr(item->name, '@');
+ if (tmp) {
+ item->addr = strtoul(tmp + 1, NULL, 16);
+ *tmp = 0;
+ } else {
+ item->addr = 0x40000000;
+ }
+ if (bootlast) {
+ bootlast->next = item;
+ } else {
+ bootlist = item;
+ }
+ bootlast = item;
+ break;
+ }
case 'x':
hexmode = 1;
break;
@@ -442,8 +442,7 @@ int main(int argc, char *argv[])
for (n = 0; n < 256; n++)
valid[n] = 1;
break;
- case 'l':
- /* log */
+ case 'l': /* log */
if (argv[1][2])
logfile = &argv[1][2];
logfd = open(logfile, O_CREAT | O_WRONLY, 0644);
@@ -455,9 +454,11 @@ int main(int argc, char *argv[])
case 'h':
usage();
return 0;
+#if WITH_SWV
case 's':
decode_swv = 1;
break;
+#endif
default:
fprintf(stderr, "error: unknown option %s\n\n", argv[1]);
usage();
@@ -480,8 +481,8 @@ int main(int argc, char *argv[])
argv++;
}
- fd = openserial(device, speed);
- if (fd < 0) {
+ sfd = openserial(device, speed);
+ if (sfd < 0) {
fprintf(stderr, "error opening '%s': %s\n",
device, strerror(errno));
return -1;
@@ -493,17 +494,21 @@ int main(int argc, char *argv[])
fds[0].fd = 0;
fds[0].events = POLLIN;
- fds[1].fd = fd;
+ fds[1].fd = sfd;
fds[1].events = POLLIN;
fprintf(stderr, "[ %s ]\n", device);
+ timeout = -1;
for (;;) {
- unsigned char x, t;
+ unsigned char x;
- if (poll(fds, 2, -1) < 1)
+ r = poll(fds, 2, timeout);
+ if (r < 1 ) {
continue;
-
+ } else if (r == 0) {
+ timeout = serial_rx(-1);
+ }
if (fds[0].revents & (POLLERR | POLLHUP)) {
fprintf(stderr, "\n[ stdin port closed ]\n");
break;
@@ -513,71 +518,12 @@ int main(int argc, char *argv[])
break;
}
if ((fds[0].revents & POLLIN) && (read(0, &x, 1) == 1)) {
- switch (escape) {
- case 0:
- if (x == 27) {
- escape = 1;
- } else {
- if ((x == '\n') && map_nl_to_cr) {
- x = '\r';
- write(fd, &x, 1);
- } else {
- write(fd, &x, 1);
- }
- }
- break;
- case 1:
- if (x == 27) {
- escape = 2;
- fprintf(stderr, "\n[ (b)reak? e(x)it? ]\n");
- } else {
- escape = 0;
- write(fd, &ESC, 1);
- write(fd, &x, 1);
- }
- break;
- case 2:
- escape = 0;
- switch (x) {
- case 27:
- write(fd, &x, 1);
- break;
- case 'b':
- fprintf(stderr, "[ break ]\n");
- tcsendbreak(fd, 0);
- break;
- case 'x':
- fprintf(stderr, "[ exit ]\n");
- goto done;
- default:
- fprintf(stderr, "[ huh? ]\n");
- break;
- }
- break;
- }
+ console_rx(x);
}
- if ((fds[1].revents & POLLIN) && (read(fd, &x, 1) == 1)) {
- if (hexmode) {
- char hex[4];
- sprintf(hex, "%02x ", x);
- write(1, hex, 3);
- } else if (decode_swv) {
- handle_swv(&swv, x);
- } else {
- unsigned char c = x;
- if (!valid[x])
- c = '.';
-
- if (valid[x] != -1) {
- write(1, &c, 1);
- if (logfd != -1)
- write(logfd, &c, 1);
- }
- }
+ if ((fds[1].revents & POLLIN) && (read(sfd, &x, 1) == 1)) {
+ timeout = serial_rx(x);
}
}
-done:
- stdin_raw_restore();
return 0;
}
diff --git a/swv.c b/swv.c
@@ -0,0 +1,147 @@
+// Copyright 2005-2022, Brian Swetland <swetland@frotz.net>
+// Licensed under the Apache License, Version 2.0
+
+typedef enum {
+ SWV_SYNC,
+ SWV_1X1,
+ SWV_1X2,
+ SWV_1X4,
+ SWV_2X2,
+ SWV_2X4,
+ SWV_3X4,
+ SWV_4X4,
+ SWV_PROTO,
+ SWV_IDLE,
+} swv_state_t;
+
+typedef struct {
+ swv_state_t state;
+ unsigned zcount;
+ unsigned ccount;
+ unsigned id;
+ unsigned val;
+ unsigned char data[8];
+} swv_t;
+
+static swv_t swv = {
+ .state = SWV_SYNC,
+ .zcount = 0
+};
+
+void handle_swv_src(unsigned id, unsigned val, unsigned n) {
+ printf("SRC %s %02x %08x\n", (id & 0x100) ? "HW" : "SW", id & 0xFF, val);
+}
+
+void handle_swv_proto(unsigned char *data, unsigned len) {
+ switch (len) {
+ case 1:
+ printf("PRO %02x\n", data[0]);
+ break;
+ case 2:
+ printf("PRO %02x %02x\n", data[0], data[1]);
+ break;
+ case 3:
+ printf("PRO %02x %02x %02x\n",
+ data[0], data[1], data[2]);
+ break;
+ case 4:
+ printf("PRO %02x %02x %02x %02x\n",
+ data[0], data[1], data[2], data[3]);
+ break;
+ case 5:
+ printf("PRO %02x %02x %02x %02x %02x\n",
+ data[0], data[1], data[2], data[3], data[4]);;
+ break;
+ case 6:
+ printf("PRO %02x %02x %02x %02x %02x %02x\n",
+ data[0], data[1], data[2], data[3],
+ data[4], data[5]);
+ break;
+ case 7:
+ printf("PRO %02x %02x %02x %02x %02x %02x %02x\n",
+ data[0], data[1], data[2], data[3],
+ data[4], data[5], data[6]);
+ break;
+ }
+}
+
+void handle_swv(swv_t *swv, unsigned x) {
+ //printf("%02x ", x); fflush(stdout);
+ // any sequence ending in 00 00 00 00 00 80 is a re-sync
+ if (x == 0) {
+ swv->zcount++;
+ } else {
+ if ((swv->zcount >= 5) && (x == 0x80)) {
+ swv->state = SWV_IDLE;
+ swv->zcount = 0;
+ printf("SYNC\n");
+ return;
+ }
+ swv->zcount = 0;
+ }
+
+ switch (swv->state) {
+ case SWV_IDLE:
+ if (x & 7) {
+ // AAAAAHSS source packet
+ swv->id = (x >> 3) | ((x & 4) << 6);
+ swv->val = 0;
+ swv->state = (x & 3);
+ } else if (x != 0) {
+ // CXXXXX00 protocol packet
+ swv->data[0] = x;
+ if (x & 0x80) {
+ swv->ccount = 1;
+ swv->state = SWV_PROTO;
+ } else {
+ handle_swv_proto(swv->data, 1);
+ }
+ } else {
+ // 00 packets are for sync, ignore
+ }
+ break;
+ case SWV_PROTO:
+ swv->data[swv->ccount++] = x;
+ // protocol packets end at 7 total bytes or a byte with bit7 clear
+ if ((swv->ccount == 7) || (!(x & 0x80))) {
+ handle_swv_proto(swv->data, swv->ccount);
+ swv->state = SWV_IDLE;
+ }
+ break;
+ case SWV_1X1:
+ handle_swv_src(swv->id, x, 1);
+ swv->state = SWV_IDLE;
+ break;
+ case SWV_1X2:
+ swv->val = x;
+ swv->state = SWV_2X2;
+ break;
+ case SWV_2X2:
+ handle_swv_src(swv->id, swv->val | (x << 8), 2);
+ swv->state = SWV_IDLE;
+ break;
+ case SWV_1X4:
+ swv->val = x;
+ swv->state = SWV_2X4;
+ break;
+ case SWV_2X4:
+ swv->val |= (x << 8);
+ swv->state = SWV_3X4;
+ break;
+ case SWV_3X4:
+ swv->val |= (x << 16);
+ swv->state = SWV_4X4;
+ break;
+ case SWV_4X4:
+ handle_swv_src(swv->id, swv->val | (x << 24), 4);
+ swv->state = SWV_IDLE;
+ break;
+ case SWV_SYNC:
+ break;
+ default:
+ // impossible
+ printf("fatal error, bad state %d\n", swv->state);
+ exit(1);
+ }
+}
+
