ibeacon.c

ibeacon.c

#include <stdlib.h>
#include <errno.h>
#include <unistd.h>
#include <ctype.h>
#include <signal.h>
#include <stdio.h>
#include <stdbool.h>

#include <sys/time.h>
#include <sys/types.h>

#include <bluetooth/bluetooth.h>
#include <bluetooth/hci.h>
#include <bluetooth/hci_lib.h>

#define cmd_opcode_pack(ogf, ocf) (uint16_t)((ocf & 0x03ff)|(ogf << 10))

#define EIR_FLAGS                   0X01
#define EIR_NAME_SHORT              0x08
#define EIR_NAME_COMPLETE           0x09
#define EIR_MANUFACTURE_SPECIFIC    0xFF

#define ADV_INTERVAL_MS             200
#define TOGGLE_INTVAL_S             1
#define TOGGLE_INTVAL_US            500000

#define DEBUG
#ifdef DEBUG
  #define dbg(fmt, ...) \
   do { \
      fprintf(stderr, "[iBeaconADV]: "); \
      fprintf(stderr, fmt, ##__VA_ARGS__); \
   } while(0) 
#endif

typedef enum {
  MON_UNKNOWN = 0,
  MON_TIMEOUT,
  MON_CONNECTED,
  MON_DISCONNECTED,
  MON_INTERRUPTED,
  MON_NOTHING,
} monitor;

static volatile int signal_received = 0;

static void sigint_handler(int sig)
{
	signal_received = sig;
}

static unsigned int twoc(int in, int t)
{
  return (in < 0) ? (in + (2 << (t-1))) : in;
}

static int set_advertising_data(le_set_advertising_data_cp *adv_data) 
{
  int device_id = hci_get_route(NULL);

  int device_handle = 0;
  if((device_handle = hci_open_dev(device_id)) < 0)
  {
    perror("Could not open device");
    exit(1);
  }

  uint8_t status;
  struct hci_request rq;
  memset(&rq, 0, sizeof(rq));
  rq.ogf = OGF_LE_CTL;
  rq.ocf = OCF_LE_SET_ADVERTISING_DATA;
  rq.cparam = adv_data;
  rq.clen = LE_SET_ADVERTISING_DATA_CP_SIZE;
  rq.rparam = &status;
  rq.rlen = 1;

  int ret = 0;

  ret = hci_send_req(device_handle, &rq, 1000);

  if (ret < 0)
  {
    hci_close_dev(device_handle);
    dbg("Can't send request %s (%d)\n", 
        strerror(errno), errno);
    return(1);
  }

  hci_close_dev(device_handle);

  if(ret < 0)
  {
    dbg("Can't send request %s (%d)\n", strerror(errno), errno);
    return(1);
  }

  if (status) 
  {
    dbg("LE set advertising data returned status %d\n", status);
    return(1);
  }

  return ret;
}

static int enable_advertising(int advertising_interval)
{
  int device_id = hci_get_route(NULL);

  int device_handle = 0;
  if((device_handle = hci_open_dev(device_id)) < 0)
  {
    perror("Could not open device");
    exit(1);
  }

  le_set_advertising_parameters_cp adv_params_cp;
  memset(&adv_params_cp, 0, sizeof(adv_params_cp));
  adv_params_cp.min_interval = htobs(advertising_interval);
  adv_params_cp.max_interval = htobs(advertising_interval);
  adv_params_cp.chan_map = 7;

  uint8_t status;
  struct hci_request rq;
  memset(&rq, 0, sizeof(rq));
  rq.ogf = OGF_LE_CTL;
  rq.ocf = OCF_LE_SET_ADVERTISING_PARAMETERS;
  rq.cparam = &adv_params_cp;
  rq.clen = LE_SET_ADVERTISING_PARAMETERS_CP_SIZE;
  rq.rparam = &status;
  rq.rlen = 1;

  int ret = hci_send_req(device_handle, &rq, 1000);
  if (ret < 0)
  {
    hci_close_dev(device_handle);
    dbg("Can't send request %s (%d)\n", 
				strerror(errno), errno);
    return(1);
  }

  le_set_advertise_enable_cp advertise_cp;
  memset(&advertise_cp, 0, sizeof(advertise_cp));
  advertise_cp.enable = 0x01;

  memset(&rq, 0, sizeof(rq));
  rq.ogf = OGF_LE_CTL;
  rq.ocf = OCF_LE_SET_ADVERTISE_ENABLE;
  rq.cparam = &advertise_cp;
  rq.clen = LE_SET_ADVERTISE_ENABLE_CP_SIZE;
  rq.rparam = &status;
  rq.rlen = 1;

  ret = hci_send_req(device_handle, &rq, 1000);

  if (ret < 0)
  {
    hci_close_dev(device_handle);
    dbg("Can't send request %s (%d)\n", 
				strerror(errno), errno);
    return(1);
  }

  hci_close_dev(device_handle);

  if(ret < 0)
  {
    dbg("Can't send request %s (%d)\n", strerror(errno), errno);
    return(1);
  }

  if (status) 
  {
    dbg("LE enable advertise returned status %d\n", status);
    return(1);
  }

  dbg("Advertising enabled\n");
  return ret;
}

static int disable_advertising()
{
  int device_id = hci_get_route(NULL);

  int device_handle = 0;
  if((device_handle = hci_open_dev(device_id)) < 0)
  {
    perror("Could not open device");
    return(1);
  }

  le_set_advertise_enable_cp advertise_cp;
  uint8_t status;

  memset(&advertise_cp, 0, sizeof(advertise_cp));

  struct hci_request rq;
  memset(&rq, 0, sizeof(rq));
  rq.ogf = OGF_LE_CTL;
  rq.ocf = OCF_LE_SET_ADVERTISE_ENABLE;
  rq.cparam = &advertise_cp;
  rq.clen = LE_SET_ADVERTISE_ENABLE_CP_SIZE;
  rq.rparam = &status;
  rq.rlen = 1;

  int ret = hci_send_req(device_handle, &rq, 1000);

  hci_close_dev(device_handle);

  if (ret < 0) 
  {
    dbg("Can't set advertise mode: %s (%d)\n", 
		strerror(errno), errno);
    return(1);
  }

  if (status) 
  {
    dbg("LE set advertise enable on returned status %d\n", status);
    return(1);
  }

  dbg("Advertising disabled\n");
  return ret;
}

monitor monitor_event(int dd)
{
	unsigned char buf[HCI_MAX_EVENT_SIZE], *ptr;
	struct hci_filter nf, of;
	struct sigaction sa;
	socklen_t olen;
	int len; bool is_connected = false;
  monitor mon = MON_NOTHING;
	
	olen = sizeof(of);
	if (getsockopt(dd, SOL_HCI, HCI_FILTER, &of, &olen) < 0) {
		perror("Could not get socket options");	
		return -1;
	}	

	hci_filter_clear(&nf);
	hci_filter_set_ptype(HCI_EVENT_PKT, &nf);
	hci_filter_set_event(EVT_DISCONN_COMPLETE, &nf);
	hci_filter_set_event(EVT_LE_META_EVENT, &nf);

	if (setsockopt(dd, SOL_HCI, HCI_FILTER, &nf, sizeof(nf)) < 0) {
		perror("Could not set socket options");
		return -1;
	}

	memset(&sa, 0, sizeof(sa));
	sa.sa_flags = SA_NOCLDSTOP;
	sa.sa_handler = sigint_handler;
	sigaction(SIGINT, &sa, NULL);

	int nfds, ret;
	fd_set rfds;
  
	nfds = 0;
	FD_ZERO(&rfds);

  struct timeval tv = { TOGGLE_INTVAL_S, TOGGLE_INTVAL_US };

  do {
    FD_SET(dd, &rfds); nfds = dd;
		hci_event_hdr *hdr; len = 0;
		evt_le_meta_event *meta;
		char addr[18];
    mon = MON_NOTHING;

    ret = select(nfds + 1, &rfds, NULL, NULL, &tv);

    if (ret == 0) {
      mon = MON_TIMEOUT;
      if (is_connected) continue;
      goto done;
    }

    if (errno == EINTR && signal_received == SIGINT) {
      mon = MON_INTERRUPTED;
      goto done;
    }

    if (ret == -1) {
      dbg("select() error %d\n", errno);
      goto done;
    } else {
      if (FD_ISSET(dd, &rfds)) {
        len = read(dd, buf, sizeof(buf));

        if (len < 0) {
          if (errno == EAGAIN || errno == EINTR)
            continue;
          dbg("Error happened during read(), errno: %d\n", errno);
          goto done;
        }

        hdr = (void *)(buf + 1);
        ptr = buf + (1 + HCI_EVENT_HDR_SIZE);
        len -= (1 + HCI_EVENT_HDR_SIZE);

        switch (hdr->evt) {
          case EVT_LE_META_EVENT:
            meta = (void *) ptr;			
            if (meta->subevent == EVT_LE_CONN_COMPLETE) {
              evt_le_connection_complete *leconn = 
                    (evt_le_connection_complete *)(meta->data + 1); 
              ba2str(&leconn->peer_bdaddr, addr);
              dbg("Connected with %s\n", addr);
            }
            mon = MON_CONNECTED; is_connected = true;
            break;
          case EVT_DISCONN_COMPLETE:
            mon = MON_DISCONNECTED; // disconnected
            dbg("Disconnected.\n");
            is_connected = false;
            goto done;
          default:
            break;
        }
      }
    }
  } while (1);

done: 
	setsockopt(dd, SOL_HCI, HCI_FILTER, &of, sizeof(of));
	
	if (len < 0) return MON_UNKNOWN;
	return mon;
}

int main(void)
{
  le_set_advertising_data_cp dtrans_svc_ad = {
    .length = 19,
    .data = {
      /* advertising flags */
      htobs(0x02), htobs(0x01), htobs(0x1a),
      /* Complete Name */
      htobs(0x0b), htobs(0x09), 
      htobs('C'), htobs('C'), htobs('J'), htobs('C'), 
      htobs('0'), htobs('6'), htobs('C'), htobs('9'), 
      htobs('8'), htobs('D'),
      /* Data Transfer Service UUID (16-bit) */
      htobs(0x03), htobs(0x02), htobs(0xf0), htobs(0xa0)
    }
  };

  le_set_advertising_data_cp ibeacon_ad = {
    .length = 30,
    .data = {
      /* advertising flags (General Connectable) */
      htobs(0x02), htobs(0x01), htobs(0x1a),
      /* ibeacon prefix */
      htobs(0x1a), htobs(0xff), htobs(0x4c), 
      htobs(0x00), htobs(0x02), htobs(0x15), 
      /* ibeacon uuid (128-bit) */
      htobs(0xea), htobs(0xa3), htobs(0xa5), htobs(0x9a), 
      htobs(0x8a), htobs(0x66), htobs(0x42), htobs(0xad),
      htobs(0x96), htobs(0xe2), htobs(0xe5), htobs(0xad), 
      htobs(0x0d), htobs(0xda), htobs(0x34), htobs(0xeb),
      /* major & minor */
      htobs(0x00), htobs(0x01), // major
      htobs(0x00), htobs(0x01), // minor
      htobs(twoc(-59, 8)) // rssi (-59)
    }
  };

  int rc = 0; bool toggle = true;
  monitor mon = MON_DISCONNECTED;

  /* initialize with ibeacon_ad */
  le_set_advertising_data_cp *adv_data = &ibeacon_ad;

	int device_id = hci_get_route(NULL);
	int dd = 0;
	if ((dd = hci_open_dev(device_id)) < 0) {
		dbg("Could not open device");
		return 1;
	}	
	
  do {
    switch (mon) {
      case MON_TIMEOUT:
        toggle = !toggle;
        adv_data = toggle ? &ibeacon_ad : &dtrans_svc_ad;
        break;
      case MON_DISCONNECTED:
        dbg("Enabling advertisement..\n");
        rc = enable_advertising(ADV_INTERVAL_MS); //interval
        break;
      default:
        dbg("iBeacon: Unknown monitor state\n");
        break;
    }
    if (!rc && mon != MON_UNKNOWN) 
      rc = set_advertising_data(adv_data);
	 if (rc) break;
  } while ((mon = monitor_event(dd)) != MON_INTERRUPTED);

  hci_close_dev(dd);

  disable_advertising();

  return rc;
}