From fa35034fd6cc3659c0995a08ddb5571621acdb64 Mon Sep 17 00:00:00 2001 From: Shawn Nock Date: Thu, 2 May 2019 12:48:43 +0000 Subject: [PATCH] Initial working "image version" response parsing --- src/base64.c | 155 +++++++++++++++++++++++++++++++++++++++++++++++++++ src/base64.h | 13 +++++ src/image.c | 141 ++++++++++++++++++++++++++++++++++++++-------- src/image.h | 2 +- src/main.c | 6 +- src/nmgr.h | 3 + 6 files changed, 292 insertions(+), 28 deletions(-) create mode 100644 src/base64.c create mode 100644 src/base64.h diff --git a/src/base64.c b/src/base64.c new file mode 100644 index 0000000..aa8f464 --- /dev/null +++ b/src/base64.c @@ -0,0 +1,155 @@ +/* + * Base64 encoding/decoding (RFC1341) + * Copyright (c) 2019, Shawn Nock + * Copyright (c) 2005-2011, Jouni Malinen + * + * This software may be distributed under the terms of the BSD license. + * See README for more details. + */ + +#include + +#include "base64.h" + +static const unsigned char base64_table[65] = + "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/"; + +/** + * base64_encode - Base64 encode + * @src: Data to be encoded + * @len: Length of the data to be encoded + * @out_len: Pointer to output length variable, or %NULL if not used + * Returns: Allocated buffer of out_len bytes of encoded data, + * or %NULL on failure + * + * Caller is responsible for freeing the returned buffer. Returned buffer is + * nul terminated to make it easier to use as a C string. The nul terminator is + * not included in out_len. + */ +unsigned char * base64_encode(const unsigned char *src, size_t len, + size_t *out_len) +{ + unsigned char *out, *pos; + const unsigned char *end, *in; + size_t olen; + int line_len; + + olen = len * 4 / 3 + 4; /* 3-byte blocks to 4-byte */ + olen += olen / 72; /* line feeds */ + olen++; /* nul termination */ + if (olen < len) + return NULL; /* integer overflow */ + out = malloc(olen); + if (out == NULL) + return NULL; + + end = src + len; + in = src; + pos = out; + line_len = 0; + while (end - in >= 3) { + *pos++ = base64_table[in[0] >> 2]; + *pos++ = base64_table[((in[0] & 0x03) << 4) | (in[1] >> 4)]; + *pos++ = base64_table[((in[1] & 0x0f) << 2) | (in[2] >> 6)]; + *pos++ = base64_table[in[2] & 0x3f]; + in += 3; + line_len += 4; + if (line_len >= 72) { + *pos++ = '\n'; + line_len = 0; + } + } + + if (end - in) { + *pos++ = base64_table[in[0] >> 2]; + if (end - in == 1) { + *pos++ = base64_table[(in[0] & 0x03) << 4]; + *pos++ = '='; + } else { + *pos++ = base64_table[((in[0] & 0x03) << 4) | + (in[1] >> 4)]; + *pos++ = base64_table[(in[1] & 0x0f) << 2]; + } + *pos++ = '='; + line_len += 4; + } + + if (line_len) + *pos++ = '\n'; + + *pos = '\0'; + if (out_len) + *out_len = pos - out; + return out; +} + + +/** + * base64_decode - Base64 decode + * @src: Data to be decoded + * @len: Length of the data to be decoded + * @out_len: Pointer to output length variable + * Returns: Allocated buffer of out_len bytes of decoded data, + * or %NULL on failure + * + * Caller is responsible for freeing the returned buffer. + */ +unsigned char * base64_decode(const unsigned char *src, size_t len, + size_t *out_len) +{ + unsigned char dtable[256], *out, *pos, block[4], tmp; + size_t i, count, olen; + int pad = 0; + + memset(dtable, 0x80, 256); + for (i = 0; i < sizeof(base64_table) - 1; i++) + dtable[base64_table[i]] = (unsigned char) i; + dtable['='] = 0; + + count = 0; + for (i = 0; i < len; i++) { + if (dtable[src[i]] != 0x80) + count++; + } + + if (count == 0 || count % 4) + return NULL; + + olen = count / 4 * 3; + pos = out = malloc(olen); + if (out == NULL) + return NULL; + + count = 0; + for (i = 0; i < len; i++) { + tmp = dtable[src[i]]; + if (tmp == 0x80) + continue; + + if (src[i] == '=') + pad++; + block[count] = tmp; + count++; + if (count == 4) { + *pos++ = (block[0] << 2) | (block[1] >> 4); + *pos++ = (block[1] << 4) | (block[2] >> 2); + *pos++ = (block[2] << 6) | block[3]; + count = 0; + if (pad) { + if (pad == 1) + pos--; + else if (pad == 2) + pos -= 2; + else { + /* Invalid padding */ + free(out); + return NULL; + } + break; + } + } + } + + *out_len = pos - out; + return out; +} diff --git a/src/base64.h b/src/base64.h new file mode 100644 index 0000000..f75f1f0 --- /dev/null +++ b/src/base64.h @@ -0,0 +1,13 @@ +// +// Created by nock on 2019-05-02. +// + +#ifndef UMCUMGR_BASE64_H +#define UMCUMGR_BASE64_H + +#include + +unsigned char * base64_encode(const unsigned char *src, size_t len, size_t *out_len); +unsigned char * base64_decode(const unsigned char *src, size_t len, size_t *out_len); + +#endif //UMCUMGR_BASE64_H diff --git a/src/image.c b/src/image.c index a50310e..96efbda 100644 --- a/src/image.c +++ b/src/image.c @@ -6,12 +6,17 @@ #include #include #include +#include +#include "base64.h" #include "image.h" #include "nmgr.h" +#include "cbor.h" + +static int serial_fd = -1; void image_help(void){ - printf("umcumgr image [list|upload|reset]\n"); + printf("umcumgr image [version|upload]\n"); } static char *hex(char in) { @@ -26,31 +31,119 @@ static char *hex(char in) { return out; } -void image_main(int fd, int argc, char **argv){ - if (!strcmp(argv[1], "list")) { - int rc = write(fd, &(char []){SHELL_NLIP_PKT_START1, SHELL_NLIP_PKT_START2}, 2); - rc = write(fd, IMAGE_LIST_CMD, sizeof(IMAGE_LIST_CMD)-1); - if (rc < sizeof(IMAGE_LIST_CMD)-1) { - fprintf(stderr, "Failed to send command: %s", strerror(errno)); - } - rc = write(fd, "\n", 1); - //return; - char tmp[256] = {0}; - while (1) { - if (read(fd, &tmp, 255) < 0) { - if (errno == EAGAIN) { - continue; - } - fprintf(stderr, "Failed to read from port: %s\n", strerror(errno)); - break; +static void hex_print(char *in, size_t len) { + char tmp[len*2+1]; + tmp[len] = 0; + for (int i = 0; i < len; i++) { + strcat(tmp, hex(in[i])); + } + printf("%s\n", tmp); +} + +static void printlen(uint8_t *src, size_t len){ + for (size_t i = 0; i < len; i++) { + putchar(src[i]); + } + putchar('\n'); +} + +static void assert_pkt_start(uint8_t *buf){ + assert(buf[0] == SHELL_NLIP_PKT_START1); + assert(buf[1] == SHELL_NLIP_PKT_START2); +} + +static void cbor_require_type(CborValue *it, CborType type) { + CborType actual_type = cbor_value_get_type(it); + if (actual_type != type) { + fprintf(stderr, "Cbor deserialization error. Expected type %d, received %d\n", type, actual_type); + exit(1); + } +} + +static void do_version(void) { + int rc = write(serial_fd, IMAGE_LIST_CMD, sizeof(IMAGE_LIST_CMD) - 1); + if (rc < sizeof(IMAGE_LIST_CMD) - 1) { + fprintf(stderr, "Failed to send command: %s", strerror(errno)); + 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; } - printf("%s ", tmp); - break; + fprintf(stderr, "Reading from serial failed: %s\n", strerror(errno)); + return; } - } else if (!strcmp(argv[1], "upload")) { - printf("upload\n"); - } else if (!strcmp(argv[1], "reset")) { - printf("reset\n"); + break; + } + + // Process the response + CborParser parser; + CborValue it; + + assert_pkt_start(tmp); + size_t out_len; + + uint8_t *decoded_buf = base64_decode(tmp+2, in_len-2, &out_len); + + // 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); + + // 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); + cbor_parser_init(p, out_len, 0, &parser, &it); + + cbor_require_type(&it, CborMapType); + CborValue map; + cbor_value_enter_container(&it, &map); + cbor_require_type(&map, CborTextStringType); + cbor_value_advance(&map); + cbor_require_type(&map, CborArrayType); + CborValue img_array; + cbor_value_enter_container(&map, &img_array); + + 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)); + cbor_value_advance(&img_array); + continue; + } + char *version; + size_t len; + cbor_value_dup_text_string(&version_tag, &version, &len, NULL); + printf("%s\n", version); + free(version); + break; + } + free(decoded_buf); +} + +void do_upload(void){ + return; +} + +void image_main(int fd, int argc, char **argv){ + serial_fd = fd; + if (argc < 1) { + image_help(); + return; + } + if (!strcmp(argv[0], "version")) { + do_version(); + } else if (!strcmp(argv[0], "upload")) { + do_upload(); } else { image_help(); } diff --git a/src/image.h b/src/image.h index b1e5b74..c4ca07e 100644 --- a/src/image.h +++ b/src/image.h @@ -7,7 +7,7 @@ #include "nmgr.h" -#define IMAGE_LIST_CMD "AAsAAAABAAFCAKD1Mw==" +#define IMAGE_LIST_CMD "\6\tAAsAAAABAAFCAKD1Mw==\n" void image_main(int, int, char **); diff --git a/src/main.c b/src/main.c index 9af177d..b7841a8 100644 --- a/src/main.c +++ b/src/main.c @@ -26,12 +26,12 @@ int main(int argc, char **argv) { } } - int serial_fd = open(serial_port, O_RDWR | O_NOCTTY | O_NDELAY); + int serial_fd = open(serial_port, O_RDWR | O_NOCTTY); if (serial_fd < 0) { fprintf(stderr, "Unable to open %s: %s\n", serial_port, strerror(errno)); return 1; } - printf("Opened %s\n", serial_port); + //printf("Opened %s\n", serial_port); struct termios t_options; @@ -51,7 +51,7 @@ int main(int argc, char **argv) { uint8_t rem = (uint8_t)(argc - optind); if (rem) { if (strcmp(argv[optind], "image") == 0) { - image_main(serial_fd, rem, &argv[optind]); + image_main(serial_fd, rem-1, &argv[optind+1]); } } } \ No newline at end of file diff --git a/src/nmgr.h b/src/nmgr.h index afc3812..d52ad58 100644 --- a/src/nmgr.h +++ b/src/nmgr.h @@ -13,6 +13,9 @@ #define SHELL_NLIP_DATA_START1 4 #define SHELL_NLIP_DATA_START2 20 +#define BOOT_SERIAL_OUT_MAX 512 +#define BOOT_SERIAL_IN_MAX 80 + struct nmgr_hdr { uint8_t nh_op; /* NMGR_OP_XXX */ uint8_t nh_flags;