From 817c1bc0b61d9a35422cc9add67d0666ba72aebd Mon Sep 17 00:00:00 2001 From: David Woodhouse Date: Thu, 9 Aug 2007 16:23:56 +0800 Subject: Add utilities for multicast send/receive of MTD images. Signed-off-by: David Woodhouse --- recv_image.c | 356 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 356 insertions(+) create mode 100644 recv_image.c (limited to 'recv_image.c') diff --git a/recv_image.c b/recv_image.c new file mode 100644 index 0000000..d2b8813 --- /dev/null +++ b/recv_image.c @@ -0,0 +1,356 @@ + +#define _XOPEN_SOURCE 500 + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "crc32.h" +#include "mtd/mtd-user.h" +#include "mcast_image.h" + +#define min(x,y) ( (x)>(y)?(y):(x) ) + +int main(int argc, char **argv) +{ + struct sockaddr_storage server_addr; + socklen_t server_addrlen = sizeof(server_addr); + struct addrinfo *ai; + struct addrinfo hints; + struct addrinfo *runp; + int ret; + int sock; + struct image_pkt pktbuf; + size_t len; + int flfd; + struct mtd_info_user meminfo; + unsigned char *eb_buf; + unsigned char *blockmap = NULL; + unsigned char *subblockmap; + int nr_blocks = 0; + int nr_subblocks = 0; + int pkts_per_block; + int block_nr = -1; + uint32_t image_crc; + uint32_t blocks_received = 0; + uint32_t block_ofs; + loff_t mtdoffset = 0; + int *stats; + int badcrcs = 0; + int duplicates = 0; + int missing = -1; + int file_mode = 0; + + if (argc != 4) { + fprintf(stderr, "usage: %s \n", + (strrchr(argv[0], '/')?:argv[0]-1)+1); + exit(1); + } + /* Open the device */ + flfd = open(argv[3], O_RDWR); + + if (flfd >= 0) { + /* Fill in MTD device capability structure */ + if (ioctl(flfd, MEMGETINFO, &meminfo) != 0) { + perror("MEMGETINFO"); + close(flfd); + flfd = -1; + } else { + printf("Receive to MTD device %s with erasesize %d\n", + argv[3], meminfo.erasesize); + } + } + if (flfd == -1) { + /* Try again, as if it's a file */ + flfd = open(argv[3], O_CREAT|O_TRUNC|O_WRONLY, 0644); + if (flfd < 0) { + perror("open"); + exit(1); + } + meminfo.erasesize = 131072; + file_mode = 1; + printf("Receive to file %s with (assumed) erasesize %d\n", + argv[3], meminfo.erasesize); + } + + pkts_per_block = (meminfo.erasesize + PKT_SIZE - 1) / PKT_SIZE; + + stats = malloc(pkts_per_block + 1); + if (!stats) { + fprintf(stderr, "No memory for statistics\n"); + exit(1); + } + memset(stats, 0, sizeof(int) * (pkts_per_block + 1)); + + eb_buf = malloc(pkts_per_block * PKT_SIZE); + if (!eb_buf) { + fprintf(stderr, "No memory for eraseblock buffer\n"); + exit(1); + } + memset(eb_buf, 0, pkts_per_block * PKT_SIZE); + + subblockmap = malloc(pkts_per_block + 1); + if (!subblockmap) { + fprintf(stderr, "No memory for subblock map\n"); + exit(1); + } + memset(subblockmap, 0, pkts_per_block + 1); + + memset(&hints, 0, sizeof(hints)); + hints.ai_flags = AI_ADDRCONFIG; + hints.ai_socktype = SOCK_DGRAM; + + ret = getaddrinfo(argv[1], argv[2], &hints, &ai); + if (ret) { + fprintf(stderr, "getaddrinfo: %s\n", gai_strerror(ret)); + exit(1); + } + runp = ai; + for (runp = ai; runp; runp = runp->ai_next) { + sock = socket(runp->ai_family, runp->ai_socktype, + runp->ai_protocol); + if (sock == -1) { + perror("socket"); + continue; + } + if (runp->ai_family == AF_INET && + IN_MULTICAST( ntohl(((struct sockaddr_in *)runp->ai_addr)->sin_addr.s_addr))) { + struct ip_mreq rq; + rq.imr_multiaddr = ((struct sockaddr_in *)runp->ai_addr)->sin_addr; + rq.imr_interface.s_addr = INADDR_ANY; + if (setsockopt(sock, IPPROTO_IP, IP_ADD_MEMBERSHIP, &rq, sizeof(rq))) { + perror("IP_ADD_MEMBERSHIP"); + close(sock); + continue; + } + + } else if (runp->ai_family == AF_INET6 && + ((struct sockaddr_in6 *)runp->ai_addr)->sin6_addr.s6_addr[0] == 0xff) { + struct ipv6_mreq rq; + rq.ipv6mr_multiaddr = ((struct sockaddr_in6 *)runp->ai_addr)->sin6_addr; + rq.ipv6mr_interface = 0; + if (setsockopt(sock, IPPROTO_IPV6, IPV6_ADD_MEMBERSHIP, &rq, sizeof(rq))) { + perror("IPV6_ADD_MEMBERSHIP"); + close(sock); + continue; + } + } else printf("not multicast?\n"); + if (bind(sock, runp->ai_addr, runp->ai_addrlen)) { + perror("bind"); + close(sock); + continue; + } + break; + } + if (!runp) + exit(1); + + while ((len = read(sock, &pktbuf, sizeof(pktbuf))) >= 0) { + if (len < sizeof(pktbuf.hdr)) { + fprintf(stderr, "Short read %d bytes\n", len); + continue; + } + if (len != sizeof(pktbuf.hdr) + ntohl(pktbuf.hdr.thislen)) { + fprintf(stderr, "Wrong length %d bytes (expected %d+%d)\n", + len, sizeof(pktbuf.hdr), ntohl(pktbuf.hdr.thislen)); + continue; + } + /* Holds _data_ length */ + len -= sizeof(pktbuf.hdr); + + if (!blockmap) { + image_crc = pktbuf.hdr.totcrc; + if (meminfo.erasesize != ntohl(pktbuf.hdr.blocksize)) { + fprintf(stderr, "Erasesize mismatch (0x%x not 0x%x)\n", + ntohl(pktbuf.hdr.blocksize), meminfo.erasesize); + exit(1); + } + nr_blocks = ntohl(pktbuf.hdr.nr_blocks); + nr_subblocks = pkts_per_block + 2; + blockmap = malloc(nr_blocks); + if (!blockmap) { + fprintf(stderr, "No memory for block map\n"); + exit(1); + } + memset(blockmap, 0, nr_blocks); + } + if (image_crc != pktbuf.hdr.totcrc) { + fprintf(stderr, "Image CRC changed from 0x%x to 0x%x. Aborting\n", + ntohl(image_crc), ntohl(pktbuf.hdr.totcrc)); + exit(1); + } + if (ntohl(pktbuf.hdr.block_nr) != block_nr) { + /* Hm, new block */ + if (nr_subblocks < pkts_per_block && + block_nr != -1) + printf("Lost image block at %08x with only %d/%d packets\n", + block_nr * meminfo.erasesize, nr_subblocks, + pkts_per_block + 1); + + + if (nr_subblocks < pkts_per_block + 2) + stats[nr_subblocks]++; + + nr_subblocks = 0; + missing = -1; + memset(subblockmap, 0, pkts_per_block + 1); + block_nr = ntohl(pktbuf.hdr.block_nr); + if (block_nr > nr_blocks) { + fprintf(stderr, "Erroneous block_nr %d (> %d)\n", + block_nr, nr_blocks); + exit(1); + } + if (blockmap[block_nr]) { + printf("Discard chunk at 0x%08x for already-flashed eraseblock (%d to go)\n", + block_nr * meminfo.erasesize, nr_blocks - blocks_received); + nr_subblocks = pkts_per_block + 2; + continue; + } + } + if (nr_subblocks == pkts_per_block) { + /* We have a parity block but we didn't need it */ + nr_subblocks++; + continue; + } + if (blockmap[block_nr]) + continue; + + block_ofs = ntohl(pktbuf.hdr.block_ofs); + if (block_ofs == meminfo.erasesize) + block_ofs = PKT_SIZE * pkts_per_block; + + if (len != PKT_SIZE && len + block_ofs != meminfo.erasesize) { + fprintf(stderr, "Bogus packet size 0x%x (expected 0x%x)\n", + ntohl(pktbuf.hdr.thislen), + min(PKT_SIZE, meminfo.erasesize - block_ofs)); + exit(1); + } + + if (crc32(-1, pktbuf.data, len) != ntohl(pktbuf.hdr.thiscrc)) { + printf("Discard chunk %08x with bad CRC (%08x not %08x)\n", + block_nr * meminfo.erasesize + block_ofs, + crc32(-1, pktbuf.data, pktbuf.hdr.thislen), + ntohl(pktbuf.hdr.thiscrc)); + badcrcs++; + continue; + } + if (subblockmap[block_ofs / PKT_SIZE]) { + printf("Discarding duplicate packet at %08x\n", + block_nr * meminfo.erasesize + block_ofs); + duplicates++; + continue; + } + subblockmap[block_ofs / PKT_SIZE] = 1; + nr_subblocks++; + if (block_ofs < meminfo.erasesize) { + /* Normal data packet */ + memcpy(eb_buf + block_ofs, pktbuf.data, len); +// printf("Received data block at %08x\n", block_nr * meminfo.erasesize + block_ofs); + } else { + /* Parity block */ + int i; + + /* If we don't have enough to recover, skip */ + if (nr_subblocks < pkts_per_block) + continue; + + for (i = 0; i= meminfo.size) { + fprintf(stderr, "Run out of space on flash\n"); + exit(1); + } + while (ioctl(flfd, MEMGETBADBLOCK, &mtdoffset) > 0) { + printf("Skipping flash bad block at %08x\n", (uint32_t)mtdoffset); + mtdoffset += meminfo.erasesize; + } + wrotelen = pwrite(flfd, eb_buf, meminfo.erasesize, mtdoffset); + if (wrotelen != meminfo.erasesize) { + struct erase_info_user erase; + + if (wrotelen < 0) + perror("flash write"); + else + fprintf(stderr, "Short write to flash at %08x: %zd bytes\n", + (uint32_t)mtdoffset, wrotelen); + + erase.start = mtdoffset; + erase.length = meminfo.erasesize; + + if (ioctl(flfd, MEMERASE, erase)) { + perror("MEMERASE"); + exit(1); + } + /* skip it */ + // ioctl(flfd, MEMSETBADBLOCK, &mtdoffset); + mtdoffset += meminfo.erasesize; + goto again; + } + printf("Wrote image block %08x (%d/%d) to flash offset %08x%s\n", + block_nr * meminfo.erasesize, + blocks_received, nr_blocks, + (uint32_t)mtdoffset, + (missing==-1)?"":" (parity)"); + mtdoffset += meminfo.erasesize; + } + if (!(blocks_received%100) || blocks_received == nr_blocks) { + int i, printed = 0; + for (i=0; i <= pkts_per_block + 1; i++) { + if (printed || stats[i]) { + printf("Number of blocks with %d packets received: %d\n", + i, stats[i]); + printed = 1; + } + } + printf("Bad CRC: %d\n", badcrcs); + printf("Duplicate: %d\n", duplicates); + + } + if (blocks_received == nr_blocks) { + printf("Got all %08x bytes of image. Bye!\n", + nr_blocks * meminfo.erasesize); + exit(0); + } + } + } + close(sock); +} -- cgit v1.2.3