int nxffs_rdblkhdr(FAR struct nxffs_volume_s *volume, off_t offset,
FAR uint16_t *datlen)
{
struct nxffs_data_s blkhdr;
uint32_t ecrc;
uint32_t crc;
uint16_t doffset;
uint16_t dlen;
int ret;
/* Make sure that the block containing the data block header is in the cache */
nxffs_ioseek(volume, offset);
ret = nxffs_rdcache(volume, volume->ioblock);
if (ret < 0)
{
fdbg("ERROR: Failed to read data into cache: %d\n", ret);
return ret;
}
/* Read the header at the FLASH offset */
doffset = volume->iooffset;
memcpy(&blkhdr, &volume->cache[doffset], SIZEOF_NXFFS_DATA_HDR);
/* Extract the data length */
dlen = nxffs_rdle16(blkhdr.datlen);
/* Get the offset to the beginning of the data */
doffset += SIZEOF_NXFFS_DATA_HDR;
/* Make sure that all of the data fits within the block */
if ((uint32_t)doffset + (uint32_t)dlen > (uint32_t)volume->geo.blocksize)
{
fdbg("ERROR: Data length=%d is unreasonable at offset=%d\n", dlen, doffset);
return -EIO;
}
/* Extract the expected CRC and calculate the CRC of the data block */
ecrc = nxffs_rdle32(blkhdr.crc);
nxffs_wrle32(blkhdr.crc, 0);
crc = crc32((FAR const uint8_t *)&blkhdr, SIZEOF_NXFFS_DATA_HDR);
crc = crc32part(&volume->cache[doffset], dlen, crc);
if (crc != ecrc)
{
fdbg("ERROR: CRC failure\n");
return -EIO;
}
/* Looks good! Return the data length and success */
*datlen = dlen;
return OK;
}
/// @brief Responds to a CalcFileCRC32 command
MavlinkFTP::ErrorCode
MavlinkFTP::_workCalcFileCRC32(PayloadHeader* payload)
{
char file_buf[256];
uint32_t checksum = 0;
ssize_t bytes_read;
strncpy(file_buf, _data_as_cstring(payload), kMaxDataLength);
int fd = ::open(file_buf, O_RDONLY);
if (fd < 0) {
return kErrFailErrno;
}
do {
bytes_read = ::read(fd, file_buf, sizeof(file_buf));
if (bytes_read < 0) {
int r_errno = errno;
::close(fd);
errno = r_errno;
return kErrFailErrno;
}
checksum = crc32part((uint8_t*)file_buf, bytes_read, checksum);
} while (bytes_read == sizeof(file_buf));
::close(fd);
payload->size = sizeof(uint32_t);
*((uint32_t*)payload->data) = checksum;
return kErrNone;
}
/// @brief Responds to a CalcFileCRC32 command
MavlinkFTP::ErrorCode
MavlinkFTP::_workCalcFileCRC32(PayloadHeader *payload)
{
uint32_t checksum = 0;
ssize_t bytes_read;
strncpy(_work_buffer2, _root_dir, _work_buffer2_len);
strncpy(_work_buffer2 + _root_dir_len, _data_as_cstring(payload), _work_buffer2_len - _root_dir_len);
// ensure termination
_work_buffer2[_work_buffer2_len - 1] = '\0';
int fd = ::open(_work_buffer2, O_RDONLY);
if (fd < 0) {
return kErrFailErrno;
}
do {
bytes_read = ::read(fd, _work_buffer2, _work_buffer2_len);
if (bytes_read < 0) {
int r_errno = errno;
::close(fd);
errno = r_errno;
return kErrFailErrno;
}
checksum = crc32part((uint8_t *)_work_buffer2, bytes_read, checksum);
} while (bytes_read == _work_buffer2_len);
::close(fd);
payload->size = sizeof(uint32_t);
std::memcpy(payload->data, &checksum, payload->size);
return kErrNone;
}
static int zm_dataevent(FAR struct zm_state_s *pzm)
{
zmdbg("Received type: %d length: %d\n", pzm->pkttype, pzm->pktlen);
zmdbg("PSTATE %d:%d->%d:%d\n",
pzm->pstate, pzm->psubstate, PSTATE_IDLE, PIDLE_ZPAD);
/* Revert to the IDLE state */
pzm->pstate = PSTATE_IDLE;
pzm->psubstate = PIDLE_ZPAD;
/* Verify the checksum. 16- or 32-bit? */
if (pzm->hdrfmt == ZBIN32)
{
uint32_t crc;
crc = crc32part(pzm->pktbuf, pzm->pktlen, 0xffffffff);
if (crc != 0xdebb20e3)
{
zmdbg("ERROR: ZBIN32 CRC32 failure: %08x vs debb20e3\n", crc);
pzm->flags &= ~ZM_FLAG_CRKOK;
}
else
{
pzm->flags |= ZM_FLAG_CRKOK;
}
/* Adjust the back length to exclude the packet type length of the 4-
* byte checksum.
*/
pzm->pktlen -= 5;
}
else
{
uint16_t crc;
crc = crc16part(pzm->pktbuf, pzm->pktlen, 0);
if (crc != 0)
{
zmdbg("ERROR: ZBIN/ZHEX CRC16 failure: %04x vs 0000\n", crc);
pzm->flags &= ~ZM_FLAG_CRKOK;
}
else
{
pzm->flags |= ZM_FLAG_CRKOK;
}
/* Adjust the back length to exclude the packet type length of the 2-
* byte checksum.
*/
pzm->pktlen -= 3;
}
/* Then handle the data received event */
return zm_event(pzm, ZME_DATARCVD);
}
/**
* @fn :tc_libc_misc_crc32part
* @brief :Continue CRC calculation on a part of the buffer.
* @scenario :Continue CRC calculation on a part of the buffer.
* @API's covered :crc32part
* @Preconditions :None
* @Postconditions :None
* @Return :void
*/
static void tc_libc_misc_crc32part(void)
{
uint32_t ret_chk;
uint8_t src_arr[1] = { VAL_100 };
uint32_t crc_32val = 0;
size_t length = 1;
/* Return value should be 0x4adfa541 as calculated by crc32part */
ret_chk = crc32part(src_arr, length, crc_32val);
TC_ASSERT_EQ("crc32part", ret_chk, (uint32_t)VAL_CRC32_1);
/* Return value should be 0x67dd4acc as calculated by crc32part */
crc_32val = VAL_255;
ret_chk = (uint32_t)crc32part(src_arr, length, crc_32val);
TC_ASSERT_EQ("crc32part", ret_chk, (uint32_t)VAL_CRC32_3);
TC_SUCCESS_RESULT();
}
static inline uint32_t infilecrc32(int infd, size_t len, size_t padlen)
{
off_t offset;
uint8_t buffer[IO_BUF_SIZE];
ssize_t nbytes;
size_t bytesread;
uint32_t crc;
offset = lseek(infd, 0, SEEK_SET);
if (offset == (off_t)-1)
{
fprintf(stderr, "lseek failed: %s\n", strerror(errno));
exit(4);
}
crc = 0;
for (bytesread = 0; bytesread < len; bytesread += nbytes)
{
nbytes = read(infd, buffer, IO_BUF_SIZE);
if (nbytes < 0)
{
fprintf(stderr, "read failed: %s\n", strerror(errno));
exit(4);
}
else if (nbytes == 0)
{
fprintf(stderr, "Unexpected end-of-file: %s\n", strerror(errno));
exit(4);
}
else
{
crc = crc32part(buffer, nbytes, crc);
}
}
/* Add the zero-padding at the end of the binary in the CRC */
memset(buffer, 0, IO_BUF_SIZE);
return crc32part(buffer, padlen, crc);
}
static void
calculate_fw_crc(void)
{
#define APP_SIZE_MAX 0xf000
#define APP_LOAD_ADDRESS 0x08001000
// compute CRC of the current firmware
uint32_t sum = 0;
for (unsigned p = 0; p < APP_SIZE_MAX; p += 4) {
uint32_t bytes = *(uint32_t *)(p + APP_LOAD_ADDRESS);
sum = crc32part((uint8_t *)&bytes, sizeof(bytes), sum);
}
r_page_setup[PX4IO_P_SETUP_CRC] = sum & 0xFFFF;
r_page_setup[PX4IO_P_SETUP_CRC+1] = sum >> 16;
}
static inline ssize_t nxffs_analyzedata(FAR struct nxffs_blkinfo_s *blkinfo,
int offset)
{
struct nxffs_data_s dathdr;
uint32_t ecrc;
uint16_t datlen;
uint32_t crc;
/* Copy and unpack the data block header */
memcpy(&dathdr, &blkinfo->buffer[offset], SIZEOF_NXFFS_DATA_HDR);
ecrc = nxffs_rdle32(dathdr.crc);
datlen = nxffs_rdle16(dathdr.datlen);
/* Sanity checks */
if (offset + SIZEOF_NXFFS_DATA_HDR + datlen > blkinfo->geo.blocksize)
{
/* Data does not fit in within the block, this can't be a data block */
return ERROR;
}
/* Calculate the CRC */
nxffs_wrle32(dathdr.crc, 0);
crc = crc32((FAR const uint8_t *)&dathdr, SIZEOF_NXFFS_DATA_HDR);
crc = crc32part(&blkinfo->buffer[offset + SIZEOF_NXFFS_DATA_HDR], datlen, crc);
if (crc != ecrc)
{
syslog(LOG_NOTICE, g_format,
blkinfo->block, offset, "DATA ", "CRC BAD", datlen);
return ERROR;
}
/* If must be a good header */
if (blkinfo->verbose)
{
syslog(LOG_NOTICE, g_format,
blkinfo->block, offset, "DATA ", "OK ", datlen);
}
return SIZEOF_NXFFS_DATA_HDR + datlen;
}
static int zm_hdrevent(FAR struct zm_state_s *pzm)
{
zmdbg("Received type: %d data: %02x %02x %02x %02x\n",
pzm->hdrdata[0],
pzm->hdrdata[1], pzm->hdrdata[2], pzm->hdrdata[3], pzm->hdrdata[4]);
zmdbg("PSTATE %d:%d->%d:%d\n",
pzm->pstate, pzm->psubstate, PSTATE_IDLE, PIDLE_ZPAD);
/* Revert to the IDLE state */
pzm->pstate = PSTATE_IDLE;
pzm->psubstate = PIDLE_ZPAD;
/* Verify the checksum. 16- or 32-bit? */
if (pzm->hdrfmt == ZBIN32)
{
uint32_t crc;
/* Checksum is over 9 bytes: The header type, 4 data bytes, plus 4 CRC bytes */
crc = crc32part(pzm->hdrdata, 9, 0xffffffff);
if (crc != 0xdebb20e3)
{
zmdbg("ERROR: ZBIN32 CRC32 failure: %08x vs debb20e3\n", crc);
return zm_nakhdr(pzm);
}
}
else
{
uint16_t crc;
/* Checksum is over 7 bytes: The header type, 4 data bytes, plus 2 CRC bytes */
crc = crc16part(pzm->hdrdata, 7, 0);
if (crc != 0)
{
zmdbg("ERROR: ZBIN/ZHEX CRC16 failure: %04x vs 0000\n", crc);
return zm_nakhdr(pzm);
}
}
return zm_event(pzm, pzm->hdrdata[0]);
}
int nxffs_wrinode(FAR struct nxffs_volume_s *volume,
FAR struct nxffs_entry_s *entry)
{
FAR struct nxffs_inode_s *inode;
uint32_t crc;
int namlen;
int ret;
/* Seek to the inode header position and assure that it is in the volume
* cache.
*/
nxffs_ioseek(volume, entry->hoffset);
ret = nxffs_rdcache(volume, volume->ioblock);
if (ret < 0)
{
fdbg("ERROR: Failed to read inode header block %d: %d\n",
volume->ioblock, -ret);
goto errout;
}
/* Get the length of the inode name */
namlen = strlen(entry->name);
DEBUGASSERT(namlen < CONFIG_NXFFS_MAXNAMLEN); /* This was verified earlier */
/* Initialize the inode header */
inode = (FAR struct nxffs_inode_s *)&volume->cache[volume->iooffset];
memcpy(inode->magic, g_inodemagic, NXFFS_MAGICSIZE);
inode->state = CONFIG_NXFFS_ERASEDSTATE;
inode->namlen = namlen;
nxffs_wrle32(inode->noffs, entry->noffset);
nxffs_wrle32(inode->doffs, entry->doffset);
nxffs_wrle32(inode->utc, entry->utc);
nxffs_wrle32(inode->crc, 0);
nxffs_wrle32(inode->datlen, entry->datlen);
/* Calculate the CRC */
crc = crc32((FAR const uint8_t *)inode, SIZEOF_NXFFS_INODE_HDR);
crc = crc32part((FAR const uint8_t *)entry->name, namlen, crc);
/* Finish the inode header */
inode->state = INODE_STATE_FILE;
nxffs_wrle32(inode->crc, crc);
/* Write the block with the inode header */
ret = nxffs_wrcache(volume);
if (ret < 0)
{
fdbg("ERROR: Failed to write inode header block %d: %d\n",
volume->ioblock, -ret);
}
/* The volume is now available for other writers */
errout:
sem_post(&volume->wrsem);
return ret;
}
static inline ssize_t nxffs_analyzeinode(FAR struct nxffs_blkinfo_s *blkinfo,
int offset)
{
FAR struct nxffs_inode_s inode;
off_t nextblock;
uint8_t state;
uint32_t noffs;
uint32_t doffs;
uint32_t utc;
uint32_t ecrc;
uint32_t datlen;
uint32_t crc;
size_t spaceleft;
/* Verify that there is space for an inode header remaining in the block */
if (offset + SIZEOF_NXFFS_INODE_HDR > blkinfo->geo.blocksize)
{
/* No.. then this can't be an inode header */
return ERROR;
}
/* Unpack the header */
memcpy(&inode, &blkinfo->buffer[offset], SIZEOF_NXFFS_INODE_HDR);
noffs = nxffs_rdle32(inode.noffs);
doffs = nxffs_rdle32(inode.doffs);
utc = nxffs_rdle32(inode.utc);
ecrc = nxffs_rdle32(inode.crc);
datlen = nxffs_rdle32(inode.datlen);
/* Misc. sanity checks */
if (noffs < blkinfo->offset + offset + SIZEOF_NXFFS_BLOCK_HDR)
{
/* The name begins before the inode header. This can't can't be
* a real inode header (or it is a corrupted one).
*/
return ERROR;
}
/* Can we verify the inode? We need to have the inode name in the same
* block to do that (or get access to the next block)
*/
if (doffs < blkinfo->offset + offset + SIZEOF_NXFFS_BLOCK_HDR)
{
/* The first data block begins before the inode header. This can't can't
* be a real inode header (or it is a corrupted one).
*/
return ERROR;
}
spaceleft = (blkinfo->nblocks - blkinfo->block) * blkinfo->geo.blocksize;
spaceleft -= (offset + SIZEOF_NXFFS_BLOCK_HDR);
if (datlen > spaceleft)
{
/* The data length is greater than what would fit in the rest of FLASH
* (even ignoring block and data header sizes.
*/
return ERROR;
}
/* The name begins after the inode header. Does it begin in this block? */
nextblock = blkinfo->offset + blkinfo->geo.blocksize;
if (noffs > nextblock)
{
/* Not than we cannot verify the inode header */
if (blkinfo->verbose)
{
fdbg(g_format, blkinfo->block, offset, "INODE", "UNVERFD", datlen);
}
return ERROR;
}
/* The name begins in this block. Does it also end in this block? */
if (noffs + inode.namlen > nextblock)
{
/* No.. Assume that this is not an inode. */
return ERROR;
}
/* Calculate the CRC */
state = inode.state;
inode.state = CONFIG_NXFFS_ERASEDSTATE;
nxffs_wrle32(inode.crc, 0);
crc = crc32((FAR const uint8_t *)&inode, SIZEOF_NXFFS_INODE_HDR);
crc = crc32part(&blkinfo->buffer[noffs - blkinfo->offset], inode.namlen, crc);
if (crc != ecrc)
{
fdbg(g_format, blkinfo->block, offset, "INODE", "CRC BAD", datlen);
return ERROR;
}
/* If must be a good header */
if (state == INODE_STATE_FILE)
{
if (blkinfo->verbose)
{
fdbg(g_format, blkinfo->block, offset, "INODE", "OK ", datlen);
}
}
else if (state == INODE_STATE_DELETED)
{
if (blkinfo->verbose)
{
fdbg(g_format, blkinfo->block, offset, "INODE", "DELETED", datlen);
}
}
else
{
fdbg(g_format, blkinfo->block, offset, "INODE", "CORRUPT", datlen);
}
/* Return the block-relative offset to the next byte after the inode name */
return noffs + inode.namlen - offset - blkinfo->offset;
}
int
PX4IO_Uploader::verify_rev3(size_t fw_size_local)
{
int ret;
uint8_t file_buf[4];
ssize_t count;
uint32_t sum = 0;
uint32_t bytes_read = 0;
uint32_t crc = 0;
uint32_t fw_size_remote;
uint8_t fill_blank = 0xff;
log("verify...");
lseek(_fw_fd, 0, SEEK_SET);
ret = get_info(INFO_FLASH_SIZE, fw_size_remote);
send(PROTO_EOC);
if (ret != OK) {
log("could not read firmware size");
return ret;
}
/* read through the firmware file again and calculate the checksum*/
while (bytes_read < fw_size_local) {
size_t n = fw_size_local - bytes_read;
if (n > sizeof(file_buf)) {
n = sizeof(file_buf);
}
count = read_with_retry(_fw_fd, file_buf, n);
if (count != (ssize_t)n) {
log("firmware read of %u bytes at %u failed -> %d errno %d",
(unsigned)n,
(unsigned)bytes_read,
(int)count,
(int)errno);
}
/* set the rest to ff */
if (count == 0) {
break;
}
/* stop if the file cannot be read */
if (count < 0)
return -errno;
/* calculate crc32 sum */
sum = crc32part((uint8_t *)&file_buf, sizeof(file_buf), sum);
bytes_read += count;
}
/* fill the rest with 0xff */
while (bytes_read < fw_size_remote) {
sum = crc32part(&fill_blank, sizeof(fill_blank), sum);
bytes_read += sizeof(fill_blank);
}
/* request CRC from IO */
send(PROTO_GET_CRC);
send(PROTO_EOC);
ret = recv_bytes((uint8_t*)(&crc), sizeof(crc));
if (ret != OK) {
log("did not receive CRC checksum");
return ret;
}
/* compare the CRC sum from the IO with the one calculated */
if (sum != crc) {
log("CRC wrong: received: %d, expected: %d", crc, sum);
return -EINVAL;
}
return OK;
}
static int nxffs_rdentry(FAR struct nxffs_volume_s *volume, off_t offset,
FAR struct nxffs_entry_s *entry)
{
struct nxffs_inode_s inode;
uint32_t ecrc;
uint32_t crc;
uint8_t state;
int namlen;
int ret;
DEBUGASSERT(volume && entry);
memset(entry, 0, sizeof(struct nxffs_entry_s));
/* Read the header at the FLASH offset */
nxffs_ioseek(volume, offset);
memcpy(&inode, &volume->cache[volume->iooffset], SIZEOF_NXFFS_INODE_HDR);
/* Check if the file state is recognized. */
state = inode.state;
if (state != INODE_STATE_FILE && state != INODE_STATE_DELETED)
{
/* This can't be a valid inode.. don't bother with the rest */
ret = -ENOENT;
goto errout_no_offset;
}
/* Copy the packed header into the user-friendly buffer */
entry->hoffset = offset;
entry->noffset = nxffs_rdle32(inode.noffs);
entry->doffset = nxffs_rdle32(inode.doffs);
entry->utc = nxffs_rdle32(inode.utc);
entry->datlen = nxffs_rdle32(inode.datlen);
/* Modify the packed header and perform the (partial) CRC calculation */
ecrc = nxffs_rdle32(inode.crc);
inode.state = CONFIG_NXFFS_ERASEDSTATE;
memset(inode.crc, 0, 4);
crc = crc32((FAR const uint8_t *)&inode, SIZEOF_NXFFS_INODE_HDR);
/* Allocate memory to hold the variable-length file name */
namlen = inode.namlen;
entry->name = (FAR char *)kmalloc(namlen + 1);
if (!entry->name)
{
fdbg("ERROR: Failed to allocate name, namlen: %d\n", namlen);
ret = -ENOMEM;
goto errout_no_offset;
}
/* Seek to the expected location of the name in FLASH */
nxffs_ioseek(volume, entry->noffset);
/* Make sure that the block is in memory (the name may not be in the
* same block as the inode header.
*/
ret = nxffs_rdcache(volume, volume->ioblock);
if (ret < 0)
{
fdbg("ERROR: nxffsx_rdcache failed: %d\n", -ret);
goto errout_with_name;
}
/* Read the file name from the expected offset in FLASH */
memcpy(entry->name, &volume->cache[volume->iooffset], namlen);
entry->name[namlen] = '\0';
/* Finish the CRC calculation and verify the entry */
crc = crc32part((FAR const uint8_t *)entry->name, namlen, crc);
if (crc != ecrc)
{
fdbg("ERROR: CRC entry: %08x CRC calculated: %08x\n", ecrc, crc);
ret = -EIO;
goto errout_with_name;
}
/* We have a good inode header.. but it still could a deleted file.
* Check the file state.
*/
if (state != INODE_STATE_FILE)
{
/* It is a deleted file. But still, the data offset and the
* start size are good so we can use this information to advance
* further in FLASH memory and reduce the search time.
*/
offset = nxffs_inodeend(volume, entry);
nxffs_freeentry(entry);
ret = -ENOENT;
goto errout;
}
/* Everything is good.. leave the offset pointing to the valid inode
* header.
*/
return OK;
/* On errors where we are suspicious of the validity of the inode header,
* we need to increment the file position to just after the "good" magic
* word.
*/
errout_with_name:
nxffs_freeentry(entry);
errout_no_offset:
offset += NXFFS_MAGICSIZE;
errout:
nxffs_ioseek(volume, offset);
return ret;
}
