Momentum-Apps/seos/seos_att.c

#include "seos_att_i.h"

#define TAG "SeosAtt"

struct att_read_by_type_req {
    uint8_t opcode;
    uint16_t start_handle;
    uint16_t end_handle;
    union {
        uint16_t short_uuid;
        uint8_t long_uuid[16];
    } attribute_type;
} __packed;

struct att_find_info_req {
    uint8_t opcode;
    uint16_t start_handle;
    uint16_t end_handle;
} __packed;

struct att_find_type_value_req {
    uint8_t opcode;
    uint16_t start_handle;
    uint16_t end_handle;
    uint16_t att_type;
    uint8_t att_value[0];
} __packed;

struct att_write_req {
    uint8_t opcode;
    uint16_t handle;
} __packed;

static uint8_t seos_reader_service_backwards[] =
    {0x02, 0x00, 0x00, 0x7a, 0x17, 0x00, 0x00, 0x80, 0x00, 0x10, 0x00, 0x00, 0x00, 0x98, 0x00, 0x00};
static uint8_t seos_cred_service_backwards[] =
    {0x02, 0x00, 0x00, 0x7a, 0x17, 0x00, 0x00, 0x80, 0x00, 0x10, 0x00, 0x00, 0x01, 0x98, 0x00, 0x00};

SeosAtt* seos_att_alloc(Seos* seos) {
    SeosAtt* seos_att = malloc(sizeof(SeosAtt));
    memset(seos_att, 0, sizeof(SeosAtt));

    seos_att->seos = seos;
    seos_att->seos_l2cap = seos_l2cap_alloc(seos);
    seos_l2cap_set_receive_callback(seos_att->seos_l2cap, seos_att_process_payload, seos_att);
    seos_l2cap_set_central_connection_callback(
        seos_att->seos_l2cap, seos_att_central_connection_start, seos_att);

    seos_att->tx_buf = bit_buffer_alloc(128);

    seos_att->tx_mtu = 0;
    seos_att->rx_mtu = 0x0200;

    return seos_att;
}

void seos_att_free(SeosAtt* seos_att) {
    furi_assert(seos_att);

    seos_l2cap_free(seos_att->seos_l2cap);

    bit_buffer_free(seos_att->tx_buf);
    free(seos_att);
}

void seos_att_start(SeosAtt* seos_att, BleMode mode, FlowMode flow_mode) {
    seos_att->ble_mode = mode;
    seos_att->flow_mode = flow_mode;
    seos_l2cap_start(seos_att->seos_l2cap, mode, flow_mode);
}

void seos_att_stop(SeosAtt* seos_att) {
    seos_l2cap_stop(seos_att->seos_l2cap);
}

void seos_att_central_connection_start(void* context) {
    SeosAtt* seos_att = (SeosAtt*)context;
    FURI_LOG_D(TAG, "central connnection start");
    bit_buffer_reset(seos_att->tx_buf);

    if(seos_att->flow_mode == FLOW_READER) {
        uint16_t start = 0x0001;
        uint16_t end = 0xffff;
        uint16_t attribute_type = PRIMARY_SERVICE;

        bit_buffer_append_byte(seos_att->tx_buf, ATT_FIND_BY_TYPE_VALUE_REQ);
        bit_buffer_append_bytes(seos_att->tx_buf, (uint8_t*)&start, sizeof(start));
        bit_buffer_append_bytes(seos_att->tx_buf, (uint8_t*)&end, sizeof(end));
        bit_buffer_append_bytes(
            seos_att->tx_buf, (uint8_t*)&attribute_type, sizeof(attribute_type));
        bit_buffer_append_bytes(
            seos_att->tx_buf, seos_cred_service_backwards, sizeof(seos_cred_service_backwards));
    } else if(seos_att->flow_mode == FLOW_CRED) {
        // First thing to do with any new connection is the MTU
        bit_buffer_append_byte(seos_att->tx_buf, ATT_EXCHANGE_MTU_REQ);
        bit_buffer_append_bytes(
            seos_att->tx_buf, (uint8_t*)&seos_att->rx_mtu, sizeof(seos_att->rx_mtu));
    }

    seos_l2cap_send(seos_att->seos_l2cap, seos_att->tx_buf);
}

void seos_att_notify(SeosAtt* seos_att, uint16_t handle, BitBuffer* content) {
    seos_log_bitbuffer(TAG, "notify", content);
    size_t content_len = bit_buffer_get_size_bytes(content);

    BitBuffer* tx = bit_buffer_alloc(1 + sizeof(handle) + content_len);

    bit_buffer_append_byte(tx, ATT_HANDLE_VALUE_NTF);
    bit_buffer_append_bytes(tx, (uint8_t*)&handle, sizeof(handle));
    bit_buffer_append_bytes(tx, bit_buffer_get_data(content), content_len);

    seos_l2cap_send(seos_att->seos_l2cap, tx);
    bit_buffer_free(tx);
}

void seos_att_write_request(SeosAtt* seos_att, BitBuffer* content) {
    seos_log_bitbuffer(TAG, "write_request", content);
    size_t content_len = bit_buffer_get_size_bytes(content);

    BitBuffer* tx = bit_buffer_alloc(1 + sizeof(seos_att->value_handle) + content_len);

    bit_buffer_append_byte(tx, ATT_WRITE_CMD);
    bit_buffer_append_bytes(
        tx, (uint8_t*)&(seos_att->value_handle), sizeof(seos_att->value_handle));
    bit_buffer_append_bytes(tx, bit_buffer_get_data(content), content_len);

    seos_l2cap_send(seos_att->seos_l2cap, tx);
    bit_buffer_free(tx);
}

// TODO: figure out the proper name for data that comes in
void seos_att_process_payload(void* context, BitBuffer* message) {
    SeosAtt* seos_att = (SeosAtt*)context;
    // seos_log_bitbuffer(TAG, "process payload", message);

    bit_buffer_reset(seos_att->tx_buf);
    const uint8_t* data = bit_buffer_get_data(message);
    uint8_t opcode = data[0];
    struct att_read_by_type_req* s;
    uint16_t* start_handle;
    uint16_t* end_handle;
    uint16_t attribute_type;
    size_t length = 0;

    if(seos_att->ble_mode == BLE_CENTRAL && ((opcode & 0x01) == 0x00)) {
        bool reject = true;
        FURI_LOG_I(
            TAG,
            "%s request(0x%02x) when operating as Central",
            reject ? "Rejecting" : "Ignoring",
            opcode);
        if(reject) {
            bit_buffer_append_byte(seos_att->tx_buf, ATT_ERROR_RSP);
            bit_buffer_append_byte(seos_att->tx_buf, opcode);
            bit_buffer_append_bytes(seos_att->tx_buf, data + 1, sizeof(uint16_t));
            bit_buffer_append_byte(seos_att->tx_buf, 0x0a);
            seos_l2cap_send(seos_att->seos_l2cap, seos_att->tx_buf);
        }
        return;
    }

    switch(opcode) {
    case ATT_ERROR_RSP:
        uint8_t error_with_opcode = data[1];
        FURI_LOG_W(TAG, "Error with command %02x", error_with_opcode);
        break;
    case ATT_EXCHANGE_MTU_REQ: // MTU request

        // Trying a new way to copy the uint16_t
        seos_att->tx_mtu = *(uint16_t*)(data + 1);
        FURI_LOG_D(TAG, "MTU REQ = %04x", seos_att->tx_mtu);

        bit_buffer_append_byte(seos_att->tx_buf, ATT_EXCHANGE_MTU_RSP);
        bit_buffer_append_bytes(
            seos_att->tx_buf, (uint8_t*)&seos_att->rx_mtu, sizeof(seos_att->rx_mtu));
        break;
    case ATT_EXCHANGE_MTU_RSP:
        seos_att->tx_mtu = *(uint16_t*)(data + 1);
        FURI_LOG_D(TAG, "MTU RSP = %04x", seos_att->tx_mtu);

        uint16_t start = 0x0001;
        uint16_t end = 0xffff;
        attribute_type = PRIMARY_SERVICE;

        bit_buffer_append_byte(seos_att->tx_buf, ATT_FIND_BY_TYPE_VALUE_REQ);
        bit_buffer_append_bytes(seos_att->tx_buf, (uint8_t*)&start, sizeof(start));
        bit_buffer_append_bytes(seos_att->tx_buf, (uint8_t*)&end, sizeof(end));
        bit_buffer_append_bytes(
            seos_att->tx_buf, (uint8_t*)&attribute_type, sizeof(attribute_type));
        bit_buffer_append_bytes(
            seos_att->tx_buf,
            seos_reader_service_backwards,
            sizeof(seos_reader_service_backwards));
        break;
    case ATT_READ_BY_TYPE_REQ:
        s = (struct att_read_by_type_req*)(data);
        FURI_LOG_D(
            TAG,
            "ATT read by type %04x - %04x, type %04x",
            s->start_handle,
            s->end_handle,
            s->attribute_type.short_uuid);

        if(s->attribute_type.short_uuid == CHARACTERISTIC) {
            if(s->start_handle == 0x0006) {
                bit_buffer_append_byte(seos_att->tx_buf, ATT_READ_BY_TYPE_RSP);
                uint8_t response[] = {0x07, 0x07, 0x00, 0x20, 0x08, 0x00, 0x05, 0x2a};
                bit_buffer_append_bytes(seos_att->tx_buf, response, sizeof(response));

            } else if(s->start_handle == 0x000a) {
                bit_buffer_append_byte(seos_att->tx_buf, ATT_READ_BY_TYPE_RSP);
                uint8_t flow_mode_byte = seos_att->flow_mode == FLOW_READER ? 0x00 : 0x01;
                uint8_t response[] = {0x15, 0x0b, 0x00,           0x14, 0x0c, 0x00, 0x02, 0x00,
                                      0x00, 0x7a, 0x17,           0x00, 0x00, 0x80, 0x00, 0x10,
                                      0x00, 0x00, flow_mode_byte, 0xaa, 0x00, 0x00};
                bit_buffer_append_bytes(seos_att->tx_buf, response, sizeof(response));
            }
        } else if(s->attribute_type.short_uuid == DEVICE_NAME) {
            if(s->start_handle == 0x0001) {
                uint8_t response[] = {0x09, 0x03, 0x00, 0x46, 0x6c, 0x69, 0x70, 0x70, 0x65, 0x72};
                bit_buffer_append_bytes(seos_att->tx_buf, response, sizeof(response));
            }
        }

        if(bit_buffer_get_size_bytes(seos_att->tx_buf) == 0) {
            FURI_LOG_W(TAG, "Return error");
            bit_buffer_reset(seos_att->tx_buf);
            bit_buffer_append_byte(seos_att->tx_buf, ATT_ERROR_RSP);
            bit_buffer_append_byte(seos_att->tx_buf, opcode);
            bit_buffer_append_bytes(
                seos_att->tx_buf, (uint8_t*)&s->start_handle, sizeof(s->start_handle));
            bit_buffer_append_byte(seos_att->tx_buf, ATT_ERROR_CODE_ATTRIBUTE_NOT_FOUND);
        }
        break;
    case ATT_READ_BY_TYPE_RSP:
        seos_log_buffer(
            TAG, "ATT_READ_BY_TYPE_RSP", (uint8_t*)data, bit_buffer_get_size_bytes(message));

        uint16_t* handle;
        handle = (uint16_t*)(data + 2);
        seos_att->characteristic_handle = *handle;
        // skip properties byte
        handle = (uint16_t*)(data + 5);
        seos_att->value_handle = *handle;

        *handle = *handle + 1;
        bit_buffer_append_byte(seos_att->tx_buf, ATT_FIND_INFORMATION_REQ);
        bit_buffer_append_bytes(seos_att->tx_buf, (uint8_t*)handle, sizeof(uint16_t));
        bit_buffer_append_bytes(
            seos_att->tx_buf, (uint8_t*)&seos_att->service_end_handle, sizeof(uint16_t));
        break;
    case ATT_READ_BY_GROUP_TYPE_REQ:
        s = (struct att_read_by_type_req*)(data);
        FURI_LOG_D(
            TAG,
            "ATT read by group type %04x - %04x, type %04x",
            s->start_handle,
            s->end_handle,
            s->attribute_type.short_uuid);
        if(s->attribute_type.short_uuid == PRIMARY_SERVICE) {
            if(s->start_handle == 0x0001) {
                bit_buffer_append_byte(seos_att->tx_buf, ATT_READ_BY_GROUP_TYPE_RSP);
                uint8_t response[] = {
                    0x06, 0x01, 0x00, 0x05, 0x00, 0x00, 0x18, 0x06, 0x00, 0x09, 0x00, 0x01, 0x18};
                bit_buffer_append_bytes(seos_att->tx_buf, response, sizeof(response));
            } else if(s->start_handle == 0x000a) {
                bit_buffer_append_byte(seos_att->tx_buf, ATT_READ_BY_GROUP_TYPE_RSP);
                uint8_t flow_mode_byte = seos_att->flow_mode == FLOW_READER ? 0x00 : 0x01;
                uint8_t response[] = {0x14, 0x0a, 0x00, 0x0d,           0x00, 0x02, 0x00,
                                      0x00, 0x7a, 0x17, 0x00,           0x00, 0x80, 0x00,
                                      0x10, 0x00, 0x00, flow_mode_byte, 0x98, 0x00, 0x00};
                bit_buffer_append_bytes(seos_att->tx_buf, response, sizeof(response));
            }
        }

        // Didn't match either attribute_type or start_handle
        if(bit_buffer_get_size_bytes(seos_att->tx_buf) == 0) {
            FURI_LOG_W(TAG, "Return error");
            bit_buffer_reset(seos_att->tx_buf);
            bit_buffer_append_byte(seos_att->tx_buf, ATT_ERROR_RSP);
            bit_buffer_append_byte(seos_att->tx_buf, opcode);
            bit_buffer_append_bytes(
                seos_att->tx_buf, (uint8_t*)&s->start_handle, sizeof(s->start_handle));
            bit_buffer_append_byte(seos_att->tx_buf, ATT_ERROR_CODE_ATTRIBUTE_NOT_FOUND);
        }
        break;
    case ATT_READ_BY_GROUP_TYPE_RSP:
        seos_log_buffer(
            TAG, "ATT_READ_BY_GROUP_TYPE_RSP", (uint8_t*)data, bit_buffer_get_size_bytes(message));
        // NOTE: this might not be actively used

        uint8_t size = data[1];
        size_t i = 2;
        do {
            start_handle = (uint16_t*)(data + i);
            end_handle = (uint16_t*)(data + sizeof(uint16_t) + i);

            // +4 for 2 uint16_t
            if(size == (sizeof(seos_cred_service_backwards) + 4)) {
                if(memcmp(
                       seos_cred_service_backwards,
                       data + i + 4,
                       sizeof(seos_cred_service_backwards)) == 0) {
                    seos_att->service_start_handle = *start_handle;
                    seos_att->service_end_handle = *end_handle;
                }
            }
            i += size;
        } while(i < bit_buffer_get_size_bytes(message));
        *end_handle = *end_handle + 1;

        bit_buffer_append_byte(seos_att->tx_buf, ATT_READ_BY_GROUP_TYPE_REQ);
        bit_buffer_append_bytes(seos_att->tx_buf, (uint8_t*)end_handle, sizeof(uint16_t));
        uint8_t suffix[] = {0xff, 0xff, 0x00, 0x28};
        bit_buffer_append_bytes(seos_att->tx_buf, suffix, sizeof(suffix));

        break;
    case ATT_FIND_INFORMATION_REQ:
        struct att_find_info_req* e = (struct att_find_info_req*)(data);
        FURI_LOG_D(TAG, "ATT find information %04x - %04x", e->start_handle, e->end_handle);
        bit_buffer_append_byte(seos_att->tx_buf, ATT_FIND_INFORMATION_RSP);

        if(e->start_handle == 0x0009) {
            uint8_t response[] = {0x01, 0x09, 0x00, 0x02, 0x29};
            bit_buffer_append_bytes(seos_att->tx_buf, response, sizeof(response));
        } else if(e->start_handle == 0x000d) {
            uint8_t response[] = {0x01, 0x0d, 0x00, 0x02, 0x29};
            bit_buffer_append_bytes(seos_att->tx_buf, response, sizeof(response));
        } else {
            FURI_LOG_W(TAG, "unhandled handle in ATT_FIND_INFORMATION_REQ");
        }
        break;
    case ATT_FIND_INFORMATION_RSP:
        seos_log_buffer(
            TAG, "ATT_FIND_INFORMATION_RSP", (uint8_t*)data, bit_buffer_get_size_bytes(message));

        // 05 01 3c00 0029 3d00 0229
        uint8_t format = data[1];
        if(format == 1) { // short UUID
            uint16_t* handle;
            uint16_t* uuid;
            size_t i = 2;
            do {
                handle = (uint16_t*)(data + i);
                uuid = (uint16_t*)(data + i + 2);
                i += 4;
                if(*uuid == CCCD) {
                    seos_att->cccd_handle = *handle;
                }
            } while(i < bit_buffer_get_size_bytes(message));

            if(seos_att->cccd_handle > 0) {
                FURI_LOG_I(TAG, "Subscribe to phone/device");
                uint16_t value = ENABLE_NOTIFICATION_VALUE;
                bit_buffer_append_byte(seos_att->tx_buf, ATT_WRITE_CMD);
                bit_buffer_append_bytes(
                    seos_att->tx_buf, (uint8_t*)&seos_att->cccd_handle, sizeof(uint16_t));
                bit_buffer_append_bytes(seos_att->tx_buf, (uint8_t*)&value, sizeof(value));
            }
        }

        break;
    case ATT_FIND_BY_TYPE_VALUE_REQ:
        struct att_find_type_value_req* t = (struct att_find_type_value_req*)(data);
        FURI_LOG_D(
            TAG,
            "ATT_FIND_BY_TYPE_VALUE_REQ %04x - %04x for %04x",
            t->start_handle,
            t->end_handle,
            t->att_type);
        if(t->att_type == PRIMARY_SERVICE) {
            bit_buffer_append_byte(seos_att->tx_buf, ATT_FIND_BY_TYPE_VALUE_RSP);
            if(seos_att->flow_mode == FLOW_CRED) {
                uint8_t response[] = {0x0a, 0x00, 0x0e, 0x00};
                bit_buffer_append_bytes(seos_att->tx_buf, response, sizeof(response));
            } else if(seos_att->flow_mode == FLOW_READER) {
                uint8_t response[] = {0x0c, 0x00, 0x0e, 0x00};
                bit_buffer_append_bytes(seos_att->tx_buf, response, sizeof(response));
            }
        }
        break;
    case ATT_FIND_BY_TYPE_VALUE_RSP:
        seos_log_buffer(
            TAG, "ATT_FIND_BY_TYPE_VALUE_RSP", (uint8_t*)data, bit_buffer_get_size_bytes(message));
        start_handle = (uint16_t*)(data + 1);
        end_handle = (uint16_t*)(data + 3);

        seos_att->service_start_handle = *start_handle;
        seos_att->service_end_handle = *end_handle;

        attribute_type = CHARACTERISTIC;
        bit_buffer_append_byte(seos_att->tx_buf, ATT_READ_BY_TYPE_REQ);
        bit_buffer_append_bytes(seos_att->tx_buf, (uint8_t*)start_handle, sizeof(uint16_t));
        bit_buffer_append_bytes(seos_att->tx_buf, (uint8_t*)end_handle, sizeof(uint16_t));
        bit_buffer_append_bytes(
            seos_att->tx_buf, (uint8_t*)&attribute_type, sizeof(attribute_type));
        break;
    case ATT_WRITE_REQ:
        struct att_write_req* w = (struct att_write_req*)(data);
        length = bit_buffer_get_size_bytes(message) - sizeof(struct att_write_req);
        FURI_LOG_D(TAG, "ATT Write Req %d bytes to %04x", length, w->handle);
        if(w->handle == 0x0009) {
            bit_buffer_append_byte(seos_att->tx_buf, ATT_WRITE_RSP);
        } else if(w->handle == 0x000d) {
            uint16_t* value = (uint16_t*)(data + sizeof(struct att_write_req));
            if(*value == DISABLE_NOTIFICATION_VALUE) {
                FURI_LOG_I(TAG, "Unsubscribe");
            } else if(*value == ENABLE_NOTIFICATION_VALUE) {
                FURI_LOG_I(TAG, "Subscribe");
                if(seos_att->on_subscribe) {
                    // comes in as 0x000d, but we need to use 0x000c: I'm sure there is a reason for this that I'm just not aware of
                    seos_att->on_subscribe(seos_att->on_subscribe_context, w->handle - 1);
                    bit_buffer_append_byte(seos_att->tx_buf, ATT_WRITE_RSP);
                } else {
                    FURI_LOG_W(TAG, "No on_subscribe callback defined");
                }
            }
        }
        break;
    case ATT_WRITE_RSP:
        FURI_LOG_D(TAG, "ATT_WRITE_RSP");
        break;
    case ATT_WRITE_CMD:
        struct att_write_req* c = (struct att_write_req*)(data);
        length = bit_buffer_get_size_bytes(message) - sizeof(struct att_write_req);
        FURI_LOG_D(TAG, "ATT Write CMD %d bytes to %04x", length, c->handle);

        if(c->handle == 0x000c) {
            BitBuffer* attribute_value = bit_buffer_alloc(length);
            bit_buffer_append_bytes(
                attribute_value,
                bit_buffer_get_data(message) + sizeof(struct att_write_req),
                length);
            if(seos_att->write_request) {
                seos_att->write_request(seos_att->write_request_context, attribute_value);
            }
            bit_buffer_free(attribute_value);
        } else {
            seos_log_bitbuffer(TAG, "write to unsupported handle", message);
        }
        // No response to CMD expected
        break;
    case ATT_HANDLE_VALUE_NTF:
        struct att_write_req* n = (struct att_write_req*)(data);
        length = bit_buffer_get_size_bytes(message) - sizeof(struct att_write_req);
        FURI_LOG_D(TAG, "ATT handle value notify %d bytes to %04x", length, n->handle);
        if(n->handle == 0x000d) {
            if(seos_att->notify) {
                seos_att->notify(
                    seos_att->notify_context,
                    bit_buffer_get_data(message) + sizeof(struct att_write_req),
                    length);
            }
        } else {
            FURI_LOG_W(TAG, "Notify with unhandled handle");
        }
        break;
    case ATT_HANDLE_VALUE_CFM:
        FURI_LOG_I(TAG, "Indication confirmation");
        break;
    default:
        FURI_LOG_W(TAG, "seos_att_process_message no handler for 0x%02x", opcode);
        break;
    }

    if(bit_buffer_get_size_bytes(seos_att->tx_buf) > 0) {
        seos_log_bitbuffer(TAG, "sending", seos_att->tx_buf);
        seos_l2cap_send(seos_att->seos_l2cap, seos_att->tx_buf);
    }
}

void seos_att_set_on_subscribe_callback(
    SeosAtt* seos_att,
    SeosAttOnSubscribeCallback callback,
    void* context) {
    seos_att->on_subscribe = callback;
    seos_att->on_subscribe_context = context;
}

void seos_att_set_write_request_callback(
    SeosAtt* seos_att,
    SeosAttWriteRequestCallback callback,
    void* context) {
    seos_att->write_request = callback;
    seos_att->write_request_context = context;
}

void seos_att_set_notify_callback(SeosAtt* seos_att, SeosAttNotifyCallback callback, void* context) {
    seos_att->notify = callback;
    seos_att->notify_context = context;
}