qi-hardware/ben-blinkenlights
1
0
Fork 0
mirror of git://projects.qi-hardware.com/ben-blinkenlights.git synced 2024-11-27 15:41:53 +02:00
Code

lpc111x-isp/lpc111x.c: add printf-style dialog() variants

Browse Source
This commit is contained in:
Werner Almesberger 2012-12-28 23:26:13 -03:00
parent c4d64fafa4
commit 0553fed424
1 changed files with 41 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

58
lpc111x-isp/lpc111x.c
View File

@ -11,6 +11,8 @@
*/
#define _GNU_SOURCE /* for vasprintf */
#include <stdarg.h>
#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
@ -79,29 +81,29 @@ static void trace_in(const uint8_t *s, int len)
/* ----- Dialog functions -------------------------------------------------- */
static int dialog(const char *cmd, void *buf, int buf_len, int idle)
static int dialog_fixed(void *buf, int buf_len, int idle, const char *msg)
{
int cmd_len = strlen(cmd);
char *tx_buf = alloca(cmd_len+3);
uint8_t *rx_buf = alloca(cmd_len+2+buf_len);
int msg_len = strlen(msg);
char *tx_buf = alloca(msg_len+3);
uint8_t *rx_buf = alloca(msg_len+2+buf_len);
int got;
memcpy(tx_buf, cmd, cmd_len);
memcpy(tx_buf+cmd_len, "\r\n", 2);
memcpy(tx_buf, msg, msg_len);
memcpy(tx_buf+msg_len, "\r\n", 2);
if (TRACE_PROGRESS)
trace_out(tx_buf, cmd_len+2);
got = swuart_trx(tx_buf, cmd_len+2, rx_buf, cmd_len+2+buf_len,
trace_out(tx_buf, msg_len+2);
got = swuart_trx(tx_buf, msg_len+2, rx_buf, msg_len+2+buf_len,
idle, idle);
if (got < cmd_len+2) {
if (got < msg_len+2) {
if (TRACE_FATAL)
trace_in(rx_buf, got);
fprintf(stderr, "response too short for echo\n");
exit(1);
}
if (memcmp(rx_buf, cmd, cmd_len) ||
rx_buf[cmd_len] != '\r' || rx_buf[cmd_len+1] != '\n') {
if (memcmp(rx_buf, msg, msg_len) ||
rx_buf[msg_len] != '\r' || rx_buf[msg_len+1] != '\n') {
if (TRACE_FATAL)
trace_in(rx_buf, got);
fprintf(stderr, "echo mismatch\n");
@ -109,15 +111,37 @@ static int dialog(const char *cmd, void *buf, int buf_len, int idle)
}
if (TRACE_ECHO)
trace_in(rx_buf, cmd_len+2);
trace_in(rx_buf, msg_len+2);
got -= cmd_len+2;
memcpy(buf, rx_buf+cmd_len+2, got);
got -= msg_len+2;
memcpy(buf, rx_buf+msg_len+2, got);
return got;
}
static int vdialog(void *buf, int buf_len, int idle,
const char *cmd, va_list ap)
{
char *msg;
vasprintf(&msg, cmd, ap);
return dialog_fixed(buf, buf_len, idle, msg);
}
static int dialog(void *buf, int buf_len, int idle, const char *cmd, ...)
{
va_list ap;
int res;
va_start(ap, cmd);
res = vdialog(buf, buf_len, idle, cmd, ap);
va_end(ap);
return res;
}
static int autobaud(void)
{
uint8_t reply[100];
@ -140,7 +164,7 @@ static int autobaud(void)
if (TRACE_PROGRESS)
trace_in(reply, got);
got = dialog(SYNC, reply, sizeof(reply), 100);
got = dialog(reply, sizeof(reply), 100, SYNC);
if (got == 4 && !memcmp(reply, "OK\r\n", 4)) {
if (TRACE_PROGRESS)
@ -191,7 +215,7 @@ static void start_isp(void)
exit(1);
}
got = dialog("12000", reply, sizeof(reply), 100);
got = dialog(reply, sizeof(reply), 100, "12000");
if (got != 4 || memcmp(reply, "OK\r\n", 4)) {
if (TRACE_FATAL)
trace_in(reply, got);
@ -239,7 +263,7 @@ int main(int argc, char **argv)
start_isp();
got = dialog("J", reply, sizeof(reply), 100);
got = dialog(reply, sizeof(reply), 100, "J");
trace_in(reply, got);
return 0;