Bluetooth LE: Add connection update functionality.

Implemented for BlueZ only.

Change-Id: I358a98bbc7499d5ce5437fb0d4672fde46c3b831
Reviewed-by: Alex Blasche <alexander.blasche@theqtcompany.com>

3 files changed, 153 insertions, 0 deletions

diff --git a/src/bluetooth/bluez/bluez_data_p.h b/src/bluetooth/bluez/bluez_data_p.h
index 7db0e50b..aa0d0166 100644
--- a/src/bluetooth/bluez/bluez_data_p.h
+++ b/src/bluetooth/bluez/bluez_data_p.h
@@ -348,6 +348,7 @@ enum OpCodeCommandField {
     OcfLeSetAdvEnable = 0xa,
     OcfLeClearWhiteList = 0x10,
     OcfLeAddToWhiteList = 0x11,
+    OcfLeConnectionUpdate = 0x13,
 };
 
 /* Command opcode pack/unpack */
diff --git a/src/bluetooth/bluez/hcimanager.cpp b/src/bluetooth/bluez/hcimanager.cpp
index 388f3e0c..3ff03fd4 100644
--- a/src/bluetooth/bluez/hcimanager.cpp
+++ b/src/bluetooth/bluez/hcimanager.cpp
@@ -35,6 +35,7 @@
 #include "hcimanager_p.h"
 
 #include "qbluetoothsocket_p.h"
+#include "qlowenergyconnectionparameters.h"
 
 #include <QtCore/qloggingcategory.h>
 
@@ -258,6 +259,108 @@ QBluetoothAddress HciManager::addressForConnectionHandle(quint16 handle) const
     return QBluetoothAddress();
 }
 
+quint16 forceIntervalIntoRange(double connectionInterval)
+{
+    return qMin<double>(qMax<double>(7.5, connectionInterval), 4000) / 1.25;
+}
+
+struct ConnectionUpdateData {
+    quint16 minInterval;
+    quint16 maxInterval;
+    quint16 slaveLatency;
+    quint16 timeout;
+};
+ConnectionUpdateData connectionUpdateData(const QLowEnergyConnectionParameters &params)
+{
+    ConnectionUpdateData data;
+    const quint16 minInterval = forceIntervalIntoRange(params.minimumInterval());
+    const quint16 maxInterval = forceIntervalIntoRange(params.maximumInterval());
+    data.minInterval = qToLittleEndian(minInterval);
+    data.maxInterval = qToLittleEndian(maxInterval);
+    const quint16 latency = qMax<quint16>(0, qMin<quint16>(params.latency(), 499));
+    data.slaveLatency = qToLittleEndian(latency);
+    const quint16 timeout
+            = qMax<quint16>(100, qMin<quint16>(32000, params.supervisionTimeout())) / 10;
+    data.timeout = qToLittleEndian(timeout);
+    return data;
+}
+
+bool HciManager::sendConnectionUpdateCommand(quint16 handle,
+                                             const QLowEnergyConnectionParameters &params)
+{
+    struct CommandParams {
+        quint16 handle;
+        ConnectionUpdateData data;
+        quint16 minCeLength;
+        quint16 maxCeLength;
+    } commandParams;
+    commandParams.handle = qToLittleEndian(handle);
+    commandParams.data = connectionUpdateData(params);
+    commandParams.minCeLength = 0;
+    commandParams.maxCeLength = qToLittleEndian(quint16(0xffff));
+    const QByteArray data = QByteArray::fromRawData(reinterpret_cast<char *>(&commandParams),
+                                                    sizeof commandParams);
+    return sendCommand(OgfLinkControl, OcfLeConnectionUpdate, data);
+}
+
+bool HciManager::sendConnectionParameterUpdateRequest(quint16 handle,
+                                                      const QLowEnergyConnectionParameters &params)
+{
+    ConnectionUpdateData connUpdateData = connectionUpdateData(params);
+
+    // Vol 3, part A, 4
+    struct SignalingPacket {
+        quint8 code;
+        quint8 identifier;
+        quint16 length;
+    } signalingPacket;
+    signalingPacket.code = 0x12;
+    signalingPacket.identifier = ++sigPacketIdentifier;
+    const quint16 sigPacketLen = sizeof connUpdateData;
+    signalingPacket.length = qToLittleEndian(sigPacketLen);
+
+    struct L2CapHeader {
+        quint16 length;
+        quint16 channelId;
+    } l2CapHeader;
+    const quint16 l2CapHeaderLen = sizeof signalingPacket + sigPacketLen;
+    l2CapHeader.length = qToLittleEndian(l2CapHeaderLen);
+    l2CapHeader.channelId = qToLittleEndian(quint16(5));
+
+    // Vol 2, part E, 5.4.2
+    struct AclData {
+        quint16 handle: 12;
+        quint16 pbFlag: 2;
+        quint16 bcFlag: 2;
+        quint16 dataLen;
+    } aclData;
+    aclData.handle = qToLittleEndian(handle); // Works because the next two values are zero.
+    aclData.pbFlag = 0;
+    aclData.bcFlag = 0;
+    aclData.dataLen = qToLittleEndian(quint16(sizeof l2CapHeader + l2CapHeaderLen));
+
+    struct iovec iv[5];
+    quint8 packetType = 2;
+    iv[0].iov_base = &packetType;
+    iv[0].iov_len  = 1;
+    iv[1].iov_base = &aclData;
+    iv[1].iov_len  = sizeof aclData;
+    iv[2].iov_base = &l2CapHeader;
+    iv[2].iov_len = sizeof l2CapHeader;
+    iv[3].iov_base = &signalingPacket;
+    iv[3].iov_len = sizeof signalingPacket;
+    iv[4].iov_base = &connUpdateData;
+    iv[4].iov_len = sizeof connUpdateData;
+    while (writev(hciSocket, iv, sizeof iv / sizeof *iv) < 0) {
+        if (errno == EAGAIN || errno == EINTR)
+            continue;
+        qCDebug(QT_BT_BLUEZ()) << "failure writing HCI ACL packet:" << strerror(errno);
+        return false;
+    }
+    qCDebug(QT_BT_BLUEZ) << "Connection Update Request packet sent successfully";
+    return true;
+}
+
 /*!
  * Process all incoming HCI events. Function cannot process anything else but events.
  */
@@ -319,10 +422,47 @@ void HciManager::_q_readNotify()
         emit commandCompleted(event->opcode, status, additionalData);
     }
         break;
+    case LeMetaEvent:
+        handleLeMetaEvent(data);
+        break;
     default:
         break;
     }
 }
 
+void HciManager::handleLeMetaEvent(const quint8 *data)
+{
+    // Spec v4.2, Vol 2, part E, 7.7.65ff
+    switch (*data) {
+    case 0x1: {
+        const quint16 handle = bt_get_le16(data + 2);
+        emit connectionComplete(handle);
+        break;
+    }
+    case 0x3: {
+        // TODO: From little endian!
+        struct ConnectionUpdateData {
+            quint8 status;
+            quint16 handle;
+            quint16 interval;
+            quint16 latency;
+            quint16 timeout;
+        } __attribute((packed));
+        const auto * const updateData
+                = reinterpret_cast<const ConnectionUpdateData *>(data + 1);
+        if (updateData->status == 0) {
+            QLowEnergyConnectionParameters params;
+            const double interval = qFromLittleEndian(updateData->interval) * 1.25;
+            params.setIntervalRange(interval, interval);
+            params.setLatency(qFromLittleEndian(updateData->latency));
+            params.setSupervisionTimeout(qFromLittleEndian(updateData->timeout) * 10);
+            emit connectionUpdate(qFromLittleEndian(updateData->handle), params);
+        }
+        break;
+    }
+    default:
+        break;
+    }
+}
 
 QT_END_NAMESPACE
diff --git a/src/bluetooth/bluez/hcimanager_p.h b/src/bluetooth/bluez/hcimanager_p.h
index c8f2fe56..f1bd6d46 100644
--- a/src/bluetooth/bluez/hcimanager_p.h
+++ b/src/bluetooth/bluez/hcimanager_p.h
@@ -53,6 +53,8 @@
 
 QT_BEGIN_NAMESPACE
 
+class QLowEnergyConnectionParameters;
+
 class HciManager : public QObject
 {
     Q_OBJECT
@@ -60,6 +62,7 @@ public:
     enum HciEvent {
         EncryptChangeEvent = EVT_ENCRYPT_CHANGE,
         CommandCompleteEvent = EVT_CMD_COMPLETE,
+        LeMetaEvent = 0x3e,
     };
 
     explicit HciManager(const QBluetoothAddress &deviceAdapter, QObject *parent = 0);
@@ -68,21 +71,30 @@ public:
     bool isValid() const;
     bool monitorEvent(HciManager::HciEvent event);
     bool sendCommand(OpCodeGroupField ogf, OpCodeCommandField ocf, const QByteArray &parameters);
+
     void stopEvents();
     QBluetoothAddress addressForConnectionHandle(quint16 handle) const;
 
+    bool sendConnectionUpdateCommand(quint16 handle, const QLowEnergyConnectionParameters &params);
+    bool sendConnectionParameterUpdateRequest(quint16 handle,
+                                              const QLowEnergyConnectionParameters &params);
+
 signals:
     void encryptionChangedEvent(const QBluetoothAddress &address, bool wasSuccess);
     void commandCompleted(quint16 opCode, quint8 status, const QByteArray &data);
+    void connectionComplete(quint16 handle);
+    void connectionUpdate(quint16 handle, const QLowEnergyConnectionParameters &parameters);
 
 private slots:
     void _q_readNotify();
 
 private:
     int hciForAddress(const QBluetoothAddress &deviceAdapter);
+    void handleLeMetaEvent(const quint8 *data);
 
     int hciSocket;
     int hciDev;
+    quint8 sigPacketIdentifier = 0;
     QSocketNotifier *notifier;
     QSet<HciManager::HciEvent> runningEvents;
 };