ds4emu.c

#include <stdlib.h>
#include <unistd.h>

#include <sys/ioctl.h>
#include <sys/socket.h>

#include <bluetooth/bluetooth.h>
#include <bluetooth/hci.h>
#include <bluetooth/hci_lib.h>
#include <bluetooth/l2cap.h>
#include <bluetooth/sdp.h>
#include <bluetooth/sdp_lib.h>

#define PSM_SDP 0x01
#define PSM_CTL 0x11
#define PSM_INT 0x13

#define DEV_ID 0
// #define NAME "ds4emu"
#define NAME "Wireless Controller"

void reset(int ctl, int hdev){
	if(ioctl(ctl, HCIDEVRESET, hdev)<0){
		perror("Can't reset.");
		exit(1);
	}
}
void write_class(int dd, int cod){
	if(hci_write_class_of_dev(dd, cod, 2000)<0){
		perror("Can't write local class.");
		exit(1);
	}
}
void change_name(int dd, const char *name){
	if(hci_write_local_name(dd, name, 2000)<0){
		perror("Can't change local name.");
		exit(1);
	}
}
void enable_ssp(int dd, uint8_t flag){
	if(hci_write_simple_pairing_mode(dd, flag, 2000)<0){
		perror("Can't enable Simple Pairing mode.");
		exit(1);
	}
}
void set_inq_parms(int dd, uint16_t interval, uint16_t window, int type){
	// setting interval & window
	struct hci_request rq;
	memset(&rq, 0, sizeof(rq));

	write_inq_activity_cp cp;

	rq.ogf = OGF_HOST_CTL;
	rq.ocf = OCF_WRITE_INQ_ACTIVITY;
	rq.cparam = &cp;
	rq.clen = WRITE_INQ_ACTIVITY_CP_SIZE;

	cp.window = htobs(window);
	cp.interval = htobs(interval);

	if(hci_send_req(dd, &rq, 2000)<0){
		perror("Can't set inq params.");
		exit(1);
	}
	
	// setting type
	if(hci_write_inquiry_scan_type(dd, type, 2000)<0){
		perror("Can't set inq scan type.");
		exit(1);
	}
}
void set_page_parms(int dd, int interval, int window, int type){
	// setting interval & window
	struct hci_request rq;
	memset(&rq, 0, sizeof(rq));

	static write_page_activity_cp cp;

	rq.ogf = OGF_HOST_CTL;
	rq.ocf = OCF_WRITE_PAGE_ACTIVITY;
	rq.cparam = &cp;
	rq.clen = WRITE_PAGE_ACTIVITY_CP_SIZE;

	cp.window = htobs(window);
	cp.interval = htobs(interval);

	if(hci_send_req(dd, &rq, 2000)<0){
		perror("Can't set page params.");
		exit(1);
	}

	// setting type
	// There is not hci_write_page_scan_type in Linux kernel.
	// Why???
	memset(&rq, 0, sizeof(rq));

	static write_inquiry_scan_type_cp cp2;

	rq.ogf = OGF_HOST_CTL;
	rq.ocf = OCF_WRITE_PAGE_SCAN_TYPE;
	rq.cparam = &cp2;
	rq.clen = WRITE_INQUIRY_SCAN_TYPE_CP_SIZE;

	cp2.type = PAGE_SCAN_TYPE_INTERLACED;

	if(hci_send_req(dd, &rq, 2000)<0){
		perror("Can't set page scan type.");
		exit(1);
	}
}
void set_scan(int ctl, int hdev, int opt){
	struct hci_dev_req dr;

	dr.dev_id = hdev;
	dr.dev_opt = opt;

	if(ioctl(ctl, HCISETSCAN, (unsigned long)&dr)<0){
		perror("Can't set scan mode.");
		exit(1);
	}
}
void page_timeout(int dd, uint16_t slot){
	struct hci_request rq;
	memset(&rq, 0, sizeof(rq));

	write_page_timeout_cp cp;

	rq.ogf = OGF_HOST_CTL;
	rq.ocf = OCF_WRITE_PAGE_TIMEOUT;
	rq.cparam = &cp;
	rq.clen = WRITE_PAGE_TIMEOUT_CP_SIZE;

	cp.timeout = htobs(slot);

	if(hci_send_req(dd, &rq, 2000)<0){
		perror("Can't set page timeout.");
		exit(1);
	}
}
void conn_accept_timeout(int dd, uint16_t slot){
	struct hci_request rq;
	memset(&rq, 0, sizeof(rq));

	write_conn_accept_timeout_cp cp;

	rq.ogf = OGF_HOST_CTL;
	rq.ocf = OCF_WRITE_CONN_ACCEPT_TIMEOUT;
	rq.cparam = &cp;
	rq.clen = WRITE_CONN_ACCEPT_TIMEOUT_CP_SIZE;

	cp.timeout = htobs(slot);

	if(hci_send_req(dd, &rq, 2000)<0){
		perror("Can't set connection accept timeout.");
		exit(1);
	}	
}
void write_extended_inq_res(int dd){
	// if you use this function
	// device name will be DUALSHOCK4

	static uint8_t data[] = \
		"\x05\x03\x24\x11\x00\x12\x14\x09\x57\x69\x72\x65\x6c\x65\x73\x73" \
		"\x20\x43\x6f\x6e\x74\x72\x6f\x6c\x6c\x65\x72\x09\x10\x02\x00\x4c" \
		"\x05\xcc\x09\x00\x01\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00" \
		"\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00" \
		"\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00" \
		"\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00" \
		"\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00" \
		"\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00" \
		"\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00" \
		"\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00" \
		"\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00" \
		"\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00" \
		"\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00" \
		"\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00" \
		"\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00";
	;
	uint8_t fec = 1;
	
	if(hci_write_ext_inquiry_response(dd, fec, data, 2000)<0){
		perror("Can't set extended inquiry response.");
		exit(1);
	}
}
void delkey(int dd){
	bdaddr_t bdaddr;

	uint8_t all = 1;
	bacpy(&bdaddr, BDADDR_ANY);

	if(hci_delete_stored_link_key(dd, &bdaddr, all, 1000)<0){
		perror("Can't delete stored link key.");
		exit(1);
	}
}
void set_filter_mask(int dd, uint8_t *mask){
	// all clear
	struct hci_request rq;
	memset(&rq, 0, sizeof(rq));

	static set_event_flt_cp cp1;

	rq.ogf = OGF_HOST_CTL;
	rq.ocf = OCF_SET_EVENT_FLT;
	rq.cparam = &cp1;
	rq.clen = 1;

	cp1.flt_type = FLT_CLEAR_ALL;

	if(hci_send_req(dd, &rq, 2000)<0){
		perror("Can't clear masks.");
		exit(1);
	}

	// set mask
	memset(&rq, 0, sizeof(rq));

	static set_event_mask_cp cp2;

	rq.ogf = OGF_HOST_CTL;
	rq.ocf = OCF_SET_EVENT_MASK;
	rq.cparam = &cp2;
	rq.clen = SET_EVENT_MASK_CP_SIZE;

	for(int i=0; i<8; i++)
		cp2.mask[i] = mask[i];

	if(hci_send_req(dd, &rq, 2000)<0){
		perror("Can't set masks.");
		exit(1);
	}
}
void read_bd_addr(int dd){
	bdaddr_t bdaddr;

	if(hci_read_bd_addr(dd, &bdaddr, 1000)<0){
		perror("Can't read bd_addr.");
		exit(1);
	}
}
void read_local_features(int dd){
	uint8_t features[8];

	if(hci_read_local_features(dd, features, 1000)<0){
		perror("Can't read supported features.");
		exit(1);
	}
}
void read_extended_features(int dd){
	uint8_t features[8];

	if(hci_read_local_ext_features(dd, 1, NULL, features, 1000)<0){
		perror("Can't read supported features.");
		exit(1);
	}	
}
void write_inq_mode(int dd, int mode){
	if(hci_write_inquiry_mode(dd, mode, 2000)<0){
		perror("Can't set inquiry mode.");
		exit(1);
	}
}

// void sdp_init(){
// 	// Do I need to unregister services?

// 	static uint8_t sdp_ds4[] =
// 	{
// 	  0x07, 0x00, 0x01, 0x02, 0xbf, 0x02, 0xbc, 0x36, 0x02, 0xb9, 0x36, 0x02, 0x61, 0x09, 0x00, 0x00,
// 	  0x0a, 0x00, 0x01, 0x00, 0x01, 0x09, 0x00, 0x01, 0x35, 0x03, 0x19, 0x11, 0x24, 0x09, 0x00, 0x04,
// 	  0x35, 0x0d, 0x35, 0x06, 0x19, 0x01, 0x00, 0x09, 0x00, 0x11, 0x35, 0x03, 0x19, 0x00, 0x11, 0x09,
// 	  0x00, 0x06, 0x35, 0x09, 0x09, 0x65, 0x6e, 0x09, 0x00, 0x6a, 0x09, 0x01, 0x00, 0x09, 0x00, 0x09,
// 	  0x35, 0x08, 0x35, 0x06, 0x19, 0x11, 0x24, 0x09, 0x01, 0x00, 0x09, 0x00, 0x0d, 0x35, 0x0f, 0x35,
// 	  0x0d, 0x35, 0x06, 0x19, 0x01, 0x00, 0x09, 0x00, 0x13, 0x35, 0x03, 0x19, 0x00, 0x11, 0x09, 0x01,
// 	  0x00, 0x25, 0x13, 0x57, 0x69, 0x72, 0x65, 0x6c, 0x65, 0x73, 0x73, 0x20, 0x43, 0x6f, 0x6e, 0x74,
// 	  0x72, 0x6f, 0x6c, 0x6c, 0x65, 0x72, 0x09, 0x01, 0x01, 0x25, 0x0f, 0x47, 0x61, 0x6d, 0x65, 0x20,
// 	  0x43, 0x6f, 0x6e, 0x74, 0x72, 0x6f, 0x6c, 0x6c, 0x65, 0x72, 0x09, 0x01, 0x02, 0x25, 0x1b, 0x53,
// 	  0x6f, 0x6e, 0x79, 0x20, 0x43, 0x6f, 0x6d, 0x70, 0x75, 0x74, 0x65, 0x72, 0x20, 0x45, 0x6e, 0x74,
// 	  0x65, 0x72, 0x74, 0x61, 0x69, 0x6e, 0x6d, 0x65, 0x6e, 0x74, 0x09, 0x02, 0x00, 0x09, 0x01, 0x00,
// 	  0x09, 0x02, 0x01, 0x09, 0x01, 0x11, 0x09, 0x02, 0x02, 0x08, 0x08, 0x09, 0x02, 0x03, 0x08, 0x00,
// 	  0x09, 0x02, 0x04, 0x28, 0x00, 0x09, 0x02, 0x05, 0x28, 0x01, 0x09, 0x02, 0x06, 0x36, 0x01, 0x6c,
// 	  0x36, 0x01, 0x69, 0x08, 0x22, 0x26, 0x01, 0x64, 0x05, 0x01, 0x09, 0x05, 0xa1, 0x01, 0x85, 0x01,
// 	  0x09, 0x30, 0x09, 0x31, 0x09, 0x32, 0x09, 0x35, 0x15, 0x00, 0x26, 0xff, 0x00, 0x75, 0x08, 0x95,
// 	  0x04, 0x81, 0x02, 0x09, 0x39, 0x15, 0x00, 0x25, 0x07, 0x75, 0x04, 0x95, 0x01, 0x81, 0x42, 0x05,
// 	  0x09, 0x19, 0x01, 0x29, 0x0e, 0x15, 0x00, 0x25, 0x01, 0x75, 0x01, 0x95, 0x0e, 0x81, 0x02, 0x75,
// 	  0x06, 0x95, 0x01, 0x81, 0x01, 0x05, 0x01, 0x09, 0x33, 0x09, 0x34, 0x15, 0x00, 0x26, 0xff, 0x00,
// 	  0x75, 0x08, 0x95, 0x02, 0x81, 0x02, 0x06, 0x04, 0xff, 0x85, 0x02, 0x09, 0x24, 0x95, 0x24, 0xb1,
// 	  0x02, 0x85, 0xa3, 0x09, 0x25, 0x95, 0x30, 0xb1, 0x02, 0x85, 0x05, 0x09, 0x26, 0x95, 0x28, 0xb1,
// 	  0x02, 0x85, 0x06, 0x09, 0x27, 0x95, 0x34, 0xb1, 0x02, 0x85, 0x07, 0x09, 0x28, 0x95, 0x30, 0xb1,
// 	  0x02, 0x85, 0x08, 0x09, 0x29, 0x95, 0x2f, 0xb1, 0x02, 0x06, 0x03, 0xff, 0x85, 0x03, 0x09, 0x21,
// 	  0x95, 0x26, 0xb1, 0x02, 0x85, 0x04, 0x09, 0x22, 0x95, 0x2e, 0xb1, 0x02, 0x85, 0xf0, 0x09, 0x47,
// 	  0x95, 0x3f, 0xb1, 0x02, 0x85, 0xf1, 0x09, 0x48, 0x95, 0x3f, 0xb1, 0x02, 0x85, 0xf2, 0x09, 0x49,
// 	  0x95, 0x0f, 0xb1, 0x02, 0x06, 0x00, 0xff, 0x85, 0x11, 0x09, 0x20, 0x15, 0x00, 0x26, 0xff, 0x00,
// 	  0x75, 0x08, 0x95, 0x4d, 0x81, 0x02, 0x09, 0x21, 0x91, 0x02, 0x85, 0x12, 0x09, 0x22, 0x95, 0x8d,
// 	  0x81, 0x02, 0x09, 0x23, 0x91, 0x02, 0x85, 0x13, 0x09, 0x24, 0x95, 0xcd, 0x81, 0x02, 0x09, 0x25,
// 	  0x91, 0x02, 0x85, 0x14, 0x09, 0x26, 0x96, 0x0d, 0x01, 0x81, 0x02, 0x09, 0x27, 0x91, 0x02, 0x85,
// 	  0x15, 0x09, 0x28, 0x96, 0x4d, 0x01, 0x81, 0x02, 0x09, 0x29, 0x91, 0x02, 0x85, 0x16, 0x09, 0x2a,
// 	  0x96, 0x8d, 0x01, 0x81, 0x02, 0x09, 0x2b, 0x91, 0x02, 0x85, 0x17, 0x09, 0x2c, 0x96, 0xcd, 0x01,
// 	  0x81, 0x02, 0x09, 0x2d, 0x91, 0x02, 0x85, 0x18, 0x09, 0x2e, 0x96, 0x0d, 0x02, 0x81, 0x02, 0x09,
// 	  0x2f, 0x91, 0x02, 0x85, 0x19, 0x09, 0x30, 0x96, 0x22, 0x02, 0x81, 0x02, 0x09, 0x31, 0x91, 0x02,
// 	  0x06, 0x80, 0xff, 0x85, 0x82, 0x09, 0x22, 0x95, 0x3f, 0xb1, 0x02, 0x85, 0x83, 0x09, 0x23, 0xb1,
// 	  0x02, 0x85, 0x84, 0x09, 0x24, 0xb1, 0x02, 0x85, 0x90, 0x09, 0x30, 0xb1, 0x02, 0x85, 0x91, 0x09,
// 	  0x31, 0xb1, 0x02, 0x85, 0x92, 0x09, 0x32, 0xb1, 0x02, 0x85, 0x93, 0x09, 0x33, 0xb1, 0x02, 0x85,
// 	  0xa0, 0x09, 0x40, 0xb1, 0x02, 0x85, 0xa4, 0x09, 0x44, 0xb1, 0x02, 0xc0, 0x09, 0x02, 0x07, 0x35,
// 	  0x08, 0x35, 0x06, 0x09, 0x04, 0x09, 0x09, 0x01, 0x00, 0x09, 0x02, 0x08, 0x28, 0x00, 0x09, 0x02,
// 	  0x09, 0x28, 0x01, 0x09, 0x02, 0x0a, 0x28, 0x01, 0x09, 0x02, 0x0b, 0x09, 0x01, 0x00, 0x09, 0x02,
// 	  0x0c, 0x09, 0x1f, 0x40, 0x09, 0x02, 0x0d, 0x28, 0x00, 0x09, 0x02, 0x0e, 0x28, 0x00, 0x36, 0x00,
// 	  0x52, 0x09, 0x00, 0x00, 0x0a, 0x00, 0x01, 0x00, 0x02, 0x09, 0x00, 0x01, 0x35, 0x03, 0x19, 0x12,
// 	  0x00, 0x09, 0x00, 0x04, 0x35, 0x0d, 0x35, 0x06, 0x19, 0x01, 0x00, 0x09, 0x00, 0x01, 0x35, 0x03,
// 	  0x19, 0x00, 0x01, 0x09, 0x00, 0x09, 0x35, 0x08, 0x35, 0x06, 0x19, 0x12, 0x00, 0x09, 0x01, 0x03,
// 	  0x09, 0x02, 0x00, 0x09, 0x01, 0x03, 0x09, 0x02, 0x01, 0x09, 0x05, 0x4c, 0x09, 0x02, 0x02, 0x09,
// 	  0x05, 0xc4, 0x09, 0x02, 0x03, 0x09, 0x01, 0x00, 0x09, 0x02, 0x04, 0x28, 0x01, 0x09, 0x02, 0x05,
// 	  0x09, 0x00, 0x02, 0x00
// 	};

// 	sdp_session_t *session = sdp_connect(BDADDR_ANY, BDADDR_LOCAL, 0);

// 	uint32_t handle;
// 	uint8_t flags = 0;
// 	sdp_device_record_register_binary(session, BDADDR_ANY, sdp_ds4, sizeof(sdp_ds4), flags, &handle);

// 	printf("Handle: %x\n", handle);
// }

void setup(){
	int ctl = socket(AF_BLUETOOTH, SOCK_RAW, BTPROTO_HCI);
	if(ctl < 0){
		perror("Can't open HCI socket.");
		exit(1);
	}
	int hdev = DEV_ID;
	int dd = hci_open_dev(hdev);
	if(dd < 0){
		perror("Can't open device.");
		exit(1);
	}

	reset(ctl, hdev);
	read_bd_addr(dd);
	read_local_features(dd);
	// read_buff_size();
	write_class(dd, 0x002508);
	change_name(dd, NAME);
	
	write_extended_inq_res(dd);
	delkey(dd);
	uint8_t mask[8] = {0xff, 0x9f, 0xfb, 0x7f, 0x07, 0xf8, 0xbf, 0x1d};
	set_filter_mask(dd, mask);
	write_inq_mode(dd, 2);
	page_timeout(dd, 24000);
	conn_accept_timeout(dd, 46400);
	enable_ssp(dd, 1);
	read_extended_features(dd);
	set_inq_parms(dd, 108, 18, 1);
	set_page_parms(dd, 180, 18, 1);
	
	set_scan(ctl, hdev, SCAN_DISABLED);
	set_scan(ctl, hdev, SCAN_PAGE|SCAN_INQUIRY);

	hci_close_dev(dd);
	close(ctl);
}

void initial_connection(){
	// connect with sdp and setting timeout
	struct sockaddr_l2 loc_addr = {}, rem_addr = {};
	int opt = sizeof(rem_addr);
	char buf[1024] = {};

	int s = socket(AF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);

	memset(&loc_addr, 0, sizeof(loc_addr));
	loc_addr.l2_family = AF_BLUETOOTH;
	loc_addr.l2_bdaddr = *BDADDR_ANY;
	loc_addr.l2_bdaddr_type = BDADDR_BREDR;
	loc_addr.l2_psm = htobs(PSM_SDP);
	
	bind(s, (struct sockaddr *)&loc_addr, sizeof(loc_addr));
	listen(s, 1);

	printf("accepting sdp...\n");
	int csdp = accept(s, (struct sockaddr *)&rem_addr, &opt);
	
	ba2str(&rem_addr.l2_bdaddr, buf);
	fprintf(stderr, "%s\n", buf);

	memset(buf, 0, sizeof(buf));
	printf("getting sdp request...\n");
	int bytes_read;

	bytes_read = recv(csdp, buf, sizeof(buf), 0);
	printf("%d\n", bytes_read);

	// printf("sending sdp...\n");
	// int x = send(csdp, sdp_ds4, sizeof(sdp_ds4), 0);
	
}


int main(){
	setup();
	// sdp_init();

	initial_connection();


	sleep(1000000);

	
	return 0;
}