| /* |
| * Copyright (c) 2017 Intel Corporation |
| * |
| * SPDX-License-Identifier: Apache-2.0 |
| */ |
| |
| #if defined(CONFIG_NET_DEBUG_COAP) |
| #define SYS_LOG_DOMAIN "coap" |
| #define NET_LOG_ENABLED 1 |
| #endif |
| |
| #include <stdlib.h> |
| #include <stddef.h> |
| #include <zephyr/types.h> |
| #include <string.h> |
| #include <stdbool.h> |
| #include <errno.h> |
| |
| #include <misc/byteorder.h> |
| #include <net/buf.h> |
| #include <net/net_pkt.h> |
| #include <net/net_ip.h> |
| |
| #include <net/coap.h> |
| |
| struct option_context { |
| u16_t delta; |
| u16_t offset; |
| struct net_buf *frag; |
| }; |
| |
| #define COAP_VERSION 1 |
| |
| #define COAP_MARKER 0xFF |
| |
| #define BASIC_HEADER_SIZE 4 |
| |
| #define PKT_WAIT_TIME K_SECONDS(1) |
| |
| /* Values as per RFC 7252, section-3.1. |
| * |
| * Option Delta/Length: 4-bit unsigned integer. A value between 0 and |
| * 12 indicates the Option Delta/Length. Three values are reserved for |
| * special constructs: |
| * 13: An 8-bit unsigned integer precedes the Option Value and indicates |
| * the Option Delta/Length minus 13. |
| * 14: A 16-bit unsigned integer in network byte order precedes the |
| * Option Value and indicates the Option Delta/Length minus 269. |
| * 15: Reserved for future use. |
| */ |
| #define COAP_OPTION_NO_EXT 12 /* Option's Delta/Length without extended data */ |
| #define COAP_OPTION_EXT_13 13 |
| #define COAP_OPTION_EXT_14 14 |
| #define COAP_OPTION_EXT_15 15 |
| #define COAP_OPTION_EXT_269 269 |
| |
| static u8_t option_header_get_delta(u8_t opt) |
| { |
| return (opt & 0xF0) >> 4; |
| } |
| |
| static u8_t option_header_get_len(u8_t opt) |
| { |
| return opt & 0x0F; |
| } |
| |
| static void option_header_set_delta(u8_t *opt, u8_t delta) |
| { |
| *opt = (delta & 0xF) << 4; |
| } |
| |
| static void option_header_set_len(u8_t *opt, u8_t len) |
| { |
| *opt |= (len & 0xF); |
| } |
| |
| static u16_t get_coap_packet_len(struct net_pkt *pkt) |
| { |
| u16_t len; |
| |
| /* TODO: verify with smaller packets */ |
| len = net_pkt_get_len(pkt) |
| - net_pkt_ip_hdr_len(pkt) |
| - net_pkt_ipv6_ext_len(pkt) |
| - NET_UDPH_LEN; |
| |
| return len; |
| } |
| |
| static int check_frag_read_status(const struct net_buf *frag, u16_t offset) |
| { |
| if (!frag && offset == 0xffff) { |
| return -EINVAL; |
| } else if (!frag && offset == 0) { |
| return 0; |
| } |
| |
| return 1; |
| } |
| |
| static int decode_delta(struct option_context *context, u16_t opt, |
| u16_t *opt_ext, u16_t *hdr_len) |
| { |
| int ret = 0; |
| |
| if (opt == COAP_OPTION_EXT_13) { |
| u8_t val; |
| |
| *hdr_len = 1; |
| context->frag = net_frag_read_u8(context->frag, |
| context->offset, |
| &context->offset, |
| &val); |
| ret = check_frag_read_status(context->frag, |
| context->offset); |
| if (ret < 0) { |
| return -EINVAL; |
| } |
| |
| opt = val + COAP_OPTION_EXT_13; |
| } else if (opt == COAP_OPTION_EXT_14) { |
| u16_t val; |
| |
| *hdr_len = 2; |
| context->frag = net_frag_read_be16(context->frag, |
| context->offset, |
| &context->offset, |
| &val); |
| ret = check_frag_read_status(context->frag, |
| context->offset); |
| if (ret < 0) { |
| return -EINVAL; |
| } |
| |
| opt = val + COAP_OPTION_EXT_269; |
| } else if (opt == COAP_OPTION_EXT_15) { |
| return -EINVAL; |
| } |
| |
| *opt_ext = opt; |
| |
| return ret; |
| } |
| |
| static int parse_option(const struct coap_packet *cpkt, |
| struct option_context *context, |
| struct coap_option *option, |
| u16_t *opt_len) |
| { |
| u16_t hdr_len; |
| u16_t delta; |
| u16_t len; |
| u8_t opt; |
| int r; |
| |
| context->frag = net_frag_read_u8(context->frag, |
| context->offset, |
| &context->offset, |
| &opt); |
| r = check_frag_read_status(context->frag, context->offset); |
| if (r < 0) { |
| return r; |
| } |
| |
| *opt_len += 1; |
| |
| /* This indicates that options have ended */ |
| if (opt == COAP_MARKER) { |
| /* packet w/ marker but no payload is malformed */ |
| return r > 0 ? 0 : -EINVAL; |
| } |
| |
| delta = option_header_get_delta(opt); |
| len = option_header_get_len(opt); |
| |
| /* r == 0 means no more data to read from fragment, but delta |
| * field shows that packet should contain more data, it must |
| * be a malformed packet. |
| */ |
| if (r == 0 && delta > COAP_OPTION_NO_EXT) { |
| return -EINVAL; |
| } |
| |
| if (delta > COAP_OPTION_NO_EXT) { |
| /* In case 'delta' doesn't fit the option fixed header. */ |
| r = decode_delta(context, delta, &delta, &hdr_len); |
| if ((r < 0) || (r == 0 && len > COAP_OPTION_NO_EXT)) { |
| return -EINVAL; |
| } |
| |
| *opt_len += hdr_len; |
| } |
| |
| if (len > COAP_OPTION_NO_EXT) { |
| /* In case 'len' doesn't fit the option fixed header. */ |
| r = decode_delta(context, len, &len, &hdr_len); |
| if (r < 0) { |
| return -EINVAL; |
| } |
| |
| *opt_len += hdr_len; |
| } |
| |
| *opt_len += len; |
| |
| if (r == 0) { |
| if (len == 0) { |
| context->delta += delta; |
| return r; |
| } |
| |
| /* r == 0 means no more data to read from fragment, but len |
| * field shows that packet should contain more data, it must |
| * be a malformed packet. |
| */ |
| return -EINVAL; |
| } |
| |
| if (option) { |
| /* |
| * Make sure the option data will fit into the value field of |
| * coap_option. |
| * NOTE: To expand the size of the value field set: |
| * CONFIG_COAP_EXTENDED_OPTIONS_LEN=y |
| * CONFIG_COAP_EXTENDED_OPTIONS_LEN_VALUE=<size> |
| */ |
| if (len > sizeof(option->value)) { |
| NET_ERR("%u is > sizeof(coap_option->value)(%zu)!", |
| len, sizeof(option->value)); |
| return -ENOMEM; |
| } |
| |
| option->delta = context->delta + delta; |
| option->len = len; |
| context->frag = net_frag_read(context->frag, context->offset, |
| &context->offset, len, |
| &option->value[0]); |
| } else { |
| context->frag = net_frag_skip(context->frag, context->offset, |
| &context->offset, len); |
| } |
| |
| r = check_frag_read_status(context->frag, context->offset); |
| if (r < 0) { |
| return r; |
| } |
| |
| context->delta += delta; |
| |
| return r; |
| } |
| |
| static int parse_options(const struct coap_packet *cpkt, |
| struct coap_option *options, u8_t opt_num) |
| { |
| struct option_context context = { |
| .delta = 0, |
| .frag = NULL, |
| .offset = 0 |
| }; |
| u16_t opt_len; |
| u8_t num; |
| int r; |
| |
| /* Skip CoAP header */ |
| context.frag = net_frag_skip(cpkt->frag, cpkt->offset, |
| &context.offset, cpkt->hdr_len); |
| r = check_frag_read_status(context.frag, context.offset); |
| if (r <= 0) { |
| return r; |
| } |
| |
| num = 0; |
| opt_len = 0; |
| |
| while (true) { |
| struct coap_option *option; |
| |
| option = num < opt_num ? &options[num++] : NULL; |
| r = parse_option(cpkt, &context, option, &opt_len); |
| if (r <= 0) { |
| break; |
| } |
| } |
| |
| if (r < 0) { |
| return r; |
| } |
| |
| return opt_len; |
| } |
| |
| static u8_t get_header_tkl(const struct coap_packet *cpkt) |
| { |
| struct net_buf *frag; |
| u16_t offset; |
| u8_t tkl; |
| |
| frag = net_frag_read_u8(cpkt->frag, cpkt->offset, &offset, &tkl); |
| |
| return tkl & 0xF; |
| } |
| |
| static u16_t get_pkt_len(const struct coap_packet *cpkt) |
| { |
| struct net_buf *frag; |
| u16_t len; |
| |
| len = cpkt->frag->len - cpkt->offset; |
| |
| for (frag = cpkt->frag->frags; frag; frag = frag->frags) { |
| len += frag->len; |
| } |
| |
| return len; |
| } |
| |
| static int get_header_len(struct coap_packet *cpkt) |
| { |
| u8_t tkl; |
| u16_t len; |
| int hdrlen; |
| |
| len = get_pkt_len(cpkt); |
| |
| hdrlen = BASIC_HEADER_SIZE; |
| |
| if (len < hdrlen) { |
| return -EINVAL; |
| } |
| |
| tkl = get_header_tkl(cpkt); |
| |
| /* Token lenghts 9-15 are reserved. */ |
| if (tkl > 8) { |
| return -EINVAL; |
| } |
| |
| if (len < hdrlen + tkl) { |
| return -EINVAL; |
| } |
| |
| cpkt->hdr_len = hdrlen + tkl; |
| |
| return 0; |
| } |
| |
| int coap_packet_parse(struct coap_packet *cpkt, struct net_pkt *pkt, |
| struct coap_option *options, u8_t opt_num) |
| { |
| int ret; |
| |
| if (!cpkt || !pkt || !pkt->frags) { |
| return -EINVAL; |
| } |
| |
| cpkt->pkt = pkt; |
| cpkt->hdr_len = 0; |
| cpkt->opt_len = 0; |
| |
| cpkt->frag = net_frag_skip(pkt->frags, 0, &cpkt->offset, |
| net_pkt_ip_hdr_len(pkt) + |
| NET_UDPH_LEN + |
| net_pkt_ipv6_ext_len(pkt)); |
| ret = check_frag_read_status(cpkt->frag, cpkt->offset); |
| if (ret <= 0) { |
| return ret; |
| } |
| |
| ret = get_header_len(cpkt); |
| if (ret < 0) { |
| return -EINVAL; |
| } |
| |
| ret = parse_options(cpkt, options, opt_num); |
| if (ret < 0) { |
| return -EINVAL; |
| } |
| |
| cpkt->opt_len = ret; |
| |
| return 0; |
| } |
| |
| int coap_packet_init(struct coap_packet *cpkt, struct net_pkt *pkt, |
| u8_t ver, u8_t type, u8_t tokenlen, |
| u8_t *token, u8_t code, u16_t id) |
| { |
| u8_t hdr; |
| bool res; |
| |
| if (!cpkt || !pkt || !pkt->frags) { |
| return -EINVAL; |
| } |
| |
| memset(cpkt, 0, sizeof(*cpkt)); |
| cpkt->pkt = pkt; |
| cpkt->frag = pkt->frags; |
| cpkt->offset = 0; |
| cpkt->last_delta = 0; |
| |
| hdr = (ver & 0x3) << 6; |
| hdr |= (type & 0x3) << 4; |
| hdr |= tokenlen & 0xF; |
| |
| net_pkt_append_u8(pkt, hdr); |
| net_pkt_append_u8(pkt, code); |
| net_pkt_append_be16(pkt, id); |
| |
| if (token && tokenlen) { |
| res = net_pkt_append_all(pkt, tokenlen, token, PKT_WAIT_TIME); |
| if (!res) { |
| return -ENOMEM; |
| } |
| } |
| |
| /* Header length : (version + type + tkl) + code + id + [token] */ |
| cpkt->hdr_len = 1 + 1 + 2 + tokenlen; |
| |
| return 0; |
| } |
| |
| int coap_pending_init(struct coap_pending *pending, |
| const struct coap_packet *request, |
| const struct sockaddr *addr) |
| { |
| memset(pending, 0, sizeof(*pending)); |
| pending->id = coap_header_get_id(request); |
| memcpy(&pending->addr, addr, sizeof(*addr)); |
| |
| /* Will increase the reference count when the pending is cycled */ |
| pending->pkt = request->pkt; |
| |
| return 0; |
| } |
| |
| struct coap_pending *coap_pending_next_unused( |
| struct coap_pending *pendings, size_t len) |
| { |
| struct coap_pending *p; |
| size_t i; |
| |
| for (i = 0, p = pendings; i < len; i++, p++) { |
| if (p->timeout == 0 && !p->pkt) { |
| return p; |
| } |
| } |
| |
| return NULL; |
| } |
| |
| struct coap_reply *coap_reply_next_unused( |
| struct coap_reply *replies, size_t len) |
| { |
| struct coap_reply *r; |
| size_t i; |
| |
| for (i = 0, r = replies; i < len; i++, r++) { |
| if (!r->reply) { |
| return r; |
| } |
| } |
| |
| return NULL; |
| } |
| |
| static inline bool is_addr_unspecified(const struct sockaddr *addr) |
| { |
| if (addr->sa_family == AF_UNSPEC) { |
| return true; |
| } |
| |
| if (addr->sa_family == AF_INET6) { |
| return net_is_ipv6_addr_unspecified( |
| &(net_sin6(addr)->sin6_addr)); |
| } else if (addr->sa_family == AF_INET) { |
| return net_sin(addr)->sin_addr.s4_addr32[0] == 0; |
| } |
| |
| return false; |
| } |
| |
| struct coap_observer *coap_observer_next_unused( |
| struct coap_observer *observers, size_t len) |
| { |
| struct coap_observer *o; |
| size_t i; |
| |
| for (i = 0, o = observers; i < len; i++, o++) { |
| if (is_addr_unspecified(&o->addr)) { |
| return o; |
| } |
| } |
| |
| return NULL; |
| } |
| |
| struct coap_pending *coap_pending_received( |
| const struct coap_packet *response, |
| struct coap_pending *pendings, size_t len) |
| { |
| struct coap_pending *p; |
| u16_t resp_id = coap_header_get_id(response); |
| size_t i; |
| |
| for (i = 0, p = pendings; i < len; i++, p++) { |
| if (!p->timeout) { |
| continue; |
| } |
| |
| if (resp_id != p->id) { |
| continue; |
| } |
| |
| coap_pending_clear(p); |
| return p; |
| } |
| |
| return NULL; |
| } |
| |
| struct coap_pending *coap_pending_next_to_expire( |
| struct coap_pending *pendings, size_t len) |
| { |
| struct coap_pending *p, *found = NULL; |
| size_t i; |
| |
| for (i = 0, p = pendings; i < len; i++, p++) { |
| if (p->timeout && (!found || found->timeout < p->timeout)) { |
| found = p; |
| } |
| } |
| |
| return found; |
| } |
| |
| #define LAST_TIMEOUT (2345 * 4) |
| |
| static s32_t next_timeout(s32_t previous) |
| { |
| switch (previous) { |
| case 0: |
| return 2345; |
| case 2345: |
| return 2345 * 2; |
| case (2345 * 2): |
| return LAST_TIMEOUT; |
| case LAST_TIMEOUT: |
| return LAST_TIMEOUT; |
| } |
| |
| return 2345; |
| } |
| |
| bool coap_pending_cycle(struct coap_pending *pending) |
| { |
| s32_t old = pending->timeout; |
| bool cont; |
| |
| pending->timeout = next_timeout(pending->timeout); |
| |
| /* If the timeout changed, it's not the last, continue... */ |
| cont = (old != pending->timeout); |
| if (cont) { |
| /* When it it is the last retransmission, the buffer |
| * will be destroyed when it is transmitted. |
| */ |
| net_pkt_ref(pending->pkt); |
| } |
| |
| return cont; |
| } |
| |
| void coap_pending_clear(struct coap_pending *pending) |
| { |
| pending->timeout = 0; |
| net_pkt_unref(pending->pkt); |
| pending->pkt = NULL; |
| } |
| |
| static bool uri_path_eq(const struct coap_packet *cpkt, |
| const char * const *path, |
| struct coap_option *options, |
| u8_t opt_num) |
| { |
| u8_t i; |
| u8_t j = 0; |
| |
| for (i = 0; i < opt_num && path[j]; i++) { |
| if (options[i].delta != COAP_OPTION_URI_PATH) { |
| continue; |
| } |
| |
| if (options[i].len != strlen(path[j])) { |
| return false; |
| } |
| |
| if (memcmp(options[i].value, path[j], options[i].len)) { |
| return false; |
| } |
| |
| j++; |
| } |
| |
| if (path[j]) { |
| return false; |
| } |
| |
| for (; i < opt_num; i++) { |
| if (options[i].delta == COAP_OPTION_URI_PATH) { |
| return false; |
| } |
| } |
| |
| return true; |
| } |
| |
| static coap_method_t method_from_code(const struct coap_resource *resource, |
| u8_t code) |
| { |
| switch (code) { |
| case COAP_METHOD_GET: |
| return resource->get; |
| case COAP_METHOD_POST: |
| return resource->post; |
| case COAP_METHOD_PUT: |
| return resource->put; |
| case COAP_METHOD_DELETE: |
| return resource->del; |
| default: |
| return NULL; |
| } |
| } |
| |
| static bool is_request(const struct coap_packet *cpkt) |
| { |
| u8_t code = coap_header_get_code(cpkt); |
| |
| return !(code & ~COAP_REQUEST_MASK); |
| } |
| |
| int coap_handle_request(struct coap_packet *cpkt, |
| struct coap_resource *resources, |
| struct coap_option *options, |
| u8_t opt_num) |
| { |
| struct coap_resource *resource; |
| |
| if (!is_request(cpkt)) { |
| return 0; |
| } |
| |
| /* FIXME: deal with hierarchical resources */ |
| for (resource = resources; resource && resource->path; resource++) { |
| coap_method_t method; |
| u8_t code; |
| |
| if (!uri_path_eq(cpkt, resource->path, options, opt_num)) { |
| continue; |
| } |
| |
| code = coap_header_get_code(cpkt); |
| method = method_from_code(resource, code); |
| if (!method) { |
| return 0; |
| } |
| |
| return method(resource, cpkt); |
| } |
| |
| return -ENOENT; |
| } |
| |
| unsigned int coap_option_value_to_int(const struct coap_option *option) |
| { |
| switch (option->len) { |
| case 0: |
| return 0; |
| case 1: |
| return option->value[0]; |
| case 2: |
| return (option->value[1] << 0) | (option->value[0] << 8); |
| case 3: |
| return (option->value[2] << 0) | (option->value[1] << 8) | |
| (option->value[0] << 16); |
| case 4: |
| return (option->value[2] << 0) | (option->value[2] << 8) | |
| (option->value[1] << 16) | (option->value[0] << 24); |
| default: |
| return 0; |
| } |
| |
| return 0; |
| } |
| |
| static int get_observe_option(const struct coap_packet *cpkt) |
| { |
| struct coap_option option = {}; |
| u16_t count = 1; |
| int r; |
| |
| r = coap_find_options(cpkt, COAP_OPTION_OBSERVE, &option, count); |
| if (r <= 0) { |
| return -ENOENT; |
| } |
| |
| return coap_option_value_to_int(&option); |
| } |
| |
| struct coap_reply *coap_response_received( |
| const struct coap_packet *response, |
| const struct sockaddr *from, |
| struct coap_reply *replies, size_t len) |
| { |
| struct coap_reply *r; |
| u8_t token[8]; |
| u16_t id; |
| u8_t tkl; |
| size_t i; |
| |
| id = coap_header_get_id(response); |
| tkl = coap_header_get_token(response, (u8_t *)token); |
| |
| for (i = 0, r = replies; i < len; i++, r++) { |
| int age; |
| |
| if ((r->id == 0) && (r->tkl == 0)) { |
| continue; |
| } |
| |
| /* Piggybacked must match id when token is empty */ |
| if ((r->id != id) && (tkl == 0)) { |
| continue; |
| } |
| |
| if (tkl > 0 && memcmp(r->token, token, tkl)) { |
| continue; |
| } |
| |
| age = get_observe_option(response); |
| if (age > 0) { |
| /* age == 2 means that the notifications wrapped, |
| * or this is the first one |
| */ |
| if (r->age > age && age != 2) { |
| continue; |
| } |
| |
| r->age = age; |
| } |
| |
| r->reply(response, r, from); |
| return r; |
| } |
| |
| return NULL; |
| } |
| |
| void coap_reply_init(struct coap_reply *reply, |
| const struct coap_packet *request) |
| { |
| u8_t token[8]; |
| u8_t tkl; |
| int age; |
| |
| reply->id = coap_header_get_id(request); |
| tkl = coap_header_get_token(request, (u8_t *)&token); |
| |
| if (tkl > 0) { |
| memcpy(reply->token, token, tkl); |
| } |
| |
| reply->tkl = tkl; |
| |
| age = get_observe_option(request); |
| |
| /* It means that the request enabled observing a resource */ |
| if (age == 0) { |
| reply->age = 2; |
| } |
| } |
| |
| void coap_reply_clear(struct coap_reply *reply) |
| { |
| reply->id = 0; |
| reply->tkl = 0; |
| reply->reply = NULL; |
| } |
| |
| int coap_resource_notify(struct coap_resource *resource) |
| { |
| struct coap_observer *o; |
| |
| resource->age++; |
| |
| if (!resource->notify) { |
| return -ENOENT; |
| } |
| |
| SYS_SLIST_FOR_EACH_CONTAINER(&resource->observers, o, list) { |
| resource->notify(resource, o); |
| } |
| |
| return 0; |
| } |
| |
| bool coap_request_is_observe(const struct coap_packet *request) |
| { |
| return get_observe_option(request) == 0; |
| } |
| |
| void coap_observer_init(struct coap_observer *observer, |
| const struct coap_packet *request, |
| const struct sockaddr *addr) |
| { |
| u8_t token[8]; |
| u8_t tkl; |
| |
| tkl = coap_header_get_token(request, (u8_t *)&token); |
| |
| if (tkl > 0) { |
| memcpy(observer->token, token, tkl); |
| } |
| |
| observer->tkl = tkl; |
| |
| net_ipaddr_copy(&observer->addr, addr); |
| } |
| |
| bool coap_register_observer(struct coap_resource *resource, |
| struct coap_observer *observer) |
| { |
| bool first; |
| |
| sys_slist_append(&resource->observers, &observer->list); |
| |
| first = resource->age == 0; |
| if (first) { |
| resource->age = 2; |
| } |
| |
| return first; |
| } |
| |
| void coap_remove_observer(struct coap_resource *resource, |
| struct coap_observer *observer) |
| { |
| sys_slist_find_and_remove(&resource->observers, &observer->list); |
| } |
| |
| static bool sockaddr_equal(const struct sockaddr *a, |
| const struct sockaddr *b) |
| { |
| /* FIXME: Should we consider ipv6-mapped ipv4 addresses as equal to |
| * ipv4 addresses? |
| */ |
| if (a->sa_family != b->sa_family) { |
| return false; |
| } |
| |
| if (a->sa_family == AF_INET) { |
| const struct sockaddr_in *a4 = net_sin(a); |
| const struct sockaddr_in *b4 = net_sin(b); |
| |
| if (a4->sin_port != b4->sin_port) { |
| return false; |
| } |
| |
| return net_ipv4_addr_cmp(&a4->sin_addr, &b4->sin_addr); |
| } |
| |
| if (b->sa_family == AF_INET6) { |
| const struct sockaddr_in6 *a6 = net_sin6(a); |
| const struct sockaddr_in6 *b6 = net_sin6(b); |
| |
| if (a6->sin6_scope_id != b6->sin6_scope_id) { |
| return false; |
| } |
| |
| if (a6->sin6_port != b6->sin6_port) { |
| return false; |
| } |
| |
| return net_ipv6_addr_cmp(&a6->sin6_addr, &b6->sin6_addr); |
| } |
| |
| /* Invalid address family */ |
| return false; |
| } |
| |
| struct coap_observer *coap_find_observer_by_addr( |
| struct coap_observer *observers, size_t len, |
| const struct sockaddr *addr) |
| { |
| size_t i; |
| |
| for (i = 0; i < len; i++) { |
| struct coap_observer *o = &observers[i]; |
| |
| if (sockaddr_equal(&o->addr, addr)) { |
| return o; |
| } |
| } |
| |
| return NULL; |
| } |
| |
| int coap_packet_append_payload_marker(struct coap_packet *cpkt) |
| { |
| return net_pkt_append_u8(cpkt->pkt, COAP_MARKER) ? 0 : -EINVAL; |
| } |
| |
| int coap_packet_append_payload(struct coap_packet *cpkt, u8_t *payload, |
| u16_t payload_len) |
| { |
| bool status; |
| |
| status = net_pkt_append_all(cpkt->pkt, payload_len, payload, |
| PKT_WAIT_TIME); |
| |
| return status ? 0 : -EINVAL; |
| } |
| |
| static u8_t encode_extended_option(u16_t num, u8_t *opt, u16_t *ext) |
| { |
| if (num < COAP_OPTION_EXT_13) { |
| *opt = num; |
| *ext = 0; |
| |
| return 0; |
| } else if (num < COAP_OPTION_EXT_269) { |
| *opt = COAP_OPTION_EXT_13; |
| *ext = num - COAP_OPTION_EXT_13; |
| |
| return 1; |
| } |
| |
| *opt = COAP_OPTION_EXT_14; |
| *ext = num - COAP_OPTION_EXT_269; |
| |
| return 2; |
| } |
| |
| static int encode_option(struct coap_packet *cpkt, u16_t code, |
| const u8_t *value, u16_t len) |
| { |
| u16_t delta_ext; /* Extended delta */ |
| u16_t len_ext; /* Extended length */ |
| u8_t opt; /* delta | len */ |
| u8_t opt_delta; |
| u8_t opt_len; |
| u8_t delta_size; |
| u8_t len_size; |
| bool res; |
| |
| delta_size = encode_extended_option(code, &opt_delta, &delta_ext); |
| len_size = encode_extended_option(len, &opt_len, &len_ext); |
| |
| option_header_set_delta(&opt, opt_delta); |
| option_header_set_len(&opt, opt_len); |
| |
| net_pkt_append_u8(cpkt->pkt, opt); |
| |
| if (delta_size == 1) { |
| net_pkt_append_u8(cpkt->pkt, (u8_t) delta_ext); |
| } else if (delta_size == 2) { |
| net_pkt_append_be16(cpkt->pkt, delta_ext); |
| } |
| |
| if (len_size == 1) { |
| net_pkt_append_u8(cpkt->pkt, (u8_t) len_ext); |
| } else if (delta_size == 2) { |
| net_pkt_append_be16(cpkt->pkt, len_ext); |
| } |
| |
| if (len && value) { |
| res = net_pkt_append_all(cpkt->pkt, len, value, PKT_WAIT_TIME); |
| if (!res) { |
| return -EINVAL; |
| } |
| } |
| |
| return (1 + delta_size + len_size + len); |
| } |
| |
| /* TODO Add support for inserting options in proper place |
| * and modify other option's delta accordingly. |
| */ |
| int coap_packet_append_option(struct coap_packet *cpkt, u16_t code, |
| const u8_t *value, u16_t len) |
| { |
| struct net_buf *frag; |
| u16_t offset; |
| int r; |
| |
| if (!cpkt) { |
| return -EINVAL; |
| } |
| |
| if (len && !value) { |
| return -EINVAL; |
| } |
| |
| if (code < cpkt->last_delta) { |
| NET_ERR("Options should be in ascending order"); |
| return -EINVAL; |
| } |
| |
| /* Skip CoAP packet header */ |
| frag = net_frag_skip(cpkt->frag, cpkt->offset, &offset, cpkt->hdr_len); |
| if (!frag && offset == 0xffff) { |
| return -EINVAL; |
| } |
| |
| /* Calculate delta, if this option is not the first one */ |
| if (cpkt->opt_len) { |
| code = (code == cpkt->last_delta) ? 0 : |
| code - cpkt->last_delta; |
| } |
| |
| r = encode_option(cpkt, code, value, len); |
| if (r < 0) { |
| return -EINVAL; |
| } |
| |
| cpkt->opt_len += r; |
| cpkt->last_delta += code; |
| |
| return 0; |
| } |
| |
| int coap_append_option_int(struct coap_packet *cpkt, u16_t code, |
| unsigned int val) |
| { |
| u8_t data[4], len; |
| |
| if (val == 0) { |
| data[0] = 0; |
| len = 0; |
| } else if (val < 0xFF) { |
| data[0] = (u8_t) val; |
| len = 1; |
| } else if (val < 0xFFFF) { |
| sys_put_be16(val, data); |
| len = 2; |
| } else if (val < 0xFFFFFF) { |
| sys_put_be16(val, data); |
| data[2] = val >> 16; |
| len = 3; |
| } else { |
| sys_put_be32(val, data); |
| len = 4; |
| } |
| |
| return coap_packet_append_option(cpkt, code, data, len); |
| } |
| |
| int coap_find_options(const struct coap_packet *cpkt, u16_t code, |
| struct coap_option *options, u16_t veclen) |
| { |
| struct option_context context = { |
| .delta = 0, |
| .frag = NULL, |
| .offset = 0 |
| }; |
| u16_t opt_len; |
| int count; |
| int r; |
| |
| if (!cpkt || !cpkt->pkt || !cpkt->pkt->frags || !cpkt->hdr_len) { |
| return -EINVAL; |
| } |
| |
| /* Skip CoAP header */ |
| context.frag = net_frag_skip(cpkt->frag, cpkt->offset, |
| &context.offset, cpkt->hdr_len); |
| r = check_frag_read_status(context.frag, context.offset); |
| if (r <= 0) { |
| return r; |
| } |
| |
| opt_len = 0; |
| count = 0; |
| |
| while (context.delta <= code && count < veclen) { |
| r = parse_option(cpkt, &context, &options[count], &opt_len); |
| if (r < 0) { |
| return -EINVAL; |
| } |
| |
| if (code == options[count].delta) { |
| count++; |
| } |
| |
| if (r == 0) { |
| break; |
| } |
| } |
| |
| return count; |
| } |
| |
| u8_t coap_header_get_version(const struct coap_packet *cpkt) |
| { |
| struct net_buf *frag; |
| u16_t offset; |
| u8_t version; |
| |
| frag = net_frag_read_u8(cpkt->frag, cpkt->offset, &offset, &version); |
| if (!frag && offset == 0xffff) { |
| return 0; |
| } |
| |
| return (version & 0xC0) >> 6; |
| } |
| |
| u8_t coap_header_get_type(const struct coap_packet *cpkt) |
| { |
| struct net_buf *frag; |
| u16_t offset; |
| u8_t type; |
| |
| frag = net_frag_read_u8(cpkt->frag, cpkt->offset, &offset, &type); |
| if (!frag && offset == 0xffff) { |
| return 0; |
| } |
| |
| return (type & 0x30) >> 4; |
| } |
| |
| static u8_t __coap_header_get_code(const struct coap_packet *cpkt) |
| { |
| struct net_buf *frag; |
| u16_t offset; |
| u8_t code; |
| |
| frag = net_frag_skip(cpkt->frag, cpkt->offset, &offset, 1); |
| frag = net_frag_read_u8(frag, offset, &offset, &code); |
| if (!frag && offset == 0xffff) { |
| return 0; |
| } |
| |
| return code; |
| } |
| |
| u8_t coap_header_get_token(const struct coap_packet *cpkt, u8_t *token) |
| { |
| struct net_buf *frag; |
| u16_t offset; |
| u8_t tkl; |
| |
| if (!cpkt || !token) { |
| return 0; |
| } |
| |
| tkl = get_header_tkl(cpkt); |
| if (!tkl) { |
| return 0; |
| } |
| |
| frag = net_frag_skip(cpkt->frag, cpkt->offset, &offset, |
| BASIC_HEADER_SIZE); |
| frag = net_frag_read(frag, offset, &offset, tkl, token); |
| if (!frag && offset == 0xffff) { |
| return 0; |
| } |
| |
| return tkl; |
| } |
| |
| u8_t coap_header_get_code(const struct coap_packet *cpkt) |
| { |
| u8_t code = __coap_header_get_code(cpkt); |
| |
| switch (code) { |
| /* Methods are encoded in the code field too */ |
| case COAP_METHOD_GET: |
| case COAP_METHOD_POST: |
| case COAP_METHOD_PUT: |
| case COAP_METHOD_DELETE: |
| |
| /* All the defined response codes */ |
| case COAP_RESPONSE_CODE_OK: |
| case COAP_RESPONSE_CODE_CREATED: |
| case COAP_RESPONSE_CODE_DELETED: |
| case COAP_RESPONSE_CODE_VALID: |
| case COAP_RESPONSE_CODE_CHANGED: |
| case COAP_RESPONSE_CODE_CONTENT: |
| case COAP_RESPONSE_CODE_CONTINUE: |
| case COAP_RESPONSE_CODE_BAD_REQUEST: |
| case COAP_RESPONSE_CODE_UNAUTHORIZED: |
| case COAP_RESPONSE_CODE_BAD_OPTION: |
| case COAP_RESPONSE_CODE_FORBIDDEN: |
| case COAP_RESPONSE_CODE_NOT_FOUND: |
| case COAP_RESPONSE_CODE_NOT_ALLOWED: |
| case COAP_RESPONSE_CODE_NOT_ACCEPTABLE: |
| case COAP_RESPONSE_CODE_INCOMPLETE: |
| case COAP_RESPONSE_CODE_PRECONDITION_FAILED: |
| case COAP_RESPONSE_CODE_REQUEST_TOO_LARGE: |
| case COAP_RESPONSE_CODE_UNSUPPORTED_CONTENT_FORMAT: |
| case COAP_RESPONSE_CODE_INTERNAL_ERROR: |
| case COAP_RESPONSE_CODE_NOT_IMPLEMENTED: |
| case COAP_RESPONSE_CODE_BAD_GATEWAY: |
| case COAP_RESPONSE_CODE_SERVICE_UNAVAILABLE: |
| case COAP_RESPONSE_CODE_GATEWAY_TIMEOUT: |
| case COAP_RESPONSE_CODE_PROXYING_NOT_SUPPORTED: |
| case COAP_CODE_EMPTY: |
| return code; |
| default: |
| return COAP_CODE_EMPTY; |
| } |
| } |
| |
| u16_t coap_header_get_id(const struct coap_packet *cpkt) |
| { |
| struct net_buf *frag; |
| u16_t offset; |
| u16_t id; |
| |
| frag = net_frag_skip(cpkt->frag, cpkt->offset, &offset, 2); |
| frag = net_frag_read_be16(frag, offset, &offset, &id); |
| if (!frag && offset == 0xffff) { |
| return 0; |
| } |
| |
| return id; |
| } |
| |
| struct net_buf *coap_packet_get_payload(const struct coap_packet *cpkt, |
| u16_t *offset, u16_t *len) |
| { |
| struct net_buf *frag = NULL; |
| u16_t coap_pkt_len; |
| |
| if (!cpkt || !cpkt->pkt || !offset || !len) { |
| return NULL; |
| } |
| |
| *offset = 0xffff; |
| *len = 0; |
| |
| coap_pkt_len = get_coap_packet_len(cpkt->pkt); |
| |
| frag = net_frag_skip(cpkt->frag, cpkt->offset, offset, |
| cpkt->hdr_len + cpkt->opt_len); |
| *len = coap_pkt_len - cpkt->hdr_len - cpkt->opt_len; |
| |
| return frag; |
| } |
| |
| |
| int coap_block_transfer_init(struct coap_block_context *ctx, |
| enum coap_block_size block_size, |
| size_t total_size) |
| { |
| ctx->block_size = block_size; |
| ctx->total_size = total_size; |
| ctx->current = 0; |
| |
| return 0; |
| } |
| |
| #define GET_BLOCK_SIZE(v) (((v) & 0x7)) |
| #define GET_MORE(v) (!!((v) & 0x08)) |
| #define GET_NUM(v) ((v) >> 4) |
| |
| #define SET_BLOCK_SIZE(v, b) (v |= ((b) & 0x07)) |
| #define SET_MORE(v, m) ((v) |= (m) ? 0x08 : 0x00) |
| #define SET_NUM(v, n) ((v) |= ((n) << 4)) |
| |
| int coap_append_block1_option(struct coap_packet *cpkt, |
| struct coap_block_context *ctx) |
| { |
| u16_t bytes = coap_block_size_to_bytes(ctx->block_size); |
| unsigned int val = 0; |
| int r; |
| |
| if (is_request(cpkt)) { |
| SET_BLOCK_SIZE(val, ctx->block_size); |
| SET_MORE(val, ctx->current + bytes < ctx->total_size); |
| SET_NUM(val, ctx->current / bytes); |
| } else { |
| SET_BLOCK_SIZE(val, ctx->block_size); |
| SET_NUM(val, ctx->current / bytes); |
| } |
| |
| r = coap_append_option_int(cpkt, COAP_OPTION_BLOCK1, val); |
| |
| return r; |
| } |
| |
| int coap_append_block2_option(struct coap_packet *cpkt, |
| struct coap_block_context *ctx) |
| { |
| int r, val = 0; |
| u16_t bytes = coap_block_size_to_bytes(ctx->block_size); |
| |
| if (is_request(cpkt)) { |
| SET_BLOCK_SIZE(val, ctx->block_size); |
| SET_NUM(val, ctx->current / bytes); |
| } else { |
| SET_BLOCK_SIZE(val, ctx->block_size); |
| SET_MORE(val, ctx->current + bytes < ctx->total_size); |
| SET_NUM(val, ctx->current / bytes); |
| } |
| |
| r = coap_append_option_int(cpkt, COAP_OPTION_BLOCK2, val); |
| |
| return r; |
| } |
| |
| int coap_append_size1_option(struct coap_packet *cpkt, |
| struct coap_block_context *ctx) |
| { |
| return coap_append_option_int(cpkt, COAP_OPTION_SIZE1, ctx->total_size); |
| } |
| |
| int coap_append_size2_option(struct coap_packet *cpkt, |
| struct coap_block_context *ctx) |
| { |
| return coap_append_option_int(cpkt, COAP_OPTION_SIZE2, ctx->total_size); |
| } |
| |
| static int get_block_option(const struct coap_packet *cpkt, u16_t code) |
| { |
| struct coap_option option; |
| unsigned int val; |
| int count = 1; |
| |
| count = coap_find_options(cpkt, code, &option, count); |
| if (count <= 0) { |
| return -ENOENT; |
| } |
| |
| val = coap_option_value_to_int(&option); |
| |
| return val; |
| } |
| |
| static int update_descriptive_block(struct coap_block_context *ctx, |
| int block, int size) |
| { |
| size_t new_current = GET_NUM(block) << (GET_BLOCK_SIZE(block) + 4); |
| |
| if (block == -ENOENT) { |
| return 0; |
| } |
| |
| if (size && ctx->total_size && ctx->total_size != size) { |
| return -EINVAL; |
| } |
| |
| if (ctx->current > 0 && GET_BLOCK_SIZE(block) > ctx->block_size) { |
| return -EINVAL; |
| } |
| |
| if (ctx->total_size && new_current > ctx->total_size) { |
| return -EINVAL; |
| } |
| |
| if (size) { |
| ctx->total_size = size; |
| } |
| ctx->current = new_current; |
| ctx->block_size = min(GET_BLOCK_SIZE(block), ctx->block_size); |
| |
| return 0; |
| } |
| |
| static int update_control_block1(struct coap_block_context *ctx, |
| int block, int size) |
| { |
| size_t new_current = GET_NUM(block) << (GET_BLOCK_SIZE(block) + 4); |
| |
| if (block == -ENOENT) { |
| return 0; |
| } |
| |
| if (new_current != ctx->current) { |
| return -EINVAL; |
| } |
| |
| if (GET_BLOCK_SIZE(block) > ctx->block_size) { |
| return -EINVAL; |
| } |
| |
| ctx->block_size = GET_BLOCK_SIZE(block); |
| ctx->total_size = size; |
| |
| return 0; |
| } |
| |
| static int update_control_block2(struct coap_block_context *ctx, |
| int block, int size) |
| { |
| size_t new_current = GET_NUM(block) << (GET_BLOCK_SIZE(block) + 4); |
| |
| if (block == -ENOENT) { |
| return 0; |
| } |
| |
| if (GET_MORE(block)) { |
| return -EINVAL; |
| } |
| |
| if (GET_NUM(block) > 0 && GET_BLOCK_SIZE(block) != ctx->block_size) { |
| return -EINVAL; |
| } |
| |
| ctx->current = new_current; |
| ctx->block_size = min(GET_BLOCK_SIZE(block), ctx->block_size); |
| |
| return 0; |
| } |
| |
| int coap_update_from_block(const struct coap_packet *cpkt, |
| struct coap_block_context *ctx) |
| { |
| int r, block1, block2, size1, size2; |
| |
| block1 = get_block_option(cpkt, COAP_OPTION_BLOCK1); |
| block2 = get_block_option(cpkt, COAP_OPTION_BLOCK2); |
| size1 = get_block_option(cpkt, COAP_OPTION_SIZE1); |
| size2 = get_block_option(cpkt, COAP_OPTION_SIZE2); |
| |
| size1 = size1 == -ENOENT ? 0 : size1; |
| size2 = size2 == -ENOENT ? 0 : size2; |
| |
| if (is_request(cpkt)) { |
| r = update_control_block2(ctx, block2, size2); |
| if (r) { |
| return r; |
| } |
| |
| return update_descriptive_block(ctx, block1, size1); |
| } |
| |
| r = update_control_block1(ctx, block1, size1); |
| if (r) { |
| return r; |
| } |
| |
| return update_descriptive_block(ctx, block2, size2); |
| } |
| |
| size_t coap_next_block(const struct coap_packet *cpkt, |
| struct coap_block_context *ctx) |
| { |
| int block; |
| |
| if (is_request(cpkt)) { |
| block = get_block_option(cpkt, COAP_OPTION_BLOCK1); |
| } else { |
| block = get_block_option(cpkt, COAP_OPTION_BLOCK2); |
| } |
| |
| if (!GET_MORE(block)) { |
| return 0; |
| } |
| |
| ctx->current += coap_block_size_to_bytes(ctx->block_size); |
| |
| return ctx->current; |
| } |
| |
| u8_t *coap_next_token(void) |
| { |
| static u32_t rand[2]; |
| |
| rand[0] = sys_rand32_get(); |
| rand[1] = sys_rand32_get(); |
| |
| return (u8_t *) rand; |
| } |