int read_le_buffer_size(int s)
{
ng_hci_le_read_buffer_size_rp rp;
int e;
int n = sizeof(rp);
e = hci_simple_request(s,
NG_HCI_OPCODE(NG_HCI_OGF_LE,
NG_HCI_OCF_LE_READ_BUFFER_SIZE),
(void *)&rp, &n);
printf("READ_LE_BUFFER_SIZE %d %d %d %d\n", e, rp.status, rp.hc_le_data_packet_length,
rp.hc_total_num_le_data_packets);
if(rp.status == 0 && rp.hc_le_data_packet_length==0){
ng_hci_read_buffer_size_rp brp;
n = sizeof(brp);
e = hci_simple_request(s,
NG_HCI_OPCODE(NG_HCI_OGF_INFO,
NG_HCI_OCF_READ_BUFFER_SIZE),
(void *)&brp, &n);
printf("READ BUFFER SIZE %d %d %d %d %d %d \n", e, brp.status,
brp.max_acl_size,
brp.max_sco_size,
brp.num_acl_pkt,
brp.num_sco_pkt);
}
return 0;
}
/* Send PIN_Code_[Negative]_Reply */
static int
send_pin_code_reply(int sock, struct sockaddr_hci *addr,
bdaddr_p bdaddr, char const *pin)
{
uint8_t buffer[HCSECD_BUFFER_SIZE];
ng_hci_cmd_pkt_t *cmd = NULL;
memset(buffer, 0, sizeof(buffer));
cmd = (ng_hci_cmd_pkt_t *) buffer;
cmd->type = NG_HCI_CMD_PKT;
if (pin != NULL) {
ng_hci_pin_code_rep_cp *cp = NULL;
cmd->opcode = htole16(NG_HCI_OPCODE(NG_HCI_OGF_LINK_CONTROL,
NG_HCI_OCF_PIN_CODE_REP));
cmd->length = sizeof(*cp);
cp = (ng_hci_pin_code_rep_cp *)(cmd + 1);
memcpy(&cp->bdaddr, bdaddr, sizeof(cp->bdaddr));
strncpy((char *) cp->pin, pin, sizeof(cp->pin));
cp->pin_size = strlen((char const *) cp->pin);
syslog(LOG_DEBUG, "Sending PIN_Code_Reply to '%s' " \
"for remote bdaddr %s",
addr->hci_node, bt_ntoa(bdaddr, NULL));
} else {
ng_hci_pin_code_neg_rep_cp *cp = NULL;
cmd->opcode = htole16(NG_HCI_OPCODE(NG_HCI_OGF_LINK_CONTROL,
NG_HCI_OCF_PIN_CODE_NEG_REP));
cmd->length = sizeof(*cp);
cp = (ng_hci_pin_code_neg_rep_cp *)(cmd + 1);
memcpy(&cp->bdaddr, bdaddr, sizeof(cp->bdaddr));
syslog(LOG_DEBUG, "Sending PIN_Code_Negative_Reply to '%s' " \
"for remote bdaddr %s",
addr->hci_node, bt_ntoa(bdaddr, NULL));
}
again:
if (sendto(sock, buffer, sizeof(*cmd) + cmd->length, 0,
(struct sockaddr *) addr, sizeof(*addr)) < 0) {
if (errno == EINTR)
goto again;
syslog(LOG_ERR, "Could not send PIN code reply to '%s' " \
"for remote bdaddr %s. %s (%d)",
addr->hci_node, bt_ntoa(bdaddr, NULL),
strerror(errno), errno);
return (-1);
}
return (0);
}
/* Send Link_Key_[Negative]_Reply */
static int
send_link_key_reply(int sock, struct sockaddr_hci *addr,
bdaddr_p bdaddr, uint8_t *key)
{
uint8_t buffer[HCSECD_BUFFER_SIZE];
ng_hci_cmd_pkt_t *cmd = NULL;
memset(buffer, 0, sizeof(buffer));
cmd = (ng_hci_cmd_pkt_t *) buffer;
cmd->type = NG_HCI_CMD_PKT;
if (key != NULL) {
ng_hci_link_key_rep_cp *cp = NULL;
cmd->opcode = htole16(NG_HCI_OPCODE(NG_HCI_OGF_LINK_CONTROL,
NG_HCI_OCF_LINK_KEY_REP));
cmd->length = sizeof(*cp);
cp = (ng_hci_link_key_rep_cp *)(cmd + 1);
memcpy(&cp->bdaddr, bdaddr, sizeof(cp->bdaddr));
memcpy(&cp->key, key, sizeof(cp->key));
syslog(LOG_DEBUG, "Sending Link_Key_Reply to '%s' " \
"for remote bdaddr %s",
addr->hci_node, bt_ntoa(bdaddr, NULL));
} else {
ng_hci_link_key_neg_rep_cp *cp = NULL;
cmd->opcode = htole16(NG_HCI_OPCODE(NG_HCI_OGF_LINK_CONTROL,
NG_HCI_OCF_LINK_KEY_NEG_REP));
cmd->length = sizeof(*cp);
cp = (ng_hci_link_key_neg_rep_cp *)(cmd + 1);
memcpy(&cp->bdaddr, bdaddr, sizeof(cp->bdaddr));
syslog(LOG_DEBUG, "Sending Link_Key_Negative_Reply to '%s' " \
"for remote bdaddr %s",
addr->hci_node, bt_ntoa(bdaddr, NULL));
}
again:
if (sendto(sock, buffer, sizeof(*cmd) + cmd->length, 0,
(struct sockaddr *) addr, sizeof(*addr)) < 0) {
if (errno == EINTR)
goto again;
syslog(LOG_ERR, "Could not send link key reply to '%s' " \
"for remote bdaddr %s. %s (%d)",
addr->hci_node, bt_ntoa(bdaddr, NULL),
strerror(errno), errno);
return (-1);
}
return (0);
}
/* Send Read_Local_Supported_Features command to the unit */
static int
hci_read_local_supported_features(int s, int argc, char **argv)
{
ng_hci_read_local_features_rp rp;
int n;
char buffer[1024];
n = sizeof(rp);
if (hci_simple_request(s, NG_HCI_OPCODE(NG_HCI_OGF_INFO,
NG_HCI_OCF_READ_LOCAL_FEATURES),
(char *) &rp, &n) == ERROR)
return (ERROR);
if (rp.status != 0x00) {
fprintf(stdout, "Status: %s [%#02x]\n",
hci_status2str(rp.status), rp.status);
return (FAILED);
}
fprintf(stdout, "Features: ");
for (n = 0; n < sizeof(rp.features); n++)
fprintf(stdout, "%#02x ", rp.features[n]);
fprintf(stdout, "\n%s\n", hci_features2str(rp.features,
buffer, sizeof(buffer)));
return (OK);
} /* hci_read_local_supported_features */
/* Send Read_Local_Version_Information command to the unit */
static int
hci_read_local_version_information(int s, int argc, char **argv)
{
ng_hci_read_local_ver_rp rp;
int n;
n = sizeof(rp);
if (hci_simple_request(s, NG_HCI_OPCODE(NG_HCI_OGF_INFO,
NG_HCI_OCF_READ_LOCAL_VER), (char *) &rp, &n) == ERROR)
return (ERROR);
if (rp.status != 0x00) {
fprintf(stdout, "Status: %s [%#02x]\n",
hci_status2str(rp.status), rp.status);
return (FAILED);
}
rp.manufacturer = le16toh(rp.manufacturer);
fprintf(stdout, "HCI version: %s [%#02x]\n",
hci_ver2str(rp.hci_version), rp.hci_version);
fprintf(stdout, "HCI revision: %#04x\n",
le16toh(rp.hci_revision));
fprintf(stdout, "LMP version: %s [%#02x]\n",
hci_lmpver2str(rp.lmp_version), rp.lmp_version);
fprintf(stdout, "LMP sub-version: %#04x\n",
le16toh(rp.lmp_subversion));
fprintf(stdout, "Manufacturer: %s [%#04x]\n",
hci_manufacturer2str(rp.manufacturer), rp.manufacturer);
return (OK);
} /* hci_read_local_version_information */
/* Sent Read_Buffer_Size command to the unit */
static int
hci_read_buffer_size(int s, int argc, char **argv)
{
ng_hci_read_buffer_size_rp rp;
int n;
n = sizeof(rp);
if (hci_simple_request(s, NG_HCI_OPCODE(NG_HCI_OGF_INFO,
NG_HCI_OCF_READ_BUFFER_SIZE),
(char *) &rp, &n) == ERROR)
return (ERROR);
if (rp.status != 0x00) {
fprintf(stdout, "Status: %s [%#02x]\n",
hci_status2str(rp.status), rp.status);
return (FAILED);
}
fprintf(stdout, "Max. ACL packet size: %d bytes\n",
le16toh(rp.max_acl_size));
fprintf(stdout, "Number of ACL packets: %d\n",
le16toh(rp.num_acl_pkt));
fprintf(stdout, "Max. SCO packet size: %d bytes\n",
rp.max_sco_size);
fprintf(stdout, "Number of SCO packets: %d\n",
le16toh(rp.num_sco_pkt));
return (OK);
} /* hci_read_buffer_size */
/**
LE Set Scan Param
Vol2/Part E/7.8.10
*/
static int
le_set_scan_param(int s, int type, int interval, int window, int adrtype, int policy)
{
ng_hci_le_set_scan_parameters_cp cp;
ng_hci_le_set_scan_parameters_rp rp;
int e, n;
n = sizeof(rp);
cp.le_scan_type = type;
cp.le_scan_interval = interval;
cp.own_address_type = adrtype;
cp.le_scan_window = window;
cp.scanning_filter_policy = policy;
e = hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE,
NG_HCI_OCF_LE_SET_SCAN_PARAMETERS),
(void *)&cp, sizeof(cp),
(void *)&rp, &n);
printf("* LE_Set_Scan_Param %d %d %d\n", e, rp.status, n);
printf(" Scan_Type: %d\n", type);
printf(" LE_Scan_interval: %d\n", interval);
printf(" LE_Scan_window: %d\n", window);
printf(" Scanning_filter_policy: %d\n", policy);
printf(" Own_Address_Type: %d\n", adrtype);
return e;
}
int request_disconnect(int s, int handle, int reason)
{
ng_hci_discon_cp cp;
ng_hci_status_rp ep;
int n;
cp.con_handle = handle;
cp.reason = reason;
n = sizeof(ep);
hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LINK_CONTROL,NG_HCI_OCF_DISCON),
(char *)&cp, sizeof(cp), (char *)&ep, &n);
printf("DISCONNECT:%d\n", ep.status);
return ep.status;
}
int le_read_local_supported_features(int s)
{
ng_hci_le_read_local_supported_features_rp rp;
int e;
int n = sizeof(rp);
e = hci_simple_request(s,
NG_HCI_OPCODE(NG_HCI_OGF_LE,
NG_HCI_OCF_LE_READ_LOCAL_SUPPORTED_FEATURES),
(void *)&rp, &n);
printf("LOCAL SUPPOREDED:%d %d %lu\n", e, rp.status, rp.le_features);
return 0;
}
int le_read_supported_status(int s)
{
ng_hci_le_read_supported_status_rp rp;
int e;
int n = sizeof(rp);
e = hci_simple_request(s,
NG_HCI_OPCODE(NG_HCI_OGF_LE,
NG_HCI_OCF_LE_READ_SUPPORTED_STATUS),
(void *)&rp, &n);
printf("LE_STATUS:%d %d %lx\n", e, rp.status, rp.le_status);
return 0;
}
int le_set_scan_enable(int s, int enable)
{
ng_hci_le_set_scan_enable_cp cp;
ng_hci_le_set_scan_enable_rp rp;
int e,n;
cp.le_scan_enable = enable;
cp.filter_duplicates = 0;
e = hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE,
NG_HCI_OCF_LE_SET_SCAN_ENABLE),
(void *)&cp, sizeof(cp), (void *)&rp, &n);
printf("SCAN ENABLE%d %d %d\n", e, rp.status, n);
return 0;
}
int set_event_mask(int s, uint64_t mask)
{
ng_hci_set_event_mask_cp semc;
ng_hci_set_event_mask_rp rp;
int i,n,e;
n = sizeof(rp);
for(i=0; i< NG_HCI_EVENT_MASK_SIZE;i++){
semc.event_mask[i] = mask&0xff;
mask>>= 8;
}
e = hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_HC_BASEBAND, NG_HCI_OCF_SET_EVENT_MASK), (void *)&semc, sizeof(semc), (void *)&rp, &n);
printf("SET EVENT MASK%d %d\n",e, rp.status);
return 0;
}
/* Send Write_Link_Policy_Settings command to the unit */
static int
hci_write_link_policy_settings(int s, int argc, char **argv)
{
ng_hci_write_link_policy_settings_cp cp;
ng_hci_write_link_policy_settings_rp rp;
int n;
/* parse command parameters */
switch (argc) {
case 2:
/* connection handle */
if (sscanf(argv[0], "%d", &n) != 1 || n <= 0 || n > 0x0eff)
return (USAGE);
cp.con_handle = (uint16_t) (n & 0x0fff);
cp.con_handle = htole16(cp.con_handle);
/* link policy settings */
if (sscanf(argv[1], "%x", &n) != 1)
return (USAGE);
cp.settings = (uint16_t) (n & 0x0ffff);
cp.settings = htole16(cp.settings);
break;
default:
return (USAGE);
}
/* send request */
n = sizeof(rp);
if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LINK_POLICY,
NG_HCI_OCF_WRITE_LINK_POLICY_SETTINGS),
(char const *) &cp, sizeof(cp),
(char *) &rp, &n) == ERROR)
return (ERROR);
if (rp.status != 0x00) {
fprintf(stdout, "Status: %s [%#02x]\n",
hci_status2str(rp.status), rp.status);
return (FAILED);
}
return (OK);
} /* hci_write_link_policy_settings */
int le_connect(int s, bdaddr_t *bd)
{
ng_hci_le_create_connection_cp cp;
ng_hci_status_rp rp;
int n,n2;
n = sizeof(cp);
n2 = sizeof(rp);
memcpy(&cp.peer_addr, bd, sizeof(*bd));
printf("%x %x %x %x %x %x \n", cp.peer_addr.b[0],
cp.peer_addr.b[1],
cp.peer_addr.b[2],
cp.peer_addr.b[3],
cp.peer_addr.b[4],
cp.peer_addr.b[5]);
printf("%x %x %x %x %x %x \n", bd->b[0],
bd->b[1],
bd->b[2],
bd->b[3],
bd->b[4],
bd->b[5]);
cp.peer_addr_type = 0;
cp.own_address_type = 0;
cp.scan_interval = htobs(0x4);
cp.scan_window = htobs(0x04);
cp.filter_policy = 0;
cp.conn_interval_min = htobs(0x0f);
cp.conn_interval_max = htobs(0x0f); //4sec.
cp.conn_latency = htobs(0);
cp.supervision_timeout = htobs(0xc80);
cp.min_ce_length = htobs(1);
cp.max_ce_length = htobs(1);
hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE,
NG_HCI_OCF_LE_CREATE_CONNECTION),
(const char *)&cp, n, (char *)&rp, &n2);
printf("STATUS:%d\n", rp.status);
return rp.status;
}
/**
Read Buffer Size
Vol2/Part E/7.4.5
*/
static int
read_buffer_size(int s)
{
ng_hci_read_buffer_size_rp brp;
int n, e;
n = sizeof(brp);
e = hci_simple_request(s,
NG_HCI_OPCODE(NG_HCI_OGF_INFO,
NG_HCI_OCF_READ_BUFFER_SIZE),
(void *)&brp, &n);
printf("* Read_Buffer_Size: %d %d %d %d %d %d \n", e, brp.status,
brp.max_acl_size,
brp.max_sco_size,
brp.num_acl_pkt,
brp.num_sco_pkt);
return e;
}
/**
Read BD_ADDR
Vol2/Part E/7.4.6
*/
static int
read_bd_addr(int s, bdaddr_t *bdaddr)
{
ng_hci_read_bdaddr_rp rp;
int n = sizeof(rp);
int e = hci_simple_request(s,
NG_HCI_OPCODE(NG_HCI_OGF_INFO,
NG_HCI_OCF_READ_BDADDR),
(void *)&rp, &n);
if(e == 0){
if(bdaddr)
memcpy(bdaddr, &(rp.bdaddr), sizeof(*bdaddr));
char buf[18];
bt_ntoa(&(rp.bdaddr), buf);
printf("* Read_BD_ADDR: %s\n", buf);
}
return e;
}
/* Send Role Discovery to the unit */
static int
hci_role_discovery(int s, int argc, char **argv)
{
ng_hci_role_discovery_cp cp;
ng_hci_role_discovery_rp rp;
int n;
/* parse command parameters */
switch (argc) {
case 1:
/* connection handle */
if (sscanf(argv[0], "%d", &n) != 1 || n <= 0 || n > 0x0eff)
return (USAGE);
cp.con_handle = (uint16_t) (n & 0x0fff);
cp.con_handle = htole16(cp.con_handle);
break;
default:
return (USAGE);
}
/* send request */
n = sizeof(rp);
if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LINK_POLICY,
NG_HCI_OCF_ROLE_DISCOVERY),
(char const *) &cp, sizeof(cp),
(char *) &rp, &n) == ERROR)
return (ERROR);
if (rp.status != 0x00) {
fprintf(stdout, "Status: %s [%#02x]\n",
hci_status2str(rp.status), rp.status);
return (FAILED);
}
fprintf(stdout, "Connection handle: %d\n", le16toh(rp.con_handle));
fprintf(stdout, "Role: %s [%#x]\n",
(rp.role == NG_HCI_ROLE_MASTER)? "Master" : "Slave", rp.role);
return (OK);
} /* hci_role_discovery */
/**
LE Read Buffer Size
Vol2/Part E/7.8.2
*/
static int
le_read_buffer_size(int s)
{
ng_hci_le_read_buffer_size_rp rp;
int n, e;
n = sizeof(rp);
e = hci_simple_request(s,
NG_HCI_OPCODE(NG_HCI_OGF_LE,
NG_HCI_OCF_LE_READ_BUFFER_SIZE),
(void *)&rp, &n);
printf("* LE_Read_Buffer_Size: %d %d %d %d\n", e, rp.status, rp.hc_le_data_packet_length,
rp.hc_total_num_le_data_packets);
if(rp.status == 0 && rp.hc_le_data_packet_length == 0)
read_buffer_size(s);
return e;
}
/**
LE Set Event Mask
Vol2/Part E/7.8.1
*/
static int
le_set_event_mask(int s, uint64_t mask)
{
ng_hci_le_set_event_mask_cp semc;
ng_hci_le_set_event_mask_rp rp;
int i, n ,e;
n = sizeof(rp);
for(i=0; i< NG_HCI_LE_EVENT_MASK_SIZE;i++){
semc.event_mask[i] = mask & 0xff;
mask >>= 8;
}
e = hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE,
NG_HCI_OCF_LE_SET_EVENT_MASK),
(void *)&semc, sizeof(semc),
(void *)&rp, &n);
printf("* LE_Set_Event_Mask: %d %d\n",e, rp.status);
return e;
}
/* Send Read_BD_ADDR command to the unit */
static int
hci_read_bd_addr(int s, int argc, char **argv)
{
ng_hci_read_bdaddr_rp rp;
int n;
n = sizeof(rp);
if (hci_simple_request(s, NG_HCI_OPCODE(NG_HCI_OGF_INFO,
NG_HCI_OCF_READ_BDADDR), (char *) &rp, &n) == ERROR)
return (ERROR);
if (rp.status != 0x00) {
fprintf(stdout, "Status: %s [%#02x]\n",
hci_status2str(rp.status), rp.status);
return (FAILED);
}
fprintf(stdout, "BD_ADDR: %s\n", bt_ntoa(&rp.bdaddr, NULL));
return (OK);
} /* hci_read_bd_addr */
int le_set_scan_response(int s, int len, char *scan_data)
{
ng_hci_le_set_scan_response_data_cp cp;
ng_hci_le_set_scan_response_data_rp rp;
int n,e;
if(len > NG_HCI_ADVERTISING_DATA_SIZE){
printf("ERROR\n");
return 1;
}
cp.scan_response_data_length = len;
memcpy(cp.scan_response_data, scan_data, len);
n = sizeof(rp);
e = hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE,
NG_HCI_OCF_LE_SET_SCAN_RESPONSE_DATA),
(void *)&cp, sizeof(cp), (void *)&rp, &n);
printf("SEt SCAN RESPONSE %d %d %d\n", e, rp.status, n);
return 0;
}
int le_set_scan_param(int s, int type, int interval, int window, int adrtype,int policy)
{
ng_hci_le_set_scan_parameters_cp cp;
ng_hci_le_set_scan_parameters_rp rp;
int e,n;
printf("SCANTYPE%d INTERVAL%d ADDRTYPE%d WINDOW%d POLICY%d\n",
type, interval, adrtype,window,policy);
cp.le_scan_type = type;
cp.le_scan_interval = interval;
cp.own_address_type = adrtype;
cp.le_scan_window = window;
cp.scanning_filter_policy = policy;
n = sizeof(rp);
e = hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE,
NG_HCI_OCF_LE_SET_SCAN_PARAMETERS),
(void *)&cp, sizeof(cp), (void *)&rp, &n);
printf("SCAN_PARAM %d %d %d\n", e, rp.status, n);
return 0;
}
/* Send Read_Failed_Contact_Counter command to the unit */
static int
hci_read_failed_contact_counter(int s, int argc, char **argv)
{
ng_hci_read_failed_contact_cntr_cp cp;
ng_hci_read_failed_contact_cntr_rp rp;
int n;
switch (argc) {
case 1:
/* connection handle */
if (sscanf(argv[0], "%d", &n) != 1 || n <= 0 || n > 0x0eff)
return (USAGE);
cp.con_handle = (uint16_t) (n & 0x0fff);
cp.con_handle = htole16(cp.con_handle);
break;
default:
return (USAGE);
}
/* send command */
n = sizeof(rp);
if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_STATUS,
NG_HCI_OCF_READ_FAILED_CONTACT_CNTR),
(char const *) &cp, sizeof(cp),
(char *) &rp, &n) == ERROR)
return (ERROR);
if (rp.status != 0x00) {
fprintf(stdout, "Status: %s [%#02x]\n",
hci_status2str(rp.status), rp.status);
return (FAILED);
}
fprintf(stdout, "Connection handle: %d\n", le16toh(rp.con_handle));
fprintf(stdout, "Failed contact counter: %d\n", le16toh(rp.counter));
return (OK);
} /* hci_read_failed_contact_counter */
/* Sent Get_Link_Quality command to the unit */
static int
hci_get_link_quality(int s, int argc, char **argv)
{
ng_hci_get_link_quality_cp cp;
ng_hci_get_link_quality_rp rp;
int n;
switch (argc) {
case 1:
/* connection handle */
if (sscanf(argv[0], "%d", &n) != 1 || n <= 0 || n > 0x0eff)
return (USAGE);
cp.con_handle = (uint16_t) (n & 0x0fff);
cp.con_handle = htole16(cp.con_handle);
break;
default:
return (USAGE);
}
/* send command */
n = sizeof(rp);
if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_STATUS,
NG_HCI_OCF_GET_LINK_QUALITY),
(char const *) &cp, sizeof(cp),
(char *) &rp, &n) == ERROR)
return (ERROR);
if (rp.status != 0x00) {
fprintf(stdout, "Status: %s [%#02x]\n",
hci_status2str(rp.status), rp.status);
return (FAILED);
}
fprintf(stdout, "Connection handle: %d\n", le16toh(rp.con_handle));
fprintf(stdout, "Link quality: %d\n", le16toh(rp.quality));
return (OK);
} /* hci_get_link_quality */
/* Send Read_Country_Code command to the unit */
static int
hci_read_country_code(int s, int argc, char **argv)
{
ng_hci_read_country_code_rp rp;
int n;
n = sizeof(rp);
if (hci_simple_request(s, NG_HCI_OPCODE(NG_HCI_OGF_INFO,
NG_HCI_OCF_READ_COUNTRY_CODE),
(char *) &rp, &n) == ERROR)
return (ERROR);
if (rp.status != 0x00) {
fprintf(stdout, "Status: %s [%#02x]\n",
hci_status2str(rp.status), rp.status);
return (FAILED);
}
fprintf(stdout, "Country code: %s [%#02x]\n",
hci_cc2str(rp.country_code), rp.country_code);
return (OK);
} /* hci_read_country_code */
int
ng_hci_lp_discon_req(ng_hci_unit_p unit, item_p item, hook_p hook)
{
struct discon_req {
ng_hci_cmd_pkt_t hdr;
ng_hci_discon_cp cp;
} __attribute__ ((packed)) *req = NULL;
ng_hci_lp_discon_req_ep *ep = NULL;
ng_hci_unit_con_p con = NULL;
struct mbuf *m = NULL;
int error = 0;
/* Check if unit is ready */
if ((unit->state & NG_HCI_UNIT_READY) != NG_HCI_UNIT_READY) {
NG_HCI_WARN(
"%s: %s - unit is not ready, state=%#x\n",
__func__, NG_NODE_NAME(unit->node), unit->state);
error = ENXIO;
goto out;
}
if (NGI_MSG(item)->header.arglen != sizeof(*ep)) {
NG_HCI_ALERT(
"%s: %s - invalid LP_DisconnectReq message size=%d\n",
__func__, NG_NODE_NAME(unit->node),
NGI_MSG(item)->header.arglen);
error = EMSGSIZE;
goto out;
}
ep = (ng_hci_lp_discon_req_ep *)(NGI_MSG(item)->data);
con = ng_hci_con_by_handle(unit, ep->con_handle);
if (con == NULL) {
NG_HCI_ERR(
"%s: %s - invalid connection handle=%d\n",
__func__, NG_NODE_NAME(unit->node), ep->con_handle);
error = ENOENT;
goto out;
}
if (con->state != NG_HCI_CON_OPEN) {
NG_HCI_ERR(
"%s: %s - invalid connection state=%d, handle=%d\n",
__func__, NG_NODE_NAME(unit->node), con->state,
ep->con_handle);
error = EINVAL;
goto out;
}
/*
* Create HCI command
*/
MGETHDR(m, M_DONTWAIT, MT_DATA);
if (m == NULL) {
error = ENOBUFS;
goto out;
}
m->m_pkthdr.len = m->m_len = sizeof(*req);
req = mtod(m, struct discon_req *);
req->hdr.type = NG_HCI_CMD_PKT;
req->hdr.length = sizeof(req->cp);
req->hdr.opcode = htole16(NG_HCI_OPCODE(NG_HCI_OGF_LINK_CONTROL,
NG_HCI_OCF_DISCON));
req->cp.con_handle = htole16(ep->con_handle);
req->cp.reason = ep->reason;
/*
* Queue and send HCI command
*/
NG_BT_MBUFQ_ENQUEUE(&unit->cmdq, m);
if (!(unit->state & NG_HCI_UNIT_COMMAND_PENDING))
error = ng_hci_send_command(unit);
out:
NG_FREE_ITEM(item);
return (error);
} /* ng_hci_lp_discon_req */
static int
ng_hci_lp_sco_con_req(ng_hci_unit_p unit, item_p item, hook_p hook)
{
struct sco_con_req {
ng_hci_cmd_pkt_t hdr;
ng_hci_add_sco_con_cp cp;
} __attribute__ ((packed)) *req = NULL;
ng_hci_lp_con_req_ep *ep = NULL;
ng_hci_unit_con_p acl_con = NULL, sco_con = NULL;
struct mbuf *m = NULL;
int error = 0;
ep = (ng_hci_lp_con_req_ep *)(NGI_MSG(item)->data);
/*
* SCO connection without ACL link
*
* If upper layer requests SCO connection and there is no open ACL
* connection to the desired remote unit, we will reject the request.
*/
LIST_FOREACH(acl_con, &unit->con_list, next)
if (acl_con->link_type == NG_HCI_LINK_ACL &&
acl_con->state == NG_HCI_CON_OPEN &&
bcmp(&acl_con->bdaddr, &ep->bdaddr, sizeof(bdaddr_t)) == 0)
break;
if (acl_con == NULL) {
NG_HCI_INFO(
"%s: %s - No open ACL connection to bdaddr=%x:%x:%x:%x:%x:%x\n",
__func__, NG_NODE_NAME(unit->node),
ep->bdaddr.b[5], ep->bdaddr.b[4], ep->bdaddr.b[3],
ep->bdaddr.b[2], ep->bdaddr.b[1], ep->bdaddr.b[0]);
error = ENOENT;
goto out;
}
/*
* Multiple SCO connections can exist between the same pair of units.
* We assume that multiple SCO connections have to be opened one after
* another.
*
* Try to find SCO connection descriptor that matches the following:
*
* 1) sco_con->link_type == NG_HCI_LINK_SCO
*
* 2) sco_con->state == NG_HCI_CON_W4_LP_CON_RSP ||
* sco_con->state == NG_HCI_CON_W4_CONN_COMPLETE
*
* 3) sco_con->bdaddr == ep->bdaddr
*
* Two cases:
*
* 1) We do not have connection descriptor. This is simple. Just
* create new connection and submit Add_SCO_Connection command.
*
* 2) We do have connection descriptor. We need to check the state.
*
* 2.1) NG_HCI_CON_W4_LP_CON_RSP means we in the middle of accepting
* connection from the remote unit. This is a race condition and
* we will ignore the request.
*
* 2.2) NG_HCI_CON_W4_CONN_COMPLETE means upper layer already requested
* connection or we just accepted it.
*/
LIST_FOREACH(sco_con, &unit->con_list, next)
if (sco_con->link_type == NG_HCI_LINK_SCO &&
(sco_con->state == NG_HCI_CON_W4_LP_CON_RSP ||
sco_con->state == NG_HCI_CON_W4_CONN_COMPLETE) &&
bcmp(&sco_con->bdaddr, &ep->bdaddr, sizeof(bdaddr_t)) == 0)
break;
if (sco_con != NULL) {
switch (sco_con->state) {
case NG_HCI_CON_W4_LP_CON_RSP: /* XXX */
error = EALREADY;
break;
case NG_HCI_CON_W4_CONN_COMPLETE:
sco_con->flags |= NG_HCI_CON_NOTIFY_SCO;
break;
default:
panic(
"%s: %s - Inavalid connection state=%d\n",
__func__, NG_NODE_NAME(unit->node),
sco_con->state);
break;
}
goto out;
}
/*
* If we got here then we need to create new SCO connection descriptor
* and submit HCI command.
*/
sco_con = ng_hci_new_con(unit, NG_HCI_LINK_SCO);
if (sco_con == NULL) {
error = ENOMEM;
goto out;
}
bcopy(&ep->bdaddr, &sco_con->bdaddr, sizeof(sco_con->bdaddr));
/*
* Create HCI command
*/
MGETHDR(m, M_DONTWAIT, MT_DATA);
if (m == NULL) {
ng_hci_free_con(sco_con);
error = ENOBUFS;
goto out;
}
m->m_pkthdr.len = m->m_len = sizeof(*req);
req = mtod(m, struct sco_con_req *);
req->hdr.type = NG_HCI_CMD_PKT;
req->hdr.length = sizeof(req->cp);
req->hdr.opcode = htole16(NG_HCI_OPCODE(NG_HCI_OGF_LINK_CONTROL,
NG_HCI_OCF_ADD_SCO_CON));
req->cp.con_handle = htole16(acl_con->con_handle);
req->cp.pkt_type = NG_HCI_PKT_HV1;
if (unit->features[1] & NG_HCI_LMP_HV2_PKT)
req->cp.pkt_type |= NG_HCI_PKT_HV2;
if (unit->features[1] & NG_HCI_LMP_HV3_PKT)
req->cp.pkt_type |= NG_HCI_PKT_HV3;
req->cp.pkt_type &= unit->packet_mask;
if ((req->cp.pkt_type & (NG_HCI_PKT_HV1|
NG_HCI_PKT_HV2|
NG_HCI_PKT_HV3)) == 0)
req->cp.pkt_type = NG_HCI_PKT_HV1;
req->cp.pkt_type = htole16(req->cp.pkt_type);
/*
* Adust connection state
*/
sco_con->flags |= NG_HCI_CON_NOTIFY_SCO;
sco_con->state = NG_HCI_CON_W4_CONN_COMPLETE;
ng_hci_con_timeout(sco_con);
/*
* Queue and send HCI command
*/
NG_BT_MBUFQ_ENQUEUE(&unit->cmdq, m);
if (!(unit->state & NG_HCI_UNIT_COMMAND_PENDING))
error = ng_hci_send_command(unit);
out:
NG_FREE_ITEM(item);
return (error);
} /* ng_hci_lp_sco_con_req */
static int
ng_hci_lp_acl_con_req(ng_hci_unit_p unit, item_p item, hook_p hook)
{
struct acl_con_req {
ng_hci_cmd_pkt_t hdr;
ng_hci_create_con_cp cp;
} __attribute__ ((packed)) *req = NULL;
ng_hci_lp_con_req_ep *ep = NULL;
ng_hci_unit_con_p con = NULL;
ng_hci_neighbor_t *n = NULL;
struct mbuf *m = NULL;
int error = 0;
ep = (ng_hci_lp_con_req_ep *)(NGI_MSG(item)->data);
/*
* Only one ACL connection can exist between each pair of units.
* So try to find ACL connection descriptor (in any state) that
* has requested remote BD_ADDR.
*
* Two cases:
*
* 1) We do not have connection to the remote unit. This is simple.
* Just create new connection descriptor and send HCI command to
* create new connection.
*
* 2) We do have connection descriptor. We need to check connection
* state:
*
* 2.1) NG_HCI_CON_W4_LP_CON_RSP means that we are in the middle of
* accepting connection from the remote unit. This is a race
* condition. We will ignore this message.
*
* 2.2) NG_HCI_CON_W4_CONN_COMPLETE means that upper layer already
* requested connection or we just accepted it. In any case
* all we need to do here is set appropriate notification bit
* and wait.
*
* 2.3) NG_HCI_CON_OPEN means connection is open. Just reply back
* and let upper layer know that we have connection already.
*/
con = ng_hci_con_by_bdaddr(unit, &ep->bdaddr, NG_HCI_LINK_ACL);
if (con != NULL) {
switch (con->state) {
case NG_HCI_CON_W4_LP_CON_RSP: /* XXX */
error = EALREADY;
break;
case NG_HCI_CON_W4_CONN_COMPLETE:
if (hook == unit->acl)
con->flags |= NG_HCI_CON_NOTIFY_ACL;
else
con->flags |= NG_HCI_CON_NOTIFY_SCO;
break;
case NG_HCI_CON_OPEN: {
struct ng_mesg *msg = NULL;
ng_hci_lp_con_cfm_ep *cfm = NULL;
if (hook != NULL && NG_HOOK_IS_VALID(hook)) {
NGI_GET_MSG(item, msg);
NG_FREE_MSG(msg);
NG_MKMESSAGE(msg, NGM_HCI_COOKIE,
NGM_HCI_LP_CON_CFM, sizeof(*cfm),
M_NOWAIT);
if (msg != NULL) {
cfm = (ng_hci_lp_con_cfm_ep *)msg->data;
cfm->status = 0;
cfm->link_type = con->link_type;
cfm->con_handle = con->con_handle;
bcopy(&con->bdaddr, &cfm->bdaddr,
sizeof(cfm->bdaddr));
/*
* This will forward item back to
* sender and set item to NULL
*/
_NGI_MSG(item) = msg;
NG_FWD_ITEM_HOOK(error, item, hook);
} else
error = ENOMEM;
} else
NG_HCI_INFO(
"%s: %s - Source hook is not valid, hook=%p\n",
__func__, NG_NODE_NAME(unit->node),
hook);
} break;
default:
panic(
"%s: %s - Invalid connection state=%d\n",
__func__, NG_NODE_NAME(unit->node), con->state);
break;
}
goto out;
}
/*
* If we got here then we need to create new ACL connection descriptor
* and submit HCI command. First create new connection desriptor, set
* bdaddr and notification flags.
*/
con = ng_hci_new_con(unit, NG_HCI_LINK_ACL);
if (con == NULL) {
error = ENOMEM;
goto out;
}
bcopy(&ep->bdaddr, &con->bdaddr, sizeof(con->bdaddr));
/*
* Create HCI command
*/
MGETHDR(m, M_DONTWAIT, MT_DATA);
if (m == NULL) {
ng_hci_free_con(con);
error = ENOBUFS;
goto out;
}
m->m_pkthdr.len = m->m_len = sizeof(*req);
req = mtod(m, struct acl_con_req *);
req->hdr.type = NG_HCI_CMD_PKT;
req->hdr.length = sizeof(req->cp);
req->hdr.opcode = htole16(NG_HCI_OPCODE(NG_HCI_OGF_LINK_CONTROL,
NG_HCI_OCF_CREATE_CON));
bcopy(&ep->bdaddr, &req->cp.bdaddr, sizeof(req->cp.bdaddr));
req->cp.pkt_type = (NG_HCI_PKT_DM1|NG_HCI_PKT_DH1);
if (unit->features[0] & NG_HCI_LMP_3SLOT)
req->cp.pkt_type |= (NG_HCI_PKT_DM3|NG_HCI_PKT_DH3);
if (unit->features[0] & NG_HCI_LMP_5SLOT)
req->cp.pkt_type |= (NG_HCI_PKT_DM5|NG_HCI_PKT_DH5);
req->cp.pkt_type &= unit->packet_mask;
if ((req->cp.pkt_type & (NG_HCI_PKT_DM1|NG_HCI_PKT_DH1|
NG_HCI_PKT_DM3|NG_HCI_PKT_DH3|
NG_HCI_PKT_DM5|NG_HCI_PKT_DH5)) == 0)
req->cp.pkt_type = (NG_HCI_PKT_DM1|NG_HCI_PKT_DH1);
req->cp.pkt_type = htole16(req->cp.pkt_type);
if ((unit->features[0] & NG_HCI_LMP_SWITCH) && unit->role_switch)
req->cp.accept_role_switch = 1;
else
req->cp.accept_role_switch = 0;
/*
* We may speed up connect by specifying valid parameters.
* So check the neighbor cache.
*/
n = ng_hci_get_neighbor(unit, &ep->bdaddr);
if (n == NULL) {
req->cp.page_scan_rep_mode = 0;
req->cp.page_scan_mode = 0;
req->cp.clock_offset = 0;
} else {
req->cp.page_scan_rep_mode = n->page_scan_rep_mode;
req->cp.page_scan_mode = n->page_scan_mode;
req->cp.clock_offset = htole16(n->clock_offset);
}
/*
* Adust connection state
*/
if (hook == unit->acl)
con->flags |= NG_HCI_CON_NOTIFY_ACL;
else
con->flags |= NG_HCI_CON_NOTIFY_SCO;
con->state = NG_HCI_CON_W4_CONN_COMPLETE;
ng_hci_con_timeout(con);
/*
* Queue and send HCI command
*/
NG_BT_MBUFQ_ENQUEUE(&unit->cmdq, m);
if (!(unit->state & NG_HCI_UNIT_COMMAND_PENDING))
error = ng_hci_send_command(unit);
out:
if (item != NULL)
NG_FREE_ITEM(item);
return (error);
} /* ng_hci_lp_acl_con_req */
/* Send Swith Role to the unit */
static int
hci_switch_role(int s, int argc, char **argv)
{
int n0;
char b[512];
ng_hci_switch_role_cp cp;
ng_hci_event_pkt_t *e = (ng_hci_event_pkt_t *) b;
/* parse command parameters */
switch (argc) {
case 2:
/* bdaddr */
if (!bt_aton(argv[0], &cp.bdaddr)) {
struct hostent *he = NULL;
if ((he = bt_gethostbyname(argv[0])) == NULL)
return (USAGE);
memcpy(&cp.bdaddr, he->h_addr, sizeof(cp.bdaddr));
}
/* role */
if (sscanf(argv[1], "%d", &n0) != 1)
return (USAGE);
cp.role = n0? NG_HCI_ROLE_SLAVE : NG_HCI_ROLE_MASTER;
break;
default:
return (USAGE);
}
/* send request and expect status response */
n0 = sizeof(b);
if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LINK_POLICY,
NG_HCI_OCF_SWITCH_ROLE),
(char const *) &cp, sizeof(cp), b, &n0) == ERROR)
return (ERROR);
if (*b != 0x00)
return (FAILED);
/* wait for event */
again:
n0 = sizeof(b);
if (hci_recv(s, b, &n0) == ERROR)
return (ERROR);
if (n0 < sizeof(*e)) {
errno = EIO;
return (ERROR);
}
if (e->event == NG_HCI_EVENT_ROLE_CHANGE) {
ng_hci_role_change_ep *ep = (ng_hci_role_change_ep *)(e + 1);
if (ep->status != 0x00) {
fprintf(stdout, "Status: %s [%#02x]\n",
hci_status2str(ep->status), ep->status);
return (FAILED);
}
fprintf(stdout, "BD_ADDR: %s\n", hci_bdaddr2str(&ep->bdaddr));
fprintf(stdout, "Role: %s [%#x]\n",
(ep->role == NG_HCI_ROLE_MASTER)? "Master" : "Slave",
ep->role);
} else
goto again;
return (OK);
} /* hci_switch_role */
static int
ng_hci_lp_le_con_req(ng_hci_unit_p unit, item_p item, hook_p hook, int link_type)
{
struct acl_con_req {
ng_hci_cmd_pkt_t hdr;
ng_hci_le_create_connection_cp cp;
} __attribute__ ((packed)) *req = NULL;
ng_hci_lp_con_req_ep *ep = NULL;
ng_hci_unit_con_p con = NULL;
struct mbuf *m = NULL;
int error = 0;
ep = (ng_hci_lp_con_req_ep *)(NGI_MSG(item)->data);
if((link_type != NG_HCI_LINK_LE_PUBLIC)&&
(link_type != NG_HCI_LINK_LE_RANDOM)){
printf("%s: Link type %d Cannot be here \n", __func__,
link_type);
}
/*
* Only one ACL connection can exist between each pair of units.
* So try to find ACL connection descriptor (in any state) that
* has requested remote BD_ADDR.
*
* Two cases:
*
* 1) We do not have connection to the remote unit. This is simple.
* Just create new connection descriptor and send HCI command to
* create new connection.
*
* 2) We do have connection descriptor. We need to check connection
* state:
*
* 2.1) NG_HCI_CON_W4_LP_CON_RSP means that we are in the middle of
* accepting connection from the remote unit. This is a race
* condition. We will ignore this message.
*
* 2.2) NG_HCI_CON_W4_CONN_COMPLETE means that upper layer already
* requested connection or we just accepted it. In any case
* all we need to do here is set appropriate notification bit
* and wait.
*
* 2.3) NG_HCI_CON_OPEN means connection is open. Just reply back
* and let upper layer know that we have connection already.
*/
con = ng_hci_con_by_bdaddr(unit, &ep->bdaddr, link_type);
if (con != NULL) {
switch (con->state) {
case NG_HCI_CON_W4_LP_CON_RSP: /* XXX */
error = EALREADY;
break;
case NG_HCI_CON_W4_CONN_COMPLETE:
if (hook != unit->sco)
con->flags |= NG_HCI_CON_NOTIFY_ACL;
else
con->flags |= NG_HCI_CON_NOTIFY_SCO;
break;
case NG_HCI_CON_OPEN: {
struct ng_mesg *msg = NULL;
ng_hci_lp_con_cfm_ep *cfm = NULL;
if (hook != NULL && NG_HOOK_IS_VALID(hook)) {
NGI_GET_MSG(item, msg);
NG_FREE_MSG(msg);
NG_MKMESSAGE(msg, NGM_HCI_COOKIE,
NGM_HCI_LP_CON_CFM, sizeof(*cfm),
M_NOWAIT);
if (msg != NULL) {
cfm = (ng_hci_lp_con_cfm_ep *)msg->data;
cfm->status = 0;
cfm->link_type = con->link_type;
cfm->con_handle = con->con_handle;
bcopy(&con->bdaddr, &cfm->bdaddr,
sizeof(cfm->bdaddr));
/*
* This will forward item back to
* sender and set item to NULL
*/
_NGI_MSG(item) = msg;
NG_FWD_ITEM_HOOK(error, item, hook);
} else
error = ENOMEM;
} else
NG_HCI_INFO(
"%s: %s - Source hook is not valid, hook=%p\n",
__func__, NG_NODE_NAME(unit->node),
hook);
} break;
default:
panic(
"%s: %s - Invalid connection state=%d\n",
__func__, NG_NODE_NAME(unit->node), con->state);
break;
}
goto out;
}
/*
* If we got here then we need to create new ACL connection descriptor
* and submit HCI command. First create new connection desriptor, set
* bdaddr and notification flags.
*/
con = ng_hci_new_con(unit, link_type);
if (con == NULL) {
error = ENOMEM;
goto out;
}
bcopy(&ep->bdaddr, &con->bdaddr, sizeof(con->bdaddr));
/*
* Create HCI command
*/
MGETHDR(m, M_NOWAIT, MT_DATA);
if (m == NULL) {
ng_hci_free_con(con);
error = ENOBUFS;
goto out;
}
m->m_pkthdr.len = m->m_len = sizeof(*req);
req = mtod(m, struct acl_con_req *);
req->hdr.type = NG_HCI_CMD_PKT;
req->hdr.length = sizeof(req->cp);
req->hdr.opcode = htole16(NG_HCI_OPCODE(NG_HCI_OGF_LE,
NG_HCI_OCF_LE_CREATE_CONNECTION));
bcopy(&ep->bdaddr, &req->cp.peer_addr, sizeof(req->cp.peer_addr));
req->cp.own_address_type = 0;
req->cp.peer_addr_type = (link_type == NG_HCI_LINK_LE_RANDOM)? 1:0;
req->cp.scan_interval = htole16(4);
req->cp.scan_window = htole16(4);
req->cp.filter_policy = 0;
req->cp.conn_interval_min = htole16(0xf);
req->cp.conn_interval_max = htole16(0xf);
req->cp.conn_latency = htole16(0);
req->cp.supervision_timeout = htole16(0xc80);
req->cp.min_ce_length = htole16(1);
req->cp.max_ce_length = htole16(1);
/*
* Adust connection state
*/
if (hook != unit->sco)
con->flags |= NG_HCI_CON_NOTIFY_ACL;
else
con->flags |= NG_HCI_CON_NOTIFY_SCO;
con->state = NG_HCI_CON_W4_CONN_COMPLETE;
ng_hci_con_timeout(con);
/*
* Queue and send HCI command
*/
NG_BT_MBUFQ_ENQUEUE(&unit->cmdq, m);
if (!(unit->state & NG_HCI_UNIT_COMMAND_PENDING))
error = ng_hci_send_command(unit);
out:
if (item != NULL)
NG_FREE_ITEM(item);
return (error);
} /* ng_hci_lp_acl_con_req */
