uint64_t read_uint64(const uint8_t ** buffer) {
return (((uint64_t) read_uint32(buffer)) << 32) |
((uint64_t) read_uint32(buffer));
}
/* Open dmg image */
int dmg_open(int fd, bdev_desc_t *bdev)
{
u32 count;
u32 max_compressed_size=1,max_sectors_per_chunk=1,i;
bdev->priv = malloc(sizeof(BDRVDMGState));
if(bdev->priv == NULL)
goto fail;
BDRVDMGState *s = DMG_PRIV(bdev);
CLEAR(*s);
off_t info_begin,info_end,last_in_offset,last_out_offset;
/* Init the bdev struct */
bdev->fd = fd;
bdev->read = &wrap_read;
bdev->real_read = &dmg_read;
bdev->write = NULL;
bdev->seek = &dmg_seek;
bdev->close = &dmg_close;
s->fd = fd;
/* RO */
bdev->flags &= ~BF_ENABLE_WRITE;
s->n_chunks = 0;
s->offsets = s->lengths = s->sectors = s->sectorcounts = 0;
/* read offset of info blocks */
if(lseek(s->fd,-0x1d8,SEEK_END)<0)
goto fail;
info_begin=read_off(s->fd);
if(info_begin==0)
goto fail;
if(lseek(s->fd,info_begin,SEEK_SET)<0)
goto fail;
if(read_uint32(s->fd)!=0x100)
goto fail;
if((count = read_uint32(s->fd))==0)
goto fail;
info_end = info_begin+count;
if(lseek(s->fd,0xf8,SEEK_CUR)<0)
goto fail;
/* read offsets */
last_in_offset = last_out_offset = 0;
while(lseek(s->fd,0,SEEK_CUR)<info_end) {
u32 type;
count = read_uint32(s->fd);
if(count==0)
goto fail;
type = read_uint32(s->fd);
if(type!=0x6d697368 || count<244)
lseek(s->fd,count-4,SEEK_CUR);
else {
int new_size, chunk_count;
if(lseek(s->fd,200,SEEK_CUR)<0)
goto fail;
chunk_count = (count-204)/40;
new_size = sizeof(u64) * (s->n_chunks + chunk_count);
s->types = realloc(s->types, new_size/2);
s->offsets = realloc(s->offsets, new_size);
s->lengths = realloc(s->lengths, new_size);
s->sectors = realloc(s->sectors, new_size);
s->sectorcounts = realloc(s->sectorcounts, new_size);
for(i=s->n_chunks;i<s->n_chunks+chunk_count;i++) {
s->types[i] = read_uint32(s->fd);
if(s->types[i]!=0x80000005 && s->types[i]!=1 && s->types[i]!=2) {
if(s->types[i]==0xffffffff) {
last_in_offset = s->offsets[i-1]+s->lengths[i-1];
last_out_offset = s->sectors[i-1]+s->sectorcounts[i-1];
}
chunk_count--;
i--;
if(lseek(s->fd,36,SEEK_CUR)<0)
goto fail;
continue;
}
read_uint32(s->fd);
s->sectors[i] = last_out_offset+read_off(s->fd);
s->sectorcounts[i] = read_off(s->fd);
s->offsets[i] = last_in_offset+read_off(s->fd);
s->lengths[i] = read_off(s->fd);
if(s->lengths[i]>max_compressed_size)
max_compressed_size = s->lengths[i];
if(s->sectorcounts[i]>max_sectors_per_chunk)
max_sectors_per_chunk = s->sectorcounts[i];
}
s->n_chunks+=chunk_count;
}
}
bdev->size = s->n_chunks * 512;
/* initialize zlib engine */
if(!(s->compressed_chunk=(char*)malloc(max_compressed_size+1)))
goto fail;
if(!(s->uncompressed_chunk=(char*)malloc(512*max_sectors_per_chunk)))
goto fail;
if(inflateInit(&s->zstream) != Z_OK)
goto fail;
s->current_chunk = s->n_chunks;
return 0;
fail:
return -1;
}
/* Reads a property of type 'prop_type' from 'data', and puts the
* result 'out'. Returns 0 upon success, a negative value otherwise */
static int read_ply_property(struct file_data *data, const int prop_type,
const int swap_bytes, void *out)
{
int c, rcode=0;
/* Just local pointers to copy the result from the 'read_[type]'
* functions into... */
t_int8 *tmp_int8;
t_uint8 *tmp_uint8;
t_int16 *tmp_int16;
t_uint16 *tmp_uint16;
t_int32 *tmp_int32;
t_uint32 *tmp_uint32;
float *tmp_float32;
double *tmp_float64;
switch(prop_type) {
case uint8:
c = getc(data);
if (c == EOF)
rcode = EOF;
else {
tmp_uint8 = out;
*tmp_uint8 = (t_uint8)c;
}
break;
case int8:
c = getc(data);
if (c == EOF)
rcode = EOF;
else {
tmp_int8= out;
*tmp_int8 = (t_int8)c;
}
break;
case uint16:
tmp_uint16 = out;
rcode = read_uint16(data, swap_bytes, tmp_uint16);
break;
case int16:
tmp_int16 = out;
rcode = read_int16(data, swap_bytes, tmp_int16);
break;
case uint32:
tmp_uint32 = out;
rcode = read_uint32(data, swap_bytes, tmp_uint32);
break;
case int32:
tmp_int32 = out;
rcode = read_int32(data, swap_bytes, tmp_int32);
break;
case float32:
tmp_float32 = out;
rcode = read_float32(data, swap_bytes, tmp_float32);
break;
case float64:
tmp_float64 = out;
rcode = read_float64(data, swap_bytes, tmp_float64);
break;
default:
rcode = MESH_CORRUPTED;
break;
}
return rcode;
}
static bmp_result bmp_analyse_header(bmp_image *bmp, uint8_t *data) {
uint32_t header_size;
uint32_t i;
uint8_t j;
int32_t width, height;
uint8_t palette_size;
unsigned int flags = 0;
/* a variety of different bitmap headers can follow, depending
* on the BMP variant. A full description of the various headers
* can be found at
* http://msdn.microsoft.com/en-us/library/ms532301(VS.85).aspx
*/
header_size = read_uint32(data, 0);
if (bmp->buffer_size < (14 + header_size))
return BMP_INSUFFICIENT_DATA;
if (header_size == 12) {
/* the following header is for os/2 and windows 2.x and consists of:
*
* +0 UINT32 size of this header (in bytes)
* +4 INT16 image width (in pixels)
* +6 INT16 image height (in pixels)
* +8 UINT16 number of colour planes (always 1)
* +10 UINT16 number of bits per pixel
*/
width = read_int16(data, 4);
height = read_int16(data, 6);
if ((width <= 0) || (height == 0))
return BMP_DATA_ERROR;
if (height < 0) {
bmp->reversed = true;
height = -height;
}
/* ICOs only support 256*256 resolutions
* In the case of the ICO header, the height is actually the added
* height of XOR-Bitmap and AND-Bitmap (double the visible height)
* Technically we could remove this check and ICOs with bitmaps
* of any size could be processed; this is to conform to the spec.
*/
if (bmp->ico) {
if ((width > 256) || (height > 512)) {
return BMP_DATA_ERROR;
} else {
bmp->width = width;
bmp->height = height / 2;
}
} else {
bmp->width = width;
bmp->height = height;
}
if (read_uint16(data, 8) != 1)
return BMP_DATA_ERROR;
bmp->bpp = read_uint16(data, 10);
/**
* The bpp value should be in the range 1-32, but the only
* values considered legal are:
* RGB ENCODING: 1, 4, 8, 16, 24 and 32
*/
if ((bmp->bpp != 1) && (bmp->bpp != 4) &&
(bmp->bpp != 8) &&
(bmp->bpp != 16) &&
(bmp->bpp != 24) &&
(bmp->bpp != 32))
return BMP_DATA_ERROR;
bmp->colours = (1 << bmp->bpp);
palette_size = 3;
} else if (header_size < 40) {
return BMP_DATA_ERROR;
} else {
/* the following header is for windows 3.x and onwards. it is a
* minimum of 40 bytes and (as of Windows 95) a maximum of 108 bytes.
*
* +0 UINT32 size of this header (in bytes)
* +4 INT32 image width (in pixels)
* +8 INT32 image height (in pixels)
* +12 UINT16 number of colour planes (always 1)
* +14 UINT16 number of bits per pixel
* +16 UINT32 compression methods used
* +20 UINT32 size of bitmap (in bytes)
* +24 UINT32 horizontal resolution (in pixels per meter)
* +28 UINT32 vertical resolution (in pixels per meter)
* +32 UINT32 number of colours in the image
* +36 UINT32 number of important colours
* +40 UINT32 mask identifying bits of red component
* +44 UINT32 mask identifying bits of green component
* +48 UINT32 mask identifying bits of blue component
* +52 UINT32 mask identifying bits of alpha component
* +56 UINT32 color space type
* +60 UINT32 x coordinate of red endpoint
* +64 UINT32 y coordinate of red endpoint
* +68 UINT32 z coordinate of red endpoint
* +72 UINT32 x coordinate of green endpoint
* +76 UINT32 y coordinate of green endpoint
* +80 UINT32 z coordinate of green endpoint
* +84 UINT32 x coordinate of blue endpoint
* +88 UINT32 y coordinate of blue endpoint
* +92 UINT32 z coordinate of blue endpoint
* +96 UINT32 gamma red coordinate scale value
* +100 UINT32 gamma green coordinate scale value
* +104 UINT32 gamma blue coordinate scale value
*/
width = read_int32(data, 4);
height = read_int32(data, 8);
if ((width <= 0) || (height == 0))
return BMP_DATA_ERROR;
if (height < 0) {
bmp->reversed = true;
height = -height;
}
/* ICOs only support 256*256 resolutions
* In the case of the ICO header, the height is actually the added
* height of XOR-Bitmap and AND-Bitmap (double the visible height)
* Technically we could remove this check and ICOs with bitmaps
* of any size could be processed; this is to conform to the spec.
*/
if (bmp->ico) {
if ((width > 256) || (height > 512)) {
return BMP_DATA_ERROR;
} else {
bmp->width = width;
bmp->height = height / 2;
}
} else {
bmp->width = width;
bmp->height = height;
}
if (read_uint16(data, 12) != 1)
return BMP_DATA_ERROR;
bmp->bpp = read_uint16(data, 14);
if (bmp->bpp == 0)
bmp->bpp = 8;
bmp->encoding = read_uint32(data, 16);
/**
* The bpp value should be in the range 1-32, but the only
* values considered legal are:
* RGB ENCODING: 1, 4, 8, 16, 24 and 32
* RLE4 ENCODING: 4
* RLE8 ENCODING: 8
* BITFIELD ENCODING: 16 and 32
*/
switch (bmp->encoding) {
case BMP_ENCODING_RGB:
if ((bmp->bpp != 1) && (bmp->bpp != 4) &&
(bmp->bpp != 8) &&
(bmp->bpp != 16) &&
(bmp->bpp != 24) &&
(bmp->bpp != 32))
return BMP_DATA_ERROR;
break;
case BMP_ENCODING_RLE8:
if (bmp->bpp != 8)
return BMP_DATA_ERROR;
break;
case BMP_ENCODING_RLE4:
if (bmp->bpp != 4)
return BMP_DATA_ERROR;
break;
case BMP_ENCODING_BITFIELDS:
if ((bmp->bpp != 16) && (bmp->bpp != 32))
return BMP_DATA_ERROR;
break;
/* invalid encoding */
default:
return BMP_DATA_ERROR;
break;
}
/* Bitfield encoding means we have red, green, blue, and alpha masks.
* Here we aquire the masks and determine the required bit shift to
* align them in our 24-bit color 8-bit alpha format.
*/
if (bmp->encoding == BMP_ENCODING_BITFIELDS) {
if (header_size == 40) {
header_size += 12;
if (bmp->buffer_size < (14 + header_size))
return BMP_INSUFFICIENT_DATA;
for (i = 0; i < 3; i++)
bmp->mask[i] = read_uint32(data, 40 + (i << 2));
} else {
for (i = 0; i < 4; i++)
bmp->mask[i] = read_uint32(data, 40 + (i << 2));
}
for (i = 0; i < 4; i++) {
if (bmp->mask[i] == 0)
break;
for (j = 31; j > 0; j--)
if (bmp->mask[i] & (1 << j)) {
if ((j - 7) > 0)
bmp->mask[i] &= 0xff << (j - 7);
else
bmp->mask[i] &= 0xff >> (-(j - 7));
bmp->shift[i] = (i << 3) - (j - 7);
break;
}
}
}
bmp->colours = read_uint32(data, 32);
if (bmp->colours == 0)
bmp->colours = (1 << bmp->bpp);
palette_size = 4;
}
data += header_size;
/* if there's no alpha mask, flag the bmp opaque */
if ((!bmp->ico) && (bmp->mask[3] == 0)) {
flags |= BMP_OPAQUE;
bmp->opaque = true;
}
/* we only have a palette for <16bpp */
if (bmp->bpp < 16) {
/* we now have a series of palette entries of the format:
*
* +0 BYTE blue
* +1 BYTE green
* +2 BYTE red
*
* if the palette is from an OS/2 or Win2.x file then the entries
* are padded with an extra byte.
*/
/* boundary checking */
if (bmp->buffer_size < (14 + header_size + ((uint64_t)4 * bmp->colours)))
return BMP_INSUFFICIENT_DATA;
/* create the colour table */
bmp->colour_table = (uint32_t *)malloc(bmp->colours * 4);
if (!bmp->colour_table)
return BMP_INSUFFICIENT_MEMORY;
for (i = 0; i < bmp->colours; i++) {
bmp->colour_table[i] = data[2] | (data[1] << 8) | (data[0] << 16);
if (bmp->opaque)
bmp->colour_table[i] |= (0xff << 24);
data += palette_size;
bmp->colour_table[i] = read_uint32((uint8_t *)&bmp->colour_table[i],0);
}
}
/* create our bitmap */
flags |= BMP_NEW | BMP_CLEAR_MEMORY;
bmp->bitmap = bmp->bitmap_callbacks.bitmap_create(bmp->width, bmp->height, flags);
if (!bmp->bitmap) {
if (bmp->colour_table)
free(bmp->colour_table);
bmp->colour_table = NULL;
return BMP_INSUFFICIENT_MEMORY;
}
/* BMPs within ICOs don't have BMP file headers, so the image data should
* always be right after the colour table.
*/
if (bmp->ico)
bmp->bitmap_offset = (intptr_t)data - (intptr_t)bmp->bmp_data;
return BMP_OK;
}
/* state->src and state->src_size contain the freshly read bytes
we must accumulate any partial state between calls.
*/
int slip_decode(struct slip_decode_state *state)
{
switch(state->encapsulator) {
case SLIP_FORMAT_SLIP:
{
/*
Examine received bytes for end of packet marker.
The challenge is that we need to make sure that the packet encapsulation
is self-synchronising in the event that a data error occurs (including
failure to receive an arbitrary number of bytes).
*/
while(state->src_offset < state->src_size){
// clear the valid bit flag if we hit the end of the destination buffer
if (state->dst_offset >= sizeof state->dst)
state->state&=~DC_VALID;
if (state->state&DC_ESC){
// clear escape bit
state->state&=~DC_ESC;
switch(state->src[state->src_offset]) {
case SLIP_ESC_END: // escaped END byte
state->dst[state->dst_offset++]=SLIP_END;
break;
case SLIP_ESC_ESC: // escaped escape character
state->dst[state->dst_offset++]=SLIP_ESC;
break;
case SLIP_ESC_0a:
state->dst[state->dst_offset++]=SLIP_0a;
break;
case SLIP_ESC_0d:
state->dst[state->dst_offset++]=SLIP_0d;
break;
case SLIP_ESC_0f:
state->dst[state->dst_offset++]=SLIP_0f;
break;
case SLIP_ESC_1b:
state->dst[state->dst_offset++]=SLIP_1b;
break;
default: /* Unknown escape character. This is an error. */
if (config.debug.packetradio)
WARNF("Packet radio stream contained illegal escaped byte 0x%02x -- resetting parser.",state->src[state->src_offset]);
state->dst_offset=0;
// skip everything until the next SLIP_END
state->state=0;
}
}else{
// non-escape character
switch(state->src[state->src_offset]) {
case SLIP_ESC:
// set escape bit
state->state|=DC_ESC;
break;
case SLIP_END:
if (state->dst_offset>4){
uint32_t src_crc = read_uint32(state->dst + state->dst_offset -4);
uint32_t crc=Crc32_ComputeBuf( 0, state->dst, state->dst_offset -4);
if (src_crc != crc){
DEBUGF("Dropping frame due to CRC failure (%08x vs %08x)", src_crc, crc);
dump("frame", state->dst, state->dst_offset);
state->dst_offset=0;
state->state=0;
break;
}
// return once we've successfully parsed a valid packet that isn't empty
state->packet_length=state->dst_offset -4;
return 1;
}
// set the valid flag to begin parsing the next packet
state->state=DC_VALID;
break;
default:
if (state->state&DC_VALID)
state->dst[state->dst_offset++]=state->src[state->src_offset];
}
}
state->src_offset++;
}
return 0;
}
case SLIP_FORMAT_UPPER7:
{
if (config.debug.slip) {
if (state->rssi_len>=RSSI_TEXT_SIZE) state->rssi_len=RSSI_TEXT_SIZE-1;
state->rssi_text[state->rssi_len]=0;
DEBUGF("RX state=%d, rssi_len=%u, rssi_text='%s',src=%p, src_size=%u",
state->state,state->rssi_len,state->rssi_text,
state->src,state->src_size);
}
while(state->src_offset<state->src_size) {
if (upper7_decode(state,state->src[state->src_offset++])==1) {
if (config.debug.slip) {
dump("de-slipped packet",state->dst,state->packet_length);
}
// Check that CRC matches
uint32_t crc=Crc32_ComputeBuf( 0, state->dst, state->packet_length);
if (crc!=state->crc) {
if (config.debug.packetradio||config.debug.rejecteddata)
DEBUGF("Rejected packet of %u bytes due to CRC mis-match (%08x vs %08x)",
state->packet_length,crc,state->crc);
if (config.debug.rejecteddata) {
dump("bad packet",state->dst,state->packet_length);
}
} else {
if (config.debug.packetradio)
DEBUGF("Accepted packet of %u bytes (CRC ok)",state->packet_length);
return 1;
}
}
}
}
return 0;
default:
return WHYF("Unknown SLIP encapsulation format #%d",state->encapsulator);
}
}
static int dmg_read_resource_fork(BlockDriverState *bs, DmgHeaderState *ds,
uint64_t info_begin, uint64_t info_length)
{
BDRVDMGState *s = bs->opaque;
int ret;
uint32_t count, rsrc_data_offset;
uint8_t *buffer = NULL;
uint64_t info_end;
uint64_t offset;
/* read offset from begin of resource fork (info_begin) to resource data */
ret = read_uint32(bs, info_begin, &rsrc_data_offset);
if (ret < 0) {
goto fail;
} else if (rsrc_data_offset > info_length) {
ret = -EINVAL;
goto fail;
}
/* read length of resource data */
ret = read_uint32(bs, info_begin + 8, &count);
if (ret < 0) {
goto fail;
} else if (count == 0 || rsrc_data_offset + count > info_length) {
ret = -EINVAL;
goto fail;
}
/* begin of resource data (consisting of one or more resources) */
offset = info_begin + rsrc_data_offset;
/* end of resource data (there is possibly a following resource map
* which will be ignored). */
info_end = offset + count;
/* read offsets (mish blocks) from one or more resources in resource data */
while (offset < info_end) {
/* size of following resource */
ret = read_uint32(bs, offset, &count);
if (ret < 0) {
goto fail;
} else if (count == 0 || count > info_end - offset) {
ret = -EINVAL;
goto fail;
}
offset += 4;
buffer = g_realloc(buffer, count);
ret = bdrv_pread(bs->file, offset, buffer, count);
if (ret < 0) {
goto fail;
}
ret = dmg_read_mish_block(s, ds, buffer, count);
if (ret < 0) {
goto fail;
}
/* advance offset by size of resource */
offset += count;
}
ret = 0;
fail:
g_free(buffer);
return ret;
}
uint64_t
read_uint64(unsigned char *p) {
uint32_t v1 = read_uint32(p);
uint32_t v2 = read_uint32(p+4);
return (uint64_t)(v1 + ((uint64_t)v2<<32));
}
void *read_pipe_(void* a)
{
*((int*)a) = read_uint32("outpipe");
}
void pok_loader_load_partition (const uint32_t part_id, uint32_t *entry)
{
(void) part_id;
uint32_t part_entry;
uint32_t segments;
if(read_uint32(&part_entry) != sizeof(uint32_t))
{
#ifdef POK_NEEDS_ERROR_HANDLING
pok_partition_error (part_id, POK_ERROR_KIND_PARTITION_CONFIGURATION);
#else
#ifdef POK_NEEDS_DEBUG
#include <core/debug.h>
#include <stdio.h>
pok_fatal ("No partition entry\n");
#endif
#endif
}
if(read_uint32(&segments) != sizeof(uint32_t))
{
#ifdef POK_NEEDS_ERROR_HANDLING
pok_partition_error (part_id, POK_ERROR_KIND_PARTITION_CONFIGURATION);
#else
#ifdef POK_NEEDS_DEBUG
#include <core/debug.h>
#include <stdio.h>
pok_fatal ("No partition size\n");
#endif
#endif
}
#ifdef POK_NEEDS_DEBUG
printf("[DEBUG]\t [Reading partition %d] Entry address: %p\n", part_id, (void*)(part_entry));
printf("[DEBUG]\t [Reading partition %d] Segments number: %d\n", part_id, segments);
#endif
unsigned int segment = 0;
unsigned int part_size = 0;
while (segment < segments)
{
uint32_t segment_address;
uint32_t segment_size;
if (read_uint32(&segment_address) != sizeof(uint32_t))
{
#ifdef POK_NEEDS_ERROR_HANDLING
pok_partition_error (part_id, POK_ERROR_KIND_PARTITION_CONFIGURATION);
#else
#ifdef POK_NEEDS_DEBUG
#include <core/debug.h>
#include <stdio.h>
pok_fatal ("No segment address\n");
#endif
#endif
}
#ifdef POK_NEEDS_DEBUG
printf("[DEBUG]\t [Reading partition %d] Segment %d address: %p\n", part_id, segment, (void*)segment_address);
#endif
if (read_uint32(&segment_size) != sizeof(uint32_t))
{
#ifdef POK_NEEDS_ERROR_HANDLING
pok_partition_error (part_id, POK_ERROR_KIND_PARTITION_CONFIGURATION);
#else
#ifdef POK_NEEDS_DEBUG
#include <core/debug.h>
#include <stdio.h>
pok_fatal ("No segment size\n");
#endif
#endif
}
#ifdef POK_NEEDS_DEBUG
printf("[DEBUG]\t [Reading partition %d] Segment %d size: %d\n", part_id, segment, segment_size);
#endif
if (read_data(segment_size, (char*)(segment_address)) != segment_size) {
#ifdef POK_NEEDS_ERROR_HANDLING
pok_partition_error (part_id, POK_ERROR_KIND_PARTITION_CONFIGURATION);
#else
#ifdef POK_NEEDS_DEBUG
#include <core/debug.h>
#include <stdio.h>
printf("No segment data\n");
#endif
#endif
}
part_size = part_size + segment_size;
#ifdef POK_NEEDS_DEBUG
printf("[DEBUG]\t [Reading partition %d] Segment %d loaded\n", part_id, segment);
#endif
segment++;
}
*entry = (uint32_t) (part_entry);
}
int ape_parseheader(int fd, struct ape_ctx_t* ape_ctx)
{
int i,n;
/* TODO: Skip any leading junk such as id3v2 tags */
ape_ctx->junklength = 0;
lseek(fd,ape_ctx->junklength,SEEK_SET);
n = read(fd,&ape_ctx->magic,4);
if (n != 4) return -1;
if (memcmp(ape_ctx->magic,"MAC ",4)!=0)
{
return -1;
}
if (read_int16(fd,&ape_ctx->fileversion) < 0)
return -1;
if (ape_ctx->fileversion >= 3980)
{
if (read_int16(fd,&ape_ctx->padding1) < 0)
return -1;
if (read_uint32(fd,&ape_ctx->descriptorlength) < 0)
return -1;
if (read_uint32(fd,&ape_ctx->headerlength) < 0)
return -1;
if (read_uint32(fd,&ape_ctx->seektablelength) < 0)
return -1;
if (read_uint32(fd,&ape_ctx->wavheaderlength) < 0)
return -1;
if (read_uint32(fd,&ape_ctx->audiodatalength) < 0)
return -1;
if (read_uint32(fd,&ape_ctx->audiodatalength_high) < 0)
return -1;
if (read_uint32(fd,&ape_ctx->wavtaillength) < 0)
return -1;
if (read(fd,&ape_ctx->md5,16) != 16)
return -1;
/* Skip any unknown bytes at the end of the descriptor. This is for future
compatibility */
if (ape_ctx->descriptorlength > 52)
lseek(fd,ape_ctx->descriptorlength - 52, SEEK_CUR);
/* Read header data */
if (read_uint16(fd,&ape_ctx->compressiontype) < 0)
return -1;
if (read_uint16(fd,&ape_ctx->formatflags) < 0)
return -1;
if (read_uint32(fd,&ape_ctx->blocksperframe) < 0)
return -1;
if (read_uint32(fd,&ape_ctx->finalframeblocks) < 0)
return -1;
if (read_uint32(fd,&ape_ctx->totalframes) < 0)
return -1;
if (read_uint16(fd,&ape_ctx->bps) < 0)
return -1;
if (read_uint16(fd,&ape_ctx->channels) < 0)
return -1;
if (read_uint32(fd,&ape_ctx->samplerate) < 0)
return -1;
} else {
ape_ctx->descriptorlength = 0;
ape_ctx->headerlength = 32;
if (read_uint16(fd,&ape_ctx->compressiontype) < 0)
return -1;
if (read_uint16(fd,&ape_ctx->formatflags) < 0)
return -1;
if (read_uint16(fd,&ape_ctx->channels) < 0)
return -1;
if (read_uint32(fd,&ape_ctx->samplerate) < 0)
return -1;
if (read_uint32(fd,&ape_ctx->wavheaderlength) < 0)
return -1;
if (read_uint32(fd,&ape_ctx->wavtaillength) < 0)
return -1;
if (read_uint32(fd,&ape_ctx->totalframes) < 0)
return -1;
if (read_uint32(fd,&ape_ctx->finalframeblocks) < 0)
return -1;
if (ape_ctx->formatflags & MAC_FORMAT_FLAG_HAS_PEAK_LEVEL)
{
lseek(fd, 4, SEEK_CUR); /* Skip the peak level */
ape_ctx->headerlength += 4;
}
if (ape_ctx->formatflags & MAC_FORMAT_FLAG_HAS_SEEK_ELEMENTS)
{
if (read_uint32(fd,&ape_ctx->seektablelength) < 0)
return -1;
ape_ctx->headerlength += 4;
ape_ctx->seektablelength *= sizeof(int32_t);
} else {
ape_ctx->seektablelength = ape_ctx->totalframes * sizeof(int32_t);
}
if (ape_ctx->formatflags & MAC_FORMAT_FLAG_8_BIT)
ape_ctx->bps = 8;
else if (ape_ctx->formatflags & MAC_FORMAT_FLAG_24_BIT)
ape_ctx->bps = 24;
else
ape_ctx->bps = 16;
if (ape_ctx->fileversion >= 3950)
ape_ctx->blocksperframe = 73728 * 4;
else if ((ape_ctx->fileversion >= 3900) || (ape_ctx->fileversion >= 3800 && ape_ctx->compressiontype >= 4000))
ape_ctx->blocksperframe = 73728;
else
ape_ctx->blocksperframe = 9216;
/* Skip any stored wav header */
if (!(ape_ctx->formatflags & MAC_FORMAT_FLAG_CREATE_WAV_HEADER))
{
lseek(fd, ape_ctx->wavheaderlength, SEEK_CUR);
}
}
ape_ctx->totalsamples = ape_ctx->finalframeblocks;
if (ape_ctx->totalframes > 1)
ape_ctx->totalsamples += ape_ctx->blocksperframe * (ape_ctx->totalframes-1);
if (ape_ctx->seektablelength > 0)
{
ape_ctx->seektable = malloc(ape_ctx->seektablelength);
if (ape_ctx->seektable == NULL)
return -1;
for (i=0; i < ape_ctx->seektablelength / sizeof(uint32_t); i++)
{
if (read_uint32(fd,&ape_ctx->seektable[i]) < 0)
{
free(ape_ctx->seektable);
return -1;
}
}
}
ape_ctx->firstframe = ape_ctx->junklength + ape_ctx->descriptorlength +
ape_ctx->headerlength + ape_ctx->seektablelength +
ape_ctx->wavheaderlength;
return 0;
}
int main(int argc, char *argv[]) {
// set scpi lib debug level: 0 for no debug info, 10 for lots
const int scsi_verbose = 2;
char *dev_name;
switch (argc) {
case 1:
fputs(
"\nUsage: stlink-access-test /dev/sg0, sg1, ...\n"
"\n*** Notice: The stlink firmware violates the USB standard.\n"
"*** If you plug-in the discovery's stlink, wait a several\n"
"*** minutes to let the kernel driver swallow the broken device.\n"
"*** Watch:\ntail -f /var/log/messages\n"
"*** This command sequence can shorten the waiting time and fix some issues.\n"
"*** Unplug the stlink and execute once as root:\n"
"modprobe -r usb-storage && modprobe usb-storage quirks=483:3744:lrwsro\n\n",
stderr);
return EXIT_FAILURE;
case 2:
dev_name = argv[1];
break;
default:
return EXIT_FAILURE;
}
fputs("*** stlink access test ***\n", stderr);
fprintf(stderr, "Using sg_lib %s : scsi_pt %s\n", sg_lib_version(),
scsi_pt_version());
stlink_t *sl = stlink_quirk_open(dev_name, scsi_verbose);
if (sl == NULL)
return EXIT_FAILURE;
// we are in mass mode, go to swd
stlink_enter_swd_mode(sl);
stlink_current_mode(sl);
stlink_core_id(sl);
//----------------------------------------------------------------------
stlink_status(sl);
//stlink_force_debug(sl);
stlink_reset(sl);
stlink_status(sl);
#if 0
// core system control block
stlink_read_mem32(sl, 0xe000ed00, 4);
DD(sl, "cpu id base register: SCB_CPUID = got 0x%08x expect 0x411fc231", read_uint32(sl->q_buf, 0));
// no MPU
stlink_read_mem32(sl, 0xe000ed90, 4);
DD(sl, "mpu type register: MPU_TYPER = got 0x%08x expect 0x0", read_uint32(sl->q_buf, 0));
stlink_read_mem32(sl, 0xe000edf0, 4);
DD(sl, "DHCSR = 0x%08x", read_uint32(sl->q_buf, 0));
stlink_read_mem32(sl, 0x4001100c, 4);
DD(sl, "GPIOC_ODR = 0x%08x", read_uint32(sl->q_buf, 0));
#endif
#if 0
// happy new year 2011: let blink all the leds
// see "RM0041 Reference manual - STM32F100xx advanced ARM-based 32-bit MCUs"
#define GPIOC 0x40011000 // port C
#define GPIOC_CRH (GPIOC + 0x04) // port configuration register high
#define GPIOC_ODR (GPIOC + 0x0c) // port output data register
#define LED_BLUE (1<<8) // pin 8
#define LED_GREEN (1<<9) // pin 9
stlink_read_mem32(sl, GPIOC_CRH, 4);
uint32_t io_conf = read_uint32(sl->q_buf, 0);
DD(sl, "GPIOC_CRH = 0x%08x", io_conf);
// set: general purpose output push-pull, output mode, max speed 10 MHz.
write_uint32(sl->q_buf, 0x44444411);
stlink_write_mem32(sl, GPIOC_CRH, 4);
clear_buf(sl);
for (int i = 0; i < 100; i++) {
write_uint32(sl->q_buf, LED_BLUE | LED_GREEN);
stlink_write_mem32(sl, GPIOC_ODR, 4);
/* stlink_read_mem32(sl, 0x4001100c, 4); */
/* DD(sl, "GPIOC_ODR = 0x%08x", read_uint32(sl->q_buf, 0)); */
delay(100);
clear_buf(sl);
stlink_write_mem32(sl, GPIOC_ODR, 4); // PC lo
delay(100);
}
write_uint32(sl->q_buf, io_conf); // set old state
#endif
#if 0
// TODO rtfm: stlink doesn't have flash write routines
// writing to the flash area confuses the fw for the next read access
//stlink_read_mem32(sl, 0, 1024*6);
// flash 0x08000000 128kB
fputs("++++++++++ read a flash at 0x0800 0000\n", stderr);
stlink_read_mem32(sl, 0x08000000, 1024 * 6); //max 6kB
clear_buf(sl);
stlink_read_mem32(sl, 0x08000c00, 5);
stlink_read_mem32(sl, 0x08000c00, 4);
mark_buf(sl);
stlink_write_mem32(sl, 0x08000c00, 4);
stlink_read_mem32(sl, 0x08000c00, 256);
stlink_read_mem32(sl, 0x08000c00, 256);
#endif
#if 0
// sram 0x20000000 8kB
fputs("\n++++++++++ read/write 8bit, sram at 0x2000 0000 ++++++++++++++++\n\n", stderr);
clear_buf(sl);
stlink_write_mem8(sl, 0x20000000, 16);
mark_buf(sl);
stlink_write_mem8(sl, 0x20000000, 1);
stlink_write_mem8(sl, 0x20000001, 1);
stlink_write_mem8(sl, 0x2000000b, 3);
stlink_read_mem32(sl, 0x20000000, 16);
#endif
#if 0
// a not aligned mem32 access doesn't work indeed
fputs("\n++++++++++ read/write 32bit, sram at 0x2000 0000 ++++++++++++++++\n\n", stderr);
clear_buf(sl);
stlink_write_mem8(sl, 0x20000000, 32);
mark_buf(sl);
stlink_write_mem32(sl, 0x20000000, 1);
stlink_read_mem32(sl, 0x20000000, 16);
mark_buf(sl);
stlink_write_mem32(sl, 0x20000001, 1);
stlink_read_mem32(sl, 0x20000000, 16);
mark_buf(sl);
stlink_write_mem32(sl, 0x2000000b, 3);
stlink_read_mem32(sl, 0x20000000, 16);
mark_buf(sl);
stlink_write_mem32(sl, 0x20000000, 17);
stlink_read_mem32(sl, 0x20000000, 32);
#endif
#if 0
// sram 0x20000000 8kB
fputs("++++++++++ read/write 32bit, sram at 0x2000 0000 ++++++++++++\n", stderr);
mark_buf(sl);
stlink_write_mem8(sl, 0x20000000, 64);
stlink_read_mem32(sl, 0x20000000, 64);
mark_buf(sl);
stlink_write_mem32(sl, 0x20000000, 1024 * 8); //8kB
stlink_read_mem32(sl, 0x20000000, 1024 * 6);
stlink_read_mem32(sl, 0x20000000 + 1024 * 6, 1024 * 2);
#endif
#if 0
stlink_read_all_regs(sl);
stlink_step(sl);
fputs("++++++++++ write r0 = 0x12345678\n", stderr);
stlink_write_reg(sl, 0x12345678, 0);
stlink_read_reg(sl, 0);
stlink_read_all_regs(sl);
#endif
#if 0
stlink_run(sl);
stlink_status(sl);
stlink_force_debug(sl);
stlink_status(sl);
#endif
#if 1 /* read the system bootloader */
fputs("\n++++++++++ reading bootloader ++++++++++++++++\n\n", stderr);
stlink_fread(sl, "/tmp/barfoo", sl->sys_base, sl->sys_size);
#endif
#if 0 /* read the flash memory */
fputs("\n+++++++ read flash memory\n\n", stderr);
/* mark_buf(sl); */
stlink_read_mem32(sl, 0x08000000, 4);
#endif
#if 0 /* flash programming */
fputs("\n+++++++ program flash memory\n\n", stderr);
stlink_fwrite_flash(sl, "/tmp/foobar", 0x08000000);
#endif
#if 0 /* check file contents */
fputs("\n+++++++ check flash memory\n\n", stderr);
{
const int res = stlink_fcheck_flash(sl, "/tmp/foobar", 0x08000000);
printf("_____ stlink_fcheck_flash() == %d\n", res);
}
#endif
#if 0
fputs("\n+++++++ sram write and execute\n\n", stderr);
stlink_fwrite_sram(sl, "/tmp/foobar", sl->sram_base);
stlink_run_at(sl, sl->sram_base);
#endif
stlink_run(sl);
stlink_status(sl);
//----------------------------------------------------------------------
// back to mass mode, just in case ...
stlink_exit_debug_mode(sl);
stlink_current_mode(sl);
stlink_close(sl);
//fflush(stderr); fflush(stdout);
return EXIT_SUCCESS;
}
bool Indigo::detail::ZipFile::open(const char* filename)
{
static const OOBase::uint8_t END_OF_CDR[4] = { 0x50, 0x4b, 0x05, 0x06 };
static const OOBase::uint8_t CDR_HEADER[4] = { 0x50, 0x4b, 0x01, 0x02 };
int err = m_file.open(filename,false);
if (err)
LOG_ERROR_RETURN(("Failed to open file %s: %s",filename,OOBase::system_error_text(err)),false);
OOBase::uint64_t len = m_file.length();
if (len == OOBase::uint64_t(-1))
LOG_ERROR_RETURN(("Failed to get file length: %s",OOBase::system_error_text()),false);
// Step backwards looking for end of central directory record
OOBase::uint32_t cdr_size = 0;
OOBase::ScopedArrayPtr<OOBase::uint8_t,OOBase::ThreadLocalAllocator,256> buf;
for (OOBase::int64_t offset = len - 256;!cdr_size;)
{
if (offset < 0)
offset = 0;
OOBase::uint64_t p = m_file.seek(offset,OOBase::File::seek_begin);
if (p == OOBase::uint64_t(-1))
LOG_ERROR_RETURN(("Failed to get seek in file: %s",OOBase::system_error_text()),false);
size_t chunk_len = m_file.read(buf.get(),256);
if (chunk_len == size_t(-1))
LOG_ERROR_RETURN(("Failed to read file: %s",OOBase::system_error_text()),false);
// Scan forwards looking for signatures
for (OOBase::uint8_t* c = buf.get(); c < buf.get() + chunk_len - 4; ++c)
{
if (memcmp(c,END_OF_CDR,4) == 0)
{
OOBase::uint64_t eof_cdr = p + (c - buf.get());
if (len - eof_cdr >= 22 && m_file.seek(eof_cdr,OOBase::File::seek_begin) == eof_cdr)
{
m_file.read(buf.get(),22);
OOBase::uint16_t disk_no = read_uint16(buf,4);
OOBase::uint16_t cdr_disk_no = read_uint16(buf,6);
OOBase::uint16_t cdr_entries_disk = read_uint16(buf,8);
OOBase::uint16_t cdr_entries = read_uint16(buf,10);
OOBase::uint32_t cdr_size_i = read_uint32(buf,12);
OOBase::uint32_t cdr_offset = read_uint32(buf,16);
OOBase::uint16_t comments = read_uint16(buf,20);
if (cdr_size_i >= 46 && (eof_cdr + 22 + comments == len) &&
(disk_no == 0 && cdr_disk_no == 0 && cdr_entries_disk == cdr_entries) &&
(cdr_offset + cdr_size_i <= eof_cdr))
{
if (m_file.seek(cdr_offset,OOBase::File::seek_begin) == cdr_offset)
{
m_file.read(buf.get(),46);
if (memcmp(buf.get(),CDR_HEADER,4) == 0)
{
cdr_size = cdr_size_i;
break;
}
}
}
}
}
}
if (offset > 0)
offset -= 253;
else
break;
}
if (!cdr_size)
LOG_ERROR_RETURN(("Failed to find end of central dictionary in zip %s",filename),false);
// Now loop reading file entries
for (;;)
{
struct Info info = {0};
info.m_length = read_uint32(buf,24);
OOBase::uint16_t filename_len = read_uint16(buf,28);
OOBase::uint16_t extra_len = read_uint16(buf,30);
OOBase::uint16_t comments = read_uint16(buf,32);
OOBase::uint16_t disk_no = read_uint16(buf,34);
info.m_offset = read_uint32(buf,42);
if (disk_no != 0)
LOG_ERROR_RETURN(("Multi-disk zip file not supported: %s",filename),false);
if (filename_len == 0)
LOG_WARNING(("Ignoring empty filename in %s",filename));
else
{
if (!buf.resize(filename_len))
LOG_ERROR_RETURN(("Failed to resize buffer: %s",OOBase::system_error_text()),false);
if (m_file.read(buf.get(),filename_len) != filename_len)
LOG_ERROR_RETURN(("Failed to read file %s: %s",filename,OOBase::system_error_text()),false);
OOBase::String strFilename;
if (!strFilename.assign(reinterpret_cast<char*>(buf.get()),filename_len))
LOG_ERROR_RETURN(("Failed to assign string: %s",OOBase::system_error_text()),false);
if (!m_mapFiles.insert(strFilename,info))
LOG_ERROR_RETURN(("Failed to insert zip entry: %s",OOBase::system_error_text()),false);
}
if (cdr_size <= OOBase::uint32_t(filename_len) + extra_len + comments + 46)
break;
cdr_size -= 46 + filename_len + extra_len + comments;
if (m_file.seek(extra_len + comments,OOBase::File::seek_current) == OOBase::uint64_t(-1))
LOG_ERROR_RETURN(("Failed to get seek in file: %s",OOBase::system_error_text()),false);
if (m_file.read(buf.get(),46) != 46)
LOG_ERROR_RETURN(("Failed to read central dictionary header in zip %s",filename),false);
if (memcmp(buf.get(),CDR_HEADER,4) != 0)
LOG_ERROR_RETURN(("Invalid central dictionary header in zip %s",filename),false);
}
return true;
}
OOBase::SharedPtr<const char> Indigo::detail::ZipFile::load(const OOBase::String& prefix, const char* name)
{
OOBase::SharedPtr<const char> ret;
OOBase::String filename(prefix);
if (!filename.append(name))
LOG_ERROR_RETURN(("Failed to append string: %s",OOBase::system_error_text()),ret);
OOBase::Table<OOBase::String,Info>::iterator i=m_mapFiles.find(filename);
if (!i)
return ret;
// Read the local file header
if (m_file.seek(i->second.m_offset,OOBase::File::seek_begin) == OOBase::uint64_t(-1))
LOG_ERROR_RETURN(("Failed to get seek in file: %s",OOBase::system_error_text()),ret);
OOBase::uint8_t header[30];
if (m_file.read(header,30) != 30)
LOG_ERROR_RETURN(("Failed to read local file header header in zip"),ret);
static const OOBase::uint8_t LFR_HEADER[4] = { 0x50, 0x4b, 0x03, 0x04 };
if (memcmp(header,LFR_HEADER,4) != 0)
LOG_ERROR_RETURN(("Invalid local file header header in zip"),ret);
OOBase::uint16_t compression = read_uint16(header,8);
OOBase::uint32_t compressed_size = read_uint32(header,18);
size_t offset = 30 + read_uint16(header,26) + read_uint16(header,28);
OOBase::SharedPtr<const char> mapping = m_file.auto_map<const char>(false,i->second.m_offset + offset,compressed_size);
if (!mapping)
LOG_ERROR_RETURN(("Failed to map file content: %s",OOBase::system_error_text()),ret);
if (compression == 0)
{
ret = mapping;
}
else if (compression == 8)
{
void* p = OOBase::CrtAllocator::allocate(i->second.m_length,1);
if (!p)
LOG_ERROR_RETURN(("Failed to allocate: %s",OOBase::system_error_text()),ret);
if (stbi_zlib_decode_noheader_buffer(static_cast<char*>(p),(int)i->second.m_length,mapping.get(),(int)compressed_size) == -1)
{
LOG_ERROR(("Failed to inflate file: %s",stbi_failure_reason()));
OOBase::CrtAllocator::free(p);
}
else
{
ret = OOBase::const_pointer_cast<const char>(OOBase::make_shared<char>(static_cast<char*>(p)));
if (!ret)
{
LOG_ERROR(("Failed to allocate: %s",OOBase::system_error_text()));
OOBase::CrtAllocator::free(p);
}
}
}
else
LOG_ERROR(("Unsupported zip compression method: %u",compression));
return ret;
}
/* Process the differences file in input stream, starting at offset 8 (the
header has already been read and verified), creating a new block reference
list. */
static void process_input(InputStream *is)
{
FILE *fp;
uint32_t S;
uint16_t C, A;
size_t num_blocks;
off_t offset;
BlockRef br;
uint8_t digest1[DS], digest2[DS];
fp = tmpfile();
if (fp == NULL)
{
fprintf(stderr, "Couldn't open temporary file!\n");
exit(EXIT_FAILURE);
}
offset = 8;
num_blocks = 0;
for (;;)
{
S = read_uint32(is);
C = read_uint16(is);
A = read_uint16(is);
offset += 8;
/* Check for end-of-instructions. */
if (S == 0xffffffffu && C == 0xffffu && A == 0xffffu) break;
if ( (C > 0x7fffu || A > 0x7fffu) || (C > 0xffffffffu - S) ||
(S == 0xffffffffu && C != 0) || (S != 0xffffffffu && C == 0) )
{
fprintf(stderr, "Invalid instruction in differences file!\n");
exit(EXIT_FAILURE);
}
while (C--)
{
if (last_blocks == NULL)
{
br.is = NULL;
br.offset = BS*S++;
}
else
{
if (S >= last_num_blocks)
{
fprintf(stderr, "Invalid block index in differences file!\n");
exit(EXIT_FAILURE);
}
br = last_blocks[S++];
}
if (fwrite(&br, sizeof(br), 1, fp) != 1)
{
fprintf(stderr, "Write to temporary file failed!\n");
exit(EXIT_FAILURE);
}
++num_blocks;
}
while (A--)
{
br.is = is;
br.offset = offset;
offset += BS;
if (fwrite(&br, sizeof(br), 1, fp) != 1)
{
fprintf(stderr, "Write to temporary file failed!\n");
exit(EXIT_FAILURE);
}
++num_blocks;
}
is->seek(is, offset);
}
/* Verify MD5 digests */
read_data(is, digest2, DS);
if (is->read(is, digest1, DS) == DS)
{
if (last_blocks == NULL)
{
orig_digest_known = true;
memcpy(orig_digest, digest1, DS);
}
else
if (memcmp(digest1, last_digest, DS) != 0)
{
fprintf(stderr, "Invalid sequence of differences files!\n");
exit(EXIT_FAILURE);
}
}
else
{
fprintf(stderr, "WARNING: differences file is missing original file "
"digest; patch integrity cannot be guaranteed.\n");
}
memcpy(last_digest, digest2, DS);
/* Unmap old block data */
if (last_blocks != NULL)
{
if (munmap(last_blocks, last_num_blocks*sizeof(BlockRef)) != 0)
{
fprintf(stderr, "munmap() failed!\n");
exit(EXIT_FAILURE);
}
}
/* Map new block data */
fflush(fp); /* must flush to ensure all data can be mmap()ed */
last_blocks = mmap( NULL, num_blocks*sizeof(BlockRef), PROT_READ,
MAP_SHARED, fileno(fp), 0 );
if (last_blocks == NULL)
{
fprintf(stderr, "mmap() failed!\n");
exit(EXIT_FAILURE);
}
last_num_blocks = num_blocks;
fclose(fp);
}
//int ape_parseheader(int fd, struct ape_parser_ctx_t* ape_ctx)
int ape_parseheader(FILE* fp, struct ape_parser_ctx_t* ape_ctx)
{
int i,n;
/* Skip any leading junk such as id3v2 tags */
memset(ape_ctx, 0, sizeof(struct ape_parser_ctx_t));
while (1) {
char sync[5];
if (4 != fread(sync, 1, 4, fp))
return -1;
sync[4] = 0;
if (strcmp(sync,"MAC ") == 0)
break;
else if (strcmp(sync + 1,"MAC") == 0) {
fseek(fp, -1, SEEK_CUR);
ape_ctx->junklength += 1;
}
else if (strcmp(sync + 2, "MA") == 0) {
fseek(fp, -2, SEEK_CUR);
ape_ctx->junklength += 2;
}
else if (sync[3]=='M') {
fseek(fp, -3, SEEK_CUR);
ape_ctx->junklength += 3;
}
else {
ape_ctx->junklength += 4;
}
}
fseek(fp, ape_ctx->junklength, SEEK_SET);
n = fread(&ape_ctx->magic,1, 4, fp);
if (n != 4) return -1;
if (memcmp(ape_ctx->magic,"MAC ",4)!=0)
{
return -1;
}
if (read_int16(fp,&ape_ctx->fileversion) < 0)
return -1;
if (ape_ctx->fileversion >= 3980)
{
if (read_int16(fp,&ape_ctx->padding1) < 0)
return -1;
if (read_uint32(fp,&ape_ctx->descriptorlength) < 0)
return -1;
if (read_uint32(fp,&ape_ctx->headerlength) < 0)
return -1;
if (read_uint32(fp,&ape_ctx->seektablelength) < 0)
return -1;
if (read_uint32(fp,&ape_ctx->wavheaderlength) < 0)
return -1;
if (read_uint32(fp,&ape_ctx->audiodatalength) < 0)
return -1;
if (read_uint32(fp,&ape_ctx->audiodatalength_high) < 0)
return -1;
if (read_uint32(fp,&ape_ctx->wavtaillength) < 0)
return -1;
if (fread(&ape_ctx->md5,1, 16, fp) != 16)
return -1;
/* Skip any unknown bytes at the end of the descriptor. This is for future
compatibility */
if (ape_ctx->descriptorlength > 52)
fseek(fp, ape_ctx->descriptorlength - 52, SEEK_CUR);
/* Read header data */
if (read_uint16(fp,&ape_ctx->compressiontype) < 0)
return -1;
if (read_uint16(fp,&ape_ctx->formatflags) < 0)
return -1;
if (read_uint32(fp,&ape_ctx->blocksperframe) < 0)
return -1;
if (read_uint32(fp,&ape_ctx->finalframeblocks) < 0)
return -1;
if (read_uint32(fp,&ape_ctx->totalframes) < 0)
return -1;
if (read_uint16(fp,&ape_ctx->bps) < 0)
return -1;
if (read_uint16(fp,&ape_ctx->channels) < 0)
return -1;
if (read_uint32(fp,&ape_ctx->samplerate) < 0)
return -1;
} else {
ape_ctx->descriptorlength = 0;
ape_ctx->headerlength = 32;
if (read_uint16(fp,&ape_ctx->compressiontype) < 0)
return -1;
if (read_uint16(fp,&ape_ctx->formatflags) < 0)
return -1;
if (read_uint16(fp,&ape_ctx->channels) < 0)
return -1;
if (read_uint32(fp,&ape_ctx->samplerate) < 0)
return -1;
if (read_uint32(fp,&ape_ctx->wavheaderlength) < 0)
return -1;
if (read_uint32(fp,&ape_ctx->wavtaillength) < 0)
return -1;
if (read_uint32(fp,&ape_ctx->totalframes) < 0)
return -1;
if (read_uint32(fp,&ape_ctx->finalframeblocks) < 0)
return -1;
if (ape_ctx->formatflags & MAC_FORMAT_FLAG_HAS_PEAK_LEVEL)
{
fseek(fp, 4, SEEK_CUR);
ape_ctx->headerlength += 4;
}
if (ape_ctx->formatflags & MAC_FORMAT_FLAG_HAS_SEEK_ELEMENTS)
{
if (read_uint32(fp,&ape_ctx->seektablelength) < 0)
return -1;
ape_ctx->headerlength += 4;
ape_ctx->seektablelength *= sizeof(ape_parser_int32_t);
} else {
ape_ctx->seektablelength = ape_ctx->totalframes * sizeof(ape_parser_int32_t);
}
if (ape_ctx->formatflags & MAC_FORMAT_FLAG_8_BIT)
ape_ctx->bps = 8;
else if (ape_ctx->formatflags & MAC_FORMAT_FLAG_24_BIT)
ape_ctx->bps = 24;
else
ape_ctx->bps = 16;
if (ape_ctx->fileversion >= 3950)
ape_ctx->blocksperframe = 73728 * 4;
else if ((ape_ctx->fileversion >= 3900) || (ape_ctx->fileversion >= 3800 && ape_ctx->compressiontype >= 4000))
ape_ctx->blocksperframe = 73728;
else
ape_ctx->blocksperframe = 9216;
/* Skip any stored wav header */
if (!(ape_ctx->formatflags & MAC_FORMAT_FLAG_CREATE_WAV_HEADER))
{
fseek(fp, ape_ctx->wavheaderlength, SEEK_CUR);
}
}
ape_ctx->totalsamples = ape_ctx->finalframeblocks;
if (ape_ctx->totalframes > 1)
ape_ctx->totalsamples += ape_ctx->blocksperframe * (ape_ctx->totalframes-1);
if (ape_ctx->seektablelength > 0)
{
ape_ctx->seektable = malloc(ape_ctx->seektablelength);
if (ape_ctx->seektable == NULL)
return -1;
for (i=0; i < ape_ctx->seektablelength / sizeof(ape_parser_uint32_t); i++)
{
if (read_uint32(fp,&ape_ctx->seektable[i]) < 0)
{
free(ape_ctx->seektable);
return -1;
}
}
}
ape_ctx->firstframe = ape_ctx->junklength + ape_ctx->descriptorlength +
ape_ctx->headerlength + ape_ctx->seektablelength +
ape_ctx->wavheaderlength;
return 0;
}
int main ( int argc, char *argv[] )
{
stlink_t* sl;
unsigned int ra;
unsigned int rb;
unsigned int flen;
int ret;
if(argc<2)
{
printf(".bin file not specified\n");
return(1);
}
fp=fopen(argv[1],"rb");
if(fp==NULL)
{
printf("Error opening file [%s]\n",argv[1]);
return(1);
}
memset(pdata,0xFF,sizeof(pdata));
flen=fread(pdata,1,sizeof(pdata),fp);
flen+=3;
flen>>=2;
fclose(fp);
sl = stlink_open_usb(10);
if(sl==NULL)
{
printf("stlink_open_usb failed\n");
return(1);
}
printf("-- version\n");
stlink_version(sl);
printf("mode before doing anything: %d\n", stlink_current_mode(sl));
if (stlink_current_mode(sl) == STLINK_DEV_DFU_MODE) {
printf("-- exit_dfu_mode\n");
stlink_exit_dfu_mode(sl);
}
printf("-- enter_swd_mode\n");
stlink_enter_swd_mode(sl);
printf("-- mode after entering swd mode: %d\n", stlink_current_mode(sl));
printf("-- chip id: %#x\n", sl->chip_id);
printf("-- core_id: %#x\n", sl->core_id);
cortex_m3_cpuid_t cpuid;
stlink_cpu_id(sl, &cpuid);
printf("cpuid:impl_id = %0#x, variant = %#x\n", cpuid.implementer_id, cpuid.variant);
printf("cpuid:part = %#x, rev = %#x\n", cpuid.part, cpuid.revision);
// printf("-- status\n");
//stlink_status(sl);
printf("-- reset\n");
stlink_reset(sl);
stlink_force_debug(sl);
// printf("-- status\n");
// stlink_status(sl);
#ifdef LOAD_RAM
printf("-- load\n");
for(ra=0;ra<flen;ra++)
{
write_uint32(sl->q_buf,pdata[ra]);
stlink_write_mem32(sl, 0x20000000+(ra<<2), 4);
}
for(ra=0;ra<8;ra++)
{
stlink_read_mem32(sl, 0x20000000+(ra<<2), 4);
rb=read_uint32(sl->q_buf,0);
printf("[0x%08X] 0x%08X 0x%08X\n",ra,rb,pdata[ra]);
}
printf("-- run\n");
stlink_write_reg(sl, 0x20020000, 13); /* pc register */
stlink_write_reg(sl, 0x20000000, 15); /* pc register */
stlink_run(sl);
ret =0;
#endif //LOAD_RAM
#ifdef LOAD_FLASH
ra=0;
rb=0;
ret=stlink_write_flash(sl,0x08000000,(unsigned char *)pdata,0x4000);
if(ret)
{
printf("stlink_write_flasin error\n");
}
#endif //LOAD_FLASH
printf("-- exit_debug_mode\n");
stlink_exit_debug_mode(sl);
stlink_close(sl);
return 0;
}
crf1mm_t* crf1mm_new(const char *filename)
{
FILE *fp = NULL;
uint8_t* p = NULL;
crf1mm_t *model = NULL;
header_t *header = NULL;
model = (crf1mm_t*)calloc(1, sizeof(crf1mm_t));
if (model == NULL) {
goto error_exit;
}
fp = fopen(filename, "rb");
if (fp == NULL) {
goto error_exit;
}
fseek(fp, 0, SEEK_END);
model->size = (uint32_t)ftell(fp);
fseek(fp, 0, SEEK_SET);
model->buffer = model->buffer_orig = (uint8_t*)malloc(model->size + 16);
while ((uint32_t)model->buffer % 16 != 0) {
++model->buffer;
}
fread(model->buffer, 1, model->size, fp);
fclose(fp);
/* Write the file header. */
header = (header_t*)calloc(1, sizeof(header_t));
p = model->buffer;
p += read_uint8_array(p, header->magic, sizeof(header->magic));
p += read_uint32(p, &header->size);
p += read_uint8_array(p, header->type, sizeof(header->type));
p += read_uint32(p, &header->version);
p += read_uint32(p, &header->num_features);
p += read_uint32(p, &header->num_labels);
p += read_uint32(p, &header->num_attrs);
p += read_uint32(p, &header->off_features);
p += read_uint32(p, &header->off_labels);
p += read_uint32(p, &header->off_attrs);
p += read_uint32(p, &header->off_labelrefs);
p += read_uint32(p, &header->off_attrrefs);
model->header = header;
model->labels = cqdb_reader(
model->buffer + header->off_labels,
model->size - header->off_labels
);
model->attrs = cqdb_reader(
model->buffer + header->off_attrs,
model->size - header->off_attrs
);
return model;
error_exit:
if (model != NULL) {
free(model);
}
if (fp != NULL) {
fclose(fp);
}
return NULL;
}
IFS_DATA* load_ifs_file (const char* filename) {
int infd;
IFS_DATA* ifs_data = NULL;
float version;
char* ifstag = NULL;
unsigned int i;
unsigned int nVertices = 0;
unsigned int nTriangles = 0;
unsigned int tmp_Index = 0;
if ((infd = open(filename, O_RDONLY)) < 2) {
fprintf(stderr, "Error opening an input IFS file\n");
exit(-1);
}
ifs_data = (IFS_DATA*) malloc(sizeof(IFS_DATA));
ifs_data->modelName = NULL;
ifs_data->numVertices = 0;
ifs_data->vertices = NULL;
ifs_data->numTriangles = 0;
ifs_data->triangles = NULL;
read_string32(infd, &ifstag);
if (strcmp(ifstag, "IFS") != 0) {
fprintf(stderr, "Not IFS filetype\n");
exit(-1);
}
free(ifstag); ifstag = NULL;
read_float32(infd, &version);
if (version != 1.0) {
fprintf(stderr, "Invalid version number: %f\n", version);
exit(-1);
}
read_string32(infd, &(ifs_data->modelName));
read_string32(infd, &ifstag);
if (strcmp(ifstag, "VERTICES") != 0) {
fprintf(stderr, "Not IFS filetype\n");
exit(-1);
}
free(ifstag); ifstag = NULL;
read_uint32(infd, &nVertices);
ifs_data->numVertices = nVertices;
ifs_data->vertices = (VERTEX*) malloc(nVertices * sizeof(VERTEX));
for (i =0; i < ifs_data->numVertices; ++i) {
ifs_data->vertices[i].id = i;
read_float32(infd, &((ifs_data->vertices)[i].x));
read_float32(infd, &((ifs_data->vertices)[i].y));
read_float32(infd, &((ifs_data->vertices)[i].z));
}
read_string32(infd, &ifstag);
if (strcmp(ifstag, "TRIANGLES") != 0) {
fprintf(stderr, "Not IFS filetype\n");
exit(-1);
}
free(ifstag); ifstag = NULL;
read_uint32(infd, &nTriangles);
ifs_data->numTriangles = nTriangles;
ifs_data->triangles = (TRIANGLE*) malloc(nTriangles * sizeof(TRIANGLE));
for (i =0; i < ifs_data->numTriangles; ++i) {
read_uint32(infd, &tmp_Index);
if (tmp_Index >= nVertices) {
fprintf(stderr, "Invalid Vertex index\n");
exit(-1);
}
ifs_data->triangles[i].a = &((ifs_data->vertices)[tmp_Index]);
read_uint32(infd, &tmp_Index);
if (tmp_Index >= nVertices) {
fprintf(stderr, "Invalid Vertex index\n");
exit(-1);
}
ifs_data->triangles[i].b = &((ifs_data->vertices)[tmp_Index]);
read_uint32(infd, &tmp_Index);
if (tmp_Index >= nVertices) {
fprintf(stderr, "Invalid Vertex index\n");
exit(-1);
}
ifs_data->triangles[i].c = &((ifs_data->vertices)[tmp_Index]);
}
if (close(infd) == -1) {
fprintf(stderr, "Error closing an input IFS file\n");
exit(-1);
}
return ifs_data;
}
static GHashTable *read_directory(FILE *f, int64_t *diroff,
GHashTable *loop_detector,
uint16_t endian) {
int64_t off = *diroff;
*diroff = 0;
GHashTable *result = NULL;
int64_t *key = NULL;
int64_t nextdiroff = -1;
int dircount = -1;
// g_debug("diroff: %" PRId64, off);
if (off <= 0) {
g_warning("Bad offset");
goto FAIL;
}
// loop detection
if (g_hash_table_lookup_extended(loop_detector, &off, NULL, NULL)) {
// loop
g_warning("Loop detected");
goto FAIL;
}
key = g_slice_new(int64_t);
*key = off;
g_hash_table_insert(loop_detector, key, NULL);
// no loop, let's seek
if (fseeko(f, off, SEEK_SET) != 0) {
g_warning("Cannot seek to offset");
goto FAIL;
}
// read directory count
dircount = read_uint16(f, endian);
if (dircount == -1) {
g_warning("Cannot read dircount");
goto FAIL;
}
// g_debug("dircount: %d", dircount);
// initial checks passed, initialized the hashtable
result = g_hash_table_new_full(g_int_hash, g_int_equal,
g_free, tiffdump_item_destroy);
// read all directory entries
for (int i = 0; i < dircount; i++) {
int32_t tag = read_uint16(f, endian);
int32_t type = read_uint16(f, endian);
int64_t count = read_uint32(f, endian);
if ((tag == -1) || (type == -1) || (count == -1)) {
g_warning("Cannot read tag, type, and count");
goto FAIL;
}
// g_debug(" tag: %d, type: %d, count: %" PRId64, tag, type, count);
// read in the value/offset
uint8_t value[4];
if (fread(value, 1, 4, f) != 4) {
g_warning("Cannot read value/offset");
goto FAIL;
}
uint32_t offset;
memcpy(&offset, value, 4);
if (endian == TIFF_BIGENDIAN) {
offset = GUINT32_FROM_BE(offset);
} else {
offset = GUINT32_FROM_LE(offset);
}
// allocate the item
struct _openslide_tiffdump_item *data =
g_slice_new(struct _openslide_tiffdump_item);
data->type = (TIFFDataType) type;
data->count = count;
// load the value
switch (type) {
case TIFF_BYTE:
case TIFF_ASCII:
case TIFF_SBYTE:
case TIFF_UNDEFINED:
data->value = read_tiff_tag_1(f, count, offset, value);
break;
case TIFF_SHORT:
case TIFF_SSHORT:
data->value = read_tiff_tag_2(f, count, offset, value, endian);
break;
case TIFF_LONG:
case TIFF_SLONG:
case TIFF_FLOAT:
case TIFF_IFD:
data->value = read_tiff_tag_4(f, count, offset, value, endian);
break;
case TIFF_RATIONAL:
case TIFF_SRATIONAL:
data->value = read_tiff_tag_4(f, count * 2, offset, value, endian);
break;
case TIFF_DOUBLE:
data->value = read_tiff_tag_8(f, count, offset, endian);
break;
default:
g_warning("Unknown type encountered: %d", type);
goto FAIL;
}
if (data->value == NULL) {
g_warning("Cannot read value");
goto FAIL;
}
// add this tag to the hashtable
int *key = g_new(int, 1);
*key = tag;
g_hash_table_insert(result, key, data);
}
// read the next dir offset
nextdiroff = read_uint32(f, endian);
if (nextdiroff == -1) {
g_warning("Cannot read next directory offset");
goto FAIL;
}
*diroff = nextdiroff;
// success
return result;
FAIL:
if (result != NULL) {
g_hash_table_unref(result);
}
return NULL;
}
address read_v4_address(InIt&& in)
{
std::uint32_t const ip = read_uint32(in);
return address_v4(ip);
}
/**
* Analyse an ICO prior to decoding.
*
* This function will scan the data provided and perform simple checks to
* ensure the data is a valid ICO.
*
* This function must be called before ico_find().
*
* \param ico the ICO image to analyse
* \return BMP_OK on success
*/
bmp_result ico_analyse(ico_collection *ico, size_t size, uint8_t *data) {
uint16_t count, i;
bmp_result result;
int area, max_area = 0;
/* ensure we aren't already initialised */
if (ico->first)
return BMP_OK;
/* initialize values */
ico->buffer_size = size;
ico->ico_data = data;
/* 6-byte ICO file header is:
*
* +0 INT16 Reserved (should be 0)
* +2 UINT16 Type (1 for ICO, 2 for CUR)
* +4 UINT16 Number of BMPs to follow
*/
if (ico->buffer_size < ICO_FILE_HEADER_SIZE)
return BMP_INSUFFICIENT_DATA;
// if (read_int16(data, 2) != 0x0000)
// return BMP_DATA_ERROR;
if (read_uint16(data, 2) != 0x0001)
return BMP_DATA_ERROR;
count = read_uint16(data, 4);
if (count == 0)
return BMP_DATA_ERROR;
data += ICO_FILE_HEADER_SIZE;
/* check if we have enough data for the directory */
if (ico->buffer_size < (uint32_t)(ICO_FILE_HEADER_SIZE + (ICO_DIR_ENTRY_SIZE * count)))
return BMP_INSUFFICIENT_DATA;
/* Decode the BMP files.
*
* 16-byte ICO directory entry is:
*
* +0 UINT8 Width (0 for 256 pixels)
* +1 UINT8 Height (0 for 256 pixels)
* +2 UINT8 Colour count (0 if more than 256 colours)
* +3 INT8 Reserved (should be 0, but may not be)
* +4 UINT16 Colour Planes (should be 0 or 1)
* +6 UINT16 Bits Per Pixel
* +8 UINT32 Size of BMP info header + bitmap data in bytes
* +12 UINT32 Offset (points to the BMP info header, not the bitmap data)
*/
for (i = 0; i < count; i++) {
ico_image *image;
image = calloc(1, sizeof(ico_image));
if (!image)
return BMP_INSUFFICIENT_MEMORY;
result = next_ico_image(ico, image);
if (result != BMP_OK)
return result;
image->bmp.width = read_uint8(data, 0);
if (image->bmp.width == 0)
image->bmp.width = 256;
image->bmp.height = read_uint8(data, 1);
if (image->bmp.height == 0)
image->bmp.height = 256;
image->bmp.buffer_size = read_uint32(data, 8);
image->bmp.bmp_data = ico->ico_data + read_uint32(data, 12);
image->bmp.ico = true;
data += ICO_DIR_ENTRY_SIZE;
/* Ensure that the bitmap data resides in the buffer */
if (image->bmp.bmp_data - ico->ico_data >= 0 &&
(uint32_t)(image->bmp.bmp_data -
ico->ico_data) >= ico->buffer_size)
return BMP_DATA_ERROR;
/* Ensure that we have sufficient data to read the bitmap */
if (image->bmp.buffer_size - ICO_DIR_ENTRY_SIZE >=
ico->buffer_size - (ico->ico_data - data))
return BMP_INSUFFICIENT_DATA;
result = bmp_analyse_header(&image->bmp, image->bmp.bmp_data);
if (result != BMP_OK)
return result;
/* adjust the size based on the images available */
area = image->bmp.width * image->bmp.height;
if (area > max_area) {
ico->width = image->bmp.width;
ico->height = image->bmp.height;
max_area = area;
}
}
return BMP_OK;
}
static enum dlep_status_code parse_session_init_resp_message(const uint8_t* data_items, uint16_t len, uint32_t* heartbeat_interval, enum dlep_status_code* sc)
{
const uint8_t* data_item = data_items;
printf("Valid Session Initialization Response message from modem:\n");
/* The message has been validated so just scan for the relevant data_items */
while (data_item < data_items + len)
{
/* Octets 0 and 1 are the data item type */
enum dlep_data_item item_id = read_uint16(data_item);
/* Octets 2 and 3 are the data item length */
uint16_t item_len = read_uint16(data_item + 2);
/* Increment data_item to point to the data */
data_item += 4;
switch (item_id)
{
case DLEP_HEARTBEAT_INTERVAL_DATA_ITEM:
*heartbeat_interval = read_uint32(data_item);
printf(" Heartbeat Interval: %ums\n",*heartbeat_interval);
break;
case DLEP_PEER_TYPE_DATA_ITEM:
printf(" Peer Type: '%.*s'%s\n",(int)item_len-1,data_item + 1,data_item[0] ? " - Secured Medium" : "");
break;
case DLEP_STATUS_DATA_ITEM:
*sc = data_item[0];
printf_status(*sc);
break;
case DLEP_MDRR_DATA_ITEM:
printf(" Default MDDR: %"PRIu64"bps\n",read_uint64(data_item));
break;
case DLEP_MDRT_DATA_ITEM:
printf(" Default MDDT: %"PRIu64"bps\n",read_uint64(data_item));
break;
case DLEP_CDRR_DATA_ITEM:
printf(" Default CDDR: %"PRIu64"bps\n",read_uint64(data_item));
break;
case DLEP_CDRT_DATA_ITEM:
printf(" Default CDDT: %"PRIu64"bps\n",read_uint64(data_item));
break;
case DLEP_LATENCY_DATA_ITEM:
printf(" Default Latency: %"PRIu64"\x03\xBCs\n",read_uint64(data_item));
break;
case DLEP_RESOURCES_DATA_ITEM:
printf(" Default Resources: %u%%\n",data_item[0]);
break;
case DLEP_RLQR_DATA_ITEM:
printf(" Default RLQR: %u\n",data_item[0]);
break;
case DLEP_RLQT_DATA_ITEM:
printf(" Default RLQT: %u\n",data_item[0]);
break;
case DLEP_MTU_DATA_ITEM:
printf(" Default MTU: %"PRIu32"\n",read_uint32(data_item));
break;
case DLEP_EXTS_SUPP_DATA_ITEM:
if (item_len > 0)
{
size_t i = 0;
printf(" Extensions advertised by peer:\n");
for (; i < item_len; i += 2)
{
printf(" Unknown DLEP extension %u (which we don't support)\n",read_uint16(data_item + i));
}
}
break;
case DLEP_IPV4_ADDRESS_DATA_ITEM:
case DLEP_IPV6_ADDRESS_DATA_ITEM:
printf(" Modem ");
parse_address(data_item,item_len,0);
break;
case DLEP_IPV4_ATT_SUBNET_DATA_ITEM:
case DLEP_IPV6_ATT_SUBNET_DATA_ITEM:
printf(" Modem ");
parse_attached_subnet(data_item,item_len,0);
break;
default:
/* Others will be caught by the check function */
break;
}
/* Increment data_item to point to the next data item */
data_item += item_len;
}
return DLEP_SC_SUCCESS;
}
/**
* Decode BMP data stored in 24bpp colour.
*
* \param bmp the BMP image to decode
* \param start the data to decode, updated to last byte read on success
* \param bytes the number of bytes of data available
* \return BMP_OK on success
* BMP_INSUFFICIENT_DATA if the bitmap data ends unexpectedly;
* in this case, the image may be partially viewable
*/
static bmp_result bmp_decode_rgb24(bmp_image *bmp, uint8_t **start, int bytes) {
uint8_t *top, *bottom, *end, *data;
uint32_t *scanline;
uint32_t x, y;
uint32_t swidth, skip;
intptr_t addr;
uint8_t i;
uint32_t word;
data = *start;
swidth = bmp->bitmap_callbacks.bitmap_get_bpp(bmp->bitmap) * bmp->width;
top = bmp->bitmap_callbacks.bitmap_get_buffer(bmp->bitmap);
if (!top)
return BMP_INSUFFICIENT_MEMORY;
bottom = top + (uint64_t)swidth * (bmp->height - 1);
end = data + bytes;
addr = ((intptr_t)data) & 3;
skip = bmp->bpp >> 3;
bmp->decoded = true;
/* Determine transparent index */
if (bmp->limited_trans) {
if ((data + skip) > end)
return BMP_INSUFFICIENT_DATA;
if (bmp->encoding == BMP_ENCODING_BITFIELDS)
bmp->transparent_index = read_uint32(data, 0);
else
bmp->transparent_index = data[2] | (data[1] << 8) | (data[0] << 16);
}
for (y = 0; y < bmp->height; y++) {
while (addr != (((intptr_t)data) & 3))
data++;
if ((data + (skip * bmp->width)) > end)
return BMP_INSUFFICIENT_DATA;
if (bmp->reversed)
scanline = (void *)(top + (y * swidth));
else
scanline = (void *)(bottom - (y * swidth));
if (bmp->encoding == BMP_ENCODING_BITFIELDS) {
for (x = 0; x < bmp->width; x++) {
word = read_uint32(data, 0);
for (i = 0; i < 4; i++)
if (bmp->shift[i] > 0)
scanline[x] |= ((word & bmp->mask[i]) << bmp->shift[i]);
else
scanline[x] |= ((word & bmp->mask[i]) >> (-bmp->shift[i]));
/* 32-bit BMPs have alpha masks, but sometimes they're not utilized */
if (bmp->opaque)
scanline[x] |= (0xff << 24);
data += skip;
scanline[x] = read_uint32((uint8_t *)&scanline[x],0);
}
} else {
for (x = 0; x < bmp->width; x++) {
scanline[x] = data[2] | (data[1] << 8) | (data[0] << 16);
if ((bmp->limited_trans) && (scanline[x] == bmp->transparent_index))
scanline[x] = bmp->trans_colour;
if (bmp->opaque)
scanline[x] |= (0xff << 24);
data += skip;
scanline[x] = read_uint32((uint8_t *)&scanline[x],0);
}
}
}
static void parse_destination_update_message(const uint8_t* data_items, uint16_t len)
{
const uint8_t* data_item = data_items;
printf("Received Destination Update message from modem:\n");
/* The message has been validated so just scan for the relevant data_items */
while (data_item < data_items + len)
{
/* Octets 0 and 1 are the data item type */
enum dlep_data_item item_id = read_uint16(data_item);
/* Octets 2 and 3 are the data item length */
uint16_t item_len = read_uint16(data_item + 2);
/* Increment data_item to point to the data */
data_item += 4;
switch (item_id)
{
case DLEP_MAC_ADDRESS_DATA_ITEM:
printf(" MAC Address: %02X:%02X:%02X:%02X:%02X:%02X\n",data_item[0],data_item[1],data_item[2],data_item[3],data_item[4],data_item[5]);
break;
case DLEP_IPV4_ADDRESS_DATA_ITEM:
case DLEP_IPV6_ADDRESS_DATA_ITEM:
printf(" ");
parse_address(data_item,item_len,1);
break;
case DLEP_IPV4_ATT_SUBNET_DATA_ITEM:
case DLEP_IPV6_ATT_SUBNET_DATA_ITEM:
printf(" ");
parse_attached_subnet(data_item,item_len,1);
break;
case DLEP_MDRR_DATA_ITEM:
printf(" MDDR: %"PRIu64"bps\n",read_uint64(data_item));
break;
case DLEP_MDRT_DATA_ITEM:
printf(" MDDT: %"PRIu64"bps\n",read_uint64(data_item));
break;
case DLEP_CDRR_DATA_ITEM:
printf(" CDDR: %"PRIu64"bps\n",read_uint64(data_item));
break;
case DLEP_CDRT_DATA_ITEM:
printf(" CDDT: %"PRIu64"bps\n",read_uint64(data_item));
break;
case DLEP_LATENCY_DATA_ITEM:
printf(" Latency: %"PRIu64"\x03\xBCs\n",read_uint64(data_item));
break;
case DLEP_RESOURCES_DATA_ITEM:
printf(" Resources (Receive): %u%%\n",data_item[0]);
break;
case DLEP_RLQR_DATA_ITEM:
printf(" RLQR: %u\n",data_item[0]);
break;
case DLEP_RLQT_DATA_ITEM:
printf(" RLQT: %u\n",data_item[0]);
break;
case DLEP_MTU_DATA_ITEM:
printf(" MTU: %"PRIu32"\n",read_uint32(data_item));
break;
default:
/* Others will be caught by the check function */
break;
}
/* Increment data_item to point to the next data item */
data_item += item_len;
}
}
int opus_header_parse(const unsigned char *packet, int len, OpusHeader *h)
{
char str[9];
ROPacket p;
unsigned char ch;
uint16_t shortval;
p.data = packet;
p.maxlen = len;
p.pos = 0;
str[8] = 0;
if (len<19)return 0;
read_chars(&p, (unsigned char*)str, 8);
if (memcmp(str, "OpusHead", 8)!=0)
return 0;
if (!read_chars(&p, &ch, 1))
return 0;
h->version = ch;
if((h->version&240) != 0) /* Only major version 0 supported. */
return 0;
if (!read_chars(&p, &ch, 1))
return 0;
h->channels = ch;
if (h->channels == 0)
return 0;
if (!read_uint16(&p, &shortval))
return 0;
h->preskip = shortval;
if (!read_uint32(&p, &h->input_sample_rate))
return 0;
if (!read_uint16(&p, &shortval))
return 0;
h->gain = (short)shortval;
if (!read_chars(&p, &ch, 1))
return 0;
h->channel_mapping = ch;
if (h->channel_mapping != 0)
{
if (!read_chars(&p, &ch, 1))
return 0;
if (ch<1)
return 0;
h->nb_streams = ch;
if (!read_chars(&p, &ch, 1))
return 0;
if (ch>h->nb_streams || (ch+h->nb_streams)>255)
return 0;
h->nb_coupled = ch;
/* Multi-stream support */
for (int i=0;i<h->channels;i++)
{
if (!read_chars(&p, &h->stream_map[i], 1))
return 0;
if (h->stream_map[i]>(h->nb_streams+h->nb_coupled) && h->stream_map[i]!=255)
return 0;
}
} else {
if(h->channels>2)
return 0;
h->nb_streams = 1;
h->nb_coupled = h->channels>1;
h->stream_map[0]=0;
h->stream_map[1]=1;
}
/*For version 0/1 we know there won't be any more data
so reject any that have data past the end.*/
if ((h->version==0 || h->version==1) && p.pos != len)
return 0;
return 1;
}
/**
* Wraps a CDB mass storage command in the appropriate gunk to get it down
* @param handle
* @param endpoint
* @param cdb
* @param cdb_length
* @param lun
* @param flags
* @param expected_rx_size
* @return
*/
int send_usb_mass_storage_command(libusb_device_handle *handle, uint8_t endpoint_out,
uint8_t *cdb, uint8_t cdb_length,
uint8_t lun, uint8_t flags, uint32_t expected_rx_size) {
DLOG("Sending usb m-s cmd: cdblen:%d, rxsize=%d\n", cdb_length, expected_rx_size);
dump_CDB_command(cdb, cdb_length);
static uint32_t tag;
if (tag == 0) {
tag = 1;
}
int try = 0;
int ret = 0;
int real_transferred;
int i = 0;
uint8_t c_buf[STLINK_SG_SIZE];
// tag is allegedly ignored... TODO - verify
c_buf[i++] = 'U';
c_buf[i++] = 'S';
c_buf[i++] = 'B';
c_buf[i++] = 'C';
write_uint32(&c_buf[i], tag);
uint32_t this_tag = tag++;
write_uint32(&c_buf[i+4], expected_rx_size);
i+= 8;
c_buf[i++] = flags;
c_buf[i++] = lun;
c_buf[i++] = cdb_length;
// Now the actual CDB request
assert(cdb_length <= CDB_SL);
memcpy(&(c_buf[i]), cdb, cdb_length);
int sending_length = STLINK_SG_SIZE;
// send....
do {
ret = libusb_bulk_transfer(handle, endpoint_out, c_buf, sending_length,
&real_transferred, SG_TIMEOUT_MSEC);
if (ret == LIBUSB_ERROR_PIPE) {
libusb_clear_halt(handle, endpoint_out);
}
try++;
} while ((ret == LIBUSB_ERROR_PIPE) && (try < 3));
if (ret != LIBUSB_SUCCESS) {
WLOG("sending failed: %d\n", ret);
return -1;
}
return this_tag;
}
/**
* Straight from stm8 stlink code...
* @param handle
* @param endpoint_in
* @param endpoint_out
*/
static void
get_sense(libusb_device_handle *handle, uint8_t endpoint_in, uint8_t endpoint_out)
{
DLOG("Fetching sense...\n");
uint8_t cdb[16];
memset(cdb, 0, sizeof(cdb));
#define REQUEST_SENSE 0x03
#define REQUEST_SENSE_LENGTH 18
cdb[0] = REQUEST_SENSE;
cdb[4] = REQUEST_SENSE_LENGTH;
uint32_t tag = send_usb_mass_storage_command(handle, endpoint_out, cdb, sizeof(cdb), 0,
LIBUSB_ENDPOINT_IN, REQUEST_SENSE_LENGTH);
if (tag == 0) {
WLOG("refusing to send request sense with tag 0\n");
return;
}
unsigned char sense[REQUEST_SENSE_LENGTH];
int transferred;
int ret;
int try = 0;
do {
ret = libusb_bulk_transfer(handle, endpoint_in, sense, sizeof(sense),
&transferred, SG_TIMEOUT_MSEC);
if (ret == LIBUSB_ERROR_PIPE) {
libusb_clear_halt(handle, endpoint_in);
}
try++;
} while ((ret == LIBUSB_ERROR_PIPE) && (try < 3));
if (ret != LIBUSB_SUCCESS) {
WLOG("receiving sense failed: %d\n", ret);
return;
}
if (transferred != sizeof(sense)) {
WLOG("received unexpected amount of sense: %d != %d\n", transferred, sizeof(sense));
}
uint32_t received_tag;
int status = get_usb_mass_storage_status(handle, endpoint_in, &received_tag);
if (status != 0) {
WLOG("receiving sense failed with status: %02x\n", status);
return;
}
if (sense[0] != 0x70 && sense[0] != 0x71) {
WLOG("No sense data\n");
} else {
WLOG("Sense KCQ: %02X %02X %02X\n", sense[2] & 0x0f, sense[12], sense[13]);
}
}
/**
* Just send a buffer on an endpoint, no questions asked.
* Handles repeats, and time outs. Also handles reading status reports and sense
* @param handle libusb device *
* @param endpoint_out sends
* @param endpoint_in used to read status reports back in
* @param cbuf what to send
* @param length how much to send
* @return number of bytes actually sent, or -1 for failures.
*/
int send_usb_data_only(libusb_device_handle *handle, unsigned char endpoint_out,
unsigned char endpoint_in, unsigned char *cbuf, unsigned int length) {
int ret;
int real_transferred;
int try = 0;
do {
ret = libusb_bulk_transfer(handle, endpoint_out, cbuf, length,
&real_transferred, SG_TIMEOUT_MSEC);
if (ret == LIBUSB_ERROR_PIPE) {
libusb_clear_halt(handle, endpoint_out);
}
try++;
} while ((ret == LIBUSB_ERROR_PIPE) && (try < 3));
if (ret != LIBUSB_SUCCESS) {
WLOG("sending failed: %d\n", ret);
return -1;
}
// now, swallow up the status, so that things behave nicely...
uint32_t received_tag;
// -ve is for my errors, 0 is good, +ve is libusb sense status bytes
int status = get_usb_mass_storage_status(handle, endpoint_in, &received_tag);
if (status < 0) {
WLOG("receiving status failed: %d\n", status);
return -1;
}
if (status != 0) {
WLOG("receiving status not passed :(: %02x\n", status);
}
if (status == 1) {
get_sense(handle, endpoint_in, endpoint_out);
return -1;
}
return real_transferred;
}
int stlink_q(stlink_t *sl) {
struct stlink_libsg* sg = sl->backend_data;
//uint8_t cdb_len = 6; // FIXME varies!!!
uint8_t cdb_len = 10; // FIXME varies!!!
uint8_t lun = 0; // always zero...
uint32_t tag = send_usb_mass_storage_command(sg->usb_handle, sg->ep_req,
sg->cdb_cmd_blk, cdb_len, lun, LIBUSB_ENDPOINT_IN, sl->q_len);
// now wait for our response...
// length copied from stlink-usb...
int rx_length = sl->q_len;
int try = 0;
int real_transferred;
int ret;
if (rx_length > 0) {
do {
ret = libusb_bulk_transfer(sg->usb_handle, sg->ep_rep, sl->q_buf, rx_length,
&real_transferred, SG_TIMEOUT_MSEC);
if (ret == LIBUSB_ERROR_PIPE) {
libusb_clear_halt(sg->usb_handle, sg->ep_req);
}
try++;
} while ((ret == LIBUSB_ERROR_PIPE) && (try < 3));
if (ret != LIBUSB_SUCCESS) {
WLOG("Receiving failed: %d\n", ret);
return -1;
}
if (real_transferred != rx_length) {
WLOG("received unexpected amount: %d != %d\n", real_transferred, rx_length);
}
}
uint32_t received_tag;
// -ve is for my errors, 0 is good, +ve is libusb sense status bytes
int status = get_usb_mass_storage_status(sg->usb_handle, sg->ep_rep, &received_tag);
if (status < 0) {
WLOG("receiving status failed: %d\n", status);
return -1;
}
if (status != 0) {
WLOG("receiving status not passed :(: %02x\n", status);
}
if (status == 1) {
get_sense(sg->usb_handle, sg->ep_rep, sg->ep_req);
return -1;
}
if (received_tag != tag) {
WLOG("received tag %d but expected %d\n", received_tag, tag);
//return -1;
}
if (rx_length > 0 && real_transferred != rx_length) {
return -1;
}
return 0;
}
// TODO thinking, cleanup
void stlink_stat(stlink_t *stl, char *txt) {
if (stl->q_len <= 0)
return;
stlink_print_data(stl);
switch (stl->q_buf[0]) {
case STLINK_OK:
DLOG(" %s: ok\n", txt);
return;
case STLINK_FALSE:
DLOG(" %s: false\n", txt);
return;
default:
DLOG(" %s: unknown\n", txt);
}
}
int _stlink_sg_version(stlink_t *stl) {
struct stlink_libsg *sl = stl->backend_data;
clear_cdb(sl);
sl->cdb_cmd_blk[0] = STLINK_GET_VERSION;
stl->q_len = 6;
sl->q_addr = 0;
return stlink_q(stl);
}
// Get stlink mode:
// STLINK_DEV_DFU_MODE || STLINK_DEV_MASS_MODE || STLINK_DEV_DEBUG_MODE
// usb dfu || usb mass || jtag or swd
int _stlink_sg_current_mode(stlink_t *stl) {
struct stlink_libsg *sl = stl->backend_data;
clear_cdb(sl);
sl->cdb_cmd_blk[0] = STLINK_GET_CURRENT_MODE;
stl->q_len = 2;
sl->q_addr = 0;
if (stlink_q(stl))
return -1;
return stl->q_buf[0];
}
// Exit the mass mode and enter the swd debug mode.
int _stlink_sg_enter_swd_mode(stlink_t *sl) {
struct stlink_libsg *sg = sl->backend_data;
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_DEBUG_ENTER;
sg->cdb_cmd_blk[2] = STLINK_DEBUG_ENTER_SWD;
sl->q_len = 0; // >0 -> aboard
return stlink_q(sl);
}
// Exit the mass mode and enter the jtag debug mode.
// (jtag is disabled in the discovery's stlink firmware)
int _stlink_sg_enter_jtag_mode(stlink_t *sl) {
struct stlink_libsg *sg = sl->backend_data;
DLOG("\n*** stlink_enter_jtag_mode ***\n");
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_DEBUG_ENTER;
sg->cdb_cmd_blk[2] = STLINK_DEBUG_ENTER_JTAG;
sl->q_len = 0;
return stlink_q(sl);
}
// XXX kernel driver performs reset, the device temporally disappears
// Suspect this is no longer the case when we have ignore on? RECHECK
int _stlink_sg_exit_dfu_mode(stlink_t *sl) {
struct stlink_libsg *sg = sl->backend_data;
DLOG("\n*** stlink_exit_dfu_mode ***\n");
clear_cdb(sg);
sg->cdb_cmd_blk[0] = STLINK_DFU_COMMAND;
sg->cdb_cmd_blk[1] = STLINK_DFU_EXIT;
sl->q_len = 0; // ??
return stlink_q(sl);
/*
[135121.844564] sd 19:0:0:0: [sdb] Unhandled error code
[135121.844569] sd 19:0:0:0: [sdb] Result: hostbyte=DID_ERROR driverbyte=DRIVER_OK
[135121.844574] sd 19:0:0:0: [sdb] CDB: Read(10): 28 00 00 00 10 00 00 00 08 00
[135121.844584] end_request: I/O error, dev sdb, sector 4096
[135121.844590] Buffer I/O error on device sdb, logical block 512
[135130.122567] usb 6-1: reset full speed USB device using uhci_hcd and address 7
[135130.274551] usb 6-1: device firmware changed
[135130.274618] usb 6-1: USB disconnect, address 7
[135130.275186] VFS: busy inodes on changed media or resized disk sdb
[135130.275424] VFS: busy inodes on changed media or resized disk sdb
[135130.286758] VFS: busy inodes on changed media or resized disk sdb
[135130.292796] VFS: busy inodes on changed media or resized disk sdb
[135130.301481] VFS: busy inodes on changed media or resized disk sdb
[135130.304316] VFS: busy inodes on changed media or resized disk sdb
[135130.431113] usb 6-1: new full speed USB device using uhci_hcd and address 8
[135130.629444] usb-storage 6-1:1.0: Quirks match for vid 0483 pid 3744: 102a1
[135130.629492] scsi20 : usb-storage 6-1:1.0
[135131.625600] scsi 20:0:0:0: Direct-Access STM32 PQ: 0 ANSI: 0
[135131.627010] sd 20:0:0:0: Attached scsi generic sg2 type 0
[135131.633603] sd 20:0:0:0: [sdb] 64000 512-byte logical blocks: (32.7 MB/31.2 MiB)
[135131.633613] sd 20:0:0:0: [sdb] Assuming Write Enabled
[135131.633620] sd 20:0:0:0: [sdb] Assuming drive cache: write through
[135131.640584] sd 20:0:0:0: [sdb] Assuming Write Enabled
[135131.640592] sd 20:0:0:0: [sdb] Assuming drive cache: write through
[135131.640609] sdb:
[135131.652634] sd 20:0:0:0: [sdb] Assuming Write Enabled
[135131.652639] sd 20:0:0:0: [sdb] Assuming drive cache: write through
[135131.652645] sd 20:0:0:0: [sdb] Attached SCSI removable disk
[135131.671536] sd 20:0:0:0: [sdb] Result: hostbyte=DID_OK driverbyte=DRIVER_SENSE
[135131.671548] sd 20:0:0:0: [sdb] Sense Key : Illegal Request [current]
[135131.671553] sd 20:0:0:0: [sdb] Add. Sense: Logical block address out of range
[135131.671560] sd 20:0:0:0: [sdb] CDB: Read(10): 28 00 00 00 f9 80 00 00 08 00
[135131.671570] end_request: I/O error, dev sdb, sector 63872
[135131.671575] Buffer I/O error on device sdb, logical block 7984
[135131.678527] sd 20:0:0:0: [sdb] Result: hostbyte=DID_OK driverbyte=DRIVER_SENSE
[135131.678532] sd 20:0:0:0: [sdb] Sense Key : Illegal Request [current]
[135131.678537] sd 20:0:0:0: [sdb] Add. Sense: Logical block address out of range
[135131.678542] sd 20:0:0:0: [sdb] CDB: Read(10): 28 00 00 00 f9 80 00 00 08 00
[135131.678551] end_request: I/O error, dev sdb, sector 63872
...
[135131.853565] end_request: I/O error, dev sdb, sector 4096
*/
}
int _stlink_sg_core_id(stlink_t *sl) {
struct stlink_libsg *sg = sl->backend_data;
int ret;
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_DEBUG_READCOREID;
sl->q_len = 4;
sg->q_addr = 0;
ret = stlink_q(sl);
if (ret)
return ret;
sl->core_id = read_uint32(sl->q_buf, 0);
return 0;
}
// Arm-core reset -> halted state.
int _stlink_sg_reset(stlink_t *sl) {
struct stlink_libsg *sg = sl->backend_data;
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_DEBUG_RESETSYS;
sl->q_len = 2;
sg->q_addr = 0;
if (stlink_q(sl))
return -1;
stlink_stat(sl, "core reset");
return 0;
}
// Arm-core reset -> halted state.
int _stlink_sg_jtag_reset(stlink_t *sl, int value) {
struct stlink_libsg *sg = sl->backend_data;
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_JTAG_DRIVE_NRST;
sg->cdb_cmd_blk[2] = (value)?0:1;
sl->q_len = 3;
sg->q_addr = 2;
if (stlink_q(sl))
return -1;
stlink_stat(sl, "core reset");
return 0;
}
// Arm-core status: halted or running.
int _stlink_sg_status(stlink_t *sl) {
struct stlink_libsg *sg = sl->backend_data;
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_DEBUG_GETSTATUS;
sl->q_len = 2;
sg->q_addr = 0;
return stlink_q(sl);
}
// Force the core into the debug mode -> halted state.
int _stlink_sg_force_debug(stlink_t *sl) {
struct stlink_libsg *sg = sl->backend_data;
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_DEBUG_FORCEDEBUG;
sl->q_len = 2;
sg->q_addr = 0;
if (stlink_q(sl))
return -1;
stlink_stat(sl, "force debug");
return 0;
}
// Read all arm-core registers.
int _stlink_sg_read_all_regs(stlink_t *sl, reg *regp) {
struct stlink_libsg *sg = sl->backend_data;
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_DEBUG_READALLREGS;
sl->q_len = 84;
sg->q_addr = 0;
if (stlink_q(sl))
return -1;
stlink_print_data(sl);
// TODO - most of this should be re-extracted up....
// 0-3 | 4-7 | ... | 60-63 | 64-67 | 68-71 | 72-75 | 76-79 | 80-83
// r0 | r1 | ... | r15 | xpsr | main_sp | process_sp | rw | rw2
for (int i = 0; i < 16; i++) {
regp->r[i] = read_uint32(sl->q_buf, 4 * i);
if (sl->verbose > 1)
DLOG("r%2d = 0x%08x\n", i, regp->r[i]);
}
regp->xpsr = read_uint32(sl->q_buf, 64);
regp->main_sp = read_uint32(sl->q_buf, 68);
regp->process_sp = read_uint32(sl->q_buf, 72);
regp->rw = read_uint32(sl->q_buf, 76);
regp->rw2 = read_uint32(sl->q_buf, 80);
if (sl->verbose < 2)
return 0;
DLOG("xpsr = 0x%08x\n", regp->xpsr);
DLOG("main_sp = 0x%08x\n", regp->main_sp);
DLOG("process_sp = 0x%08x\n", regp->process_sp);
DLOG("rw = 0x%08x\n", regp->rw);
DLOG("rw2 = 0x%08x\n", regp->rw2);
return 0;
}
// Read an arm-core register, the index must be in the range 0..20.
// 0 | 1 | ... | 15 | 16 | 17 | 18 | 19 | 20
// r0 | r1 | ... | r15 | xpsr | main_sp | process_sp | rw | rw2
int _stlink_sg_read_reg(stlink_t *sl, int r_idx, reg *regp) {
struct stlink_libsg *sg = sl->backend_data;
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_DEBUG_READREG;
sg->cdb_cmd_blk[2] = r_idx;
sl->q_len = 4;
sg->q_addr = 0;
if (stlink_q(sl))
return -1;
// 0 | 1 | ... | 15 | 16 | 17 | 18 | 19 | 20
// 0-3 | 4-7 | ... | 60-63 | 64-67 | 68-71 | 72-75 | 76-79 | 80-83
// r0 | r1 | ... | r15 | xpsr | main_sp | process_sp | rw | rw2
stlink_print_data(sl);
uint32_t r = read_uint32(sl->q_buf, 0);
DLOG("r_idx (%2d) = 0x%08x\n", r_idx, r);
switch (r_idx) {
case 16:
regp->xpsr = r;
break;
case 17:
regp->main_sp = r;
break;
case 18:
regp->process_sp = r;
break;
case 19:
regp->rw = r; //XXX ?(primask, basemask etc.)
break;
case 20:
regp->rw2 = r; //XXX ?(primask, basemask etc.)
break;
default:
regp->r[r_idx] = r;
}
return 0;
}
// Write an arm-core register. Index:
// 0 | 1 | ... | 15 | 16 | 17 | 18 | 19 | 20
// r0 | r1 | ... | r15 | xpsr | main_sp | process_sp | rw | rw2
int _stlink_sg_write_reg(stlink_t *sl, uint32_t reg, int idx) {
struct stlink_libsg *sg = sl->backend_data;
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEREG;
// 2: reg index
// 3-6: reg content
sg->cdb_cmd_blk[2] = idx;
write_uint32(sg->cdb_cmd_blk + 3, reg);
sl->q_len = 2;
sg->q_addr = 0;
if (stlink_q(sl))
return -1;
stlink_stat(sl, "write reg");
return 0;
}
// Write a register of the debug module of the core.
// XXX ?(atomic writes)
// TODO test
void stlink_write_dreg(stlink_t *sl, uint32_t reg, uint32_t addr) {
struct stlink_libsg *sg = sl->backend_data;
DLOG("\n*** stlink_write_dreg ***\n");
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEDEBUGREG;
// 2-5: address of reg of the debug module
// 6-9: reg content
write_uint32(sg->cdb_cmd_blk + 2, addr);
write_uint32(sg->cdb_cmd_blk + 6, reg);
sl->q_len = 2;
sg->q_addr = addr;
stlink_q(sl);
stlink_stat(sl, "write debug reg");
}
// Force the core exit the debug mode.
int _stlink_sg_run(stlink_t *sl) {
struct stlink_libsg *sg = sl->backend_data;
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_DEBUG_RUNCORE;
sl->q_len = 2;
sg->q_addr = 0;
if (stlink_q(sl))
return -1;
stlink_stat(sl, "run core");
return 0;
}
// Step the arm-core.
int _stlink_sg_step(stlink_t *sl) {
struct stlink_libsg *sg = sl->backend_data;
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_DEBUG_STEPCORE;
sl->q_len = 2;
sg->q_addr = 0;
if (stlink_q(sl))
return -1;
stlink_stat(sl, "step core");
return 0;
}
// TODO test
// see Cortex-M3 Technical Reference Manual
// TODO make delegate!
void stlink_set_hw_bp(stlink_t *sl, int fp_nr, uint32_t addr, int fp) {
DLOG("\n*** stlink_set_hw_bp ***\n");
struct stlink_libsg *sg = sl->backend_data;
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_DEBUG_SETFP;
// 2:The number of the flash patch used to set the breakpoint
// 3-6: Address of the breakpoint (LSB)
// 7: FP_ALL (0x02) / FP_UPPER (0x01) / FP_LOWER (0x00)
sl->q_buf[2] = fp_nr;
write_uint32(sl->q_buf, addr);
sl->q_buf[7] = fp;
sl->q_len = 2;
stlink_q(sl);
stlink_stat(sl, "set flash breakpoint");
}
// TODO test
// TODO make delegate!
void stlink_clr_hw_bp(stlink_t *sl, int fp_nr) {
struct stlink_libsg *sg = sl->backend_data;
DLOG("\n*** stlink_clr_hw_bp ***\n");
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_DEBUG_CLEARFP;
sg->cdb_cmd_blk[2] = fp_nr;
sl->q_len = 2;
stlink_q(sl);
stlink_stat(sl, "clear flash breakpoint");
}
// Read a "len" bytes to the sl->q_buf from the memory, max 6kB (6144 bytes)
int _stlink_sg_read_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
struct stlink_libsg *sg = sl->backend_data;
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_DEBUG_READMEM_32BIT;
// 2-5: addr
// 6-7: len
write_uint32(sg->cdb_cmd_blk + 2, addr);
write_uint16(sg->cdb_cmd_blk + 6, len);
// data_in 0-0x40-len
// !!! len _and_ q_len must be max 6k,
// i.e. >1024 * 6 = 6144 -> aboard)
// !!! if len < q_len: 64*k, 1024*n, n=1..5 -> aboard
// (broken residue issue)
sl->q_len = len;
sg->q_addr = addr;
if (stlink_q(sl))
return -1;
stlink_print_data(sl);
return 0;
}
// Write a "len" bytes from the sl->q_buf to the memory, max 64 Bytes.
int _stlink_sg_write_mem8(stlink_t *sl, uint32_t addr, uint16_t len) {
struct stlink_libsg *sg = sl->backend_data;
int ret;
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEMEM_8BIT;
// 2-5: addr
// 6-7: len (>0x40 (64) -> aboard)
write_uint32(sg->cdb_cmd_blk + 2, addr);
write_uint16(sg->cdb_cmd_blk + 6, len);
// this sends the command...
ret = send_usb_mass_storage_command(sg->usb_handle,
sg->ep_req, sg->cdb_cmd_blk, CDB_SL, 0, 0, 0);
if (ret == -1)
return ret;
// This sends the data...
ret = send_usb_data_only(sg->usb_handle,
sg->ep_req, sg->ep_rep, sl->q_buf, len);
if (ret == -1)
return ret;
stlink_print_data(sl);
return 0;
}
// Write a "len" bytes from the sl->q_buf to the memory, max Q_BUF_LEN bytes.
int _stlink_sg_write_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
struct stlink_libsg *sg = sl->backend_data;
int ret;
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEMEM_32BIT;
// 2-5: addr
// 6-7: len "unlimited"
write_uint32(sg->cdb_cmd_blk + 2, addr);
write_uint16(sg->cdb_cmd_blk + 6, len);
// this sends the command...
ret = send_usb_mass_storage_command(sg->usb_handle,
sg->ep_req, sg->cdb_cmd_blk, CDB_SL, 0, 0, 0);
if (ret == -1)
return ret;
// This sends the data...
ret = send_usb_data_only(sg->usb_handle,
sg->ep_req, sg->ep_rep, sl->q_buf, len);
if (ret == -1)
return ret;
stlink_print_data(sl);
return 0;
}
// Write one DWORD data to memory
int _stlink_sg_write_debug32(stlink_t *sl, uint32_t addr, uint32_t data) {
struct stlink_libsg *sg = sl->backend_data;
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_JTAG_WRITEDEBUG_32BIT;
// 2-5: addr
write_uint32(sg->cdb_cmd_blk + 2, addr);
write_uint32(sg->cdb_cmd_blk + 6, data);
sl->q_len = 2;
return stlink_q(sl);
}
// Read one DWORD data from memory
int _stlink_sg_read_debug32(stlink_t *sl, uint32_t addr, uint32_t *data) {
struct stlink_libsg *sg = sl->backend_data;
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_JTAG_READDEBUG_32BIT;
// 2-5: addr
write_uint32(sg->cdb_cmd_blk + 2, addr);
sl->q_len = 8;
if (stlink_q(sl))
return -1;
*data = read_uint32(sl->q_buf, 4);
return 0;
}
// Exit the jtag or swd mode and enter the mass mode.
int _stlink_sg_exit_debug_mode(stlink_t *stl)
{
if (stl) {
struct stlink_libsg* sl = stl->backend_data;
clear_cdb(sl);
sl->cdb_cmd_blk[1] = STLINK_DEBUG_EXIT;
stl->q_len = 0; // >0 -> aboard
return stlink_q(stl);
}
return 0;
}
// 1) open a sg device, switch the stlink from dfu to mass mode
// 2) wait 5s until the kernel driver stops reseting the broken device
// 3) reopen the device
// 4) the device driver is now ready for a switch to jtag/swd mode
// TODO thinking, better error handling, wait until the kernel driver stops reseting the plugged-in device
stlink_backend_t _stlink_sg_backend = {
_stlink_sg_close,
_stlink_sg_exit_debug_mode,
_stlink_sg_enter_swd_mode,
_stlink_sg_enter_jtag_mode,
_stlink_sg_exit_dfu_mode,
_stlink_sg_core_id,
_stlink_sg_reset,
_stlink_sg_jtag_reset,
_stlink_sg_run,
_stlink_sg_status,
_stlink_sg_version,
_stlink_sg_read_debug32,
_stlink_sg_read_mem32,
_stlink_sg_write_debug32,
_stlink_sg_write_mem32,
_stlink_sg_write_mem8,
_stlink_sg_read_all_regs,
_stlink_sg_read_reg,
NULL, /* read_all_unsupported_regs */
NULL, /* read_unsupported_regs */
NULL, /* write_unsupported_regs */
_stlink_sg_write_reg,
_stlink_sg_step,
_stlink_sg_current_mode,
_stlink_sg_force_debug,
NULL
};
static stlink_t* stlink_open(const int verbose) {
stlink_t *sl = malloc(sizeof (stlink_t));
memset(sl, 0, sizeof(stlink_t));
struct stlink_libsg *slsg = malloc(sizeof (struct stlink_libsg));
if (sl == NULL || slsg == NULL) {
WLOG("Couldn't malloc stlink and stlink_sg structures out of memory!\n");
return NULL;
}
if (libusb_init(&(slsg->libusb_ctx))) {
WLOG("failed to init libusb context, wrong version of libraries?\n");
free(sl);
free(slsg);
return NULL;
}
libusb_set_debug(slsg->libusb_ctx, 3);
slsg->usb_handle = libusb_open_device_with_vid_pid(slsg->libusb_ctx, USB_ST_VID, USB_STLINK_PID);
if (slsg->usb_handle == NULL) {
WLOG("Failed to find an stlink v1 by VID:PID\n");
libusb_close(slsg->usb_handle);
libusb_exit(slsg->libusb_ctx);
free(sl);
free(slsg);
return NULL;
}
// TODO
// Could read the interface config descriptor, and assert lots of the assumptions
// assumption: numInterfaces is always 1...
if (libusb_kernel_driver_active(slsg->usb_handle, 0) == 1) {
int r = libusb_detach_kernel_driver(slsg->usb_handle, 0);
if (r < 0) {
WLOG("libusb_detach_kernel_driver(() error %s\n", strerror(-r));
libusb_close(slsg->usb_handle);
libusb_exit(slsg->libusb_ctx);
free(sl);
free(slsg);
return NULL;
}
DLOG("Kernel driver was successfully detached\n");
}
int config;
if (libusb_get_configuration(slsg->usb_handle, &config)) {
/* this may fail for a previous configured device */
WLOG("libusb_get_configuration()\n");
libusb_close(slsg->usb_handle);
libusb_exit(slsg->libusb_ctx);
free(sl);
free(slsg);
return NULL;
}
// assumption: bConfigurationValue is always 1
if (config != 1) {
WLOG("Your stlink got into a real weird configuration, trying to fix it!\n");
DLOG("setting new configuration (%d -> 1)\n", config);
if (libusb_set_configuration(slsg->usb_handle, 1)) {
/* this may fail for a previous configured device */
WLOG("libusb_set_configuration() failed\n");
libusb_close(slsg->usb_handle);
libusb_exit(slsg->libusb_ctx);
free(sl);
free(slsg);
return NULL;
}
}
if (libusb_claim_interface(slsg->usb_handle, 0)) {
WLOG("libusb_claim_interface() failed\n");
libusb_close(slsg->usb_handle);
libusb_exit(slsg->libusb_ctx);
free(sl);
free(slsg);
return NULL;
}
// assumption: endpoint config is fixed mang. really.
slsg->ep_rep = 1 /* ep rep */ | LIBUSB_ENDPOINT_IN;
slsg->ep_req = 2 /* ep req */ | LIBUSB_ENDPOINT_OUT;
DLOG("Successfully opened stlinkv1 by libusb :)\n");
sl->verbose = verbose;
sl->backend_data = slsg;
sl->backend = &_stlink_sg_backend;
sl->core_stat = STLINK_CORE_STAT_UNKNOWN;
slsg->q_addr = 0;
return sl;
}
vba_project_t *
cli_vba_readdir(const char *dir, struct uniq *U, uint32_t which)
{
unsigned char *buf;
const unsigned char vba56_signature[] = { 0xcc, 0x61 };
uint16_t record_count, buflen, ffff, byte_count;
uint32_t offset;
int i, j, fd, big_endian = FALSE;
vba_project_t *vba_project;
struct vba56_header v56h;
off_t seekback;
char fullname[1024], *hash;
cli_dbgmsg("in cli_vba_readdir()\n");
if(dir == NULL)
return NULL;
/*
* _VBA_PROJECT files are embedded within office documents (OLE2)
*/
if (!uniq_get(U, "_vba_project", 12, &hash))
return NULL;
snprintf(fullname, sizeof(fullname), "%s"PATHSEP"%s_%u", dir, hash, which);
fullname[sizeof(fullname)-1] = '\0';
fd = open(fullname, O_RDONLY|O_BINARY);
if(fd == -1)
return NULL;
if(cli_readn(fd, &v56h, sizeof(struct vba56_header)) != sizeof(struct vba56_header)) {
close(fd);
return NULL;
}
if (memcmp(v56h.magic, vba56_signature, sizeof(v56h.magic)) != 0) {
close(fd);
return NULL;
}
i = vba_read_project_strings(fd, TRUE);
if ((seekback = lseek(fd, 0, SEEK_CUR)) == -1) {
cli_dbgmsg("vba_readdir: lseek() failed. Unable to guess VBA type\n");
close(fd);
return NULL;
}
if (lseek(fd, sizeof(struct vba56_header), SEEK_SET) == -1) {
cli_dbgmsg("vba_readdir: lseek() failed. Unable to guess VBA type\n");
close(fd);
return NULL;
}
j = vba_read_project_strings(fd, FALSE);
if(!i && !j) {
close(fd);
cli_dbgmsg("vba_readdir: Unable to guess VBA type\n");
return NULL;
}
if (i > j) {
big_endian = TRUE;
if (lseek(fd, seekback, SEEK_SET) == -1) {
cli_dbgmsg("vba_readdir: call to lseek() while guessing big-endian has failed\n");
close(fd);
return NULL;
}
cli_dbgmsg("vba_readdir: Guessing big-endian\n");
} else {
cli_dbgmsg("vba_readdir: Guessing little-endian\n");
}
/* junk some more stuff */
do
if (cli_readn(fd, &ffff, 2) != 2) {
close(fd);
return NULL;
}
while(ffff != 0xFFFF);
/* check for alignment error */
if(!seekandread(fd, -3, SEEK_CUR, &ffff, sizeof(uint16_t))) {
close(fd);
return NULL;
}
if (ffff != 0xFFFF) {
if (lseek(fd, 1, SEEK_CUR) == -1) {
cli_dbgmsg("call to lseek() while checking alignment error has failed\n");
close(fd);
return NULL;
}
}
if(!read_uint16(fd, &ffff, big_endian)) {
close(fd);
return NULL;
}
if(ffff != 0xFFFF) {
if (lseek(fd, ffff, SEEK_CUR) == -1) {
cli_dbgmsg("call to lseek() while checking alignment error has failed\n");
close(fd);
return NULL;
}
}
if(!read_uint16(fd, &ffff, big_endian)) {
close(fd);
return NULL;
}
if(ffff == 0xFFFF)
ffff = 0;
if (lseek(fd, ffff + 100, SEEK_CUR) == -1) {
cli_dbgmsg("call to lseek() failed\n");
close(fd);
return NULL;
}
if(!read_uint16(fd, &record_count, big_endian)) {
close(fd);
return NULL;
}
cli_dbgmsg("vba_readdir: VBA Record count %d\n", record_count);
if (record_count == 0) {
/* No macros, assume clean */
close(fd);
return NULL;
}
if (record_count > MAX_VBA_COUNT) {
/* Almost certainly an error */
cli_dbgmsg("vba_readdir: VBA Record count too big\n");
close(fd);
return NULL;
}
vba_project = create_vba_project(record_count, dir, U);
if(vba_project == NULL) {
close(fd);
return NULL;
}
buf = NULL;
buflen = 0;
for(i = 0; i < record_count; i++) {
uint16_t length;
char *ptr;
vba_project->colls[i] = 0;
if(!read_uint16(fd, &length, big_endian))
break;
if (length == 0) {
cli_dbgmsg("vba_readdir: zero name length\n");
break;
}
if(length > buflen) {
unsigned char *newbuf = (unsigned char *)cli_realloc(buf, length);
if(newbuf == NULL)
break;
buflen = length;
buf = newbuf;
}
if (cli_readn(fd, buf, length) != length) {
cli_dbgmsg("vba_readdir: read name failed\n");
break;
}
ptr = get_unicode_name((const char *)buf, length, big_endian);
if(ptr == NULL) break;
if (!(vba_project->colls[i]=uniq_get(U, ptr, strlen(ptr), &hash))) {
cli_dbgmsg("vba_readdir: cannot find project %s (%s)\n", ptr, hash);
free(ptr);
break;
}
cli_dbgmsg("vba_readdir: project name: %s (%s)\n", ptr, hash);
free(ptr);
vba_project->name[i] = hash;
if(!read_uint16(fd, &length, big_endian))
break;
lseek(fd, length, SEEK_CUR);
if(!read_uint16(fd, &ffff, big_endian))
break;
if (ffff == 0xFFFF) {
lseek(fd, 2, SEEK_CUR);
if(!read_uint16(fd, &ffff, big_endian))
break;
lseek(fd, ffff + 8, SEEK_CUR);
} else
lseek(fd, ffff + 10, SEEK_CUR);
if(!read_uint16(fd, &byte_count, big_endian))
break;
lseek(fd, (8 * byte_count) + 5, SEEK_CUR);
if(!read_uint32(fd, &offset, big_endian))
break;
cli_dbgmsg("vba_readdir: offset: %u\n", (unsigned int)offset);
vba_project->offset[i] = offset;
lseek(fd, 2, SEEK_CUR);
}
if(buf)
free(buf);
close(fd);
if(i < record_count) {
free(vba_project->name);
free(vba_project->colls);
free(vba_project->dir);
free(vba_project->offset);
free(vba_project);
return NULL;
}
return vba_project;
}
address read_v4_address(InIt& in)
{
unsigned long ip = read_uint32(in);
return address_v4(ip);
}
static int dmg_open(BlockDriverState *bs, int flags)
{
BDRVDMGState *s = bs->opaque;
uint64_t info_begin,info_end,last_in_offset,last_out_offset;
uint32_t count, tmp;
uint32_t max_compressed_size=1,max_sectors_per_chunk=1,i;
int64_t offset;
int ret;
bs->read_only = 1;
s->n_chunks = 0;
s->offsets = s->lengths = s->sectors = s->sectorcounts = NULL;
/* read offset of info blocks */
offset = bdrv_getlength(bs->file);
if (offset < 0) {
ret = offset;
goto fail;
}
offset -= 0x1d8;
ret = read_uint64(bs, offset, &info_begin);
if (ret < 0) {
goto fail;
} else if (info_begin == 0) {
ret = -EINVAL;
goto fail;
}
ret = read_uint32(bs, info_begin, &tmp);
if (ret < 0) {
goto fail;
} else if (tmp != 0x100) {
ret = -EINVAL;
goto fail;
}
ret = read_uint32(bs, info_begin + 4, &count);
if (ret < 0) {
goto fail;
} else if (count == 0) {
ret = -EINVAL;
goto fail;
}
info_end = info_begin + count;
offset = info_begin + 0x100;
/* read offsets */
last_in_offset = last_out_offset = 0;
while (offset < info_end) {
uint32_t type;
ret = read_uint32(bs, offset, &count);
if (ret < 0) {
goto fail;
} else if (count == 0) {
ret = -EINVAL;
goto fail;
}
offset += 4;
ret = read_uint32(bs, offset, &type);
if (ret < 0) {
goto fail;
}
if (type == 0x6d697368 && count >= 244) {
int new_size, chunk_count;
offset += 4;
offset += 200;
chunk_count = (count-204)/40;
new_size = sizeof(uint64_t) * (s->n_chunks + chunk_count);
s->types = g_realloc(s->types, new_size/2);
s->offsets = g_realloc(s->offsets, new_size);
s->lengths = g_realloc(s->lengths, new_size);
s->sectors = g_realloc(s->sectors, new_size);
s->sectorcounts = g_realloc(s->sectorcounts, new_size);
for (i = s->n_chunks; i < s->n_chunks + chunk_count; i++) {
ret = read_uint32(bs, offset, &s->types[i]);
if (ret < 0) {
goto fail;
}
offset += 4;
if(s->types[i]!=0x80000005 && s->types[i]!=1 && s->types[i]!=2) {
if(s->types[i]==0xffffffff) {
last_in_offset = s->offsets[i-1]+s->lengths[i-1];
last_out_offset = s->sectors[i-1]+s->sectorcounts[i-1];
}
chunk_count--;
i--;
offset += 36;
continue;
}
offset += 4;
ret = read_uint64(bs, offset, &s->sectors[i]);
if (ret < 0) {
goto fail;
}
s->sectors[i] += last_out_offset;
offset += 8;
ret = read_uint64(bs, offset, &s->sectorcounts[i]);
if (ret < 0) {
goto fail;
}
offset += 8;
ret = read_uint64(bs, offset, &s->offsets[i]);
if (ret < 0) {
goto fail;
}
s->offsets[i] += last_in_offset;
offset += 8;
ret = read_uint64(bs, offset, &s->lengths[i]);
if (ret < 0) {
goto fail;
}
offset += 8;
if(s->lengths[i]>max_compressed_size)
max_compressed_size = s->lengths[i];
if(s->sectorcounts[i]>max_sectors_per_chunk)
max_sectors_per_chunk = s->sectorcounts[i];
}
s->n_chunks+=chunk_count;
}
}
/* initialize zlib engine */
s->compressed_chunk = g_malloc(max_compressed_size+1);
s->uncompressed_chunk = g_malloc(512*max_sectors_per_chunk);
if(inflateInit(&s->zstream) != Z_OK) {
ret = -EINVAL;
goto fail;
}
s->current_chunk = s->n_chunks;
qemu_co_mutex_init(&s->lock);
return 0;
fail:
g_free(s->types);
g_free(s->offsets);
g_free(s->lengths);
g_free(s->sectors);
g_free(s->sectorcounts);
g_free(s->compressed_chunk);
g_free(s->uncompressed_chunk);
return ret;
}
static uint8_t * parse_msgc_tunnel_service_add(uint8_t *message_start, uint8_t *message_end, int minor, size_t *size, message_destructor_t *free_message)
{
SPICE_GNUC_UNUSED uint8_t *pos;
uint8_t *start = message_start;
uint8_t *data = NULL;
size_t mem_size, nw_size;
uint8_t *in, *end;
SPICE_GNUC_UNUSED intptr_t ptr_size;
uint32_t n_ptr=0;
PointerInfo ptr_info[2];
size_t name__extra_size;
size_t description__extra_size;
size_t u__nw_size;
uint16_t type__value;
SpiceMsgcTunnelAddGenericService *out;
uint32_t i;
{ /* name */
uint32_t name__value;
uint32_t name__array__nw_size;
uint32_t name__array__mem_size;
pos = (start + 14);
if (SPICE_UNLIKELY(pos + 4 > message_end)) {
goto error;
}
name__value = read_uint32(pos);
if (SPICE_UNLIKELY(message_start + name__value >= message_end)) {
goto error;
}
name__array__nw_size = spice_strnlen((char *)message_start + name__value, message_end - (message_start + name__value));
if (SPICE_UNLIKELY(*(message_start + name__value + name__array__nw_size) != 0)) {
goto error;
}
name__array__mem_size = name__array__nw_size;
/* @nocopy, so no extra size */
name__extra_size = 0;
}
{ /* description */
uint32_t description__value;
uint32_t description__array__nw_size;
uint32_t description__array__mem_size;
pos = (start + 18);
if (SPICE_UNLIKELY(pos + 4 > message_end)) {
goto error;
}
description__value = read_uint32(pos);
if (SPICE_UNLIKELY(message_start + description__value >= message_end)) {
goto error;
}
description__array__nw_size = spice_strnlen((char *)message_start + description__value, message_end - (message_start + description__value));
if (SPICE_UNLIKELY(*(message_start + description__value + description__array__nw_size) != 0)) {
goto error;
}
description__array__mem_size = description__array__nw_size;
/* @nocopy, so no extra size */
description__extra_size = 0;
}
{ /* u */
pos = start + 0;
if (SPICE_UNLIKELY(pos + 2 > message_end)) {
goto error;
}
type__value = read_uint16(pos);
if (type__value == SPICE_TUNNEL_SERVICE_TYPE_IPP) {
SPICE_GNUC_UNUSED uint8_t *start2 = (start + 22);
size_t u__nw_size;
uint16_t type__value;
{ /* u */
uint32_t u__nelements;
pos = start2 + 0;
if (SPICE_UNLIKELY(pos + 2 > message_end)) {
goto error;
}
type__value = read_uint16(pos);
if (type__value == SPICE_TUNNEL_IP_TYPE_IPv4) {
u__nelements = 4;
u__nw_size = u__nelements;
} else {
u__nw_size = 0;
}
}
u__nw_size = 2 + u__nw_size;
} else {
u__nw_size = 0;
}
}
nw_size = 22 + u__nw_size;
mem_size = sizeof(SpiceMsgcTunnelAddGenericService) + name__extra_size + description__extra_size;
/* Check if message fits in reported side */
if (start + nw_size > message_end) {
return NULL;
}
/* Validated extents and calculated size */
data = (uint8_t *)malloc(mem_size);
if (SPICE_UNLIKELY(data == NULL)) {
goto error;
}
end = data + sizeof(SpiceMsgcTunnelAddGenericService);
in = start;
out = (SpiceMsgcTunnelAddGenericService *)data;
out->type = consume_uint16(&in);
out->id = consume_uint32(&in);
out->group = consume_uint32(&in);
out->port = consume_uint32(&in);
/* Reuse data from network message */
out->name = (size_t)(message_start + consume_uint32(&in));
/* Reuse data from network message */
out->description = (size_t)(message_start + consume_uint32(&in));
if (out->type == SPICE_TUNNEL_SERVICE_TYPE_IPP) {
out->u.ip.type = consume_uint16(&in);
if (out->u.ip.type == SPICE_TUNNEL_IP_TYPE_IPv4) {
uint32_t ipv4__nelements;
ipv4__nelements = 4;
memcpy(out->u.ip.u.ipv4, in, ipv4__nelements);
in += ipv4__nelements;
}
}
assert(in <= message_end);
for (i = 0; i < n_ptr; i++) {
if (ptr_info[i].offset == 0) {
*ptr_info[i].dest = NULL;
} else {
/* Align to 32 bit */
end = (uint8_t *)SPICE_ALIGN((size_t)end, 4);
*ptr_info[i].dest = (void *)end;
end = ptr_info[i].parse(message_start, message_end, end, &ptr_info[i], minor);
if (SPICE_UNLIKELY(end == NULL)) {
goto error;
}
}
}
assert(end <= data + mem_size);
*size = end - data;
*free_message = (message_destructor_t) free;
return data;
error:
if (data != NULL) {
free(data);
}
return NULL;
}
