PresenceDetection/Bluetooth/Functions.cpp

#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