--- profiles/heartrate/heartrate.c | 171 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 171 insertions(+) diff --git a/profiles/heartrate/heartrate.c b/profiles/heartrate/heartrate.c index f78d91d..6549d4b 100644 --- a/profiles/heartrate/heartrate.c +++ b/profiles/heartrate/heartrate.c @@ -42,9 +42,16 @@ #include "log.h" #define HEART_RATE_MANAGER_IFACE "org.bluez.HeartRateManager" +#define HEART_RATE_WATCHER_IFACE "org.bluez.HeartRateWatcher" #define MIN_NOTIFICATION_LEN 3 /* 1-byte opcode + 2-byte handle */ +#define HR_VALUE_FORMAT 0x01 +#define SENSOR_CONTACT_DETECTED 0x02 +#define SENSOR_CONTACT_SUPPORT 0x04 +#define ENERGY_EXP_STATUS 0x08 +#define RR_INTERVAL 0x10 + struct heartrate_adapter { struct btd_adapter *adapter; GSList *devices; @@ -76,6 +83,17 @@ struct descriptor { bt_uuid_t uuid; }; +struct measurement { + struct heartrate *hr; + uint16_t value; + gboolean has_energy; + uint16_t energy; + gboolean has_contact; + gboolean contact; + uint16_t num_interval; + uint16_t *interval; +}; + struct watcher { struct heartrate_adapter *hra; guint id; @@ -123,6 +141,14 @@ static gint cmp_descriptor(gconstpointer a, gconstpointer b) return bt_uuid_cmp(&desc->uuid, uuid); } +static gint cmp_char_val_handle(gconstpointer a, gconstpointer b) +{ + const struct characteristic *ch = a; + const uint16_t *handle = b; + + return ch->attr.value_handle - *handle; +} + static gint cmp_watcher(gconstpointer a, gconstpointer b) { const struct watcher *watcher = a; @@ -552,12 +578,157 @@ static void configure_heartrate_cb(GSList *characteristics, guint8 status, } } +static void update_watcher(gpointer data, gpointer user_data) +{ + struct watcher *w = data; + struct measurement *m = user_data; + struct heartrate *hr = m->hr; + const gchar *path = device_get_path(hr->dev); + DBusMessageIter iter; + DBusMessageIter dict; + DBusMessage *msg; + + msg = dbus_message_new_method_call(w->srv, w->path, + HEART_RATE_WATCHER_IFACE, + "MeasurementReceived"); + if (msg == NULL) + return; + + dbus_message_iter_init_append(msg, &iter); + + dbus_message_iter_append_basic(&iter, DBUS_TYPE_OBJECT_PATH , &path); + + dbus_message_iter_open_container(&iter, DBUS_TYPE_ARRAY, + DBUS_DICT_ENTRY_BEGIN_CHAR_AS_STRING + DBUS_TYPE_STRING_AS_STRING DBUS_TYPE_VARIANT_AS_STRING + DBUS_DICT_ENTRY_END_CHAR_AS_STRING, &dict); + + dict_append_entry(&dict, "Value", DBUS_TYPE_UINT16, &m->value); + + if (m->has_energy) + dict_append_entry(&dict, "Energy", DBUS_TYPE_UINT16, + &m->energy); + + if (m->has_contact) + dict_append_entry(&dict, "Contact", DBUS_TYPE_BOOLEAN, + &m->contact); + + if (m->num_interval > 0) + dict_append_array(&dict, "Interval", DBUS_TYPE_UINT16, + &m->interval, m->num_interval); + + dbus_message_iter_close_container(&iter, &dict); + + dbus_message_set_no_reply(msg, TRUE); + g_dbus_send_message(get_dbus_connection(), msg); +} + +static void recv_measurement(struct heartrate *hr, struct measurement *m) +{ + GSList *wlist = hr->hra->watchers; + + m->hr = hr; + + g_slist_foreach(wlist, update_watcher, m); +} + +static void proc_measurement(struct heartrate *hr, const uint8_t *pdu, + uint16_t len) +{ + struct measurement m; + uint8_t flags; + + if (len < 4) { + error("Mandatory flags are not provided"); + return; + } + + flags = pdu[3]; + + pdu += 4; + len -= 4; + + memset(&m, 0, sizeof(m)); + + if (flags & HR_VALUE_FORMAT) { + if (len < 2) { + error("Heart Rate Measurement field missing"); + return; + } + + m.value = att_get_u16(pdu); + pdu += 2; + len -= 2; + } else { + if (len < 1) { + error("Heart Rate Measurement field missing"); + return; + } + + m.value = *pdu; + pdu++; + len--; + } + + if (flags & ENERGY_EXP_STATUS) { + if (len < 2) { + error("Energy Expended field missing"); + return; + } + + m.has_energy = TRUE; + m.energy = att_get_u16(pdu); + pdu += 2; + len -= 2; + } + + if (flags & RR_INTERVAL) { + int i; + + if (len == 0 || (len % 2 != 0)) { + error("RR-Interval field malformed"); + return; + } + + m.num_interval = len / 2; + m.interval = g_new(uint16_t, m.num_interval); + + for (i = 0; i < m.num_interval; pdu += 2, i++) + m.interval[i] = att_get_u16(pdu); + } + + if (flags & SENSOR_CONTACT_SUPPORT) { + m.has_contact = TRUE; + m.contact = !!(flags & SENSOR_CONTACT_DETECTED); + } + + recv_measurement(hr, &m); + + g_free(m.interval); +} + static void notify_handler(const uint8_t *pdu, uint16_t len, gpointer user_data) { + struct heartrate *hr = user_data; + const struct characteristic *ch; + uint16_t handle; + GSList *l; + if (len < MIN_NOTIFICATION_LEN) { error("Bad pdu received"); return; } + + handle = att_get_u16(&pdu[1]); + l = g_slist_find_custom(hr->chars, &handle, cmp_char_val_handle); + if (l == NULL) { + error("Unexpected handle: 0x%04x", handle); + return; + } + + ch = l->data; + if (g_strcmp0(ch->attr.uuid, HEART_RATE_MEASUREMENT_UUID) == 0) + proc_measurement(hr, pdu, len); } static void attio_connected_cb(GAttrib *attrib, gpointer user_data) -- 1.7.11.3 -- To unsubscribe from this list: send the line "unsubscribe linux-bluetooth" in the body of a message to majordomo@xxxxxxxxxxxxxxx More majordomo info at http://vger.kernel.org/majordomo-info.html