PresenceDetection/Bluetooth/Functions.cpp

#include "Functions.h"
#include "Socket.h"
#include <fcntl.h>
#include <unistd.h>
#include <bluetooth/bluetooth.h>
#include <bluetooth/hci.h>
#include <bluetooth/l2cap.h>
#include <poll.h>
#include <sstream>
#include <stdexcept>
#include <sys/time.h>
#include <vector>


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({{0, 0, 0, 0, 0, 0}});
	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);

	struct timeval timeoutStruct;
	timeoutStruct.tv_sec = 1;
	timeoutStruct.tv_usec = 0; //500000;

	fd_set writeDescriptor, errorDescriptor;
	FD_ZERO(&writeDescriptor);
	FD_ZERO(&errorDescriptor);
	FD_SET(socketDescriptor, &writeDescriptor);
	FD_SET(socketDescriptor, &errorDescriptor);

	int flags;
	if((flags = fcntl(socketDescriptor, F_GETFL, 0) < 0))
		throw std::runtime_error("Can't get socket options");

	if(fcntl(socketDescriptor, F_SETFL, flags | O_NONBLOCK) < 0)
		throw std::runtime_error("Can't set socket options");

	int ret = 0;
	if ((ret = connect(socketDescriptor, reinterpret_cast<struct sockaddr*>(&socketAddress), sizeof(socketAddress)) < 0))
	{
		if (errno != EINPROGRESS)
			return false;
	}

	if (ret != 0)
	{
		if (select(socketDescriptor + 1, &errorDescriptor, &writeDescriptor, NULL, &timeoutStruct) <= 0)
			return false;

		int error = 0;
		socklen_t len = sizeof(error);
		if (FD_ISSET(socketDescriptor, &errorDescriptor) || FD_ISSET(socketDescriptor, &writeDescriptor))
		{
			if (getsockopt(socketDescriptor, SOL_SOCKET, SO_ERROR, &error, &len) < 0)
				return false;
		}
		else
		{
			return false;
		}

		if (error)
			return false;
	}

	if(fcntl(socketDescriptor, F_SETFL, flags) < 0)
		throw std::runtime_error("Can't set socket options");

	connect(socketDescriptor, reinterpret_cast<struct sockaddr*>(&socketAddress), sizeof(socketAddress));

	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 = 200;
		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