/* write end command and crc command to memory. */
static void add_end_cmd(void)
{
uint32_t crc32_pbl;
int i;
unsigned char *p = (unsigned char *)&pbl_end_cmd;
if (ENDIANNESS == 'l') {
for (i = 0; i < 4; i++)
pbl_end_cmd[i] = reverse_byte(pbl_end_cmd[i]);
}
for (i = 0; i < 16; i++) {
*pmem_buf++ = *p++;
pbl_size++;
}
/* Add PBI CRC command. */
*pmem_buf++ = 0x08;
*pmem_buf++ = pbi_crc_cmd1;
*pmem_buf++ = pbi_crc_cmd2;
*pmem_buf++ = 0x40;
pbl_size += 4;
/* calculated CRC32 and write it to memory. */
crc32_pbl = pbl_crc32(0, (const char *)mem_buf, pbl_size);
*pmem_buf++ = (crc32_pbl >> 24) & 0xff;
*pmem_buf++ = (crc32_pbl >> 16) & 0xff;
*pmem_buf++ = (crc32_pbl >> 8) & 0xff;
*pmem_buf++ = (crc32_pbl) & 0xff;
pbl_size += 4;
}
static int sb_build_command_load(struct sb_image_ctx *ictx,
struct sb_cmd_list *cmd)
{
struct sb_section_ctx *sctx = ictx->sect_tail;
struct sb_cmd_ctx *cctx;
struct sb_command *ccmd;
char *tok;
int ret, is_ivt = 0, is_dcd = 0;
uint32_t dest, dcd = 0;
cctx = calloc(1, sizeof(*cctx));
if (!cctx)
return -ENOMEM;
ccmd = &cctx->payload;
/*
* Prepare the command.
*/
tok = strtok(cmd->cmd, " ");
if (!tok) {
fprintf(stderr, "#%i ERR: Missing LOAD address or 'IVT'!\n",
cmd->lineno);
ret = -EINVAL;
goto err;
}
/* Check for "IVT" flag. */
if (!strcmp(tok, "IVT"))
is_ivt = 1;
if (!strcmp(tok, "DCD"))
is_dcd = 1;
if (is_ivt || is_dcd) {
tok = strtok(NULL, " ");
if (!tok) {
fprintf(stderr, "#%i ERR: Missing LOAD address!\n",
cmd->lineno);
ret = -EINVAL;
goto err;
}
}
/* Read load destination address. */
ret = sb_token_to_long(tok, &dest);
if (ret) {
fprintf(stderr, "#%i ERR: Incorrect LOAD address!\n",
cmd->lineno);
goto err;
}
/* Read filename or IVT entrypoint or DCD block ID. */
tok = strtok(NULL, " ");
if (!tok) {
fprintf(stderr,
"#%i ERR: Missing LOAD filename or IVT ep or DCD block ID!\n",
cmd->lineno);
ret = -EINVAL;
goto err;
}
if (is_ivt) {
/* Handle IVT. */
struct sb_ivt_header *ivt;
uint32_t ivtep;
ret = sb_token_to_long(tok, &ivtep);
if (ret) {
fprintf(stderr,
"#%i ERR: Incorrect IVT entry point!\n",
cmd->lineno);
goto err;
}
ivt = calloc(1, sizeof(*ivt));
if (!ivt) {
ret = -ENOMEM;
goto err;
}
ivt->header = sb_hab_ivt_header();
ivt->entry = ivtep;
ivt->self = dest;
cctx->data = (uint8_t *)ivt;
cctx->length = sizeof(*ivt);
} else if (is_dcd) {
struct sb_dcd_ctx *dctx = ictx->dcd_head;
uint32_t dcdid;
uint8_t *payload;
uint32_t asize;
ret = sb_token_to_long(tok, &dcdid);
if (ret) {
fprintf(stderr,
"#%i ERR: Incorrect DCD block ID!\n",
cmd->lineno);
goto err;
}
while (dctx) {
if (dctx->id == dcdid)
break;
dctx = dctx->dcd;
}
if (!dctx) {
fprintf(stderr, "#%i ERR: DCD block %08x not found!\n",
cmd->lineno, dcdid);
goto err;
}
asize = roundup(dctx->size, SB_BLOCK_SIZE);
payload = calloc(1, asize);
if (!payload) {
ret = -ENOMEM;
goto err;
}
memcpy(payload, dctx->payload, dctx->size);
cctx->data = payload;
cctx->length = asize;
/* Set the Load DCD flag. */
dcd = ROM_LOAD_CMD_FLAG_DCD_LOAD;
} else {
/* Regular LOAD of a file. */
ret = sb_load_file(cctx, tok);
if (ret) {
fprintf(stderr, "#%i ERR: Cannot load '%s'!\n",
cmd->lineno, tok);
goto err;
}
}
if (cctx->length & (SB_BLOCK_SIZE - 1)) {
fprintf(stderr, "#%i ERR: Unaligned payload!\n",
cmd->lineno);
}
/*
* Construct the command.
*/
ccmd->header.checksum = 0x5a;
ccmd->header.tag = ROM_LOAD_CMD;
ccmd->header.flags = dcd;
ccmd->load.address = dest;
ccmd->load.count = cctx->length;
ccmd->load.crc32 = pbl_crc32(0,
(const char *)cctx->data,
cctx->length);
cctx->size = sizeof(*ccmd) + cctx->length;
/*
* Append the command to the last section.
*/
if (!sctx->cmd_head) {
sctx->cmd_head = cctx;
sctx->cmd_tail = cctx;
} else {
sctx->cmd_tail->cmd = cctx;
sctx->cmd_tail = cctx;
}
return 0;
err:
free(cctx);
return ret;
}
static int sb_verify_command(struct sb_image_ctx *ictx,
struct sb_cmd_ctx *cctx, FILE *fp,
unsigned long *tsize)
{
struct sb_command *ccmd = &cctx->payload;
unsigned long size, asize;
char *csum, *flag = "";
int ret;
unsigned int i;
uint8_t csn, csc = ccmd->header.checksum;
ccmd->header.checksum = 0x5a;
csn = sb_command_checksum(ccmd);
ccmd->header.checksum = csc;
if (csc == csn)
ret = 0;
else
ret = -EINVAL;
csum = ret ? "checksum BAD" : "checksum OK";
switch (ccmd->header.tag) {
case ROM_NOP_CMD:
soprintf(ictx, " NOOP # %s\n", csum);
return ret;
case ROM_TAG_CMD:
if (ccmd->header.flags & ROM_TAG_CMD_FLAG_ROM_LAST_TAG)
flag = "LAST";
soprintf(ictx, " TAG %s # %s\n", flag, csum);
sb_aes_reinit(ictx, 0);
return ret;
case ROM_LOAD_CMD:
soprintf(ictx, " LOAD addr=0x%08x length=0x%08x # %s\n",
ccmd->load.address, ccmd->load.count, csum);
cctx->length = ccmd->load.count;
asize = roundup(cctx->length, SB_BLOCK_SIZE);
cctx->data = malloc(asize);
if (!cctx->data)
return -ENOMEM;
size = fread(cctx->data, 1, asize, fp);
if (size != asize) {
fprintf(stderr,
"ERR: SB LOAD command payload too short!\n");
return -EINVAL;
}
*tsize += size;
EVP_DigestUpdate(&ictx->md_ctx, cctx->data, asize);
sb_aes_crypt(ictx, cctx->data, cctx->data, asize);
if (ccmd->load.crc32 != pbl_crc32(0,
(const char *)cctx->data,
asize)) {
fprintf(stderr,
"ERR: SB LOAD command payload CRC32 invalid!\n");
return -EINVAL;
}
return 0;
case ROM_FILL_CMD:
soprintf(ictx,
" FILL addr=0x%08x length=0x%08x pattern=0x%08x # %s\n",
ccmd->fill.address, ccmd->fill.count,
ccmd->fill.pattern, csum);
return 0;
case ROM_JUMP_CMD:
if (ccmd->header.flags & ROM_JUMP_CMD_FLAG_HAB)
flag = " HAB";
soprintf(ictx,
" JUMP%s addr=0x%08x r0_arg=0x%08x # %s\n",
flag, ccmd->fill.address, ccmd->jump.argument, csum);
return 0;
case ROM_CALL_CMD:
if (ccmd->header.flags & ROM_CALL_CMD_FLAG_HAB)
flag = " HAB";
soprintf(ictx,
" CALL%s addr=0x%08x r0_arg=0x%08x # %s\n",
flag, ccmd->fill.address, ccmd->jump.argument, csum);
return 0;
case ROM_MODE_CMD:
for (i = 0; i < ARRAY_SIZE(modetable); i++) {
if (ccmd->mode.mode == modetable[i].mode) {
soprintf(ictx, " MODE %s # %s\n",
modetable[i].name, csum);
break;
}
}
fprintf(stderr, " MODE !INVALID! # %s\n", csum);
return 0;
}
return ret;
}