void t2_putcommacode(int n)
{
while (--n>=0) {
bio_write(1, 1);
}
bio_write(0, 1);
}
static void t2_putcommacode(opj_bio_t *bio, int n)
{
while (--n >= 0) {
bio_write(bio, 1, 1);
}
bio_write(bio, 0, 1);
}
static void t2_putcommacode(opj_bio_t *bio, OPJ_UINT32 n) {
while
(--n != -1)
{
bio_write(bio, 1, 1);
}
bio_write(bio, 0, 1);
}
uint8_t sclass_store(bio *fd) {
uint8_t databuff[10+MAXSCLASSNLENG+3*(1+9*4*MASKORGROUP)];
uint8_t *ptr;
uint16_t i,j,k;
int32_t wsize;
if (fd==NULL) {
return 0x16;
}
ptr = databuff;
put8bit(&ptr,MASKORGROUP);
if (bio_write(fd,databuff,1)!=1) {
syslog(LOG_NOTICE,"write error");
return 0xFF;
}
for (i=1 ; i<firstneverused ; i++) {
if ((sclasstab[i].nleng)>0) {
ptr = databuff;
put16bit(&ptr,i);
put8bit(&ptr,sclasstab[i].nleng);
put8bit(&ptr,sclasstab[i].admin_only);
put8bit(&ptr,sclasstab[i].create_mode);
put16bit(&ptr,sclasstab[i].arch_delay);
put8bit(&ptr,sclasstab[i].create_labelscnt);
put8bit(&ptr,sclasstab[i].keep_labelscnt);
put8bit(&ptr,sclasstab[i].arch_labelscnt);
memcpy(ptr,sclasstab[i].name,sclasstab[i].nleng);
ptr+=sclasstab[i].nleng;
for (j=0 ; j<sclasstab[i].create_labelscnt ; j++) {
for (k=0 ; k<MASKORGROUP ; k++) {
put32bit(&ptr,sclasstab[i].create_labelmasks[j][k]);
}
}
for (j=0 ; j<sclasstab[i].keep_labelscnt ; j++) {
for (k=0 ; k<MASKORGROUP ; k++) {
put32bit(&ptr,sclasstab[i].keep_labelmasks[j][k]);
}
}
for (j=0 ; j<sclasstab[i].arch_labelscnt ; j++) {
for (k=0 ; k<MASKORGROUP ; k++) {
put32bit(&ptr,sclasstab[i].arch_labelmasks[j][k]);
}
}
wsize = 10+sclasstab[i].nleng+(sclasstab[i].create_labelscnt+sclasstab[i].keep_labelscnt+sclasstab[i].arch_labelscnt)*4*MASKORGROUP;
if (bio_write(fd,databuff,wsize)!=wsize) {
syslog(LOG_NOTICE,"write error");
return 0xFF;
}
}
}
memset(databuff,0,10);
if (bio_write(fd,databuff,10)!=10) {
syslog(LOG_NOTICE,"write error");
return 0xFF;
}
return 0;
}
static void t2_putnumpasses(opj_bio_t *bio, int n) {
if (n == 1) {
bio_write(bio, 0, 1);
} else if (n == 2) {
bio_write(bio, 2, 2);
} else if (n <= 5) {
bio_write(bio, 0xc | (n - 3), 4);
} else if (n <= 36) {
bio_write(bio, 0x1e0 | (n - 6), 9);
} else if (n <= 164) {
bio_write(bio, 0xff80 | (n - 37), 16);
}
}
void t2_putnumpasses(int n)
{
if (n==1) {
bio_write(0, 1);
} else if (n==2) {
bio_write(2, 2);
} else if (n<=5) {
bio_write(0xc|(n-3), 4);
} else if (n<=36) {
bio_write(0x1e0|(n-6), 9);
} else if (n<=164) {
bio_write(0xff80|(n-37), 16);
}
}
static s32_t devfs_write(struct vnode * node , struct file * fp, void * buf, loff_t size, loff_t * result)
{
struct device * dev = (struct device *)(node->v_data);
struct chrdev * chr;
struct blkdev * blk;
loff_t len;
*result = 0;
if(node->v_type == VDIR)
return EISDIR;
if(node->v_type == VCHR)
{
chr = (struct chrdev *)(dev->priv);
len = chr->write(chr, buf, size);
fp->f_offset = 0;
*result = len;
return 0;
}
else if(node->v_type == VBLK)
{
blk = (struct blkdev *)(dev->priv);
len = bio_write(blk, buf, fp->f_offset, size);
fp->f_offset += len;
*result = len;
return 0;
}
return -1;
}
/* This function is blocking and it writes from the socket exactly 'count'
* number of bytes
*/
int ipc_write(int fd, void *buf, size_t count)
{
int rc;
if ((rc = bio_write(fd, buf, count)))
rg_error_f(LERR, "%d: failed write() %m", fd);
return rc;
}
/* </summary> */
void tgt_encode(tgt_tree_t * tree, int leafno, int threshold)
{
tgt_node_t *stk[31];
tgt_node_t **stkptr;
tgt_node_t *node;
int low;
stkptr = stk;
node = &tree->nodes[leafno];
while (node->parent) {
*stkptr++ = node;
node = node->parent;
}
low = 0;
for (;;) {
if (low > node->low) {
node->low = low;
} else {
low = node->low;
}
while (low < threshold) {
if (low >= node->value) {
if (!node->known) {
bio_write(1, 1);
node->known = 1;
}
break;
}
bio_write(0, 1);
++low;
}
node->low = low;
if (stkptr == stk)
break;
node = *--stkptr;
}
}
void tgt_encode(opj_bio_t *bio, opj_tgt_tree_t *tree, S32 leafno, S32 threshold) {
opj_tgt_node_t *stk[31];
opj_tgt_node_t **stkptr;
opj_tgt_node_t *node;
S32 low;
stkptr = stk;
node = &tree->nodes[leafno];
while (node->parent) {
*stkptr++ = node;
node = node->parent;
}
low = 0;
for (;;) {
if (low > node->low) {
node->low = low;
} else {
low = node->low;
}
while (low < threshold) {
if (low >= node->value) {
if (!node->known) {
bio_write(bio, 1, 1);
node->known = 1;
}
break;
}
bio_write(bio, 0, 1);
++low;
}
node->low = low;
if (stkptr == stk)
break;
node = *--stkptr;
}
}
int
key_state_write_plaintext_const (struct key_state_ssl *ks_ssl, const uint8_t *data, int len)
{
int ret = 0;
perf_push (PERF_BIO_WRITE_PLAINTEXT);
ASSERT (NULL != ks_ssl);
ret = bio_write (ks_ssl->ssl_bio, data, len, "tls_write_plaintext_const");
perf_pop ();
return ret;
}
int
key_state_write_ciphertext (struct key_state_ssl *ks_ssl, struct buffer *buf)
{
int ret = 0;
perf_push (PERF_BIO_WRITE_CIPHERTEXT);
ASSERT (NULL != ks_ssl);
ret = bio_write (ks_ssl->ct_in, BPTR(buf), BLEN(buf), "tls_write_ciphertext");
bio_write_post (ret, buf);
perf_pop ();
return ret;
}
uint8_t flock_store(bio *fd) {
uint8_t storebuff[FLOCK_REC_SIZE*FLOCK_STORE_BLOCK_CNT];
uint8_t *ptr;
uint32_t h,j;
inodelocks *il;
lock *l;
if (fd==NULL) {
return 0x10;
}
j = 0;
ptr = storebuff;
for (h=0 ; h<FLOCK_INODE_HASHSIZE ; h++) {
for (il = inodehash[h] ; il ; il=il->next) {
for (l=il->active ; l ; l=l->next) {
put32bit(&ptr,il->inode);
put64bit(&ptr,l->owner);
put32bit(&ptr,l->sessionid);
put8bit(&ptr,l->ltype);
j++;
if (j==FLOCK_STORE_BLOCK_CNT) {
if (bio_write(fd,storebuff,FLOCK_REC_SIZE*FLOCK_STORE_BLOCK_CNT)!=(FLOCK_REC_SIZE*FLOCK_STORE_BLOCK_CNT)) {
return 0xFF;
}
j = 0;
ptr = storebuff;
}
}
}
}
memset(ptr,0,FLOCK_REC_SIZE);
j++;
if (bio_write(fd,storebuff,FLOCK_REC_SIZE*j)!=(FLOCK_REC_SIZE*j)) {
return 0xFF;
}
return 0;
}
/**
* Write data buffer.
*
* @return amount of bytes written, or -1 on error.
*/
static ssize_t
tx_link_write(txdrv_t *tx, gconstpointer data, size_t len)
{
struct attr *attr = tx->opaque;
ssize_t r;
r = bio_write(attr->bio, data, len);
if ((ssize_t) -1 == r)
return tx_link_write_error(tx, "tx_link_write");
if (attr->cb->add_tx_written != NULL)
attr->cb->add_tx_written(tx->owner, r);
return r;
}
int
key_state_write_plaintext (struct key_state_ssl *ks_ssl, struct buffer *buf)
{
int ret = 0;
perf_push (PERF_BIO_WRITE_PLAINTEXT);
#ifdef ENABLE_CRYPTO_OPENSSL
ASSERT (NULL != ks_ssl);
ret = bio_write (ks_ssl->ssl_bio, BPTR(buf), BLEN(buf),
"tls_write_plaintext");
bio_write_post (ret, buf);
#endif /* ENABLE_CRYPTO_OPENSSL */
perf_pop ();
return ret;
}
// return NULL for success, error string for failure
int lkb_handle_command(lkb_t *lkb, const char *cmd, const char *arg, size_t len, const char **result)
{
*result = NULL;
struct lkb_command *lcmd;
for (lcmd = lkb_cmd_list; lcmd; lcmd = lcmd->next) {
if (!strcmp(lcmd->name, cmd)) {
*result = lcmd->handler(lkb, arg, len, lcmd->cookie);
return 0;
}
}
if (!strcmp(cmd, "flash") || !strcmp(cmd, "erase")) {
struct ptable_entry entry;
bdev_t *bdev;
if (ptable_find(arg, &entry) < 0) {
size_t plen = len;
/* doesn't exist, make one */
if (ptable_add(arg, plen, 0) < 0) {
*result = "error creating partition";
return -1;
}
if (ptable_find(arg, &entry) < 0) {
*result = "couldn't find partition after creating it";
return -1;
}
}
if (len > entry.length) {
*result = "partition too small";
return -1;
}
if (!(bdev = ptable_get_device())) {
*result = "ptable_get_device failed";
return -1;
}
printf("lkboot: erasing partition of size %llu\n", entry.length);
if (bio_erase(bdev, entry.offset, entry.length) != (ssize_t)entry.length) {
*result = "bio_erase failed";
return -1;
}
if (!strcmp(cmd, "flash")) {
printf("lkboot: writing to partition\n");
void *buf = malloc(bdev->block_size);
if (!buf) {
*result = "memory allocation failed";
return -1;
}
size_t pos = 0;
while (pos < len) {
size_t toread = MIN(len - pos, bdev->block_size);
LTRACEF("offset %zu, toread %zu\n", pos, toread);
if (lkb_read(lkb, buf, toread)) {
*result = "io error";
free(buf);
return -1;
}
if (bio_write(bdev, buf, entry.offset + pos, toread) != (ssize_t)toread) {
*result = "bio_write failed";
free(buf);
return -1;
}
pos += toread;
}
free(buf);
}
} else if (!strcmp(cmd, "remove")) {
if (ptable_remove(arg) < 0) {
*result = "remove failed";
return -1;
}
} else if (!strcmp(cmd, "fpga")) {
#if PLATFORM_ZYNQ
void *buf = malloc(len);
if (!buf) {
*result = "error allocating buffer";
return -1;
}
/* translate to physical address */
paddr_t pa = vaddr_to_paddr(buf);
if (pa == 0) {
*result = "error allocating buffer";
free(buf);
return -1;
}
if (lkb_read(lkb, buf, len)) {
*result = "io error";
free(buf);
return -1;
}
/* make sure the cache is flushed for this buffer for DMA coherency purposes */
arch_clean_cache_range((vaddr_t)buf, len);
/* program the fpga */
zynq_reset_fpga();
zynq_program_fpga(pa, len);
free(buf);
#else
*result = "no fpga";
return -1;
#endif
} else if (!strcmp(cmd, "boot")) {
return do_boot(lkb, len, result);
} else if (!strcmp(cmd, "getsysparam")) {
const void *ptr;
size_t len;
if (sysparam_get_ptr(arg, &ptr, &len) == 0) {
lkb_write(lkb, ptr, len);
}
} else if (!strcmp(cmd, "reboot")) {
thread_resume(thread_create("reboot", &do_reboot, NULL,
DEFAULT_PRIORITY, DEFAULT_STACK_SIZE));
} else {
*result = "unknown command";
return -1;
}
return 0;
}
static ssize_t subdev_write(struct bdev *_dev, const void *buf, off_t offset, size_t len)
{
subdev_t *subdev = (subdev_t *)_dev;
return bio_write(subdev->parent, buf, offset + subdev->offset * subdev->dev.block_size, len);
}
static int cmd_bio(int argc, const cmd_args *argv)
{
int rc = 0;
if (argc < 2) {
printf("not enough arguments:\n");
usage:
printf("%s list\n", argv[0].str);
printf("%s read <device> <address> <offset> <len>\n", argv[0].str);
printf("%s write <device> <address> <offset> <len>\n", argv[0].str);
printf("%s erase <device> <offset> <len>\n", argv[0].str);
printf("%s ioctl <device> <request> <arg>\n", argv[0].str);
printf("%s remove <device>\n", argv[0].str);
#if WITH_LIB_PARTITION
printf("%s partscan <device> [offset]\n", argv[0].str);
#endif
return -1;
}
if (!strcmp(argv[1].str, "list")) {
bio_dump_devices();
} else if (!strcmp(argv[1].str, "read")) {
if (argc < 6) {
printf("not enough arguments:\n");
goto usage;
}
addr_t address = argv[3].u;
off_t offset = argv[4].u; // XXX use long
size_t len = argv[5].u;
bdev_t *dev = bio_open(argv[2].str);
if (!dev) {
printf("error opening block device\n");
return -1;
}
time_t t = current_time();
ssize_t err = bio_read(dev, (void *)address, offset, len);
t = current_time() - t;
dprintf(INFO, "bio_read returns %d, took %u msecs (%d bytes/sec)\n", (int)err, (uint)t, (uint32_t)((uint64_t)err * 1000 / t));
bio_close(dev);
rc = err;
} else if (!strcmp(argv[1].str, "write")) {
if (argc < 6) {
printf("not enough arguments:\n");
goto usage;
}
addr_t address = argv[3].u;
off_t offset = argv[4].u; // XXX use long
size_t len = argv[5].u;
bdev_t *dev = bio_open(argv[2].str);
if (!dev) {
printf("error opening block device\n");
return -1;
}
time_t t = current_time();
ssize_t err = bio_write(dev, (void *)address, offset, len);
t = current_time() - t;
dprintf(INFO, "bio_write returns %d, took %u msecs (%d bytes/sec)\n", (int)err, (uint)t, (uint32_t)((uint64_t)err * 1000 / t));
bio_close(dev);
rc = err;
} else if (!strcmp(argv[1].str, "erase")) {
if (argc < 5) {
printf("not enough arguments:\n");
goto usage;
}
off_t offset = argv[3].u; // XXX use long
size_t len = argv[4].u;
bdev_t *dev = bio_open(argv[2].str);
if (!dev) {
printf("error opening block device\n");
return -1;
}
time_t t = current_time();
ssize_t err = bio_erase(dev, offset, len);
t = current_time() - t;
dprintf(INFO, "bio_erase returns %d, took %u msecs (%d bytes/sec)\n", (int)err, (uint)t, (uint32_t)((uint64_t)err * 1000 / t));
bio_close(dev);
rc = err;
} else if (!strcmp(argv[1].str, "ioctl")) {
if (argc < 4) {
printf("not enough arguments:\n");
goto usage;
}
int request = argv[3].u;
int arg = (argc == 5) ? argv[4].u : 0;
bdev_t *dev = bio_open(argv[2].str);
if (!dev) {
printf("error opening block device\n");
return -1;
}
int err = bio_ioctl(dev, request, (void *)arg);
dprintf(INFO, "bio_ioctl returns %d\n", err);
bio_close(dev);
rc = err;
} else if (!strcmp(argv[1].str, "remove")) {
if (argc < 3) {
printf("not enough arguments:\n");
goto usage;
}
bdev_t *dev = bio_open(argv[2].str);
if (!dev) {
printf("error opening block device\n");
return -1;
}
bio_unregister_device(dev);
bio_close(dev);
#if WITH_LIB_PARTITION
} else if (!strcmp(argv[1].str, "partscan")) {
if (argc < 3) {
printf("not enough arguments:\n");
goto usage;
}
off_t offset = 0;
if (argc > 3)
offset = argv[3].u;
rc = partition_publish(argv[2].str, offset);
dprintf(INFO, "partition_publish returns %d\n", rc);
#endif
} else {
printf("unrecognized subcommand\n");
goto usage;
}
return rc;
}
static int bio_puts(BIO *b, const char *str)
{
return bio_write(b, str, strlen(str));
}
static int t2_encode_packet(opj_tcd_tile_t * tile, opj_tcp_t * tcp, opj_pi_iterator_t *pi, unsigned char *dest, int length, opj_codestream_info_t *cstr_info, int tileno) {
int bandno, cblkno;
unsigned char *c = dest;
int compno = pi->compno; /* component value */
int resno = pi->resno; /* resolution level value */
int precno = pi->precno; /* precinct value */
int layno = pi->layno; /* quality layer value */
opj_tcd_tilecomp_t *tilec = &tile->comps[compno];
opj_tcd_resolution_t *res = &tilec->resolutions[resno];
opj_bio_t *bio = NULL; /* BIO component */
/* <SOP 0xff91> */
if (tcp->csty & J2K_CP_CSTY_SOP) {
c[0] = 255;
c[1] = 145;
c[2] = 0;
c[3] = 4;
c[4] = (unsigned char)((tile->packno % 65536) / 256);
c[5] = (unsigned char)((tile->packno % 65536) % 256);
c += 6;
}
/* </SOP> */
if (!layno) {
for (bandno = 0; bandno < res->numbands; bandno++) {
opj_tcd_band_t *band = &res->bands[bandno];
opj_tcd_precinct_t *prc = &band->precincts[precno];
tgt_reset(prc->incltree);
tgt_reset(prc->imsbtree);
for (cblkno = 0; cblkno < prc->cw * prc->ch; cblkno++) {
opj_tcd_cblk_enc_t* cblk = &prc->cblks.enc[cblkno];
cblk->numpasses = 0;
tgt_setvalue(prc->imsbtree, cblkno, band->numbps - cblk->numbps);
}
}
}
bio = bio_create();
bio_init_enc(bio, c, length);
bio_write(bio, 1, 1); /* Empty header bit */
/* Writing Packet header */
for (bandno = 0; bandno < res->numbands; bandno++) {
opj_tcd_band_t *band = &res->bands[bandno];
opj_tcd_precinct_t *prc = &band->precincts[precno];
for (cblkno = 0; cblkno < prc->cw * prc->ch; cblkno++) {
opj_tcd_cblk_enc_t* cblk = &prc->cblks.enc[cblkno];
opj_tcd_layer_t *layer = &cblk->layers[layno];
if (!cblk->numpasses && layer->numpasses) {
tgt_setvalue(prc->incltree, cblkno, layno);
}
}
for (cblkno = 0; cblkno < prc->cw * prc->ch; cblkno++) {
opj_tcd_cblk_enc_t* cblk = &prc->cblks.enc[cblkno];
opj_tcd_layer_t *layer = &cblk->layers[layno];
int increment = 0;
int nump = 0;
int len = 0, passno;
/* cblk inclusion bits */
if (!cblk->numpasses) {
tgt_encode(bio, prc->incltree, cblkno, layno + 1);
} else {
bio_write(bio, layer->numpasses != 0, 1);
}
/* if cblk not included, go to the next cblk */
if (!layer->numpasses) {
continue;
}
/* if first instance of cblk --> zero bit-planes information */
if (!cblk->numpasses) {
cblk->numlenbits = 3;
tgt_encode(bio, prc->imsbtree, cblkno, 999);
}
/* number of coding passes included */
t2_putnumpasses(bio, layer->numpasses);
/* computation of the increase of the length indicator and insertion in the header */
for (passno = cblk->numpasses; passno < cblk->numpasses + layer->numpasses; passno++) {
opj_tcd_pass_t *pass = &cblk->passes[passno];
nump++;
len += pass->len;
if (pass->term || passno == (cblk->numpasses + layer->numpasses) - 1) {
increment = int_max(increment, int_floorlog2(len) + 1 - (cblk->numlenbits + int_floorlog2(nump)));
len = 0;
nump = 0;
}
}
t2_putcommacode(bio, increment);
/* computation of the new Length indicator */
cblk->numlenbits += increment;
/* insertion of the codeword segment length */
for (passno = cblk->numpasses; passno < cblk->numpasses + layer->numpasses; passno++) {
opj_tcd_pass_t *pass = &cblk->passes[passno];
nump++;
len += pass->len;
if (pass->term || passno == (cblk->numpasses + layer->numpasses) - 1) {
bio_write(bio, len, cblk->numlenbits + int_floorlog2(nump));
len = 0;
nump = 0;
}
}
}
}
if (bio_flush(bio)) {
bio_destroy(bio);
return -999; /* modified to eliminate longjmp !! */
}
c += bio_numbytes(bio);
bio_destroy(bio);
/* <EPH 0xff92> */
if (tcp->csty & J2K_CP_CSTY_EPH) {
c[0] = 255;
c[1] = 146;
c += 2;
}
/* </EPH> */
/* << INDEX */
/* End of packet header position. Currently only represents the distance to start of packet
// Will be updated later by incrementing with packet start value */
if(cstr_info && cstr_info->index_write) {
opj_packet_info_t *info_PK = &cstr_info->tile[tileno].packet[cstr_info->packno];
info_PK->end_ph_pos = (int)(c - dest);
}
/* INDEX >> */
/* Writing the packet body */
for (bandno = 0; bandno < res->numbands; bandno++) {
opj_tcd_band_t *band = &res->bands[bandno];
opj_tcd_precinct_t *prc = &band->precincts[precno];
for (cblkno = 0; cblkno < prc->cw * prc->ch; cblkno++) {
opj_tcd_cblk_enc_t* cblk = &prc->cblks.enc[cblkno];
opj_tcd_layer_t *layer = &cblk->layers[layno];
if (!layer->numpasses) {
continue;
}
if (c + layer->len > dest + length) {
return -999;
}
memcpy(c, layer->data, layer->len);
cblk->numpasses += layer->numpasses;
c += layer->len;
/* << INDEX */
if(cstr_info && cstr_info->index_write) {
opj_packet_info_t *info_PK = &cstr_info->tile[tileno].packet[cstr_info->packno];
info_PK->disto += layer->disto;
if (cstr_info->D_max < info_PK->disto) {
cstr_info->D_max = info_PK->disto;
}
}
/* INDEX >> */
}
}
return (c - dest);
}
// return NULL for success, error string for failure
const char *lkb_handle_command(lkb_t *lkb, const char *cmd, const char *arg, unsigned len) {
struct lkb_command *lcmd;
for (lcmd = lkb_cmd_list; lcmd; lcmd = lcmd->next) {
if (!strcmp(lcmd->name, cmd)) {
return lcmd->handler(lkb, arg, len, lcmd->cookie);
}
}
if (len > lkb_iobuffer_size) {
return "buffer too small";
}
if (!strcmp(cmd, "flash") || !strcmp(cmd, "erase")) {
struct ptable_entry entry;
bdev_t *bdev;
if (ptable_find(arg, &entry) < 0) {
return "no such partition";
}
if (len > entry.length) {
return "partition too small";
}
if (lkb_read(lkb, lkb_iobuffer, len)) {
return "io error";
}
if (!(bdev = bio_open(bootdevice))) {
return "bio_open failed";
}
if (bio_erase(bdev, entry.offset, entry.length) != (ssize_t)entry.length) {
bio_close(bdev);
return "bio_erase failed";
}
if (!strcmp(cmd, "flash")) {
if (bio_write(bdev, lkb_iobuffer, entry.offset, len) != (ssize_t)len) {
bio_close(bdev);
return "bio_write failed";
}
}
bio_close(bdev);
return NULL;
} else if (!strcmp(cmd, "fpga")) {
#if PLATFORM_ZYNQ
unsigned *x = lkb_iobuffer;
if (lkb_read(lkb, lkb_iobuffer, len)) {
return "io error";
}
for (unsigned n = 0; n < len; n+= 4) {
*x = SWAP_32(*x);
x++;
}
zynq_reset_fpga();
zynq_program_fpga(lkb_iobuffer_phys, len);
return NULL;
#else
return "no fpga";
#endif
} else if (!strcmp(cmd, "boot")) {
if (lkb_read(lkb, lkb_iobuffer, len)) {
return "io error";
}
thread_resume(thread_create("ramboot", &do_ramboot, NULL,
DEFAULT_PRIORITY, DEFAULT_STACK_SIZE));
return NULL;
} else if (!strcmp(cmd, "getsysparam")) {
const void *ptr;
size_t len;
if (sysparam_get_ptr(arg, &ptr, &len) == 0) {
lkb_write(lkb, ptr, len);
}
return NULL;
} else if (!strcmp(cmd, "reboot")) {
thread_resume(thread_create("reboot", &do_reboot, NULL,
DEFAULT_PRIORITY, DEFAULT_STACK_SIZE));
return NULL;
} else {
return "unknown command";
}
}
static bool t2_encode_packet(
OPJ_UINT32 tileno,
opj_tcd_tile_t * tile,
opj_tcp_t * tcp,
opj_pi_iterator_t *pi,
OPJ_BYTE *dest,
OPJ_UINT32 * p_data_written,
OPJ_UINT32 length,
opj_codestream_info_t *cstr_info)
{
OPJ_UINT32 bandno, cblkno;
OPJ_BYTE *c = dest;
OPJ_UINT32 l_nb_bytes;
OPJ_UINT32 compno = pi->compno; /* component value */
OPJ_UINT32 resno = pi->resno; /* resolution level value */
OPJ_UINT32 precno = pi->precno; /* precinct value */
OPJ_UINT32 layno = pi->layno; /* quality layer value */
OPJ_UINT32 l_nb_blocks;
opj_tcd_band_t *band = 00;
opj_tcd_cblk_enc_t* cblk = 00;
opj_tcd_pass_t *pass = 00;
opj_tcd_tilecomp_t *tilec = &tile->comps[compno];
opj_tcd_resolution_t *res = &tilec->resolutions[resno];
opj_bio_t *bio = 00; /* BIO component */
/* <SOP 0xff91> */
if (tcp->csty & J2K_CP_CSTY_SOP) {
c[0] = 255;
c[1] = 145;
c[2] = 0;
c[3] = 4;
c[4] = (tile->packno % 65536) / 256;
c[5] = (tile->packno % 65536) % 256;
c += 6;
length -= 6;
}
/* </SOP> */
if (!layno) {
band = res->bands;
for
(bandno = 0; bandno < res->numbands; ++bandno)
{
opj_tcd_precinct_t *prc = &band->precincts[precno];
tgt_reset(prc->incltree);
tgt_reset(prc->imsbtree);
l_nb_blocks = prc->cw * prc->ch;
for
(cblkno = 0; cblkno < l_nb_blocks; ++cblkno)
{
opj_tcd_cblk_enc_t* cblk_v = &prc->cblks.enc[cblkno];
cblk_v->numpasses = 0;
tgt_setvalue(prc->imsbtree, cblkno, band->numbps - cblk_v->numbps);
}
++band;
}
}
bio = bio_create();
bio_init_enc(bio, c, length);
bio_write(bio, 1, 1); /* Empty header bit */
/* Writing Packet header */
band = res->bands;
for
(bandno = 0; bandno < res->numbands; ++bandno)
{
opj_tcd_precinct_t *prc = &band->precincts[precno];
l_nb_blocks = prc->cw * prc->ch;
cblk = prc->cblks.enc;
for (cblkno = 0; cblkno < l_nb_blocks; ++cblkno)
{
opj_tcd_layer_t *layer = &cblk->layers[layno];
if
(!cblk->numpasses && layer->numpasses)
{
tgt_setvalue(prc->incltree, cblkno, layno);
}
++cblk;
}
cblk = prc->cblks.enc;
for
(cblkno = 0; cblkno < l_nb_blocks; cblkno++)
{
opj_tcd_layer_t *layer = &cblk->layers[layno];
OPJ_UINT32 increment = 0;
OPJ_UINT32 nump = 0;
OPJ_UINT32 len = 0, passno;
OPJ_UINT32 l_nb_passes;
/* cblk inclusion bits */
if (!cblk->numpasses) {
tgt_encode(bio, prc->incltree, cblkno, layno + 1);
} else {
bio_write(bio, layer->numpasses != 0, 1);
}
/* if cblk not included, go to the next cblk */
if
(!layer->numpasses)
{
++cblk;
continue;
}
/* if first instance of cblk --> zero bit-planes information */
if
(!cblk->numpasses)
{
cblk->numlenbits = 3;
tgt_encode(bio, prc->imsbtree, cblkno, 999);
}
/* number of coding passes included */
t2_putnumpasses(bio, layer->numpasses);
l_nb_passes = cblk->numpasses + layer->numpasses;
pass = cblk->passes + cblk->numpasses;
/* computation of the increase of the length indicator and insertion in the header */
for
(passno = cblk->numpasses; passno < l_nb_passes; ++passno)
{
++nump;
len += pass->len;
if
(pass->term || passno == (cblk->numpasses + layer->numpasses) - 1)
{
increment = int_max(increment, int_floorlog2(len) + 1 - (cblk->numlenbits + int_floorlog2(nump)));
len = 0;
nump = 0;
}
++pass;
}
t2_putcommacode(bio, increment);
/* computation of the new Length indicator */
cblk->numlenbits += increment;
pass = cblk->passes + cblk->numpasses;
/* insertion of the codeword segment length */
for
(passno = cblk->numpasses; passno < l_nb_passes; ++passno)
{
nump++;
len += pass->len;
if
(pass->term || passno == (cblk->numpasses + layer->numpasses) - 1)
{
bio_write(bio, len, cblk->numlenbits + int_floorlog2(nump));
len = 0;
nump = 0;
}
++pass;
}
++cblk;
}
++band;
}
if
(bio_flush(bio))
{
bio_destroy(bio);
return false; /* modified to eliminate longjmp !! */
}
l_nb_bytes = bio_numbytes(bio);
c += l_nb_bytes;
length -= l_nb_bytes;
bio_destroy(bio);
/* <EPH 0xff92> */
if (tcp->csty & J2K_CP_CSTY_EPH) {
c[0] = 255;
c[1] = 146;
c += 2;
length -= 2;
}
/* </EPH> */
/* << INDEX */
// End of packet header position. Currently only represents the distance to start of packet
// Will be updated later by incrementing with packet start value
if(cstr_info && cstr_info->index_write) {
opj_packet_info_t *info_PK = &cstr_info->tile[tileno].packet[cstr_info->packno];
info_PK->end_ph_pos = (OPJ_INT32)(c - dest);
}
/* INDEX >> */
/* Writing the packet body */
band = res->bands;
for
(bandno = 0; bandno < res->numbands; bandno++)
{
opj_tcd_precinct_t *prc = &band->precincts[precno];
l_nb_blocks = prc->cw * prc->ch;
cblk = prc->cblks.enc;
for
(cblkno = 0; cblkno < l_nb_blocks; ++cblkno)
{
opj_tcd_layer_t *layer = &cblk->layers[layno];
if
(!layer->numpasses)
{
++cblk;
continue;
}
if
(layer->len > length)
{
return false;
}
memcpy(c, layer->data, layer->len);
cblk->numpasses += layer->numpasses;
c += layer->len;
length -= layer->len;
/* << INDEX */
if(cstr_info && cstr_info->index_write) {
opj_packet_info_t *info_PK = &cstr_info->tile[tileno].packet[cstr_info->packno];
info_PK->disto += layer->disto;
if (cstr_info->D_max < info_PK->disto) {
cstr_info->D_max = info_PK->disto;
}
}
++cblk;
/* INDEX >> */
}
++band;
}
* p_data_written += (c - dest);
return true;
}
static int cmd_bio(int argc, const cmd_args *argv)
{
int rc = 0;
if (argc < 2) {
notenoughargs:
printf("not enough arguments:\n");
usage:
printf("%s list\n", argv[0].str);
printf("%s read <device> <address> <offset> <len>\n", argv[0].str);
printf("%s write <device> <address> <offset> <len>\n", argv[0].str);
printf("%s dump <device> <offset> <len>\n", argv[0].str);
printf("%s erase <device> <offset> <len>\n", argv[0].str);
printf("%s ioctl <device> <request> <arg>\n", argv[0].str);
printf("%s remove <device>\n", argv[0].str);
printf("%s test <device>\n", argv[0].str);
#if WITH_LIB_PARTITION
printf("%s partscan <device> [offset]\n", argv[0].str);
#endif
#if WITH_LIB_CKSUM
printf("%s crc32 <device> <offset> <len> [repeat]\n", argv[0].str);
#endif
return -1;
}
if (!strcmp(argv[1].str, "list")) {
bio_dump_devices();
} else if (!strcmp(argv[1].str, "read")) {
if (argc < 6) goto notenoughargs;
addr_t address = argv[3].u;
off_t offset = argv[4].u; // XXX use long
size_t len = argv[5].u;
bdev_t *dev = bio_open(argv[2].str);
if (!dev) {
printf("error opening block device\n");
return -1;
}
lk_time_t t = current_time();
ssize_t err = bio_read(dev, (void *)address, offset, len);
t = current_time() - t;
dprintf(INFO, "bio_read returns %d, took %u msecs (%d bytes/sec)\n", (int)err, (uint)t, (uint32_t)((uint64_t)err * 1000 / t));
bio_close(dev);
rc = err;
} else if (!strcmp(argv[1].str, "write")) {
if (argc < 6) goto notenoughargs;
addr_t address = argv[3].u;
off_t offset = argv[4].u; // XXX use long
size_t len = argv[5].u;
bdev_t *dev = bio_open(argv[2].str);
if (!dev) {
printf("error opening block device\n");
return -1;
}
lk_time_t t = current_time();
ssize_t err = bio_write(dev, (void *)address, offset, len);
t = current_time() - t;
dprintf(INFO, "bio_write returns %d, took %u msecs (%d bytes/sec)\n", (int)err, (uint)t, (uint32_t)((uint64_t)err * 1000 / t));
bio_close(dev);
rc = err;
} else if (!strcmp(argv[1].str, "dump")) {
if (argc < 5) {
printf("not enough arguments:\n");
goto usage;
}
off_t offset = argv[3].u; // XXX use long
size_t len = argv[4].u;
bdev_t *dev = bio_open(argv[2].str);
if (!dev) {
printf("error opening block device\n");
return -1;
}
uint8_t *buf = memalign(CACHE_LINE, 256);
ssize_t err = 0;
while (len > 0) {
size_t amt = MIN(256, len);
ssize_t err = bio_read(dev, buf, offset, amt);
if (err < 0) {
dprintf(ALWAYS, "read error %s %zu@%zu (err %d)\n",
argv[2].str, amt, (size_t)offset, (int)err);
break;
}
DEBUG_ASSERT((size_t)err <= amt);
hexdump8_ex(buf, err, offset);
if ((size_t)err != amt) {
dprintf(ALWAYS, "short read from %s @%zu (wanted %zu, got %zu)\n",
argv[2].str, (size_t)offset, amt, (size_t)err);
break;
}
offset += amt;
len -= amt;
}
bio_close(dev);
rc = err;
} else if (!strcmp(argv[1].str, "erase")) {
if (argc < 5) goto notenoughargs;
off_t offset = argv[3].u; // XXX use long
size_t len = argv[4].u;
bdev_t *dev = bio_open(argv[2].str);
if (!dev) {
printf("error opening block device\n");
return -1;
}
lk_time_t t = current_time();
ssize_t err = bio_erase(dev, offset, len);
t = current_time() - t;
dprintf(INFO, "bio_erase returns %d, took %u msecs (%d bytes/sec)\n", (int)err, (uint)t, (uint32_t)((uint64_t)err * 1000 / t));
bio_close(dev);
rc = err;
} else if (!strcmp(argv[1].str, "ioctl")) {
if (argc < 4) goto notenoughargs;
int request = argv[3].u;
int arg = (argc == 5) ? argv[4].u : 0;
bdev_t *dev = bio_open(argv[2].str);
if (!dev) {
printf("error opening block device\n");
return -1;
}
int err = bio_ioctl(dev, request, (void *)arg);
dprintf(INFO, "bio_ioctl returns %d\n", err);
bio_close(dev);
rc = err;
} else if (!strcmp(argv[1].str, "remove")) {
if (argc < 3) goto notenoughargs;
bdev_t *dev = bio_open(argv[2].str);
if (!dev) {
printf("error opening block device\n");
return -1;
}
bio_unregister_device(dev);
bio_close(dev);
} else if (!strcmp(argv[1].str, "test")) {
if (argc < 3) goto notenoughargs;
bdev_t *dev = bio_open(argv[2].str);
if (!dev) {
printf("error opening block device\n");
return -1;
}
int err = bio_test_device(dev);
bio_close(dev);
rc = err;
#if WITH_LIB_PARTITION
} else if (!strcmp(argv[1].str, "partscan")) {
if (argc < 3) goto notenoughargs;
off_t offset = 0;
if (argc > 3)
offset = argv[3].u;
rc = partition_publish(argv[2].str, offset);
dprintf(INFO, "partition_publish returns %d\n", rc);
#endif
#if WITH_LIB_CKSUM
} else if (!strcmp(argv[1].str, "crc32")) {
if (argc < 5) goto notenoughargs;
off_t offset = argv[3].u; // XXX use long
size_t len = argv[4].u;
bdev_t *dev = bio_open(argv[2].str);
if (!dev) {
printf("error opening block device\n");
return -1;
}
void *buf = malloc(dev->block_size);
bool repeat = false;
if (argc >= 6 && !strcmp(argv[5].str, "repeat")) {
repeat = true;
}
do {
ulong crc = 0;
off_t pos = offset;
while (pos < offset + len) {
ssize_t err = bio_read(dev, buf, pos, MIN(len - (pos - offset), dev->block_size));
if (err <= 0) {
printf("error reading at offset 0x%llx\n", offset + pos);
break;
}
crc = crc32(crc, buf, err);
pos += err;
}
printf("crc 0x%08lx\n", crc);
} while (repeat);
bio_close(dev);
free(buf);
#endif
} else {
printf("unrecognized subcommand\n");
goto usage;
}
return rc;
}
// return NULL for success, error string for failure
const char *lkb_handle_command(lkb_t *lkb, const char *cmd, const char *arg, unsigned len) {
struct lkb_command *lcmd;
for (lcmd = lkb_cmd_list; lcmd; lcmd = lcmd->next) {
if (!strcmp(lcmd->name, cmd)) {
return lcmd->handler(lkb, arg, len, lcmd->cookie);
}
}
if (len > lkb_iobuffer_size) {
return "buffer too small";
}
if (!strcmp(cmd, "flash") || !strcmp(cmd, "erase")) {
struct ptable_entry entry;
bdev_t *bdev;
if (ptable_find(arg, &entry) < 0) {
size_t plen = len;
/* doesn't exist, make one */
#if PLATFORM_ZYNQ
/* XXX not really the right place, should be in the ptable/bio layer */
plen = ROUNDUP(plen, 256*1024);
#endif
off_t off = ptable_allocate(plen, 0);
if (off < 0) {
return "no space to allocate partition";
}
if (ptable_add(arg, off, plen, 0) < 0) {
return "error creating partition";
}
if (ptable_find(arg, &entry) < 0) {
return "couldn't find partition after creating it";
}
}
if (len > entry.length) {
return "partition too small";
}
if (lkb_read(lkb, lkb_iobuffer, len)) {
return "io error";
}
if (!(bdev = bio_open(bootdevice))) {
return "bio_open failed";
}
if (bio_erase(bdev, entry.offset, entry.length) != (ssize_t)entry.length) {
bio_close(bdev);
return "bio_erase failed";
}
if (!strcmp(cmd, "flash")) {
if (bio_write(bdev, lkb_iobuffer, entry.offset, len) != (ssize_t)len) {
bio_close(bdev);
return "bio_write failed";
}
}
bio_close(bdev);
return NULL;
} else if (!strcmp(cmd, "remove")) {
if (ptable_remove(arg) < 0) {
return "remove failed";
}
return NULL;
} else if (!strcmp(cmd, "fpga")) {
#if PLATFORM_ZYNQ
unsigned *x = lkb_iobuffer;
if (lkb_read(lkb, lkb_iobuffer, len)) {
return "io error";
}
for (unsigned n = 0; n < len; n+= 4) {
*x = SWAP_32(*x);
x++;
}
zynq_reset_fpga();
zynq_program_fpga(lkb_iobuffer_phys, len);
return NULL;
#else
return "no fpga";
#endif
} else if (!strcmp(cmd, "boot")) {
if (lkb_read(lkb, lkb_iobuffer, len)) {
return "io error";
}
thread_resume(thread_create("boot", &do_boot, NULL,
DEFAULT_PRIORITY, DEFAULT_STACK_SIZE));
return NULL;
} else if (!strcmp(cmd, "getsysparam")) {
const void *ptr;
size_t len;
if (sysparam_get_ptr(arg, &ptr, &len) == 0) {
lkb_write(lkb, ptr, len);
}
return NULL;
} else if (!strcmp(cmd, "reboot")) {
thread_resume(thread_create("reboot", &do_reboot, NULL,
DEFAULT_PRIORITY, DEFAULT_STACK_SIZE));
return NULL;
} else {
return "unknown command";
}
}
int t2_encode_packet(tcd_tile_t *tile, j2k_tcp_t *tcp, int compno, int resno, int precno, int layno, unsigned char *dest, int len) {
int bandno, cblkno;
tcd_tilecomp_t *tilec=&tile->comps[compno];
tcd_resolution_t *res=&tilec->resolutions[resno];
unsigned char *c=dest;
if (!layno) {
for (bandno=0; bandno<res->numbands; bandno++) {
tcd_band_t *band=&res->bands[bandno];
tcd_precinct_t *prc=&band->precincts[precno];
tgt_reset(prc->incltree);
tgt_reset(prc->imsbtree);
for (cblkno=0; cblkno<prc->cw*prc->ch; cblkno++) {
tcd_cblk_t *cblk=&prc->cblks[cblkno];
cblk->numpasses=0;
tgt_setvalue(prc->imsbtree, cblkno, band->numbps-cblk->numbps);
}
}
}
bio_init_enc(c, len);
bio_write(1, 1);
for (bandno=0; bandno<res->numbands; bandno++) {
tcd_band_t *band=&res->bands[bandno];
tcd_precinct_t *prc=&band->precincts[precno];
for (cblkno=0; cblkno<prc->cw*prc->ch; cblkno++) {
tcd_cblk_t *cblk=&prc->cblks[cblkno];
tcd_layer_t *layer=&cblk->layers[layno];
if (!cblk->numpasses && layer->numpasses) {
tgt_setvalue(prc->incltree, cblkno, layno);
}
}
for (cblkno=0; cblkno<prc->cw*prc->ch; cblkno++) {
tcd_cblk_t *cblk=&prc->cblks[cblkno];
tcd_layer_t *layer=&cblk->layers[layno];
int increment;
if (!cblk->numpasses) {
tgt_encode(prc->incltree, cblkno, layno+1);
} else {
bio_write(layer->numpasses!=0, 1);
}
if (!layer->numpasses) {
continue;
}
if (!cblk->numpasses) {
cblk->numlenbits=3;
tgt_encode(prc->imsbtree, cblkno, 999);
}
t2_putnumpasses(layer->numpasses);
increment=int_max(0, int_floorlog2(layer->len)+1-(cblk->numlenbits+int_floorlog2(layer->numpasses)));
t2_putcommacode(increment);
cblk->numlenbits+=increment;
bio_write(layer->len, cblk->numlenbits+int_floorlog2(layer->numpasses));
}
}
bio_flush();
c+=bio_numbytes();
for (bandno=0; bandno<res->numbands; bandno++) {
tcd_band_t *band=&res->bands[bandno];
tcd_precinct_t *prc=&band->precincts[precno];
for (cblkno=0; cblkno<prc->cw*prc->ch; cblkno++) {
tcd_cblk_t *cblk=&prc->cblks[cblkno];
tcd_layer_t *layer=&cblk->layers[layno];
if (!layer->numpasses) continue;
if (c+layer->len>dest+len) {
longjmp(j2k_error, 1);
}
memcpy(c, layer->data, layer->len);
cblk->numpasses+=layer->numpasses;
c+=layer->len;
}
}
return c-dest;
}
static int t2_encode_packet(opj_tcd_tile_t * tile, opj_tcp_t * tcp, opj_pi_iterator_t *pi, unsigned char *dest, int length, opj_image_info_t * image_info, int tileno) {
int bandno, cblkno;
unsigned char *sop = 0, *eph = 0;
unsigned char *c = dest;
int compno = pi->compno; /* component value */
int resno = pi->resno; /* resolution level value */
int precno = pi->precno; /* precinct value */
int layno = pi->layno; /* quality layer value */
opj_tcd_tilecomp_t *tilec = &tile->comps[compno];
opj_tcd_resolution_t *res = &tilec->resolutions[resno];
opj_bio_t *bio = NULL; /* BIO component */
/* <SOP 0xff91> */
if (tcp->csty & J2K_CP_CSTY_SOP) {
sop = (unsigned char *) opj_malloc(6 * sizeof(unsigned char));
sop[0] = 255;
sop[1] = 145;
sop[2] = 0;
sop[3] = 4;
sop[4] = (image_info->num % 65536) / 256;
sop[5] = (image_info->num % 65536) % 256;
memcpy(c, sop, 6);
opj_free(sop);
c += 6;
}
/* </SOP> */
if (!layno) {
for (bandno = 0; bandno < res->numbands; bandno++) {
opj_tcd_band_t *band = &res->bands[bandno];
opj_tcd_precinct_t *prc = &band->precincts[precno];
tgt_reset(prc->incltree);
tgt_reset(prc->imsbtree);
for (cblkno = 0; cblkno < prc->cw * prc->ch; cblkno++) {
opj_tcd_cblk_t *cblk = &prc->cblks[cblkno];
cblk->numpasses = 0;
tgt_setvalue(prc->imsbtree, cblkno, band->numbps - cblk->numbps);
}
}
}
bio = bio_create();
bio_init_enc(bio, c, length);
bio_write(bio, 1, 1); /* Empty header bit */
/* Writing Packet header */
for (bandno = 0; bandno < res->numbands; bandno++) {
opj_tcd_band_t *band = &res->bands[bandno];
opj_tcd_precinct_t *prc = &band->precincts[precno];
for (cblkno = 0; cblkno < prc->cw * prc->ch; cblkno++) {
opj_tcd_cblk_t *cblk = &prc->cblks[cblkno];
opj_tcd_layer_t *layer = &cblk->layers[layno];
if (!cblk->numpasses && layer->numpasses) {
tgt_setvalue(prc->incltree, cblkno, layno);
}
}
for (cblkno = 0; cblkno < prc->cw * prc->ch; cblkno++) {
opj_tcd_cblk_t *cblk = &prc->cblks[cblkno];
opj_tcd_layer_t *layer = &cblk->layers[layno];
int increment = 0;
int nump = 0;
int len = 0, passno;
/* cblk inclusion bits */
if (!cblk->numpasses) {
tgt_encode(bio, prc->incltree, cblkno, layno + 1);
} else {
bio_write(bio, layer->numpasses != 0, 1);
}
/* if cblk not included, go to the next cblk */
if (!layer->numpasses) {
continue;
}
/* if first instance of cblk --> zero bit-planes information */
if (!cblk->numpasses) {
cblk->numlenbits = 3;
tgt_encode(bio, prc->imsbtree, cblkno, 999);
}
/* number of coding passes included */
t2_putnumpasses(bio, layer->numpasses);
/* computation of the increase of the length indicator and insertion in the header */
for (passno = cblk->numpasses; passno < cblk->numpasses + layer->numpasses; passno++) {
opj_tcd_pass_t *pass = &cblk->passes[passno];
nump++;
len += pass->len;
if (pass->term || passno == (cblk->numpasses + layer->numpasses) - 1) {
increment = int_max(increment, int_floorlog2(len) + 1 - (cblk->numlenbits + int_floorlog2(nump)));
len = 0;
nump = 0;
}
}
t2_putcommacode(bio, increment);
/* computation of the new Length indicator */
cblk->numlenbits += increment;
/* insertion of the codeword segment length */
for (passno = cblk->numpasses; passno < cblk->numpasses + layer->numpasses; passno++) {
opj_tcd_pass_t *pass = &cblk->passes[passno];
nump++;
len += pass->len;
if (pass->term || passno == (cblk->numpasses + layer->numpasses) - 1) {
bio_write(bio, len, cblk->numlenbits + int_floorlog2(nump));
len = 0;
nump = 0;
}
}
}
}
if (bio_flush(bio)) {
return -999; /* modified to eliminate longjmp !! */
}
c += bio_numbytes(bio);
bio_destroy(bio);
/* <EPH 0xff92> */
if (tcp->csty & J2K_CP_CSTY_EPH) {
eph = (unsigned char *) opj_malloc(2 * sizeof(unsigned char));
eph[0] = 255;
eph[1] = 146;
memcpy(c, eph, 2);
opj_free(eph);
c += 2;
}
/* </EPH> */
/* Writing the packet body */
for (bandno = 0; bandno < res->numbands; bandno++) {
opj_tcd_band_t *band = &res->bands[bandno];
opj_tcd_precinct_t *prc = &band->precincts[precno];
for (cblkno = 0; cblkno < prc->cw * prc->ch; cblkno++) {
opj_tcd_cblk_t *cblk = &prc->cblks[cblkno];
opj_tcd_layer_t *layer = &cblk->layers[layno];
if (!layer->numpasses) {
continue;
}
if (c + layer->len > dest + length) {
return -999;
}
memcpy(c, layer->data, layer->len);
cblk->numpasses += layer->numpasses;
c += layer->len;
/* ADD for index Cfr. Marcela --> delta disto by packet */
if(image_info && image_info->index_write && image_info->index_on) {
opj_tile_info_t *info_TL = &image_info->tile[tileno];
opj_packet_info_t *info_PK = &info_TL->packet[image_info->num];
info_PK->disto += layer->disto;
if (image_info->D_max < info_PK->disto) {
image_info->D_max = info_PK->disto;
}
}
/* </ADD> */
}
}
return (c - dest);
}
uint sparse_write_to_device(bdev_t * dev, void *data, uint32_t sz)
{
uint32_t chunk;
uint32_t chunk_data_sz;
uint32_t *fill_buf = NULL;
uint32_t fill_val;
uint32_t chunk_blk_cnt = 0;
sparse_header_t *sparse_header;
chunk_header_t *chunk_header;
uint32_t total_blocks = 0;
uint32_t i;
/* Read and skip over sparse image header */
sparse_header = (sparse_header_t *) data;
if ((sparse_header->total_blks * sparse_header->blk_sz) > dev->size) {
fastboot_fail("size too large");
return -1;
}
data += sparse_header->file_hdr_sz;
if (sparse_header->file_hdr_sz > sizeof(sparse_header_t)) {
/* Skip the remaining bytes in a header that is longer than
* we expected.
*/
data += (sparse_header->file_hdr_sz - sizeof(sparse_header_t));
}
dprintf(SPEW, "=== Sparse Image Header ===\n");
dprintf(SPEW, "magic: 0x%x\n", sparse_header->magic);
dprintf(SPEW, "major_version: 0x%x\n", sparse_header->major_version);
dprintf(SPEW, "minor_version: 0x%x\n", sparse_header->minor_version);
dprintf(SPEW, "file_hdr_sz: %d\n", sparse_header->file_hdr_sz);
dprintf(SPEW, "chunk_hdr_sz: %d\n", sparse_header->chunk_hdr_sz);
dprintf(SPEW, "blk_sz: %d\n", sparse_header->blk_sz);
dprintf(SPEW, "total_blks: %d\n", sparse_header->total_blks);
dprintf(SPEW, "total_chunks: %d\n", sparse_header->total_chunks);
/* Start processing chunks */
for (chunk = 0; chunk < sparse_header->total_chunks; chunk++) {
/* Read and skip over chunk header */
chunk_header = (chunk_header_t *) data;
data += sizeof(chunk_header_t);
dprintf(SPEW, "=== Chunk Header ===\n");
dprintf(SPEW, "chunk_type: 0x%x\n", chunk_header->chunk_type);
dprintf(SPEW, "chunk_data_sz: 0x%x\n", chunk_header->chunk_sz);
dprintf(SPEW, "total_size: 0x%x\n", chunk_header->total_sz);
if (sparse_header->chunk_hdr_sz > sizeof(chunk_header_t)) {
/* Skip the remaining bytes in a header that is longer than
* we expected.
*/
data +=
(sparse_header->chunk_hdr_sz -
sizeof(chunk_header_t));
}
chunk_data_sz = sparse_header->blk_sz * chunk_header->chunk_sz;
switch (chunk_header->chunk_type) {
case CHUNK_TYPE_RAW:
if (chunk_header->total_sz !=
(sparse_header->chunk_hdr_sz + chunk_data_sz)) {
fastboot_fail
("Bogus chunk size for chunk type Raw");
return -1;
}
if (bio_write
(dev, data,
((uint64_t) total_blocks * sparse_header->blk_sz),
chunk_data_sz)) {
fastboot_fail("flash write failure");
return -1;
}
total_blocks += chunk_header->chunk_sz;
data += chunk_data_sz;
break;
case CHUNK_TYPE_FILL:
if (chunk_header->total_sz !=
(sparse_header->chunk_hdr_sz + sizeof(uint32_t))) {
fastboot_fail
("Bogus chunk size for chunk type FILL");
return -1;
}
fill_buf =
(uint32_t *) memalign(CACHE_LINE,
ALIGN(sparse_header->blk_sz,
CACHE_LINE));
if (!fill_buf) {
fastboot_fail
("Malloc failed for: CHUNK_TYPE_FILL");
return -1;
}
fill_val = *(uint32_t *) data;
data = (char *)data + sizeof(uint32_t);
chunk_blk_cnt = chunk_data_sz / sparse_header->blk_sz;
for (i = 0;
i < (sparse_header->blk_sz / sizeof(fill_val));
i++) {
fill_buf[i] = fill_val;
}
for (i = 0; i < chunk_blk_cnt; i++) {
if (bio_write
(dev, fill_buf,
((uint64_t) total_blocks *
sparse_header->blk_sz),
sparse_header->blk_sz)) {
fastboot_fail("flash write failure");
free(fill_buf);
return -1;
}
total_blocks++;
}
free(fill_buf);
break;
case CHUNK_TYPE_DONT_CARE:
total_blocks += chunk_header->chunk_sz;
break;
case CHUNK_TYPE_CRC32:
if (chunk_header->total_sz !=
sparse_header->chunk_hdr_sz) {
fastboot_fail
("Bogus chunk size for chunk type Dont Care");
return -1;
}
total_blocks += chunk_header->chunk_sz;
data += chunk_data_sz;
break;
default:
dprintf(CRITICAL, "Unkown chunk type: %x\n",
chunk_header->chunk_type);
fastboot_fail("Unknown chunk type");
return -1;
}
}
dprintf(INFO, "Wrote %d blocks, expected to write %d blocks\n",
total_blocks, sparse_header->total_blks);
if (total_blocks != sparse_header->total_blks) {
fastboot_fail("sparse image write failure");
return -1;
}
fastboot_okay("");
return 0;
}
