Initial Commit

Bluetooth/Functions.cpp

@@ -0,0 +1,110 @@
+#include <sstream>
+#include <vector>
+#include <stdexcept>
+#include <syslog.h>
+#include <bluetooth/bluetooth.h>
+#include <bluetooth/hci.h>
+#include <bluetooth/l2cap.h>
+
+#include <unistd.h>
+#include <sys/poll.h>
+#include "Socket.h"
+#include "Functions.h"
+
+
+namespace PresenceDetection {
+namespace Bluetooth {
+
+bool Functions::Ping(const std::string& macAddress)
+{
+	struct sockaddr_l2 socketAddress;
+
+	int size = 44;
+	int messageSize = L2CAP_CMD_HDR_SIZE + size;
+	std::vector<unsigned char> sendBuffer(messageSize);
+	std::vector<unsigned char> receiveBuffer(messageSize);
+
+	Socket socket;
+	int socketDescriptor = socket.Descriptor();
+	if (socketDescriptor < 0)
+		throw std::runtime_error("Can't create Bluetooth socket");
+
+	memset(&socketAddress, 0, sizeof(socketAddress));
+	socketAddress.l2_family = AF_BLUETOOTH;
+
+	bdaddr_t bdaddr;
+	bacpy(&socketAddress.l2_bdaddr, &bdaddr);
+
+	if (bind(socketDescriptor, reinterpret_cast<struct sockaddr*>(&socketAddress), sizeof(socketAddress)) < 0)
+		throw std::runtime_error("Can't bind socket");
+
+	memset(&socketAddress, 0, sizeof(socketAddress));
+	socketAddress.l2_family = AF_BLUETOOTH;
+	str2ba(macAddress.c_str(), &socketAddress.l2_bdaddr);
+
+	if (connect(socketDescriptor, reinterpret_cast<struct sockaddr*>(&socketAddress), sizeof(socketAddress)) < 0)
+		return false;
+
+	memset(&socketAddress, 0, sizeof(socketAddress));
+	socklen_t optlen = sizeof(socketAddress);
+
+	if (getsockname(socketDescriptor, reinterpret_cast<struct sockaddr*>(&socketAddress), &optlen) < 0)
+		throw std::runtime_error("Can't get local address");
+
+	char str[18];
+	ba2str(&socketAddress.l2_bdaddr, str);
+
+	for (int i = 0; i < size; ++i)
+		sendBuffer[L2CAP_CMD_HDR_SIZE + i] = (i % 40) + 'A';
+
+	int ident   = 200;
+	uint8_t id = ident;
+
+	l2cap_cmd_hdr* send_cmd = reinterpret_cast<l2cap_cmd_hdr*>(sendBuffer.data());
+	l2cap_cmd_hdr* recv_cmd = reinterpret_cast<l2cap_cmd_hdr*>(receiveBuffer.data());
+
+	send_cmd->ident = id;
+	send_cmd->len   = htobs(size);
+	send_cmd->code = L2CAP_ECHO_REQ;
+
+	if (send(socketDescriptor, sendBuffer.data(), messageSize, 0) <= 0)
+		throw std::runtime_error("Send failed");
+
+	while (1)
+	{
+		struct pollfd pf[1];
+
+		pf[0].fd = socketDescriptor;
+		pf[0].events = POLLIN;
+
+		int err;
+		int timeout = 500;
+		if ((err = poll(pf, 1, timeout)) < 0)
+			throw std::runtime_error("Poll failed");
+
+		if (!err)
+			return false;
+
+		if ((err = recv(socketDescriptor, receiveBuffer.data(), messageSize, 0)) < 0)
+			return false;
+
+		if (!err)
+			throw std::runtime_error("Disconnected");
+
+		recv_cmd->len = btohs(recv_cmd->len);
+
+		if (recv_cmd->ident != id)
+			continue;
+
+		if (recv_cmd->code == L2CAP_ECHO_RSP)
+			break;
+
+		if (recv_cmd->code == L2CAP_COMMAND_REJ)
+			throw std::runtime_error("Peer doesn't support Echo packets");
+	}
+
+	return true;
+}
+
+} // namespace Bluetooth
+} // namespace PresenceDetection