static void io_acl_data(void *data)
{
struct vhci_conn *conn = data;
unsigned char buf[HCI_MAX_FRAME_SIZE], *ptr;
hci_acl_hdr *ah;
uint16_t flags;
int len;
ptr = buf + 1;
if (read_n(conn->fd, ptr, HCI_ACL_HDR_SIZE) <= 0) {
close_connection(conn);
return;
}
ah = (void *) ptr;
ptr += HCI_ACL_HDR_SIZE;
len = btohs(ah->dlen);
if (read_n(conn->fd, ptr, len) <= 0) {
close_connection(conn);
return;
}
buf[0] = HCI_ACLDATA_PKT;
flags = acl_flags(btohs(ah->handle));
ah->handle = htobs(acl_handle_pack(conn->handle, flags));
len += HCI_ACL_HDR_SIZE + 1;
write_snoop(vdev.dd, HCI_ACLDATA_PKT, 1, buf, len);
if (write(vdev.dev_fd, buf, len) < 0)
syslog(LOG_ERR, "ACL data write error");
}
void packet_hci_acldata(struct timeval *tv, uint16_t index, bool in,
const void *data, uint16_t size)
{
const hci_acl_hdr *hdr = data;
uint16_t handle = btohs(hdr->handle);
uint16_t dlen = btohs(hdr->dlen);
uint8_t flags = acl_flags(handle);
btsnoop_write(tv, index, in ? 0x01 : 0x00, data, size);
print_header(tv, index);
if (size < HCI_ACL_HDR_SIZE) {
printf("* Malformed ACL Data %s packet\n", in ? "RX" : "TX");
return;
}
printf("%c ACL Data: handle %d flags 0x%2.2x dlen %d\n",
in ? '>' : '<', acl_handle(handle), flags, dlen);
data += HCI_ACL_HDR_SIZE;
size -= HCI_ACL_HDR_SIZE;
if (filter_mask & PACKET_FILTER_SHOW_ACL_DATA)
packet_hexdump(data, size);
}
void hci_dump(struct frame *frm)
{
__u8 type = *(__u8 *)frm->ptr;
frm->ptr++; frm->len--;
if(type == HCI_ACLDATA_PKT)
{
hci_acl_hdr *hdr = (void *) frm->ptr;
__u16 handle = btohs(hdr->handle);
__u16 dlen = btohs(hdr->dlen);
__u8 flags = acl_flags(handle);
printf("ACL data: handle 0x%4.4x flags 0x%2.2x dlen %d\n",
acl_handle(handle), flags, dlen);
}
}
static gboolean io_acl_data(GIOChannel *chan, GIOCondition cond, gpointer data)
{
struct vhci_conn *conn = (struct vhci_conn *) data;
unsigned char buf[HCI_MAX_FRAME_SIZE], *ptr;
hci_acl_hdr *ah;
uint16_t flags;
int fd, err, len;
if (cond & G_IO_NVAL) {
g_io_channel_unref(chan);
return FALSE;
}
if (cond & G_IO_HUP) {
close_connection(conn);
return FALSE;
}
fd = g_io_channel_unix_get_fd(chan);
ptr = buf + 1;
if (read_n(fd, ptr, HCI_ACL_HDR_SIZE) <= 0) {
close_connection(conn);
return FALSE;
}
ah = (void *) ptr;
ptr += HCI_ACL_HDR_SIZE;
len = btohs(ah->dlen);
if (read_n(fd, ptr, len) <= 0) {
close_connection(conn);
return FALSE;
}
buf[0] = HCI_ACLDATA_PKT;
flags = acl_flags(btohs(ah->handle));
ah->handle = htobs(acl_handle_pack(conn->handle, flags));
len += HCI_ACL_HDR_SIZE + 1;
write_snoop(vdev.dd, HCI_ACLDATA_PKT, 1, buf, len);
err = write(vdev.fd, buf, len);
return TRUE;
}
int bu_hci::on_sock_data(uint8_t code, const sdata& buffer) //received
{
uint8_t eventType = buffer.data[0];
uint16_t blen = buffer.len;
std::string scase="NOT HANDLED ";
bybuff trace(buffer.data, buffer.len);
TRACE("{-->["<< int(buffer.len) <<"]"<< trace.to_string());
if (HCI_EVENT_PKT == eventType)
{
uint8_t subEventType = buffer.data[1];
//_TRACE(" Event:" << int(eventType) << ", subevent:" << int(subEventType));
switch(subEventType)
{
case EVT_DISCONN_COMPLETE:
scase="EVT_DISCONN_COMPLETE";
{
evt_disconn_complete* pdc = (evt_disconn_complete*)(buffer.data+4);
pdc->handle = htobs(pdc->handle);
memcpy(&_dcached, pdc, sizeof(_dcached));
#ifdef ACL_MTU_FRAG
flush_acl();
#endif //ACL_MTU_FRAG
_clear();
_pev->on_disconnect(pdc);
_connected=false;
}
break;
case EVT_ENCRYPT_CHANGE:
scase="EVT_ENCRYPT_CHANGE";
{
evt_encrypt_change* pec = (evt_encrypt_change*)(buffer.data+4);
pec->handle=htobs(pec->handle);
_pev->on_encrypt_chnage(pec);
}
break;
case EVT_CMD_COMPLETE:
scase=" [EVT_CMD_COMPLETE]";
{
no_evt_cmd_complete* necc = (no_evt_cmd_complete*)(buffer.data+3);
necc->cmd=htobs(necc->cmd);
this->_oncmd_complette(necc);
}
break;
case EVT_LE_META_EVENT:
scase="EVT_LE_META_EVENT";
{
no_evt_le_meta_event* pev = (no_evt_le_meta_event*)(buffer.data+3);
this->_onmeta(pev);
}
break;
case EVT_CONN_REQUEST:
scase="EVT_CONN_REQUEST";
{
evt_conn_request* preq= (evt_conn_request*)(buffer.data+4);
bdaddr_t dest;
baswap(&dest, &preq->bdaddr);
memcpy(&preq->bdaddr,&dest,sizeof(dest));
}
break;
case EVT_CMD_STATUS: //OCF_AUTH_REQUESTED
scase="EVT_CMD_STATUS";
{
evt_cmd_status* pevs = (evt_cmd_status*)(buffer.data+4);
pevs->opcode = htobs(pevs->opcode);
uint16_t ogf = CMD_OPCODE_OGF(pevs->opcode);
uint16_t ocf = CMD_OPCODE_OCF(pevs->opcode);
TRACE("CMD_STATUS status:" <<int(pevs->status)<<" ncmd:" <<
int(pevs->ncmd) << " opcode(C/G):" <<
std::hex<<int(ocf) <<"/"<<int(ogf) << std::dec);
if(ocf == OCF_EXIT_PERIODIC_INQUIRY)
{
//send_cmd(OCF_INQUIRY_CANCEL, OGF_LINK_CTL,0,0);
}
}
break;
case EVT_REMOTE_NAME_REQ_COMPLETE:
scase="EVT_REMOTE_NAME_REQ_COMPLETE";
{
evt_remote_name_req_complete* pnc = (evt_remote_name_req_complete*)(buffer.data+4);
TRACE("remote name: " << pnc->name);
}
break;
#ifdef ACL_MTU_FRAG
case EVT_NUM_COMP_PKTS:
scase="EVT_NUM_COMP_PKTS";
{
uint8_t nhandles = uint8_t(buffer.data[3]);
// TRACE("GOT number of completted acl packets:" << int(nhandles));
for(uint8_t h=0; h<nhandles; h++)
{
no_acl_handler_packet* pconfirm = (no_acl_handler_packet*)(buffer.data + 4 + (h*4));
pconfirm->handler = htobs(pconfirm->handler);
pconfirm->packet = htobs(pconfirm->packet);
// TRACE("GOT Pending handler:" << int(pconfirm->handler) << ", " << int(pconfirm->packet));
const auto& ah = _aclPending.find(pconfirm->handler);
if(ah == this->_aclPending.end())
{
// TRACE("HANDLER "<< int(pconfirm->handler) <<" NOT FOUND");
continue;
}
else
{
ah->second -= pconfirm->packet;
if(ah->second <= 0)
{
// TRACE("DELETE ALL HANDLER packets:" <<",["<<int(pconfirm->handler) <<"]" << int(ah->second));
_erase_AclOut(ah->first);
}
}
}
this->flush_acl();
}
break;
#endif //ACL_MTU_FRAG
default:
break;
}//switch
}
else if (HCI_ACLDATA_PKT == eventType)
{
uint16_t val = oa2t<uint16_t>(buffer.data,1);
uint16_t flags = acl_flags(val);
uint16_t handle = acl_handle(val);
if(ACL_START_NO_FLUSH == flags)
{
//_clear_aclInQueue(); TODO
scase="ACL_START_NO_FLUSH";
flags = ACL_START;
}
if (ACL_START == flags)
{
uint16_t cid = oa2t<uint16_t>(buffer.data,7);
uint16_t expectedlen = oa2t<uint16_t>(buffer.data,5);
uint16_t chunklen = blen-9;
sdata sd;
scase="ACL_START";
sd.len = expectedlen;
sd.data = buffer.data + 9;
if (expectedlen == chunklen)
{
_pev->on_acl_packet(handle,cid, sd);
}
else
{
/// assert(_aclIn.find(handle) == _aclIn.end());
//accumulate data bt handler
if(_aclIn.find(handle) == _aclIn.end())
{
no_acl_start_len_dynamic* pd = new(no_acl_start_len_dynamic);
pd->cit = (cid);
pd->len = (chunklen);
pd->expectedlen = (expectedlen);
pd->byarr.insert(pd->byarr.end(), sd.data, sd.data + sd.len);
_aclIn[handle] = pd;
}
else
{
_TRACE("ERROR AHNDLER NOT FOUND ....!. \n" << handle);
}
}
}
else if (ACL_CONT == flags)
{
/// keep accumulating, or discard
uint16_t chunklen = blen-9;
sdata sd;
scase="ACL_CONT";
auto el = _aclIn.find(handle);
if (el == _aclIn.end())
return true;
no_acl_start_len_dynamic* pd = el->second;
pd->len += chunklen;
pd->byarr.insert(pd->byarr.end(), sd.data, sd.data + sd.len);
//assert(pd->expectedlen == expectedlen);
if(pd->expectedlen == pd->len )
{
sd.len = pd->len;
sd.data = &pd->byarr[0];
_pev->on_acl_packet(handle, pd->cit, sd);
delete pd;
_aclIn.erase(handle);
}
}
else
{
_TRACE("!!! HCI_ACLDATA_PKT unknown flag: " << std::hex << int(flags) << std::dec);
}
}
else{
_TRACE("!!! NO KNOWN on_sock_data EVENTTYPE " << std::hex << int(eventType) << std::dec );
}
//TRACE("HCI: " << scase << " }");
return true;
}
