cyclingspeed: Add support for Request Supported Sensor Locations

diff --git a/profiles/cyclingspeed/cyclingspeed.c b/profiles/cyclingspeed/cyclingspeed.c
index 866fac8..8283894 100644 (file)
--- a/profiles/cyclingspeed/cyclingspeed.c
+++ b/profiles/cyclingspeed/cyclingspeed.c
@@ -102,6 +102,8 @@ struct csc {
        uint16_t                feature;
        gboolean                has_location;
        uint8_t                 location;
+       uint8_t                 num_locations;
+       uint8_t                 *locations;
 
        struct controlpoint_req *pending_req;
 };
@@ -251,6 +253,7 @@ static void destroy_csc(gpointer user_data)
 
        btd_device_unref(csc->dev);
        g_free(csc->svc_range);
+       g_free(csc->locations);
        g_free(csc);
 }
 
@@ -275,7 +278,6 @@ static gboolean controlpoint_timeout(gpointer user_data)
        return FALSE;
 }
 
-__attribute__((unused)) /* TODO: remove once controlpoint ops are implemented */
 static void controlpoint_write_cb(guint8 status, const guint8 *pdu, guint16 len,
                                                        gpointer user_data)
 {
@@ -294,6 +296,20 @@ static void controlpoint_write_cb(guint8 status, const guint8 *pdu, guint16 len,
                                                                        req);
 }
 
+static void read_supported_locations(struct csc *csc)
+{
+       struct controlpoint_req *req;
+
+       req = g_new0(struct controlpoint_req, 1);
+       req->csc = csc;
+       req->opcode = REQUEST_SUPPORTED_SENSOR_LOC;
+
+       csc->pending_req = req;
+
+       gatt_write_char(csc->attrib, csc->controlpoint_val_handle, &req->opcode,
+                       sizeof(req->opcode), controlpoint_write_cb, req);
+}
+
 static void read_feature_cb(guint8 status, const guint8 *pdu,
                                                guint16 len, gpointer user_data)
 {
@@ -318,6 +334,10 @@ static void read_feature_cb(guint8 status, const guint8 *pdu,
        }
 
        csc->feature = att_get_u16(value);
+
+       if ((csc->feature & MULTI_SENSOR_LOC_SUPPORT)
+                                               && (csc->locations == NULL))
+               read_supported_locations(csc);
 }
 
 static void read_location_cb(guint8 status, const guint8 *pdu,
@@ -551,6 +571,7 @@ static void controlpoint_ind_handler(const uint8_t *pdu, uint16_t len,
        struct controlpoint_req *req = csc->pending_req;
        uint8_t opcode;
        uint8_t req_opcode;
+       uint8_t rsp_code;
        uint8_t *opdu;
        uint16_t olen;
        size_t plen;
@@ -584,7 +605,7 @@ static void controlpoint_ind_handler(const uint8_t *pdu, uint16_t len,
        }
 
        req_opcode = *pdu;
-       /* skip response code for now */
+       rsp_code = *(pdu + 1);
        pdu += 2;
        len -= 2;
 
@@ -593,7 +614,16 @@ static void controlpoint_ind_handler(const uint8_t *pdu, uint16_t len,
                goto done;
        }
 
-       /* TODO: handle response */
+       switch (req->opcode) {
+       case REQUEST_SUPPORTED_SENSOR_LOC:
+               if (rsp_code == RSP_SUCCESS) {
+                       csc->num_locations = len;
+                       csc->locations = g_memdup(pdu, len);
+               } else {
+                       error("Failed to read Supported Sendor Locations");
+               }
+               break;
+       }
 
        csc->pending_req = NULL;
        g_source_remove(req->timeout);
@@ -868,6 +898,29 @@ static gboolean property_exists_location(const GDBusPropertyTable *property,
        return csc->has_location;
 }
 
+static gboolean property_get_locations(const GDBusPropertyTable *property,
+                                       DBusMessageIter *iter, void *data)
+{
+       struct csc *csc = data;
+       DBusMessageIter entry;
+       int i;
+
+       if (!(csc->feature & MULTI_SENSOR_LOC_SUPPORT))
+               return FALSE;
+
+       dbus_message_iter_open_container(iter, DBUS_TYPE_ARRAY,
+                                       DBUS_TYPE_STRING_AS_STRING, &entry);
+       for (i = 0; i < csc->num_locations; i++) {
+               char *loc = g_strdup(location2str(csc->locations[i]));
+               dbus_message_iter_append_basic(&entry, DBUS_TYPE_STRING, &loc);
+               g_free(loc);
+       }
+
+       dbus_message_iter_close_container(iter, &entry);
+
+       return TRUE;
+}
+
 static gboolean property_exists_locations(const GDBusPropertyTable *property,
                                                                void *data)
 {
@@ -903,7 +956,7 @@ static gboolean property_get_multi_loc_sup(const GDBusPropertyTable *property,
 static const GDBusPropertyTable cyclingspeed_device_properties[] = {
        { "Location", "s", property_get_location, NULL,
                                                property_exists_location },
-       { "SupportedLocations", "as", NULL, NULL,
+       { "SupportedLocations", "as", property_get_locations, NULL,
                                                property_exists_locations },
        { "WheelRevolutionDataSupported", "b", property_get_wheel_rev_sup },
        { "MultipleLocationsSupported", "b", property_get_multi_loc_sup },