static uint8_t
iwm_mvm_lmac_scan_fill_channels(struct iwm_softc *sc,
struct iwm_scan_channel_cfg_lmac *chan, int n_ssids)
{
struct ieee80211com *ic = &sc->sc_ic;
struct ieee80211_scan_state *ss = ic->ic_scan;
struct ieee80211_channel *c;
uint8_t nchan;
int j;
for (nchan = j = 0;
j < ss->ss_last && nchan < sc->ucode_capa.n_scan_channels; j++) {
c = ss->ss_chans[j];
/*
* Catch other channels, in case we have 900MHz channels or
* something in the chanlist.
*/
if (!IEEE80211_IS_CHAN_2GHZ(c) && !IEEE80211_IS_CHAN_5GHZ(c)) {
IWM_DPRINTF(sc, IWM_DEBUG_RESET | IWM_DEBUG_EEPROM,
"%s: skipping channel (freq=%d, ieee=%d, flags=0x%08x)\n",
__func__, c->ic_freq, c->ic_ieee, c->ic_flags);
continue;
}
IWM_DPRINTF(sc, IWM_DEBUG_RESET | IWM_DEBUG_EEPROM,
"Adding channel %d (%d Mhz) to the list\n",
nchan, c->ic_freq);
chan->channel_num = htole16(ieee80211_mhz2ieee(c->ic_freq, 0));
chan->iter_count = htole16(1);
chan->iter_interval = htole32(0);
chan->flags = htole32(IWM_UNIFIED_SCAN_CHANNEL_PARTIAL);
chan->flags |= htole32(IWM_SCAN_CHANNEL_NSSIDS(n_ssids));
/* XXX IEEE80211_SCAN_NOBCAST flag is never set. */
if (!IEEE80211_IS_CHAN_PASSIVE(c) &&
(!(ss->ss_flags & IEEE80211_SCAN_NOBCAST) || n_ssids != 0))
chan->flags |= htole32(IWM_SCAN_CHANNEL_TYPE_ACTIVE);
chan++;
nchan++;
}
return nchan;
}
void send_assoc(struct params *p)
{
union {
struct ieee80211_frame w;
char buf[2048];
} u;
struct ieee80211_frame *wh;
char *data;
int len, capinfo, lintval;
memset(&u, 0, sizeof(u));
wh = (struct ieee80211_frame*) &u.w;
fill_basic(wh, p);
wh->i_fc[0] |= IEEE80211_FC0_TYPE_MGT | IEEE80211_FC0_SUBTYPE_ASSOC_REQ;
data = (char*) (wh + 1);
/* capability */
capinfo = IEEE80211_CAPINFO_ESS;
if (p->wep_len)
capinfo |= IEEE80211_CAPINFO_PRIVACY;
*(uint16_t *)data = htole16(capinfo);
data += 2;
/* listen interval */
*(uint16_t *)data = htole16(100);
data += 2;
data = ieee80211_add_ssid(data, p->ssid, strlen(p->ssid));
*data++ = 1; /* rates */
*data++ = 4;
*data++ = 2 | 0x80;
*data++ = 4 | 0x80;
*data++ = 11;
*data++ = 22;
len = data - (char*)wh;
send_frame(p, u.buf, len);
}
int
host_hci_cmd_rd_rem_version(uint16_t handle)
{
int rc;
uint8_t cmd[sizeof(uint16_t)];
htole16(cmd, handle);
rc = host_hci_cmd_send(BLE_HCI_OGF_LINK_CTRL,
BLE_HCI_OCF_RD_REM_VER_INFO, sizeof(uint16_t), cmd);
return rc;
}
void
gdt_mpr_copy_cmd(struct gdt_softc *gdt, struct gdt_ccb *gccb)
{
u_int16_t cp_count = roundup(gccb->gc_cmd_len, sizeof (u_int32_t));
u_int16_t dp_offset = gdt->sc_cmd_off;
u_int16_t cmd_no = gdt->sc_cmd_cnt++;
GDT_DPRINTF(GDT_D_CMD, ("gdt_mpr_copy_cmd(%p) ", gdt));
gdt->sc_cmd_off += cp_count;
bus_write_region_4(gdt->sc_dpmem, GDT_MPR_IC + GDT_DPR_CMD + dp_offset,
(u_int32_t *)gccb->gc_cmd, cp_count >> 2);
bus_write_2(gdt->sc_dpmem,
GDT_MPR_IC + GDT_COMM_QUEUE + cmd_no * GDT_COMM_Q_SZ + GDT_OFFSET,
htole16(GDT_DPMEM_COMMAND_OFFSET + dp_offset));
bus_write_2(gdt->sc_dpmem,
GDT_MPR_IC + GDT_COMM_QUEUE + cmd_no * GDT_COMM_Q_SZ + GDT_SERV_ID,
htole16(gccb->gc_service));
}
gboolean
qmi_message_tlv_add (QmiMessage *self,
guint8 type,
gsize length,
gconstpointer value,
GError **error)
{
size_t tlv_len;
struct tlv *tlv;
g_assert (self != NULL);
g_assert ((length == 0) || value != NULL);
/* Make sure nothing's broken to start. */
if (!qmi_message_check (self, error)) {
g_prefix_error (error, "Invalid QMI message detected: ");
return FALSE;
}
/* Find length of new TLV. */
tlv_len = sizeof (struct tlv) + length;
/* Check for overflow of message size. */
if (qmux_length (self) + tlv_len > UINT16_MAX) {
g_set_error (error,
QMI_CORE_ERROR,
QMI_CORE_ERROR_TLV_TOO_LONG,
"TLV to add is too long");
return FALSE;
}
/* Resize buffer. */
self->len += tlv_len;
self->buf = g_realloc (self->buf, self->len);
/* Fill in new TLV. */
tlv = (struct tlv *)(qmi_end (self) - tlv_len);
tlv->type = type;
tlv->length = htole16 (length);
if (value)
memcpy (tlv->value, value, length);
/* Update length fields. */
set_qmux_length (self, (uint16_t)(qmux_length (self) + tlv_len));
set_qmi_tlv_length (self, (uint16_t)(qmi_tlv_length(self) + tlv_len));
/* Make sure we didn't break anything. */
if (!qmi_message_check (self, error)) {
g_prefix_error (error, "Invalid QMI message built: ");
return FALSE;
}
return TRUE;
}
static void sig_winch(int sig) {
unsigned short width,height;
struct mt_packet data;
int plen;
/* terminal height/width has changed, inform server */
if (get_terminal_size(&width, &height) != -1) {
init_packet(&data, MT_PTYPE_DATA, srcmac, dstmac, sessionkey, outcounter);
width = htole16(width);
height = htole16(height);
plen = add_control_packet(&data, MT_CPTYPE_TERM_WIDTH, &width, 2);
plen += add_control_packet(&data, MT_CPTYPE_TERM_HEIGHT, &height, 2);
outcounter += plen;
send_udp(&data, 1);
}
/* reinstate signal handler */
signal(SIGWINCH, sig_winch);
}
int gme_oidlist_contains(const struct gme_oidlist* ol,uint16_t oid) {
oid=htole16(oid);
int i=ol->len;
const uint16_t* p=ol->entries;
while (i) {
if (*p==oid) return 1;
p++;
i--;
}
return 0;
}
/**
* Read the RSSI for a given connection handle
*
* NOTE: OGF=0x05 OCF=0x0005
*
* @param handle
*
* @return int
*/
int
host_hci_cmd_read_rssi(uint16_t handle)
{
int rc;
uint8_t cmd[sizeof(uint16_t)];
htole16(cmd, handle);
rc = host_hci_cmd_send(BLE_HCI_OGF_STATUS_PARAMS, BLE_HCI_OCF_RD_RSSI,
sizeof(uint16_t), cmd);
return rc;
}
/**
* Read the channel map for a given connection.
*
* @param handle
*
* @return int
*/
int
host_hci_cmd_le_rd_chanmap(uint16_t handle)
{
int rc;
uint8_t cmd[BLE_HCI_RD_CHANMAP_LEN];
htole16(cmd, handle);
rc = host_hci_le_cmd_send(BLE_HCI_OCF_LE_RD_CHAN_MAP,
BLE_HCI_RD_CHANMAP_LEN, cmd);
return rc;
}
void
ble_hs_test_util_build_cmd_complete(uint8_t *dst, int len,
uint8_t param_len, uint8_t num_pkts,
uint16_t opcode)
{
TEST_ASSERT(len >= BLE_HCI_EVENT_CMD_COMPLETE_HDR_LEN);
dst[0] = BLE_HCI_EVCODE_COMMAND_COMPLETE;
dst[1] = 3 + param_len;
dst[2] = num_pkts;
htole16(dst + 3, opcode);
}
void set_fragno(struct packet *pkt, uint8_t frag, int last_frag) {
struct ieee_hdr *hdr = (struct ieee_hdr *) (pkt->data);
uint16_t seq = letoh16(hdr->frag_seq);
if (last_frag) hdr->flags &= 0xFB;
else hdr->flags |= 0x04;
seq &= 0xFFF0; //Clear frag bits
seq |= frag;
hdr->frag_seq = htole16(seq);
}
void
rgph_u32x3_jenkins2v_u16(uint16_t value, uint32_t seed, uint32_t *h)
{
h[0] = RGPH_JENKINS2V_SEED1;
h[1] = RGPH_JENKINS2V_SEED2;
h[2] = seed;
h[0] += htole16(value);
h[2] += sizeof(value);
rgph_jenkins2_mix(h);
}
/**
* OGF=LE, OCF=0x0016
*/
int
host_hci_cmd_le_read_rem_used_feat(uint16_t handle)
{
int rc;
uint8_t cmd[BLE_HCI_CONN_RD_REM_FEAT_LEN];
htole16(cmd, handle);
rc = host_hci_le_cmd_send(BLE_HCI_OCF_LE_RD_REM_FEAT,
BLE_HCI_CONN_RD_REM_FEAT_LEN,
cmd);
return rc;
}
static void send_auth(char *username, char *password) {
struct mt_packet data;
unsigned short width = 0;
unsigned short height = 0;
char *terminal = getenv("TERM");
char md5data[100];
unsigned char md5sum[17];
int plen;
md5_state_t state;
/* Concat string of 0 + password + encryptionkey */
md5data[0] = 0;
strncpy(md5data + 1, password, 82);
md5data[83] = '\0';
memcpy(md5data + 1 + strlen(password), encryptionkey, 16);
/* Generate md5 sum of md5data with a leading 0 */
md5_init(&state);
md5_append(&state, (const md5_byte_t *)md5data, strlen(password) + 17);
md5_finish(&state, (md5_byte_t *)md5sum + 1);
md5sum[0] = 0;
/* Send combined packet to server */
init_packet(&data, MT_PTYPE_DATA, srcmac, dstmac, sessionkey, outcounter);
plen = add_control_packet(&data, MT_CPTYPE_PASSWORD, md5sum, 17);
plen += add_control_packet(&data, MT_CPTYPE_USERNAME, username, strlen(username));
plen += add_control_packet(&data, MT_CPTYPE_TERM_TYPE, terminal, strlen(terminal));
if (is_a_tty && get_terminal_size(&width, &height) != -1) {
width = htole16(width);
height = htole16(height);
plen += add_control_packet(&data, MT_CPTYPE_TERM_WIDTH, &width, 2);
plen += add_control_packet(&data, MT_CPTYPE_TERM_HEIGHT, &height, 2);
}
outcounter += plen;
/* TODO: handle result */
send_udp(&data, 1);
}
/* Insert node into existing li-record at position. Pretty much the
* same as insert_lf_record above, but the record layout is a bit
* different.
*/
static size_t
insert_li_record (hive_h *h, size_t old_offs, size_t posn,
const char *name, hive_node_h node)
{
assert (IS_VALID_BLOCK (h, old_offs));
assert (block_id_eq (h, old_offs, "li"));
struct ntreg_ri_record *old_li =
(struct ntreg_ri_record *) ((char *) h->addr + old_offs);
size_t nr_offsets = le16toh (old_li->nr_offsets);
if (nr_offsets == UINT16_MAX) {
SET_ERRNO (EOVERFLOW,
"cannot extend record because it already contains the maximum number of subkeys (%zu)",
nr_offsets);
return 0;
}
nr_offsets++; /* in new record ... */
size_t seg_len = sizeof (struct ntreg_ri_record) + (nr_offsets-1) * 4;
/* Copy the old_li->id in case it moves during allocate_block. */
char id[2];
memcpy (id, old_li->id, sizeof id);
size_t new_offs = allocate_block (h, seg_len, id);
if (new_offs == 0)
return 0;
/* old_li could have been invalidated by allocate_block. */
old_li = (struct ntreg_ri_record *) ((char *) h->addr + old_offs);
struct ntreg_ri_record *new_li =
(struct ntreg_ri_record *) ((char *) h->addr + new_offs);
new_li->nr_offsets = htole16 (nr_offsets);
/* Copy the offsets until we reach posn, insert the new offset
* there, then copy the remaining offsets.
*/
size_t i;
for (i = 0; i < posn; ++i)
new_li->offset[i] = old_li->offset[i];
new_li->offset[i] = htole32 (node - 0x1000);
for (i = posn+1; i < nr_offsets; ++i)
new_li->offset[i] = old_li->offset[i-1];
/* Old block is unused, return new block. */
mark_block_unused (h, old_offs);
return new_offs;
}
/*
* util_convert2le_hdr -- convert pool_hdr into little-endian byte order
*/
void
util_convert2le_hdr(struct pool_hdr *hdrp)
{
hdrp->major = htole32(hdrp->major);
hdrp->compat_features = htole32(hdrp->compat_features);
hdrp->incompat_features = htole32(hdrp->incompat_features);
hdrp->ro_compat_features = htole32(hdrp->ro_compat_features);
hdrp->arch_flags.alignment_desc =
htole64(hdrp->arch_flags.alignment_desc);
hdrp->arch_flags.e_machine = htole16(hdrp->arch_flags.e_machine);
hdrp->crtime = htole64(hdrp->crtime);
hdrp->checksum = htole64(hdrp->checksum);
}
int
host_hci_cmd_le_conn_param_neg_reply(struct hci_conn_param_neg_reply *hcn)
{
uint8_t cmd[BLE_HCI_CONN_PARAM_NEG_REPLY_LEN];
int rc;
htole16(cmd + 0, hcn->handle);
cmd[2] = hcn->reason;
rc = host_hci_le_cmd_send(BLE_HCI_OCF_LE_REM_CONN_PARAM_NRR,
BLE_HCI_CONN_PARAM_NEG_REPLY_LEN, cmd);
return rc;
}
int
malo_hal_setmcast(struct malo_hal *mh, int nmc, const uint8_t macs[])
{
struct malo_cmd_mcast *cmd;
int ret;
if (nmc > MALO_HAL_MCAST_MAX)
return EINVAL;
MALO_HAL_LOCK(mh);
_CMD_SETUP(cmd, struct malo_cmd_mcast, MALO_HOSTCMD_MAC_MULTICAST_ADR);
memcpy(cmd->maclist, macs, nmc * IEEE80211_ADDR_LEN);
cmd->numaddr = htole16(nmc);
cmd->action = htole16(0xffff);
ret = malo_hal_execute_cmd(mh, MALO_HOSTCMD_MAC_MULTICAST_ADR);
MALO_HAL_UNLOCK(mh);
return ret;
}
static int
ble_att_prep_write_cmd_write(void *payload, int len,
struct ble_att_prep_write_cmd *cmd, int is_req)
{
uint8_t *u8ptr;
if (len < BLE_ATT_PREP_WRITE_CMD_BASE_SZ) {
return BLE_HS_EMSGSIZE;
}
u8ptr = payload;
if (is_req) {
u8ptr[0] = BLE_ATT_OP_PREP_WRITE_REQ;
} else {
u8ptr[0] = BLE_ATT_OP_PREP_WRITE_RSP;
}
htole16(u8ptr + 1, cmd->bapc_handle);
htole16(u8ptr + 3, cmd->bapc_offset);
return 0;
}
static uint16_t
iwm_mvm_scan_rx_chain(struct iwm_softc *sc)
{
uint16_t rx_chain;
uint8_t rx_ant;
rx_ant = IWM_FW_VALID_RX_ANT(sc);
rx_chain = rx_ant << IWM_PHY_RX_CHAIN_VALID_POS;
rx_chain |= rx_ant << IWM_PHY_RX_CHAIN_FORCE_MIMO_SEL_POS;
rx_chain |= rx_ant << IWM_PHY_RX_CHAIN_FORCE_SEL_POS;
rx_chain |= 0x1 << IWM_PHY_RX_CHAIN_DRIVER_FORCE_POS;
return htole16(rx_chain);
}
void
ble_sm_master_id_write(void *payload, int len, struct ble_sm_master_id *cmd)
{
uint8_t *u8ptr;
BLE_HS_DBG_ASSERT(len >= BLE_SM_HDR_SZ + BLE_SM_MASTER_ID_SZ);
u8ptr = payload;
u8ptr[0] = BLE_SM_OP_MASTER_ID;
htole16(u8ptr + 1, cmd->ediv);
htole64(u8ptr + 3, cmd->rand_val);
}
void
ble_hs_test_util_build_cmd_status(uint8_t *dst, int len,
uint8_t status, uint8_t num_pkts,
uint16_t opcode)
{
TEST_ASSERT(len >= BLE_HCI_EVENT_CMD_STATUS_LEN);
dst[0] = BLE_HCI_EVCODE_COMMAND_STATUS;
dst[1] = BLE_HCI_EVENT_CMD_STATUS_LEN;
dst[2] = status;
dst[3] = num_pkts;
htole16(dst + 4, opcode);
}
uint64_t
rgph_u64_xxh64s_u16(uint16_t value, uint32_t seed)
{
uint64_t h[4];
rgph_xxh64s_init(sizeof(value), seed, h);
rgph_xxh64s_fold(sizeof(value), h);
rgph_xxh64s_fmix1(htole16(value) & 0xff, h);
rgph_xxh64s_fmix1(htole16(value) >> 8, h);
rgph_xxh64s_finalise(h);
return h[0];
}
//******************************************************************************
/// \brief Write read command packet to MXT chip
/// \return #mxt_rc
static int hidraw_write_read_cmd(struct mxt_device *mxt, uint16_t start_register, uint8_t count, uint8_t *byte_count)
{
struct hid_packet cmd_pkt = { 0 };
cmd_pkt.report_id = mxt->conn->hidraw.report_id;
cmd_pkt.cmd = 0x51;
cmd_pkt.rx_bytes = 2; /* includes start address word */
cmd_pkt.tx_bytes = count;
cmd_pkt.address = htole16(start_register);
mxt_dbg(mxt->ctx, "Sending read command");
return hidraw_write_packet(mxt, &cmd_pkt, byte_count);
}
tlibc_error_code_t tlibc_binary_write_int16(tlibc_abstract_writer_t *super, const int16_t *val)
{
tlibc_binary_writer_t *self = TLIBC_CONTAINER_OF(super, tlibc_binary_writer_t, super);
if(WRITER_CAPACITY(self) < sizeof(int16_t))
{
goto not_enough_bytebuff_size;
}
*(int16_t*)WRITER_PTR(self) =htole16(*val);
self->offset += (uint32_t)sizeof(int16_t);
return E_TLIBC_NOERROR;
not_enough_bytebuff_size:
return E_TLIBC_OUT_OF_MEMORY;
}
int
host_hci_cmd_disconnect(uint16_t handle, uint8_t reason)
{
int rc;
uint8_t cmd[BLE_HCI_DISCONNECT_CMD_LEN];
htole16(cmd, handle);
cmd[2] = reason;
rc = host_hci_cmd_send(BLE_HCI_OGF_LINK_CTRL,
BLE_HCI_OCF_DISCONNECT_CMD,
BLE_HCI_DISCONNECT_CMD_LEN,
cmd);
return rc;
}
static void fill_basic(struct wstate *ws, struct ieee80211_frame* wh)
{
unsigned short *sp;
memcpy(wh->i_addr1, ws->ws_bss, 6);
memcpy(wh->i_addr2, ws->ws_mymac, 6);
memcpy(wh->i_addr3, ws->ws_bss, 6);
sp = (unsigned short*) wh->i_seq;
*sp = fnseq(0, ws->ws_psent);
sp = (unsigned short*) wh->i_dur;
*sp = htole16(32767);
}
//******************************************************************************
/// \brief Write register from MXT chip
/// \return #mxt_rc
int hidraw_write_register(struct mxt_device *mxt, unsigned char const *val, uint16_t start_register, int datalength)
{
int ret;
uint8_t byte_count;
struct hid_packet write_pkt;
memset(&write_pkt, 0x00, sizeof(struct hid_packet));
write_pkt.report_id = mxt->conn->hidraw.report_id;
write_pkt.cmd = 0x51;
write_pkt.tx_bytes = 0;
mxt_dbg(mxt->ctx, "%s - start_register:%d No. bytes to write:%d",
__func__, start_register, datalength);
ret = hidraw_open(mxt);
if (ret)
return ret;
int bytes_written = 0;
while (bytes_written < datalength) {
write_pkt.rx_bytes = MXT_HID_ADDR_SIZE +
(datalength - bytes_written <= MXT_HID_WRITE_DATA_SIZE ? datalength - bytes_written : MXT_HID_WRITE_DATA_SIZE);
write_pkt.address = htole16(start_register + bytes_written);
memcpy(write_pkt.write_data, val + bytes_written, write_pkt.rx_bytes);
ret = hidraw_write_packet(mxt, &write_pkt, &byte_count);
if (ret) {
mxt_err(mxt->ctx, "read error %s (%d)", strerror(errno), errno);
ret = mxt_errno_to_rc(errno);
goto close;
}
bytes_written += byte_count;
mxt_dbg(mxt->ctx, "Bytes Written:%d", bytes_written);
struct hid_packet response_pkt;
memset(&response_pkt, 0x00, sizeof(struct hid_packet));
ret = hidraw_read_response(mxt, &response_pkt, 2);
if (response_pkt.result != MXT_HID_READ_SUCCESS)
mxt_err(mxt->ctx, "HIDRAW write failed: 0x%x",
response_pkt.result);
}
close:
close(mxt->conn->hidraw.fd);
mxt->conn->hidraw.fd = 0;
return MXT_SUCCESS;
}
void setup_test(struct test_case *cse, struct ip6_hdr *hdr, struct ieee154_frame_addr *frame) {
uint32_t val;
val = htonl(0x6 << 28 | ((cse->tc & 0xff) << 20) | (cse->fl & 0x000fffff));
hdr->ip6_flow = val;
hdr->ip6_nxt = cse->nxt;
hdr->ip6_plen = 0;
hdr->ip6_hlim = cse->hlim;
inet_pton6(cse->ip6_src, &hdr->ip6_src);
inet_pton6(cse->ip6_dst, &hdr->ip6_dst);
memset(frame, 0, sizeof(frame));
ieee154_parse(cse->l2src, &frame->ieee_src);
ieee154_parse(cse->l2dst, &frame->ieee_dst);
frame->ieee_dstpan = htole16(cse->panid);
}
void
iwl5000_update_sched(struct iwl_softc *sc, int qid, int idx, uint8_t id,
uint16_t len)
{
uint16_t *w = &sc->sched[qid * IWL5000_SCHED_COUNT + idx];
*w = htole16(id << 12 | (len + 8));
bus_dmamap_sync(sc->sched_dma.tag, sc->sched_dma.map,
BUS_DMASYNC_PREWRITE);
if (idx < IWL_SCHED_WINSZ) {
*(w + IWL_TX_RING_COUNT) = *w;
bus_dmamap_sync(sc->sched_dma.tag, sc->sched_dma.map,
BUS_DMASYNC_PREWRITE);
}
}