thermometer/thermometer.c

   1 /*
   2  *
   3  *  BlueZ - Bluetooth protocol stack for Linux
   4  *
   5  *  Copyright (C) 2011 GSyC/LibreSoft, Universidad Rey Juan Carlos.
   6  *
   7  *  This program is free software; you can redistribute it and/or modify
   8  *  it under the terms of the GNU General Public License as published by
   9  *  the Free Software Foundation; either version 2 of the License, or
  10  *  (at your option) any later version.
  11  *
  12  *  This program is distributed in the hope that it will be useful,
  13  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  14  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15  *  GNU General Public License for more details.
  16  *
  17  *  You should have received a copy of the GNU General Public License
  18  *  along with this program; if not, write to the Free Software
  19  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  20  *
  21  */
  22
  23 #ifdef HAVE_CONFIG_H
  24 #include <config.h>
  25 #endif
  26
  27 #include <gdbus.h>
  28 #include <errno.h>
  29 #include <bluetooth/uuid.h>
  30
  31 #include "dbus-common.h"
  32 #include "adapter.h"
  33 #include "device.h"
  34 #include "error.h"
  35 #include "log.h"
  36 #include "gattrib.h"
  37 #include "attio.h"
  38 #include "att.h"
  39 #include "gatt.h"
  40 #include "thermometer.h"
  41
  42 #define THERMOMETER_INTERFACE "org.bluez.Thermometer"
  43
  44 /* Temperature measurement flag fields */
  45 #define TEMP_UNITS              0x01
  46 #define TEMP_TIME_STAMP         0x02
  47 #define TEMP_TYPE               0x04
  48
  49 #define FLOAT_MAX_MANTISSA      16777216 /* 2^24 */
  50
  51 struct thermometer {
  52         DBusConnection          *conn;          /* The connection to the bus */
  53         struct btd_device       *dev;           /* Device reference */
  54         GAttrib                 *attrib;        /* GATT connection */
  55         struct att_range        *svc_range;     /* Thermometer range */
  56         guint                   attioid;        /* Att watcher id */
  57         guint                   attindid;       /* Att incications id */
  58         guint                   attnotid;       /* Att notifications id */
  59         GSList                  *chars;         /* Characteristics */
  60         GSList                  *fwatchers;     /* Final measurements */
  61         GSList                  *iwatchers;     /* Intermediate measurements */
  62         gboolean                intermediate;
  63         uint8_t                 type;
  64         uint16_t                interval;
  65         uint16_t                max;
  66         uint16_t                min;
  67         gboolean                has_type;
  68         gboolean                has_interval;
  69 };
  70
  71 struct characteristic {
  72         struct gatt_char                attr;   /* Characteristic */
  73         GSList                  *desc;  /* Descriptors */
  74         struct thermometer      *t;     /* Thermometer where the char belongs */
  75 };
  76
  77 struct descriptor {
  78         struct characteristic   *ch;
  79         uint16_t                handle;
  80         bt_uuid_t               uuid;
  81 };
  82
  83 struct watcher {
  84         struct thermometer      *t;
  85         guint                   id;
  86         char                    *srv;
  87         char                    *path;
  88 };
  89
  90 struct measurement {
  91         int16_t         exp;
  92         int32_t         mant;
  93         uint64_t        time;
  94         gboolean        suptime;
  95         char            *unit;
  96         char            *type;
  97         char            *value;
  98 };
  99
 100 struct tmp_interval_data {
 101         struct thermometer      *thermometer;
 102         uint16_t                interval;
 103 };
 104
 105 static GSList *thermometers = NULL;
 106
 107 const char *temp_type[] = {
 108         "<reserved>",
 109         "Armpit",
 110         "Body",
 111         "Ear",
 112         "Finger",
 113         "Intestines",
 114         "Mouth",
 115         "Rectum",
 116         "Toe",
 117         "Tympanum"
 118 };
 119
 120 static const gchar *temptype2str(uint8_t value)
 121 {
 122          if (value > 0 && value < G_N_ELEMENTS(temp_type))
 123                 return temp_type[value];
 124
 125         error("Temperature type %d reserved for future use", value);
 126         return NULL;
 127 }
 128
 129 static void destroy_watcher(gpointer user_data)
 130 {
 131         struct watcher *watcher = user_data;
 132
 133         g_free(watcher->path);
 134         g_free(watcher->srv);
 135         g_free(watcher);
 136 }
 137
 138 static void remove_watcher(gpointer user_data)
 139 {
 140         struct watcher *watcher = user_data;
 141
 142         g_dbus_remove_watch(watcher->t->conn, watcher->id);
 143 }
 144
 145 static void destroy_char(gpointer user_data)
 146 {
 147         struct characteristic *c = user_data;
 148
 149         g_slist_free_full(c->desc, g_free);
 150         g_free(c);
 151 }
 152
 153 static void destroy_thermometer(gpointer user_data)
 154 {
 155         struct thermometer *t = user_data;
 156
 157         if (t->attioid > 0)
 158                 btd_device_remove_attio_callback(t->dev, t->attioid);
 159
 160         if (t->attindid > 0)
 161                 g_attrib_unregister(t->attrib, t->attindid);
 162
 163         if (t->attnotid > 0)
 164                 g_attrib_unregister(t->attrib, t->attnotid);
 165
 166         if (t->attrib != NULL)
 167                 g_attrib_unref(t->attrib);
 168
 169         if (t->chars != NULL)
 170                 g_slist_free_full(t->chars, destroy_char);
 171
 172         if (t->fwatchers != NULL)
 173                 g_slist_free_full(t->fwatchers, remove_watcher);
 174
 175         dbus_connection_unref(t->conn);
 176         btd_device_unref(t->dev);
 177         g_free(t->svc_range);
 178         g_free(t);
 179 }
 180
 181 static gint cmp_device(gconstpointer a, gconstpointer b)
 182 {
 183         const struct thermometer *t = a;
 184         const struct btd_device *dev = b;
 185
 186         if (dev == t->dev)
 187                 return 0;
 188
 189         return -1;
 190 }
 191
 192 static gint cmp_watcher(gconstpointer a, gconstpointer b)
 193 {
 194         const struct watcher *watcher = a;
 195         const struct watcher *match = b;
 196         int ret;
 197
 198         ret = g_strcmp0(watcher->srv, match->srv);
 199         if (ret != 0)
 200                 return ret;
 201
 202         return g_strcmp0(watcher->path, match->path);
 203 }
 204
 205 static gint cmp_char_uuid(gconstpointer a, gconstpointer b)
 206 {
 207         const struct characteristic *ch = a;
 208         const char *uuid = b;
 209
 210         return g_strcmp0(ch->attr.uuid, uuid);
 211 }
 212
 213 static gint cmp_char_val_handle(gconstpointer a, gconstpointer b)
 214 {
 215         const struct characteristic *ch = a;
 216         const uint16_t *handle = b;
 217
 218         return ch->attr.value_handle - *handle;
 219 }
 220
 221 static gint cmp_descriptor(gconstpointer a, gconstpointer b)
 222 {
 223         const struct descriptor *desc = a;
 224         const bt_uuid_t *uuid = b;
 225
 226         return bt_uuid_cmp(&desc->uuid, uuid);
 227 }
 228
 229 static struct characteristic *get_characteristic(struct thermometer *t,
 230                                                         const char *uuid)
 231 {
 232         GSList *l;
 233
 234         l = g_slist_find_custom(t->chars, uuid, cmp_char_uuid);
 235         if (l == NULL)
 236                 return NULL;
 237
 238         return l->data;
 239 }
 240
 241 static struct descriptor *get_descriptor(struct characteristic *ch,
 242                                                         const bt_uuid_t *uuid)
 243 {
 244         GSList *l;
 245
 246         l = g_slist_find_custom(ch->desc, uuid, cmp_descriptor);
 247         if (l == NULL)
 248                 return NULL;
 249
 250         return l->data;
 251 }
 252
 253 static void change_property(struct thermometer *t, const char *name,
 254                                                         gpointer value) {
 255         if (g_strcmp0(name, "Intermediate") == 0) {
 256                 gboolean *intermediate = value;
 257                 if (t->intermediate == *intermediate)
 258                         return;
 259
 260                 t->intermediate = *intermediate;
 261                 emit_property_changed(t->conn, device_get_path(t->dev),
 262                                         THERMOMETER_INTERFACE, name,
 263                                         DBUS_TYPE_BOOLEAN, &t->intermediate);
 264         } else if (g_strcmp0(name, "Interval") == 0) {
 265                 uint16_t *interval = value;
 266                 if (t->has_interval && t->interval == *interval)
 267                         return;
 268
 269                 t->has_interval = TRUE;
 270                 t->interval = *interval;
 271                 emit_property_changed(t->conn, device_get_path(t->dev),
 272                                         THERMOMETER_INTERFACE, name,
 273                                         DBUS_TYPE_UINT16, &t->interval);
 274         } else if (g_strcmp0(name, "Maximum") == 0) {
 275                 uint16_t *max = value;
 276                 if (t->max == *max)
 277                         return;
 278
 279                 t->max = *max;
 280                 emit_property_changed(t->conn, device_get_path(t->dev),
 281                                         THERMOMETER_INTERFACE, name,
 282                                         DBUS_TYPE_UINT16, &t->max);
 283         } else if (g_strcmp0(name, "Minimum") == 0) {
 284                 uint16_t *min = value;
 285                 if (t->min == *min)
 286                         return;
 287
 288                 t->min = *min;
 289                 emit_property_changed(t->conn, device_get_path(t->dev),
 290                                         THERMOMETER_INTERFACE, name,
 291                                         DBUS_TYPE_UINT16, &t->min);
 292         } else
 293                 DBG("%s is not a thermometer property", name);
 294 }
 295
 296 static void valid_range_desc_cb(guint8 status, const guint8 *pdu, guint16 len,
 297                                                         gpointer user_data)
 298 {
 299         struct descriptor *desc = user_data;
 300         uint8_t value[ATT_MAX_MTU];
 301         uint16_t max, min;
 302         int vlen;
 303
 304         if (status != 0) {
 305                 DBG("Valid Range descriptor read failed: %s",
 306                                                         att_ecode2str(status));
 307                 return;
 308         }
 309
 310         if (!dec_read_resp(pdu, len, value, &vlen)) {
 311                 DBG("Protocol error\n");
 312                 return;
 313         }
 314
 315         if (vlen < 4) {
 316                 DBG("Invalid range received");
 317                 return;
 318         }
 319
 320         min = att_get_u16(&value[0]);
 321         max = att_get_u16(&value[2]);
 322
 323         if (min == 0 || min > max) {
 324                 DBG("Invalid range");
 325                 return;
 326         }
 327
 328         change_property(desc->ch->t, "Maximum", &max);
 329         change_property(desc->ch->t, "Minimum", &min);
 330 }
 331
 332 static void measurement_cb(guint8 status, const guint8 *pdu,
 333                                                 guint16 len, gpointer user_data)
 334 {
 335         char *msg = user_data;
 336
 337         if (status != 0)
 338                 error("%s failed", msg);
 339
 340         g_free(msg);
 341 }
 342
 343 static void process_thermometer_desc(struct descriptor *desc)
 344 {
 345         struct characteristic *ch = desc->ch;
 346         char uuidstr[MAX_LEN_UUID_STR];
 347         bt_uuid_t btuuid;
 348
 349         bt_uuid16_create(&btuuid, GATT_CLIENT_CHARAC_CFG_UUID);
 350
 351         if (bt_uuid_cmp(&desc->uuid, &btuuid) == 0) {
 352                 uint8_t atval[2];
 353                 uint16_t val;
 354                 char *msg;
 355
 356                 if (g_strcmp0(ch->attr.uuid,
 357                                         TEMPERATURE_MEASUREMENT_UUID) == 0) {
 358                         if (g_slist_length(ch->t->fwatchers) == 0)
 359                                 return;
 360
 361                         val = GATT_CLIENT_CHARAC_CFG_IND_BIT;
 362                         msg = g_strdup("Enable Temperature Measurement "
 363                                                                 "indication");
 364                 } else if (g_strcmp0(ch->attr.uuid,
 365                                         INTERMEDIATE_TEMPERATURE_UUID) == 0) {
 366                         if (g_slist_length(ch->t->iwatchers) == 0)
 367                                 return;
 368
 369                         val = GATT_CLIENT_CHARAC_CFG_NOTIF_BIT;
 370                         msg = g_strdup("Enable Intermediate Temperature "
 371                                                                 "notification");
 372                 } else if (g_strcmp0(ch->attr.uuid,
 373                                         MEASUREMENT_INTERVAL_UUID) == 0) {
 374                         val = GATT_CLIENT_CHARAC_CFG_IND_BIT;
 375                         msg = g_strdup("Enable Measurement Interval "
 376                                                                 "indication");
 377                 } else
 378                         goto done;
 379
 380                 att_put_u16(val, atval);
 381                 gatt_write_char(ch->t->attrib, desc->handle, atval, 2,
 382                                                         measurement_cb, msg);
 383                 return;
 384         }
 385
 386         bt_uuid16_create(&btuuid, GATT_CHARAC_VALID_RANGE_UUID);
 387
 388         if (bt_uuid_cmp(&desc->uuid, &btuuid) == 0 && g_strcmp0(ch->attr.uuid,
 389                                         MEASUREMENT_INTERVAL_UUID) == 0) {
 390                 gatt_read_char(ch->t->attrib, desc->handle, 0,
 391                                                 valid_range_desc_cb, desc);
 392                 return;
 393         }
 394
 395 done:
 396         bt_uuid_to_string(&desc->uuid, uuidstr, MAX_LEN_UUID_STR);
 397         DBG("Ignored descriptor %s in characteristic %s", uuidstr,
 398                                                                 ch->attr.uuid);
 399 }
 400
 401 static void discover_desc_cb(guint8 status, const guint8 *pdu, guint16 len,
 402                                                         gpointer user_data)
 403 {
 404         struct characteristic *ch = user_data;
 405         struct att_data_list *list;
 406         uint8_t format;
 407         int i;
 408
 409         if (status != 0) {
 410                 error("Discover all characteristic descriptors failed [%s]: %s",
 411                                         ch->attr.uuid, att_ecode2str(status));
 412                 return;
 413         }
 414
 415         list = dec_find_info_resp(pdu, len, &format);
 416         if (list == NULL)
 417                 return;
 418
 419         for (i = 0; i < list->num; i++) {
 420                 struct descriptor *desc;
 421                 uint8_t *value;
 422
 423                 value = list->data[i];
 424                 desc = g_new0(struct descriptor, 1);
 425                 desc->handle = att_get_u16(value);
 426                 desc->ch = ch;
 427
 428                 if (format == 0x01)
 429                         desc->uuid = att_get_uuid16(&value[2]);
 430                 else
 431                         desc->uuid = att_get_uuid128(&value[2]);
 432
 433                 ch->desc = g_slist_append(ch->desc, desc);
 434                 process_thermometer_desc(desc);
 435         }
 436
 437         att_data_list_free(list);
 438 }
 439
 440 static void read_temp_type_cb(guint8 status, const guint8 *pdu, guint16 len,
 441                                                         gpointer user_data)
 442 {
 443         struct characteristic *ch = user_data;
 444         struct thermometer *t = ch->t;
 445         uint8_t value[ATT_MAX_MTU];
 446         int vlen;
 447
 448         if (status != 0) {
 449                 DBG("Temperature Type value read failed: %s",
 450                                                         att_ecode2str(status));
 451                 return;
 452         }
 453
 454         if (!dec_read_resp(pdu, len, value, &vlen)) {
 455                 DBG("Protocol error.");
 456                 return;
 457         }
 458
 459         if (vlen != 1) {
 460                 DBG("Invalid length for Temperature type");
 461                 return;
 462         }
 463
 464         t->has_type = TRUE;
 465         t->type = value[0];
 466 }
 467
 468 static void read_interval_cb(guint8 status, const guint8 *pdu, guint16 len,
 469                                                         gpointer user_data)
 470 {
 471         struct characteristic *ch = user_data;
 472         uint8_t value[ATT_MAX_MTU];
 473         uint16_t interval;
 474         int vlen;
 475
 476         if (status != 0) {
 477                 DBG("Measurement Interval value read failed: %s",
 478                                                         att_ecode2str(status));
 479                 return;
 480         }
 481
 482         if (!dec_read_resp(pdu, len, value, &vlen)) {
 483                 DBG("Protocol error\n");
 484                 return;
 485         }
 486
 487         if (vlen < 2) {
 488                 DBG("Invalid Interval received");
 489                 return;
 490         }
 491
 492         interval = att_get_u16(&value[0]);
 493         change_property(ch->t, "Interval", &interval);
 494 }
 495
 496 static void process_thermometer_char(struct characteristic *ch)
 497 {
 498         if (g_strcmp0(ch->attr.uuid, INTERMEDIATE_TEMPERATURE_UUID) == 0) {
 499                 gboolean intermediate = TRUE;
 500                 change_property(ch->t, "Intermediate", &intermediate);
 501                 return;
 502         } else if (g_strcmp0(ch->attr.uuid, TEMPERATURE_TYPE_UUID) == 0)
 503                 gatt_read_char(ch->t->attrib, ch->attr.value_handle, 0,
 504                                                         read_temp_type_cb, ch);
 505         else if (g_strcmp0(ch->attr.uuid, MEASUREMENT_INTERVAL_UUID) == 0)
 506                 gatt_read_char(ch->t->attrib, ch->attr.value_handle, 0,
 507                                                         read_interval_cb, ch);
 508 }
 509
 510 static void configure_thermometer_cb(GSList *characteristics, guint8 status,
 511                                                         gpointer user_data)
 512 {
 513         struct thermometer *t = user_data;
 514         GSList *l;
 515
 516         if (status != 0) {
 517                 error("Discover thermometer characteristics: %s",
 518                                                         att_ecode2str(status));
 519                 return;
 520         }
 521
 522         for (l = characteristics; l; l = l->next) {
 523                 struct gatt_char *c = l->data;
 524                 struct characteristic *ch;
 525                 uint16_t start, end;
 526
 527                 ch = g_new0(struct characteristic, 1);
 528                 ch->attr.handle = c->handle;
 529                 ch->attr.properties = c->properties;
 530                 ch->attr.value_handle = c->value_handle;
 531                 memcpy(ch->attr.uuid, c->uuid, MAX_LEN_UUID_STR + 1);
 532                 ch->t = t;
 533
 534                 t->chars = g_slist_append(t->chars, ch);
 535
 536                 process_thermometer_char(ch);
 537
 538                 start = c->value_handle + 1;
 539
 540                 if (l->next != NULL) {
 541                         struct gatt_char *c = l->next->data;
 542                         if (start == c->handle)
 543                                 continue;
 544                         end = c->handle - 1;
 545                 } else if (c->value_handle != t->svc_range->end)
 546                         end = t->svc_range->end;
 547                 else
 548                         continue;
 549
 550                 gatt_find_info(t->attrib, start, end, discover_desc_cb, ch);
 551         }
 552 }
 553
 554 static DBusMessage *get_properties(DBusConnection *conn, DBusMessage *msg,
 555                                                                 void *data)
 556 {
 557         struct thermometer *t = data;
 558         DBusMessageIter iter;
 559         DBusMessageIter dict;
 560         DBusMessage *reply;
 561
 562         reply = dbus_message_new_method_return(msg);
 563         if (reply == NULL)
 564                 return NULL;
 565
 566         dbus_message_iter_init_append(reply, &iter);
 567
 568         dbus_message_iter_open_container(&iter, DBUS_TYPE_ARRAY,
 569                         DBUS_DICT_ENTRY_BEGIN_CHAR_AS_STRING
 570                         DBUS_TYPE_STRING_AS_STRING DBUS_TYPE_VARIANT_AS_STRING
 571                         DBUS_DICT_ENTRY_END_CHAR_AS_STRING, &dict);
 572
 573         dict_append_entry(&dict, "Intermediate", DBUS_TYPE_BOOLEAN,
 574                                                         &t->intermediate);
 575
 576         if (t->has_interval) {
 577                 dict_append_entry(&dict, "Interval", DBUS_TYPE_UINT16,
 578                                                                 &t->interval);
 579                 dict_append_entry(&dict, "Maximum", DBUS_TYPE_UINT16, &t->max);
 580                 dict_append_entry(&dict, "Minimum", DBUS_TYPE_UINT16, &t->min);
 581         }
 582
 583         dbus_message_iter_close_container(&iter, &dict);
 584
 585         return reply;
 586 }
 587
 588 static void write_interval_cb (guint8 status, const guint8 *pdu, guint16 len,
 589                                                         gpointer user_data)
 590 {
 591         struct tmp_interval_data *data = user_data;
 592
 593         if (status != 0) {
 594                 error("Interval Write Request failed %s",
 595                                                         att_ecode2str(status));
 596                 goto done;
 597         }
 598
 599         if (!dec_write_resp(pdu, len)) {
 600                 error("Interval Write Request: protocol error");
 601                 goto done;
 602         }
 603
 604         change_property(data->thermometer, "Interval", &data->interval);
 605
 606 done:
 607         g_free(user_data);
 608 }
 609
 610 static DBusMessage *write_attr_interval(struct thermometer *t, DBusMessage *msg,
 611                                                                 uint16_t value)
 612 {
 613         struct tmp_interval_data *data;
 614         struct characteristic *ch;
 615         uint8_t atval[2];
 616
 617         if (t->attrib == NULL)
 618                 return btd_error_not_connected(msg);
 619
 620         ch = get_characteristic(t, MEASUREMENT_INTERVAL_UUID);
 621         if (ch == NULL)
 622                 return btd_error_not_available(msg);
 623
 624         if (value < t->min || value > t->max)
 625                 return btd_error_invalid_args(msg);
 626
 627         att_put_u16(value, &atval[0]);
 628
 629         data = g_new0(struct tmp_interval_data, 1);
 630         data->thermometer = t;
 631         data->interval = value;
 632         gatt_write_char(t->attrib, ch->attr.value_handle, atval, 2,
 633                                                 write_interval_cb, data);
 634
 635         return dbus_message_new_method_return(msg);
 636 }
 637
 638 static DBusMessage *set_property(DBusConnection *conn, DBusMessage *msg,
 639                                                                 void *data)
 640 {
 641         struct thermometer *t = data;
 642         const char *property;
 643         DBusMessageIter iter;
 644         DBusMessageIter sub;
 645         uint16_t value;
 646
 647         if (!dbus_message_iter_init(msg, &iter))
 648                 return btd_error_invalid_args(msg);
 649
 650         if (dbus_message_iter_get_arg_type(&iter) != DBUS_TYPE_STRING)
 651                 return btd_error_invalid_args(msg);
 652
 653         dbus_message_iter_get_basic(&iter, &property);
 654         if (g_strcmp0("Interval", property) != 0)
 655                 return btd_error_invalid_args(msg);
 656
 657         if (!t->has_interval)
 658                 return btd_error_not_available(msg);
 659
 660         dbus_message_iter_next(&iter);
 661         if (dbus_message_iter_get_arg_type(&iter) != DBUS_TYPE_VARIANT)
 662                 return btd_error_invalid_args(msg);
 663
 664         dbus_message_iter_recurse(&iter, &sub);
 665
 666         if (dbus_message_iter_get_arg_type(&sub) != DBUS_TYPE_UINT16)
 667                 return btd_error_invalid_args(msg);
 668
 669         dbus_message_iter_get_basic(&sub, &value);
 670
 671         return write_attr_interval(t, msg, value);
 672 }
 673
 674 static void enable_final_measurement(struct thermometer *t)
 675 {
 676         struct characteristic *ch;
 677         struct descriptor *desc;
 678         bt_uuid_t btuuid;
 679         uint8_t atval[2];
 680         char *msg;
 681
 682         if (t->attrib == NULL)
 683                 return;
 684
 685         ch = get_characteristic(t, TEMPERATURE_MEASUREMENT_UUID);
 686         if (ch == NULL) {
 687                 DBG("Temperature measurement characteristic not found");
 688                 return;
 689         }
 690
 691         bt_uuid16_create(&btuuid, GATT_CLIENT_CHARAC_CFG_UUID);
 692         desc = get_descriptor(ch, &btuuid);
 693         if (desc == NULL) {
 694                 DBG("Client characteristic configuration descriptor not found");
 695                 return;
 696         }
 697
 698         atval[0] = 0x02;
 699         atval[1] = 0x00;
 700         msg = g_strdup("Enable final measurement");
 701         gatt_write_char(t->attrib, desc->handle, atval, 2, measurement_cb, msg);
 702 }
 703
 704 static void enable_intermediate_measurement(struct thermometer *t)
 705 {
 706         struct characteristic *ch;
 707         struct descriptor *desc;
 708         bt_uuid_t btuuid;
 709         uint8_t atval[2];
 710         char *msg;
 711
 712         if (t->attrib == NULL)
 713                 return;
 714
 715         ch = get_characteristic(t, INTERMEDIATE_TEMPERATURE_UUID);
 716         if (ch == NULL) {
 717                 DBG("Intermediate measurement characteristic not found");
 718                 return;
 719         }
 720
 721         bt_uuid16_create(&btuuid, GATT_CLIENT_CHARAC_CFG_UUID);
 722         desc = get_descriptor(ch, &btuuid);
 723         if (desc == NULL) {
 724                 DBG("Client characteristic configuration descriptor not found");
 725                 return;
 726         }
 727
 728         atval[0] = 0x01;
 729         atval[1] = 0x00;
 730         msg = g_strdup("Enable intermediate measurement");
 731         gatt_write_char(t->attrib, desc->handle, atval, 2, measurement_cb, msg);
 732 }
 733
 734 static void disable_final_measurement(struct thermometer *t)
 735 {
 736         struct characteristic *ch;
 737         struct descriptor *desc;
 738         bt_uuid_t btuuid;
 739         uint8_t atval[2];
 740         char *msg;
 741
 742         if (t->attrib == NULL)
 743                 return;
 744
 745         ch = get_characteristic(t, TEMPERATURE_MEASUREMENT_UUID);
 746         if (ch == NULL) {
 747                 DBG("Temperature measurement characteristic not found");
 748                 return;
 749         }
 750
 751         bt_uuid16_create(&btuuid, GATT_CLIENT_CHARAC_CFG_UUID);
 752         desc = get_descriptor(ch, &btuuid);
 753         if (desc == NULL) {
 754                 DBG("Client characteristic configuration descriptor not found");
 755                 return;
 756         }
 757
 758         atval[0] = 0x00;
 759         atval[1] = 0x00;
 760         msg = g_strdup("Disable final measurement");
 761         gatt_write_char(t->attrib, desc->handle, atval, 2, measurement_cb, msg);
 762 }
 763
 764 static void disable_intermediate_measurement(struct thermometer *t)
 765 {
 766         struct characteristic *ch;
 767         struct descriptor *desc;
 768         bt_uuid_t btuuid;
 769         uint8_t atval[2];
 770         char *msg;
 771
 772         if (t->attrib == NULL)
 773                 return;
 774
 775         ch = get_characteristic(t, INTERMEDIATE_TEMPERATURE_UUID);
 776         if (ch == NULL) {
 777                 DBG("Intermediate measurement characteristic not found");
 778                 return;
 779         }
 780
 781         bt_uuid16_create(&btuuid, GATT_CLIENT_CHARAC_CFG_UUID);
 782         desc = get_descriptor(ch, &btuuid);
 783         if (desc == NULL) {
 784                 DBG("Client characteristic configuration descriptor not found");
 785                 return;
 786         }
 787
 788         atval[0] = 0x00;
 789         atval[1] = 0x00;
 790         msg = g_strdup("Disable intermediate measurement");
 791         gatt_write_char(t->attrib, desc->handle, atval, 2, measurement_cb, msg);
 792 }
 793
 794 static void remove_int_watcher(struct thermometer *t, struct watcher *w)
 795 {
 796         if (!g_slist_find(t->iwatchers, w))
 797                 return;
 798
 799         t->iwatchers = g_slist_remove(t->iwatchers, w);
 800
 801         if (g_slist_length(t->iwatchers) == 0)
 802                 disable_intermediate_measurement(t);
 803 }
 804
 805 static void watcher_exit(DBusConnection *conn, void *user_data)
 806 {
 807         struct watcher *watcher = user_data;
 808         struct thermometer *t = watcher->t;
 809
 810         DBG("Thermometer watcher %s disconnected", watcher->path);
 811
 812         remove_int_watcher(t, watcher);
 813
 814         t->fwatchers = g_slist_remove(t->fwatchers, watcher);
 815         g_dbus_remove_watch(watcher->t->conn, watcher->id);
 816
 817         if (g_slist_length(t->fwatchers) == 0)
 818                 disable_final_measurement(t);
 819 }
 820
 821 static struct watcher *find_watcher(GSList *list, const char *sender,
 822                                                         const char *path)
 823 {
 824         struct watcher *match;
 825         GSList *l;
 826
 827         match = g_new0(struct watcher, 1);
 828         match->srv = g_strdup(sender);
 829         match->path = g_strdup(path);
 830
 831         l = g_slist_find_custom(list, match, cmp_watcher);
 832         destroy_watcher(match);
 833
 834         if (l != NULL)
 835                 return l->data;
 836
 837         return NULL;
 838 }
 839
 840 static DBusMessage *register_watcher(DBusConnection *conn, DBusMessage *msg,
 841                                                                 void *data)
 842 {
 843         const char *sender = dbus_message_get_sender(msg);
 844         struct thermometer *t = data;
 845         struct watcher *watcher;
 846         char *path;
 847
 848         if (!dbus_message_get_args(msg, NULL, DBUS_TYPE_OBJECT_PATH, &path,
 849                                                         DBUS_TYPE_INVALID))
 850                 return btd_error_invalid_args(msg);
 851
 852         watcher = find_watcher(t->fwatchers, sender, path);
 853         if (watcher != NULL)
 854                 return btd_error_already_exists(msg);
 855
 856         DBG("Thermometer watcher %s registered", path);
 857
 858         watcher = g_new0(struct watcher, 1);
 859         watcher->srv = g_strdup(sender);
 860         watcher->path = g_strdup(path);
 861         watcher->t = t;
 862         watcher->id = g_dbus_add_disconnect_watch(conn, sender, watcher_exit,
 863                                                 watcher, destroy_watcher);
 864
 865         if (g_slist_length(t->fwatchers) == 0)
 866                 enable_final_measurement(t);
 867
 868         t->fwatchers = g_slist_prepend(t->fwatchers, watcher);
 869
 870         return dbus_message_new_method_return(msg);
 871 }
 872
 873 static DBusMessage *unregister_watcher(DBusConnection *conn, DBusMessage *msg,
 874                                                                 void *data)
 875 {
 876         const char *sender = dbus_message_get_sender(msg);
 877         struct thermometer *t = data;
 878         struct watcher *watcher;
 879         char *path;
 880
 881         if (!dbus_message_get_args(msg, NULL, DBUS_TYPE_OBJECT_PATH, &path,
 882                                                         DBUS_TYPE_INVALID))
 883                 return btd_error_invalid_args(msg);
 884
 885         watcher = find_watcher(t->fwatchers, sender, path);
 886         if (watcher == NULL)
 887                 return btd_error_does_not_exist(msg);
 888
 889         DBG("Thermometer watcher %s unregistered", path);
 890
 891         remove_int_watcher(t, watcher);
 892
 893         t->fwatchers = g_slist_remove(t->fwatchers, watcher);
 894         g_dbus_remove_watch(watcher->t->conn, watcher->id);
 895
 896         if (g_slist_length(t->fwatchers) == 0)
 897                 disable_final_measurement(t);
 898
 899         return dbus_message_new_method_return(msg);
 900 }
 901
 902 static DBusMessage *enable_intermediate(DBusConnection *conn, DBusMessage *msg,
 903                                                                 void *data)
 904 {
 905         const char *sender = dbus_message_get_sender(msg);
 906         struct thermometer *t = data;
 907         struct watcher *watcher;
 908         char *path;
 909
 910         if (!t->intermediate)
 911                 return btd_error_not_supported(msg);
 912
 913         if (!dbus_message_get_args(msg, NULL, DBUS_TYPE_OBJECT_PATH, &path,
 914                                                         DBUS_TYPE_INVALID))
 915                 return btd_error_invalid_args(msg);
 916
 917         watcher = find_watcher(t->fwatchers, sender, path);
 918         if (watcher == NULL)
 919                 return btd_error_does_not_exist(msg);
 920
 921         if (find_watcher(t->iwatchers, sender, path))
 922                 return btd_error_already_exists(msg);
 923
 924         DBG("Intermediate measurement watcher %s registered", path);
 925
 926         if (g_slist_length(t->iwatchers) == 0)
 927                 enable_intermediate_measurement(t);
 928
 929         t->iwatchers = g_slist_prepend(t->iwatchers, watcher);
 930
 931         return dbus_message_new_method_return(msg);
 932 }
 933
 934 static DBusMessage *disable_intermediate(DBusConnection *conn, DBusMessage *msg,
 935                                                                 void *data)
 936 {
 937         const char *sender = dbus_message_get_sender(msg);
 938         struct thermometer *t = data;
 939         struct watcher *watcher;
 940         char *path;
 941
 942         if (!dbus_message_get_args(msg, NULL, DBUS_TYPE_OBJECT_PATH, &path,
 943                                                         DBUS_TYPE_INVALID))
 944                 return btd_error_invalid_args(msg);
 945
 946         watcher = find_watcher(t->iwatchers, sender, path);
 947         if (watcher == NULL)
 948                 return btd_error_does_not_exist(msg);
 949
 950         DBG("Intermediate measurement %s unregistered", path);
 951
 952         remove_int_watcher(t, watcher);
 953
 954         return dbus_message_new_method_return(msg);
 955 }
 956
 957 static const GDBusMethodTable thermometer_methods[] = {
 958         { GDBUS_METHOD("GetProperties",
 959                         NULL, GDBUS_ARGS({ "properties", "a{sv}" }),
 960                         get_properties) },
 961         { GDBUS_ASYNC_METHOD("SetProperty",
 962                         GDBUS_ARGS({ "name", "s" }, { "value", "v" }), NULL,
 963                         set_property) },
 964         { GDBUS_METHOD("RegisterWatcher",
 965                         GDBUS_ARGS({ "agent", "o" }), NULL,
 966                         register_watcher) },
 967         { GDBUS_METHOD("UnregisterWatcher",
 968                         GDBUS_ARGS({ "agent", "o" }), NULL,
 969                         unregister_watcher) },
 970         { GDBUS_METHOD("EnableIntermediateMeasurement",
 971                         GDBUS_ARGS({ "agent", "o" }), NULL,
 972                         enable_intermediate) },
 973         { GDBUS_METHOD("DisableIntermediateMeasurement",
 974                         GDBUS_ARGS({ "agent", "o" }), NULL,
 975                         disable_intermediate) },
 976         { }
 977 };
 978
 979 static const GDBusSignalTable thermometer_signals[] = {
 980         { GDBUS_SIGNAL("PropertyChanged",
 981                         GDBUS_ARGS({ "name", "s" }, { "value", "v" })) },
 982         { }
 983 };
 984
 985 static void update_watcher(gpointer data, gpointer user_data)
 986 {
 987         struct watcher *w = data;
 988         struct measurement *m = user_data;
 989         DBusConnection *conn = w->t->conn;
 990         DBusMessageIter iter;
 991         DBusMessageIter dict;
 992         DBusMessage *msg;
 993
 994         msg = dbus_message_new_method_call(w->srv, w->path,
 995                                 "org.bluez.ThermometerWatcher",
 996                                 "MeasurementReceived");
 997         if (msg == NULL)
 998                 return;
 999
1000         dbus_message_iter_init_append(msg, &iter);
1001
1002         dbus_message_iter_open_container(&iter, DBUS_TYPE_ARRAY,
1003                         DBUS_DICT_ENTRY_BEGIN_CHAR_AS_STRING
1004                         DBUS_TYPE_STRING_AS_STRING DBUS_TYPE_VARIANT_AS_STRING
1005                         DBUS_DICT_ENTRY_END_CHAR_AS_STRING, &dict);
1006
1007         dict_append_entry(&dict, "Exponent", DBUS_TYPE_INT16, &m->exp);
1008         dict_append_entry(&dict, "Mantissa", DBUS_TYPE_INT32, &m->mant);
1009         dict_append_entry(&dict, "Unit", DBUS_TYPE_STRING, &m->unit);
1010
1011         if (m->suptime)
1012                 dict_append_entry(&dict, "Time", DBUS_TYPE_UINT64, &m->time);
1013
1014         dict_append_entry(&dict, "Type", DBUS_TYPE_STRING, &m->type);
1015         dict_append_entry(&dict, "Measurement", DBUS_TYPE_STRING, &m->value);
1016
1017         dbus_message_iter_close_container(&iter, &dict);
1018
1019         dbus_message_set_no_reply(msg, TRUE);
1020         g_dbus_send_message(conn, msg);
1021 }
1022
1023 static void recv_measurement(struct thermometer *t, struct measurement *m)
1024 {
1025         GSList *wlist;
1026
1027         if (g_strcmp0(m->value, "Intermediate") == 0)
1028                 wlist = t->iwatchers;
1029         else
1030                 wlist = t->fwatchers;
1031
1032         g_slist_foreach(wlist, update_watcher, m);
1033 }
1034
1035 static void proc_measurement(struct thermometer *t, const uint8_t *pdu,
1036                                                 uint16_t len, gboolean final)
1037 {
1038         struct measurement m;
1039         const char *type;
1040         uint8_t flags;
1041         uint32_t raw;
1042
1043         if (len < 4) {
1044                 DBG("Mandatory flags are not provided");
1045                 return;
1046         }
1047
1048         flags = pdu[3];
1049         if (flags & TEMP_UNITS)
1050                 m.unit = "Fahrenheit";
1051         else
1052                 m.unit = "Celsius";
1053
1054         if (len < 8) {
1055                 DBG("Temperature measurement value is not provided");
1056                 return;
1057         }
1058
1059         raw = att_get_u32(&pdu[4]);
1060         m.mant = raw & 0x00FFFFFF;
1061         m.exp = ((int32_t) raw) >> 24;
1062
1063         if (m.mant & 0x00800000) {
1064                 /* convert to C2 negative value */
1065                 m.mant = m.mant - FLOAT_MAX_MANTISSA;
1066         }
1067
1068         if (flags & TEMP_TIME_STAMP) {
1069                 struct tm ts;
1070                 time_t time;
1071
1072                 if (len < 15) {
1073                         DBG("Can't get time stamp value");
1074                         return;
1075                 }
1076
1077                 ts.tm_year = att_get_u16(&pdu[8]) - 1900;
1078                 ts.tm_mon = pdu[10] - 1;
1079                 ts.tm_mday = pdu[11];
1080                 ts.tm_hour = pdu[12];
1081                 ts.tm_min = pdu[13];
1082                 ts.tm_sec = pdu[14];
1083                 ts.tm_isdst = -1;
1084
1085                 time = mktime(&ts);
1086                 m.time = (uint64_t) time;
1087                 m.suptime = TRUE;
1088         } else
1089                 m.suptime = FALSE;
1090
1091         if (flags & TEMP_TYPE) {
1092                 uint8_t index;
1093
1094                 if (m.suptime && len >= 16)
1095                         index = 15;
1096                 else if (!m.suptime && len >= 9)
1097                         index = 9;
1098                 else {
1099                         DBG("Can't get temperature type");
1100                         return;
1101                 }
1102
1103                 type = temptype2str(pdu[index]);
1104         } else if (t->has_type)
1105                 type = temptype2str(t->type);
1106         else
1107                 type = NULL;
1108
1109         m.type = type ? g_strdup(type) : NULL;
1110         m.value = final ? "Final" : "Intermediate";
1111
1112         recv_measurement(t, &m);
1113         g_free(m.type);
1114 }
1115
1116 static void proc_measurement_interval(struct thermometer *t, const uint8_t *pdu,
1117                                                                 uint16_t len)
1118 {
1119         uint16_t interval;
1120
1121         if (len < 5) {
1122                 DBG("Measurement interval value is not provided");
1123                 return;
1124         }
1125
1126         interval = att_get_u16(&pdu[3]);
1127
1128         change_property(t, "Interval", &interval);
1129 }
1130
1131 static void ind_handler(const uint8_t *pdu, uint16_t len, gpointer user_data)
1132 {
1133         struct thermometer *t = user_data;
1134         const struct characteristic *ch;
1135         uint8_t opdu[ATT_MAX_MTU];
1136         uint16_t handle, olen;
1137         GSList *l;
1138
1139         if (len < 3) {
1140                 DBG("Bad pdu received");
1141                 return;
1142         }
1143
1144         handle = att_get_u16(&pdu[1]);
1145         l = g_slist_find_custom(t->chars, &handle, cmp_char_val_handle);
1146         if (l == NULL) {
1147                 DBG("Unexpected handle: 0x%04x", handle);
1148                 return;
1149         }
1150
1151         ch = l->data;
1152
1153         if (g_strcmp0(ch->attr.uuid, TEMPERATURE_MEASUREMENT_UUID) == 0)
1154                 proc_measurement(t, pdu, len, TRUE);
1155         else if (g_strcmp0(ch->attr.uuid, MEASUREMENT_INTERVAL_UUID) == 0)
1156                 proc_measurement_interval(t, pdu, len);
1157
1158         olen = enc_confirmation(opdu, sizeof(opdu));
1159
1160         if (olen > 0)
1161                 g_attrib_send(t->attrib, 0, opdu[0], opdu, olen, NULL, NULL,
1162                                                                         NULL);
1163 }
1164
1165 static void notif_handler(const uint8_t *pdu, uint16_t len, gpointer user_data)
1166 {
1167         struct thermometer *t = user_data;
1168         const struct characteristic *ch;
1169         uint16_t handle;
1170         GSList *l;
1171
1172         if (len < 3) {
1173                 DBG("Bad pdu received");
1174                 return;
1175         }
1176
1177         handle = att_get_u16(&pdu[1]);
1178         l = g_slist_find_custom(t->chars, &handle, cmp_char_val_handle);
1179         if (l == NULL) {
1180                 DBG("Unexpected handle: 0x%04x", handle);
1181                 return;
1182         }
1183
1184         ch = l->data;
1185         if (g_strcmp0(ch->attr.uuid, INTERMEDIATE_TEMPERATURE_UUID) == 0)
1186                 proc_measurement(t, pdu, len, FALSE);
1187 }
1188
1189 static void attio_connected_cb(GAttrib *attrib, gpointer user_data)
1190 {
1191         struct thermometer *t = user_data;
1192
1193         t->attrib = g_attrib_ref(attrib);
1194
1195         t->attindid = g_attrib_register(t->attrib, ATT_OP_HANDLE_IND,
1196                                                         ind_handler, t, NULL);
1197         t->attnotid = g_attrib_register(t->attrib, ATT_OP_HANDLE_NOTIFY,
1198                                                         notif_handler, t, NULL);
1199         gatt_discover_char(t->attrib, t->svc_range->start, t->svc_range->end,
1200                                         NULL, configure_thermometer_cb, t);
1201 }
1202
1203 static void attio_disconnected_cb(gpointer user_data)
1204 {
1205         struct thermometer *t = user_data;
1206
1207         DBG("GATT Disconnected");
1208
1209         if (t->attindid > 0) {
1210                 g_attrib_unregister(t->attrib, t->attindid);
1211                 t->attindid = 0;
1212         }
1213
1214         if (t->attnotid > 0) {
1215                 g_attrib_unregister(t->attrib, t->attnotid);
1216                 t->attnotid = 0;
1217         }
1218
1219         g_attrib_unref(t->attrib);
1220         t->attrib = NULL;
1221 }
1222
1223 int thermometer_register(DBusConnection *connection, struct btd_device *device,
1224                                                 struct gatt_primary *tattr)
1225 {
1226         const gchar *path = device_get_path(device);
1227         struct thermometer *t;
1228
1229         t = g_new0(struct thermometer, 1);
1230         t->conn = dbus_connection_ref(connection);
1231         t->dev = btd_device_ref(device);
1232         t->svc_range = g_new0(struct att_range, 1);
1233         t->svc_range->start = tattr->range.start;
1234         t->svc_range->end = tattr->range.end;
1235
1236         if (!g_dbus_register_interface(t->conn, path, THERMOMETER_INTERFACE,
1237                                 thermometer_methods, thermometer_signals,
1238                                 NULL, t, destroy_thermometer)) {
1239                 error("D-Bus failed to register %s interface",
1240                                                         THERMOMETER_INTERFACE);
1241                 destroy_thermometer(t);
1242                 return -EIO;
1243         }
1244
1245         thermometers = g_slist_prepend(thermometers, t);
1246
1247         t->attioid = btd_device_add_attio_callback(device, attio_connected_cb,
1248                                                 attio_disconnected_cb, t);
1249         return 0;
1250 }
1251
1252 void thermometer_unregister(struct btd_device *device)
1253 {
1254         struct thermometer *t;
1255         GSList *l;
1256
1257         l = g_slist_find_custom(thermometers, device, cmp_device);
1258         if (l == NULL)
1259                 return;
1260
1261         t = l->data;
1262         thermometers = g_slist_remove(thermometers, t);
1263         g_dbus_unregister_interface(t->conn, device_get_path(t->dev),
1264                                                         THERMOMETER_INTERFACE);
1265 }