1
0
mirror of git://projects.qi-hardware.com/antorcha.git synced 2024-11-01 07:24:58 +02:00
antorcha/tools/antorcha.c

558 lines
11 KiB
C

/*
* tools/antorcha.c - Antorcha control utility
*
* Written 2012 by Werner Almesberger
* Copyright 2012 Werner Almesberger
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*/
#include <stdint.h>
#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <string.h>
#include <signal.h>
#include <assert.h>
#include <at86rf230.h>
#include <atrf.h>
#include <misctxrx.h>
#include <proto.h>
#include "hash.h"
#include "plot.h"
static int verbose = 1;
static int debug = 0;
static void rf_init(struct atrf_dsc *dsc, int trim, int channel)
{
atrf_reg_write(dsc, REG_TRX_STATE, TRX_CMD_TRX_OFF);
atrf_reg_write(dsc, REG_XOSC_CTRL,
(XTAL_MODE_INT << XTAL_MODE_SHIFT) | trim);
atrf_reg_write(dsc, REG_PHY_CC_CCA, (1 << CCA_MODE_SHIFT) | channel);
atrf_reg_write(dsc, REG_IRQ_MASK, 0xff);
flush_interrupts(dsc);
atrf_reg_write(dsc, REG_TRX_STATE, TRX_CMD_PLL_ON);
wait_for_interrupt(dsc, IRQ_PLL_LOCK, IRQ_PLL_LOCK, 1);
}
static void rf_send(struct atrf_dsc *dsc, void *buf, int len)
{
uint8_t tmp[MAX_PSDU];
/* Copy the message to append the CRC placeholders */
memcpy(tmp, buf, len);
atrf_reg_write(dsc, REG_TRX_STATE, TRX_CMD_PLL_ON);
flush_interrupts(dsc);
atrf_buf_write(dsc, tmp, len+2);
atrf_reg_write(dsc, REG_TRX_STATE, TRX_CMD_TX_START);
wait_for_interrupt(dsc, IRQ_TRX_END,
IRQ_TRX_END | IRQ_PLL_LOCK, 10);
atrf_reg_write(dsc, REG_TRX_STATE, TRX_CMD_RX_ON);
if (debug) {
int i;
fprintf(stderr, "\r%d:", len);
for (i = 0; i != len; i++)
fprintf(stderr, " %02x", ((uint8_t *) buf)[i]);;
fprintf(stderr, "\n");
}
}
static int rf_recv(struct atrf_dsc *dsc, void *buf, int size)
{
uint8_t irq;
int len;
/* Wait up to 100 ms */
irq = wait_for_interrupt(dsc, IRQ_TRX_END,
IRQ_TRX_END | IRQ_RX_START | IRQ_AMI, 100);
if (!irq)
return 0;
len = atrf_buf_read(dsc, buf, size);
if (len < 0)
exit(1);
if (len < 3) /* CRC and LQI */
return 0;
return atrf_reg_read(dsc, REG_PHY_RSSI) & RX_CRC_VALID ? len-3 : 0;
}
static void packet_noack(struct atrf_dsc *dsc,
uint8_t type, uint8_t seq, uint8_t last, const void *payload, int len)
{
uint8_t tx_buf[PAYLOAD+3] = { type, seq, last };
assert(len == PAYLOAD);
memcpy(tx_buf+3, payload, len);
rf_send(dsc, tx_buf, sizeof(tx_buf));
}
static void packet(struct atrf_dsc *dsc,
uint8_t type, uint8_t seq, uint8_t last, const void *payload, int len)
{
uint8_t rx_buf[10];
int got;
while (1) {
packet_noack(dsc, type, seq, last, payload, len);
if (verbose)
write(2, ">", 1);
got = rf_recv(dsc, rx_buf, sizeof(rx_buf));
if (got <= 0) {
if (!seq && verbose)
write(2, "\b", 1);
continue;
}
if (verbose)
write(2, "\b?", 2);
if (got < 3)
continue;
if (verbose)
write(2, "\bA", 2);
if (rx_buf[0] != type+1 || rx_buf[1] != seq)
continue;
if (verbose)
write(2, "\b.", 2);
break;
}
}
/* ----- Ping -------------------------------------------------------------- */
static void ping(struct atrf_dsc *dsc)
{
uint8_t ping[] = { PING, 0, 0 };
uint8_t buf[100];
int got, i;
rf_send(dsc, ping, sizeof(ping));
got = rf_recv(dsc, buf, sizeof(buf));
if (!got)
return;
printf("%d: ", got);
for (i = 3; i != got; i++)
printf("%c", buf[i] >= ' ' && buf[i] <= '~' ? buf[i] : '?');
printf("\n");
}
/* ----- Firmware upload --------------------------------------------------- */
static const uint8_t unlock_secret[PAYLOAD] = {
#include "unlock-secret.inc"
};
static void send_firmware(struct atrf_dsc *dsc, void *buf, int len)
{
uint8_t payload[PAYLOAD];
uint8_t last, seq;
if (verbose)
write(2, "firmware ", 9);
last = (len+63)/64;
seq = 0;
packet_noack(dsc, RESET, 0, 0, unlock_secret, PAYLOAD);
packet(dsc, FIRMWARE, seq++, last, unlock_secret, PAYLOAD);
while (len >= PAYLOAD) {
packet(dsc, FIRMWARE, seq++, last, buf, PAYLOAD);
buf += PAYLOAD;
len -= PAYLOAD;
}
if (len) {
memcpy(payload, buf, len);
memset(payload+len, 0, PAYLOAD-len);
packet(dsc, FIRMWARE, seq++, last, payload, PAYLOAD);
}
if (verbose)
write(2, "\n", 1);
}
static void firmware(struct atrf_dsc *dsc, const char *name)
{
FILE *file;
uint8_t fw[12*1024]; /* we have 8-12kB */
ssize_t len;
file = fopen(name, "r");
if (!file) {
perror(name);
exit(1);
}
len = fread(fw, 1, sizeof(fw), file);
if (len < 0) {
perror(name);
exit(1);
}
fclose(file);
send_firmware(dsc, fw, len);
}
/* ----- Image upload ------------------------------------------------------ */
static const uint8_t image_secret[PAYLOAD*2] = {
#include "image-secret.inc"
};
static void send_image(struct atrf_dsc *dsc, void *buf, int len)
{
uint8_t payload[PAYLOAD];
uint8_t last, seq;
hash_init();
hash_merge(image_secret, sizeof(image_secret));
if (verbose)
write(2, "image ", 6);
last = (len+63)/64+3;
seq = 0;
while (len >= PAYLOAD) {
packet(dsc, IMAGE, seq++, last, buf, PAYLOAD);
hash_merge(buf, PAYLOAD);
buf += PAYLOAD;
len -= PAYLOAD;
}
if (len) {
memcpy(payload, buf, len);
memset(payload+len, 0, PAYLOAD-len);
packet(dsc, IMAGE, seq++, last, payload, PAYLOAD);
hash_merge(payload, PAYLOAD);
}
/* @@@ salt */
packet(dsc, IMAGE, seq++, last, payload, PAYLOAD);
hash_merge(payload, PAYLOAD);
packet(dsc, IMAGE, seq++, last, payload, PAYLOAD);
hash_merge(payload, PAYLOAD);
hash_end();
/* hash */
hash_cp(payload, PAYLOAD, 0);
packet(dsc, IMAGE, seq++, last, payload, PAYLOAD);
hash_cp(payload, PAYLOAD, PAYLOAD);
packet(dsc, IMAGE, seq++, last, payload, PAYLOAD);
if (verbose)
write(2, "\n", 1);
}
static void image(struct atrf_dsc *dsc, const char *name)
{
FILE *file;
uint8_t img[200] = { 0 };
ssize_t len;
file = fopen(name, "r");
if (!file) {
perror(name);
exit(1);
}
len = fread(img, 1, sizeof(img), file);
if (len < 0) {
perror(name);
exit(1);
}
fclose(file);
send_image(dsc, img, len);
}
/* ----- Parameter upload -------------------------------------------------- */
static struct params params = {
.xa_high = XA_HIGH_DEFAULT,
.xa_low = XA_LOW_DEFAULT,
.px_fwd_left = PX_FWD_LEFT_DEFAULT,
.px_fwd_right = PX_FWD_RIGHT_DEFAULT,
.px_bwd_left = PX_BWD_LEFT_DEFAULT,
.px_bwd_right = PX_BWD_RIGHT_DEFAULT,
.tp_fwd_start = TP_FWD_START_DEFAULT,
.tp_bwd_start = TP_BWD_START_DEFAULT,
.tp_fwd_pix = TP_FWD_PIX_DEFAULT,
.tp_bwd_pix = TP_BWD_PIX_DEFAULT,
};
static int have_params = 0;
static void add_param(const char *arg, const char *eq)
{
struct map {
const char *name; /* NULL to mark end */
void *p;
int bytes; /* 1, 2, or 4 */
} map[] = {
{ "xa_high", &params.xa_high, 2 },
{ "xa_low", &params.xa_low, 2 },
{ "px_fwd_left", &params.px_fwd_left, 1 },
{ "px_bwd_left", &params.px_bwd_left, 1 },
{ "px_fwd_right", &params.px_fwd_right, 1 },
{ "px_bwd_right", &params.px_bwd_right, 1 },
{ "tp_fwd_start", &params.tp_fwd_start, 4 },
{ "tp_bwd_start", &params.tp_bwd_start, 4 },
{ "tp_fwd_pix", &params.tp_fwd_pix, 2 },
{ "tp_bwd_pix", &params.tp_bwd_pix, 2 },
{ NULL }
};
const struct map *m;
size_t len;
unsigned long v;
char *end;
uint8_t *p;
int i;
len = eq-arg;
for (m = map; m->name; m++) {
if (strlen(m->name) != len)
continue;
if (!strncmp(arg, m->name, len))
break;
}
if (!m->name) {
fprintf(stderr, "unknown parameter \"%.*s\"\n",
(int) len, arg);
exit(1);
}
v = strtoul(eq+1, &end, 10);
if (*end) {
fprintf(stderr, "invalid value \"%s\"\n", eq+1);
exit(1);
}
p = m->p;
for (i = 0; i != m->bytes; i++) {
*p++ = v;
v >>= 8;
}
if (v) {
fprintf(stderr, "value \"%s\" is too large\n", eq+1);
exit(1);
}
have_params = 1;
}
static void param(struct atrf_dsc *dsc)
{
uint8_t payload[PAYLOAD];
if (!have_params)
return;
hash_init();
hash_merge(image_secret, sizeof(image_secret));
if (verbose)
write(2, "param ", 6);
memset(payload, 0, PAYLOAD);
memcpy(payload, &params, sizeof(params));
packet(dsc, PARAM, 0, 4, payload, PAYLOAD);
hash_merge(payload, PAYLOAD);
/* @@@ salt */
packet(dsc, PARAM, 1, 4, payload, PAYLOAD);
hash_merge(payload, PAYLOAD);
packet(dsc, PARAM, 2, 4, payload, PAYLOAD);
hash_merge(payload, PAYLOAD);
hash_end();
/* hash */
hash_cp(payload, PAYLOAD, 0);
packet(dsc, PARAM, 3, 4, payload, PAYLOAD);
hash_cp(payload, PAYLOAD, PAYLOAD);
packet(dsc, PARAM, 4, 4, payload, PAYLOAD);
if (verbose)
write(2, "\n", 1);
}
/* ----- Samples ----------------------------------------------------------- */
static volatile int run = 1;
static void sigint(int sig)
{
run = 0;
}
static int read_sample(uint8_t **s, uint16_t *t_high, uint16_t *t_low,
uint16_t *last)
{
int v;
*t_low = *(*s)++;
*t_low |= *(*s)++ << 8;
if (*t_low < *last)
(*t_high)++;
*last = *t_low;
v = *(*s)++;
v |= *(*s)++ << 8;
return v;
}
static void samples(struct atrf_dsc *dsc, int gui)
{
uint8_t buf[MAX_PSDU] = { 0, };
int got;
uint8_t *s;
uint16_t t_high, t_low, last;
int x, y;
double t;
buf[0] = 1;
packet(dsc, SAMPLE, 0, 0, buf, PAYLOAD);
if (gui)
plot_init();
else
signal(SIGINT, sigint);
while (run) {
got = rf_recv(dsc, buf, sizeof(buf));
if (got <= 3)
continue;
if (buf[0] != SAMPLES)
continue;
if (debug > 1) {
int i;
for (i = 0; i != got; i++)
fprintf(stderr, " %02x", buf[i]);
fprintf(stderr, "\n");
}
if (debug)
fprintf(stderr, "%d:", got);
s = buf+3;
t_high = *s++;
t_high |= *s++ << 8;
last = 0;
while (s < buf+got-2) {
x = read_sample(&s, &t_high, &t_low, &last);
t = (t_high << 16 | t_low)/1000000.0;
if (debug)
fprintf(stderr, "\t%11.6f %d", t, x);
if (!gui)
printf("%11.6f\t%d\t", t, x);
y = read_sample(&s, &t_high, &t_low, &last);
t = (t_high << 16 | t_low)/1000000.0;
if (debug)
fprintf(stderr, "\t%11.6f %d\n", t, y);
if (!gui)
printf("%11.6f\t%d\n", t, y);
if (gui && !plot(x, y))
goto quit;
}
}
quit:
buf[0] = 0;
packet(dsc, SAMPLE, 0, 0, buf, PAYLOAD);
}
/* ----- Command-line processing ------------------------------------------- */
static void usage(const char *name)
{
fprintf(stderr,
"usage: %s [-d] [param=value ...] image_file\n"
"%6s %s [-d] -F firmware_file\n"
"%6s %s [-d] -P\n"
"%6s %s [-d] -S [-S]\n"
, name, "", name, "", name, "", name);
exit(1);
}
int main(int argc, char **argv)
{
const char *fw = NULL;
int do_ping = 0, do_sample = 0;
struct atrf_dsc *dsc;
int c;
while ((c = getopt(argc, argv, "dF:PS")) != EOF)
switch (c) {
case 'd':
debug++;
break;
case 'F':
fw = optarg;
break;
case 'P':
do_ping = 1;
break;
case 'S':
do_sample++;
break;
default:
usage(*argv);
}
if (do_ping+!!do_sample+!!fw > 1)
usage(*argv);
if (do_ping || do_sample || fw) {
if (argc != optind)
usage(*argv);
} else {
if (argc == optind)
usage(*argv);
}
dsc = atrf_open(NULL);
if (!dsc)
return 1;
rf_init(dsc, 8, 15);
if (do_ping)
ping(dsc);
else if (do_sample)
samples(dsc, do_sample > 1);
else if (fw)
firmware(dsc, fw);
else {
const char *eq;
while (optind != argc) {
eq = strchr(argv[optind], '=');
if (!eq)
break;
add_param(argv[optind], eq);
optind++;
}
if (argc > optind+1)
usage(*argv);
if (optind != argc)
image(dsc, argv[optind]);
param(dsc);
}
return 0;
}