#!/usr/bin/python

import serial,os,sys,struct

s = serial.Serial(0,115200)
s.open()

HCI_COMMAND = 1
HCI_ACL = 2
HCI_SCO = 3
HCI_EVENT = 4

HCI_G_LINKCONTROL = 1
HCI_G_LINKPOLICY = 2
HCI_G_CONTROLLER = 3
HCI_G_INFORMATIONAL = 4
HCI_G_STATUS = 5
HCI_G_TESTING = 6

HCI_C_RESET = 0x0003
HCI_C_WRITE_SCAN_ENABLE = 0x001a

HCI_LC_CONNECT = 0x0005


def send_packet(ptype,data):
	s.write(chr(ptype))
	s.write(data)

def send_cmd(ogf,ocf,pdesc,*parms):
	ogf &= 0x3F
	ocf &= 0x3FF
	cmd = ocf | (ogf<<10)
	pdata = struct.pack(pdesc,*parms)
	print ("<[CMD] ogf %d ocf %03X cmd %04X len %d data"%(ogf,ocf,cmd,len(pdata))),
	for i in pdata:
		print "%02X"%ord(i),
	print ""
	send_packet(HCI_COMMAND,struct.pack("HB",cmd,len(pdata))+pdata)
	
def send_acl(ch,pb,bc,adata):
	dlen = len(adata)
	print ("<[ACL] ch %04X pb %d bc %d len %d data"%(ch,pb,bc,dlen)),
	for i in adata:
		print "%02X"%ord(i),
	print ""
	ch |= (pb<<12)
	ch |= (bc<<14)
	send_packet(HCI_ACL,struct.pack("HH",ch,len(adata))+adata)

def send_l2cap(ach,cid,payload):
	hdr=struct.pack("HH",len(payload),cid)
	send_acl(ach,2,0,hdr+payload)

def receive_packet():
	ptype = ord(s.read(1))
	if ptype == HCI_EVENT:
		ecode,plen = struct.unpack("BB",s.read(2))
		pdata = s.read(plen)
		print (">[EVENT] code 0x%02X len %d data"%(ecode,plen)),
		for i in pdata:
			print "%02X"%ord(i),
		print ""
		if ecode==0x13:
			return receive_packet()
		return (ptype,ecode,plen,pdata)
	elif ptype == HCI_ACL:
		ahdr,dlen =  struct.unpack("HH",s.read(4))
		adata = s.read(dlen)
		ch = ahdr&0xFFF
		pb = (ahdr>>12)&3
		bc = (ahdr>>14)&3
		print (">[ACL] ch %04X pb %d bc %d len %d data"%(ch,pb,bc,dlen)),
		for i in adata:
			print "%02X"%ord(i),
		print ""
		return (ch,pb,bc,dlen,adata)
	else:
		print "WTF!!!!!!!! %d"%ptype

#x = receive_packet()
send_cmd(HCI_G_CONTROLLER,HCI_C_RESET,"")
x = receive_packet()
#send_cmd(HCI_G_CONTROLLER,HCI_C_WRITE_SCAN_ENABLE,"B",3)
#x = receive_packet()
send_cmd(HCI_G_LINKCONTROL,HCI_LC_CONNECT,"6sHBBHB","\x65\x37\x33\xAB\x17\x00",0x0008,2,0,0,0)
x = receive_packet()
pt,ec,pl,pd = receive_packet()
ch = struct.unpack("H",pd[1:3])[0]
send_l2cap(ch,1,struct.pack("BBHHH",2,1,4,19,0x41))
x = receive_packet()
x,x,x,x,pd = receive_packet()
dcid = struct.unpack("H",pd[8:10])[0]
send_l2cap(ch,1,struct.pack("BBHHH",4,1,4,dcid,0))
x = receive_packet()
x = receive_packet()
send_l2cap(ch,1,struct.pack("BBHHHH",5,1,6,dcid,0,0))
while True:
	x = receive_packet()