razzor
/
multiboot_tool
1
0
Fork 0
Code Issues 1 Pull Requests 0 Releases 0 Wiki Activity
Browse Source

change indent style..

master
Olaf Rempel 3 years ago
parent
84b8ddd1f5
commit
5ab1e36fc7
8 changed files with 1359 additions and 499 deletions
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. +13
    -4
      chipinfo_avr.c
  2. +221
    -77
      filedata.c
  3. +334
    -108
      funk.c
  4. +314
    -111
      mpm.c
  5. +146
    -73
      multiboot.c
  6. +4
    -2
      multiboot.h
  7. +68
    -22
      optarg.c
  8. +259
    -102
      twi.c

+ 13
- 4
chipinfo_avr.c View File

@@ -27,26 +27,35 @@

#define ARRAY_SIZE(x) (sizeof(x) / sizeof(*x))

struct chipinfo {
struct chipinfo
{
uint8_t sig[3];
const char name[16];
};

static struct chipinfo chips[] = {
static struct chipinfo chips[] =
{
{ { 0x1E, 0x93, 0x07 }, "AVR Mega 8" },
{ { 0x1E, 0x93, 0x0A }, "AVR Mega 88" },
{ { 0x1E, 0x94, 0x06 }, "AVR Mega 168" },
{ { 0x1E, 0x95, 0x02 }, "AVR Mega 32" },
};

/* *************************************************************************
* chipinfo_get_avr_name
* ************************************************************************* */
const char * chipinfo_get_avr_name(const uint8_t *sig)
{
int i;
for (i = 0; i < ARRAY_SIZE(chips); i++) {

for (i = 0; i < ARRAY_SIZE(chips); i++)
{
struct chipinfo *chip = &chips[i];
if (chip->sig[0] == sig[0] && chip->sig[1] == sig[1] && chip->sig[2] == sig[2])
{
return chip->name;
}
}

return "unknown";
}
} /* chipinfo_get_avr_name */

+ 221
- 77
filedata.c View File

@@ -31,10 +31,14 @@
#define FILETYPE_BINARY 1
#define FILETYPE_INTELHEX 2

/* *************************************************************************
* dbuf_alloc
* ************************************************************************* */
struct databuf * dbuf_alloc(uint32_t size)
{
struct databuf *dbuf = malloc(sizeof(struct databuf) + size);
if (dbuf == NULL) {
if (dbuf == NULL)
{
perror("dbuf_alloc");
return NULL;
}
@@ -43,71 +47,108 @@ struct databuf * dbuf_alloc(uint32_t size)
dbuf->size = size;
dbuf->length = 0;
return dbuf;
}
} /* dbuf_alloc */


/* *************************************************************************
* dbuf_free
* ************************************************************************* */
void dbuf_free(struct databuf *dbuf)
{
free(dbuf);
}
} /* dbuf_free */


/* *************************************************************************
* dbuf_dump
* ************************************************************************* */
static void dbuf_dump(struct databuf *dbuf)
{
int pos = 0, oldskip = 0;

while (pos < dbuf->length) {
while (pos < dbuf->length)
{
char buf[128];
int j, i = 0;

int skip = 1;
for (j = 0; j < 16; j++) {
for (j = 0; j < 16; j++)
{
if (pos + j < dbuf->length)
{
i += sprintf(buf + i, "%02X", dbuf->data[pos + j]);
else
} else {
i += sprintf(buf + i, " ");
}

if (j % 2)
{
buf[i++] = ' ';
}
}

for (j = 0; j < 16; j++) {
if (pos + j < dbuf->length) {
for (j = 0; j < 16; j++)
{
if (pos + j < dbuf->length)
{
unsigned char val = dbuf->data[pos + j];

if (val >= 0x20 && val < 0x7F)
{
buf[i++] = val;
else
} else {
buf[i++] = '.';
}

if (val != 0xFF)
{
skip = 0;
} else {
}
}
else
{
buf[i++] = ' ';
}
}

if (pos == 0 || (pos + 16) >= dbuf->length || skip == 0) {
if ((pos == 0) ||
((pos + 16) >= dbuf->length) ||
(skip == 0)
)
{
buf[i++] = '\0';
printf("%04X: %s\r\n", pos, buf);
oldskip = 0;

} else if (skip == 1 && oldskip == 0) {
}
else if ((skip == 1) &&
(oldskip == 0)
)
{
printf("****\n");
oldskip = 1;
}

pos += 16;
}
}
} /* dbuf_dump */


/* *************************************************************************
* binfile_getsize
* ************************************************************************* */
static int binfile_getsize(const char *filename, uint32_t *size)
{
int fd = open(filename, O_RDONLY);
if (fd < 0) {
if (fd < 0)
{
perror("binfile_getsize(): open()");
return -1;
}

struct stat filestat;
if (fstat(fd, &filestat) < 0) {
if (fstat(fd, &filestat) < 0)
{
perror("binfile_getsize(): fstat()");
close(fd);
return -1;
@@ -117,18 +158,24 @@ static int binfile_getsize(const char *filename, uint32_t *size)

close(fd);
return 0;
}
} /* binfile_getsize */


/* *************************************************************************
* binfile_read
* ************************************************************************* */
static int binfile_read(const char *filename, struct databuf *dbuf)
{
int fd = open(filename, O_RDONLY);
if (fd < 0) {
if (fd < 0)
{
perror("binfile_read(): open()");
return -1;
}

ssize_t readsize = read(fd, dbuf->data, dbuf->size);
if (readsize <= 0) {
if (readsize <= 0)
{
perror("binfile_read(): read()");
close(fd);
return -1;
@@ -138,18 +185,24 @@ static int binfile_read(const char *filename, struct databuf *dbuf)

close(fd);
return 0;
}
} /* binfile_read */


/* *************************************************************************
* binfile_write
* ************************************************************************* */
static int binfile_write(const char *filename, struct databuf *dbuf)
{
int fd = open(filename, O_RDWR | O_CREAT | O_TRUNC, 0644);
if (fd < 0) {
if (fd < 0)
{
perror("binfile_write(): open()");
return -1;
}

ssize_t writesize = write(fd, dbuf->data, dbuf->length);
if (writesize != dbuf->length) {
if (writesize != dbuf->length)
{
perror("binfile_write(): write()");
close(fd);
return -1;
@@ -157,9 +210,11 @@ static int binfile_write(const char *filename, struct databuf *dbuf)

close(fd);
return 0;
}
} /* binfile_write */

struct ihex_record {

struct ihex_record
{
uint8_t byte_count;
uint16_t address;
uint8_t type;
@@ -168,46 +223,61 @@ struct ihex_record {
uint8_t chksum;
};


/* *************************************************************************
* hex2byte
* ************************************************************************* */
static uint8_t hex2byte(const char *ptr)
{
int i;
uint8_t result = 0;

for (i = 0; i < 2; i++) {
for (i = 0; i < 2; i++)
{
result <<= 4;
result |= (ptr[i] >= '0' && ptr[i] <= '9') ? (ptr[i] - '0') :
(((ptr[i] & 0xDF) >= 'A' && (ptr[i] & 0xDF) <= 'F') ? (ptr[i] - 'A' + 0x0A) :
0x00);
result |= (ptr[i] >= '0' && ptr[i] <= '9') ? (ptr[i] - '0')
: (((ptr[i] & 0xDF) >= 'A' && (ptr[i] & 0xDF) <= 'F') ? (ptr[i] - 'A' + 0x0A)
: 0x00);
}

return result;
}
} /* hex2byte */


/* *************************************************************************
* hexfile_getrecord
* ************************************************************************* */
static int hexfile_getrecord(FILE *stream, struct ihex_record *record)
{
char *hexline = NULL;
size_t size;

ssize_t length = getline(&hexline, &size, stream);
if (length == -1) {
if (!feof(stream)) {
if (length == -1)
{
if (!feof(stream))
{
perror("hexfile_getrecord(): getline()");
}

return -1;
}

if (length < 12) {
fprintf(stderr, "record too short (%d)\n", length);
if (length < 12)
{
fprintf(stderr, "record too short (%ld)\n", length);
free(hexline);
return -1;
}

int pos = 0;
if (hexline[pos] != ':') {
if (hexline[pos] != ':')
{
fprintf(stderr, "invalid startcode\n");
free(hexline);
return -1;
}

pos++;

uint8_t chksum = 0x00;
@@ -216,9 +286,11 @@ static int hexfile_getrecord(FILE *stream, struct ihex_record *record)
chksum += record->byte_count;
pos += 2;

if (record->byte_count > 0) {
if (record->byte_count > 0)
{
record->data = malloc(record->byte_count);
if (record->data == NULL) {
if (record->data == NULL)
{
perror("hexfile_getrecord(): malloc()");
free(hexline);
return -1;
@@ -236,7 +308,8 @@ static int hexfile_getrecord(FILE *stream, struct ihex_record *record)
pos += 2;

int i;
for (i = 0; i < record->byte_count; i++) {
for (i = 0; i < record->byte_count; i++)
{
record->data[i] = hex2byte(&hexline[pos]);
chksum += record->data[i];
pos += 2;
@@ -246,18 +319,26 @@ static int hexfile_getrecord(FILE *stream, struct ihex_record *record)
chksum += record->chksum;
pos += 2;

if (chksum != 0x00) {
if (chksum != 0x00)
{
fprintf(stderr, "invalid checksum (0x%02X)\n", chksum);
if (record->byte_count > 0)
{
free(record->data);
}

free(hexline);
return -1;
}

free(hexline);
return 0;
}
} /* hexfile_getrecord */


/* *************************************************************************
* hexfile_putrecord
* ************************************************************************* */
static int hexfile_putrecord(FILE *stream, struct ihex_record *record)
{
uint8_t chksum = record->byte_count;
@@ -269,39 +350,62 @@ static int hexfile_putrecord(FILE *stream, struct ihex_record *record)
char buf[64];

buf[0] = '\0';
for (i = 0; i < record->byte_count; i++) {
for (i = 0; i < record->byte_count; i++)
{
len += snprintf(buf + len, sizeof(buf) - len, "%02X", record->data[i]);
chksum += record->data[i];
}

fprintf(stream, ":%02X%04X%02X%s%02X\n", record->byte_count, record->address, record->type, buf, (uint8_t)(0x100 - chksum));
fprintf(stream, ":%02X%04X%02X%s%02X\n",
record->byte_count,
record->address,
record->type,
buf,
(uint8_t)(0x100 - chksum));

return -1;
}
} /* hexfile_putrecord */


/* *************************************************************************
* hexfile_getsize
* ************************************************************************* */
static int hexfile_getsize(const char *filename, uint32_t *size)
{
*size = 0x10000;
return 0;
}
} /* hexfile_getsize */


/* *************************************************************************
* hexfile_read
* ************************************************************************* */
static int hexfile_read(const char *filename, struct databuf *dbuf)
{
FILE *stream = fopen(filename, "r");
if (stream == NULL) {
if (stream == NULL)
{
perror("hexfile_read(): fopen()");
return -1;
}

while (1) {
while (1)
{
struct ihex_record record;
memset(&record, 0x00, sizeof(struct ihex_record));

int result = hexfile_getrecord(stream, &record);
if (result == -1)
{
break;
}

if (record.type == 0x00) {
if (record.address > dbuf->size || record.address + record.byte_count > dbuf->size) {
if (record.type == 0x00)
{
if ((record.address > dbuf->size) ||
(record.address + record.byte_count > dbuf->size)
)
{
fprintf(stderr, "hexfile_read(): data out of bounds\n");
break;
}
@@ -313,12 +417,17 @@ static int hexfile_read(const char *filename, struct databuf *dbuf)

fclose(stream);
return 0;
}
} /* hexfile_read */


/* *************************************************************************
* hexfile_write
* ************************************************************************* */
static int hexfile_write(const char *filename, struct databuf *dbuf)
{
FILE *stream = fopen(filename, "w");
if (stream == NULL) {
if (stream == NULL)
{
perror("hexfile_write(): fopen()");
return -1;
}
@@ -326,21 +435,30 @@ static int hexfile_write(const char *filename, struct databuf *dbuf)
int i;
int addr_min = dbuf->length;
int addr_max = 0;
for (i = 0; i < dbuf->length; i++) {
for (i = 0; i < dbuf->length; i++)
{
if (dbuf->data[i] == 0xFF)
{
continue;
}

if (addr_min > i)
{
addr_min = i;
}

if (addr_max < i)
{
addr_max = i;
}
}

addr_min = addr_min & ~0x0F;
addr_max = (addr_max + 0x0F) & ~0x0F;

struct ihex_record record;
for (i = addr_min; i < addr_max; i += 0x10) {
for (i = addr_min; i < addr_max; i += 0x10)
{
record.byte_count = 0x10;
record.address = i;
record.type = 0x00;
@@ -357,67 +475,93 @@ static int hexfile_write(const char *filename, struct databuf *dbuf)

fclose(stream);
return 0;
}
} /* hexfile_write */


/* *************************************************************************
* get_filetype
* ************************************************************************* */
static int get_filetype(const char *filename)
{
const char *ext = filename + (strlen(filename) -4);

if (ext < filename)
{
return FILETYPE_UNKNOWN;
}

if (strncmp(ext, ".bin", 4) == 0)
{
return FILETYPE_BINARY;
}

if (strncmp(ext, ".hex", 4) == 0)
{
return FILETYPE_INTELHEX;
}

return FILETYPE_UNKNOWN;
}
} /* get_filetype */


/* *************************************************************************
* file_getsize
* ************************************************************************* */
int file_getsize(const char *filename, uint32_t *size)
{
switch (get_filetype(filename)) {
case FILETYPE_BINARY:
return binfile_getsize(filename, size);
switch (get_filetype(filename))
{
case FILETYPE_BINARY:
return binfile_getsize(filename, size);

case FILETYPE_INTELHEX:
return hexfile_getsize(filename, size);
case FILETYPE_INTELHEX:
return hexfile_getsize(filename, size);

default:
return -1;
default:
return -1;
}
}
} /* file_getsize */


/* *************************************************************************
* file_read
* ************************************************************************* */
int file_read(const char *filename, struct databuf *dbuf)
{
switch (get_filetype(filename)) {
case FILETYPE_BINARY:
return binfile_read(filename, dbuf);
switch (get_filetype(filename))
{
case FILETYPE_BINARY:
return binfile_read(filename, dbuf);

case FILETYPE_INTELHEX:
return hexfile_read(filename, dbuf);
case FILETYPE_INTELHEX:
return hexfile_read(filename, dbuf);

default:
return -1;
default:
return -1;
}
}
} /* file_read */


/* *************************************************************************
* file_write
* ************************************************************************* */
int file_write(const char *filename, struct databuf *dbuf)
{
if (strncmp(filename, "-", 1) == 0) {
if (strncmp(filename, "-", 1) == 0)
{
dbuf_dump(dbuf);
return 0;
}

switch (get_filetype(filename)) {
case FILETYPE_BINARY:
return binfile_write(filename, dbuf);
switch (get_filetype(filename))
{
case FILETYPE_BINARY:
return binfile_write(filename, dbuf);

case FILETYPE_INTELHEX:
return hexfile_write(filename, dbuf);
case FILETYPE_INTELHEX:
return hexfile_write(filename, dbuf);

default:
return -1;
default:
return -1;
}
}
} /* file_write */

+ 334
- 108
funk.c View File

@@ -124,7 +124,8 @@ struct rfm12_pkt

struct multiboot_ops funk_ops;

struct funk_privdata {
struct funk_privdata
{
char *device;
int fd;
int connected;
@@ -141,30 +142,35 @@ struct funk_privdata {
};


static struct option funk_optargs[] = {
{"address", 1, 0, 'a'}, /* -a <addr> */
{"device", 1, 0, 'd'}, /* [ -d <device> ] */
static struct option funk_optargs[] =
{
{ "address", 1, 0, 'a'}, /* -a <addr> */
{ "device", 1, 0, 'd'}, /* [ -d <device> ] */
};


/* *************************************************************************
* funk_optarg_cb
* ************************************************************************* */
static int funk_optarg_cb(int val, const char *arg, void *privdata)
{
struct funk_privdata *funk = (struct funk_privdata *)privdata;

switch (val) {
case 'a': /* address */
{
char *endptr;
funk->address = strtol(arg, &endptr, 16);
if (*endptr != '\0' || funk->address < 0x00 || funk->address > 0xFF) {
fprintf(stderr, "invalid address: '%s'\n", arg);
return -1;
switch (val)
{
case 'a': /* address */
{
char *endptr;

funk->address = strtol(arg, &endptr, 16);
if (*endptr != '\0' || funk->address < 0x00 || funk->address > 0xFF) {
fprintf(stderr, "invalid address: '%s'\n", arg);
return -1;
}
}
}
break;
break;

case 'd': /* device */
{
case 'd': /* device */
if (funk->device != NULL) {
fprintf(stderr, "invalid device: '%s'\n", optarg);
return -1;
@@ -175,11 +181,10 @@ static int funk_optarg_cb(int val, const char *arg, void *privdata)
perror("strdup()");
return -1;
}
}
break;
break;

case 'h':
case '?': /* error */
case 'h':
case '?': /* error */
fprintf(stderr, "Usage: funkboot [options]\n"
" -a <address> - selects rfm12 address (0x00 - 0xFF)\n"
" -d <device> - selects funkbridge device\n"
@@ -192,25 +197,31 @@ static int funk_optarg_cb(int val, const char *arg, void *privdata)
"\n");
return -1;

default:
return 1;
default:
return 1;
}

return 0;
} /* funk_optarg_cb */


/* *************************************************************************
* funk_alloc
* ************************************************************************* */
static struct multiboot * funk_alloc(void)
{
struct multiboot * mboot = malloc(sizeof(struct multiboot));
if (mboot == NULL)
{
return NULL;
}

memset(mboot, 0x00, sizeof(struct multiboot));
mboot->ops = &funk_ops;

struct funk_privdata *funk = malloc(sizeof(struct funk_privdata));
if (funk == NULL) {
if (funk == NULL)
{
free(mboot);
return NULL;
}
@@ -226,55 +237,78 @@ static struct multiboot * funk_alloc(void)
} /* funk_alloc */


/* *************************************************************************
* funk_free
* ************************************************************************* */
static void funk_free(struct multiboot *mboot)
{
struct funk_privdata *funk = (struct funk_privdata *)mboot->privdata;

if (funk->device != NULL)
{
free(funk->device);
}

free(funk);
free(mboot);
} /* funk_free */


static int funk_get_memtype(struct multiboot *mboot, const char *memname)
/* *************************************************************************
* optarg_copy
* ************************************************************************* */
static int funk_get_memtype(struct multiboot *mboot,
const char *memname)
{
if (strcmp(memname, "flash") == 0)
{
return MEMTYPE_FLASH;

}
else if (strcmp(memname, "eeprom") == 0)
{
return MEMTYPE_EEPROM;
}

return -1;
} /* funk_get_memtype */


static int funk_get_memsize(struct multiboot *mboot, int memtype)
/* *************************************************************************
* optarg_copy
* ************************************************************************* */
static int funk_get_memsize(struct multiboot *mboot,
int memtype)
{
struct funk_privdata *funk = (struct funk_privdata *)mboot->privdata;

if (!funk->connected)
{
return 0;
}

switch (memtype) {
case MEMTYPE_FLASH:
return funk->flashsize;
switch (memtype)
{
case MEMTYPE_FLASH:
return funk->flashsize;

case MEMTYPE_EEPROM:
return funk->eepromsize;
case MEMTYPE_EEPROM:
return funk->eepromsize;

default:
return 0;
default:
return 0;
}
} /* funk_get_memsize */


/* *************************************************************************
* funk_serial_read
* ************************************************************************* */
static int funk_serial_read(int fd, void *data, int size)
{
int pos = 0;

while (1) {
while (1)
{
fd_set fdset;
struct timeval timeout = { .tv_sec = 1, .tv_usec = 0 };

@@ -282,21 +316,29 @@ static int funk_serial_read(int fd, void *data, int size)
FD_SET(fd, &fdset);

int ret = select(fd +1, &fdset, NULL, NULL, &timeout);
if (ret == -1) {
if (ret == -1)
{
perror("select");
return -1;

} else if (ret == 0) {
}
else if (ret == 0)
{
break;

} else if (FD_ISSET(fd, &fdset)) {
}
else if (FD_ISSET(fd, &fdset))
{
int len = read(fd, data + pos, size - pos);
if (len < 0) {
if (len < 0)
{
return -1;

} else {
}
else
{
pos += len;
if (pos == size) {
if (pos == size)
{
break;
}
}
@@ -308,31 +350,48 @@ static int funk_serial_read(int fd, void *data, int size)


#if (FUNK_BRIDGE_DEBUG == 1) || (FUNK_PACKET_DEBUG == 1)
/* *************************************************************************
* funk_print_data
* ************************************************************************* */
static char * funk_print_data(uint8_t *data, uint16_t length)
{
int pos = 0, i = 0, j;
char *buf = malloc(length * 4 + 64);

while (pos < length) {
while (pos < length)
{
i += sprintf(buf + i, "%04X: ", pos);
for (j = 0; j < 16; j++) {
for (j = 0; j < 16; j++)
{
if (pos + j < length)
{
i += sprintf(buf + i, "%02X", data[pos + j]);
}
else
{
i += sprintf(buf + i, " ");
if (j % 2)
{
buf[i++] = ' ';
}
}

if (j % 2)
buf[i++] = ' ';
}

for (j = 0; j < 16; j++) {
if (pos + j < length) {
for (j = 0; j < 16; j++)
{
if (pos + j < length)
{
unsigned char val = data[pos + j];
if (val >= 0x20 && val < 0x80)
{
buf[i++] = val;
}
else
{
buf[i++] = '.';
} else {
}
}
else
{
buf[i++] = ' ';
}
}
@@ -348,16 +407,27 @@ static char * funk_print_data(uint8_t *data, uint16_t length)
#endif


static int funk_bridge_send(struct funk_privdata *funk, uint8_t *header, uint8_t headerlength, uint8_t *data, uint8_t datalength)
/* *************************************************************************
* funk_bridge_send
* ************************************************************************* */
static int funk_bridge_send(struct funk_privdata *funk,
uint8_t *header,
uint8_t headerlength,
uint8_t *data,
uint8_t datalength)
{
if (headerlength > 0) {
if (write(funk->fd, header, headerlength) != headerlength) {
if (headerlength > 0)
{
if (write(funk->fd, header, headerlength) != headerlength)
{
return -1;
}
}

if (datalength > 0) {
if (write(funk->fd, data, datalength) != datalength) {
if (datalength > 0)
{
if (write(funk->fd, data, datalength) != datalength)
{
return -1;
}
}
@@ -372,7 +442,14 @@ static int funk_bridge_send(struct funk_privdata *funk, uint8_t *header, uint8_t
} /* funk_bridge_send */


static int funk_bridge_recv(struct funk_privdata *funk, uint8_t command, uint8_t *cause, uint8_t *buffer, int buffersize)
/* *************************************************************************
* funk_bridge_recv
* ************************************************************************* */
static int funk_bridge_recv(struct funk_privdata *funk,
uint8_t command,
uint8_t *cause,
uint8_t *buffer,
int buffersize)
{
uint8_t response[3];
int len;
@@ -384,45 +461,63 @@ static int funk_bridge_recv(struct funk_privdata *funk, uint8_t command, uint8_t
return -1;
}

if (response[0] != command) {
fprintf(stderr, "invalid command response (0x%02x != 0x%02x)\n", response[0], command);
if (response[0] != command)
{
fprintf(stderr, "invalid command response (0x%02x != 0x%02x)\n",
response[0], command);

return -1;
}

*cause = response[1];
uint16_t length = response[2];
uint16_t bufferpos = 0;
while (length > 0) {

/* free space in output buffer? */
if ((bufferpos < buffersize) && (buffer != NULL)) {
while (length > 0)
{

/* free space in output buffer? */
if ((bufferpos < buffersize) && (buffer != NULL))
{
uint16_t size = MIN(buffersize - bufferpos, length);

len = funk_serial_read(funk->fd, buffer + bufferpos, size);
if (len <= 0) {
fprintf(stderr, "short read() from device (%d != %d)\n", len, size);
if (len <= 0)
{
fprintf(stderr, "short read() from device (%d != %d)\n",
len, size);

return -1;
}

bufferpos += len;
length -= len;

} else {
}
else
{
uint8_t dummy[256];

/* no space in output buffer, but device still sends data -> do dummy read */
uint16_t size = MIN(sizeof(dummy), length);

len = funk_serial_read(funk->fd, dummy, size);
if (len <= 0) {
fprintf(stderr, "short read() from device (%d != %d)\n", len, size);
if (len <= 0)
{
fprintf(stderr, "short read() from device (%d != %d)\n",
len, size);

return -1;
}

length -= len;
}
}

#if (FUNK_BRIDGE_DEBUG == 1)
char *dump = funk_print_data(buffer, bufferpos);
printf("funk_bridge_recv() cmd=0x%02x cause=0x%02x length=0x%02x\n%s\n", command, *cause, length, dump);
printf("funk_bridge_recv() cmd=0x%02x cause=0x%02x length=0x%02x\n%s\n",
command, *cause, length, dump);

free(dump);
#endif

@@ -430,22 +525,33 @@ static int funk_bridge_recv(struct funk_privdata *funk, uint8_t command, uint8_t
} /* funk_bridge_recv */


static int funk_send_packet(struct funk_privdata *funk, struct rfm12_pkt *pkt, int length)
/* *************************************************************************
* funk_send_packet
* ************************************************************************* */
static int funk_send_packet(struct funk_privdata *funk,
struct rfm12_pkt *pkt,
int length)
{
uint8_t request[] = { BRIDGE_CMD_TRANSMIT, length };

int ret = funk_bridge_send(funk, request, sizeof(request), (uint8_t *)pkt, length);
if (ret < 0)
{
return ret;
}

uint8_t cause = BRIDGE_CAUSE_SUCCESS;
ret = funk_bridge_recv(funk, request[0], &cause, NULL, 0);
if (ret != 0)
{
return -1;
}

#if (FUNK_PACKET_DEBUG == 1)
char *dump = funk_print_data((uint8_t *)pkt, length);
printf("funk_send_packet() cause=0x%02x length=0x%02x\n%s\n", cause, length, dump);
printf("funk_send_packet() cause=0x%02x length=0x%02x\n%s\n",
cause, length, dump);

free(dump);
#endif

@@ -453,18 +559,27 @@ static int funk_send_packet(struct funk_privdata *funk, struct rfm12_pkt *pkt, i
} /* funk_send_packet */


static int funk_recv_packet(struct funk_privdata *funk, struct rfm12_pkt *pkt, int *length)
/* *************************************************************************
* funk_recv_packet
* ************************************************************************* */
static int funk_recv_packet(struct funk_privdata *funk,
struct rfm12_pkt *pkt,
int *length)
{
uint8_t request[] = { BRIDGE_CMD_RECEIVE, 0 };

int ret = funk_bridge_send(funk, request, sizeof(request), NULL, 0);
if (ret < 0)
{
return ret;
}

uint8_t cause = BRIDGE_CAUSE_SUCCESS;
ret = funk_bridge_recv(funk, request[0], &cause, (uint8_t *)pkt, *length);
if (ret < 0)
{
return -1;
}

*length = ret;

@@ -478,18 +593,27 @@ static int funk_recv_packet(struct funk_privdata *funk, struct rfm12_pkt *pkt, i
} /* funk_recv_packet */


static int funk_bridge_version(struct funk_privdata *funk, uint8_t *version, int size)
/* *************************************************************************
* funk_bridge_version
* ************************************************************************* */
static int funk_bridge_version(struct funk_privdata *funk,
uint8_t *version,
int size)
{
uint8_t request[] = { BRIDGE_CMD_VERSION, 0 };

int ret = funk_bridge_send(funk, request, sizeof(request), NULL, 0);
if (ret < 0)
{
return ret;
}

uint8_t cause = BRIDGE_CAUSE_SUCCESS;
ret = funk_bridge_recv(funk, request[0], &cause, version, size);
if (ret < 0)
{
return ret;
}

version[ret] = '\0';

@@ -497,6 +621,9 @@ static int funk_bridge_version(struct funk_privdata *funk, uint8_t *version, int
} /* funk_bridge_version */


/* *************************************************************************
* funk_close_device
* ************************************************************************* */
static void funk_close_device(struct funk_privdata *funk)
{
/* delay close() / tcsetattr() */
@@ -507,15 +634,20 @@ static void funk_close_device(struct funk_privdata *funk)
} /* funk_close_device */


/* *************************************************************************
* funk_open_device
* ************************************************************************* */
static int funk_open_device(struct funk_privdata *funk)
{
funk->fd = open(funk->device, O_RDWR | O_NOCTTY | O_CLOEXEC);
if (funk->fd < 0) {
if (funk->fd < 0)
{
perror("open()");
return -1;
}

if (tcgetattr(funk->fd, &funk->oldtio) < 0) {
if (tcgetattr(funk->fd, &funk->oldtio) < 0)
{
perror("tcgetattr(oldtio)");
close(funk->fd);
return -1;
@@ -531,7 +663,8 @@ static int funk_open_device(struct funk_privdata *funk)
newtio.c_cc[VTIME] = 0;

int err = tcsetattr(funk->fd, TCSANOW, &newtio);
if (err < 0) {
if (err < 0)
{
perror("tcsetattr(newtio)");
close(funk->fd);
return -1;
@@ -542,7 +675,11 @@ static int funk_open_device(struct funk_privdata *funk)
} /* funk_open_device */


static int funk_switch_application(struct funk_privdata *funk, uint8_t application)
/* *************************************************************************
* funk_switch_application
* ************************************************************************* */
static int funk_switch_application(struct funk_privdata *funk,
uint8_t application)
{
struct rfm12_pkt packet;

@@ -556,14 +693,16 @@ static int funk_switch_application(struct funk_privdata *funk, uint8_t applicati
packet.msg.p.switchapp.app = application;

int ret = funk_send_packet(funk, &packet, 4 + packet.data_length);
if (ret < 0) {
if (ret < 0)
{
fprintf(stderr, "funk_switch_application(): funk_send_packet()\n");
return ret;
}

int response_size = sizeof(packet);
ret = funk_recv_packet(funk, &packet, &response_size);
if (ret < 0) {
if (ret < 0)
{
fprintf(stderr, "funk_switch_application(): funk_recv_packet()\n");
return ret;
}
@@ -579,7 +718,12 @@ static int funk_switch_application(struct funk_privdata *funk, uint8_t applicati
} /* funk_switch_application */


static int funk_read_version(struct funk_privdata *funk, uint8_t *version, uint16_t length)
/* *************************************************************************
* funk_read_version
* ************************************************************************* */
static int funk_read_version(struct funk_privdata *funk,
uint8_t *version,
uint16_t length)
{
struct rfm12_pkt packet;

@@ -592,14 +736,16 @@ static int funk_read_version(struct funk_privdata *funk, uint8_t *version, uint1
packet.msg.cause = CAUSE_SUCCESS;

int ret = funk_send_packet(funk, &packet, 4 + packet.data_length);
if (ret < 0) {
if (ret < 0)
{
fprintf(stderr, "funk_read_version(): funk_send_packet()\n");
return ret;
}

int response_size = sizeof(packet);
ret = funk_recv_packet(funk, &packet, &response_size);
if (ret < 0) {
if (ret < 0)
{
fprintf(stderr, "funk_read_version(): funk_recv_packet()\n");
return ret;
}
@@ -613,7 +759,9 @@ static int funk_read_version(struct funk_privdata *funk, uint8_t *version, uint1

int i;
for (i = 0; i < packet.data_length -3; i++)
{
version[i] = packet.msg.p.version.data[i] & 0x7F;
}

version[i] = '\0';

@@ -621,7 +769,12 @@ static int funk_read_version(struct funk_privdata *funk, uint8_t *version, uint1
} /* funk_read_version */


static int funk_read_chipinfo(struct funk_privdata *funk, uint8_t *chipinfo, uint16_t length)
/* *************************************************************************
* funk_read_chipinfo
* ************************************************************************* */
static int funk_read_chipinfo(struct funk_privdata *funk,
uint8_t *chipinfo,
uint16_t length)
{
struct rfm12_pkt packet;

@@ -634,7 +787,8 @@ static int funk_read_chipinfo(struct funk_privdata *funk, uint8_t *chipinfo, uin
packet.msg.cause = CAUSE_SUCCESS;

int ret = funk_send_packet(funk, &packet, 4 + packet.data_length);
if (ret < 0) {
if (ret < 0)
{
fprintf(stderr, "funk_read_chipinfo(): funk_send_packet()\n");
return ret;
}
@@ -659,7 +813,14 @@ static int funk_read_chipinfo(struct funk_privdata *funk, uint8_t *chipinfo, uin
} /* funk_read_chipinfo */


static int funk_read_memory(struct funk_privdata *funk, uint8_t *buffer, uint16_t size, uint8_t memtype, uint16_t address)
/* *************************************************************************
* funk_read_memory
* ************************************************************************* */
static int funk_read_memory(struct funk_privdata *funk,
uint8_t *buffer,
uint16_t size,
uint8_t memtype,
uint16_t address)
{
struct rfm12_pkt packet;

@@ -700,7 +861,14 @@ static int funk_read_memory(struct funk_privdata *funk, uint8_t *buffer, uint16_
} /* funk_read_memory */


static int __funk_write_memory(struct funk_privdata *funk, uint8_t *buffer, uint16_t size, uint8_t memtype, uint16_t address)
/* *************************************************************************
* __funk_write_memory
* ************************************************************************* */
static int __funk_write_memory(struct funk_privdata *funk,
uint8_t *buffer,
uint16_t size,
uint8_t memtype,
uint16_t address)
{
struct rfm12_pkt packet;

@@ -718,14 +886,16 @@ static int __funk_write_memory(struct funk_privdata *funk, uint8_t *buffer, uint
memcpy(packet.msg.p.write_req.data, buffer, size);

int ret = funk_send_packet(funk, &packet, 4 + packet.data_length);
if (ret < 0) {
if (ret < 0)
{
fprintf(stderr, "funk_write_memory(): funk_send_packet()\n");
return ret;
}

int response_size = sizeof(packet);
ret = funk_recv_packet(funk, &packet, &response_size);
if (ret < 0) {
if (ret < 0)
{
fprintf(stderr, "funk_write_memory(): funk_recv_packet()\n");
return ret;
}
@@ -741,7 +911,14 @@ static int __funk_write_memory(struct funk_privdata *funk, uint8_t *buffer, uint
} /* __funk_write_memory */


static int funk_write_memory(struct funk_privdata *funk, uint8_t *buffer, uint16_t size, uint8_t memtype, uint16_t address)
/* *************************************************************************
* funk_write_memory
* ************************************************************************* */
static int funk_write_memory(struct funk_privdata *funk,
uint8_t *buffer,
uint16_t size,
uint8_t memtype,
uint16_t address)
{
if (memtype == MEMTYPE_EEPROM)
{
@@ -769,7 +946,9 @@ static int funk_write_memory(struct funk_privdata *funk, uint8_t *buffer, uint16
{
ret = __funk_write_memory(funk, &pagebuf[pos], WRITE_BLOCK_SIZE, memtype, address + pos);
if (ret < 0)
{
break;
}
}

free(pagebuf);
@@ -777,39 +956,52 @@ static int funk_write_memory(struct funk_privdata *funk, uint8_t *buffer, uint16
} /* funk_write_memory */


/* *************************************************************************
* funk_close
* ************************************************************************* */
static int funk_close(struct multiboot *mboot)
{
struct funk_privdata *funk = (struct funk_privdata *)mboot->privdata;

if (funk->connected)
{
funk_switch_application(funk, BOOTTYPE_APPLICATION);
}

funk_close_device(funk);
return 0;
} /* funk_close */


/* *************************************************************************
* funk_open
* ************************************************************************* */
static int funk_open(struct multiboot *mboot)
{
struct funk_privdata *funk = (struct funk_privdata *)mboot->privdata;

if (funk->address == 0) {
if (funk->address == 0)
{
fprintf(stderr, "abort: no address given\n");
return -1;
}

if (funk->device == NULL) {
if (funk->device == NULL)
{
fprintf(stderr, "abort: no device given\n");
return -1;
}

if (funk_open_device(funk) < 0)
{
return -1;
}

printf("funkbridge dev : %-16s\n", funk->device);

char bridge_version[20];
if (funk_bridge_version(funk, (uint8_t *)bridge_version, sizeof(bridge_version))) {
if (funk_bridge_version(funk, (uint8_t *)bridge_version, sizeof(bridge_version)))
{
fprintf(stderr, "failed to get funkbridge version\n");
funk_close(mboot);
return -1;
@@ -817,7 +1009,8 @@ static int funk_open(struct multiboot *mboot)

printf("funkbridge ver : %-16s\n", bridge_version);

if (funk_switch_application(funk, BOOTTYPE_BOOTLOADER)) {
if (funk_switch_application(funk, BOOTTYPE_BOOTLOADER))
{
fprintf(stderr, "failed to switch to bootloader (invalid address?)\n");
funk_close(mboot);
return -1;
@@ -829,14 +1022,16 @@ static int funk_open(struct multiboot *mboot)
usleep(100000);

char version[20];
if (funk_read_version(funk, (uint8_t *)version, sizeof(version))) {
if (funk_read_version(funk, (uint8_t *)version, sizeof(version)))
{
fprintf(stderr, "failed to get bootloader version\n");
funk_close(mboot);
return -1;
}

uint8_t chipinfo[8];
if (funk_read_chipinfo(funk, chipinfo, sizeof(chipinfo))) {
if (funk_read_chipinfo(funk, chipinfo, sizeof(chipinfo)))
{
fprintf(stderr, "failed to get bootloader chipinfo\n");
funk_close(mboot);
return -1;
@@ -848,26 +1043,39 @@ static int funk_open(struct multiboot *mboot)
funk->flashsize = (chipinfo[4] << 8) + chipinfo[5];
funk->eepromsize = (chipinfo[6] << 8) + chipinfo[7];

printf("version : %-16s (sig: 0x%02x 0x%02x 0x%02x => %s)\n", version, chipinfo[0], chipinfo[1], chipinfo[2], chipname);
printf("flash size : 0x%04x / %5d (0x%02x bytes/page)\n", funk->flashsize, funk->flashsize, funk->flashpage);
printf("eeprom size : 0x%04x / %5d\n", funk->eepromsize, funk->eepromsize);
printf("version : %-16s (sig: 0x%02x 0x%02x 0x%02x => %s)\n",
version, chipinfo[0], chipinfo[1], chipinfo[2], chipname);

printf("flash size : 0x%04x / %5d (0x%02x bytes/page)\n",
funk->flashsize, funk->flashsize, funk->flashpage);

printf("eeprom size : 0x%04x / %5d\n",
funk->eepromsize, funk->eepromsize);

return 0;
} /* funk_open */


static int funk_read(struct multiboot *mboot, struct databuf *dbuf, int memtype)
/* *************************************************************************
* funk_read
* ************************************************************************* */
static int funk_read(struct multiboot *mboot,
struct databuf *dbuf,
int memtype)
{
struct funk_privdata *funk = (struct funk_privdata *)mboot->privdata;
char *progress_msg = (memtype == MEMTYPE_FLASH) ? "reading flash" : "reading eeprom";

int pos = 0;
int size = (memtype == MEMTYPE_FLASH) ? funk->flashsize : funk->eepromsize;
while (pos < size) {

while (pos < size)
{
mboot->progress_cb(progress_msg, pos, size);

int len = MIN(READ_BLOCK_SIZE, size - pos);
if (funk_read_memory(funk, dbuf->data + pos, len, memtype, pos)) {
if (funk_read_memory(funk, dbuf->data + pos, len, memtype, pos))
{
mboot->progress_cb(progress_msg, -1, -1);
return -1;
}
@@ -882,19 +1090,27 @@ static int funk_read(struct multiboot *mboot, struct databuf *dbuf, int memtype)
} /* funk_read */


static int funk_write(struct multiboot *mboot, struct databuf *dbuf, int memtype)
/* *************************************************************************
* funk_write
* ************************************************************************* */
static int funk_write(struct multiboot *mboot,
struct databuf *dbuf,
int memtype)
{
struct funk_privdata *funk = (struct funk_privdata *)mboot->privdata;
char *progress_msg = (memtype == MEMTYPE_FLASH) ? "writing flash" : "writing eeprom";

int pos = 0;
while (pos < dbuf->length) {
while (pos < dbuf->length)
{
mboot->progress_cb(progress_msg, pos, dbuf->length);

int len = (memtype == MEMTYPE_FLASH) ? funk->flashpage : WRITE_BLOCK_SIZE;

len = MIN(len, dbuf->length - pos);
if (funk_write_memory(funk, dbuf->data + pos, len, memtype, pos)) {

if (funk_write_memory(funk, dbuf->data + pos, len, memtype, pos))
{
mboot->progress_cb(progress_msg, -1, -1);
return -1;
}
@@ -907,23 +1123,32 @@ static int funk_write(struct multiboot *mboot, struct databuf *dbuf, int memtype
} /* funk_write */


static int funk_verify(struct multiboot *mboot, struct databuf *dbuf, int memtype)
/* *************************************************************************
* funk_verify
* ************************************************************************* */
static int funk_verify(struct multiboot *mboot,
struct databuf *dbuf,
int memtype)
{
struct funk_privdata *funk = (struct funk_privdata *)mboot->privdata;
char *progress_msg = (memtype == MEMTYPE_FLASH) ? "verifing flash" : "verifing eeprom";

int pos = 0;
uint8_t comp[READ_BLOCK_SIZE];
while (pos < dbuf->length) {

while (pos < dbuf->length)
{
mboot->progress_cb(progress_msg, pos, dbuf->length);

int len = MIN(READ_BLOCK_SIZE, dbuf->length - pos);
if (funk_read_memory(funk, comp, len, memtype, pos)) {
if (funk_read_memory(funk, comp, len, memtype, pos))
{
mboot->progress_cb(progress_msg, -1, -1);
return -1;
}

if (memcmp(comp, dbuf->data + pos, len) != 0x00) {
if (memcmp(comp, dbuf->data + pos, len) != 0x00)
{
mboot->progress_cb(progress_msg, -1, -1);
fprintf(stderr, "verify failed at page 0x%04x!!\n", pos);
return -1;
@@ -939,7 +1164,8 @@ static int funk_verify(struct multiboot *mboot, struct databuf *dbuf, int memtyp
} /* funk_verify */


struct multiboot_ops funk_ops = {
struct multiboot_ops funk_ops =
{
.alloc = funk_alloc,
.free = funk_free,
.get_memtype = funk_get_memtype,


+ 314
- 111
mpm.c View File

@@ -40,7 +40,8 @@

struct multiboot_ops mpm_ops;

struct mpm_privdata {
struct mpm_privdata
{
char *device;
int fd;
int connected;
@@ -54,44 +55,52 @@ struct mpm_privdata {
struct termios oldtio;
};

static struct option mpm_optargs[] = {
{"address", 1, 0, 'a'}, /* -a <addr> */
{"device", 1, 0, 'd'}, /* [ -d <device> ] */
static struct option mpm_optargs[] =
{
{ "address", 1, 0, 'a'}, /* -a <addr> */
{ "device", 1, 0, 'd'}, /* [ -d <device> ] */
};


/* *************************************************************************
* mpm_optarg_cb
* ************************************************************************* */
static int mpm_optarg_cb(int val, const char *arg, void *privdata)
{
struct mpm_privdata *mpm = (struct mpm_privdata *)privdata;

switch (val) {
case 'a': /* address */
{
char *endptr;
mpm->address = strtol(arg, &endptr, 16);
if (*endptr != '\0' || mpm->address < 0x01 || mpm->address > 0x7F) {
fprintf(stderr, "invalid address: '%s'\n", arg);
return -1;
switch (val)
{
case 'a': /* address */
{
char *endptr;
mpm->address = strtol(arg, &endptr, 16);

if (*endptr != '\0' || mpm->address < 0x01 || mpm->address > 0x7F)
{
fprintf(stderr, "invalid address: '%s'\n", arg);
return -1;
}
}
}
break;
break;

case 'd': /* device */
{
if (mpm->device != NULL) {
case 'd': /* device */
if (mpm->device != NULL)
{
fprintf(stderr, "invalid device: '%s'\n", optarg);
return -1;
}

mpm->device = strdup(optarg);
if (mpm->device == NULL) {
if (mpm->device == NULL)
{
perror("strdup()");
return -1;
}
}
break;
break;

case 'h':
case '?': /* error */
case 'h':
case '?': /* error */
fprintf(stderr, "Usage: mpmboot [options]\n"
" -a <address> - selects mpm address (0x01 - 0xFF)\n"
" -d <device> - selects mpm device\n"
@@ -102,26 +111,34 @@ static int mpm_optarg_cb(int val, const char *arg, void *privdata)
"\n"
"Example: mpmboot -d /dev/ttyUSB0 -a 0x22 -w flash:blmc.hex -w flash:blmc_eeprom.hex\n"
"\n");

return -1;

default:
return 1;
default:
return 1;
}

return 0;
}
} /* mpm_optarg_cb */


/* *************************************************************************
* mpm_alloc
* ************************************************************************* */
static struct multiboot * mpm_alloc(void)
{
struct multiboot * mboot = malloc(sizeof(struct multiboot));
if (mboot == NULL)
{
return NULL;
}

memset(mboot, 0x00, sizeof(struct multiboot));
mboot->ops = &mpm_ops;

struct mpm_privdata *mpm = malloc(sizeof(struct mpm_privdata));
if (mpm == NULL) {
if (mpm == NULL)
{
free(mboot);
return NULL;
}
@@ -130,65 +147,94 @@ static struct multiboot * mpm_alloc(void)
mpm->device = NULL;
mpm->address = 0;

optarg_register(mpm_optargs, ARRAY_SIZE(mpm_optargs), mpm_optarg_cb, (void *)mpm);
optarg_register(mpm_optargs, ARRAY_SIZE(mpm_optargs),
mpm_optarg_cb, (void *)mpm);

mboot->privdata = mpm;
return mboot;
}
} /* mpm_alloc */


/* *************************************************************************
* mpm_free
* ************************************************************************* */
static void mpm_free(struct multiboot *mboot)
{
struct mpm_privdata *mpm = (struct mpm_privdata *)mboot->privdata;

if (mpm->device != NULL)
{
free(mpm->device);
}

free(mpm);
free(mboot);
}
} /* mpm_free */


static int mpm_get_memtype(struct multiboot *mboot, const char *memname)
/* *************************************************************************
* mpm_get_memtype
* ************************************************************************* */
static int mpm_get_memtype(struct multiboot *mboot,
const char *memname)
{
if (strcmp(memname, "flash") == 0)
{
return MEMTYPE_FLASH;

}
else if (strcmp(memname, "eeprom") == 0)
{
return MEMTYPE_EEPROM;
}

return -1;
}
} /* mpm_get_memtype */


/* *************************************************************************
* mpm_get_memsize
* ************************************************************************* */
static int mpm_get_memsize(struct multiboot *mboot, int memtype)
{
struct mpm_privdata *mpm = (struct mpm_privdata *)mboot->privdata;

if (!mpm->connected)
{
return 0;
}

switch (memtype) {
case MEMTYPE_FLASH:
return mpm->flashsize;
switch (memtype)
{
case MEMTYPE_FLASH:
return mpm->flashsize;

case MEMTYPE_EEPROM:
return mpm->eepromsize;
case MEMTYPE_EEPROM:
return mpm->eepromsize;

default:
return 0;
default:
return 0;
}
}
} /* mpm_get_memsize */

static int mpm_send(struct mpm_privdata *mpm, uint8_t command, uint8_t *data, int length)

/* *************************************************************************
* mpm_send
* ************************************************************************* */
static int mpm_send(struct mpm_privdata *mpm, uint8_t command,
uint8_t *data, int length)
{
struct termios tio;

if (tcgetattr(mpm->fd, &tio) < 0) {
if (tcgetattr(mpm->fd, &tio) < 0)
{
perror("tcgetattr(tio)");
return -1;
}

tio.c_cflag |= PARODD;

if (tcsetattr(mpm->fd, TCSAFLUSH, &tio) < 0) {
if (tcsetattr(mpm->fd, TCSAFLUSH, &tio) < 0)
{
perror("tcsetattr(tio)");
return -1;
}
@@ -196,7 +242,8 @@ static int mpm_send(struct mpm_privdata *mpm, uint8_t command, uint8_t *data, in
// usleep(5000);

uint8_t address = mpm->address;
if (write(mpm->fd, &address, sizeof(address)) != sizeof(address)) {
if (write(mpm->fd, &address, sizeof(address)) != sizeof(address))
{
perror("write(address)");
return -1;
}
@@ -205,7 +252,8 @@ static int mpm_send(struct mpm_privdata *mpm, uint8_t command, uint8_t *data, in

tio.c_cflag &= ~(PARODD);

if (tcsetattr(mpm->fd, TCSAFLUSH, &tio) < 0) {
if (tcsetattr(mpm->fd, TCSAFLUSH, &tio) < 0)
{
perror("tcsetattr(tio)");
return -1;
}
@@ -215,26 +263,34 @@ static int mpm_send(struct mpm_privdata *mpm, uint8_t command, uint8_t *data, in
header[1] = (length >> 8) & 0xFF;
header[2] = length & 0xFF;

if (write(mpm->fd, header, sizeof(header)) != sizeof(header)) {
if (write(mpm->fd, header, sizeof(header)) != sizeof(header))
{
perror("write(header)");
return -1;
}

if (data != NULL && length != 0) {
if (write(mpm->fd, data, length) != length) {
if (data != NULL && length != 0)
{
if (write(mpm->fd, data, length) != length)
{
perror("write(data)");
return -1;
}
}

return 0;
}
} /* mpm_send */


/* *************************************************************************
* myread
* ************************************************************************* */
static int myread(int fd, void *data, int size)
{
int pos = 0;

while (1) {
while (1)
{
fd_set fdset;
struct timeval timeout = { .tv_sec = 1, .tv_usec = 0 };

@@ -242,21 +298,28 @@ static int myread(int fd, void *data, int size)
FD_SET(fd, &fdset);

int ret = select(fd +1, &fdset, NULL, NULL, &timeout);
if (ret == -1) {
if (ret == -1)
{
perror("select");
return -1;

} else if (ret == 0) {
}
else if (ret == 0)
{
break;

} else if (FD_ISSET(fd, &fdset)) {
}
else if (FD_ISSET(fd, &fdset))
{
int len = read(fd, data + pos, size - pos);
if (len < 0) {
if (len < 0)
{
return -1;

} else {
}
else
{
pos += len;
if (pos == size) {
if (pos == size)
{
break;
}
}
@@ -264,20 +327,30 @@ static int myread(int fd, void *data, int size)
}

return pos;
}
} /* myread */

static int mpm_recv(struct mpm_privdata *mpm, uint8_t command, uint8_t *cause, uint8_t *buffer, uint16_t buffersize)

/* *************************************************************************
* mpm_recv
* ************************************************************************* */
static int mpm_recv(struct mpm_privdata *mpm,
uint8_t command,
uint8_t *cause,
uint8_t *buffer,
uint16_t buffersize)
{
int len;
uint8_t header[4];

len = myread(mpm->fd, header, sizeof(header));
if (len != sizeof(header)) {
if (len != sizeof(header))
{
fprintf(stderr, "short read() from device (not addressed?)\n");
return -1;
}

if (header[0] != command) {
if (header[0] != command)
{
fprintf(stderr, "invalid command response (0x%02x != 0x%02x)\n", header[0], command);
return -1;
}
@@ -288,37 +361,51 @@ static int mpm_recv(struct mpm_privdata *mpm, uint8_t command, uint8_t *cause, u
// printf("mpm_recv() cmd=0x%02x cause=0x%02x length=0x%04x\n", command, *cause, length);

uint16_t bufferpos = 0;
while (length > 0) {
while (length > 0)
{

/* free space in output buffer? */
if ((bufferpos < buffersize) && (buffer != NULL)) {
if ((bufferpos < buffersize) && (buffer != NULL))
{

uint16_t size = MIN(buffersize - bufferpos, length);

len = myread(mpm->fd, buffer + bufferpos, size);
if (len <= 0) {
if (len <= 0)
{
fprintf(stderr, "short read() from device (%d != %d)\n", len, size);
return -1;
}

bufferpos += len;
length -= len;

} else {
}
else
{
uint8_t dummy[256];

/* no space in output buffer, but device still sends data -> do dummy read */
uint16_t size = MIN(sizeof(dummy), length);

len = myread(mpm->fd, dummy, size);
if (len <= 0) {
if (len <= 0)
{
fprintf(stderr, "short read() from device (%d != %d)\n", len, size);
return -1;
}

length -= len;
}
}

return bufferpos;
}
} /* mpm_recv */


/* *************************************************************************
* mpm_close_device
* ************************************************************************* */
static void mpm_close_device(struct mpm_privdata *mpm)
{
/* delay close() / tcsetattr() */
@@ -326,17 +413,23 @@ static void mpm_close_device(struct mpm_privdata *mpm)

tcsetattr(mpm->fd, TCSANOW, &mpm->oldtio);
close(mpm->fd);
}
} /* mpm_close_device */


/* *************************************************************************
* mpm_open_device
* ************************************************************************* */
static int mpm_open_device(struct mpm_privdata *mpm)
{
mpm->fd = open(mpm->device, O_RDWR | O_NOCTTY | O_CLOEXEC);
if (mpm->fd < 0) {
if (mpm->fd < 0)
{
perror("open()");
return -1;
}

if (tcgetattr(mpm->fd, &mpm->oldtio) < 0) {
if (tcgetattr(mpm->fd, &mpm->oldtio) < 0)
{
perror("tcgetattr(oldtio)");
close(mpm->fd);
return -1;
@@ -352,7 +445,8 @@ static int mpm_open_device(struct mpm_privdata *mpm)
newtio.c_cc[VTIME] = 0;

int err = tcsetattr(mpm->fd, TCSANOW, &newtio);
if (err < 0) {
if (err < 0)
{
perror("tcsetattr(newtio)");
close(mpm->fd);
return -1;
@@ -360,58 +454,94 @@ static int mpm_open_device(struct mpm_privdata *mpm)

mpm->connected = 1;
return 0;
}
} /* mpm_open_device */

static int mpm_switch_application(struct mpm_privdata *mpm, uint8_t application)

/* *************************************************************************
* mpm_switch_application
* ************************************************************************* */
static int mpm_switch_application(struct mpm_privdata *mpm,
uint8_t application)
{
uint8_t data[] = { application };

int ret = mpm_send(mpm, CMD_SWITCH_APPLICATION, data, sizeof(data));
if (ret < 0)
{
return ret;