diff --git a/src/base64.c b/src/base64.c index 73c3819..daa4d1d 100644 --- a/src/base64.c +++ b/src/base64.c @@ -76,8 +76,9 @@ unsigned char * base64_encode(const unsigned char *src, size_t len, line_len += 4; } - if (line_len) + /*if (line_len) *pos++ = '\n'; + */ *pos = '\0'; if (out_len) diff --git a/src/image.c b/src/image.c index ee2b12e..35c347d 100644 --- a/src/image.c +++ b/src/image.c @@ -16,7 +16,8 @@ #include "nmgr.h" #include "cbor.h" -#define IMAGE_CHUNK_SIZE 60 +#define IMAGE_CHUNK_SIZE 124 +#define MACRO_CHUNK_SIZE 340 #define CRC16_INITIAL_CRC 0 /* what to seed crc16 with */ #define CRC_CITT_POLYMINAL 0x1021 @@ -40,6 +41,31 @@ static char *hex(char in) { return out; } +static void read_response(uint8_t *dest, size_t *len) { + static uint8_t buf[BOOT_SERIAL_IN_MAX*2] = {0}; + static uint8_t *p = buf; + while (true) { + int bytes_read = read(serial_fd, p, BOOT_SERIAL_IN_MAX); + if (bytes_read < 0) { + if (errno == EAGAIN) { + continue; + } + fprintf(stderr, "Failed to read from serial port: %s\n", strerror(errno)); + exit(2); + } + p += bytes_read; + uint8_t *nl_p = memchr(buf, '\n', p-buf); + if (nl_p) { + *len = nl_p - buf; + memcpy(dest, buf, *len); + uint16_t new_buf_len = p - nl_p; + memcpy(buf, nl_p+1, new_buf_len); + p = buf+new_buf_len; + break; + } + } +} + static void hex_print(char *in, size_t len) { char tmp[len*2+1]; tmp[len] = 0; @@ -76,30 +102,23 @@ static void do_version(void) { return; } uint8_t tmp[BOOT_SERIAL_IN_MAX]; - int in_len; - while (1) { - if ((in_len = read(serial_fd, tmp, BOOT_SERIAL_IN_MAX)) < 0) { - if (errno == EAGAIN) { - fprintf(stderr, "non-blocking\n"); - continue; - } - fprintf(stderr, "Reading from serial failed: %s\n", strerror(errno)); - return; - } - break; - } + size_t in_len; + + read_response(tmp, &in_len); // Process the response CborParser parser; CborValue it; - assert_pkt_start(tmp); - size_t out_len; - + // Validate packet start marker if (in_len < 2) { fprintf(stderr, "packet too small, bailing"); exit(5); } + assert_pkt_start(tmp); + + // Decode the payload + size_t out_len; uint8_t *decoded_buf = base64_decode(tmp+2, in_len-2, &out_len); if (!decoded_buf) { @@ -108,17 +127,19 @@ static void do_version(void) { } // Unclear why this would be useful in a line oriented (cannonical mode) program - uint8_t *p = decoded_buf; - uint16_t pkt_len = ntohs(*(uint16_t *)p); - fflush(stdout); - p += sizeof(pkt_len); + uint16_t pkt_len = ntohs(*(uint16_t *)decoded_buf); + uint16_t crc = crc16(decoded_buf+sizeof(pkt_len), pkt_len, CRC_CITT_POLYMINAL, CRC16_INITIAL_CRC, true); + if (crc) { + fprintf(stderr, "CRC failed on response packet"); + exit(6); + } + uint8_t *hdr_start = decoded_buf + sizeof(pkt_len); - // Hdr also seems a bit pointless, maybe could check for success before CBOR deserialization? - struct nmgr_hdr hdr; - memcpy(&hdr, p, sizeof(struct nmgr_hdr)); - p += sizeof(struct nmgr_hdr); + // The header seems a bit pointless... at least for serial + struct nmgr_hdr *hdr = (struct nmgr_hdr *)(decoded_buf + sizeof(pkt_len)); + + uint8_t *p = (uint8_t *)hdr + sizeof(struct nmgr_hdr); cbor_parser_init(p, out_len, 0, &parser, &it); - cbor_require_type(&it, CborMapType); CborValue map; cbor_value_enter_container(&it, &map); @@ -128,32 +149,36 @@ static void do_version(void) { CborValue img_array; cbor_value_enter_container(&map, &img_array); + char *version = NULL; while (!cbor_value_at_end(&img_array)) { assert(cbor_value_get_type(&img_array) == CborMapType); CborValue version_tag; CborError err = cbor_value_map_find_value(&img_array, "version", &version_tag); - if (err) { - // There's no version key for this slot, probably an error - fprintf(stderr, "Failed to find version info: %s", cbor_error_string(err)); + if (cbor_value_get_type(&version_tag) == CborInvalidType) { cbor_value_advance(&img_array); continue; } - char *version; size_t len; cbor_value_dup_text_string(&version_tag, &version, &len, NULL); + break; + } + if (!version) { + printf("No applicaiton image found\n"); + } else { printf("%s\n", version); free(version); - break; } free(decoded_buf); } static void wrap_and_send_pkt(uint8_t *data, size_t len) { - uint8_t b64_buf[BOOT_SERIAL_OUT_MAX]; + fprintf(stderr, "Trying to wrap %d bytes", len); + uint8_t b64_buf[BOOT_SERIAL_OUT_MAX*2]; struct nmgr_hdr hdr; hdr.nh_op = NMGR_OP_WRITE; hdr.nh_group = htons(MGMT_GROUP_ID_IMAGE); hdr.nh_id = IMGMGR_NMGR_ID_UPLOAD; + hdr.nh_seq = 66; uint16_t crc = crc16((uint8_t*)&hdr, sizeof(struct nmgr_hdr), CRC_CITT_POLYMINAL, CRC16_INITIAL_CRC, false); crc = crc16(data, len, CRC_CITT_POLYMINAL, crc, true); @@ -172,15 +197,25 @@ static void wrap_and_send_pkt(uint8_t *data, size_t len) { size_t out_len; uint8_t *out_buf = base64_encode(b64_buf, tot_len, &out_len); + pos = out_buf; + while (pos < out_buf+out_len) { + uint8_t start_pkt[] = {SHELL_NLIP_PKT_START1, SHELL_NLIP_PKT_START2}; + uint8_t start_data[] = {SHELL_NLIP_DATA_START1, SHELL_NLIP_DATA_START2}; - fprintf(stderr, "Trying to wrap %u bytes\n", (unsigned) len); + if (pos == out_buf) { + write(serial_fd, start_pkt, sizeof(start_pkt)); + } else { + write(serial_fd, start_data, sizeof(start_data)); + } - uint8_t start[] = {SHELL_NLIP_PKT_START1, SHELL_NLIP_PKT_START2}; - write(serial_fd, start, sizeof(start)); - write(serial_fd, out_buf, out_len-1); - write(serial_fd, "\n", 1); - fsync(serial_fd); + size_t remaining = out_buf + out_len - pos; + size_t to_write = remaining > IMAGE_CHUNK_SIZE ? IMAGE_CHUNK_SIZE : remaining; + write(serial_fd, pos, to_write); + pos += to_write; + write(serial_fd, "\n", 1); + } free(out_buf); + fsync(serial_fd); } void do_upload(char *filename){ @@ -196,34 +231,37 @@ void do_upload(char *filename){ while (pos < len){ CborEncoder root, map; - uint8_t cbor_buf[BOOT_SERIAL_OUT_MAX]; - cbor_encoder_init(&root, cbor_buf, BOOT_SERIAL_OUT_MAX, 0); + uint8_t cbor_buf[BOOT_SERIAL_OUT_MAX*2]; + cbor_encoder_init(&root, cbor_buf, BOOT_SERIAL_OUT_MAX*2, 0); cbor_encoder_create_map(&root, &map, pos == 0 ? 3 : 2); - if (pos == 0) { - cbor_encode_text_stringz(&map, "len"); - cbor_encode_int(&map, len); - } - - cbor_encode_text_stringz(&map, "off"); - cbor_encode_int(&map, pos); - cbor_encode_text_stringz(&map, "data"); - uint8_t data[IMAGE_CHUNK_SIZE]; + uint8_t data[MACRO_CHUNK_SIZE]; int actual_size; - if ((actual_size = read(fd, data, IMAGE_CHUNK_SIZE)) < IMAGE_CHUNK_SIZE) { + if ((actual_size = read(fd, data, MACRO_CHUNK_SIZE)) < MACRO_CHUNK_SIZE) { if (errno != 0) { fprintf(stderr, "Error reading the firmware file: %s", strerror(errno)); exit(2); } } cbor_encode_byte_string(&map, data, actual_size); + + if (pos == 0) { + cbor_encode_text_stringz(&map, "len"); + cbor_encode_uint(&map, len); + } + + cbor_encode_text_stringz(&map, "off"); + cbor_encode_uint(&map, pos); + cbor_encoder_close_container(&root, &map); wrap_and_send_pkt(data, cbor_encoder_get_buffer_size(&root, cbor_buf)); uint8_t resp_buf[BOOT_SERIAL_IN_MAX]; - int in_len = read(serial_fd, &resp_buf, BOOT_SERIAL_IN_MAX); + size_t in_len; + read_response(resp_buf, &in_len); + /*int in_len = read(serial_fd, &resp_buf, BOOT_SERIAL_IN_MAX); if (in_len < 0) { fprintf(stderr, "Failed to read from serial port: %s", strerror(errno)); exit(4); @@ -236,7 +274,7 @@ void do_upload(char *filename){ printlen(resp_buf, in_len); fflush(stdout); - +*/ size_t out_len; uint8_t *out = base64_decode(resp_buf+2, in_len-2, &out_len); diff --git a/src/main.c b/src/main.c index 8ed0e4b..b1c9fff 100644 --- a/src/main.c +++ b/src/main.c @@ -44,7 +44,8 @@ int main(int argc, char **argv) { return 2; } - t_options.c_cflag[] + t_options.c_lflag &= ~(ICANON | ECHO | ECHOE); + t_options.c_oflag &= ~OPOST; if (tcsetattr(serial_fd, TCSANOW, &t_options) < 0){ fprintf(stderr, "Failed to set termios attrs.\n"); } diff --git a/src/sha256.c b/src/sha256.c new file mode 100644 index 0000000..eb9c5c0 --- /dev/null +++ b/src/sha256.c @@ -0,0 +1,158 @@ +/********************************************************************* +* Filename: sha256.c +* Author: Brad Conte (brad AT bradconte.com) +* Copyright: +* Disclaimer: This code is presented "as is" without any guarantees. +* Details: Implementation of the SHA-256 hashing algorithm. + SHA-256 is one of the three algorithms in the SHA2 + specification. The others, SHA-384 and SHA-512, are not + offered in this implementation. + Algorithm specification can be found here: + * http://csrc.nist.gov/publications/fips/fips180-2/fips180-2withchangenotice.pdf + This implementation uses little endian byte order. +*********************************************************************/ + +/*************************** HEADER FILES ***************************/ +#include +#include +#include "sha256.h" + +/****************************** MACROS ******************************/ +#define ROTLEFT(a,b) (((a) << (b)) | ((a) >> (32-(b)))) +#define ROTRIGHT(a,b) (((a) >> (b)) | ((a) << (32-(b)))) + +#define CH(x,y,z) (((x) & (y)) ^ (~(x) & (z))) +#define MAJ(x,y,z) (((x) & (y)) ^ ((x) & (z)) ^ ((y) & (z))) +#define EP0(x) (ROTRIGHT(x,2) ^ ROTRIGHT(x,13) ^ ROTRIGHT(x,22)) +#define EP1(x) (ROTRIGHT(x,6) ^ ROTRIGHT(x,11) ^ ROTRIGHT(x,25)) +#define SIG0(x) (ROTRIGHT(x,7) ^ ROTRIGHT(x,18) ^ ((x) >> 3)) +#define SIG1(x) (ROTRIGHT(x,17) ^ ROTRIGHT(x,19) ^ ((x) >> 10)) + +/**************************** VARIABLES *****************************/ +static const WORD k[64] = { + 0x428a2f98,0x71374491,0xb5c0fbcf,0xe9b5dba5,0x3956c25b,0x59f111f1,0x923f82a4,0xab1c5ed5, + 0xd807aa98,0x12835b01,0x243185be,0x550c7dc3,0x72be5d74,0x80deb1fe,0x9bdc06a7,0xc19bf174, + 0xe49b69c1,0xefbe4786,0x0fc19dc6,0x240ca1cc,0x2de92c6f,0x4a7484aa,0x5cb0a9dc,0x76f988da, + 0x983e5152,0xa831c66d,0xb00327c8,0xbf597fc7,0xc6e00bf3,0xd5a79147,0x06ca6351,0x14292967, + 0x27b70a85,0x2e1b2138,0x4d2c6dfc,0x53380d13,0x650a7354,0x766a0abb,0x81c2c92e,0x92722c85, + 0xa2bfe8a1,0xa81a664b,0xc24b8b70,0xc76c51a3,0xd192e819,0xd6990624,0xf40e3585,0x106aa070, + 0x19a4c116,0x1e376c08,0x2748774c,0x34b0bcb5,0x391c0cb3,0x4ed8aa4a,0x5b9cca4f,0x682e6ff3, + 0x748f82ee,0x78a5636f,0x84c87814,0x8cc70208,0x90befffa,0xa4506ceb,0xbef9a3f7,0xc67178f2 +}; + +/*********************** FUNCTION DEFINITIONS ***********************/ +void sha256_transform(SHA256_CTX *ctx, const BYTE data[]) +{ + WORD a, b, c, d, e, f, g, h, i, j, t1, t2, m[64]; + + for (i = 0, j = 0; i < 16; ++i, j += 4) + m[i] = (data[j] << 24) | (data[j + 1] << 16) | (data[j + 2] << 8) | (data[j + 3]); + for ( ; i < 64; ++i) + m[i] = SIG1(m[i - 2]) + m[i - 7] + SIG0(m[i - 15]) + m[i - 16]; + + a = ctx->state[0]; + b = ctx->state[1]; + c = ctx->state[2]; + d = ctx->state[3]; + e = ctx->state[4]; + f = ctx->state[5]; + g = ctx->state[6]; + h = ctx->state[7]; + + for (i = 0; i < 64; ++i) { + t1 = h + EP1(e) + CH(e,f,g) + k[i] + m[i]; + t2 = EP0(a) + MAJ(a,b,c); + h = g; + g = f; + f = e; + e = d + t1; + d = c; + c = b; + b = a; + a = t1 + t2; + } + + ctx->state[0] += a; + ctx->state[1] += b; + ctx->state[2] += c; + ctx->state[3] += d; + ctx->state[4] += e; + ctx->state[5] += f; + ctx->state[6] += g; + ctx->state[7] += h; +} + +void sha256_init(SHA256_CTX *ctx) +{ + ctx->datalen = 0; + ctx->bitlen = 0; + ctx->state[0] = 0x6a09e667; + ctx->state[1] = 0xbb67ae85; + ctx->state[2] = 0x3c6ef372; + ctx->state[3] = 0xa54ff53a; + ctx->state[4] = 0x510e527f; + ctx->state[5] = 0x9b05688c; + ctx->state[6] = 0x1f83d9ab; + ctx->state[7] = 0x5be0cd19; +} + +void sha256_update(SHA256_CTX *ctx, const BYTE data[], size_t len) +{ + WORD i; + + for (i = 0; i < len; ++i) { + ctx->data[ctx->datalen] = data[i]; + ctx->datalen++; + if (ctx->datalen == 64) { + sha256_transform(ctx, ctx->data); + ctx->bitlen += 512; + ctx->datalen = 0; + } + } +} + +void sha256_final(SHA256_CTX *ctx, BYTE hash[]) +{ + WORD i; + + i = ctx->datalen; + + // Pad whatever data is left in the buffer. + if (ctx->datalen < 56) { + ctx->data[i++] = 0x80; + while (i < 56) + ctx->data[i++] = 0x00; + } + else { + ctx->data[i++] = 0x80; + while (i < 64) + ctx->data[i++] = 0x00; + sha256_transform(ctx, ctx->data); + memset(ctx->data, 0, 56); + } + + // Append to the padding the total message's length in bits and transform. + ctx->bitlen += ctx->datalen * 8; + ctx->data[63] = ctx->bitlen; + ctx->data[62] = ctx->bitlen >> 8; + ctx->data[61] = ctx->bitlen >> 16; + ctx->data[60] = ctx->bitlen >> 24; + ctx->data[59] = ctx->bitlen >> 32; + ctx->data[58] = ctx->bitlen >> 40; + ctx->data[57] = ctx->bitlen >> 48; + ctx->data[56] = ctx->bitlen >> 56; + sha256_transform(ctx, ctx->data); + + // Since this implementation uses little endian byte ordering and SHA uses big endian, + // reverse all the bytes when copying the final state to the output hash. + for (i = 0; i < 4; ++i) { + hash[i] = (ctx->state[0] >> (24 - i * 8)) & 0x000000ff; + hash[i + 4] = (ctx->state[1] >> (24 - i * 8)) & 0x000000ff; + hash[i + 8] = (ctx->state[2] >> (24 - i * 8)) & 0x000000ff; + hash[i + 12] = (ctx->state[3] >> (24 - i * 8)) & 0x000000ff; + hash[i + 16] = (ctx->state[4] >> (24 - i * 8)) & 0x000000ff; + hash[i + 20] = (ctx->state[5] >> (24 - i * 8)) & 0x000000ff; + hash[i + 24] = (ctx->state[6] >> (24 - i * 8)) & 0x000000ff; + hash[i + 28] = (ctx->state[7] >> (24 - i * 8)) & 0x000000ff; + } +} diff --git a/src/sha256.h b/src/sha256.h new file mode 100644 index 0000000..7123a30 --- /dev/null +++ b/src/sha256.h @@ -0,0 +1,34 @@ +/********************************************************************* +* Filename: sha256.h +* Author: Brad Conte (brad AT bradconte.com) +* Copyright: +* Disclaimer: This code is presented "as is" without any guarantees. +* Details: Defines the API for the corresponding SHA1 implementation. +*********************************************************************/ + +#ifndef SHA256_H +#define SHA256_H + +/*************************** HEADER FILES ***************************/ +#include + +/****************************** MACROS ******************************/ +#define SHA256_BLOCK_SIZE 32 // SHA256 outputs a 32 byte digest + +/**************************** DATA TYPES ****************************/ +typedef unsigned char BYTE; // 8-bit byte +typedef unsigned int WORD; // 32-bit word, change to "long" for 16-bit machines + +typedef struct { + BYTE data[64]; + WORD datalen; + unsigned long long bitlen; + WORD state[8]; +} SHA256_CTX; + +/*********************** FUNCTION DECLARATIONS **********************/ +void sha256_init(SHA256_CTX *ctx); +void sha256_update(SHA256_CTX *ctx, const BYTE data[], size_t len); +void sha256_final(SHA256_CTX *ctx, BYTE hash[]); + +#endif // SHA256_H