Bluetooth LE: Add support for Signed Write command.

- This is how we get at the signature resolving key:
    1) On connection from a client, we read the key from the
       respective BlueZ settings file (BlueZ 5 only, as I did
       not manage to find out where BlueZ 4 keeps this information).
    2) Also monitor the HCI traffic for key updates (due to re-pairing).
- While there is an autotest for the actual hashing procedure, the
  overall feature cannot be easily tested for various reasons (there is
  no signed write support in our client API, for one). However, to help
  with manual testing, the server part of our autotest now exposes a
  characteristic that supports signed writes.
- This feature requires a Linux kernel >= 3.7.

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

9 files changed, 514 insertions, 29 deletions

diff --git a/src/bluetooth/bluetooth.pro b/src/bluetooth/bluetooth.pro
index ef5337b5..99c9fe07 100644
--- a/src/bluetooth/bluetooth.pro
+++ b/src/bluetooth/bluetooth.pro
@@ -1,5 +1,5 @@
 TARGET = QtBluetooth
-QT = core
+QT = core core-private
 QT_PRIVATE = concurrent
 
 
@@ -51,6 +51,7 @@ PRIVATE_HEADERS += \
     qlowenergycontroller_p.h \
     qlowenergyserviceprivate_p.h \
     qleadvertiser_p.h \
+    lecmacverifier_p.h
 
 SOURCES += \
     qbluetoothaddress.cpp\
@@ -102,7 +103,10 @@ config_bluez:qtHaveModule(dbus) {
     config_bluez_le {
         SOURCES +=  \
             qleadvertiser_bluez.cpp \
-            qlowenergycontroller_bluez.cpp
+            qlowenergycontroller_bluez.cpp \
+            lecmacverifier.cpp
+        config_linux_crypto_api:DEFINES += CONFIG_LINUX_CRYPTO_API
+        else:message("Linux crypto API not present, signed writes will not work.")
     } else {
         message("Bluez version is too old to support Bluetooth Low Energy.")
         message("Only classic Bluetooth will be available.")
diff --git a/src/bluetooth/bluez/bluez_data_p.h b/src/bluetooth/bluez/bluez_data_p.h
index e8d1ec62..8c2dc43e 100644
--- a/src/bluetooth/bluez/bluez_data_p.h
+++ b/src/bluetooth/bluez/bluez_data_p.h
@@ -58,6 +58,10 @@
 
 QT_BEGIN_NAMESPACE
 
+#define ATTRIBUTE_CHANNEL_ID 4
+#define SIGNALING_CHANNEL_ID 5
+#define SECURITY_CHANNEL_ID 6
+
 #define BTPROTO_L2CAP   0
 #define BTPROTO_HCI     1
 #define BTPROTO_RFCOMM  3
@@ -173,9 +177,14 @@ static inline void ntoh128(const quint128 *src, quint128 *dst)
 #error "Unknown byte order"
 #endif
 
+template<typename T> inline T getBtData(const void *ptr)
+{
+    return qFromLittleEndian<T>(reinterpret_cast<const uchar *>(ptr));
+}
+
 static inline quint16 bt_get_le16(const void *ptr)
 {
-    return qFromLittleEndian<quint16>(reinterpret_cast<const uchar *>(ptr));
+    return getBtData<quint16>(ptr);
 }
 
 template<typename T> inline void putBtData(T src, void *dst)
@@ -200,6 +209,7 @@ template<> inline void putBtData(quint128 src, void *dst)
 
 // HCI packet types
 #define HCI_COMMAND_PKT 0x01
+#define HCI_ACL_PKT     0x02
 #define HCI_EVENT_PKT   0x04
 #define HCI_VENDOR_PKT  0xff
 
@@ -337,6 +347,18 @@ struct evt_cmd_complete {
     quint16 opcode;
 } __attribute__ ((packed));
 
+struct AclData {
+    quint16 handle: 12;
+    quint16 pbFlag: 2;
+    quint16 bcFlag: 2;
+    quint16 dataLen;
+};
+
+struct L2CapHeader {
+    quint16 length;
+    quint16 channelId;
+};
+
 struct hci_command_hdr {
     quint16 opcode;         /* OCF & OGF */
     quint8 plen;
diff --git a/src/bluetooth/bluez/hcimanager.cpp b/src/bluetooth/bluez/hcimanager.cpp
index 123a16ab..35ce2184 100644
--- a/src/bluetooth/bluez/hcimanager.cpp
+++ b/src/bluetooth/bluez/hcimanager.cpp
@@ -183,6 +183,29 @@ bool HciManager::monitorEvent(HciManager::HciEvent event)
     return true;
 }
 
+bool HciManager::monitorAclPackets()
+{
+    if (!isValid())
+        return false;
+
+    hci_filter filter;
+    socklen_t length = sizeof(hci_filter);
+    if (getsockopt(hciSocket, SOL_HCI, HCI_FILTER, &filter, &length) < 0) {
+        qCWarning(QT_BT_BLUEZ) << "Cannot retrieve HCI filter settings";
+        return false;
+    }
+
+    hci_filter_set_ptype(HCI_ACL_PKT, &filter);
+    hci_filter_all_events(&filter);
+
+    if (setsockopt(hciSocket, SOL_HCI, HCI_FILTER, &filter, sizeof(hci_filter)) < 0) {
+        qCWarning(QT_BT_BLUEZ) << "Could not set HCI socket options:" << strerror(errno);
+        return false;
+    }
+
+    return true;
+}
+
 bool HciManager::sendCommand(OpCodeGroupField ogf, OpCodeCommandField ocf, const QByteArray &parameters)
 {
     qCDebug(QT_BT_BLUEZ) << "sending command; ogf:" << ogf << "ocf:" << ocf;
@@ -325,28 +348,20 @@ bool HciManager::sendConnectionParameterUpdateRequest(quint16 handle,
     const quint16 sigPacketLen = sizeof connUpdateData;
     signalingPacket.length = qToLittleEndian(sigPacketLen);
 
-    struct L2CapHeader {
-        quint16 length;
-        quint16 channelId;
-    } l2CapHeader;
+    L2CapHeader l2CapHeader;
     const quint16 l2CapHeaderLen = sizeof signalingPacket + sigPacketLen;
     l2CapHeader.length = qToLittleEndian(l2CapHeaderLen);
-    l2CapHeader.channelId = qToLittleEndian(quint16(5));
+    l2CapHeader.channelId = qToLittleEndian(quint16(SIGNALING_CHANNEL_ID));
 
     // Vol 2, part E, 5.4.2
-    struct AclData {
-        quint16 handle: 12;
-        quint16 pbFlag: 2;
-        quint16 bcFlag: 2;
-        quint16 dataLen;
-    } aclData;
+    AclData 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;
+    quint8 packetType = HCI_ACL_PKT;
     iv[0].iov_base = &packetType;
     iv[0].iov_len  = 1;
     iv[1].iov_base = &aclData;
@@ -372,8 +387,7 @@ bool HciManager::sendConnectionParameterUpdateRequest(quint16 handle,
  */
 void HciManager::_q_readNotify()
 {
-
-    unsigned char buffer[HCI_MAX_EVENT_SIZE];
+    unsigned char buffer[qMax<int>(HCI_MAX_EVENT_SIZE, sizeof(AclData))];
     int size;
 
     size = ::read(hciSocket, buffer, sizeof(buffer));
@@ -384,16 +398,29 @@ void HciManager::_q_readNotify()
         return;
     }
 
-    const unsigned char *data = buffer;
+    switch (buffer[0]) {
+    case HCI_EVENT_PKT:
+        handleHciEventPacket(buffer + 1, size - 1);
+        break;
+    case HCI_ACL_PKT:
+        handleHciAclPacket(buffer + 1, size - 1);
+        break;
+    default:
+        qCWarning(QT_BT_BLUEZ) << "Ignoring unexpected HCI packet type" << buffer[0];
+    }
+}
 
-    // Not interested in anything but valid HCI events
-    if ((size < HCI_EVENT_HDR_SIZE + 1) || buffer[0] != HCI_EVENT_PKT)
+void HciManager::handleHciEventPacket(const quint8 *data, int size)
+{
+    if (size < HCI_EVENT_HDR_SIZE) {
+        qCWarning(QT_BT_BLUEZ) << "Unexpected HCI event packet size:" << size;
         return;
+    }
 
-    hci_event_hdr *header = (hci_event_hdr *)(&buffer[1]);
+    hci_event_hdr *header = (hci_event_hdr *) data;
 
-    size = size - HCI_EVENT_HDR_SIZE - 1;
-    data = data + HCI_EVENT_HDR_SIZE + 1;
+    size -= HCI_EVENT_HDR_SIZE;
+    data += HCI_EVENT_HDR_SIZE;
 
     if (header->plen != size) {
         qCWarning(QT_BT_BLUEZ) << "Invalid HCI event packet size";
@@ -434,6 +461,62 @@ void HciManager::_q_readNotify()
     default:
         break;
     }
+
+}
+
+void HciManager::handleHciAclPacket(const quint8 *data, int size)
+{
+    if (size < int(sizeof(AclData))) {
+        qCWarning(QT_BT_BLUEZ) << "Unexpected HCI ACL packet size";
+        return;
+    }
+
+    quint16 rawAclData[sizeof(AclData) / sizeof(quint16)];
+    rawAclData[0] = bt_get_le16(data);
+    rawAclData[1] = bt_get_le16(data + sizeof(quint16));
+    const AclData *aclData = reinterpret_cast<AclData *>(rawAclData);
+    data += sizeof *aclData;
+    size -= sizeof *aclData;
+    if (size < aclData->dataLen) {
+        qCWarning(QT_BT_BLUEZ) << "HCI ACL packet data size" << size
+                               << "is smaller than specified size" << aclData->dataLen;
+        return;
+    }
+
+//    qCDebug(QT_BT_BLUEZ) << "handle:" << aclData->handle << "PB:" << aclData->pbFlag
+//                         << "BC:" << aclData->bcFlag << "data len:" << aclData->dataLen;
+
+    // Consider only directed, complete messages from controller to host (i.e. incoming packets).
+    if (aclData->pbFlag != 2 || aclData->bcFlag != 0)
+        return;
+
+    if (size < int(sizeof(L2CapHeader))) {
+        qCWarning(QT_BT_BLUEZ) << "Unexpected HCI ACL packet size";
+        return;
+    }
+    L2CapHeader l2CapHeader = *reinterpret_cast<const L2CapHeader*>(data);
+    l2CapHeader.channelId = qFromLittleEndian(l2CapHeader.channelId);
+    l2CapHeader.length = qFromLittleEndian(l2CapHeader.length);
+    data += sizeof l2CapHeader;
+    size -= sizeof l2CapHeader;
+    if (size < l2CapHeader.length) {
+        qCWarning(QT_BT_BLUEZ) << "L2Cap payload size" << size << "is smaller than specified size"
+                               << l2CapHeader.length;
+        return;
+    }
+//    qCDebug(QT_BT_BLUEZ) << "l2cap channel id:" << l2CapHeader.channelId
+//                         << "payload length:" << l2CapHeader.length;
+    if (l2CapHeader.channelId != SECURITY_CHANNEL_ID)
+        return;
+    if (*data != 0xa) // "Signing Information". Spec v4.2, Vol 3, Part H, 3.6.6
+        return;
+    if (size != 17) {
+        qCWarning(QT_BT_BLUEZ) << "Unexpected key size" << size << "in Signing Information packet";
+        return;
+    }
+    quint128 csrk;
+    memcpy(&csrk, data + 1, sizeof csrk);
+    emit signatureResolvingKeyReceived(aclData->handle, csrk);
 }
 
 void HciManager::handleLeMetaEvent(const quint8 *data)
diff --git a/src/bluetooth/bluez/hcimanager_p.h b/src/bluetooth/bluez/hcimanager_p.h
index b2edb15b..eb899c79 100644
--- a/src/bluetooth/bluez/hcimanager_p.h
+++ b/src/bluetooth/bluez/hcimanager_p.h
@@ -76,6 +76,7 @@ public:
 
     bool isValid() const;
     bool monitorEvent(HciManager::HciEvent event);
+    bool monitorAclPackets();
     bool sendCommand(OpCodeGroupField ogf, OpCodeCommandField ocf, const QByteArray &parameters);
 
     void stopEvents();
@@ -90,12 +91,15 @@ signals:
     void commandCompleted(quint16 opCode, quint8 status, const QByteArray &data);
     void connectionComplete(quint16 handle);
     void connectionUpdate(quint16 handle, const QLowEnergyConnectionParameters &parameters);
+    void signatureResolvingKeyReceived(quint16 connHandle, const quint128 &csrk);
 
 private slots:
     void _q_readNotify();
 
 private:
     int hciForAddress(const QBluetoothAddress &deviceAdapter);
+    void handleHciEventPacket(const quint8 *data, int size);
+    void handleHciAclPacket(const quint8 *data, int size);
     void handleLeMetaEvent(const quint8 *data);
 
     int hciSocket;
diff --git a/src/bluetooth/lecmacverifier.cpp b/src/bluetooth/lecmacverifier.cpp
new file mode 100644
index 00000000..f8ded361
--- /dev/null
+++ b/src/bluetooth/lecmacverifier.cpp
@@ -0,0 +1,171 @@
+/****************************************************************************
+**
+** Copyright (C) 2016 The Qt Company Ltd.
+** Contact: https://www.qt.io/licensing/
+**
+** This file is part of the QtBluetooth module of the Qt Toolkit.
+**
+** $QT_BEGIN_LICENSE:LGPL$
+** Commercial License Usage
+** Licensees holding valid commercial Qt licenses may use this file in
+** accordance with the commercial license agreement provided with the
+** Software or, alternatively, in accordance with the terms contained in
+** a written agreement between you and The Qt Company. For licensing terms
+** and conditions see https://www.qt.io/terms-conditions. For further
+** information use the contact form at https://www.qt.io/contact-us.
+**
+** GNU Lesser General Public License Usage
+** Alternatively, this file may be used under the terms of the GNU Lesser
+** General Public License version 3 as published by the Free Software
+** Foundation and appearing in the file LICENSE.LGPL3 included in the
+** packaging of this file. Please review the following information to
+** ensure the GNU Lesser General Public License version 3 requirements
+** will be met: https://www.gnu.org/licenses/lgpl-3.0.html.
+**
+** GNU General Public License Usage
+** Alternatively, this file may be used under the terms of the GNU
+** General Public License version 2.0 or (at your option) the GNU General
+** Public license version 3 or any later version approved by the KDE Free
+** Qt Foundation. The licenses are as published by the Free Software
+** Foundation and appearing in the file LICENSE.GPL2 and LICENSE.GPL3
+** included in the packaging of this file. Please review the following
+** information to ensure the GNU General Public License requirements will
+** be met: https://www.gnu.org/licenses/gpl-2.0.html and
+** https://www.gnu.org/licenses/gpl-3.0.html.
+**
+** $QT_END_LICENSE$
+**
+****************************************************************************/
+#include "lecmacverifier_p.h"
+
+#include "bluez/bluez_data_p.h"
+
+#include <QtCore/qbytearray.h>
+#include <QtCore/qloggingcategory.h>
+#include <QtCore/private/qcore_unix_p.h>
+
+#include <cstring>
+#include <sys/socket.h>
+#include <sys/types.h>
+#include <unistd.h>
+
+#ifdef CONFIG_LINUX_CRYPTO_API
+#include <linux/if_alg.h>
+#endif
+
+QT_BEGIN_NAMESPACE
+
+Q_DECLARE_LOGGING_CATEGORY(QT_BT_BLUEZ)
+
+LeCmacVerifier::LeCmacVerifier()
+{
+#ifdef CONFIG_LINUX_CRYPTO_API
+    m_baseSocket = socket(AF_ALG, SOCK_SEQPACKET, 0);
+    if (m_baseSocket == -1) {
+        qCWarning(QT_BT_BLUEZ) << "failed to create first level crypto socket:"
+                               << strerror(errno);
+        return;
+    }
+    sockaddr_alg sa;
+    using namespace std;
+    memset(&sa, 0, sizeof sa);
+    sa.salg_family = AF_ALG;
+    strcpy(reinterpret_cast<char *>(sa.salg_type), "hash");
+    strcpy(reinterpret_cast<char *>(sa.salg_name), "cmac(aes)");
+    if (bind(m_baseSocket, reinterpret_cast<sockaddr *>(&sa), sizeof sa) == -1) {
+        qCWarning(QT_BT_BLUEZ) << "bind() failed for crypto socket:" << strerror(errno);
+        return;
+    }
+#else // CONFIG_LINUX_CRYPTO_API
+    qCWarning(QT_BT_BLUEZ) << "Linux crypto API not present, CMAC verification will fail.";
+#endif
+}
+
+LeCmacVerifier::~LeCmacVerifier()
+{
+    if (m_baseSocket != -1)
+        close(m_baseSocket);
+}
+
+QByteArray LeCmacVerifier::createFullMessage(const QByteArray &message, quint32 signCounter)
+{
+    // Spec v4.2, Vol 3, Part H, 2.4.5
+    QByteArray fullMessage = message;
+    fullMessage.resize(fullMessage.count() + sizeof signCounter);
+    putBtData(signCounter, fullMessage.data() + message.count());
+    return fullMessage;
+}
+
+bool LeCmacVerifier::verify(const QByteArray &message, const quint128 &csrk,
+                           quint64 expectedMac) const
+{
+#ifdef CONFIG_LINUX_CRYPTO_API
+    if (m_baseSocket == -1)
+        return false;
+    quint128 csrkMsb;
+    std::reverse_copy(std::begin(csrk.data), std::end(csrk.data), std::begin(csrkMsb.data));
+    qCDebug(QT_BT_BLUEZ) << "CSRK (MSB):" << QByteArray(reinterpret_cast<char *>(csrkMsb.data),
+                                                        sizeof csrkMsb).toHex();
+    if (setsockopt(m_baseSocket, 279 /* SOL_ALG */, ALG_SET_KEY, csrkMsb.data, sizeof csrkMsb) == -1) {
+        qCWarning(QT_BT_BLUEZ) << "setsockopt() failed for crypto socket:" << strerror(errno);
+        return false;
+    }
+
+    class SocketWrapper
+    {
+    public:
+        SocketWrapper(int socket) : m_socket(socket) {}
+        ~SocketWrapper() {
+            if (m_socket != -1)
+                close(m_socket);
+        }
+
+        int value() const { return m_socket; }
+    private:
+        int m_socket;
+    };
+    SocketWrapper cryptoSocket(accept(m_baseSocket, nullptr, 0));
+    if (cryptoSocket.value() == -1) {
+        qCWarning(QT_BT_BLUEZ) << "accept() failed for crypto socket:" << strerror(errno);
+        return false;
+    }
+
+    QByteArray messageSwapped(message.count(), Qt::Uninitialized);
+    std::reverse_copy(message.begin(), message.end(), messageSwapped.begin());
+    qint64 totalBytesWritten = 0;
+    do {
+        const qint64 bytesWritten = qt_safe_write(cryptoSocket.value(),
+                                                  messageSwapped.constData() + totalBytesWritten,
+                                                  messageSwapped.count() - totalBytesWritten);
+        if (bytesWritten == -1) {
+            qCWarning(QT_BT_BLUEZ) << "writing to crypto socket failed:" << strerror(errno);
+            return false;
+        }
+        totalBytesWritten += bytesWritten;
+    } while (totalBytesWritten < messageSwapped.count());
+    quint64 mac;
+    quint8 * const macPtr = reinterpret_cast<quint8 *>(&mac);
+    qint64 totalBytesRead = 0;
+    do {
+        const qint64 bytesRead = qt_safe_read(cryptoSocket.value(), macPtr + totalBytesRead,
+                                              sizeof mac - totalBytesRead);
+        if (bytesRead == -1) {
+            qCWarning(QT_BT_BLUEZ) << "reading from crypto socket failed:" << strerror(errno);
+            return false;
+        }
+        totalBytesRead += bytesRead;
+    } while (totalBytesRead < qint64(sizeof mac));
+    mac = qFromBigEndian(mac);
+    if (mac != expectedMac) {
+        qCWarning(QT_BT_BLUEZ) << hex << "signature verification failed: calculated mac:" << mac
+                               << "expected mac:" << expectedMac;
+        return false;
+    }
+    return true;
+#else // CONFIG_LINUX_CRYPTO_API
+    qCWarning(QT_BT_BLUEZ) << "CMAC verification failed due to missing Linux crypto API.";
+    return false;
+#endif
+}
+
+QT_END_NAMESPACE
diff --git a/src/bluetooth/lecmacverifier_p.h b/src/bluetooth/lecmacverifier_p.h
new file mode 100644
index 00000000..e09b6013
--- /dev/null
+++ b/src/bluetooth/lecmacverifier_p.h
@@ -0,0 +1,76 @@
+/****************************************************************************
+**
+** Copyright (C) 2016 The Qt Company Ltd.
+** Contact: https://www.qt.io/licensing/
+**
+** This file is part of the QtBluetooth module of the Qt Toolkit.
+**
+** $QT_BEGIN_LICENSE:LGPL$
+** Commercial License Usage
+** Licensees holding valid commercial Qt licenses may use this file in
+** accordance with the commercial license agreement provided with the
+** Software or, alternatively, in accordance with the terms contained in
+** a written agreement between you and The Qt Company. For licensing terms
+** and conditions see https://www.qt.io/terms-conditions. For further
+** information use the contact form at https://www.qt.io/contact-us.
+**
+** GNU Lesser General Public License Usage
+** Alternatively, this file may be used under the terms of the GNU Lesser
+** General Public License version 3 as published by the Free Software
+** Foundation and appearing in the file LICENSE.LGPL3 included in the
+** packaging of this file. Please review the following information to
+** ensure the GNU Lesser General Public License version 3 requirements
+** will be met: https://www.gnu.org/licenses/lgpl-3.0.html.
+**
+** GNU General Public License Usage
+** Alternatively, this file may be used under the terms of the GNU
+** General Public License version 2.0 or (at your option) the GNU General
+** Public license version 3 or any later version approved by the KDE Free
+** Qt Foundation. The licenses are as published by the Free Software
+** Foundation and appearing in the file LICENSE.GPL2 and LICENSE.GPL3
+** included in the packaging of this file. Please review the following
+** information to ensure the GNU General Public License requirements will
+** be met: https://www.gnu.org/licenses/gpl-2.0.html and
+** https://www.gnu.org/licenses/gpl-3.0.html.
+**
+** $QT_END_LICENSE$
+**
+****************************************************************************/
+#ifndef LECMACVERIFIER_H
+#define LECMACVERIFIER_H
+
+//
+//  W A R N I N G
+//  -------------
+//
+// This file is not part of the Qt API.  It exists purely as an
+// implementation detail.  This header file may change from version to
+// version without notice, or even be removed.
+//
+// We mean it.
+//
+
+#include <QtCore/qglobal.h>
+
+QT_BEGIN_NAMESPACE
+
+struct quint128;
+
+class Q_AUTOTEST_EXPORT LeCmacVerifier
+{
+public:
+    LeCmacVerifier();
+    ~LeCmacVerifier();
+
+    static QByteArray createFullMessage(const QByteArray &message, quint32 signCounter);
+
+    bool verify(const QByteArray &message, const quint128 &csrk, quint64 expectedMac) const;
+
+private:
+    int m_baseSocket = -1;
+};
+
+
+QT_END_NAMESPACE
+
+#endif // Header guard
diff --git a/src/bluetooth/qlowenergycontroller.cpp b/src/bluetooth/qlowenergycontroller.cpp
index 0cbf6582..238ed92d 100644
--- a/src/bluetooth/qlowenergycontroller.cpp
+++ b/src/bluetooth/qlowenergycontroller.cpp
@@ -185,7 +185,9 @@ Q_DECLARE_LOGGING_CATEGORY(QT_BT)
    \sa QLowEnergyController::createCentral()
    \sa QLowEnergyController::createPeripheral()
    \since 5.7
-   \note The peripheral role is currently only supported on Linux.
+   \note The peripheral role is currently only supported on Linux. In addition, handling the
+         "Signed Write" ATT command on the server side requires BlueZ 5 and kernel version 3.7
+         or newer.
  */
 
 
diff --git a/src/bluetooth/qlowenergycontroller_bluez.cpp b/src/bluetooth/qlowenergycontroller_bluez.cpp
index 1562c3f9..06586192 100644
--- a/src/bluetooth/qlowenergycontroller_bluez.cpp
+++ b/src/bluetooth/qlowenergycontroller_bluez.cpp
@@ -38,13 +38,16 @@
 **
 ****************************************************************************/
 
+#include "lecmacverifier_p.h"
 #include "qlowenergycontroller_p.h"
 #include "qbluetoothsocket_p.h"
 #include "qleadvertiser_p.h"
 #include "bluez/bluez_data_p.h"
 #include "bluez/hcimanager_p.h"
 
+#include <QtCore/QFileInfo>
 #include <QtCore/QLoggingCategory>
+#include <QtCore/QSettings>
 #include <QtBluetooth/QBluetoothLocalDevice>
 #include <QtBluetooth/QBluetoothSocket>
 #include <QtBluetooth/QLowEnergyCharacteristicData>
@@ -60,8 +63,6 @@
 #include <sys/socket.h>
 #include <unistd.h>
 
-#define ATTRIBUTE_CHANNEL_ID 4
-
 #define ATT_DEFAULT_LE_MTU 23
 #define ATT_MAX_LE_MTU 0x200
 
@@ -271,6 +272,7 @@ QLowEnergyControllerPrivate::QLowEnergyControllerPrivate()
     connect(hciManager, SIGNAL(encryptionChangedEvent(QBluetoothAddress,bool)),
             this, SLOT(encryptionChangedEvent(QBluetoothAddress,bool)));
     hciManager->monitorEvent(HciManager::LeMetaEvent);
+    hciManager->monitorAclPackets();
     connect(hciManager, &HciManager::connectionComplete, [this](quint16 handle) {
         connectionHandle = handle;
         qCDebug(QT_BT_BLUEZ) << "received connection complete event, handle:" << handle;
@@ -281,11 +283,22 @@ QLowEnergyControllerPrivate::QLowEnergyControllerPrivate()
                     emit q_ptr->connectionUpdated(params);
             }
     );
+    connect(hciManager, &HciManager::signatureResolvingKeyReceived,
+            [this](quint16 handle, const quint128 &csrk) {
+                if (handle == connectionHandle) {
+                    qCDebug(QT_BT_BLUEZ) << "received new signature resolving key"
+                                         << QByteArray(reinterpret_cast<const char *>(csrk.data),
+                                                       sizeof csrk).toHex();
+                    signingData.insert(remoteDevice.toUInt64(), SigningData(csrk));
+                }
+            }
+    );
 }
 
 QLowEnergyControllerPrivate::~QLowEnergyControllerPrivate()
 {
     closeServerSocket();
+    delete cmacVerifier;
 }
 
 class ServerSocket
@@ -337,6 +350,7 @@ private:
     int m_socket = -1;
 };
 
+
 void QLowEnergyControllerPrivate::startAdvertising(const QLowEnergyAdvertisingParameters &params,
         const QLowEnergyAdvertisingData &advertisingData,
         const QLowEnergyAdvertisingData &scanResponseData)
@@ -2462,9 +2476,32 @@ void QLowEnergyControllerPrivate::handleWriteRequestOrCommand(const QByteArray &
             qCWarning(QT_BT_BLUEZ) << "Ignoring signed write on encrypted link.";
             return;
         }
-        // const QByteArray signature = packet.right(12);
-        qCWarning(QT_BT_BLUEZ) << "signed write not implemented, ignoring.";
-        return; // TODO: Check signature and continue if it's valid. Check and update sign counter.
+        const auto signingDataIt = signingData.find(remoteDevice.toUInt64());
+        if (signingDataIt == signingData.constEnd()) {
+            qCWarning(QT_BT_BLUEZ) << "No CSRK found for peer device, ignoring signed write";
+            return;
+        }
+
+        const quint32 signCounter = getBtData<quint32>(packet.data() + packet.count() - 12);
+        if (signCounter < signingDataIt.value().counter + 1) {
+            qCWarning(QT_BT_BLUEZ) << "Client's' sign counter" << signCounter
+                                   << "not greater than local sign counter"
+                                   << signingDataIt.value().counter
+                                   << "; ignoring signed write command.";
+            return;
+        }
+
+        const quint64 macFromClient = getBtData<quint64>(packet.data() + packet.count() - 8);
+        const bool signatureCorrect = verifyMac(packet.left(packet.count() - 12),
+                signingDataIt.value().key, signCounter, macFromClient);
+        if (!signatureCorrect) {
+            qCWarning(QT_BT_BLUEZ) << "Signed Write packet has wrong signature, disconnecting";
+            disconnectFromDevice(); // Recommended by spec v4.2, Vol 3, part C, 10.4.2
+            return;
+        }
+
+        signingDataIt.value().counter = signCounter;
+        storeSignCounter();
         valueLength = packet.count() - 15;
     } else {
         valueLength = packet.count() - 3;
@@ -2678,6 +2715,7 @@ void QLowEnergyControllerPrivate::handleConnectionRequest()
     l2cpSocket->setSocketDescriptor(clientSocket, QBluetoothServiceInfo::L2capProtocol,
             QBluetoothSocket::ConnectedState, QIODevice::ReadWrite | QIODevice::Unbuffered);
     restoreClientConfigurations();
+    loadSigningDataIfNecessary();
     setState(QLowEnergyController::ConnectedState);
 }
 
@@ -2773,6 +2811,64 @@ void QLowEnergyControllerPrivate::restoreClientConfigurations()
     sendNextIndication();
 }
 
+void QLowEnergyControllerPrivate::loadSigningDataIfNecessary()
+{
+    const auto signingDataIt = signingData.constFind(remoteDevice.toUInt64());
+    if (signingDataIt != signingData.constEnd())
+        return; // We are up to date for this device.
+    const QString settingsFilePath = keySettingsFilePath();
+    if (!QFileInfo(settingsFilePath).exists()) {
+        qCDebug(QT_BT_BLUEZ) << "No settings found for peer device.";
+        return;
+    }
+    QSettings settings(settingsFilePath, QSettings::IniFormat);
+    settings.beginGroup(QLatin1String("RemoteSignatureKey"));
+    const QByteArray keyString = settings.value(QLatin1String("Key")).toByteArray();
+    if (keyString.isEmpty()) {
+        qCDebug(QT_BT_BLUEZ) << "No remote signature key found in settings file";
+        return;
+    }
+    const QByteArray keyData = QByteArray::fromHex(keyString);
+    if (keyData.count() != int(sizeof(quint128))) {
+        qCWarning(QT_BT_BLUEZ) << "Remote signature key in settings file has invalid size"
+                               << keyString.count();
+        return;
+    }
+    qCDebug(QT_BT_BLUEZ) << "CSRK of peer device is" << keyString;
+    const quint32 counter = settings.value(QLatin1String("Counter"), 0).toUInt();
+    quint128 csrk;
+    using namespace std;
+    memcpy(csrk.data, keyData.constData(), keyData.count());
+    signingData.insert(remoteDevice.toUInt64(), SigningData(csrk, counter - 1));
+}
+
+void QLowEnergyControllerPrivate::storeSignCounter()
+{
+    const auto signingDataIt = signingData.constFind(remoteDevice.toUInt64());
+    if (signingDataIt == signingData.constEnd())
+        return;
+    const QString settingsFilePath = keySettingsFilePath();
+    if (!QFileInfo(settingsFilePath).exists())
+        return;
+    QSettings settings(settingsFilePath, QSettings::IniFormat);
+    if (!settings.isWritable())
+        return;
+    settings.beginGroup(QLatin1String("RemoteSignatureKey"));
+    const QString counterKey = QLatin1String("Counter");
+    if (!settings.allKeys().contains(counterKey))
+        return;
+    const quint32 counterValue = signingDataIt.value().counter + 1;
+    if (counterValue == settings.value(counterKey).toUInt())
+        return;
+    settings.setValue(counterKey, counterValue);
+}
+
+QString QLowEnergyControllerPrivate::keySettingsFilePath() const
+{
+    return QString::fromLatin1("/var/lib/bluetooth/%1/%2/info")
+            .arg(localAdapter.toString(), remoteDevice.toString());
+}
+
 static QByteArray uuidToByteArray(const QBluetoothUuid &uuid)
 {
     QByteArray ba;
@@ -2999,4 +3095,13 @@ int QLowEnergyControllerPrivate::checkReadPermissions(QVector<Attribute> &attrib
     return 0;
 }
 
+bool QLowEnergyControllerPrivate::verifyMac(const QByteArray &message, const quint128 &csrk,
+                                             quint32 signCounter, quint64 expectedMac)
+{
+    if (!cmacVerifier)
+        cmacVerifier = new LeCmacVerifier;
+    return cmacVerifier->verify(LeCmacVerifier::createFullMessage(message, signCounter), csrk,
+                                expectedMac);
+}
+
 QT_END_NAMESPACE
diff --git a/src/bluetooth/qlowenergycontroller_p.h b/src/bluetooth/qlowenergycontroller_p.h
index 095f838f..51ea5a83 100644
--- a/src/bluetooth/qlowenergycontroller_p.h
+++ b/src/bluetooth/qlowenergycontroller_p.h
@@ -92,6 +92,7 @@ class QLowEnergyServiceData;
 
 #if defined(QT_BLUEZ_BLUETOOTH) && !defined(QT_BLUEZ_NO_BTLE)
 class HciManager;
+class LeCmacVerifier;
 class QSocketNotifier;
 #elif defined(QT_ANDROID_BLUETOOTH)
 class LowEnergyNotificationHub;
@@ -248,6 +249,17 @@ private:
     };
     QHash<quint64, QVector<ClientConfigurationData>> clientConfigData;
 
+    struct SigningData {
+        SigningData() = default;
+        SigningData(const quint128 &csrk, quint32 signCounter = quint32(-1))
+            : key(csrk), counter(signCounter) {}
+
+        quint128 key;
+        quint32 counter = quint32(-1);
+    };
+    QHash<quint64, SigningData> signingData;
+    LeCmacVerifier *cmacVerifier = nullptr;
+
     bool requestPending;
     quint16 mtuSize;
     int securityLevelValue;
@@ -265,6 +277,9 @@ private:
     QVector<TempClientConfigurationData> gatherClientConfigData();
     void storeClientConfigurations();
     void restoreClientConfigurations();
+    void loadSigningDataIfNecessary();
+    void storeSignCounter();
+    QString keySettingsFilePath() const;
 
     void sendPacket(const QByteArray &packet);
     void sendNextPendingRequest();
@@ -339,6 +354,9 @@ private:
     int checkReadPermissions(const Attribute &attr);
     int checkReadPermissions(QVector<Attribute> &attributes);
 
+    bool verifyMac(const QByteArray &message, const quint128 &csrk, quint32 signCounter,
+                   quint64 expectedMac);
+
     void updateLocalAttributeValue(
             QLowEnergyHandle handle,
             const QByteArray &value,