158 lines
4.0 KiB
C++
158 lines
4.0 KiB
C++
#include <sstream>
|
|
#include <vector>
|
|
#include <stdexcept>
|
|
#include <bluetooth/bluetooth.h>
|
|
#include <bluetooth/hci.h>
|
|
#include <bluetooth/l2cap.h>
|
|
|
|
#include <fcntl.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({{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 = 3;
|
|
timeoutStruct.tv_usec = 0;
|
|
|
|
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 = 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
|