|
|
@@ -1,266 +1,125 @@
|
|
|
#include <iostream>
|
|
|
#include <string>
|
|
|
|
|
|
-#include <stdio.h>
|
|
|
-#include <errno.h>
|
|
|
+#include <memory>
|
|
|
+#include <algorithm>
|
|
|
#include <unistd.h>
|
|
|
-#include <stdlib.h>
|
|
|
-#include <string.h>
|
|
|
-#include <getopt.h>
|
|
|
-#include <signal.h>
|
|
|
-#include <sys/time.h>
|
|
|
-#include <sys/poll.h>
|
|
|
-#include <sys/socket.h>
|
|
|
-
|
|
|
-#include <bluetooth/bluetooth.h>
|
|
|
-#include <bluetooth/hci.h>
|
|
|
-#include <bluetooth/hci_lib.h>
|
|
|
-#include <bluetooth/l2cap.h>
|
|
|
+#include <chrono>
|
|
|
+#include <deque>
|
|
|
|
|
|
#include "Util/Timer.h"
|
|
|
+#include "Util/INIh.h"
|
|
|
|
|
|
-/* Defaults */
|
|
|
-static bdaddr_t bdaddr;
|
|
|
-static int size = 44;
|
|
|
-static int ident = 200;
|
|
|
-static int delay = 1;
|
|
|
-static int count = -1;
|
|
|
-static int timeout = 10;
|
|
|
-static int reverse = 0;
|
|
|
-static int verify = 0;
|
|
|
-
|
|
|
-/* Stats */
|
|
|
-static int sent_pkt = 0;
|
|
|
-static int recv_pkt = 0;
|
|
|
-
|
|
|
-static float tv2fl(struct timeval tv)
|
|
|
-{
|
|
|
- return (float)(tv.tv_sec*1000.0) + (float)(tv.tv_usec/1000.0);
|
|
|
-}
|
|
|
-
|
|
|
-static void stat(int /*sig*/)
|
|
|
-{
|
|
|
- int loss = sent_pkt ? (float)((sent_pkt-recv_pkt)/(sent_pkt/100.0)) : 0;
|
|
|
- printf("%d sent, %d received, %d%% loss\n", sent_pkt, recv_pkt, loss);
|
|
|
- exit(0);
|
|
|
-}
|
|
|
-
|
|
|
-static void ping(const char* svr)
|
|
|
-{
|
|
|
- struct sigaction sa;
|
|
|
- struct sockaddr_l2 addr;
|
|
|
- socklen_t optlen;
|
|
|
- unsigned char *send_buf;
|
|
|
- unsigned char *recv_buf;
|
|
|
- char str[18];
|
|
|
- int i, sk, lost;
|
|
|
- uint8_t id;
|
|
|
-
|
|
|
- memset(&sa, 0, sizeof(sa));
|
|
|
- sa.sa_handler = stat;
|
|
|
- sigaction(SIGINT, &sa, NULL);
|
|
|
-
|
|
|
- send_buf = static_cast<unsigned char*>(malloc(L2CAP_CMD_HDR_SIZE + size));
|
|
|
- recv_buf = static_cast<unsigned char*>(malloc(L2CAP_CMD_HDR_SIZE + size));
|
|
|
- if (!send_buf || !recv_buf) {
|
|
|
- perror("Can't allocate buffer");
|
|
|
- exit(1);
|
|
|
- }
|
|
|
-
|
|
|
- /* Create socket */
|
|
|
- sk = socket(PF_BLUETOOTH, SOCK_RAW, BTPROTO_L2CAP);
|
|
|
- if (sk < 0) {
|
|
|
- perror("Can't create socket");
|
|
|
- goto error;
|
|
|
- }
|
|
|
-
|
|
|
- /* Bind to local address */
|
|
|
- memset(&addr, 0, sizeof(addr));
|
|
|
- addr.l2_family = AF_BLUETOOTH;
|
|
|
- bacpy(&addr.l2_bdaddr, &bdaddr);
|
|
|
-
|
|
|
- if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
|
|
|
- perror("Can't bind socket");
|
|
|
- goto error;
|
|
|
- }
|
|
|
-
|
|
|
- /* Connect to remote device */
|
|
|
- memset(&addr, 0, sizeof(addr));
|
|
|
- addr.l2_family = AF_BLUETOOTH;
|
|
|
- str2ba(svr, &addr.l2_bdaddr);
|
|
|
-
|
|
|
- if (connect(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
|
|
|
- perror("Can't connect");
|
|
|
- goto error;
|
|
|
- }
|
|
|
-
|
|
|
- /* Get local address */
|
|
|
- memset(&addr, 0, sizeof(addr));
|
|
|
- optlen = sizeof(addr);
|
|
|
-
|
|
|
- if (getsockname(sk, (struct sockaddr *) &addr, &optlen) < 0) {
|
|
|
- perror("Can't get local address");
|
|
|
- goto error;
|
|
|
- }
|
|
|
-
|
|
|
- ba2str(&addr.l2_bdaddr, str);
|
|
|
- printf("Ping: %s from %s (data size %d) ...\n", svr, str, size);
|
|
|
-
|
|
|
- /* Initialize send buffer */
|
|
|
- for (i = 0; i < size; i++)
|
|
|
- send_buf[L2CAP_CMD_HDR_SIZE + i] = (i % 40) + 'A';
|
|
|
-
|
|
|
- id = ident;
|
|
|
-
|
|
|
- while (count == -1 || count-- > 0) {
|
|
|
- struct timeval tv_send, tv_recv, tv_diff;
|
|
|
- l2cap_cmd_hdr *send_cmd = (l2cap_cmd_hdr *) send_buf;
|
|
|
- l2cap_cmd_hdr *recv_cmd = (l2cap_cmd_hdr *) recv_buf;
|
|
|
-
|
|
|
- /* Build command header */
|
|
|
- send_cmd->ident = id;
|
|
|
- send_cmd->len = htobs(size);
|
|
|
-
|
|
|
- if (reverse)
|
|
|
- send_cmd->code = L2CAP_ECHO_RSP;
|
|
|
- else
|
|
|
- send_cmd->code = L2CAP_ECHO_REQ;
|
|
|
-
|
|
|
- gettimeofday(&tv_send, NULL);
|
|
|
-
|
|
|
- /* Send Echo Command */
|
|
|
- if (send(sk, send_buf, L2CAP_CMD_HDR_SIZE + size, 0) <= 0) {
|
|
|
- perror("Send failed");
|
|
|
- goto error;
|
|
|
- }
|
|
|
-
|
|
|
- /* Wait for Echo Response */
|
|
|
- lost = 0;
|
|
|
- while (1) {
|
|
|
- struct pollfd pf[1];
|
|
|
- int err;
|
|
|
-
|
|
|
- pf[0].fd = sk;
|
|
|
- pf[0].events = POLLIN;
|
|
|
-
|
|
|
- if ((err = poll(pf, 1, timeout * 1000)) < 0) {
|
|
|
- perror("Poll failed");
|
|
|
- goto error;
|
|
|
- }
|
|
|
-
|
|
|
- if (!err) {
|
|
|
- lost = 1;
|
|
|
- break;
|
|
|
- }
|
|
|
-
|
|
|
- if ((err = recv(sk, recv_buf, L2CAP_CMD_HDR_SIZE + size, 0)) < 0) {
|
|
|
- perror("Recv failed");
|
|
|
- goto error;
|
|
|
- }
|
|
|
-
|
|
|
- if (!err){
|
|
|
- printf("Disconnected\n");
|
|
|
- goto error;
|
|
|
- }
|
|
|
-
|
|
|
- recv_cmd->len = btohs(recv_cmd->len);
|
|
|
-
|
|
|
- /* Check for our id */
|
|
|
- if (recv_cmd->ident != id)
|
|
|
- continue;
|
|
|
-
|
|
|
- /* Check type */
|
|
|
- if (!reverse && recv_cmd->code == L2CAP_ECHO_RSP)
|
|
|
- break;
|
|
|
-
|
|
|
- if (recv_cmd->code == L2CAP_COMMAND_REJ) {
|
|
|
- printf("Peer doesn't support Echo packets\n");
|
|
|
- goto error;
|
|
|
- }
|
|
|
-
|
|
|
- }
|
|
|
- sent_pkt++;
|
|
|
-
|
|
|
- if (!lost) {
|
|
|
- recv_pkt++;
|
|
|
-
|
|
|
- gettimeofday(&tv_recv, NULL);
|
|
|
- timersub(&tv_recv, &tv_send, &tv_diff);
|
|
|
-
|
|
|
- if (verify) {
|
|
|
- /* Check payload length */
|
|
|
- if (recv_cmd->len != size) {
|
|
|
- fprintf(stderr, "Received %d bytes, expected %d\n",
|
|
|
- recv_cmd->len, size);
|
|
|
- goto error;
|
|
|
- }
|
|
|
-
|
|
|
- /* Check payload */
|
|
|
- if (memcmp(&send_buf[L2CAP_CMD_HDR_SIZE],
|
|
|
- &recv_buf[L2CAP_CMD_HDR_SIZE], size)) {
|
|
|
- fprintf(stderr, "Response payload different.\n");
|
|
|
- goto error;
|
|
|
- }
|
|
|
- }
|
|
|
-
|
|
|
- printf("%d bytes from %s id %d time %.2fms\n", recv_cmd->len, svr,
|
|
|
- id - ident, tv2fl(tv_diff));
|
|
|
-
|
|
|
- if (delay)
|
|
|
- sleep(delay);
|
|
|
- } else {
|
|
|
- printf("no response from %s: id %d\n", svr, id - ident);
|
|
|
- }
|
|
|
-
|
|
|
- if (++id > 254)
|
|
|
- id = ident;
|
|
|
- }
|
|
|
- stat(0);
|
|
|
- free(send_buf);
|
|
|
- free(recv_buf);
|
|
|
- return;
|
|
|
-
|
|
|
-error:
|
|
|
- close(sk);
|
|
|
- free(send_buf);
|
|
|
- free(recv_buf);
|
|
|
- exit(1);
|
|
|
-}
|
|
|
|
|
|
void print()
|
|
|
{
|
|
|
std::cout << "Print" << std::endl;
|
|
|
}
|
|
|
|
|
|
-int main(int /*argc*/, char** /*argv[]*/)
|
|
|
+void TestTimerAbort()
|
|
|
{
|
|
|
- //std::string address = "48:4B:AA:85:30:C5";
|
|
|
- //ping(address.c_str());
|
|
|
-
|
|
|
- //address = "d0:c5:f3:84:99:de";
|
|
|
- //ping(address.c_str());
|
|
|
-
|
|
|
PresenceDetection::Util::Timer timer;
|
|
|
std::function<void()> f_print = print;
|
|
|
|
|
|
std::cout << "---" << std::endl;
|
|
|
+ auto start = std::chrono::steady_clock::now();
|
|
|
+ std::clock_t begin = std::clock();
|
|
|
timer.StartContinuous(1000, f_print);
|
|
|
timer.Stop();
|
|
|
timer.Wait();
|
|
|
+ auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now() - start);
|
|
|
+ std::cout << "Elapsed: " << elapsed.count() << std::endl;
|
|
|
std::cout << "---" << std::endl << std::endl;
|
|
|
|
|
|
std::cout << "---" << std::endl;
|
|
|
+ start = std::chrono::steady_clock::now();
|
|
|
timer.StartContinuous(3000, f_print);
|
|
|
timer.Abort();
|
|
|
timer.Wait();
|
|
|
+ elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now() - start);
|
|
|
+ std::cout << "Elapsed: " << elapsed.count() << std::endl;
|
|
|
std::cout << "---" << std::endl << std::endl;
|
|
|
|
|
|
std::cout << "---" << std::endl;
|
|
|
+ start = std::chrono::steady_clock::now();
|
|
|
timer.StartContinuous(3000, f_print);
|
|
|
sleep(1);
|
|
|
timer.Abort();
|
|
|
timer.Wait();
|
|
|
+ elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now() - start);
|
|
|
+ std::cout << "Elapsed: " << elapsed.count() << std::endl;
|
|
|
std::cout << "---" << std::endl << std::endl;
|
|
|
+}
|
|
|
+
|
|
|
+typedef std::unique_ptr<PresenceDetection::Util::Timer> Pointer;
|
|
|
+
|
|
|
+template<typename T, typename... Args>
|
|
|
+std::unique_ptr<T> make_unique(Args&&... args) {
|
|
|
+ return std::unique_ptr<T>(new T(std::forward<Args>(args)...));
|
|
|
+}
|
|
|
+
|
|
|
+void OnTimerEnd(const std::string& type, int id)
|
|
|
+{
|
|
|
+ std::cout << "OnTimerEnd() " << type << " " << id <<std::endl;
|
|
|
+}
|
|
|
+
|
|
|
+void StartNewTimer(std::deque<Pointer>& timers)
|
|
|
+{
|
|
|
+ int delay(2000);
|
|
|
+ std::string identifier("Blabla");
|
|
|
+ int id(10);
|
|
|
+
|
|
|
+ // Protect!
|
|
|
+ auto timer = make_unique<PresenceDetection::Util::Timer>();
|
|
|
+ timer->StartSingle(delay, static_cast<std::function<void(const std::string&, int)>>(std::bind(&OnTimerEnd, std::placeholders::_1, std::placeholders::_2)), identifier, id);
|
|
|
+ timers.push_back(std::move(timer));
|
|
|
+}
|
|
|
+
|
|
|
+void Cleanup(std::deque<Pointer>& timers)
|
|
|
+{
|
|
|
+ timers.erase(std::remove_if(timers.begin(), timers.end(), [](Pointer& t){return !t->IsRunning();}), timers.end());
|
|
|
+}
|
|
|
+
|
|
|
+void TestTimerDeque()
|
|
|
+{
|
|
|
+ std::deque<Pointer> timers;
|
|
|
+ StartNewTimer(timers);
|
|
|
+ std::cout << "Count: " << timers.size() << std::endl;
|
|
|
+ StartNewTimer(timers);
|
|
|
+ std::cout << "Count: " << timers.size() << std::endl;
|
|
|
+
|
|
|
+ sleep(5);
|
|
|
+
|
|
|
+ std::cout << "Count: " << timers.size() << std::endl;
|
|
|
+ Cleanup(timers);
|
|
|
+ std::cout << "Count: " << timers.size() << std::endl;
|
|
|
+}
|
|
|
+
|
|
|
+void TestINIReader()
|
|
|
+{
|
|
|
+ PresenceDetection::Util::INIReader reader("PresenceDetection.ini");
|
|
|
+
|
|
|
+ std::string target = reader.Get("PresenceDetection", "Target", "");
|
|
|
+ if (!target.empty())
|
|
|
+ std::cout << "Error" << std::endl;
|
|
|
+ std::cout << target.size() << " " << target << std::endl;
|
|
|
+
|
|
|
+ bool unifi = reader.GetBoolean("PresenceDetection", "UniFi", false);
|
|
|
+ if (unifi)
|
|
|
+ std::cout << "UniFi On" << std::endl;
|
|
|
+ else
|
|
|
+ std::cout << "UniFi Off" << std::endl;
|
|
|
+
|
|
|
+ bool test = reader.GetBoolean("PresenceDetection", "Test", false);
|
|
|
+ if (test)
|
|
|
+ std::cout << "Test On" << std::endl;
|
|
|
+ else
|
|
|
+ std::cout << "Test Off" << std::endl;
|
|
|
+}
|
|
|
+
|
|
|
+int main(int /*argc*/, char** /*argv[]*/)
|
|
|
+{
|
|
|
+ TestINIReader();
|
|
|
|
|
|
return 0;
|
|
|
}
|