/* 
	set BD_ADDR on Ericsson ROK 101 008 using bluez 
	cc -o setbd-bluez setbd-bluez.c -lbluetooth 
	kf_lists[at]digitalmunition[dot]com

	*code mostly stolen from hciconfig*

	Sun Jun 26 14:50:49 EDT 2005

	threat:~# ./setbd-bluez 00:de:ad:00:ba:be
	Using BD_ADDR from command line
	00:de:ad:00:ba:be
	< HCI Command: ogf 0x3f, ocf 0x000d, plen 6
	  BE BA 00 AD DE 00
	> HCI Event: 0x0e plen 4
	  01 0D FC 00
	< HCI Command: ogf 0x3f, ocf 0x0022, plen 0
	> HCI Event: 0x0e plen 4
	  01 22 FC 12
	hci0:   Type: USB
	        BD Address: 00:DE:AD:00:BA:BE ACL MTU: 672:10 SCO MTU: 255:255
	        UP RUNNING PSCAN ISCAN
	        RX bytes:210 acl:0 sco:0 events:26 errors:0
	        TX bytes:370 acl:0 sco:0 commands:27 errors:0

*/

#include <stdio.h>
#include <errno.h>
#include <fcntl.h>
#include <unistd.h>
#include <stdlib.h>
#include <getopt.h>
#include <sys/param.h>
#include <sys/ioctl.h>
#include <sys/socket.h>

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

static void hex_dump(char *pref, int width, unsigned char *buf, int len)
{
        register int i,n;

        for (i = 0, n = 1; i < len; i++, n++) {
                if (n == 1)
                        printf("%s", pref);
                printf("%2.2X ", buf[i]);
                if (n == width) {
                        printf("\n");
                        n = 0;
                }
        }
        if (i && n!=1)
                printf("\n");
}
main(int argc, char **argv)
{
        char buf[HCI_MAX_EVENT_SIZE], *ptr = buf;
        struct hci_filter flt;
        hci_event_hdr *hdr;
        int i, opt, len, dd, dev_id;
        uint16_t ocf;
        uint8_t ogf;

	char *addr;
	
        if (dev_id < 0)
                dev_id = hci_get_route(NULL);

        errno = 0;

        if (argv[1] !=NULL)
        {
                printf("Using BD_ADDR from command line\n");
                addr = argv[1];
		printf("%s\n", addr);
        }
        else
        {
                printf("Using hardcoded BD_ADDR\n");
		addr = "00:01:02:03:04:05";
		printf("%s\n", addr);
        }

        const char *ptr2 = addr;
        int r;
	int m = 5;
        for (r = 0; r < 6; r++,m--) // reverse byte order with m--
	{
                buf[m] = (uint8_t) strtol(ptr2, NULL, 16);

                if (r != 5 && !(ptr2 = strchr(ptr2, ':')))
		{
                        ptr2 = ":00:00:00:00:00";
		}
                ptr2++;
        }

        dd = hci_open_dev(dev_id);
        if (dd < 0) {
                perror("Device open failed");
                exit(EXIT_FAILURE);
        }

        /* Setup filter */
        hci_filter_clear(&flt);
        hci_filter_set_ptype(HCI_EVENT_PKT, &flt);
        hci_filter_all_events(&flt);
        if (setsockopt(dd, SOL_HCI, HCI_FILTER, &flt, sizeof(flt)) < 0) {
                perror("HCI filter setup failed");
                exit(EXIT_FAILURE);
        }

// Place BDADDR in flash
        ogf = 0x3f;
        ocf = 0x00d;
	len = 0x6;

        printf("< HCI Command: ogf 0x%02x, ocf 0x%04x, plen %d\n", ogf, ocf, len);        
	hex_dump("  ", 20, buf, len); fflush(stdout);


        if (hci_send_cmd(dd, ogf, ocf, len, buf) < 0) {
                perror("Send failed");
                exit(EXIT_FAILURE);
        }

        len = read(dd, buf, sizeof(buf));
        if (len < 0) {
                perror("Read failed");
                exit(EXIT_FAILURE);
        }

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

        printf("> HCI Event: 0x%02x plen %d\n", hdr->evt, hdr->plen);
        hex_dump("  ", 20, ptr, len); fflush(stdout);

// write to flash mem
        ocf = 0x022;
	len = 0x0;
        printf("< HCI Command: ogf 0x%02x, ocf 0x%04x, plen %d\n", ogf, ocf, len);        
	hex_dump("  ", 20, buf, len); fflush(stdout);

        if (hci_send_cmd(dd, ogf, ocf, len, buf) < 0) {
                perror("Send failed");
                exit(EXIT_FAILURE);
        }

        len = read(dd, buf, sizeof(buf));
        if (len < 0) {
                perror("Read failed");
                exit(EXIT_FAILURE);
        }

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

        printf("> HCI Event: 0x%02x plen %d\n", hdr->evt, hdr->plen);
        hex_dump("  ", 20, ptr, len); fflush(stdout);

        hci_close_dev(dd);

	// Make sure you chmod +s this binary. =] 
	system("hciconfig hci0 down");
	system("hciconfig hci0 up");
	system("hciconfig hci0");
}