static void wlanEventHandler(dspT *dsp, ssize_t length, uint8 *rxbuf)
{
bcm_event_t *bcmEvent;
wl_event_msg_t *wlEvent;
int i;
if (length == 0 || rxbuf == 0)
return;
bcmEvent = (bcm_event_t *)rxbuf;
wlEvent = &bcmEvent->event;
wlEvent->flags = ntoh16(wlEvent->flags);
wlEvent->version = ntoh16(wlEvent->version);
wlEvent->event_type = ntoh32(wlEvent->event_type);
wlEvent->status = ntoh32(wlEvent->status);
wlEvent->reason = ntoh32(wlEvent->reason);
wlEvent->auth_type = ntoh32(wlEvent->auth_type);
wlEvent->datalen = ntoh32(wlEvent->datalen);
if (wlEvent->datalen > length - sizeof(bcm_event_t)) {
TRACE(TRACE_ERROR, "invalid data length %d > %d\n",
wlEvent->datalen, length - sizeof(bcm_event_t));
}
for (i = 0; i < MAX_WLAN_HANDLER; i++) {
if (dsp->wlanHandler[i].handler != 0)
(dsp->wlanHandler[i].handler)(
dsp->wlanHandler[i].context,
wlEvent->event_type, wlEvent,
(uint8 *)&bcmEvent[1], wlEvent->datalen);
}
}
void analyse_buffer(const unsigned char *buffer, const unsigned int length_in_bytes)
{
enhanced_packet_block_t *epb = (enhanced_packet_block_t *)buffer;
uint16_t ethertype;
void *payload;
ethernet_hdr_t *hdr = (ethernet_hdr_t *) &(epb->data);
ethertype = ntoh16(hdr->ethertype);
// Packet must be VLAN tagged
if (ethertype != 0x8100)
return;
tagged_ethernet_hdr_t *tagged_hdr = (tagged_ethernet_hdr_t *) &(epb->data);
ethertype = ntoh16(tagged_hdr->ethertype);
payload = &(tagged_hdr->payload);
if (ethertype != AVB_1722_ETHERTYPE)
return;
AVB_DataHeader_t *avb_hdr = (AVB_DataHeader_t *)payload;
unsigned int subtype = AVBTP_SUBTYPE(avb_hdr);
if (subtype == 0) {
stream_id_t id;
id.low = AVBTP_STREAM_ID0(avb_hdr);
id.high = AVBTP_STREAM_ID1(avb_hdr);
if (id.low != 0 || id.high != 0)
increment_count(&id, epb->packet_len);
}
}
static void network_to_host_path_record(IB_PATH_RECORD_NO *p_net,
IB_PATH_RECORD *p_host)
{
p_host->ServiceID = ntoh64(p_net->ServiceID);
p_host->DGID.Type.Global.SubnetPrefix = ntoh64(p_net->DGID.Type.Global.SubnetPrefix);
p_host->DGID.Type.Global.InterfaceID = ntoh64(p_net->DGID.Type.Global.InterfaceID);
p_host->SGID.Type.Global.SubnetPrefix = ntoh64(p_net->SGID.Type.Global.SubnetPrefix);
p_host->SGID.Type.Global.InterfaceID = ntoh64(p_net->SGID.Type.Global.InterfaceID);
p_host->DLID = ntoh16(p_net->DLID);
p_host->SLID = ntoh16(p_net->SLID);
p_host->u1.AsReg32 = ntohl(p_net->u1.AsReg32);
p_host->TClass = p_net->TClass;
p_host->Reversible = p_net->Reversible;
p_host->NumbPath = p_net->NumbPath;
p_host->P_Key = ntohs(p_net->P_Key);
p_host->u2.AsReg16 = ntohs(p_net->u2.AsReg16);
p_host->MtuSelector = p_net->MtuSelector;
p_host->Mtu = p_net->Mtu;
p_host->RateSelector = p_net->RateSelector;
p_host->Rate = p_net->Rate;
p_host->PktLifeTimeSelector = p_net->PktLifeTimeSelector;
p_host->PktLifeTime = p_net->PktLifeTime;
p_host->Preference = p_net->Preference;
}
static void getAllMCMemberRecords(AllocFunction alloc, void *allocUd,
int fd, int agent, UInt16 smLid,
SInt32 *numMRs, struct MCMemberRecord **MRs)
{
UInt8 umad[256];
UInt8 *buf = NULL;
SInt64 len;
SInt32 status;
SInt64 i, n;
SInt32 offset;
struct _MCMemberRecord *p;
const SInt32 timeout = 1000;
fillMAD_Get_MCMemberRecord(umad, sizeof(umad), smLid, 0, __LINE__);
libibumad_Send_MAD(fd, agent, umad, sizeof(umad), timeout, 0);
libibumad_Recv_MAD(alloc, allocUd, fd, &buf, &len, timeout);
status = mad_get_field(umad_get_mad(buf), 0, IB_MAD_STATUS_F);
if (UNLIKELY(0 != status)) {
FATAL("status is %d", status);
}
/* RMPP packets.
*/
offset = mad_get_field(umad_get_mad(buf), 0, IB_SA_ATTROFFS_F);
if (UNLIKELY(0 == offset)) {
FATAL("SA attribute offset is zero.");
}
n = (len - IB_SA_DATA_OFFS)/(offset << 3); /* offset is in 8 byte units */
*numMRs = n;
*MRs = alloc(allocUd, NULL, 0, (*numMRs)*sizeof(struct MCMemberRecord));
for (i = 0 ; i < n; ++i) {
p = (struct _MCMemberRecord *)((UInt8 *)umad_get_mad(buf) + IB_SA_DATA_OFFS + i*(offset << 3));
memcpy((*MRs)[i].mgid, p->mgid, 16);
memcpy((*MRs)[i].portGid, p->portGid, 16);
(*MRs)[i].qkey = ntoh32(p->qkey);
(*MRs)[i].mlid = ntoh16(p->mlid);
(*MRs)[i].mtu = p->mtu;
(*MRs)[i].tclass = p->tclass;
(*MRs)[i].pkey = ntoh16(p->pkey);
(*MRs)[i].rate = p->rate;
(*MRs)[i].packetLife = p->packetLife;
/* FIXME
(*MRs)[i].serviceLevel = p->serviceLevel;
(*MRs)[i].flowLabel = ntoh32(p->flowLabel);
(*MRs)[i].hopLimit = p->hopLimit;
*/
(*MRs)[i].scope = (p->scope_joinState & 0xf0) >> 4;
(*MRs)[i].joinState = p->scope_joinState & 0x0f;
(*MRs)[i].proxyJoin = p->proxyJoin;
}
buf = alloc(allocUd, buf, len, 0);
}
static void
dhd_bta_flush_hcidata(dhd_pub_t *pub, uint16 llh)
{
int prec;
struct pktq *q;
uint count = 0;
q = dhd_bus_txq(pub->bus);
if (q == NULL)
return;
DHD_BTA(("dhd: flushing HCI ACL data for logical link %u...\n", llh));
dhd_os_sdlock_txq(pub);
/* Walk through the txq and toss all HCI ACL data packets */
PKTQ_PREC_ITER(q, prec) {
void *head_pkt = NULL;
while (pktq_ppeek(q, prec) != head_pkt) {
void *pkt = pktq_pdeq(q, prec);
int ifidx;
PKTPULL(pub->osh, pkt, dhd_bus_hdrlen(pub->bus));
dhd_prot_hdrpull(pub, &ifidx, pkt);
if (PKTLEN(pub->osh, pkt) >= RFC1042_HDR_LEN) {
struct ether_header *eh =
(struct ether_header *)PKTDATA(pub->osh, pkt);
if (ntoh16(eh->ether_type) < ETHER_TYPE_MIN) {
struct dot11_llc_snap_header *lsh =
(struct dot11_llc_snap_header *)&eh[1];
if (bcmp(lsh, BT_SIG_SNAP_MPROT,
DOT11_LLC_SNAP_HDR_LEN - 2) == 0 &&
ntoh16(lsh->type) == BTA_PROT_L2CAP) {
amp_hci_ACL_data_t *ACL_data =
(amp_hci_ACL_data_t *)&lsh[1];
uint16 handle = ltoh16(ACL_data->handle);
if (HCI_ACL_DATA_HANDLE(handle) == llh) {
PKTFREE(pub->osh, pkt, TRUE);
count ++;
continue;
}
}
}
}
dhd_prot_hdrpush(pub, ifidx, pkt);
PKTPUSH(pub->osh, pkt, dhd_bus_hdrlen(pub->bus));
if (head_pkt == NULL)
head_pkt = pkt;
pktq_penq(q, prec, pkt);
}
}
int _parse_tcp4msg(struct tcp_connection* c)
{
handler_msg* msg = (handler_msg*)TPD_MSG;
if(msg == NULL)
{
//parse head
if(TPD_MSGBEGIN+4 > c->_offset)
{
return -1;
}
char* buf = c->_buf + TPD_MSGBEGIN;
uint16_t msgid = ntoh16(buf);
uint16_t msglen = ntoh16(buf+2);
//check message
msg = tcpmsg_getbyid(msgid,msglen,TPD_USERID,c->_id._i64,TPD_IP);
if(msg == NULL)
{
tcpconnection_close(c,"tcp4 error message");
return -1;
}
TPD_MSGBEGIN += 4;
//put out ,if only message head,
if(msglen == 0)
{
if(-2 == tcpmsg_putout_range(msg,NULL,0))
{
tcpconnection_close(c,"tcp4 parser error");
return -1;
}
return 0;
}
//record
TPD_MSG = msg;
}
//add message body range
if(msg && c->_offset > TPD_MSGBEGIN)
{
char* buf = c->_buf + TPD_MSGBEGIN;
int n = tcpmsg_putout_range(msg,buf,c->_offset-TPD_MSGBEGIN);
if(n >= 0)
{
TPD_MSG = NULL;
TPD_MSGBEGIN += n;
return 0;
}
else if(n == -2)
{
tcpconnection_close(c,"tcp4 parser error2");
return -1;
}
TPD_MSGBEGIN = c->_offset;
}
return -1;
}
void
wl_event_to_host_order(wl_event_msg_t * evt)
{
evt->event_type = ntoh32(evt->event_type);
evt->flags = ntoh16(evt->flags);
evt->status = ntoh32(evt->status);
evt->reason = ntoh32(evt->reason);
evt->auth_type = ntoh32(evt->auth_type);
evt->datalen = ntoh32(evt->datalen);
evt->version = ntoh16(evt->version);
}
static void getOneLinearForwardingTable(AllocFunction alloc, void *allocUd,
int fd, int agent, UInt16 smLid,
UInt16 lid, struct LinearForwardingTable *LFT)
{
UInt8 umad[256];
UInt8 *buf = NULL;
SInt64 len;
SInt32 status;
SInt64 i, n;
SInt32 j, offset;
struct _LinearForwardingTableRecord *p;
const SInt32 timeout = 1000;
/* Build the transaction id from the lid to ensure that we have unique transaction ids
* when looping over multiple lids.
*/
fillMAD_Get_LFTRecord(umad, sizeof(umad), smLid, lid, __LINE__ + lid);
libibumad_Send_MAD(fd, agent, umad, sizeof(umad), timeout, 0);
libibumad_Recv_MAD(alloc, allocUd, fd, &buf, &len, timeout);
status = mad_get_field(umad_get_mad(buf), 0, IB_MAD_STATUS_F);
if (UNLIKELY(0 != status)) {
FATAL("status is %d", status);
}
/* RMPP packets.
*/
offset = mad_get_field(umad_get_mad(buf), 0, IB_SA_ATTROFFS_F);
if (UNLIKELY(0 == offset)) {
FATAL("SA attribute offset is zero.");
}
n = (len - IB_SA_DATA_OFFS)/(offset << 3); /* offset is in 8 byte units */
LFT->len = 64*n;
LFT->lft = alloc(allocUd, NULL, 0, LFT->len*sizeof(UInt16));
for (i = 0 ; i < n; ++i) {
p = (struct _LinearForwardingTableRecord *)((UInt8 *)umad_get_mad(buf) + IB_SA_DATA_OFFS + i*(offset << 3));
LFT->lid = ntoh16(p->lid);
for (j = 0; j < 64; ++j) {
LFT->lft[64*ntoh16(p->block) + j] = p->lft[j];
}
}
buf = alloc(allocUd, buf, len, 0);
}
void
wl_event_to_host_order(wl_event_msg_t * evt)
{
/* Event struct members passed from dongle to host are stored in network
* byte order. Convert all members to host-order.
*/
evt->event_type = ntoh32(evt->event_type);
evt->flags = ntoh16(evt->flags);
evt->status = ntoh32(evt->status);
evt->reason = ntoh32(evt->reason);
evt->auth_type = ntoh32(evt->auth_type);
evt->datalen = ntoh32(evt->datalen);
evt->version = ntoh16(evt->version);
}
void show_all(struct tcpiphdr *ti) {
uint8_t UNUSED *act;
int UNUSED i;
uint16_t UNUSED *act16;
/*
act = ti;
for(i=0;i<40;i++) {
printf("%d: %d ", i, *act);
act++;
}
act16 = ti;
for(i=0;i<20;i++) {
printf("%d: d%d x%x ", i, *act16, *act16);
act16++;
}
*/
printf("\nipovly\n");
printf("ti_next %d\n", ti->ti_next);
printf("ti_prev %d\n", ti->ti_prev);
printf("ti_x1 %d\n", ti->ti_x1);
printf("ti_pr %d\n", ti->ti_pr);
printf("ti_len %d\n", ti->ti_len);
printf("ti_src %x\n", ti->ti_src.s_addr);
printf("ti_dst %x\n", ti->ti_dst.s_addr);
/*
printf("ti_len %d\n", ntoh16(ti->ti_len));
printf("ti_src %x\n", ntoh32(ti->ti_src.s_addr));
printf("ti_dst %x\n", ntoh32(ti->ti_dst.s_addr));
*/
printf("\ntcp header\n");
/*
printf("ti_sport %d\n", ti->ti_sport);
printf("ti_dport %d\n", ti->ti_dport);
printf("ti_seq %d\n", ti->ti_seq);
printf("ti_ack %d\n", ti->ti_ack);
printf("ti_win %d\n", ti->ti_win);
printf("ti_sum %d\n", ti->ti_sum);
printf("ti_urp %d\n", ti->ti_urp);
*/
printf("ti_sport %d\n", ntoh16(ti->ti_sport));
printf("ti_dport %d\n", ntoh16(ti->ti_dport));
printf("ti_seq %d\n", ntoh32(ti->ti_seq));
printf("ti_ack %d\n", ntoh32(ti->ti_ack));
printf("ti_win %d\n", ntoh16(ti->ti_win));
printf("ti_sum %d\n", ntoh16(ti->ti_sum));
printf("ti_urp %d\n", ntoh16(ti->ti_urp));
}
t_osc_err osc_mem_decodeByteorder(unsigned char typetag, char *data, char **out)
{
size_t size = osc_sizeof(typetag, data);
if(!osc_mem_shouldByteswap(typetag)){
memcpy(*out, data, size);
return OSC_ERR_NONE;
}
char tmp[size];
switch(size){
case 1:
break;
case 2:
*((uint16_t *)tmp) = ntoh16(*((uint16_t *)data));
break;
case 4:
*((uint32_t *)tmp) = ntoh32(*((uint32_t *)data));
break;
case 8:
*((uint64_t *)tmp) = ntoh64(*((uint64_t *)data));
break;
case 16:
*((uint128_t *)tmp) = ntoh128(*((uint128_t *)data));
break;
}
memcpy(*out, tmp, size);
return OSC_ERR_NONE;
}
/*** Packet formation *********************************************************/
int pptp_make_packet(PPTP_CONN * conn, void **buf, size_t *size)
{
struct pptp_header *header;
size_t bad_bytes = 0;
assert(conn && conn->call); assert(buf != NULL); assert(size != NULL);
/* Give up unless there are at least sizeof(pptp_header) bytes */
while ((conn->read_size-bad_bytes) >= sizeof(struct pptp_header)) {
/* Throw out bytes until we have a valid header. */
header = (struct pptp_header *) (conn->read_buffer + bad_bytes);
if (ntoh32(header->magic) != PPTP_MAGIC) goto throwitout;
if (ntoh16(header->reserved0) != 0)
log("reserved0 field is not zero! (0x%x) Cisco feature? \n",
ntoh16(header->reserved0));
if (ntoh16(header->length) < sizeof(struct pptp_header)) goto throwitout;
if (ntoh16(header->length) > PPTP_CTRL_SIZE_MAX) goto throwitout;
/* well. I guess it's good. Let's see if we've got it all. */
if (ntoh16(header->length) > (conn->read_size-bad_bytes))
/* nope. Let's wait until we've got it, then. */
goto flushbadbytes;
/* One last check: */
if ((ntoh16(header->pptp_type) == PPTP_MESSAGE_CONTROL) &&
(ntoh16(header->length) !=
PPTP_CTRL_SIZE(ntoh16(header->ctrl_type))))
goto throwitout;
/* well, I guess we've got it. */
*size = ntoh16(header->length);
*buf = malloc(*size);
if (*buf == NULL) { log("Out of memory."); return 0; /* ack! */ }
memcpy(*buf, conn->read_buffer + bad_bytes, *size);
/* Delete this packet from the read_buffer. */
conn->read_size -= (bad_bytes + *size);
memmove(conn->read_buffer, conn->read_buffer + bad_bytes + *size,
conn->read_size);
if (bad_bytes > 0)
log("%lu bad bytes thrown away.", (unsigned long) bad_bytes);
return 1;
throwitout:
bad_bytes++;
}
flushbadbytes:
/* no more packets. Let's get rid of those bad bytes */
conn->read_size -= bad_bytes;
memmove(conn->read_buffer, conn->read_buffer + bad_bytes, conn->read_size);
if (bad_bytes > 0)
log("%lu bad bytes thrown away.", (unsigned long) bad_bytes);
return 0;
}
int gtpie_gettv2(union gtpie_member* ie[], int type, int instance,
uint16_t *dst){
int ien;
ien = gtpie_getie(ie, type, instance);
if (ien>=0)
*dst = ntoh16(ie[ien]->tv2.v);
else
return EOF;
return 0;
}
void tcp_input_tmp(struct eth_fg *cur_fg, struct mbuf *pkt, struct ip_hdr *iphdr, void *tcphdr)
{
struct pbuf *pbuf;
pbuf = pbuf_alloc(PBUF_RAW, ntoh16(iphdr->len) - iphdr->header_len * 4, PBUF_ROM);
pbuf->payload = tcphdr;
pbuf->mbuf = pkt;
// percpu_get(ip_data).current_iphdr_dest.addr = iphdr->dst_addr.addr;
// percpu_get(ip_data).current_iphdr_src.addr = iphdr->src_addr.addr;
tcp_input(cur_fg,pbuf, &iphdr->src_addr,&iphdr->dst_addr);
}
frm_frame *fetchFRMFrame(unsigned char *buf, uint32_t buflen, uint8_t direct) {
int i = 0;
frm_frame *frame = malloc(sizeof(frm_frame));
if (!frame || buflen < 8)
return NULL;
frame->orient = direct;
frame->width = ntoh16(buf, FRMFRAME_OFFSET_WIDTH);
frame->height = ntoh16(buf, FRMFRAME_OFFSET_HEIGHT);
frame->framesize = ntoh32(buf, FRMFRAME_OFFSET_SIZE);
if (frame->framesize != frame->width * frame->height) {
printf("ERR: Framesize different: %d %d\n", frame->framesize, frame->width * frame->height);
return NULL;
}
if (buflen < (12 + frame->width * frame->height) ) {
return NULL;
}
frame->rowptr = malloc( sizeof(unsigned char *) * frame->height);
if (!frame->rowptr) {
printf("ERROR: no memory\n");
free(frame);
return NULL;
}
for(i=0; i < frame->height; ++i) {
frame->rowptr[i] = malloc(sizeof(unsigned char) * frame->width);
if (!frame->rowptr[i]) {
printf("ERROR: no memory\n");
free(frame);
return NULL;
}
memcpy(frame->rowptr[i], &buf[12]+i*frame->width, frame->width);
}
frame->next_frame = NULL;
return frame;
}
/**
* Adds a link local address/route based on the MAC address
* and joins the all-nodes mcast group
*/
void esix_intf_init_interface(esix_ll_addr lla, u8_t interface)
{
struct ip6_addr addr;
//builds our link local and associated multicast addresses
//from the MAC address given by the L2 layer.
//unicast link local
addr.addr1 = hton32(0xfe800000); //0xfe80
addr.addr2 = hton32(0x00000000);
addr.addr3 = hton32( (ntoh16(lla[0]) << 16 & 0xff0000)
| (ntoh16(lla[1]) & 0xff00)
| 0x020000ff ); //stateless autoconf, 0x02 : universal bit
addr.addr4 = hton32( (0xfe000000) //0xfe here is OK
| (ntoh16(lla[1])<< 16 & 0xff0000)
| (ntoh16(lla[2])) );
esix_intf_add_address(&addr,
0x80, // /128
0x0, //this one never expires
LINK_LOCAL);
//first row of the neighbors table is us
addr.addr1 = hton32(0xfe800000); //0xfe80
addr.addr2 = hton32(0x00000000);
addr.addr3 = hton32( (ntoh16(lla[0]) << 16 & 0xff0000)
| (ntoh16(lla[1]) & 0xff00)
| 0x020000ff ); //stateless autoconf, 0x02 : universal bit
addr.addr4 = hton32( (0xfe000000) //0xfe here is OK
| (ntoh16(lla[1])<< 16 & 0xff0000)
| (ntoh16(lla[2])) );
esix_intf_add_neighbor(&addr,
lla, // MAC address
0, // never expires
INTERFACE);
//multicast all-nodes (for router advertisements)
addr.addr1 = hton32(0xff020000); //ff02::1
addr.addr2 = 0;
addr.addr3 = 0;
addr.addr4 = hton32(1);
esix_intf_add_address(&addr,
0x80, // /128
0x0, //this one never expires
MULTICAST);
}
int dhd_adjust_tcp_winsize(int index, int pk_type, int op_mode, struct sk_buff *skb)
{
struct iphdr *ipheader;
struct tcphdr *tcpheader;
uint16 win_size;
int32 incremental_checksum;
if (!dhd_use_tcp_window_size_adjust || !(op_mode & DHD_FLAG_HOSTAP_MODE))
return 0;
if (skb == NULL || skb->data == NULL)
return 0;
if (index == 0 || pk_type == ETHER_TYPE_IP) {
ipheader = (struct iphdr*)(skb->data);
if (ipheader->protocol == IPPROTO_TCP) {
tcpheader = (struct tcphdr*) skb_pull(skb, (ipheader->ihl)<<2);
if (tcpheader) {
win_size = ntoh16(tcpheader->window);
if (win_size < MIN_TCP_WIN_SIZE &&
dhd_port_list_match(ntoh16(tcpheader->dest))) {
incremental_checksum = ntoh16(tcpheader->check);
incremental_checksum += win_size - win_size
*WIN_SIZE_SCALE_FACTOR;
if (incremental_checksum < 0)
--incremental_checksum;
tcpheader->window = hton16(win_size*WIN_SIZE_SCALE_FACTOR);
tcpheader->check = hton16((unsigned short)
incremental_checksum);
}
}
skb_push(skb, (ipheader->ihl)<<2);
}
}
return 0;
}
/*** Packet Dispatch **********************************************************/
int pptp_dispatch_packet(PPTP_CONN * conn, void * buffer, size_t size)
{
int r = 0;
struct pptp_header *header = (struct pptp_header *)buffer;
assert(conn && conn->call); assert(buffer);
assert(ntoh32(header->magic) == PPTP_MAGIC);
assert(ntoh16(header->length) == size);
switch (ntoh16(header->pptp_type)) {
case PPTP_MESSAGE_CONTROL:
r = ctrlp_disp(conn, buffer, size);
break;
case PPTP_MESSAGE_MANAGE:
/* MANAGEMENT messages aren't even part of the spec right now. */
log("PPTP management message received, but not understood.");
break;
default:
log("Unknown PPTP control message type received: %u",
(unsigned int) ntoh16(header->pptp_type));
break;
}
return r;
}
static yrmcds_error
parse_statistics(yrmcds_cnt* c, const yrmcds_cnt_response* r) {
yrmcds_cnt_statistics* s = &c->stats;
s->count = 0;
const char* p = r->body;
const char* end = r->body + r->body_length;
while( p < end ) {
if( p + 4 > end )
return YRMCDS_PROTOCOL_ERROR;
uint16_t name_len = ntoh16(p);
uint16_t value_len = ntoh16(p + 2);
if( p + 4 + name_len + value_len > end )
return YRMCDS_PROTOCOL_ERROR;
yrmcds_error err =
append_stat(s, name_len, value_len, p + 4, p + 4 + name_len);
if( err != YRMCDS_OK )
return err;
p += 4 + name_len + value_len;
}
return YRMCDS_OK;
}
int gtpie_gettlv(union gtpie_member* ie[], int type, int instance,
unsigned int *length, void *dst, unsigned int size){
int ien;
ien = gtpie_getie(ie, type, instance);
if (ien>=0) {
*length = ntoh16(ie[ien]->tlv.l);
if (*length <= size)
memcpy(dst, ie[ien]->tlv.v, *length);
else
return EOF;
}
return 0;
}
static void getAllNodeRecords(AllocFunction alloc, void *allocUd,
int fd, int agent, UInt16 smLid,
SInt32 *numNRs, struct NodeRecord **NRs)
{
UInt8 umad[256];
UInt8 *buf = NULL;
SInt64 len;
SInt32 status;
SInt64 i, n;
SInt32 offset;
struct _NodeRecord *p;
const SInt32 timeout = 1000;
fillMAD_Get_NodeRecord(umad, sizeof(umad), smLid, 0, __LINE__);
libibumad_Send_MAD(fd, agent, umad, sizeof(umad), timeout, 0);
libibumad_Recv_MAD(alloc, allocUd, fd, &buf, &len, timeout);
status = mad_get_field(umad_get_mad(buf), 0, IB_MAD_STATUS_F);
if (UNLIKELY(0 != status)) {
FATAL("status is %d", status);
}
/* RMPP packets.
*/
offset = mad_get_field(umad_get_mad(buf), 0, IB_SA_ATTROFFS_F);
if (UNLIKELY(0 == offset)) {
FATAL("SA attribute offset is zero.");
}
n = (len - IB_SA_DATA_OFFS)/(offset << 3); /* offset is in 8 byte units */
*numNRs = n;
*NRs = alloc(allocUd, NULL, 0, (*numNRs)*sizeof(struct NodeRecord));
for (i = 0 ; i < n; ++i) {
p = (struct _NodeRecord *)((UInt8 *)umad_get_mad(buf) + IB_SA_DATA_OFFS + i*(offset << 3));
(*NRs)[i].lid = ntoh16(p->lid);
(*NRs)[i].nodeType = p->nodeType;
(*NRs)[i].numPorts = p->numPorts;
(*NRs)[i].sysGuid = ntoh64(p->sysGuid);
(*NRs)[i].nodeGuid = ntoh64(p->nodeGuid);
(*NRs)[i].portGuid = ntoh64(p->portGuid);
memcpy((*NRs)[i].info, p->info, 64*sizeof(UInt8));
}
buf = alloc(allocUd, buf, len, 0);
}
static void getOneNodeRecord(AllocFunction alloc, void *allocUd,
int fd, int agent, UInt16 smLid,
UInt16 lid, struct NodeRecord *NR)
{
UInt8 umad[256];
UInt8 *buf = NULL;
SInt64 len;
SInt32 status;
SInt64 n;
SInt32 offset;
struct _NodeRecord *p;
const SInt32 timeout = 1000;
fillMAD_Get_NodeRecord(umad, sizeof(umad), smLid, lid, __LINE__ + lid);
libibumad_Send_MAD(fd, agent, umad, sizeof(umad), timeout, 0);
libibumad_Recv_MAD(alloc, allocUd, fd, &buf, &len, timeout);
status = mad_get_field(umad_get_mad(buf), 0, IB_MAD_STATUS_F);
if (UNLIKELY(0 != status)) {
FATAL("status is %d", status);
}
/* RMPP packets.
*/
offset = mad_get_field(umad_get_mad(buf), 0, IB_SA_ATTROFFS_F);
if (UNLIKELY(0 == offset)) {
FATAL("SA attribute offset is zero.");
}
n = (len - IB_SA_DATA_OFFS)/(offset << 3); /* offset is in 8 byte units */
if (UNLIKELY(1 != n)) {
FATAL("Expected one node record but found %d in answer", n);
}
p = (struct _NodeRecord *)((UInt8 *)umad_get_mad(buf) + IB_SA_DATA_OFFS + 0*(offset << 3));
NR->lid = ntoh16(p->lid);
NR->nodeType = p->nodeType;
NR->numPorts = p->numPorts;
NR->sysGuid = ntoh64(p->sysGuid);
NR->nodeGuid = ntoh64(p->nodeGuid);
NR->portGuid = ntoh64(p->portGuid);
memcpy(NR->info, p->info, 64*sizeof(UInt8));
buf = alloc(allocUd, buf, len, 0);
}
/*** report a sent packet ****************************************************/
static void ctrlp_rep( void * buffer, int size, int isbuff)
{
struct pptp_header *packet = buffer;
unsigned int type;
if(size < sizeof(struct pptp_header)) return;
type = ntoh16(packet->ctrl_type);
/* FIXME: do not report sending echo requests as long as they are
* sent in a signal handler. This may dead lock as the syslog call
* is not reentrant */
if( type == PPTP_ECHO_RQST ) return;
/* don't keep reporting sending of echo's */
if( (type == PPTP_ECHO_RQST || type == PPTP_ECHO_RPLY) && nlogecho <= 0 ) return;
log("%s control packet type is %d '%s'\n",isbuff ? "Buffered" : "Sent",
type, ctrl_msg_types[type <= MAX_CTRLMSG_TYPE ? type : 0]);
}
static yrmcds_error
parse_dump_record(yrmcds_cnt* c, yrmcds_cnt_response* r) {
if( r->body_length == 0 ) {
// End of dump
return YRMCDS_OK;
}
if( r->body_length < 10 ) {
c->invalid = 1;
return YRMCDS_PROTOCOL_ERROR;
}
r->current_consumption = ntoh32(r->body);
r->max_consumption = ntoh32(r->body + 4);
r->name_length = ntoh16(r->body + 8);
if( r->body_length < 10 + r->name_length ) {
c->invalid = 1;
return YRMCDS_PROTOCOL_ERROR;
}
r->name = r->body + 10;
return YRMCDS_OK;
}
um_status_t um_parse_header(const uint8_t* buf, size_t nbuf,
um_message_info_t* info_out) {
FBI_ASSERT(info_out);
if (nbuf < sizeof(entry_list_msg_t)) {
return um_not_enough_data;
}
entry_list_msg_t* header = (entry_list_msg_t*) buf;
if (header->msg_header.magic_byte != ENTRY_LIST_MAGIC_BYTE) {
return um_not_umbrella_message;
}
info_out->message_size = ntoh32(header->total_size);
uint16_t nentries = ntoh16(header->nentries);
info_out->header_size = sizeof(entry_list_msg_t) +
sizeof(um_elist_entry_t) * nentries;
if (info_out->header_size > info_out->message_size) {
return um_header_parse_error;
}
info_out->body_size = info_out->message_size - info_out->header_size;
return um_ok;
}
static void
wl_show_host_event(wl_event_msg_t *event, void *event_data)
{
uint i, status, reason;
bool group = FALSE, flush_txq = FALSE, link = FALSE;
char *auth_str, *event_name;
uchar *buf;
char err_msg[256], eabuf[ETHER_ADDR_STR_LEN];
static struct {uint event; char *event_name;} event_names[] = {
{WLC_E_SET_SSID, "SET_SSID"},
{WLC_E_JOIN, "JOIN"},
{WLC_E_START, "START"},
{WLC_E_AUTH, "AUTH"},
{WLC_E_AUTH_IND, "AUTH_IND"},
{WLC_E_DEAUTH, "DEAUTH"},
{WLC_E_DEAUTH_IND, "DEAUTH_IND"},
{WLC_E_ASSOC, "ASSOC"},
{WLC_E_ASSOC_IND, "ASSOC_IND"},
{WLC_E_REASSOC, "REASSOC"},
{WLC_E_REASSOC_IND, "REASSOC_IND"},
{WLC_E_DISASSOC, "DISASSOC"},
{WLC_E_DISASSOC_IND, "DISASSOC_IND"},
{WLC_E_QUIET_START, "START_QUIET"},
{WLC_E_QUIET_END, "END_QUIET"},
{WLC_E_BEACON_RX, "BEACON_RX"},
{WLC_E_LINK, "LINK"},
{WLC_E_MIC_ERROR, "MIC_ERROR"},
{WLC_E_NDIS_LINK, "NDIS_LINK"},
{WLC_E_ROAM, "ROAM"},
{WLC_E_TXFAIL, "TXFAIL"},
{WLC_E_PMKID_CACHE, "PMKID_CACHE"},
{WLC_E_RETROGRADE_TSF, "RETROGRADE_TSF"},
{WLC_E_PRUNE, "PRUNE"},
{WLC_E_AUTOAUTH, "AUTOAUTH"},
{WLC_E_EAPOL_MSG, "EAPOL_MSG"},
{WLC_E_SCAN_COMPLETE, "SCAN_COMPLETE"},
{WLC_E_ADDTS_IND, "ADDTS_IND"},
{WLC_E_DELTS_IND, "DELTS_IND"},
{WLC_E_BCNSENT_IND, "BCNSENT_IND"},
{WLC_E_BCNRX_MSG, "BCNRX_MSG"},
{WLC_E_BCNLOST_MSG, "BCNLOST_MSG"},
{WLC_E_ROAM_PREP, "ROAM_PREP"},
{WLC_E_PFN_NET_FOUND, "PNO_NET_FOUND"},
{WLC_E_PFN_NET_LOST, "PNO_NET_LOST"},
{WLC_E_RESET_COMPLETE, "RESET_COMPLETE"},
{WLC_E_JOIN_START, "JOIN_START"},
{WLC_E_ROAM_START, "ROAM_START"},
{WLC_E_ASSOC_START, "ASSOC_START"},
{WLC_E_IBSS_ASSOC, "IBSS_ASSOC"},
{WLC_E_RADIO, "RADIO"},
{WLC_E_PSM_WATCHDOG, "PSM_WATCHDOG"},
{WLC_E_PROBREQ_MSG, "PROBREQ_MSG"},
{WLC_E_SCAN_CONFIRM_IND, "SCAN_CONFIRM_IND"},
{WLC_E_PSK_SUP, "PSK_SUP"},
{WLC_E_COUNTRY_CODE_CHANGED, "COUNTRY_CODE_CHANGED"},
{WLC_E_EXCEEDED_MEDIUM_TIME, "EXCEEDED_MEDIUM_TIME"},
{WLC_E_ICV_ERROR, "ICV_ERROR"},
{WLC_E_UNICAST_DECODE_ERROR, "UNICAST_DECODE_ERROR"},
{WLC_E_MULTICAST_DECODE_ERROR, "MULTICAST_DECODE_ERROR"},
{WLC_E_TRACE, "TRACE"},
{WLC_E_ACTION_FRAME, "ACTION FRAME"},
{WLC_E_ACTION_FRAME_COMPLETE, "ACTION FRAME TX COMPLETE"},
{WLC_E_IF, "IF"},
{WLC_E_RSSI, "RSSI"},
{WLC_E_PFN_SCAN_COMPLETE, "SCAN_COMPLETE"}
};
uint event_type, flags, auth_type, datalen;
event_type = ntoh32(event->event_type);
flags = ntoh16(event->flags);
status = ntoh32(event->status);
reason = ntoh32(event->reason);
auth_type = ntoh32(event->auth_type);
datalen = ntoh32(event->datalen);
/* debug dump of event messages */
sprintf(eabuf, "%02x:%02x:%02x:%02x:%02x:%02x",
(uchar)event->addr.octet[0]&0xff,
(uchar)event->addr.octet[1]&0xff,
(uchar)event->addr.octet[2]&0xff,
(uchar)event->addr.octet[3]&0xff,
(uchar)event->addr.octet[4]&0xff,
(uchar)event->addr.octet[5]&0xff);
event_name = "UNKNOWN";
for (i = 0; i < ARRAYSIZE(event_names); i++) {
if (event_names[i].event == event_type)
event_name = event_names[i].event_name;
}
if (flags & WLC_EVENT_MSG_LINK)
link = TRUE;
if (flags & WLC_EVENT_MSG_GROUP)
group = TRUE;
if (flags & WLC_EVENT_MSG_FLUSHTXQ)
flush_txq = TRUE;
switch (event_type) {
case WLC_E_START:
case WLC_E_DEAUTH:
case WLC_E_DISASSOC:
DHD_EVENT(("MACEVENT: %s, MAC %s\n", event_name, eabuf));
break;
case WLC_E_ASSOC_IND:
case WLC_E_REASSOC_IND:
#ifdef APSTA_PINGTEST
{
int i;
for (i = 0; i < MAX_GUEST; ++i)
if (ETHER_ISNULLADDR(&guest_eas[i]))
break;
if (i < MAX_GUEST)
bcopy(event->addr.octet, guest_eas[i].octet, ETHER_ADDR_LEN);
}
#endif /* APSTA_PINGTEST */
DHD_EVENT(("MACEVENT: %s, MAC %s\n", event_name, eabuf));
break;
case WLC_E_ASSOC:
case WLC_E_REASSOC:
if (status == WLC_E_STATUS_SUCCESS) {
DHD_EVENT(("MACEVENT: %s, MAC %s, SUCCESS\n", event_name, eabuf));
} else if (status == WLC_E_STATUS_TIMEOUT) {
DHD_EVENT(("MACEVENT: %s, MAC %s, TIMEOUT\n", event_name, eabuf));
} else if (status == WLC_E_STATUS_FAIL) {
DHD_EVENT(("MACEVENT: %s, MAC %s, FAILURE, reason %d\n",
event_name, eabuf, (int)reason));
} else {
DHD_EVENT(("MACEVENT: %s, MAC %s, unexpected status %d\n",
event_name, eabuf, (int)status));
}
break;
case WLC_E_DEAUTH_IND:
case WLC_E_DISASSOC_IND:
#ifdef APSTA_PINGTEST
{
int i;
for (i = 0; i < MAX_GUEST; ++i) {
if (bcmp(guest_eas[i].octet, event->addr.octet,
ETHER_ADDR_LEN) == 0) {
bzero(guest_eas[i].octet, ETHER_ADDR_LEN);
break;
}
}
}
#endif /* APSTA_PINGTEST */
DHD_EVENT(("MACEVENT: %s, MAC %s, reason %d\n", event_name, eabuf, (int)reason));
break;
case WLC_E_AUTH:
case WLC_E_AUTH_IND:
if (auth_type == DOT11_OPEN_SYSTEM)
auth_str = "Open System";
else if (auth_type == DOT11_SHARED_KEY)
auth_str = "Shared Key";
else {
sprintf(err_msg, "AUTH unknown: %d", (int)auth_type);
auth_str = err_msg;
}
if (event_type == WLC_E_AUTH_IND) {
DHD_EVENT(("MACEVENT: %s, MAC %s, %s\n", event_name, eabuf, auth_str));
} else if (status == WLC_E_STATUS_SUCCESS) {
DHD_EVENT(("MACEVENT: %s, MAC %s, %s, SUCCESS\n",
event_name, eabuf, auth_str));
} else if (status == WLC_E_STATUS_TIMEOUT) {
DHD_EVENT(("MACEVENT: %s, MAC %s, %s, TIMEOUT\n",
event_name, eabuf, auth_str));
} else if (status == WLC_E_STATUS_FAIL) {
DHD_EVENT(("MACEVENT: %s, MAC %s, %s, FAILURE, reason %d\n",
event_name, eabuf, auth_str, (int)reason));
}
break;
case WLC_E_JOIN:
case WLC_E_ROAM:
case WLC_E_SET_SSID:
if (status == WLC_E_STATUS_SUCCESS) {
DHD_EVENT(("MACEVENT: %s, MAC %s\n", event_name, eabuf));
} else if (status == WLC_E_STATUS_FAIL) {
DHD_EVENT(("MACEVENT: %s, failed\n", event_name));
} else if (status == WLC_E_STATUS_NO_NETWORKS) {
DHD_EVENT(("MACEVENT: %s, no networks found\n", event_name));
} else {
DHD_EVENT(("MACEVENT: %s, unexpected status %d\n",
event_name, (int)status));
}
break;
case WLC_E_BEACON_RX:
if (status == WLC_E_STATUS_SUCCESS) {
DHD_EVENT(("MACEVENT: %s, SUCCESS\n", event_name));
} else if (status == WLC_E_STATUS_FAIL) {
DHD_EVENT(("MACEVENT: %s, FAIL\n", event_name));
} else {
DHD_EVENT(("MACEVENT: %s, status %d\n", event_name, status));
}
break;
case WLC_E_LINK:
DHD_EVENT(("MACEVENT: %s %s\n", event_name, link?"UP":"DOWN"));
break;
case WLC_E_MIC_ERROR:
DHD_EVENT(("MACEVENT: %s, MAC %s, Group %d, Flush %d\n",
event_name, eabuf, group, flush_txq));
break;
case WLC_E_ICV_ERROR:
case WLC_E_UNICAST_DECODE_ERROR:
case WLC_E_MULTICAST_DECODE_ERROR:
DHD_EVENT(("MACEVENT: %s, MAC %s\n",
event_name, eabuf));
break;
case WLC_E_TXFAIL:
DHD_EVENT(("MACEVENT: %s, RA %s\n", event_name, eabuf));
break;
case WLC_E_SCAN_COMPLETE:
case WLC_E_PMKID_CACHE:
DHD_EVENT(("MACEVENT: %s\n", event_name));
break;
case WLC_E_PFN_NET_FOUND:
case WLC_E_PFN_NET_LOST:
case WLC_E_PFN_SCAN_COMPLETE:
DHD_EVENT(("PNOEVENT: %s\n", event_name));
break;
case WLC_E_PSK_SUP:
case WLC_E_PRUNE:
DHD_EVENT(("MACEVENT: %s, status %d, reason %d\n",
event_name, (int)status, (int)reason));
break;
case WLC_E_TRACE:
{
static uint32 seqnum_prev = 0;
msgtrace_hdr_t hdr;
uint32 nblost;
char *s, *p;
buf = (uchar *) event_data;
memcpy(&hdr, buf, MSGTRACE_HDRLEN);
if (hdr.version != MSGTRACE_VERSION) {
printf("\nMACEVENT: %s [unsupported version --> "
"dhd version:%d dongle version:%d]\n",
event_name, MSGTRACE_VERSION, hdr.version);
/* Reset datalen to avoid display below */
datalen = 0;
break;
}
/* There are 2 bytes available at the end of data */
buf[MSGTRACE_HDRLEN + ntoh16(hdr.len)] = '\0';
if (ntoh32(hdr.discarded_bytes) || ntoh32(hdr.discarded_printf)) {
printf("\nWLC_E_TRACE: [Discarded traces in dongle -->"
"discarded_bytes %d discarded_printf %d]\n",
ntoh32(hdr.discarded_bytes), ntoh32(hdr.discarded_printf));
}
nblost = ntoh32(hdr.seqnum) - seqnum_prev - 1;
if (nblost > 0) {
printf("\nWLC_E_TRACE: [Event lost --> seqnum %d nblost %d\n",
ntoh32(hdr.seqnum), nblost);
}
seqnum_prev = ntoh32(hdr.seqnum);
/* Display the trace buffer. Advance from \n to \n to avoid display big
* printf (issue with Linux printk )
*/
p = &(buf[MSGTRACE_HDRLEN]);
while ((s = strstr(p, "\n")) != NULL) {
*s = '\0';
printf("%s\n", p);
p = s+1;
}
printf("%s\n", p);
/* Reset datalen to avoid display below */
datalen = 0;
}
break;
case WLC_E_RSSI:
DHD_EVENT(("MACEVENT: %s %d\n", event_name, ntoh32(*((int *)event_data))));
break;
default:
DHD_EVENT(("MACEVENT: %s %d, MAC %s, status %d, reason %d, auth %d\n",
event_name, event_type, eabuf, (int)status, (int)reason,
(int)auth_type));
break;
}
/* show any appended data */
if (datalen) {
buf = (uchar *) event_data;
DHD_EVENT((" data (%d) : ", datalen));
for (i = 0; i < datalen; i++)
DHD_EVENT((" 0x%02x ", *buf++));
DHD_EVENT(("\n"));
}
}
pkt_frag_t pkt_frag_info(osl_t *osh, void *p)
{
uint8 *frame;
int length;
uint8 *pt; /* Pointer to type field */
uint16 ethertype;
struct ipv4_hdr *iph; /* IP frame pointer */
int ipl; /* IP frame length */
uint16 iph_frag;
ASSERT(osh && p);
frame = PKTDATA(osh, p);
length = PKTLEN(osh, p);
/* Process Ethernet II or SNAP-encapsulated 802.3 frames */
if (length < ETHER_HDR_LEN) {
DHD_INFO(("%s: short eth frame (%d)\n", __FUNCTION__, length));
return DHD_PKT_FRAG_NONE;
} else if (ntoh16(*(uint16 *)(frame + ETHER_TYPE_OFFSET)) >= ETHER_TYPE_MIN) {
/* Frame is Ethernet II */
pt = frame + ETHER_TYPE_OFFSET;
} else if (length >= ETHER_HDR_LEN + SNAP_HDR_LEN + ETHER_TYPE_LEN &&
!bcmp(llc_snap_hdr, frame + ETHER_HDR_LEN, SNAP_HDR_LEN)) {
pt = frame + ETHER_HDR_LEN + SNAP_HDR_LEN;
} else {
DHD_INFO(("%s: non-SNAP 802.3 frame\n", __FUNCTION__));
return DHD_PKT_FRAG_NONE;
}
ethertype = ntoh16(*(uint16 *)pt);
/* Skip VLAN tag, if any */
if (ethertype == ETHER_TYPE_8021Q) {
pt += VLAN_TAG_LEN;
if (pt + ETHER_TYPE_LEN > frame + length) {
DHD_INFO(("%s: short VLAN frame (%d)\n", __FUNCTION__, length));
return DHD_PKT_FRAG_NONE;
}
ethertype = ntoh16(*(uint16 *)pt);
}
if (ethertype != ETHER_TYPE_IP) {
DHD_INFO(("%s: non-IP frame (ethertype 0x%x, length %d)\n",
__FUNCTION__, ethertype, length));
return DHD_PKT_FRAG_NONE;
}
iph = (struct ipv4_hdr *)(pt + ETHER_TYPE_LEN);
ipl = (uint)(length - (pt + ETHER_TYPE_LEN - frame));
/* We support IPv4 only */
if ((ipl < IPV4_OPTIONS_OFFSET) || (IP_VER(iph) != IP_VER_4)) {
DHD_INFO(("%s: short frame (%d) or non-IPv4\n", __FUNCTION__, ipl));
return DHD_PKT_FRAG_NONE;
}
iph_frag = ntoh16(iph->frag);
if (iph_frag & IPV4_FRAG_DONT) {
return DHD_PKT_FRAG_NONE;
} else if ((iph_frag & IPV4_FRAG_MORE) == 0) {
return DHD_PKT_FRAG_LAST;
} else {
return (iph_frag & IPV4_FRAG_OFFSET_MASK)? DHD_PKT_FRAG_CONT : DHD_PKT_FRAG_FIRST;
}
}
/*** decaps_gre ***************************************************************/
int decaps_gre (int fd, callback_t callback, int cl)
{
unsigned char buffer[PACKET_MAX + 64 /*ip header*/];
struct pptp_gre_header *header;
int status, ip_len = 0;
static int first = 1;
unsigned int headersize;
unsigned int payload_len;
u_int32_t seq;
if ((status = read (fd, buffer, sizeof(buffer))) <= 0) {
warn("short read (%d): %s", status, strerror(errno));
stats.rx_errors++;
return -1;
}
/* strip off IP header, if present */
if ((buffer[0] & 0xF0) == 0x40)
ip_len = (buffer[0] & 0xF) * 4;
header = (struct pptp_gre_header *)(buffer + ip_len);
/* verify packet (else discard) */
if ( /* version should be 1 */
((ntoh8(header->ver) & 0x7F) != PPTP_GRE_VER) ||
/* PPTP-GRE protocol for PPTP */
(ntoh16(header->protocol) != PPTP_GRE_PROTO)||
/* flag C should be clear */
PPTP_GRE_IS_C(ntoh8(header->flags)) ||
/* flag R should be clear */
PPTP_GRE_IS_R(ntoh8(header->flags)) ||
/* flag K should be set */
(!PPTP_GRE_IS_K(ntoh8(header->flags))) ||
/* routing and recursion ctrl = 0 */
((ntoh8(header->flags)&0xF) != 0)) {
/* if invalid, discard this packet */
warn("Discarding GRE: %X %X %X %X %X %X",
ntoh8(header->ver)&0x7F, ntoh16(header->protocol),
PPTP_GRE_IS_C(ntoh8(header->flags)),
PPTP_GRE_IS_R(ntoh8(header->flags)),
PPTP_GRE_IS_K(ntoh8(header->flags)),
ntoh8(header->flags) & 0xF);
stats.rx_invalid++;
return 0;
}
/* silently discard packets not for this call */
if (ntoh16(header->call_id) != pptp_gre_call_id) return 0;
/* test if acknowledgement present */
if (PPTP_GRE_IS_A(ntoh8(header->ver))) {
u_int32_t ack = (PPTP_GRE_IS_S(ntoh8(header->flags)))?
header->ack:header->seq; /* ack in different place if S = 0 */
ack = ntoh32( ack);
if (ack > ack_recv) ack_recv = ack;
/* also handle sequence number wrap-around */
if (WRAPPED(ack,ack_recv)) ack_recv = ack;
if (ack_recv == stats.pt.seq) {
int rtt = time_now_usecs() - stats.pt.time;
stats.rtt = (stats.rtt + rtt) / 2;
}
}
/* test if payload present */
if (!PPTP_GRE_IS_S(ntoh8(header->flags)))
return 0; /* ack, but no payload */
headersize = sizeof(*header);
payload_len = ntoh16(header->payload_len);
seq = ntoh32(header->seq);
/* no ack present? */
if (!PPTP_GRE_IS_A(ntoh8(header->ver))) headersize -= sizeof(header->ack);
/* check for incomplete packet (length smaller than expected) */
if (status - headersize < payload_len) {
warn("discarding truncated packet (expected %d, got %d bytes)",
payload_len, status - headersize);
stats.rx_truncated++;
return 0;
}
/* check for expected sequence number */
if ( first || (seq == seq_recv + 1)) { /* wrap-around safe */
if ( log_level >= 2 )
log("accepting packet %d", seq);
stats.rx_accepted++;
first = 0;
seq_recv = seq;
return callback(cl, buffer + ip_len + headersize, payload_len);
/* out of order, check if the number is too low and discard the packet.
* (handle sequence number wrap-around, and try to do it right) */
} else if ( seq < seq_recv + 1 || WRAPPED(seq_recv, seq) ) {
if ( log_level >= 1 )
log("discarding duplicate or old packet %d (expecting %d)",
seq, seq_recv + 1);
stats.rx_underwin++;
/* sequence number too high, is it reasonably close? */
} else if ( seq < seq_recv + MISSING_WINDOW ||
WRAPPED(seq, seq_recv + MISSING_WINDOW) ) {
stats.rx_buffered++;
if ( log_level >= 1 )
log("%s packet %d (expecting %d, lost or reordered)",
disable_buffer ? "accepting" : "buffering",
seq, seq_recv+1);
if ( disable_buffer ) {
seq_recv = seq;
stats.rx_lost += seq - seq_recv - 1;
return callback(cl, buffer + ip_len + headersize, payload_len);
} else {
pqueue_add(seq, buffer + ip_len + headersize, payload_len);
}
/* no, packet must be discarded */
} else {
if ( log_level >= 1 )
warn("discarding bogus packet %d (expecting %d)",
seq, seq_recv + 1);
stats.rx_overwin++;
}
return 0;
}
/* returns 0 on success, or <0 on read failure */
int decaps_hdlc(int fd, int (*cb)(int cl, void *pack, unsigned int len), int cl)
{
unsigned char buffer[PACKET_MAX];
unsigned int start = 0;
int end;
int status;
static unsigned int len = 0, escape = 0;
static unsigned char copy[PACKET_MAX];
static int checkedsync = 0;
/* start is start of packet. end is end of buffer data */
/* this is the only blocking read we will allow */
if ((end = read (fd, buffer, sizeof(buffer))) <= 0) {
int saved_errno = errno;
warn("short read (%d): %s", end, strerror(saved_errno));
switch (saved_errno) {
case EMSGSIZE: {
socklen_t optval, optlen = sizeof(optval);
warn("transmitted GRE packet triggered an ICMP destination unreachable, fragmentation needed, or exceeds the MTU of the network interface");
#define IP_MTU 14
if(getsockopt(fd, IPPROTO_IP, IP_MTU, &optval, &optlen) < 0)
warn("getsockopt: %s", strerror(errno));
warn("getsockopt: IP_MTU: %d\n", optval);
return 0;
}
case EIO:
warn("pppd may have shutdown, see pppd log");
break;
}
return -1;
}
/* warn if the sync options of ppp and pptp don't match */
if( !checkedsync) {
checkedsync = 1;
if( buffer[0] == HDLC_FLAG){
if( syncppp )
warn( "pptp --sync option is active, "
"yet the ppp mode is asynchronous!\n");
}
else if( !syncppp )
warn( "The ppp mode is synchronous, "
"yet no pptp --sync option is specified!\n");
}
/* in synchronous mode there are no hdlc control characters nor checksum
* bytes. Find end of packet with the length information in the PPP packet
*/
if ( syncppp ){
while ( start + 8 < end) {
len = ntoh16(*(short int *)(buffer + start + 6)) + 4;
/* note: the buffer may contain an incomplete packet at the end
* this packet will be read again at the next read() */
if ( start + len <= end)
if ((status = cb (cl, buffer + start, len)) < 0)
return status; /* error-check */
start += len;
}
return 0;
}
/* asynchronous mode */
while (start < end) {
/* Copy to 'copy' and un-escape as we go. */
while (buffer[start] != HDLC_FLAG) {
if ((escape == 0) && buffer[start] == HDLC_ESCAPE) {
escape = HDLC_TRANSPARENCY;
} else {
if (len < PACKET_MAX)
copy [len++] = buffer[start] ^ escape;
escape = 0;
}
start++;
if (start >= end)
return 0; /* No more data, but the frame is not complete yet. */
}
/* found flag. skip past it */
start++;
/* check for over-short packets and silently discard, as per RFC1662 */
if ((len < 4) || (escape != 0)) {
len = 0; escape = 0;
continue;
}
/* check, then remove the 16-bit FCS checksum field */
if (pppfcs16 (PPPINITFCS16, copy, len) != PPPGOODFCS16)
warn("Bad Frame Check Sequence during PPP to GRE decapsulation");
len -= sizeof(u_int16_t);
/* so now we have a packet of length 'len' in 'copy' */
if ((status = cb (cl, copy, len)) < 0)
return status; /* error-check */
/* Great! Let's do more! */
len = 0; escape = 0;
}
return 0;
/* No more data to process. */
}
/** ========================================================================= */
FSTATUS oib_recv_mad_no_alloc(struct oib_port *port, uint8_t *recv_mad, size_t *recv_size,
int timeout_ms, struct oib_mad_addr *addr)
{
size_t length = *recv_size;
ib_user_mad_t *umad = NULL;
int mad_agent;
uint32_t my_umad_status = 0;
FSTATUS status = FSUCCESS;
if (!port || !recv_mad || !*recv_size)
return FINVALID_PARAMETER;
umad = umad_alloc(1, length + umad_size());
if (!umad) {
OUTPUT_ERROR ("can't alloc umad length %ld\n", length);
status = FINSUFFICIENT_MEMORY;
goto done;
}
retry:
mad_agent = umad_recv(port->umad_fd, umad, (int *)&length, timeout_ms);
// There are 4 combinations:
// assorted errors: mad_agent < 0, length <= MAD_SIZE
// large RMPP response: mad_agent < 0, length > MAD_SIZE, umad_status==0
// got response: mad_agent >= 0, length <= MAD_SIZE, umad_status==0
// no response: mad_agent >= 0, length <= MAD_SIZE, umad_status == error
if (mad_agent < 0) {
if (length <= *recv_size) {
// no MAD returned. None available.
DBGPRINT ("recv error on umad (size %zu) (%s)\n", *recv_size,
strerror(errno));
if (errno == EINTR)
goto retry;
status = (errno == ETIMEDOUT) ? FNOT_DONE:FERROR;
goto done;
} else {
// this routine is not expecting large responses
OUTPUT_ERROR ("Rx Packet size %zu larger than mad-size %zu\n",
length, *recv_size);
status = FOVERRUN;
if (recv_mad)
memcpy(recv_mad, umad_get_mad(umad), *recv_size);
// Clean out Rx packet 'cause it will never go away..
umad_free(umad);
umad = umad_alloc(1, umad_size() + length);
if (!umad) {
OUTPUT_ERROR ("can't alloc umad for rx cleanup, length %ld\n", length);
status = FINSUFFICIENT_MEMORY;
goto done;
}
// just to be safe, we supply a timeout. However it
// should be unnecessary since we know we have a packet
retry2:
if (umad_recv(port->umad_fd, umad, (int *)&length, OIB_UTILS_DEF_TIMEOUT_MS) < 0) {
OUTPUT_ERROR ("recv error on cleanup, length %ld (%s)\n", length,
strerror(errno));
if (errno == EINTR)
goto retry2;
goto done;
}
if (dbg_file) {
umad_dump(umad);
oib_dump_mad(dbg_file, umad_get_mad(umad), length, "rcv mad discarded\n");
}
goto done;
}
}
if (mad_agent >= UMAD_CA_MAX_AGENTS) {
OUTPUT_ERROR ("invalid mad agent %d\n", mad_agent);
status = FERROR;
goto done;
}
my_umad_status = umad_status(umad);
DBGPRINT("UMAD Status: %s (%d)\n", strerror(my_umad_status), my_umad_status);
if (my_umad_status != 0) {
status = (my_umad_status == ETIMEDOUT) ? FTIMEOUT : FREJECT;
}
DBGPRINT("Received MAD: Agent %d, length=%ld\n", mad_agent, length);
if (dbg_file) {
umad_dump(umad);
oib_dump_mad(dbg_file, umad_get_mad(umad), length, "rcv mad\n");
}
// Copy the data
if (recv_mad && length > 0) {
*recv_size = length;
memcpy(recv_mad, umad_get_mad(umad), length);
}
if (addr != NULL) {
addr->lid = IB2STL_LID(ntoh16(umad->addr.lid));
addr->sl = umad->addr.sl;
addr->qkey = ntoh32(umad->addr.qkey);
addr->qpn = ntoh32(umad->addr.qpn);
addr->pkey = oib_find_pkey_from_idx(port, umad_get_pkey(umad));
}
done:
if (umad != NULL) {
umad_free(umad);
}
return status;
}
