void print(int *msg, int rank, int dim, unsigned int power2[12]){ int i; char *type; switch(msg[TYPEORIGINAL]){ case MSG: type = "MSG"; break; case ACK: type = "ACK"; break; } printf("%d (%s): Message (%d) #%d from %d(%s) to %d(%s) in %d hops ( ", msg[SOURCEORIGINAL], (byte2bin( msg[SOURCEORIGINAL], dim, power2)), msg[TYPEORIGINAL], msg[MSGNO], msg[SOURCEORIGINAL], (byte2bin( msg[SOURCEORIGINAL], dim, power2)), msg[SOURCE], (byte2bin( msg[SOURCE], dim, power2)), (msg[HOPCNT]-1)); for(i = 1 ; i <= msg[HOPCNT] ; i++){ printf("%s ->", (byte2bin( msg[HOPCNT+i], dim, power2))); } printf("%s)\n", (byte2bin( msg[SOURCE], dim, power2))); }
int data2bin(FAR uint8_t* dest, FAR const uint8_t *src, int nsrcbytes) { int byte; /* An even number of source bytes is expected */ if ((nsrcbytes & 1) != 0) { return -EINVAL; } /* Convert src bytes in groups of 2, writing one byte to the output on each * pass through the loop. */ while (nsrcbytes > 0) { /* Get the MS nibble (big endian order) */ byte = byte2bin(src); if (byte < 0) { return byte; } src += 2; /* And write the byte to the destination */ *dest++ = byte; nsrcbytes -= 2; } return OK; }
int hex2bin(FAR struct lib_instream_s *instream, FAR struct lib_sostream_s *outstream, uint32_t baseaddr, uint32_t endpaddr, enum hex2bin_swap_e swap) { FAR uint8_t *alloc; FAR uint8_t *line; FAR uint8_t *bin; int nbytes; int bytecount; uint32_t address; uint32_t endaddr; uint32_t expected; uint16_t extension; uint16_t address16; uint8_t checksum; unsigned int lineno; int i; int ret = OK; /* Allocate buffer memory */ alloc = (FAR uint8_t *)malloc(LINE_ALLOC + BIN_ALLOC); if (alloc == NULL) { hex2bin_debug("ERROR: Failed to allocate memory\n"); return -ENOMEM; } line = alloc; bin = &alloc[LINE_ALLOC]; extension = 0; expected = 0; lineno = 0; while ((nbytes = readstream(instream, line, lineno)) != EOF) { /* Increment the line number */ lineno++; /* Did we read enough data to do anything? */ if (nbytes < MINRECORD_ASCSIZE) { hex2bin_debug("Line %u ERROR: Record too short: %d\n", lineno, nbytes); goto errout_with_einval; } /* We should always read an even number of bytes */ if ((nbytes & 1) != 0) { hex2bin_debug("Line %u ERROR: Record length is odd: %d\n", lineno, nbytes); goto errout_with_einval; } /* Get the data byte count */ bytecount = byte2bin(&line[BYTECOUNT_LINENDX]); if (bytecount < 0) { hex2bin_debug("Line %u ERROR: Failed to read bytecount: %d\n", lineno, bytecount); ret = bytecount; goto errout_with_buffers; } /* Verify that the bytecount matches the length of the record */ if (RECORD_ASCSIZE(bytecount) != nbytes) { hex2bin_debug("Line %u ERROR: Expected %d bytes, read %d\n", lineno, RECORD_ASCSIZE(bytecount), nbytes); goto errout_with_einval; } /* Convert the entire line to binary. We need to do this for * checksum calculation which includes the entire line (minus * the start code and the checksum at the end of the line itself) */ ret = data2bin(bin, line, nbytes); if (ret < 0) { hex2bin_debug("Line %u ERROR: Failed to convert line to binary: %d\n", lineno, ret); goto errout_with_buffers; } /* Calculate and verify the checksum over all of the data */ nbytes >>= 1; /* Number of bytes in bin[] */ checksum = 0; for (i = 0; i < nbytes; i++) { checksum += bin[i]; } if (checksum != 0) { hex2bin_debug("Line %u ERROR: Bad checksum %02x\n", lineno, checksum); goto errout_with_einval; } /* Get the 16-bit (unextended) address from the record */ address16 = (uint16_t)bin[ADDRESS_BINNDX] << 8 | (uint16_t)bin[ADDRESS_BINNDX+1]; /* Handle the record by its record type */ switch (bin[RECTYPE_BINNDX]) { case RECORD_DATA: /* Data */ { /* Swap data in place in the binary buffer as required */ switch (swap) { case HEX2BIN_NOSWAP: /* No swap, stream is the correct byte order */ break; case HEX2BIN_SWAP16: /* Swap bytes in 16-bit values */ { if ((bytecount & 1) != 0) { hex2bin_debug("Line %d ERROR: Byte count %d is not a multiple of 2\n", lineno, bytecount); goto errout_with_einval; } /* Do the byte swap */ hex2bin_swap16(&bin[DATA_BINNDX], bytecount); } break; case HEX2BIN_SWAP32: /* Swap bytes in 32-bit values */ { if ((bytecount & 3) != 0) { hex2bin_debug("Line %d ERROR: Byte count %d is not a multiple of 4\n", lineno, bytecount); goto errout_with_einval; } /* Do the byte swap */ hex2bin_swap32(&bin[DATA_BINNDX], bytecount); } break; default: { hex2bin_debug("ERROR: Invalid swap argument: %d\n", swap); goto errout_with_einval; } } /* Get and verify the full 32-bit address */ address = ((uint32_t)extension << 16) | (uint32_t)address16; endaddr = address + bytecount; if (address < baseaddr || (endpaddr != 0 && endaddr >= endpaddr)) { hex2bin_debug("Line %d ERROR: Extended address %08lx is out of range\n", lineno, (unsigned long)address); goto errout_with_einval; } /* Seek to the correct position in the OUT stream if we have * made an unexpected jump in the data address. */ if (address != expected) { off_t pos = outstream->seek(outstream, address - baseaddr, SEEK_SET); if (pos == (off_t)-1) { hex2bin_debug("Line %u ERROR: Seek to address %08lu failed\n", lineno, (unsigned long)address); ret = -ESPIPE; goto errout_with_buffers; } } /* Transfer data to the OUT stream */ writedata(outstream, &bin[DATA_BINNDX], bytecount); /* This is the next data address that we expect to see */ expected = address + bytecount; } break; case RECORD_EOF: /* End of file */ /* End Of File record. Must occur exactly once per file in the * last line of the file. The byte count is 00 and the data field * is empty. Usually the address field is also 0000. */ if (bytecount == 0) { ret = OK; goto exit_with_buffers; } hex2bin_debug("Line %u ERROR: Nonzero bytecount %d in EOF\n", lineno, bytecount); goto errout_with_einval; case RECORD_EXT_SEGADDR: /* Extended segment address record */ /* The address specified by the data field is multiplied by 16 * (shifted 4 bits left) and added to the subsequent data record * addresses. This allows addressing of up to a megabyte of * address space. The address field of this record has to be * 0000, the byte count is 02 (the segment is 16-bit). The * least significant hex digit of the segment address is always * 0. */ if (bytecount != 2 || address16 != 0 || bin[DATA_BINNDX+1] != 0) { hex2bin_debug("Line %u ERROR: Invalid segment address\n", lineno); hex2bin_debug(" bytecount=%d address=%04x segment=%02x%02x\n", bytecount, address16, bin[DATA_BINNDX], bin[DATA_BINNDX+1]); goto errout_with_einval; } extension = (uint16_t)bin[DATA_BINNDX]; break; case RECORD_START_SEGADDR: /* Start segment address record */ /* For 80x86 processors, it specifies the initial content of * the CS:IP registers. The address field is 0000, the byte * count is 04, the first two bytes are the CS value, the * latter two are the IP value. */ break; case RECORD_EXT_LINADDR: /* Extended linear address record */ /* The address field is 0000, the byte count is 02. The two * data bytes (two hex digit pairs in big endian order) * represent the upper 16 bits of the 32 bit address for * all subsequent 00 type records until the next 04 type * record comes. If there is not a 04 type record, the * upper 16 bits default to 0000. To get the absolute * address for subsequent 00 type records, the address * specified by the data field of the most recent 04 record * is added to the 00 record addresses. */ if (bytecount != 2 || address16 != 0) { hex2bin_debug("Line %u ERROR: Invalid linear address\n", lineno); hex2bin_debug(" bytecount=%d address=%04x\n", bytecount, address16); goto errout_with_einval; } extension = (uint16_t)bin[DATA_BINNDX] << 8 | (uint16_t)bin[DATA_BINNDX+1]; break; case RECORD_START_LINADDR: /* Start linear address record */ /* The address field is 0000, the byte count is 04. The 4 * data bytes represent the 32-bit value loaded into the EIP * register of the 80386 and higher CPU. */ break; default: break; } } hex2bin_debug("ERROR: No EOF record found\n"); errout_with_einval: ret = -EINVAL; errout_with_buffers: exit_with_buffers: free(alloc); return ret; }