From 3c89fd52679c8ccebceb30294a4bd815b51ede19 Mon Sep 17 00:00:00 2001 From: Dimitri Sokolyuk Date: Mon, 5 Dec 2016 11:22:26 +0100 Subject: Import Drive SDK --- drive-sdk/deps/bzle/src/att.c | 1165 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 1165 insertions(+) create mode 100644 drive-sdk/deps/bzle/src/att.c (limited to 'drive-sdk/deps/bzle/src/att.c') diff --git a/drive-sdk/deps/bzle/src/att.c b/drive-sdk/deps/bzle/src/att.c new file mode 100644 index 0000000..802c9c8 --- /dev/null +++ b/drive-sdk/deps/bzle/src/att.c @@ -0,0 +1,1165 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2010 Nokia Corporation + * Copyright (C) 2010 Marcel Holtmann + * + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + +#ifdef HAVE_CONFIG_H +#include "config.h" +#endif + +#include +#include +#include + +#include + +#include + +#include +#include + +const char *att_ecode2str(uint8_t status) +{ + switch (status) { + case ATT_ECODE_INVALID_HANDLE: + return "Invalid handle"; + case ATT_ECODE_READ_NOT_PERM: + return "Attribute can't be read"; + case ATT_ECODE_WRITE_NOT_PERM: + return "Attribute can't be written"; + case ATT_ECODE_INVALID_PDU: + return "Attribute PDU was invalid"; + case ATT_ECODE_AUTHENTICATION: + return "Attribute requires authentication before read/write"; + case ATT_ECODE_REQ_NOT_SUPP: + return "Server doesn't support the request received"; + case ATT_ECODE_INVALID_OFFSET: + return "Offset past the end of the attribute"; + case ATT_ECODE_AUTHORIZATION: + return "Attribute requires authorization before read/write"; + case ATT_ECODE_PREP_QUEUE_FULL: + return "Too many prepare writes have been queued"; + case ATT_ECODE_ATTR_NOT_FOUND: + return "No attribute found within the given range"; + case ATT_ECODE_ATTR_NOT_LONG: + return "Attribute can't be read/written using Read Blob Req"; + case ATT_ECODE_INSUFF_ENCR_KEY_SIZE: + return "Encryption Key Size is insufficient"; + case ATT_ECODE_INVAL_ATTR_VALUE_LEN: + return "Attribute value length is invalid"; + case ATT_ECODE_UNLIKELY: + return "Request attribute has encountered an unlikely error"; + case ATT_ECODE_INSUFF_ENC: + return "Encryption required before read/write"; + case ATT_ECODE_UNSUPP_GRP_TYPE: + return "Attribute type is not a supported grouping attribute"; + case ATT_ECODE_INSUFF_RESOURCES: + return "Insufficient Resources to complete the request"; + case ATT_ECODE_IO: + return "Internal application error: I/O"; + case ATT_ECODE_TIMEOUT: + return "A timeout occured"; + case ATT_ECODE_ABORTED: + return "The operation was aborted"; + default: + return "Unexpected error code"; + } +} + +void att_data_list_free(struct att_data_list *list) +{ + if (list == NULL) + return; + + if (list->data) { + int i; + for (i = 0; i < list->num; i++) + g_free(list->data[i]); + } + + g_free(list->data); + g_free(list); +} + +struct att_data_list *att_data_list_alloc(uint16_t num, uint16_t len) +{ + struct att_data_list *list; + int i; + + if (len > UINT8_MAX) + return NULL; + + list = g_new0(struct att_data_list, 1); + list->len = len; + list->num = num; + + list->data = g_malloc0(sizeof(uint8_t *) * num); + + for (i = 0; i < num; i++) + list->data[i] = g_malloc0(sizeof(uint8_t) * len); + + return list; +} + +uint16_t enc_read_by_grp_req(uint16_t start, uint16_t end, bt_uuid_t *uuid, + uint8_t *pdu, size_t len) +{ + uint16_t uuid_len; + + if (!uuid) + return 0; + + if (uuid->type == BT_UUID16) + uuid_len = 2; + else if (uuid->type == BT_UUID128) + uuid_len = 16; + else + return 0; + + /* Attribute Opcode (1 octet) */ + pdu[0] = ATT_OP_READ_BY_GROUP_REQ; + /* Starting Handle (2 octets) */ + att_put_u16(start, &pdu[1]); + /* Ending Handle (2 octets) */ + att_put_u16(end, &pdu[3]); + /* Attribute Group Type (2 or 16 octet UUID) */ + att_put_uuid(*uuid, &pdu[5]); + + return 5 + uuid_len; +} + +uint16_t dec_read_by_grp_req(const uint8_t *pdu, size_t len, uint16_t *start, + uint16_t *end, bt_uuid_t *uuid) +{ + const size_t min_len = sizeof(pdu[0]) + sizeof(*start) + sizeof(*end); + + if (pdu == NULL) + return 0; + + if (start == NULL || end == NULL || uuid == NULL) + return 0; + + if (pdu[0] != ATT_OP_READ_BY_GROUP_REQ) + return 0; + + if (len != (min_len + 2) && len != (min_len + 16)) + return 0; + + *start = att_get_u16(&pdu[1]); + *end = att_get_u16(&pdu[3]); + if (len == min_len + 2) + *uuid = att_get_uuid16(&pdu[5]); + else + *uuid = att_get_uuid128(&pdu[5]); + + return len; +} + +uint16_t enc_read_by_grp_resp(struct att_data_list *list, uint8_t *pdu, + size_t len) +{ + int i; + uint16_t w; + uint8_t *ptr; + + if (list == NULL) + return 0; + + if (len < list->len + sizeof(uint8_t) * 2) + return 0; + + pdu[0] = ATT_OP_READ_BY_GROUP_RESP; + pdu[1] = list->len; + + ptr = &pdu[2]; + + for (i = 0, w = 2; i < list->num && w + list->len <= len; i++) { + memcpy(ptr, list->data[i], list->len); + ptr += list->len; + w += list->len; + } + + return w; +} + +struct att_data_list *dec_read_by_grp_resp(const uint8_t *pdu, size_t len) +{ + struct att_data_list *list; + const uint8_t *ptr; + uint16_t elen, num; + int i; + + if (pdu[0] != ATT_OP_READ_BY_GROUP_RESP) + return NULL; + + /* PDU must contain at least: + * - Attribute Opcode (1 octet) + * - Length (1 octet) + * - Attribute Data List (at least one entry): + * - Attribute Handle (2 octets) + * - End Group Handle (2 octets) + * - Attribute Value (at least 1 octet) */ + if (len < 7) + return NULL; + + elen = pdu[1]; + /* Minimum Attribute Data List size */ + if (elen < 5) + return NULL; + + /* Reject incomplete Attribute Data List */ + if ((len - 2) % elen) + return NULL; + + num = (len - 2) / elen; + list = att_data_list_alloc(num, elen); + if (list == NULL) + return NULL; + + ptr = &pdu[2]; + + for (i = 0; i < num; i++) { + memcpy(list->data[i], ptr, list->len); + ptr += list->len; + } + + return list; +} + +uint16_t enc_find_by_type_req(uint16_t start, uint16_t end, bt_uuid_t *uuid, + const uint8_t *value, size_t vlen, + uint8_t *pdu, size_t len) +{ + uint16_t min_len = sizeof(pdu[0]) + sizeof(start) + sizeof(end) + + sizeof(uint16_t); + + if (pdu == NULL) + return 0; + + if (!uuid) + return 0; + + if (uuid->type != BT_UUID16) + return 0; + + if (vlen > len - min_len) + vlen = len - min_len; + + pdu[0] = ATT_OP_FIND_BY_TYPE_REQ; + att_put_u16(start, &pdu[1]); + att_put_u16(end, &pdu[3]); + att_put_uuid16(*uuid, &pdu[5]); + + if (vlen > 0) { + memcpy(&pdu[7], value, vlen); + return min_len + vlen; + } + + return min_len; +} + +uint16_t dec_find_by_type_req(const uint8_t *pdu, size_t len, uint16_t *start, + uint16_t *end, bt_uuid_t *uuid, + uint8_t *value, size_t *vlen) +{ + if (pdu == NULL) + return 0; + + if (len < 7) + return 0; + + /* Attribute Opcode (1 octet) */ + if (pdu[0] != ATT_OP_FIND_BY_TYPE_REQ) + return 0; + + /* First requested handle number (2 octets) */ + *start = att_get_u16(&pdu[1]); + /* Last requested handle number (2 octets) */ + *end = att_get_u16(&pdu[3]); + /* 16-bit UUID to find (2 octets) */ + *uuid = att_get_uuid16(&pdu[5]); + + /* Attribute value to find */ + *vlen = len - 7; + if (*vlen > 0) + memcpy(value, pdu + 7, *vlen); + + return len; +} + +uint16_t enc_find_by_type_resp(GSList *matches, uint8_t *pdu, size_t len) +{ + GSList *l; + uint16_t offset; + + if (!pdu) + return 0; + + pdu[0] = ATT_OP_FIND_BY_TYPE_RESP; + + for (l = matches, offset = 1; + l && len >= (offset + sizeof(uint16_t) * 2); + l = l->next, offset += sizeof(uint16_t) * 2) { + struct att_range *range = l->data; + + att_put_u16(range->start, &pdu[offset]); + att_put_u16(range->end, &pdu[offset + 2]); + } + + return offset; +} + +GSList *dec_find_by_type_resp(const uint8_t *pdu, size_t len) +{ + struct att_range *range; + GSList *matches; + off_t offset; + + /* PDU should contain at least: + * - Attribute Opcode (1 octet) + * - Handles Information List (at least one entry): + * - Found Attribute Handle (2 octets) + * - Group End Handle (2 octets) */ + if (pdu == NULL || len < 5) + return NULL; + + if (pdu[0] != ATT_OP_FIND_BY_TYPE_RESP) + return NULL; + + /* Reject incomplete Handles Information List */ + if ((len - 1) % 4) + return NULL; + + for (offset = 1, matches = NULL; + len >= (offset + sizeof(uint16_t) * 2); + offset += sizeof(uint16_t) * 2) { + range = g_new0(struct att_range, 1); + range->start = att_get_u16(&pdu[offset]); + range->end = att_get_u16(&pdu[offset + 2]); + + matches = g_slist_append(matches, range); + } + + return matches; +} + +uint16_t enc_read_by_type_req(uint16_t start, uint16_t end, bt_uuid_t *uuid, + uint8_t *pdu, size_t len) +{ + uint16_t uuid_len; + + if (!uuid) + return 0; + + if (uuid->type == BT_UUID16) + uuid_len = 2; + else if (uuid->type == BT_UUID128) + uuid_len = 16; + else + return 0; + + /* Attribute Opcode (1 octet) */ + pdu[0] = ATT_OP_READ_BY_TYPE_REQ; + /* Starting Handle (2 octets) */ + att_put_u16(start, &pdu[1]); + /* Ending Handle (2 octets) */ + att_put_u16(end, &pdu[3]); + /* Attribute Type (2 or 16 octet UUID) */ + att_put_uuid(*uuid, &pdu[5]); + + return 5 + uuid_len; +} + +uint16_t dec_read_by_type_req(const uint8_t *pdu, size_t len, uint16_t *start, + uint16_t *end, bt_uuid_t *uuid) +{ + const size_t min_len = sizeof(pdu[0]) + sizeof(*start) + sizeof(*end); + + if (pdu == NULL) + return 0; + + if (start == NULL || end == NULL || uuid == NULL) + return 0; + + if (len != (min_len + 2) && len != (min_len + 16)) + return 0; + + if (pdu[0] != ATT_OP_READ_BY_TYPE_REQ) + return 0; + + *start = att_get_u16(&pdu[1]); + *end = att_get_u16(&pdu[3]); + + if (len == min_len + 2) + *uuid = att_get_uuid16(&pdu[5]); + else + *uuid = att_get_uuid128(&pdu[5]); + + return len; +} + +uint16_t enc_read_by_type_resp(struct att_data_list *list, uint8_t *pdu, + size_t len) +{ + uint8_t *ptr; + size_t i, w, l; + + if (list == NULL) + return 0; + + if (pdu == NULL) + return 0; + + l = MIN(len - 2, list->len); + + pdu[0] = ATT_OP_READ_BY_TYPE_RESP; + pdu[1] = l; + ptr = &pdu[2]; + + for (i = 0, w = 2; i < list->num && w + l <= len; i++) { + memcpy(ptr, list->data[i], l); + ptr += l; + w += l; + } + + return w; +} + +struct att_data_list *dec_read_by_type_resp(const uint8_t *pdu, size_t len) +{ + struct att_data_list *list; + const uint8_t *ptr; + uint16_t elen, num; + int i; + + if (pdu[0] != ATT_OP_READ_BY_TYPE_RESP) + return NULL; + + /* PDU must contain at least: + * - Attribute Opcode (1 octet) + * - Length (1 octet) + * - Attribute Data List (at least one entry): + * - Attribute Handle (2 octets) + * - Attribute Value (at least 1 octet) */ + if (len < 5) + return NULL; + + elen = pdu[1]; + /* Minimum Attribute Data List size */ + if (elen < 3) + return NULL; + + /* Reject incomplete Attribute Data List */ + if ((len - 2) % elen) + return NULL; + + num = (len - 2) / elen; + list = att_data_list_alloc(num, elen); + if (list == NULL) + return NULL; + + ptr = &pdu[2]; + + for (i = 0; i < num; i++) { + memcpy(list->data[i], ptr, list->len); + ptr += list->len; + } + + return list; +} + +uint16_t enc_write_cmd(uint16_t handle, const uint8_t *value, size_t vlen, + uint8_t *pdu, size_t len) +{ + const uint16_t min_len = sizeof(pdu[0]) + sizeof(handle); + + if (pdu == NULL) + return 0; + + if (vlen > len - min_len) + vlen = len - min_len; + + pdu[0] = ATT_OP_WRITE_CMD; + att_put_u16(handle, &pdu[1]); + + if (vlen > 0) { + memcpy(&pdu[3], value, vlen); + return min_len + vlen; + } + + return min_len; +} + +uint16_t dec_write_cmd(const uint8_t *pdu, size_t len, uint16_t *handle, + uint8_t *value, size_t *vlen) +{ + const uint16_t min_len = sizeof(pdu[0]) + sizeof(*handle); + + if (pdu == NULL) + return 0; + + if (value == NULL || vlen == NULL || handle == NULL) + return 0; + + if (len < min_len) + return 0; + + if (pdu[0] != ATT_OP_WRITE_CMD) + return 0; + + *handle = att_get_u16(&pdu[1]); + memcpy(value, pdu + min_len, len - min_len); + *vlen = len - min_len; + + return len; +} + +uint16_t enc_write_req(uint16_t handle, const uint8_t *value, size_t vlen, + uint8_t *pdu, size_t len) +{ + const uint16_t min_len = sizeof(pdu[0]) + sizeof(handle); + + if (pdu == NULL) + return 0; + + if (vlen > len - min_len) + vlen = len - min_len; + + pdu[0] = ATT_OP_WRITE_REQ; + att_put_u16(handle, &pdu[1]); + + if (vlen > 0) { + memcpy(&pdu[3], value, vlen); + return min_len + vlen; + } + + return min_len; +} + +uint16_t dec_write_req(const uint8_t *pdu, size_t len, uint16_t *handle, + uint8_t *value, size_t *vlen) +{ + const uint16_t min_len = sizeof(pdu[0]) + sizeof(*handle); + + if (pdu == NULL) + return 0; + + if (value == NULL || vlen == NULL || handle == NULL) + return 0; + + if (len < min_len) + return 0; + + if (pdu[0] != ATT_OP_WRITE_REQ) + return 0; + + *handle = att_get_u16(&pdu[1]); + *vlen = len - min_len; + if (*vlen > 0) + memcpy(value, pdu + min_len, *vlen); + + return len; +} + +uint16_t enc_write_resp(uint8_t *pdu) +{ + if (pdu == NULL) + return 0; + + pdu[0] = ATT_OP_WRITE_RESP; + + return sizeof(pdu[0]); +} + +uint16_t dec_write_resp(const uint8_t *pdu, size_t len) +{ + if (pdu == NULL) + return 0; + + if (pdu[0] != ATT_OP_WRITE_RESP) + return 0; + + return len; +} + +uint16_t enc_read_req(uint16_t handle, uint8_t *pdu, size_t len) +{ + if (pdu == NULL) + return 0; + + /* Attribute Opcode (1 octet) */ + pdu[0] = ATT_OP_READ_REQ; + /* Attribute Handle (2 octets) */ + att_put_u16(handle, &pdu[1]); + + return 3; +} + +uint16_t enc_read_blob_req(uint16_t handle, uint16_t offset, uint8_t *pdu, + size_t len) +{ + if (pdu == NULL) + return 0; + + /* Attribute Opcode (1 octet) */ + pdu[0] = ATT_OP_READ_BLOB_REQ; + /* Attribute Handle (2 octets) */ + att_put_u16(handle, &pdu[1]); + /* Value Offset (2 octets) */ + att_put_u16(offset, &pdu[3]); + + return 5; +} + +uint16_t dec_read_req(const uint8_t *pdu, size_t len, uint16_t *handle) +{ + const uint16_t min_len = sizeof(pdu[0]) + sizeof(*handle); + + if (pdu == NULL) + return 0; + + if (handle == NULL) + return 0; + + if (len < min_len) + return 0; + + if (pdu[0] != ATT_OP_READ_REQ) + return 0; + + *handle = att_get_u16(&pdu[1]); + + return min_len; +} + +uint16_t dec_read_blob_req(const uint8_t *pdu, size_t len, uint16_t *handle, + uint16_t *offset) +{ + const uint16_t min_len = sizeof(pdu[0]) + sizeof(*handle) + + sizeof(*offset); + + if (pdu == NULL) + return 0; + + if (handle == NULL) + return 0; + + if (offset == NULL) + return 0; + + if (len < min_len) + return 0; + + if (pdu[0] != ATT_OP_READ_BLOB_REQ) + return 0; + + *handle = att_get_u16(&pdu[1]); + *offset = att_get_u16(&pdu[3]); + + return min_len; +} + +uint16_t enc_read_resp(uint8_t *value, size_t vlen, uint8_t *pdu, size_t len) +{ + if (pdu == NULL) + return 0; + + /* If the attribute value length is longer than the allowed PDU size, + * send only the octets that fit on the PDU. The remaining octets can + * be requested using the Read Blob Request. */ + if (vlen > len - 1) + vlen = len - 1; + + pdu[0] = ATT_OP_READ_RESP; + + memcpy(pdu + 1, value, vlen); + + return vlen + 1; +} + +uint16_t enc_read_blob_resp(uint8_t *value, size_t vlen, uint16_t offset, + uint8_t *pdu, size_t len) +{ + if (pdu == NULL) + return 0; + + vlen -= offset; + if (vlen > len - 1) + vlen = len - 1; + + pdu[0] = ATT_OP_READ_BLOB_RESP; + + memcpy(pdu + 1, &value[offset], vlen); + + return vlen + 1; +} + +ssize_t dec_read_resp(const uint8_t *pdu, size_t len, uint8_t *value, + size_t vlen) +{ + if (pdu == NULL) + return -EINVAL; + + if (pdu[0] != ATT_OP_READ_RESP) + return -EINVAL; + + if (value == NULL) + return len - 1; + + if (vlen < (len - 1)) + return -ENOBUFS; + + memcpy(value, pdu + 1, len - 1); + + return len - 1; +} + +uint16_t enc_error_resp(uint8_t opcode, uint16_t handle, uint8_t status, + uint8_t *pdu, size_t len) +{ + /* Attribute Opcode (1 octet) */ + pdu[0] = ATT_OP_ERROR; + /* Request Opcode In Error (1 octet) */ + pdu[1] = opcode; + /* Attribute Handle In Error (2 octets) */ + att_put_u16(handle, &pdu[2]); + /* Error Code (1 octet) */ + pdu[4] = status; + + return 5; +} + +uint16_t enc_find_info_req(uint16_t start, uint16_t end, uint8_t *pdu, + size_t len) +{ + if (pdu == NULL) + return 0; + + /* Attribute Opcode (1 octet) */ + pdu[0] = ATT_OP_FIND_INFO_REQ; + /* Starting Handle (2 octets) */ + att_put_u16(start, &pdu[1]); + /* Ending Handle (2 octets) */ + att_put_u16(end, &pdu[3]); + + return 5; +} + +uint16_t dec_find_info_req(const uint8_t *pdu, size_t len, uint16_t *start, + uint16_t *end) +{ + const uint16_t min_len = sizeof(pdu[0]) + sizeof(*start) + sizeof(*end); + + if (pdu == NULL) + return 0; + + if (len < min_len) + return 0; + + if (start == NULL || end == NULL) + return 0; + + if (pdu[0] != ATT_OP_FIND_INFO_REQ) + return 0; + + *start = att_get_u16(&pdu[1]); + *end = att_get_u16(&pdu[3]); + + return min_len; +} + +uint16_t enc_find_info_resp(uint8_t format, struct att_data_list *list, + uint8_t *pdu, size_t len) +{ + uint8_t *ptr; + size_t i, w; + + if (pdu == NULL) + return 0; + + if (list == NULL) + return 0; + + if (len < list->len + sizeof(uint8_t) * 2) + return 0; + + pdu[0] = ATT_OP_FIND_INFO_RESP; + pdu[1] = format; + ptr = (void *) &pdu[2]; + + for (i = 0, w = 2; i < list->num && w + list->len <= len; i++) { + memcpy(ptr, list->data[i], list->len); + ptr += list->len; + w += list->len; + } + + return w; +} + +struct att_data_list *dec_find_info_resp(const uint8_t *pdu, size_t len, + uint8_t *format) +{ + struct att_data_list *list; + uint8_t *ptr; + uint16_t elen, num; + int i; + + if (pdu == NULL) + return 0; + + if (format == NULL) + return 0; + + if (pdu[0] != ATT_OP_FIND_INFO_RESP) + return 0; + + *format = pdu[1]; + elen = sizeof(pdu[0]) + sizeof(*format); + if (*format == 0x01) + elen += 2; + else if (*format == 0x02) + elen += 16; + + num = (len - 2) / elen; + + ptr = (void *) &pdu[2]; + + list = att_data_list_alloc(num, elen); + if (list == NULL) + return NULL; + + for (i = 0; i < num; i++) { + memcpy(list->data[i], ptr, list->len); + ptr += list->len; + } + + return list; +} + +uint16_t enc_notification(uint16_t handle, uint8_t *value, size_t vlen, + uint8_t *pdu, size_t len) +{ + const uint16_t min_len = sizeof(pdu[0]) + sizeof(uint16_t); + + if (pdu == NULL) + return 0; + + if (len < (vlen + min_len)) + return 0; + + pdu[0] = ATT_OP_HANDLE_NOTIFY; + att_put_u16(handle, &pdu[1]); + memcpy(&pdu[3], value, vlen); + + return vlen + min_len; +} + +uint16_t enc_indication(uint16_t handle, uint8_t *value, size_t vlen, + uint8_t *pdu, size_t len) +{ + const uint16_t min_len = sizeof(pdu[0]) + sizeof(uint16_t); + + if (pdu == NULL) + return 0; + + if (len < (vlen + min_len)) + return 0; + + pdu[0] = ATT_OP_HANDLE_IND; + att_put_u16(handle, &pdu[1]); + memcpy(&pdu[3], value, vlen); + + return vlen + min_len; +} + +uint16_t dec_indication(const uint8_t *pdu, size_t len, uint16_t *handle, + uint8_t *value, size_t vlen) +{ + const uint16_t min_len = sizeof(pdu[0]) + sizeof(uint16_t); + uint16_t dlen; + + if (pdu == NULL) + return 0; + + if (pdu[0] != ATT_OP_HANDLE_IND) + return 0; + + if (len < min_len) + return 0; + + dlen = MIN(len - min_len, vlen); + + if (handle) + *handle = att_get_u16(&pdu[1]); + + memcpy(value, &pdu[3], dlen); + + return dlen; +} + +uint16_t enc_confirmation(uint8_t *pdu, size_t len) +{ + if (pdu == NULL) + return 0; + + /* Attribute Opcode (1 octet) */ + pdu[0] = ATT_OP_HANDLE_CNF; + + return 1; +} + +uint16_t enc_mtu_req(uint16_t mtu, uint8_t *pdu, size_t len) +{ + if (pdu == NULL) + return 0; + + /* Attribute Opcode (1 octet) */ + pdu[0] = ATT_OP_MTU_REQ; + /* Client Rx MTU (2 octets) */ + att_put_u16(mtu, &pdu[1]); + + return 3; +} + +uint16_t dec_mtu_req(const uint8_t *pdu, size_t len, uint16_t *mtu) +{ + const uint16_t min_len = sizeof(pdu[0]) + sizeof(*mtu); + + if (pdu == NULL) + return 0; + + if (mtu == NULL) + return 0; + + if (len < min_len) + return 0; + + if (pdu[0] != ATT_OP_MTU_REQ) + return 0; + + *mtu = att_get_u16(&pdu[1]); + + return min_len; +} + +uint16_t enc_mtu_resp(uint16_t mtu, uint8_t *pdu, size_t len) +{ + if (pdu == NULL) + return 0; + + /* Attribute Opcode (1 octet) */ + pdu[0] = ATT_OP_MTU_RESP; + /* Server Rx MTU (2 octets) */ + att_put_u16(mtu, &pdu[1]); + + return 3; +} + +uint16_t dec_mtu_resp(const uint8_t *pdu, size_t len, uint16_t *mtu) +{ + const uint16_t min_len = sizeof(pdu[0]) + sizeof(*mtu); + + if (pdu == NULL) + return 0; + + if (mtu == NULL) + return 0; + + if (len < min_len) + return 0; + + if (pdu[0] != ATT_OP_MTU_RESP) + return 0; + + *mtu = att_get_u16(&pdu[1]); + + return min_len; +} + +uint16_t enc_prep_write_req(uint16_t handle, uint16_t offset, + const uint8_t *value, size_t vlen, + uint8_t *pdu, size_t len) +{ + const uint16_t min_len = sizeof(pdu[0]) + sizeof(handle) + + sizeof(offset); + + if (pdu == NULL) + return 0; + + if (vlen > len - min_len) + vlen = len - min_len; + + pdu[0] = ATT_OP_PREP_WRITE_REQ; + att_put_u16(handle, &pdu[1]); + att_put_u16(offset, &pdu[3]); + + if (vlen > 0) { + memcpy(&pdu[5], value, vlen); + return min_len + vlen; + } + + return min_len; +} + +uint16_t dec_prep_write_req(const uint8_t *pdu, size_t len, uint16_t *handle, + uint16_t *offset, uint8_t *value, size_t *vlen) +{ + const uint16_t min_len = sizeof(pdu[0]) + sizeof(*handle) + + sizeof(*offset); + + if (pdu == NULL) + return 0; + + if (handle == NULL || offset == NULL || value == NULL || vlen == NULL) + return 0; + + if (len < min_len) + return 0; + + if (pdu[0] != ATT_OP_PREP_WRITE_REQ) + return 0; + + *handle = att_get_u16(&pdu[1]); + *offset = att_get_u16(&pdu[3]); + + *vlen = len - min_len; + if (*vlen > 0) + memcpy(value, pdu + min_len, *vlen); + + return len; +} + +uint16_t enc_prep_write_resp(uint16_t handle, uint16_t offset, + const uint8_t *value, size_t vlen, + uint8_t *pdu, size_t len) +{ + const uint16_t min_len = sizeof(pdu[0]) + sizeof(handle) + + sizeof(offset); + + if (pdu == NULL) + return 0; + + if (vlen > len - min_len) + vlen = len - min_len; + + pdu[0] = ATT_OP_PREP_WRITE_RESP; + att_put_u16(handle, &pdu[1]); + att_put_u16(offset, &pdu[3]); + + if (vlen > 0) { + memcpy(&pdu[5], value, vlen); + return min_len + vlen; + } + + return min_len; +} + +uint16_t dec_prep_write_resp(const uint8_t *pdu, size_t len, uint16_t *handle, + uint16_t *offset, uint8_t *value, size_t *vlen) +{ + const uint16_t min_len = sizeof(pdu[0]) + sizeof(*handle) + + sizeof(*offset); + + if (pdu == NULL) + return 0; + + if (handle == NULL || offset == NULL || value == NULL || vlen == NULL) + return 0; + + if (len < min_len) + return 0; + + if (pdu[0] != ATT_OP_PREP_WRITE_REQ) + return 0; + + *handle = att_get_u16(&pdu[1]); + *offset = att_get_u16(&pdu[3]); + *vlen = len - min_len; + if (*vlen > 0) + memcpy(value, pdu + min_len, *vlen); + + return len; +} + +uint16_t enc_exec_write_req(uint8_t flags, uint8_t *pdu, size_t len) +{ + if (pdu == NULL) + return 0; + + if (flags > 1) + return 0; + + /* Attribute Opcode (1 octet) */ + pdu[0] = ATT_OP_EXEC_WRITE_REQ; + /* Flags (1 octet) */ + pdu[1] = flags; + + return 2; +} + +uint16_t dec_exec_write_req(const uint8_t *pdu, size_t len, uint8_t *flags) +{ + const uint16_t min_len = sizeof(pdu[0]) + sizeof(*flags); + + if (pdu == NULL) + return 0; + + if (flags == NULL) + return 0; + + if (len < min_len) + return 0; + + if (pdu[0] != ATT_OP_EXEC_WRITE_REQ) + return 0; + + *flags = pdu[1]; + + return min_len; +} + +uint16_t enc_exec_write_resp(uint8_t *pdu) +{ + if (pdu == NULL) + return 0; + + /* Attribute Opcode (1 octet) */ + pdu[0] = ATT_OP_EXEC_WRITE_RESP; + + return 1; +} + +uint16_t dec_exec_write_resp(const uint8_t *pdu, size_t len) +{ + const uint16_t min_len = sizeof(pdu[0]); + + if (pdu == NULL) + return 0; + + if (len < min_len) + return 0; + + if (pdu[0] != ATT_OP_EXEC_WRITE_RESP) + return 0; + + return len; +} -- cgit v1.2.3