/* Send Read_Remote_Supported_Features command to the unit */
static int
hci_read_remote_supported_features(int s, int argc, char **argv)
{
int n;
char b[512];
ng_hci_read_remote_features_cp cp;
ng_hci_event_pkt_t *e = (ng_hci_event_pkt_t *) b;
char buffer[1024];
/* parse command parameters */
switch (argc) {
case 1:
/* connecton handle */
if (sscanf(argv[0], "%d", &n) != 1 || n < 0 || n > 0x0eff)
return (USAGE);
cp.con_handle = (n & 0x0fff);
cp.con_handle = htole16(cp.con_handle);
break;
default:
return (USAGE);
}
/* send request and expect status response */
n = sizeof(b);
if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LINK_CONTROL,
NG_HCI_OCF_READ_REMOTE_FEATURES),
(char const *) &cp, sizeof(cp), b, &n) == ERROR)
return (ERROR);
if (*b != 0x00)
return (FAILED);
/* wait for event */
again:
n = sizeof(b);
if (hci_recv(s, b, &n) == ERROR)
return (ERROR);
if (n < sizeof(*e)) {
errno = EIO;
return (ERROR);
}
if (e->event == NG_HCI_EVENT_READ_REMOTE_FEATURES_COMPL) {
ng_hci_read_remote_features_compl_ep *ep =
(ng_hci_read_remote_features_compl_ep *)(e + 1);
if (ep->status != 0x00) {
fprintf(stdout, "Status: %s [%#02x]\n",
hci_status2str(ep->status), ep->status);
return (FAILED);
}
fprintf(stdout, "Connection handle: %d\n",
le16toh(ep->con_handle));
fprintf(stdout, "Features: ");
for (n = 0; n < sizeof(ep->features); n++)
fprintf(stdout, "%#02x ", ep->features[n]);
fprintf(stdout, "\n%s\n", hci_features2str(ep->features,
buffer, sizeof(buffer)));
} else
goto again;
return (OK);
} /* hci_read_remote_supported_features */
static int
repack(bool flatten, const char *dstpath, const char *srcpath)
{
char *src_zip = (char *)map_file(srcpath);
if (!src_zip) {
printf("Could not open zip file.\n");
return -1;
}
int fd = creat(dstpath, 0777);
if (fd == -1) {
printf("can't open output file\n");
return -1;
}
struct cdir_end *dirend = (struct cdir_end *)(src_zip + zip_size - sizeof(*dirend));
while ((void *)dirend > src_zip &&
le32toh(dirend->signature) != 0x06054b50)
dirend = (struct cdir_end *)((char *)dirend - 1);
if (le32toh(dirend->signature) != 0x06054b50) {
printf("couldn't find end of central directory record!\n");
return -1;
}
uint32_t cdir_offset = le32toh(dirend->cdir_offset);
uint16_t cdir_entries = le16toh(dirend->cdir_entries);
uint32_t cdir_size = le32toh(dirend->cdir_size);
TRACE("Found %d entries. cdir offset at %d\n",
cdir_entries, cdir_offset);
struct cdir_entry *cdir_start = (struct cdir_entry *)(src_zip + cdir_offset);
struct cdir_entry *new_cdir_start = (struct cdir_entry *)malloc(cdir_size);
if (!new_cdir_start) {
TRACE("couldn't allocate central directory copy\n");
return -1;
}
memcpy(new_cdir_start, cdir_start, cdir_size);
uint32_t lowest_offset = find_lowest_offset(cdir_start, cdir_entries);
uint32_t out_offset = simple_write(fd, src_zip, lowest_offset);
struct cdir_entry *current_entry = new_cdir_start;
uint16_t i = cdir_entries;
while (i--) {
struct local_file_header *file = (struct local_file_header *)(src_zip + le32toh(current_entry->offset));
int rc;
if (flatten)
rc = flatten_entry(fd, file, current_entry, out_offset);
else
rc = squeeze_entry(fd, file, current_entry, out_offset);
if (rc)
return rc;
current_entry = (struct cdir_entry *)((char *)current_entry + cdir_entry_size(current_entry));
}
uint32_t new_cdir_offset;
if (cdir_offset < lowest_offset) {
TRACE("Doing in place cdir replacement at %d\n", cdir_offset);
new_cdir_offset = cdir_offset;
lseek(fd, SEEK_SET, cdir_offset);
simple_write(fd, (char *)new_cdir_start, cdir_size);
lseek(fd, SEEK_END, 0);
} else {
new_cdir_offset = out_offset;
TRACE("Appending cdir at %d\n", new_cdir_offset);
simple_write(fd, (char *)new_cdir_start, cdir_size);
}
struct cdir_end end;
memcpy(&end, dirend, sizeof(end));
end.cdir_offset = htole32(new_cdir_offset);
simple_write(fd, (char *)&end, sizeof(end));
close(fd);
munmap(src_zip, zip_size);
return 0;
}
int
iwm_phy_db_set_section(struct iwm_phy_db *phy_db,
struct iwm_rx_packet *pkt)
{
struct iwm_calib_res_notif_phy_db *phy_db_notif =
(struct iwm_calib_res_notif_phy_db *)pkt->data;
enum iwm_phy_db_section_type type = le16toh(phy_db_notif->type);
uint16_t size = le16toh(phy_db_notif->length);
struct iwm_phy_db_entry *entry;
uint16_t chg_id = 0;
if (!phy_db)
return EINVAL;
if (type == IWM_PHY_DB_CALIB_CHG_PAPD) {
chg_id = le16toh(*(uint16_t *)phy_db_notif->data);
if (phy_db && !phy_db->calib_ch_group_papd) {
/*
* Firmware sends the largest index first, so we can use
* it to know how much we should allocate.
*/
phy_db->calib_ch_group_papd = malloc(
(chg_id + 1) * sizeof(struct iwm_phy_db_entry),
M_DEVBUF, M_NOWAIT | M_ZERO);
if (!phy_db->calib_ch_group_papd)
return ENOMEM;
phy_db->n_group_papd = chg_id + 1;
}
} else if (type == IWM_PHY_DB_CALIB_CHG_TXP) {
chg_id = le16toh(*(uint16_t *)phy_db_notif->data);
if (phy_db && !phy_db->calib_ch_group_txp) {
/*
* Firmware sends the largest index first, so we can use
* it to know how much we should allocate.
*/
phy_db->calib_ch_group_txp = malloc(
(chg_id + 1) * sizeof(struct iwm_phy_db_entry),
M_DEVBUF, M_NOWAIT | M_ZERO);
if (!phy_db->calib_ch_group_txp)
return ENOMEM;
phy_db->n_group_txp = chg_id + 1;
}
}
entry = iwm_phy_db_get_section(phy_db, type, chg_id);
if (!entry)
return EINVAL;
if (entry->data != NULL)
free(entry->data, M_DEVBUF);
entry->data = malloc(size, M_DEVBUF, M_NOWAIT);
if (!entry->data) {
entry->size = 0;
return ENOMEM;
}
memcpy(entry->data, phy_db_notif->data, size);
entry->size = size;
IWM_DPRINTF(phy_db->sc, IWM_DEBUG_RESET,
"%s(%d): [PHYDB]SET: Type %d , Size: %d\n",
__func__, __LINE__, type, size);
return 0;
}
int
parse_80211_header(const u_char* buf, int len, struct packet_info* p)
{
struct ieee80211_hdr* wh;
int hdrlen;
uint8_t *sa = NULL;
uint8_t *da = NULL;
u16 fc;
if (len < 2)
return -1;
wh = (struct ieee80211_hdr*)buf;
fc = le16toh(wh->frame_control);
hdrlen = ieee80211_get_hdrlen(fc);
DEBUG("len %d hdrlen %d\n", len, hdrlen);
if (len < hdrlen)
return -1;
p->wlan_len = len;
p->wlan_type = (fc & (IEEE80211_FCTL_FTYPE | IEEE80211_FCTL_STYPE));
switch (p->wlan_type & IEEE80211_FCTL_FTYPE) {
case IEEE80211_FTYPE_DATA:
p->pkt_types = PKT_TYPE_DATA;
switch (p->wlan_type & IEEE80211_FCTL_STYPE) {
case IEEE80211_STYPE_NULLFUNC:
p->pkt_types |= PKT_TYPE_NULL;
break;
case IEEE80211_STYPE_QOS_DATA:
/* TODO: ouch, should properly define a qos header */
p->wlan_qos_class = wh->addr4[0] & 0x7;
DEBUG("***QDATA %x\n", p->wlan_qos_class);
break;
}
sa = ieee80211_get_SA(wh);
da = ieee80211_get_DA(wh);
p->wlan_seqno = le16toh(wh->seq_ctrl) / 16;
if (fc & IEEE80211_FCTL_PROTECTED)
p->wlan_wep = 1;
if (fc & IEEE80211_FCTL_RETRY)
p->wlan_retry = 1;
break;
case IEEE80211_FTYPE_CTL:
p->pkt_types = PKT_TYPE_CTRL;
DEBUG("CTL\n");
switch (p->wlan_type & IEEE80211_FCTL_STYPE) {
case IEEE80211_STYPE_RTS:
p->pkt_types |= PKT_TYPE_RTS;
p->wlan_nav = le16toh(wh->duration_id);
DEBUG("RTS NAV %d\n", p->wlan_nav);
sa = wh->addr2;
da = wh->addr1;
break;
case IEEE80211_STYPE_CTS:
p->pkt_types |= PKT_TYPE_CTS;
p->wlan_nav = le16toh(wh->duration_id);
DEBUG("CTS NAV %d\n", p->wlan_nav);
da = wh->addr1;
break;
case IEEE80211_STYPE_ACK:
p->pkt_types |= PKT_TYPE_ACK;
p->wlan_nav = le16toh(wh->duration_id);
DEBUG("ACK NAV %d\n", p->wlan_nav);
da = wh->addr1;
break;
case IEEE80211_STYPE_PSPOLL:
sa = wh->addr2;
break;
case IEEE80211_STYPE_CFEND:
da = wh->addr1;
sa = wh->addr2;
break;
case IEEE80211_STYPE_CFENDACK:
/* dont know, dont care */
break;
}
break;
case IEEE80211_FTYPE_MGMT:
p->pkt_types = PKT_TYPE_MGMT;
DEBUG("MGMT\n");
break;
}
if (sa != NULL) {
memcpy(p->wlan_src, sa, 6);
DEBUG("SA %s\n", ether_sprintf(sa));
}
if (da != NULL) {
memcpy(p->wlan_dst, da, 6);
DEBUG("DA %s\n", ether_sprintf(da));
}
DEBUG("%s\n", get_packet_type_name(fc));
return 0;
}
static int
flatten_entry(int fd,
struct local_file_header *file,
struct cdir_entry *current_entry,
uint32_t &out_offset)
{
uint32_t compressed_size = le32toh(file->compressed_size);
uint32_t uncompressed_size = le32toh(file->uncompressed_size);
current_entry->compressed_size = current_entry->uncompressed_size;
current_entry->offset = htole32(out_offset);
current_entry->compression = 0;
TRACE("writing entry for %s (%d %d)\n", current_entry->data, compressed_size, uncompressed_size);
struct local_file_header *file_copy = (struct local_file_header *)malloc(file_header_size(file));
if (!file_copy) {
printf("couldn't allocate local file header copy\n");
return -1;
}
memcpy(file_copy, file, file_header_size(file));
file_copy->compressed_size = file_copy->uncompressed_size;
file_copy->compression = 0;
out_offset += simple_write(fd, (char *)file_copy, file_header_size(file));
free(file_copy);
if (!file->compression) {
out_offset += simple_write(fd, (char *)file + file_header_size(file), le32toh(file->uncompressed_size));
return 0;
}
char *buf = (char *)malloc(uncompressed_size);
if (!buf) {
printf("failed to malloc output buffer\n");
return -1;
}
z_stream zstr;
memset(&zstr, 0, sizeof(zstr));
if (inflateInit2(&zstr, -MAX_WBITS) != Z_OK) {
printf("inflateInit2 failed!\n");
return -1;
}
zstr.next_in = (Bytef *)(file->data + le16toh(file->filename_size) + le16toh(file->extra_field_size));
zstr.avail_in = compressed_size;
zstr.avail_out = uncompressed_size;
zstr.next_out = (Bytef *)buf;
if (inflate(&zstr, Z_SYNC_FLUSH) != Z_STREAM_END &&
zstr.total_out != uncompressed_size) {
printf("Failed to inflate everything total - %ld / %u\n", zstr.total_out, uncompressed_size);
return -1;
}
out_offset += simple_write(fd, buf, uncompressed_size);
free(buf);
inflateEnd(&zstr);
return 0;
}
static void proc_mgt(struct wstate *ws, int stype, unsigned char *body)
{
unsigned short * rc;
unsigned short * sc;
unsigned int aid;
if (stype == IEEE80211_FC0_SUBTYPE_DEAUTH) {
rc = (unsigned short*) body;
printf("\n");
time_print("Got deauth=%u\n", le16toh(*rc));
ws->ws_state = FOUND_VICTIM;
return;
} else if (stype == IEEE80211_FC0_SUBTYPE_AUTH) {
sc = (unsigned short*) body;
if (ws->ws_state != SENDING_AUTH) /* We didn't ask for it. */
return;
if (le16toh(*sc) != 0) {
time_print("Warning got auth algo=%x\n", le16toh(*sc));
exit(1);
return;
}
sc++;
if (le16toh(*sc) != 2) {
time_print("Warning got auth seq=%x\n", le16toh(*sc));
return;
}
sc++;
if (le16toh(*sc) == 1) {
time_print("Auth rejected. Spoofin mac.\n");
ws->ws_state = SPOOF_MAC;
return;
} else if (le16toh(*sc) == 0) {
time_print("Authenticated\n");
ws->ws_state = GOT_AUTH;
return;
} else {
time_print("Got auth %x\n", *sc);
exit(1);
}
}
else if (stype == IEEE80211_FC0_SUBTYPE_ASSOC_RESP) {
sc = (unsigned short*) body;
sc++; // cap
if (ws->ws_state != SENDING_ASSOC) /* We didn't ask for it. */
return;
if (le16toh(*sc) == 0) {
sc++;
aid = le16toh(*sc) & 0x3FFF;
time_print("Associated (ID=%x)\n", aid);
ws->ws_state = GOT_ASSOC;
return;
} else if (le16toh(*sc) == 12 || le16toh(*sc) == 1) {
time_print("Assoc rejected..."
" trying to spoof mac.\n");
ws->ws_state = SPOOF_MAC;
return;
} else {
time_print("got assoc %d\n", le16toh(*sc));
exit(1);
}
} else if (stype == IEEE80211_FC0_SUBTYPE_PROBE_RESP) {
return;
}
time_print("\nGOT MAN=%x\n", stype);
exit(1);
}
static void
get_radiotap_info(struct ieee80211_radiotap_iterator *iter, struct packet_info* p)
{
uint16_t x;
signed char c;
unsigned char known, flags, ht20, lgi;
switch (iter->this_arg_index) {
/* ignoring these */
case IEEE80211_RADIOTAP_TSFT:
case IEEE80211_RADIOTAP_FHSS:
case IEEE80211_RADIOTAP_LOCK_QUALITY:
case IEEE80211_RADIOTAP_TX_ATTENUATION:
case IEEE80211_RADIOTAP_DB_TX_ATTENUATION:
case IEEE80211_RADIOTAP_DBM_TX_POWER:
case IEEE80211_RADIOTAP_TX_FLAGS:
case IEEE80211_RADIOTAP_RX_FLAGS:
case IEEE80211_RADIOTAP_RTS_RETRIES:
case IEEE80211_RADIOTAP_DATA_RETRIES:
case IEEE80211_RADIOTAP_AMPDU_STATUS:
break;
case IEEE80211_RADIOTAP_FLAGS:
/* short preamble */
DEBUG("[flags %0x", *iter->this_arg);
if (*iter->this_arg & IEEE80211_RADIOTAP_F_SHORTPRE) {
p->phy_flags |= PHY_FLAG_SHORTPRE;
DEBUG(" shortpre");
}
if (*iter->this_arg & IEEE80211_RADIOTAP_F_BADFCS) {
p->phy_flags |= PHY_FLAG_BADFCS;
DEBUG(" badfcs");
}
DEBUG("]");
break;
case IEEE80211_RADIOTAP_RATE:
//TODO check!
//printf("\trate: %lf\n", (double)*iter->this_arg/2);
DEBUG("[rate %0x]", *iter->this_arg);
p->phy_rate = (*iter->this_arg)*5; /* rate is in 500kbps */
p->phy_rate_idx = rate_to_index(p->phy_rate);
break;
#define IEEE80211_CHAN_A \
(IEEE80211_CHAN_5GHZ | IEEE80211_CHAN_OFDM)
#define IEEE80211_CHAN_G \
(IEEE80211_CHAN_2GHZ | IEEE80211_CHAN_OFDM)
case IEEE80211_RADIOTAP_CHANNEL:
/* channel & channel type */
p->phy_freq = le16toh(*(uint16_t*)iter->this_arg);
DEBUG("[freq %d", p->phy_freq);
iter->this_arg = iter->this_arg + 2;
x = le16toh(*(uint16_t*)iter->this_arg);
if ((x & IEEE80211_CHAN_A) == IEEE80211_CHAN_A) {
p->phy_flags |= PHY_FLAG_A;
DEBUG("A]");
}
else if ((x & IEEE80211_CHAN_G) == IEEE80211_CHAN_G) {
p->phy_flags |= PHY_FLAG_G;
DEBUG("G]");
}
else if ((x & IEEE80211_CHAN_2GHZ) == IEEE80211_CHAN_2GHZ) {
p->phy_flags |= PHY_FLAG_B;
DEBUG("B]");
}
break;
case IEEE80211_RADIOTAP_DBM_ANTSIGNAL:
c = *(signed char*)iter->this_arg;
DEBUG("[sig %0d]", c);
/* we get the signal per rx chain with newer drivers.
* save the highest value, but make sure we don't override
* with invalid values */
if (c < 0 && (p->phy_signal == 0 || c > p->phy_signal))
p->phy_signal = c;
break;
case IEEE80211_RADIOTAP_DBM_ANTNOISE:
DEBUG("[noi %0x]", *(signed char*)iter->this_arg);
// usually not present
//p->phy_noise = *(signed char*)iter->this_arg;
break;
case IEEE80211_RADIOTAP_ANTENNA:
DEBUG("[ant %0x]", *iter->this_arg);
break;
case IEEE80211_RADIOTAP_DB_ANTSIGNAL:
DEBUG("[snr %0x]", *iter->this_arg);
// usually not present
//p->phy_snr = *iter->this_arg;
break;
case IEEE80211_RADIOTAP_DB_ANTNOISE:
//printf("\tantnoise: %02d\n", *iter->this_arg);
break;
case IEEE80211_RADIOTAP_MCS:
/* Ref http://www.radiotap.org/defined-fields/MCS */
known = *iter->this_arg++;
flags = *iter->this_arg++;
DEBUG("[MCS known %0x flags %0x index %0x]", known, flags, *iter->this_arg);
if (known & IEEE80211_RADIOTAP_MCS_HAVE_BW)
ht20 = (flags & IEEE80211_RADIOTAP_MCS_BW_MASK) == IEEE80211_RADIOTAP_MCS_BW_20;
else
ht20 = 1; /* assume HT20 if not present */
if (known & IEEE80211_RADIOTAP_MCS_HAVE_GI)
lgi = !(flags & IEEE80211_RADIOTAP_MCS_SGI);
else
lgi = 1; /* assume long GI if not present */
DEBUG(" %s %s", ht20 ? "HT20" : "HT40", lgi ? "LGI" : "SGI");
p->phy_rate_idx = 12 + *iter->this_arg;
p->phy_rate_flags = flags;
p->phy_rate = mcs_index_to_rate(*iter->this_arg, ht20, lgi);
DEBUG(" RATE %d ", p->phy_rate);
break;
default:
printlog("UNKNOWN RADIOTAP field %d", iter->this_arg_index);
break;
}
}
status_t
l2cap_receive(HciConnection* conn, net_buffer* buffer)
{
status_t error = B_OK;
uint16 dcid;
uint16 length;
#ifdef DUMP_L2CAP_FRAME
flowf("DUMP:");
for (uint i = 0; i < buffer->size; i++) {
uint8 c = 0;
gBufferModule->read(buffer, i, &c, 1);
dprintf("[%x]", c);
}
dprintf("\n");
#endif
// Check packet
if (buffer->size < sizeof(l2cap_hdr_t)) {
debugf("invalid L2CAP packet. Packet too small, len=%ld\n", buffer->size);
gBufferModule->free(buffer);
return EMSGSIZE;
}
// Get L2CAP header
NetBufferHeaderReader<l2cap_hdr_t> bufferHeader(buffer);
status_t status = bufferHeader.Status();
if (status < B_OK) {
return ENOBUFS;
}
length = bufferHeader->length = le16toh(bufferHeader->length);
dcid = bufferHeader->dcid = le16toh(bufferHeader->dcid);
debugf("len=%d cid=%x\n", length, dcid);
bufferHeader.Remove(); // pulling
// Check payload size
if (length != buffer->size ) {
debugf("Payload length mismatch, packetlen=%d, bufferlen=%ld\n",
length, buffer->size);
gBufferModule->free(buffer);
return EMSGSIZE;
}
// Process packet
switch (dcid) {
case L2CAP_SIGNAL_CID: // L2CAP command
error = l2cap_process_signal_cmd(conn, buffer);
break;
case L2CAP_CLT_CID: // Connectionless packet
// error = l2cap_cl_receive(buffer);
flowf("CL FRAME!!\n");
break;
default: // Data packet
error = l2cap_co_receive(conn, buffer, dcid);
break;
}
return (error);
}
/**
* mps_config_get_pd_pg0 - obtain raid phys disk page 0
* @sc: per adapter object
* @mpi_reply: reply mf payload returned from firmware
* @config_page: contents of the config page
* @page_address: form and handle value used to get page
* Context: sleep.
*
* Returns 0 for success, non-zero for failure.
*/
int
mps_config_get_raid_pd_pg0(struct mps_softc *sc, Mpi2ConfigReply_t *mpi_reply,
Mpi2RaidPhysDiskPage0_t *config_page, u32 page_address)
{
MPI2_CONFIG_REQUEST *request;
MPI2_CONFIG_REPLY *reply;
struct mps_command *cm;
Mpi2RaidPhysDiskPage0_t *page = NULL;
int error = 0;
u16 ioc_status;
mps_dprint(sc, MPS_TRACE, "%s\n", __func__);
if ((cm = mps_alloc_command(sc)) == NULL) {
printf("%s: command alloc failed @ line %d\n", __func__,
__LINE__);
error = EBUSY;
goto out;
}
request = (MPI2_CONFIG_REQUEST *)cm->cm_req;
bzero(request, sizeof(MPI2_CONFIG_REQUEST));
request->Function = MPI2_FUNCTION_CONFIG;
request->Action = MPI2_CONFIG_ACTION_PAGE_HEADER;
request->Header.PageType = MPI2_CONFIG_PAGETYPE_RAID_PHYSDISK;
request->Header.PageNumber = 0;
request->Header.PageVersion = MPI2_RAIDPHYSDISKPAGE0_PAGEVERSION;
cm->cm_desc.Default.RequestFlags = MPI2_REQ_DESCRIPT_FLAGS_DEFAULT_TYPE;
cm->cm_data = NULL;
/*
* This page must be polled because the IOC isn't ready yet when this
* page is needed.
*/
error = mps_request_polled(sc, cm);
reply = (MPI2_CONFIG_REPLY *)cm->cm_reply;
if (error || (reply == NULL)) {
/* FIXME */
/* If the poll returns error then we need to do diag reset */
printf("%s: poll for header completed with error %d",
__func__, error);
error = ENXIO;
goto out;
}
ioc_status = le16toh(reply->IOCStatus) & MPI2_IOCSTATUS_MASK;
bcopy(reply, mpi_reply, sizeof(MPI2_CONFIG_REPLY));
if (ioc_status != MPI2_IOCSTATUS_SUCCESS) {
/* FIXME */
/* If the poll returns error then we need to do diag reset */
printf("%s: header read with error; iocstatus = 0x%x\n",
__func__, ioc_status);
error = ENXIO;
goto out;
}
/* We have to do free and alloc for the reply-free and reply-post
* counters to match - Need to review the reply FIFO handling.
*/
mps_free_command(sc, cm);
if ((cm = mps_alloc_command(sc)) == NULL) {
printf("%s: command alloc failed @ line %d\n", __func__,
__LINE__);
error = EBUSY;
goto out;
}
request = (MPI2_CONFIG_REQUEST *)cm->cm_req;
bzero(request, sizeof(MPI2_CONFIG_REQUEST));
request->Function = MPI2_FUNCTION_CONFIG;
request->Action = MPI2_CONFIG_ACTION_PAGE_READ_CURRENT;
request->Header.PageType = MPI2_CONFIG_PAGETYPE_RAID_PHYSDISK;
request->Header.PageNumber = 0;
request->Header.PageLength = mpi_reply->Header.PageLength;
request->Header.PageVersion = mpi_reply->Header.PageVersion;
request->PageAddress = page_address;
cm->cm_length = le16toh(mpi_reply->Header.PageLength) * 4;
cm->cm_sge = &request->PageBufferSGE;
cm->cm_sglsize = sizeof(MPI2_SGE_IO_UNION);
cm->cm_flags = MPS_CM_FLAGS_SGE_SIMPLE | MPS_CM_FLAGS_DATAIN;
cm->cm_desc.Default.RequestFlags = MPI2_REQ_DESCRIPT_FLAGS_DEFAULT_TYPE;
page = malloc(cm->cm_length, M_MPT2, M_ZERO | M_NOWAIT);
if (!page) {
printf("%s: page alloc failed\n", __func__);
error = ENOMEM;
goto out;
}
cm->cm_data = page;
/*
* This page must be polled because the IOC isn't ready yet when this
* page is needed.
*/
error = mps_request_polled(sc, cm);
reply = (MPI2_CONFIG_REPLY *)cm->cm_reply;
if (error || (reply == NULL)) {
/* FIXME */
/* If the poll returns error then we need to do diag reset */
printf("%s: poll for page completed with error %d",
__func__, error);
error = ENXIO;
goto out;
}
ioc_status = le16toh(reply->IOCStatus) & MPI2_IOCSTATUS_MASK;
bcopy(reply, mpi_reply, sizeof(MPI2_CONFIG_REPLY));
if (ioc_status != MPI2_IOCSTATUS_SUCCESS) {
/* FIXME */
/* If the poll returns error then we need to do diag reset */
printf("%s: page read with error; iocstatus = 0x%x\n",
__func__, ioc_status);
error = ENXIO;
goto out;
}
bcopy(page, config_page, MIN(cm->cm_length,
sizeof(Mpi2RaidPhysDiskPage0_t)));
out:
free(page, M_MPT2);
if (cm)
mps_free_command(sc, cm);
return (error);
}
/*
* ReadFatHeader
*/
int ReadFatHeader(pts_FatHandle p_fat_handle,
pts_LibXmountMorphingInputFunctions p_input_functions,
uint8_t debug)
{
pts_FatVH p_fat_vh;
int ret;
size_t bytes_read;
uint32_t root_dir_sectors;
uint32_t fat_size;
uint32_t total_sectors;
uint32_t data_sectors;
uint32_t cluster_count;
// Init FAT handle
memset(p_fat_handle,0,sizeof(ts_FatHandle));
p_fat_handle->fat_type=FatType_Unknown;
p_fat_handle->debug=debug;
LOG_DEBUG("Trying to read FAT volume header\n");
// Alloc buffer for header
p_fat_vh=calloc(1,sizeof(ts_FatVH));
if(p_fat_vh==NULL) return UNALLOCATED_MEMALLOC_FAILED;
// Read VH from input image
ret=p_input_functions->Read(0,
(char*)(p_fat_vh),
0,
sizeof(ts_FatVH),
&bytes_read);
if(ret!=0 || bytes_read!=sizeof(ts_FatVH)) {
free(p_fat_vh);
return UNALLOCATED_FAT_CANNOT_READ_HEADER;
}
// Convert values to host endianness (FAT values are always stored in little
// endian)
p_fat_vh->bytes_per_sector=le16toh(p_fat_vh->bytes_per_sector);
p_fat_vh->reserved_sectors=le16toh(p_fat_vh->reserved_sectors);
p_fat_vh->root_entry_count=le16toh(p_fat_vh->root_entry_count);
p_fat_vh->total_sectors_16=le16toh(p_fat_vh->total_sectors_16);
p_fat_vh->fat16_sectors=le16toh(p_fat_vh->fat16_sectors);
p_fat_vh->total_sectors_32=le32toh(p_fat_vh->total_sectors_32);
p_fat_vh->fat32_sectors=le32toh(p_fat_vh->fat32_sectors);
LOG_DEBUG("FAT VH jump instruction 1: 0x%02X\n",p_fat_vh->jump_inst[0]);
LOG_DEBUG("FAT bytes per sector: %" PRIu16 "\n",
p_fat_vh->bytes_per_sector);
LOG_DEBUG("FAT sectors per cluster: %" PRIu8 "\n",
p_fat_vh->sectors_per_cluster);
LOG_DEBUG("FAT reserved sectors: %" PRIu16 "\n",
p_fat_vh->reserved_sectors);
LOG_DEBUG("FAT count: %" PRIu8 "\n",p_fat_vh->fat_count);
LOG_DEBUG("FAT root entry count: %" PRIu16 "\n",
p_fat_vh->root_entry_count);
LOG_DEBUG("FAT media type: %02X\n",p_fat_vh->media_type);
LOG_DEBUG("FAT total sector count (16bit): %" PRIu16 "\n",
p_fat_vh->total_sectors_16);
LOG_DEBUG("FAT sectors per FAT (16bit): %" PRIu16 "\n",
p_fat_vh->fat16_sectors);
LOG_DEBUG("FAT total sector count (32bit): %" PRIu32 "\n",
p_fat_vh->total_sectors_32);
LOG_DEBUG("FAT sectors per FAT (32bit): %" PRIu32 "\n",
p_fat_vh->fat32_sectors);
// Check header values
if((p_fat_vh->jump_inst[0]!=0xEB && p_fat_vh->jump_inst[0]!=0xE9) ||
p_fat_vh->bytes_per_sector==0 ||
p_fat_vh->bytes_per_sector%512!=0 ||
p_fat_vh->sectors_per_cluster==0 ||
p_fat_vh->sectors_per_cluster%2!=0 ||
p_fat_vh->reserved_sectors==0 ||
p_fat_vh->fat_count==0 ||
(p_fat_vh->total_sectors_16==0 && p_fat_vh->total_sectors_32==0) ||
(p_fat_vh->total_sectors_16!=0 && p_fat_vh->total_sectors_32!=0))
{
free(p_fat_vh);
return UNALLOCATED_FAT_INVALID_HEADER;
}
LOG_DEBUG("Determining FAT type\n");
// Determine the count of sectors occupied by the root directory
root_dir_sectors=((p_fat_vh->root_entry_count*32)+
(p_fat_vh->bytes_per_sector-1))/p_fat_vh->bytes_per_sector;
// Determine the count of sectors in the data region
if(p_fat_vh->fat16_sectors!=0) fat_size=p_fat_vh->fat16_sectors;
else fat_size=p_fat_vh->fat32_sectors;
if(p_fat_vh->total_sectors_16!=0) total_sectors=p_fat_vh->total_sectors_16;
else total_sectors=p_fat_vh->total_sectors_32;
data_sectors=total_sectors-(p_fat_vh->reserved_sectors+
(p_fat_vh->fat_count*fat_size)+root_dir_sectors);
// Determine the count of clusters
cluster_count=data_sectors/p_fat_vh->sectors_per_cluster;
// Determine FAT type
if(cluster_count<4085) {
LOG_DEBUG("FAT is of unsupported type FAT12\n");
free(p_fat_vh);
return UNALLOCATED_FAT_UNSUPPORTED_FS_TYPE;
} else if(cluster_count<65525) {
LOG_DEBUG("FAT is of type FAT16\n");
p_fat_handle->fat_type=FatType_Fat16;
} else {
LOG_DEBUG("FAT is of type FAT32\n");
p_fat_handle->fat_type=FatType_Fat32;
}
// TODO: What about newer version of FAT like exFAT etc... ??
p_fat_handle->p_fat_vh=p_fat_vh;
return UNALLOCATED_OK;
}
/**
The event occurs when an updating statement is done.
*/
Query_event::Query_event(const char* buf, unsigned int event_len,
const Format_description_event *description_event,
Log_event_type event_type)
: Binary_log_event(&buf, description_event->binlog_version,
description_event->server_version),
query(0), db(0), catalog(0), time_zone_str(0),
user(0), user_len(0), host(0), host_len(0),
db_len(0), status_vars_len(0), q_len(0),
flags2_inited(0), sql_mode_inited(0), charset_inited(0),
auto_increment_increment(1), auto_increment_offset(1),
time_zone_len(0), catalog_len(0), lc_time_names_number(0),
charset_database_number(0), table_map_for_update(0), master_data_written(0),
mts_accessed_dbs(OVER_MAX_DBS_IN_EVENT_MTS), last_committed(SEQ_UNINIT),
sequence_number(SEQ_UNINIT)
{
//buf is advanced in Binary_log_event constructor to point to
//beginning of post-header
uint32_t tmp;
uint8_t common_header_len, post_header_len;
Log_event_header::Byte *start;
const Log_event_header::Byte *end;
query_data_written= 0;
common_header_len= description_event->common_header_len;
post_header_len= description_event->post_header_len[event_type - 1];
/*
We test if the event's length is sensible, and if so we compute data_len.
We cannot rely on QUERY_HEADER_LEN here as it would not be format-tolerant.
We use QUERY_HEADER_MINIMAL_LEN which is the same for 3.23, 4.0 & 5.0.
*/
if (event_len < (unsigned int)(common_header_len + post_header_len))
return;
data_len= event_len - (common_header_len + post_header_len);
memcpy(&thread_id, buf + Q_THREAD_ID_OFFSET, sizeof(thread_id));
thread_id= le32toh(thread_id);
memcpy(&query_exec_time, buf + Q_EXEC_TIME_OFFSET, sizeof(query_exec_time));
query_exec_time= le32toh(query_exec_time);
db_len= (unsigned int)buf[Q_DB_LEN_OFFSET];
// TODO: add a check of all *_len vars
memcpy(&error_code, buf + Q_ERR_CODE_OFFSET, sizeof(error_code));
error_code= le16toh(error_code);
/*
5.0 format starts here.
Depending on the format, we may or not have affected/warnings etc
The remnent post-header to be parsed has length:
*/
tmp= post_header_len - QUERY_HEADER_MINIMAL_LEN;
if (tmp)
{
memcpy(&status_vars_len, buf + Q_STATUS_VARS_LEN_OFFSET,
sizeof(status_vars_len));
status_vars_len= le16toh(status_vars_len);
/*
Check if status variable length is corrupt and will lead to very
wrong data. We could be even more strict and require data_len to
be even bigger, but this will suffice to catch most corruption
errors that can lead to a crash.
*/
if (status_vars_len >
std::min<unsigned long>(data_len, MAX_SIZE_LOG_EVENT_STATUS))
{
query= 0;
return;
}
data_len-= status_vars_len;
tmp-= 2;
}
else
{
/*
server version < 5.0 / binlog_version < 4 master's event is
relay-logged with storing the original size of the event in
Q_MASTER_DATA_WRITTEN_CODE status variable.
The size is to be restored at reading Q_MASTER_DATA_WRITTEN_CODE-marked
event from the relay log.
*/
BAPI_ASSERT(description_event->binlog_version < 4);
master_data_written= header()->data_written;
}
/*
We have parsed everything we know in the post header for QUERY_EVENT,
the rest of post header is either comes from older version MySQL or
dedicated to derived events (e.g. Execute_load_query...)
*/
/* variable-part: the status vars; only in MySQL 5.0 */
start= (Log_event_header::Byte*) (buf + post_header_len);
end= (const Log_event_header::Byte*) (start + status_vars_len);
for (const Log_event_header::Byte* pos= start; pos < end;)
{
switch (*pos++) {
case Q_FLAGS2_CODE:
CHECK_SPACE(pos, end, 4);
flags2_inited= 1;
memcpy(&flags2, pos, sizeof(flags2));
flags2= le32toh(flags2);
pos+= 4;
break;
case Q_SQL_MODE_CODE:
{
CHECK_SPACE(pos, end, 8);
sql_mode_inited= 1;
memcpy(&sql_mode, pos, sizeof(sql_mode));
sql_mode= le64toh(sql_mode);
pos+= 8;
break;
}
case Q_CATALOG_NZ_CODE:
if ((catalog_len= *pos))
catalog= (const char*) (pos + 1);
CHECK_SPACE(pos, end, catalog_len + 1);
pos+= catalog_len + 1;
break;
case Q_AUTO_INCREMENT:
CHECK_SPACE(pos, end, 4);
memcpy(&auto_increment_increment, pos, sizeof(auto_increment_increment));
auto_increment_increment= le16toh(auto_increment_increment);
memcpy(&auto_increment_offset, pos + 2, sizeof(auto_increment_offset));
auto_increment_offset= le16toh(auto_increment_offset);
pos+= 4;
break;
case Q_CHARSET_CODE:
{
CHECK_SPACE(pos, end, 6);
charset_inited= 1;
memcpy(charset, pos, 6);
pos+= 6;
break;
}
case Q_TIME_ZONE_CODE:
{
if ((time_zone_len= *pos))
time_zone_str= (const char*)(pos + 1);
pos+= time_zone_len + 1;
break;
}
case Q_CATALOG_CODE: /* for 5.0.x where 0<=x<=3 masters */
CHECK_SPACE(pos, end, 1);
if ((catalog_len= *pos))
catalog= (const char*) (pos+1);
CHECK_SPACE(pos, end, catalog_len + 2);
pos+= catalog_len + 2; // leap over end 0
break;
case Q_LC_TIME_NAMES_CODE:
CHECK_SPACE(pos, end, 2);
memcpy(&lc_time_names_number, pos, sizeof(lc_time_names_number));
lc_time_names_number= le16toh(lc_time_names_number);
pos+= 2;
break;
case Q_CHARSET_DATABASE_CODE:
CHECK_SPACE(pos, end, 2);
memcpy(&charset_database_number, pos, sizeof(lc_time_names_number));
charset_database_number= le16toh(charset_database_number);
pos+= 2;
break;
case Q_TABLE_MAP_FOR_UPDATE_CODE:
CHECK_SPACE(pos, end, 8);
memcpy(&table_map_for_update, pos, sizeof(table_map_for_update));
table_map_for_update= le64toh(table_map_for_update);
pos+= 8;
break;
case Q_MASTER_DATA_WRITTEN_CODE:
CHECK_SPACE(pos, end, 4);
memcpy(&master_data_written, pos, sizeof(master_data_written));
master_data_written= le32toh(static_cast<uint32_t>(master_data_written));
header()->data_written= master_data_written;
pos+= 4;
break;
case Q_MICROSECONDS:
{
CHECK_SPACE(pos, end, 3);
uint32_t temp_usec= 0;
memcpy(&temp_usec, pos, 3);
header()->when.tv_usec= le32toh(temp_usec);
pos+= 3;
break;
}
case Q_INVOKER:
{
CHECK_SPACE(pos, end, 1);
user_len= *pos++;
CHECK_SPACE(pos, end, user_len);
user= (const char*)pos;
pos+= user_len;
CHECK_SPACE(pos, end, 1);
host_len= *pos++;
CHECK_SPACE(pos, end, host_len);
host= (const char*)pos;
pos+= host_len;
break;
}
case Q_UPDATED_DB_NAMES:
{
unsigned char i= 0;
#ifndef DBUG_OFF
bool is_corruption_injected= false;
#endif
CHECK_SPACE(pos, end, 1);
mts_accessed_dbs= *pos++;
/*
Notice, the following check is positive also in case of
the master's MAX_DBS_IN_EVENT_MTS > the slave's one and the event
contains e.g the master's MAX_DBS_IN_EVENT_MTS db:s.
*/
if (mts_accessed_dbs > MAX_DBS_IN_EVENT_MTS)
{
mts_accessed_dbs= OVER_MAX_DBS_IN_EVENT_MTS;
break;
}
BAPI_ASSERT(mts_accessed_dbs != 0);
for (i= 0; i < mts_accessed_dbs && pos < start + status_vars_len; i++)
{
#ifndef DBUG_OFF
/*
This is specific to mysql test run on the server
for the keyword "query_log_event_mts_corrupt_db_names"
*/
if (binary_log_debug::debug_query_mts_corrupt_db_names)
{
if (mts_accessed_dbs == 2)
{
BAPI_ASSERT(pos[sizeof("d?") - 1] == 0);
((char*) pos)[sizeof("d?") - 1]= 'a';
is_corruption_injected= true;
}
}
#endif
strncpy(mts_accessed_db_names[i], (char*) pos,
std::min<unsigned long>(NAME_LEN, start + status_vars_len - pos));
mts_accessed_db_names[i][NAME_LEN - 1]= 0;
pos+= 1 + strlen((const char*) pos);
}
if (i != mts_accessed_dbs
#ifndef DBUG_OFF
|| is_corruption_injected
#endif
)
return;
break;
}
case Q_COMMIT_TS2:
CHECK_SPACE(pos, end, COMMIT_SEQ_LEN);
last_committed= 0;
memcpy(&last_committed, pos, 8);
last_committed= le64toh(last_committed);
sequence_number= 0;
memcpy(&sequence_number, pos + 8, 8);
sequence_number= le64toh(sequence_number);
pos+= COMMIT_SEQ_LEN;
break;
default:
/* That's why you must write status vars in growing order of code */
pos= (const unsigned char*) end; // Break loop
}
}
if (catalog_len) // If catalog is given
query_data_written+= catalog_len + 1;
if (time_zone_len)
query_data_written+= time_zone_len + 1;
if (user_len > 0)
query_data_written+= user_len + 1;
if (host_len > 0)
query_data_written+= host_len + 1;
/*
if time_zone_len or catalog_len are 0, then time_zone and catalog
are uninitialized at this point. shouldn't they point to the
zero-length null-terminated strings we allocated space for in the
my_alloc call above? /sven
*/
/* A 2nd variable part; this is common to all versions */
query_data_written+= data_len + 1;
db= (const char* )end;
q_len= data_len - db_len -1;
start[status_vars_len + data_len]= '\0';
query= (const char *)(end + db_len + 1);
return;
}
/*
* ReadFat
*/
int ReadFat(pts_FatHandle p_fat_handle,
pts_LibXmountMorphingInputFunctions p_input_functions)
{
pts_FatVH p_fat_vh=p_fat_handle->p_fat_vh;
int ret;
size_t fat_size;
off_t fat_offset;
size_t bytes_read;
LOG_DEBUG("Trying to read FAT\n");
// Determine FAT size
if(p_fat_vh->fat16_sectors!=0) fat_size=p_fat_vh->fat16_sectors;
else fat_size=p_fat_vh->fat32_sectors;
fat_size*=p_fat_vh->bytes_per_sector;
// Calculate FAT offset
fat_offset=p_fat_vh->reserved_sectors*p_fat_vh->bytes_per_sector;
LOG_DEBUG("FAT consists of %zu bytes starting at offset %zu\n",
fat_size,
fat_offset);
// Alloc buffer and Read FAT
if(p_fat_handle->fat_type==FatType_Fat32) {
// Alloc buffer
p_fat_handle->u_fat.p_fat32=(uint32_t*)calloc(1,fat_size);
if(p_fat_handle->u_fat.p_fat32==NULL) return UNALLOCATED_MEMALLOC_FAILED;
// Read FAT
ret=p_input_functions->Read(0,
(char*)(p_fat_handle->u_fat.p_fat32),
fat_offset,
fat_size,
&bytes_read);
if(ret!=0 || bytes_read!=fat_size) {
free(p_fat_handle->u_fat.p_fat32);
p_fat_handle->u_fat.p_fat32=NULL;
return UNALLOCATED_FAT_CANNOT_READ_FAT;
}
// Convert FAT to host endianness
for(uint64_t i=0;i<fat_size/sizeof(uint32_t);i++) {
p_fat_handle->u_fat.p_fat32[i]=le32toh(p_fat_handle->u_fat.p_fat32[i]);
}
} else {
// Alloc buffer
p_fat_handle->u_fat.p_fat16=(uint16_t*)calloc(1,fat_size);
if(p_fat_handle->u_fat.p_fat16==NULL) return UNALLOCATED_MEMALLOC_FAILED;
// Read FAT
ret=p_input_functions->Read(0,
(char*)(p_fat_handle->u_fat.p_fat16),
fat_offset,
fat_size,
&bytes_read);
if(ret!=0 || bytes_read!=fat_size) {
free(p_fat_handle->u_fat.p_fat16);
p_fat_handle->u_fat.p_fat16=NULL;
return UNALLOCATED_FAT_CANNOT_READ_FAT;
}
// Convert FAT to host endianness
for(uint64_t i=0;i<fat_size/sizeof(uint16_t);i++) {
p_fat_handle->u_fat.p_fat16[i]=le16toh(p_fat_handle->u_fat.p_fat16[i]);
}
}
LOG_DEBUG("FAT read successfully\n");
return UNALLOCATED_OK;
}
/* Send Read_Clock_Offset command to the unit */
static int
hci_read_clock_offset(int s, int argc, char **argv)
{
int n;
char b[512];
ng_hci_read_clock_offset_cp cp;
ng_hci_event_pkt_t *e = (ng_hci_event_pkt_t *) b;
/* parse command parameters */
switch (argc) {
case 1:
/* connecton handle */
if (sscanf(argv[0], "%d", &n) != 1 || n < 0 || n > 0x0eff)
return (USAGE);
cp.con_handle = (n & 0x0fff);
cp.con_handle = htole16(cp.con_handle);
break;
default:
return (USAGE);
}
/* send request and expect status response */
n = sizeof(b);
if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LINK_CONTROL,
NG_HCI_OCF_READ_CLOCK_OFFSET),
(char const *) &cp, sizeof(cp), b, &n) == ERROR)
return (ERROR);
if (*b != 0x00)
return (FAILED);
/* wait for event */
again:
n = sizeof(b);
if (hci_recv(s, b, &n) == ERROR)
return (ERROR);
if (n < sizeof(*e)) {
errno = EIO;
return (ERROR);
}
if (e->event == NG_HCI_EVENT_READ_CLOCK_OFFSET_COMPL) {
ng_hci_read_clock_offset_compl_ep *ep =
(ng_hci_read_clock_offset_compl_ep *)(e + 1);
if (ep->status != 0x00) {
fprintf(stdout, "Status: %s [%#02x]\n",
hci_status2str(ep->status), ep->status);
return (FAILED);
}
fprintf(stdout, "Connection handle: %d\n",
le16toh(ep->con_handle));
fprintf(stdout, "Clock offset: %#04x\n",
le16toh(ep->clock_offset));
} else
goto again;
return (OK);
} /* hci_read_clock_offset */
/* Send Read_Remote_Version_Information command to the unit */
static int
hci_read_remote_version_information(int s, int argc, char **argv)
{
int n;
char b[512];
ng_hci_read_remote_ver_info_cp cp;
ng_hci_event_pkt_t *e = (ng_hci_event_pkt_t *) b;
/* parse command parameters */
switch (argc) {
case 1:
/* connecton handle */
if (sscanf(argv[0], "%d", &n) != 1 || n < 0 || n > 0x0eff)
return (USAGE);
cp.con_handle = (n & 0x0fff);
cp.con_handle = htole16(cp.con_handle);
break;
default:
return (USAGE);
}
/* send request and expect status response */
n = sizeof(b);
if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LINK_CONTROL,
NG_HCI_OCF_READ_REMOTE_VER_INFO),
(char const *) &cp, sizeof(cp), b, &n) == ERROR)
return (ERROR);
if (*b != 0x00)
return (FAILED);
/* wait for event */
again:
n = sizeof(b);
if (hci_recv(s, b, &n) == ERROR)
return (ERROR);
if (n < sizeof(*e)) {
errno = EIO;
return (ERROR);
}
if (e->event == NG_HCI_EVENT_READ_REMOTE_VER_INFO_COMPL) {
ng_hci_read_remote_ver_info_compl_ep *ep =
(ng_hci_read_remote_ver_info_compl_ep *)(e + 1);
if (ep->status != 0x00) {
fprintf(stdout, "Status: %s [%#02x]\n",
hci_status2str(ep->status), ep->status);
return (FAILED);
}
ep->manufacturer = le16toh(ep->manufacturer);
fprintf(stdout, "Connection handle: %d\n",
le16toh(ep->con_handle));
fprintf(stdout, "LMP version: %s [%#02x]\n",
hci_lmpver2str(ep->lmp_version), ep->lmp_version);
fprintf(stdout, "LMP sub-version: %#04x\n",
le16toh(ep->lmp_subversion));
fprintf(stdout, "Manufacturer: %s [%#04x]\n",
hci_manufacturer2str(ep->manufacturer),
ep->manufacturer);
} else
goto again;
return (OK);
} /* hci_read_remote_version_information */
int
ng_l2cap_l2ca_clt_receive(ng_l2cap_con_p con)
{
struct _clt_pkt {
ng_l2cap_hdr_t h;
ng_l2cap_clt_hdr_t c_h;
} __attribute__ ((packed)) *hdr = NULL;
ng_l2cap_p l2cap = con->l2cap;
int length, error = 0;
NG_L2CAP_M_PULLUP(con->rx_pkt, sizeof(*hdr));
if (con->rx_pkt == NULL)
return (ENOBUFS);
hdr = mtod(con->rx_pkt, struct _clt_pkt *);
/* Check packet */
length = con->rx_pkt->m_pkthdr.len - sizeof(*hdr);
if (length < 0) {
NG_L2CAP_ERR(
"%s: %s - invalid L2CAP CLT data packet. Packet too small, length=%d\n",
__func__, NG_NODE_NAME(l2cap->node), length);
error = EMSGSIZE;
goto drop;
}
/* Check payload size against CLT MTU */
if (length > NG_L2CAP_MTU_DEFAULT) {
NG_L2CAP_ERR(
"%s: %s - invalid L2CAP CLT data packet. Packet too big, length=%d, mtu=%d\n",
__func__, NG_NODE_NAME(l2cap->node), length,
NG_L2CAP_MTU_DEFAULT);
error = EMSGSIZE;
goto drop;
}
hdr->c_h.psm = le16toh(hdr->c_h.psm);
/*
* If we got here then everything looks good and we can sent packet
* to the upper layer protocol.
*/
/* Select upstream hook based on PSM */
switch (hdr->c_h.psm) {
case NG_L2CAP_PSM_SDP:
if (l2cap->flags & NG_L2CAP_CLT_SDP_DISABLED)
goto drop;
break;
case NG_L2CAP_PSM_RFCOMM:
if (l2cap->flags & NG_L2CAP_CLT_RFCOMM_DISABLED)
goto drop;
break;
case NG_L2CAP_PSM_TCP:
if (l2cap->flags & NG_L2CAP_CLT_TCP_DISABLED)
goto drop;
break;
}
/* Check if upstream hook is connected and valid */
if (l2cap->l2c == NULL || NG_HOOK_NOT_VALID(l2cap->l2c)) {
NG_L2CAP_ERR(
"%s: %s - unable to send L2CAP CLT data packet. " \
"Hook is not connected or valid, psm=%d\n",
__func__, NG_NODE_NAME(l2cap->node), hdr->c_h.psm);
error = ENOTCONN;
goto drop;
}
NG_SEND_DATA_ONLY(error, l2cap->l2c, con->rx_pkt);
con->rx_pkt = NULL;
drop:
NG_FREE_M(con->rx_pkt); /* checks for != NULL */
return (error);
} /* ng_l2cap_l2ca_clt_receive */
/**
* mps_config_get_man_pg10 - obtain Manufacturing Page 10 data and set flags
* accordingly. Currently, this page does not need to return to caller.
* @sc: per adapter object
* @mpi_reply: reply mf payload returned from firmware
* Context: sleep.
*
* Returns 0 for success, non-zero for failure.
*/
int
mps_config_get_man_pg10(struct mps_softc *sc, Mpi2ConfigReply_t *mpi_reply)
{
MPI2_CONFIG_REQUEST *request;
MPI2_CONFIG_REPLY *reply;
struct mps_command *cm;
pMpi2ManufacturingPagePS_t page = NULL;
uint32_t *pPS_info;
uint8_t OEM_Value = 0;
int error = 0;
u16 ioc_status;
mps_dprint(sc, MPS_TRACE, "%s\n", __func__);
if ((cm = mps_alloc_command(sc)) == NULL) {
printf("%s: command alloc failed @ line %d\n", __func__,
__LINE__);
error = EBUSY;
goto out;
}
request = (MPI2_CONFIG_REQUEST *)cm->cm_req;
bzero(request, sizeof(MPI2_CONFIG_REQUEST));
request->Function = MPI2_FUNCTION_CONFIG;
request->Action = MPI2_CONFIG_ACTION_PAGE_HEADER;
request->Header.PageType = MPI2_CONFIG_PAGETYPE_MANUFACTURING;
request->Header.PageNumber = 10;
request->Header.PageVersion = MPI2_MANUFACTURING10_PAGEVERSION;
cm->cm_desc.Default.RequestFlags = MPI2_REQ_DESCRIPT_FLAGS_DEFAULT_TYPE;
cm->cm_data = NULL;
/*
* This page must be polled because the IOC isn't ready yet when this
* page is needed.
*/
error = mps_request_polled(sc, cm);
reply = (MPI2_CONFIG_REPLY *)cm->cm_reply;
if (error || (reply == NULL)) {
/* FIXME */
/* If the poll returns error then we need to do diag reset */
printf("%s: poll for header completed with error %d",
__func__, error);
error = ENXIO;
goto out;
}
ioc_status = le16toh(reply->IOCStatus) & MPI2_IOCSTATUS_MASK;
bcopy(reply, mpi_reply, sizeof(MPI2_CONFIG_REPLY));
if (ioc_status != MPI2_IOCSTATUS_SUCCESS) {
/* FIXME */
/* If the poll returns error then we need to do diag reset */
printf("%s: header read with error; iocstatus = 0x%x\n",
__func__, ioc_status);
error = ENXIO;
goto out;
}
/* We have to do free and alloc for the reply-free and reply-post
* counters to match - Need to review the reply FIFO handling.
*/
mps_free_command(sc, cm);
if ((cm = mps_alloc_command(sc)) == NULL) {
printf("%s: command alloc failed @ line %d\n", __func__,
__LINE__);
error = EBUSY;
goto out;
}
request = (MPI2_CONFIG_REQUEST *)cm->cm_req;
bzero(request, sizeof(MPI2_CONFIG_REQUEST));
request->Function = MPI2_FUNCTION_CONFIG;
request->Action = MPI2_CONFIG_ACTION_PAGE_READ_CURRENT;
request->Header.PageType = MPI2_CONFIG_PAGETYPE_MANUFACTURING;
request->Header.PageNumber = 10;
request->Header.PageVersion = MPI2_MANUFACTURING10_PAGEVERSION;
request->Header.PageLength = mpi_reply->Header.PageLength;
cm->cm_length = le16toh(mpi_reply->Header.PageLength) * 4;
cm->cm_sge = &request->PageBufferSGE;
cm->cm_sglsize = sizeof(MPI2_SGE_IO_UNION);
cm->cm_flags = MPS_CM_FLAGS_SGE_SIMPLE | MPS_CM_FLAGS_DATAIN;
cm->cm_desc.Default.RequestFlags = MPI2_REQ_DESCRIPT_FLAGS_DEFAULT_TYPE;
page = malloc(MPS_MAN_PAGE10_SIZE, M_MPT2, M_ZERO | M_NOWAIT);
if (!page) {
printf("%s: page alloc failed\n", __func__);
error = ENOMEM;
goto out;
}
cm->cm_data = page;
/*
* This page must be polled because the IOC isn't ready yet when this
* page is needed.
*/
error = mps_request_polled(sc, cm);
reply = (MPI2_CONFIG_REPLY *)cm->cm_reply;
if (error || (reply == NULL)) {
/* FIXME */
/* If the poll returns error then we need to do diag reset */
printf("%s: poll for page completed with error %d",
__func__, error);
error = ENXIO;
goto out;
}
ioc_status = le16toh(reply->IOCStatus) & MPI2_IOCSTATUS_MASK;
bcopy(reply, mpi_reply, sizeof(MPI2_CONFIG_REPLY));
if (ioc_status != MPI2_IOCSTATUS_SUCCESS) {
/* FIXME */
/* If the poll returns error then we need to do diag reset */
printf("%s: page read with error; iocstatus = 0x%x\n",
__func__, ioc_status);
error = ENXIO;
goto out;
}
/*
* If OEM ID is unknown, fail the request.
*/
sc->WD_hide_expose = MPS_WD_HIDE_ALWAYS;
OEM_Value = (uint8_t)(page->ProductSpecificInfo & 0x000000FF);
if (OEM_Value != MPS_WD_LSI_OEM) {
mps_dprint(sc, MPS_FAULT, "Unknown OEM value for WarpDrive "
"(0x%x)\n", OEM_Value);
error = ENXIO;
goto out;
}
/*
* Set the phys disks hide/expose value.
*/
pPS_info = &page->ProductSpecificInfo;
sc->WD_hide_expose = (uint8_t)(pPS_info[5]);
sc->WD_hide_expose &= MPS_WD_HIDE_EXPOSE_MASK;
if ((sc->WD_hide_expose != MPS_WD_HIDE_ALWAYS) &&
(sc->WD_hide_expose != MPS_WD_EXPOSE_ALWAYS) &&
(sc->WD_hide_expose != MPS_WD_HIDE_IF_VOLUME)) {
mps_dprint(sc, MPS_FAULT, "Unknown value for WarpDrive "
"hide/expose: 0x%x\n", sc->WD_hide_expose);
error = ENXIO;
goto out;
}
out:
free(page, M_MPT2);
if (cm)
mps_free_command(sc, cm);
return (error);
}
static void anal(struct wstate *ws, unsigned char* buf, int rd) // yze
{
struct ieee80211_frame* wh = (struct ieee80211_frame *) buf;
int type,stype;
static int lastseq = -1;
int seq;
unsigned short *seqptr;
int for_us = 0;
if (rd < 1) {
time_print("rd=%d\n", rd);
exit(1);
}
type = wh->i_fc[0] & IEEE80211_FC0_TYPE_MASK;
stype = wh->i_fc[0] & IEEE80211_FC0_SUBTYPE_MASK;
// sort out acks
if (ws->ws_state >= FOUND_VICTIM) {
// stuff for us
if (memcmp(wh->i_addr1, ws->ws_mymac, 6) == 0) {
for_us = 1;
if (type != IEEE80211_FC0_TYPE_CTL)
send_ack(ws);
}
}
// XXX i know it aint great...
seqptr = (unsigned short*) wh->i_seq;
seq = (le16toh(*seqptr) & IEEE80211_SEQ_SEQ_MASK) >> IEEE80211_SEQ_SEQ_SHIFT;
if (seq == lastseq && (wh->i_fc[1] & IEEE80211_FC1_RETRY) &&
type != IEEE80211_FC0_TYPE_CTL) {
// printf("Ignoring dup packet... seq=%d\n", seq);
return;
}
lastseq = seq;
// management frame
if (type == IEEE80211_FC0_TYPE_MGT) {
if(ws->ws_state == FIND_VICTIM) {
if (stype == IEEE80211_FC0_SUBTYPE_BEACON ||
stype == IEEE80211_FC0_SUBTYPE_PROBE_RESP) {
if (get_victim_ssid(ws, wh, rd)) {
return;
}
}
}
}
if (ws->ws_state >= FOUND_VICTIM) {
// stuff for us
if (for_us) {
stuff_for_us(ws, wh, rd);
}
// stuff in network [even for us]
if ( ((wh->i_fc[1] & IEEE80211_FC1_DIR_TODS) &&
(memcmp(ws->ws_bss, wh->i_addr1, 6) == 0)) ||
((wh->i_fc[1] & IEEE80211_FC1_DIR_FROMDS) &&
(memcmp(ws->ws_bss, wh->i_addr2, 6) == 0))
) {
stuff_for_net(ws, wh, rd);
}
}
}
int mps_config_set_dpm_pg0(struct mps_softc *sc, Mpi2ConfigReply_t *mpi_reply,
Mpi2DriverMappingPage0_t *config_page, u16 entry_idx)
{
MPI2_CONFIG_REQUEST *request;
MPI2_CONFIG_REPLY *reply;
struct mps_command *cm;
MPI2_CONFIG_PAGE_DRIVER_MAPPING_0 *page = NULL;
int error = 0;
u16 ioc_status;
mps_dprint(sc, MPS_TRACE, "%s\n", __func__);
if ((cm = mps_alloc_command(sc)) == NULL) {
printf("%s: command alloc failed @ line %d\n", __func__,
__LINE__);
error = EBUSY;
goto out;
}
request = (MPI2_CONFIG_REQUEST *)cm->cm_req;
bzero(request, sizeof(MPI2_CONFIG_REQUEST));
request->Function = MPI2_FUNCTION_CONFIG;
request->Action = MPI2_CONFIG_ACTION_PAGE_HEADER;
request->Header.PageType = MPI2_CONFIG_PAGETYPE_EXTENDED;
request->ExtPageType = MPI2_CONFIG_EXTPAGETYPE_DRIVER_MAPPING;
request->Header.PageNumber = 0;
request->Header.PageVersion = MPI2_DRIVERMAPPING0_PAGEVERSION;
/* We can remove below two lines ????*/
request->PageAddress = 1 << MPI2_DPM_PGAD_ENTRY_COUNT_SHIFT;
request->PageAddress |= htole16(entry_idx);
cm->cm_desc.Default.RequestFlags = MPI2_REQ_DESCRIPT_FLAGS_DEFAULT_TYPE;
cm->cm_data = NULL;
error = mps_wait_command(sc, cm, 60, CAN_SLEEP);
reply = (MPI2_CONFIG_REPLY *)cm->cm_reply;
if (error || (reply == NULL)) {
/* FIXME */
/*
* If the request returns an error then we need to do a diag
* reset
*/
printf("%s: request for header completed with error %d",
__func__, error);
error = ENXIO;
goto out;
}
ioc_status = le16toh(reply->IOCStatus) & MPI2_IOCSTATUS_MASK;
bcopy(reply, mpi_reply, sizeof(MPI2_CONFIG_REPLY));
if (ioc_status != MPI2_IOCSTATUS_SUCCESS) {
/* FIXME */
/*
* If the request returns an error then we need to do a diag
* reset
*/
printf("%s: header read with error; iocstatus = 0x%x\n",
__func__, ioc_status);
error = ENXIO;
goto out;
}
/* We have to do free and alloc for the reply-free and reply-post
* counters to match - Need to review the reply FIFO handling.
*/
mps_free_command(sc, cm);
if ((cm = mps_alloc_command(sc)) == NULL) {
printf("%s: command alloc failed @ line %d\n", __func__,
__LINE__);
error = EBUSY;
goto out;
}
request = (MPI2_CONFIG_REQUEST *)cm->cm_req;
bzero(request, sizeof(MPI2_CONFIG_REQUEST));
request->Function = MPI2_FUNCTION_CONFIG;
request->Action = MPI2_CONFIG_ACTION_PAGE_WRITE_NVRAM;
request->Header.PageType = MPI2_CONFIG_PAGETYPE_EXTENDED;
request->ExtPageType = MPI2_CONFIG_EXTPAGETYPE_DRIVER_MAPPING;
request->Header.PageNumber = 0;
request->Header.PageVersion = MPI2_DRIVERMAPPING0_PAGEVERSION;
request->ExtPageLength = mpi_reply->ExtPageLength;
request->PageAddress = 1 << MPI2_DPM_PGAD_ENTRY_COUNT_SHIFT;
request->PageAddress |= htole16(entry_idx);
cm->cm_length = le16toh(mpi_reply->ExtPageLength) * 4;
cm->cm_sge = &request->PageBufferSGE;
cm->cm_sglsize = sizeof(MPI2_SGE_IO_UNION);
cm->cm_flags = MPS_CM_FLAGS_SGE_SIMPLE | MPS_CM_FLAGS_DATAOUT;
cm->cm_desc.Default.RequestFlags = MPI2_REQ_DESCRIPT_FLAGS_DEFAULT_TYPE;
page = malloc(cm->cm_length, M_MPT2, M_ZERO | M_NOWAIT);
if (!page) {
printf("%s: page alloc failed\n", __func__);
error = ENOMEM;
goto out;
}
bcopy(config_page, page, MIN(cm->cm_length,
(sizeof(Mpi2DriverMappingPage0_t))));
cm->cm_data = page;
error = mps_wait_command(sc, cm, 60, CAN_SLEEP);
reply = (MPI2_CONFIG_REPLY *)cm->cm_reply;
if (error || (reply == NULL)) {
/* FIXME */
/*
* If the request returns an error then we need to do a diag
* reset
*/
printf("%s: request to write page completed with error %d",
__func__, error);
error = ENXIO;
goto out;
}
ioc_status = le16toh(reply->IOCStatus) & MPI2_IOCSTATUS_MASK;
bcopy(reply, mpi_reply, sizeof(MPI2_CONFIG_REPLY));
if (ioc_status != MPI2_IOCSTATUS_SUCCESS) {
/* FIXME */
/*
* If the request returns an error then we need to do a diag
* reset
*/
printf("%s: page written with error; iocstatus = 0x%x\n",
__func__, ioc_status);
error = ENXIO;
goto out;
}
out:
free(page, M_MPT2);
if (cm)
mps_free_command(sc, cm);
return (error);
}
static void
show(int fd __unused)
{
uuid_t type;
off_t start;
map_t *m, *p;
struct mbr *mbr;
struct gpt_ent *ent;
unsigned int i;
printf(" %*s", lbawidth, "start");
printf(" %*s", lbawidth, "size");
printf(" index contents\n");
m = map_first();
while (m != NULL) {
printf(" %*llu", lbawidth, (long long)m->map_start);
printf(" %*llu", lbawidth, (long long)m->map_size);
putchar(' ');
putchar(' ');
if (m->map_index > 0)
printf("%5d", m->map_index);
else
printf(" ");
putchar(' ');
putchar(' ');
switch (m->map_type) {
case MAP_TYPE_MBR:
if (m->map_start != 0)
printf("Extended ");
printf("MBR");
break;
case MAP_TYPE_PRI_GPT_HDR:
printf("Pri GPT header");
break;
case MAP_TYPE_SEC_GPT_HDR:
printf("Sec GPT header");
break;
case MAP_TYPE_PRI_GPT_TBL:
printf("Pri GPT table");
break;
case MAP_TYPE_SEC_GPT_TBL:
printf("Sec GPT table");
break;
case MAP_TYPE_MBR_PART:
p = m->map_data;
if (p->map_start != 0)
printf("Extended ");
printf("MBR part ");
mbr = p->map_data;
for (i = 0; i < 4; i++) {
start = le16toh(mbr->mbr_part[i].part_start_hi);
start = (start << 16) +
le16toh(mbr->mbr_part[i].part_start_lo);
if (m->map_start == p->map_start + start)
break;
}
printf("%d", mbr->mbr_part[i].part_typ);
break;
case MAP_TYPE_GPT_PART:
printf("GPT part ");
ent = m->map_data;
if (show_label) {
printf("- \"%s\"",
utf16_to_utf8(ent->ent_name));
} else {
le_uuid_dec(&ent->ent_type, &type);
printf("- %s", friendly(&type));
}
break;
case MAP_TYPE_PMBR:
printf("PMBR");
break;
}
putchar('\n');
m = m->map_next;
}
}
/**
* mps_config_get_bios_pg3 - obtain BIOS page 3
* @sc: per adapter object
* @mpi_reply: reply mf payload returned from firmware
* @config_page: contents of the config page
* Context: sleep.
*
* Returns 0 for success, non-zero for failure.
*/
int
mps_config_get_bios_pg3(struct mps_softc *sc, Mpi2ConfigReply_t *mpi_reply,
Mpi2BiosPage3_t *config_page)
{
MPI2_CONFIG_REQUEST *request;
MPI2_CONFIG_REPLY *reply;
struct mps_command *cm;
Mpi2BiosPage3_t *page = NULL;
int error = 0;
u16 ioc_status;
mps_dprint(sc, MPS_TRACE, "%s\n", __func__);
if ((cm = mps_alloc_command(sc)) == NULL) {
printf("%s: command alloc failed @ line %d\n", __func__,
__LINE__);
error = EBUSY;
goto out;
}
request = (MPI2_CONFIG_REQUEST *)cm->cm_req;
bzero(request, sizeof(MPI2_CONFIG_REQUEST));
request->Function = MPI2_FUNCTION_CONFIG;
request->Action = MPI2_CONFIG_ACTION_PAGE_HEADER;
request->Header.PageType = MPI2_CONFIG_PAGETYPE_BIOS;
request->Header.PageNumber = 3;
request->Header.PageVersion = MPI2_BIOSPAGE3_PAGEVERSION;
cm->cm_desc.Default.RequestFlags = MPI2_REQ_DESCRIPT_FLAGS_DEFAULT_TYPE;
cm->cm_data = NULL;
error = mps_wait_command(sc, cm, 60, CAN_SLEEP);
reply = (MPI2_CONFIG_REPLY *)cm->cm_reply;
if (error || (reply == NULL)) {
/* FIXME */
/*
* If the request returns an error then we need to do a diag
* reset
*/
printf("%s: request for header completed with error %d",
__func__, error);
error = ENXIO;
goto out;
}
ioc_status = le16toh(reply->IOCStatus) & MPI2_IOCSTATUS_MASK;
bcopy(reply, mpi_reply, sizeof(MPI2_CONFIG_REPLY));
if (ioc_status != MPI2_IOCSTATUS_SUCCESS) {
/* FIXME */
/*
* If the request returns an error then we need to do a diag
* reset
*/
printf("%s: header read with error; iocstatus = 0x%x\n",
__func__, ioc_status);
error = ENXIO;
goto out;
}
/* We have to do free and alloc for the reply-free and reply-post
* counters to match - Need to review the reply FIFO handling.
*/
mps_free_command(sc, cm);
if ((cm = mps_alloc_command(sc)) == NULL) {
printf("%s: command alloc failed @ line %d\n", __func__,
__LINE__);
error = EBUSY;
goto out;
}
request = (MPI2_CONFIG_REQUEST *)cm->cm_req;
bzero(request, sizeof(MPI2_CONFIG_REQUEST));
request->Function = MPI2_FUNCTION_CONFIG;
request->Action = MPI2_CONFIG_ACTION_PAGE_READ_CURRENT;
request->Header.PageType = MPI2_CONFIG_PAGETYPE_BIOS;
request->Header.PageNumber = 3;
request->Header.PageVersion = MPI2_BIOSPAGE3_PAGEVERSION;
request->Header.PageLength = mpi_reply->Header.PageLength;
cm->cm_length = le16toh(mpi_reply->Header.PageLength) * 4;
cm->cm_sge = &request->PageBufferSGE;
cm->cm_sglsize = sizeof(MPI2_SGE_IO_UNION);
cm->cm_flags = MPS_CM_FLAGS_SGE_SIMPLE | MPS_CM_FLAGS_DATAIN;
cm->cm_desc.Default.RequestFlags = MPI2_REQ_DESCRIPT_FLAGS_DEFAULT_TYPE;
page = malloc(cm->cm_length, M_MPT2, M_ZERO | M_NOWAIT);
if (!page) {
printf("%s: page alloc failed\n", __func__);
error = ENOMEM;
goto out;
}
cm->cm_data = page;
error = mps_wait_command(sc, cm, 60, CAN_SLEEP);
reply = (MPI2_CONFIG_REPLY *)cm->cm_reply;
if (error || (reply == NULL)) {
/* FIXME */
/*
* If the request returns an error then we need to do a diag
* reset
*/
printf("%s: request for page completed with error %d",
__func__, error);
error = ENXIO;
goto out;
}
ioc_status = le16toh(reply->IOCStatus) & MPI2_IOCSTATUS_MASK;
bcopy(reply, mpi_reply, sizeof(MPI2_CONFIG_REPLY));
if (ioc_status != MPI2_IOCSTATUS_SUCCESS) {
/* FIXME */
/*
* If the request returns an error then we need to do a diag
* reset
*/
printf("%s: page read with error; iocstatus = 0x%x\n",
__func__, ioc_status);
error = ENXIO;
goto out;
}
bcopy(page, config_page, MIN(cm->cm_length, sizeof(Mpi2BiosPage3_t)));
out:
free(page, M_MPT2);
if (cm)
mps_free_command(sc, cm);
return (error);
}
/* return rest of packet length (may be 0) or -1 on error */
static int
parse_80211_header(unsigned char** buf, int len, struct packet_info* p)
{
struct wlan_frame* wh;
int hdrlen;
uint8_t* ra = NULL;
uint8_t* ta = NULL;
uint8_t* bssid = NULL;
uint16_t fc, cap_i;
if (len < 10) /* minimum frame size (CTS/ACK) */
return -1;
p->wlan_mode = WLAN_MODE_UNKNOWN;
wh = (struct wlan_frame*)*buf;
fc = le16toh(wh->fc);
p->wlan_type = (fc & WLAN_FRAME_FC_MASK);
DEBUG("wlan_type %x - type %x - stype %x\n", fc, fc & WLAN_FRAME_FC_TYPE_MASK, fc & WLAN_FRAME_FC_STYPE_MASK);
DEBUG("%s\n", get_packet_type_name(fc));
if (WLAN_FRAME_IS_DATA(fc)) {
hdrlen = 24;
if (WLAN_FRAME_IS_QOS(fc)) {
hdrlen += 2;
if (fc & WLAN_FRAME_FC_ORDER)
hdrlen += 4;
}
/* AP, STA or IBSS */
if ((fc & WLAN_FRAME_FC_FROM_DS) == 0 &&
(fc & WLAN_FRAME_FC_TO_DS) == 0) {
p->wlan_mode = WLAN_MODE_IBSS;
bssid = wh->addr3;
} else if ((fc & WLAN_FRAME_FC_FROM_DS) &&
(fc & WLAN_FRAME_FC_TO_DS)) {
p->wlan_mode = WLAN_MODE_4ADDR;
hdrlen += 6;
if (WLAN_FRAME_IS_QOS(fc)) {
uint16_t qos = le16toh(wh->u.addr4_qos_ht.qos);
DEBUG("4ADDR A-MSDU %x\n", qos & WLAN_FRAME_QOS_AMSDU_PRESENT);
if (qos & WLAN_FRAME_QOS_AMSDU_PRESENT)
bssid = wh->addr3;
// in the MSDU case BSSID is unknown
}
} else if (fc & WLAN_FRAME_FC_FROM_DS) {
p->wlan_mode = WLAN_MODE_AP;
bssid = wh->addr2;
} else if (fc & WLAN_FRAME_FC_TO_DS) {
p->wlan_mode = WLAN_MODE_STA;
bssid = wh->addr1;
}
if (len < hdrlen)
return -1;
p->wlan_nav = le16toh(wh->duration);
DEBUG("DATA NAV %d\n", p->wlan_nav);
p->wlan_seqno = le16toh(wh->seq);
DEBUG("DATA SEQ %d\n", p->wlan_seqno);
DEBUG("A1 %s\n", ether_sprintf(wh->addr1));
DEBUG("A2 %s\n", ether_sprintf(wh->addr2));
DEBUG("A3 %s\n", ether_sprintf(wh->addr3));
if (p->wlan_mode == WLAN_MODE_4ADDR) {
DEBUG("A4 %s\n", ether_sprintf(wh->u.addr4));
}
DEBUG("ToDS %d FromDS %d\n", (fc & WLAN_FRAME_FC_FROM_DS) != 0, (fc & WLAN_FRAME_FC_TO_DS) != 0);
ra = wh->addr1;
ta = wh->addr2;
/* WEP */
if (fc & WLAN_FRAME_FC_PROTECTED)
p->wlan_wep = 1;
if (fc & WLAN_FRAME_FC_RETRY)
p->wlan_retry = 1;
} else if (WLAN_FRAME_IS_CTRL(fc)) {
if (p->wlan_type == WLAN_FRAME_CTS ||
p->wlan_type == WLAN_FRAME_ACK)
hdrlen = 10;
else
hdrlen = 16;
if (len < hdrlen)
return -1;
} else if (WLAN_FRAME_IS_MGMT(fc)) {
hdrlen = 24;
if (fc & WLAN_FRAME_FC_ORDER)
hdrlen += 4;
if (len < hdrlen)
return -1;
ra = wh->addr1;
ta = wh->addr2;
bssid = wh->addr3;
p->wlan_seqno = le16toh(wh->seq);
DEBUG("MGMT SEQ %d\n", p->wlan_seqno);
if (fc & WLAN_FRAME_FC_RETRY)
p->wlan_retry = 1;
} else {
DEBUG("!!!UNKNOWN FRAME!!!");
return -1;
}
p->wlan_len = len;
switch (p->wlan_type) {
case WLAN_FRAME_NULL:
break;
case WLAN_FRAME_QDATA:
p->wlan_qos_class = le16toh(wh->u.qos) & WLAN_FRAME_QOS_TID_MASK;
DEBUG("***QDATA %x\n", p->wlan_qos_class);
break;
case WLAN_FRAME_RTS:
p->wlan_nav = le16toh(wh->duration);
DEBUG("RTS NAV %d\n", p->wlan_nav);
ra = wh->addr1;
ta = wh->addr2;
break;
case WLAN_FRAME_CTS:
p->wlan_nav = le16toh(wh->duration);
DEBUG("CTS NAV %d\n", p->wlan_nav);
ra = wh->addr1;
break;
case WLAN_FRAME_ACK:
p->wlan_nav = le16toh(wh->duration);
DEBUG("ACK NAV %d\n", p->wlan_nav);
ra = wh->addr1;
break;
case WLAN_FRAME_PSPOLL:
ra = wh->addr1;
bssid = wh->addr1;
ta = wh->addr2;
break;
case WLAN_FRAME_CF_END:
case WLAN_FRAME_CF_END_ACK:
ra = wh->addr1;
ta = wh->addr2;
bssid = wh->addr2;
break;
case WLAN_FRAME_BLKACK:
case WLAN_FRAME_BLKACK_REQ:
p->wlan_nav = le16toh(wh->duration);
ra = wh->addr1;
ta = wh->addr2;
break;
case WLAN_FRAME_BEACON:
case WLAN_FRAME_PROBE_RESP:
;
struct wlan_frame_beacon* bc = (struct wlan_frame_beacon*)(*buf + hdrlen);
p->wlan_tsf = le64toh(bc->tsf);
p->wlan_bintval = le16toh(bc->bintval);
//DEBUG("TSF %u\n BINTVAL %u", p->wlan_tsf, p->wlan_bintval);
wlan_parse_information_elements(bc->ie,
len - hdrlen - sizeof(struct wlan_frame_beacon) - 4 /* FCS */, p);
DEBUG("ESSID %s \n", p->wlan_essid );
DEBUG("CHAN %d \n", p->wlan_channel );
cap_i = le16toh(bc->capab);
if (cap_i & WLAN_CAPAB_IBSS)
p->wlan_mode = WLAN_MODE_IBSS;
else if (cap_i & WLAN_CAPAB_ESS)
p->wlan_mode = WLAN_MODE_AP;
if (cap_i & WLAN_CAPAB_PRIVACY)
p->wlan_wep = 1;
break;
case WLAN_FRAME_PROBE_REQ:
wlan_parse_information_elements((*buf + hdrlen),
len - hdrlen - 4 /* FCS */, p);
p->wlan_mode = WLAN_MODE_PROBE;
break;
case WLAN_FRAME_ASSOC_REQ:
case WLAN_FRAME_ASSOC_RESP:
case WLAN_FRAME_REASSOC_REQ:
case WLAN_FRAME_REASSOC_RESP:
case WLAN_FRAME_DISASSOC:
break;
case WLAN_FRAME_AUTH:
if (fc & WLAN_FRAME_FC_PROTECTED)
p->wlan_wep = 1;
/* no break */
case WLAN_FRAME_DEAUTH:
break;
case WLAN_FRAME_ACTION:
break;
}
if (ta != NULL) {
memcpy(p->wlan_src, ta, MAC_LEN);
DEBUG("TA %s\n", ether_sprintf(ta));
}
if (ra != NULL) {
memcpy(p->wlan_dst, ra, MAC_LEN);
DEBUG("RA %s\n", ether_sprintf(ra));
}
if (bssid != NULL) {
memcpy(p->wlan_bssid, bssid, MAC_LEN);
DEBUG("BSSID %s\n", ether_sprintf(bssid));
}
/* only data frames contain more info, otherwise stop parsing */
if (WLAN_FRAME_IS_DATA(p->wlan_type) && p->wlan_wep != 1) {
*buf = *buf + hdrlen;
return len - hdrlen;
}
return 0;
}
static int
ata_getparam(struct ata_device *atadev, int init)
{
struct ata_channel *ch = device_get_softc(device_get_parent(atadev->dev));
struct ata_request *request;
u_int8_t command = 0;
int error = ENOMEM, retries = 2;
if (ch->devices &
(atadev->unit == ATA_MASTER ? ATA_ATA_MASTER : ATA_ATA_SLAVE))
command = ATA_ATA_IDENTIFY;
if (ch->devices &
(atadev->unit == ATA_MASTER ? ATA_ATAPI_MASTER : ATA_ATAPI_SLAVE))
command = ATA_ATAPI_IDENTIFY;
if (!command)
return ENXIO;
while (retries-- > 0 && error) {
if (!(request = ata_alloc_request()))
break;
request->dev = atadev->dev;
request->timeout = 1;
request->retries = 0;
request->u.ata.command = command;
request->flags = (ATA_R_READ|ATA_R_AT_HEAD|ATA_R_DIRECT|ATA_R_QUIET);
request->data = (void *)&atadev->param;
request->bytecount = sizeof(struct ata_params);
request->donecount = 0;
request->transfersize = DEV_BSIZE;
ata_queue_request(request);
error = request->result;
ata_free_request(request);
}
if (!error && (isprint(atadev->param.model[0]) ||
isprint(atadev->param.model[1]))) {
struct ata_params *atacap = &atadev->param;
char buffer[64];
int16_t *ptr;
for (ptr = (int16_t *)atacap;
ptr < (int16_t *)atacap + sizeof(struct ata_params)/2; ptr++) {
*ptr = le16toh(*ptr);
}
if (!(!strncmp(atacap->model, "FX", 2) ||
!strncmp(atacap->model, "NEC", 3) ||
!strncmp(atacap->model, "Pioneer", 7) ||
!strncmp(atacap->model, "SHARP", 5))) {
bswap(atacap->model, sizeof(atacap->model));
bswap(atacap->revision, sizeof(atacap->revision));
bswap(atacap->serial, sizeof(atacap->serial));
}
btrim(atacap->model, sizeof(atacap->model));
bpack(atacap->model, atacap->model, sizeof(atacap->model));
btrim(atacap->revision, sizeof(atacap->revision));
bpack(atacap->revision, atacap->revision, sizeof(atacap->revision));
btrim(atacap->serial, sizeof(atacap->serial));
bpack(atacap->serial, atacap->serial, sizeof(atacap->serial));
if (bootverbose)
kprintf("ata%d-%s: pio=%s wdma=%s udma=%s cable=%s wire\n",
device_get_unit(ch->dev),
atadev->unit == ATA_MASTER ? "master" : "slave",
ata_mode2str(ata_pmode(atacap)),
ata_mode2str(ata_wmode(atacap)),
ata_mode2str(ata_umode(atacap)),
(atacap->hwres & ATA_CABLE_ID) ? "80":"40");
if (init) {
ksprintf(buffer, "%.40s/%.8s", atacap->model, atacap->revision);
device_set_desc_copy(atadev->dev, buffer);
if ((atadev->param.config & ATA_PROTO_ATAPI) &&
(atadev->param.config != ATA_CFA_MAGIC1) &&
(atadev->param.config != ATA_CFA_MAGIC2)) {
if (atapi_dma && ch->dma &&
(atadev->param.config & ATA_DRQ_MASK) != ATA_DRQ_INTR &&
ata_umode(&atadev->param) >= ATA_UDMA2)
atadev->mode = ATA_DMA_MAX;
}
else {
if (ata_dma && ch->dma &&
(ata_umode(&atadev->param) > 0 ||
ata_wmode(&atadev->param) > 0))
atadev->mode = ATA_DMA_MAX;
}
}
}
else {
if (!error)
error = ENXIO;
}
return error;
}
static void dsk_seek_track(
struct image *im, uint16_t track, unsigned int cyl, unsigned int side)
{
struct dib *dib = dib_p(im);
struct tib *tib = tib_p(im);
unsigned int i, nr;
uint32_t tracklen;
im->cur_track = track;
if (cyl >= im->nr_cyls) {
unformatted:
printk("T%u.%u: Empty\n", cyl, side);
memset(tib, 0, sizeof(*tib));
goto out;
}
im->dsk.trk_off = 0x100;
nr = (unsigned int)cyl * im->nr_sides + side;
if (im->dsk.extended) {
if (dib->track_szs[nr] == 0)
goto unformatted;
for (i = 0; i < nr; i++)
im->dsk.trk_off += dib->track_szs[i] * 256;
} else {
im->dsk.trk_off += nr * le16toh(dib->track_sz);
}
/* Read the Track Info Block and Sector Info Blocks. */
F_lseek(&im->fp, im->dsk.trk_off);
F_read(&im->fp, tib, 256, NULL);
im->dsk.trk_off += 256;
if (strncmp(tib->sig, "Track-Info", 10) || !tib->nr_secs)
goto unformatted;
printk("T%u.%u -> %u.%u: %u sectors\n", cyl, side, tib->track,
tib->side, tib->nr_secs);
/* Clamp number of sectors. */
if (tib->nr_secs > 29)
tib->nr_secs = 29;
/* Compute per-sector actual length. */
for (i = 0; i < tib->nr_secs; i++) {
tib->sib[i].actual_length = im->dsk.extended
? le16toh(tib->sib[i].actual_length)
: 128 << min_t(unsigned, tib->sec_sz, 8);
if ((tib->sib[i].actual_length > 8192)
|| (tib->sib[i].n > 6))
F_die(FR_BAD_IMAGE);
}
out:
im->dsk.idx_sz = GAP_4A;
im->dsk.idx_sz += GAP_SYNC + 4 + GAP_1;
im->dsk.idam_sz = GAP_SYNC + 8 + 2 + GAP_2;
im->dsk.dam_sz_pre = GAP_SYNC + 4;
im->dsk.dam_sz_post = 2 + tib->gap3;
/* Work out minimum track length (with no pre-index track gap). */
tracklen = (im->dsk.idam_sz + im->dsk.dam_sz_pre + im->dsk.dam_sz_post)
* tib->nr_secs;
tracklen += im->dsk.idx_sz;
for (i = 0; i < tib->nr_secs; i++) {
tracklen += data_sz(&tib->sib[i]);
if (is_gaps_sector(&tib->sib[i]))
tracklen -= im->dsk.dam_sz_post;
}
tracklen *= 16;
/* Calculate and round the track length. */
im->tracklen_bc = max_t(unsigned int, 100000, tracklen + 20*16);
im->tracklen_bc = (im->tracklen_bc + 31) & ~31;
/* Now calculate the pre-index track gap. */
im->dsk.gap4 = (im->tracklen_bc - tracklen) / 16;
/* Calculate ticks per revolution */
im->stk_per_rev = stk_sysclk(im->tracklen_bc * im->write_bc_ticks);
}
bool IFile::readule16(uint16_t &val)
{
this->read(reinterpret_cast<char *>(&val), sizeof(val));
val = le16toh(val);
return !!(*this);
}
static int
squeeze_entry(int fd,
struct local_file_header *file,
struct cdir_entry *current_entry,
uint32_t &out_offset)
{
uint32_t uncompressed_size = le32toh(file->uncompressed_size);
if (current_entry->compression) {
printf("unexpected compressed entry. aborting.\n");
return -1;
}
char *buf = (char *)malloc(uncompressed_size);
if (!buf) {
printf("failed to malloc output buffer\n");
return -1;
}
z_stream zstr;
memset(&zstr, 0, sizeof(zstr));
if (deflateInit2(&zstr, 9, Z_DEFLATED, -MAX_WBITS, MAX_MEM_LEVEL, Z_DEFAULT_STRATEGY) != Z_OK) {
printf("deflateInit2 failed!\n");
return -1;
}
zstr.next_in = (Bytef *)(file->data + le16toh(file->filename_size) + le16toh(file->extra_field_size));
zstr.avail_in = uncompressed_size;
zstr.avail_out = uncompressed_size;
zstr.next_out = (Bytef *)buf;
uint16_t compression = 8; // Deflate
if (deflate(&zstr, Z_FINISH) != Z_STREAM_END) {
compression = 0;
}
uint32_t compressed_size = zstr.total_out;
deflateEnd(&zstr);
current_entry->compressed_size = htole32(compressed_size);
current_entry->offset = htole32(out_offset);
current_entry->compression = compression;
TRACE("writing entry for %s (%d %d)\n", current_entry->data, current_entry->compressed_size, uncompressed_size);
struct local_file_header *file_copy = (struct local_file_header *)malloc(file_header_size(file));
if (!file_copy) {
printf("couldn't allocate local file header copy\n");
return -1;
}
memcpy(file_copy, file, file_header_size(file));
file_copy->compressed_size = htole32(compressed_size);
file_copy->compression = htole16(compression);
out_offset += simple_write(fd, (char *)file_copy, file_header_size(file));
free(file_copy);
if (!compression) {
out_offset += simple_write(fd, (char *)file + file_header_size(file), uncompressed_size);
} else {
out_offset += simple_write(fd, buf, compressed_size);
}
free(buf);
return 0;
}
/*
* This function determines whether the received frame is a valid UAPSD trigger.
* Called from interrupt context or DPC context depending on parameter isr_context.
*/
bool
ath_net80211_check_uapsdtrigger(ieee80211_handle_t ieee, struct ieee80211_qosframe *qwh, u_int16_t keyix, bool isr_context)
{
struct ieee80211com *ic = NET80211_HANDLE(ieee);
struct ath_softc_net80211 *scn = ATH_SOFTC_NET80211(ic);
struct ieee80211_node *ni;
int tid, ac;
u_int16_t frame_seq;
int queue_depth;
bool isapsd = false;
/*
* Locate the node for sender
*/
IEEE80211_KEYMAP_LOCK(scn);
ni = (keyix != HAL_RXKEYIX_INVALID) ? scn->sc_keyixmap[keyix] : NULL;
if (ni == NULL) {
IEEE80211_KEYMAP_UNLOCK(scn);
/*
* No key index or no entry, do a lookup
*/
if (isr_context) {
ni = ieee80211_find_rxnode_nolock(ic, (struct ieee80211_frame_min *)qwh);
}
else {
ni = ieee80211_find_rxnode(ic, (struct ieee80211_frame_min *)qwh);
}
if (ni == NULL) {
return isapsd;
}
} else {
ieee80211_ref_node(ni);
IEEE80211_KEYMAP_UNLOCK(scn);
}
if (!(ni->ni_flags & IEEE80211_NODE_UAPSD))
goto end;
if (ni->ni_flags & IEEE80211_NODE_UAPSD_SP)
goto end;
/*
* Must deal with change of state here, since otherwise there would
* be a race (on two quick frames from STA) between this code and the
* tasklet where we would:
* - miss a trigger on entry to PS if we're already trigger hunting
* - generate spurious SP on exit (due to frame following exit frame)
*/
if ((((qwh->i_fc[1] & IEEE80211_FC1_PWR_MGT) == IEEE80211_FC1_PWR_MGT) ^
((ni->ni_flags & IEEE80211_NODE_UAPSD_TRIG) == IEEE80211_NODE_UAPSD_TRIG)))
{
ni->ni_flags &= ~IEEE80211_NODE_UAPSD_SP;
if (qwh->i_fc[1] & IEEE80211_FC1_PWR_MGT) {
WME_UAPSD_NODE_TRIGSEQINIT(ni);
ni->ni_stats.ns_uapsd_triggerenabled++;
ni->ni_flags |= IEEE80211_NODE_UAPSD_TRIG;
} else {
/*
* Node transitioned from UAPSD -> Active state. Flush out UAPSD frames
*/
ni->ni_stats.ns_uapsd_active++;
ni->ni_flags &= ~IEEE80211_NODE_UAPSD_TRIG;
scn->sc_ops->process_uapsd_trigger(scn->sc_dev,
ATH_NODE_NET80211(ni)->an_sta,
WME_UAPSD_NODE_MAXQDEPTH, 0, 1, WME_UAPSD_NODE_MAXQDEPTH);
}
goto end;
}
/*
* Check for a valid trigger frame i.e. QoS Data or QoS NULL
*/
if ( ((qwh->i_fc[0] & IEEE80211_FC0_TYPE_MASK) !=
IEEE80211_FC0_TYPE_DATA ) ||
!(qwh->i_fc[0] & IEEE80211_FC0_SUBTYPE_QOS) ) {
goto end;
}
if (((qwh->i_fc[0] & IEEE80211_FC0_TYPE_MASK) == IEEE80211_FC0_TYPE_DATA) &&
(((qwh->i_fc[0] & IEEE80211_FC0_SUBTYPE_MASK) == IEEE80211_FC0_SUBTYPE_QOS) ||
((qwh->i_fc[0] & IEEE80211_FC0_SUBTYPE_MASK) == IEEE80211_FC0_SUBTYPE_QOS_NULL)))
{
tid = qwh->i_qos[0] & IEEE80211_QOS_TID;
ac = TID_TO_WME_AC(tid);
isapsd = true;
if (WME_UAPSD_AC_CAN_TRIGGER(ac, ni)) {
/*
* Detect duplicate triggers and drop if so.
*/
frame_seq = le16toh(*(u_int16_t *)qwh->i_seq);
if ((qwh->i_fc[1] & IEEE80211_FC1_RETRY) &&
frame_seq == ni->ni_uapsd_trigseq[ac])
{
ni->ni_stats.ns_uapsd_duptriggers++;
goto end;
}
/*
* SP in progress for this node, discard trigger.
*/
if (ni->ni_flags & IEEE80211_NODE_UAPSD_SP) {
ni->ni_stats.ns_uapsd_ignoretriggers++;
goto end;
}
/* start the SP */
ni->ni_stats.ns_uapsd_triggers++;
ni->ni_flags |= IEEE80211_NODE_UAPSD_SP;
ni->ni_uapsd_trigseq[ac] = frame_seq;
queue_depth = scn->sc_ops->process_uapsd_trigger(scn->sc_dev,
ATH_NODE_NET80211(ni)->an_sta,
ni->ni_uapsd_maxsp, ac, 0, WME_UAPSD_NODE_MAXQDEPTH);
if (!queue_depth &&
(ni->ni_vap->iv_set_tim != NULL) &&
IEEE80211_NODE_UAPSD_USETIM(ni))
{
ni->ni_vap->iv_set_tim(ni, 0);
}
}
}
end:
ieee80211_free_node(ni);
return isapsd;
}
static uint32_t file_header_size(struct local_file_header *file)
{
return sizeof(*file) +
le16toh(file->filename_size) +
le16toh(file->extra_field_size);
}
int
smb_rap_request(struct smb_rap *rap, struct smb_ctx *ctx)
{
u_int16_t *rp, conv;
u_int32_t *p32;
char *dp, *p = rap->r_nparam;
char ptype;
int error, rdatacnt, rparamcnt, entries, done, dlen;
rdatacnt = rap->r_rcvbuflen;
rparamcnt = rap->r_plen;
error = smb_t2_request(ctx, 0, 0, "\\PIPE\\LANMAN",
rap->r_plen, rap->r_pbuf, /* int tparamcnt, void *tparam */
0, NULL, /* int tdatacnt, void *tdata */
&rparamcnt, rap->r_pbuf, /* rparamcnt, void *rparam */
&rdatacnt, rap->r_rcvbuf /* int *rdatacnt, void *rdata */
);
if (error)
return error;
rp = (u_int16_t*)rap->r_pbuf;
rap->r_result = le16toh(*rp++);
conv = le16toh(*rp++);
rap->r_npbuf = (char*)rp;
rap->r_entries = entries = 0;
done = 0;
while (!done && *p) {
ptype = *p;
switch (ptype) {
case 'e':
rap->r_entries = entries = le16toh(*(u_int16_t*)rap->r_npbuf);
rap->r_npbuf += 2;
p++;
break;
default:
done = 1;
}
/* error = smb_rap_parserpparam(p, &p, &plen);
if (error) {
smb_error("reply parameter mismath %s", 0, p);
return EBADRPC;
}*/
}
rap->r_nparam = p;
/*
* In general, unpacking entries we may need to relocate
* entries for proper alingning. For now use them as is.
*/
dp = rap->r_rcvbuf;
while (entries--) {
p = rap->r_sdata;
while (*p) {
ptype = *p;
error = smb_rap_parserpdata(p, &p, &dlen);
if (error) {
smb_error("reply data mismath %s", 0, p);
return EBADRPC;
}
switch (ptype) {
case 'z':
p32 = (u_int32_t*)dp;
*p32 = (*p32 & 0xffff) - conv;
break;
}
dp += dlen;
}
}
return error;
}
void i2c_handler(__u8 *rbuf, size_t size)
{
struct op_header *oph;
char *tbuf;
struct op_msg *op_req, *op_rsp;
struct cport_msg *cport_req, *cport_rsp;
int i, op_count;
__u8 *write_data;
bool read_op;
int read_count = 0;
bool write_fail = false;
size_t sz;
tbuf = malloc(4 * 1024);
if (!tbuf) {
gbsim_error("failed to allocate i2c handler tx buf\n");
return;
}
cport_req = (struct cport_msg *)rbuf;
op_req = (struct op_msg *)cport_req->data;
cport_rsp = (struct cport_msg *)tbuf;
cport_rsp->cport = 0; /* FIXME hardcoded until we have connections */
op_rsp = (struct op_msg *)cport_rsp->data;
oph = (struct op_header *)&op_req->header;
switch (oph->type) {
case OP_I2C_PROTOCOL_VERSION:
sz = sizeof(struct op_header) +
sizeof(struct protocol_version_rsp);
op_rsp->header.size = htole16((__u16)sz);
op_rsp->header.id = oph->id;
op_rsp->header.type = OP_RESPONSE | OP_I2C_PROTOCOL_VERSION;
op_rsp->header.result = PROTOCOL_STATUS_SUCCESS;
op_rsp->pv_rsp.version_major = GREYBUS_VERSION_MAJOR;
op_rsp->pv_rsp.version_minor = GREYBUS_VERSION_MINOR;
gbsim_debug("Module %d -> AP CPort %d I2C protocol version response\n ",
cport_to_module_id(cport_req->cport), cport_rsp->cport);
if (verbose)
gbsim_dump((__u8 *)op_rsp, sz);
write(cport_in, cport_rsp, sz + 1);
break;
case OP_I2C_PROTOCOL_FUNCTIONALITY:
sz = sizeof(struct op_header) +
sizeof(struct i2c_functionality_rsp);
op_rsp->header.size = htole16((__u16)sz);
op_rsp->header.id = oph->id;
op_rsp->header.type = OP_RESPONSE | OP_I2C_PROTOCOL_FUNCTIONALITY;
op_rsp->header.result = PROTOCOL_STATUS_SUCCESS;
op_rsp->i2c_fcn_rsp.functionality = htole32(I2C_FUNC_I2C);
gbsim_debug("Module %d -> AP CPort %d I2C protocol functionality response\n ",
cport_to_module_id(cport_req->cport), cport_rsp->cport);
if (verbose)
gbsim_dump((__u8 *)op_rsp, sz);
write(cport_in, cport_rsp, sz + 1);
break;
case OP_I2C_PROTOCOL_TIMEOUT:
sz = sizeof(struct op_header) + 0;
op_rsp->header.size = htole16((__u16)sz);
op_rsp->header.id = oph->id;
op_rsp->header.type = OP_RESPONSE | OP_I2C_PROTOCOL_TIMEOUT;
op_rsp->header.result = PROTOCOL_STATUS_SUCCESS;
gbsim_debug("Module %d -> AP CPort %d I2C protocol timeout response\n ",
cport_to_module_id(cport_req->cport), cport_rsp->cport);
if (verbose)
gbsim_dump((__u8 *)op_rsp, sz);
write(cport_in, cport_rsp, sz + 1);
break;
case OP_I2C_PROTOCOL_RETRIES:
sz = sizeof(struct op_header) + 0;
op_rsp->header.size = htole16((__u16)sz);
op_rsp->header.id = oph->id;
op_rsp->header.type = OP_RESPONSE | OP_I2C_PROTOCOL_RETRIES;
op_rsp->header.result = PROTOCOL_STATUS_SUCCESS;
gbsim_debug("Module %d -> AP CPort %d I2C protocol retries response\n ",
cport_to_module_id(cport_req->cport), cport_rsp->cport);
if (verbose)
gbsim_dump((__u8 *)op_rsp, sz);
write(cport_in, cport_rsp, sz + 1);
break;
case OP_I2C_PROTOCOL_TRANSFER:
op_count = le16toh(op_req->i2c_xfer_req.op_count);
write_data = (__u8 *)&op_req->i2c_xfer_req.desc[op_count];
gbsim_debug("Number of transfer ops %d\n", op_count);
for (i = 0; i < op_count; i++) {
struct i2c_transfer_desc *desc;
__u16 addr;
__u16 flags;
__u16 size;
desc = &op_req->i2c_xfer_req.desc[i];
addr = le16toh(desc->addr);
flags = le16toh(desc->flags);
size = le16toh(desc->size);
read_op = (flags & I2C_M_RD) ? true : false;
gbsim_debug("op %d: %s address %04x size %04x\n",
i, (read_op ? "read" : "write"),
addr, size);
/* FIXME: need some error handling */
if (bbb_backend)
if (ioctl(ifd, I2C_SLAVE, addr) < 0)
gbsim_error("failed setting i2c slave address\n");
if (read_op) {
if (bbb_backend) {
int count;
ioctl(ifd, BLKFLSBUF);
count = read(ifd, &op_rsp->i2c_xfer_rsp.data[read_count], size);
if (count != size)
gbsim_error("op %d: failed to read %04x bytes\n", i, size);
} else {
for (i = read_count; i < (read_count + size); i++)
op_rsp->i2c_xfer_rsp.data[i] = data_byte++;
}
read_count += size;
} else {
if (bbb_backend) {
int count;
count = write(ifd, write_data, size);
if (count != size) {
gbsim_debug("op %d: failed to write %04x bytes\n", i, size);
write_fail = true;
}
}
write_data += size;
}
}
op_rsp->header.id = oph->id;
op_rsp->header.type = OP_RESPONSE | OP_I2C_PROTOCOL_TRANSFER;
if (write_fail)
op_rsp->header.result = PROTOCOL_STATUS_RETRY;
else
/* FIXME: handle read failure */
op_rsp->header.result = PROTOCOL_STATUS_SUCCESS;
if (read_op)
sz = sizeof(struct op_header) + 1 + read_count;
else
sz = sizeof(struct op_header) + 1;
op_rsp->header.size = htole16((__u16)sz);
gbsim_debug("Module %d -> AP CPort %d I2C transfer response\n ",
cport_to_module_id(cport_req->cport), cport_rsp->cport);
if (verbose)
gbsim_dump((__u8 *)op_rsp, sz);
write(cport_in, cport_rsp, sz + 1);
break;
default:
gbsim_error("i2c operation type %02x not supported\n", oph->type);
}
free(tbuf);
}
/* Send Change_Connection_Packet_Type command to the unit */
static int
hci_change_connection_packet_type(int s, int argc, char **argv)
{
int n;
char b[512];
ng_hci_change_con_pkt_type_cp cp;
ng_hci_event_pkt_t *e = (ng_hci_event_pkt_t *) b;
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);
/* packet type */
if (sscanf(argv[1], "%x", &n) != 1)
return (USAGE);
cp.pkt_type = (uint16_t) (n & 0xffff);
cp.pkt_type = htole16(cp.pkt_type);
break;
default:
return (USAGE);
}
/* send request and expect status response */
n = sizeof(b);
if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LINK_CONTROL,
NG_HCI_OCF_CHANGE_CON_PKT_TYPE),
(char const *) &cp, sizeof(cp), b, &n) == ERROR)
return (ERROR);
if (*b != 0x00)
return (FAILED);
/* wait for event */
again:
n = sizeof(b);
if (hci_recv(s, b, &n) == ERROR)
return (ERROR);
if (n < sizeof(*e)) {
errno = EIO;
return (ERROR);
}
if (e->event == NG_HCI_EVENT_CON_PKT_TYPE_CHANGED) {
ng_hci_con_pkt_type_changed_ep *ep =
(ng_hci_con_pkt_type_changed_ep *)(e + 1);
if (ep->status != 0x00) {
fprintf(stdout, "Status: %s [%#02x]\n",
hci_status2str(ep->status), ep->status);
return (FAILED);
}
fprintf(stdout, "Connection handle: %d\n",
le16toh(ep->con_handle));
fprintf(stdout, "Packet type: %#04x\n",
le16toh(ep->pkt_type));
} else
goto again;
return (OK);
} /* hci_change_connection_packet_type */