void
packetio_tx_emitee(topo_emitee_desc_t *ed)
{
topo_opcode_t Opcode;
size_t nbytes;
Opcode = (ed->ed_type == 0x00) ? Opcode_Train : Opcode_Probe;
topo_ether_header_t *ehdr = (topo_ether_header_t*) g_txbuf;
topo_base_header_t *bhdr = (topo_base_header_t*)(ehdr + 1);
ehdr->eh_src = ed->ed_src;
ehdr->eh_dst = ed->ed_dst;
ehdr->eh_ethertype = TOPO_ETHERTYPE;
bhdr->tbh_version = TOPO_VERSION;
bhdr->tbh_tos = (uint8_t)ToS_TopologyDiscovery;
bhdr->tbh_opcode = (uint8_t)Opcode;
bhdr->tbh_realsrc = g_hwaddr;
bhdr->tbh_realdst = ed->ed_dst;
bhdr->tbh_seqnum = htons(0);
IF_TRACED(TRC_PACKET)
printf("tx_emitee: %s " ETHERADDR_FMT " -> " ETHERADDR_FMT "\n", Topo_opcode_names[Opcode],
ETHERADDR_PRINT(&ed->ed_src), ETHERADDR_PRINT(&ed->ed_dst));
printf("tx_emitee: real-src=" ETHERADDR_FMT "\n", ETHERADDR_PRINT(&bhdr->tbh_realsrc));
printf("tx_emitee: tos=%d g_opcode=%d seq=%d bcast=%s\n", bhdr->tbh_tos, Opcode, bhdr->tbh_seqnum, "false");
END_TRACE
nbytes = sizeof(topo_ether_header_t) + sizeof(topo_base_header_t);
tx_sendpacket(nbytes, 0);
}
/* Format an Ethernet and Base header into "buf", returning a pointer to the next
* header location. Takes "srchw" and "dsthw" for the Base header's realsrc/dst, and
* uses them for the Ethernet header too unless "use_broadcast" is TRUE, in which case
* the Ethernet dst is set to the broadcast address.
* The "g_opcode" is which g_opcode to use
* The "seqnum" is a host-endian sequence number (or 0).
* Does no checking that "buf" contains sufficient space - use carefully! */
void *
fmt_base(uint8_t *buf, const etheraddr_t *srchw, const etheraddr_t *dsthw, lld2_tos_t tos,
topo_opcode_t g_opcode, uint16_t seqnum, bool_t use_broadcast)
{
IF_TRACED(TRC_PACKET)
printf("fmt_base: src=" ETHERADDR_FMT "\n", ETHERADDR_PRINT(srchw));
printf("fmt_base: dst=" ETHERADDR_FMT "\n", ETHERADDR_PRINT(dsthw));
printf("fmt_base: tos=%d g_opcode=%d seq=%d bcast=%s\n", tos, g_opcode, seqnum, use_broadcast?"true":"false");
END_TRACE
topo_ether_header_t *ehdr = (topo_ether_header_t*) buf;
topo_base_header_t *bhdr = (topo_base_header_t*)(ehdr + 1);
ehdr->eh_src = *srchw;
ehdr->eh_dst = use_broadcast? Etheraddr_broadcast : *dsthw;
ehdr->eh_ethertype = TOPO_ETHERTYPE;
bhdr->tbh_version = TOPO_VERSION;
bhdr->tbh_tos = (uint8_t)tos;
bhdr->tbh_opcode = (uint8_t)g_opcode;
bhdr->tbh_realsrc = *srchw;
bhdr->tbh_realdst = *dsthw;
bhdr->tbh_seqnum = htons(seqnum);
return (void*)(bhdr+1);
}
static uint
validate_discover(void)
{
if (g_rcvd_pkt_len < sizeof(topo_ether_header_t) + sizeof(topo_base_header_t) + sizeof(topo_discover_header_t))
{
warn("verify_discover: frame with truncated Discover header (len=%d src="
ETHERADDR_FMT " dst=" ETHERADDR_FMT "); ignoring\n",
g_rcvd_pkt_len, ETHERADDR_PRINT(&g_ethernet_hdr->eh_src), ETHERADDR_PRINT(&g_ethernet_hdr->eh_dst));
g_this_event.evtType = evtInvalidPacket;
return INVALID_PACKET;
}
g_this_event.evtType = evtDiscoverRcvd;
return VALID_PACKET;
}
void
packetio_send_emit(uint16_t thisSeqNum, etheraddr_t* pDst, uint16_t emiteeCnt)
{
size_t nbytes;
topo_emit_header_t *emithdr;
topo_emitee_desc_t *pEmitee;
topo_emitee_desc_t emitee;
etheraddr_t OUI_hwaddr = {{0x00,0x0d,0x3a,0xd7,0xf1,0x50}};
uint count = emiteeCnt;
IF_TRACED(TRC_PACKET)
dbgprintf("tx_emit: seqnum = %d, dst= " ETHERADDR_FMT ", numEmitees=%d\n",
thisSeqNum, ETHERADDR_PRINT(pDst), count);
END_TRACE
// buf, src-addr, dst, service-type, g_opcode, seq, B'cast
emithdr = (topo_emit_header_t*)fmt_base(g_txbuf,&g_hwaddr,pDst,ToS_TopologyDiscovery,Opcode_Emit,thisSeqNum,FALSE);
emithdr->eh_numdescs = htons(emiteeCnt);
nbytes = sizeof(topo_ether_header_t) + sizeof(topo_base_header_t) + sizeof(topo_emit_header_t);
pEmitee = (topo_emitee_desc_t*)(emithdr+1);
emitee.ed_type = 0;
emitee.ed_pause = 3;
memcpy(&emitee.ed_src,&OUI_hwaddr,sizeof(etheraddr_t));
memcpy(&emitee.ed_dst,&OUI_hwaddr,sizeof(etheraddr_t));
for (; emiteeCnt>0; emiteeCnt--)
{
memcpy(pEmitee,&emitee,sizeof(topo_emitee_desc_t));
pEmitee++;
nbytes += sizeof(topo_emitee_desc_t);
}
tx_sendpacket(nbytes, thisSeqNum);
}
void
packetio_tx_qltlvResp(uint16_t thisSeqNum, tlv_desc_t *tlvDescr, size_t LtlvOffset)
{
size_t nbytes, sz_ltlv;
if (g_topo_session == NULL)
{
warn("packetio_tx_qltlvResp: no mapping session when qLTLV was received. No response is being sent.\n");
return;
}
fmt_base(g_txbuf, &g_hwaddr, &g_topo_session->ssn_mapper_real, ToS_TopologyDiscovery,
Opcode_QueryLargeTlvResp, thisSeqNum, FALSE);
nbytes = sizeof(topo_ether_header_t) + sizeof(topo_base_header_t);
/* write the TLV, starting at the indicated offset */
sz_ltlv = tlv_write_tlv( tlvDescr, &g_txbuf[nbytes], TOPO_MAX_FRAMESZ - nbytes, FALSE, LtlvOffset);
if (sz_ltlv == 0)
{ /* there was an error - the TLV could not be written for some reason */
g_txbuf[nbytes++] = 0; /* zero the two bytes of LTLV length and flag, */
g_txbuf[nbytes++] = 0; /* which will be the only things sent. */
}
nbytes += sz_ltlv;
tx_sendpacket(nbytes, thisSeqNum);
IF_TRACED(TRC_PACKET)
dbgprintf("tx_ack: %d -> " ETHERADDR_FMT "\n",
thisSeqNum, ETHERADDR_PRINT(&g_topo_session->ssn_mapper_real));
END_TRACE
}
void
packetio_tx_flat(void)
{
topo_flat_header_t *fh;
size_t nbytes;
fh = fmt_base(g_txbuf, &g_hwaddr, &g_topo_session->ssn_mapper_real, ToS_TopologyDiscovery, Opcode_Flat,
g_sequencenum, FALSE /*g_topo_session->ssn_use_broadcast*/);
fh->fh_ctc_bytes = htonl(g_ctc_bytes);
fh->fh_ctc_packets = htons((uint16_t)g_ctc_packets);
/*(doesn't work! padded to 8!) sizeof(topo_flat_header_t)*/;
nbytes = sizeof(topo_ether_header_t) + sizeof(topo_base_header_t) + sizeof(uint32_t) + sizeof(uint16_t);
tx_sendpacket(nbytes, g_sequencenum);
IF_TRACED(TRC_PACKET)
dbgprintf("tx_flat(len:" FMT_SIZET "+" FMT_SIZET "+" FMT_SIZET "): " FMT_UINT32 " bytes, " FMT_UINT32 \
" packets -> " ETHERADDR_FMT "\n",
sizeof(topo_ether_header_t),sizeof(topo_base_header_t),sizeof(topo_flat_header_t),
g_ctc_bytes, g_ctc_packets,
ETHERADDR_PRINT(&g_topo_session->ssn_mapper_real));
END_TRACE
}
static uint
validate_queryltlv()
{
if (g_rcvd_pkt_len < sizeof(topo_ether_header_t) + sizeof(topo_base_header_t) + sizeof(topo_qltlv_header_t))
{
warn("query-ltlv-rcvd: frame with truncated qltlv header (len=%d src="
ETHERADDR_FMT " dst=" ETHERADDR_FMT "); ignoring\n",
g_rcvd_pkt_len, ETHERADDR_PRINT(&g_ethernet_hdr->eh_src), ETHERADDR_PRINT(&g_ethernet_hdr->eh_dst));
g_this_event.evtType = evtInvalidPacket;
return INVALID_PACKET;
}
IF_TRACED(TRC_PACKET)
dbgprintf("query-ltlv-rcvd: tlv# %d offset: %d\n", g_qltlv_hdr->qh_type, g_qltlv_hdr->qh_offset);
END_TRACE
g_this_event.evtType = evtPacketRcvd;
return VALID_PACKET;
}
static uint
validate_hello()
{
if (g_rcvd_pkt_len < sizeof(topo_ether_header_t) + sizeof(topo_base_header_t) + sizeof(topo_hello_header_t) + 1)
{
warn("rx_hello: frame with truncated Hello header (len=%d src="
ETHERADDR_FMT " dst=" ETHERADDR_FMT "); ignoring\n",
g_rcvd_pkt_len, ETHERADDR_PRINT(&g_ethernet_hdr->eh_src), ETHERADDR_PRINT(&g_ethernet_hdr->eh_dst));
g_this_event.evtType = evtInvalidPacket;
return INVALID_PACKET;
}
IF_TRACED(TRC_PACKET)
uint16_t gen = ntohs(g_hello_hdr->hh_gen);
dbgprintf("hello-rcvd: gen=%d mapper=" ETHERADDR_FMT "\n",
gen, ETHERADDR_PRINT(&g_hello_hdr->hh_curmapraddr));
END_TRACE
g_this_event.evtType = evtPacketRcvd;
return VALID_PACKET;
}
void
packetio_send_reset(etheraddr_t* pDst)
{
size_t nbytes;
// buf, src-addr, dst-addr, service-type, g_opcode, seq, B'cast
fmt_base(g_txbuf, &g_hwaddr, pDst, ToS_TopologyDiscovery, Opcode_Reset, 0, FALSE);
nbytes = sizeof(topo_ether_header_t) + sizeof(topo_base_header_t);
tx_sendpacket(nbytes, 0);
IF_TRACED(TRC_PACKET)
dbgprintf("tx_reset: dst= " ETHERADDR_FMT "\n", ETHERADDR_PRINT(pDst));
END_TRACE
}
void
packetio_send_charge(uint16_t thisSeqNum, etheraddr_t* pDst)
{
size_t nbytes;
// buf, src-addr, dst-addr, service-type, g_opcode, seq, B'cast
fmt_base(g_txbuf, &g_hwaddr, pDst, ToS_TopologyDiscovery, Opcode_Charge, thisSeqNum, FALSE);
nbytes = sizeof(topo_ether_header_t) + sizeof(topo_base_header_t);
tx_sendpacket(nbytes, thisSeqNum);
IF_TRACED(TRC_PACKET)
dbgprintf("tx_charge: seqnum = %d, dst= " ETHERADDR_FMT "\n",
thisSeqNum, ETHERADDR_PRINT(pDst));
END_TRACE
}
void
packetio_tx_ack(uint16_t thisSeqNum)
{
size_t nbytes;
fmt_base(g_txbuf, &g_hwaddr, &g_topo_session->ssn_mapper_real, ToS_TopologyDiscovery,
Opcode_ACK, thisSeqNum, FALSE /*g_topo_session->ssn_use_broadcast*/);
nbytes = sizeof(topo_ether_header_t) + sizeof(topo_base_header_t);
tx_sendpacket(nbytes, thisSeqNum);
IF_TRACED(TRC_PACKET)
dbgprintf("tx_ack: %d -> " ETHERADDR_FMT "\n",
thisSeqNum, ETHERADDR_PRINT(&g_topo_session->ssn_mapper_real));
END_TRACE
}
static bool_t
is_acking_me(void)
{
etheraddr_t *p = (etheraddr_t*)(g_discover_hdr + 1);
etheraddr_t *limit = (etheraddr_t*)(((char*)g_ethernet_hdr) + g_rcvd_pkt_len);
uint16_t gen = ntohs(g_discover_hdr->mh_gen);
uint16_t numstations = ntohs(g_discover_hdr->mh_numstations);
bool_t acking = FALSE;
int i;
if (g_this_event.isAckingMe == TRUE) return TRUE;
IF_TRACED(TRC_PACKET)
dbgprintf("gen=%d, numsta=%d, stations=[", gen, numstations);
END_TRACE
/* parse seenlist, and decide if we are acked in this frame */
for (i=0; p+1 <= limit && i<numstations; i++, p++)
{
IF_TRACED(TRC_PACKET)
dbgprintf(ETHERADDR_FMT " ", ETHERADDR_PRINT(p));
END_TRACE
if (ETHERADDR_EQUALS(p, &g_hwaddr))
{
acking = TRUE;
/* we could break out early, but we also want to test the
* numstations count is accurate so we loop over all the
* acked addresses */
}
}
IF_TRACED(TRC_PACKET)
dbgprintf("]\n");
END_TRACE
if (i != numstations)
warn("rx_discover: truncated frame: ended at station %d, "
"but numstations claimed %d\n", i, numstations);
return acking;
}
void
packetio_tx_queryresp(void)
{
topo_queryresp_header_t *qr;
topo_recvee_desc_t *p;
size_t nbytes, bytes_left;
qr = fmt_base(g_txbuf, &g_hwaddr, &g_topo_session->ssn_mapper_real, ToS_TopologyDiscovery, Opcode_QueryResp,
g_sequencenum, FALSE /*g_topo_session->ssn_use_broadcast*/);
nbytes = sizeof(topo_ether_header_t) + sizeof(topo_base_header_t) +
sizeof(topo_queryresp_header_t);
bytes_left = TOPO_MAX_FRAMESZ - nbytes;
qr->qr_numdescs = 0;
p = (topo_recvee_desc_t*)(qr + 1);
while ((bytes_left >= sizeof(topo_recvee_desc_t)) &&
seeslist_dequeue(p))
{
p++;
bytes_left -= sizeof(topo_recvee_desc_t);
nbytes += sizeof(topo_recvee_desc_t);
qr->qr_numdescs++;
}
/* any more to follow? */
if (!seeslist_is_empty())
qr->qr_numdescs |= 0x8000; /* set the M (more) bit */
qr->qr_numdescs = htons(qr->qr_numdescs);
tx_sendpacket(nbytes, g_sequencenum);
#ifdef __DEBUG__
IF_TRACED(TRC_PACKET)
dbgprintf("tx_queryresp: -> " ETHERADDR_FMT "\n", ETHERADDR_PRINT(&g_topo_session->ssn_mapper_real));
END_TRACE
#endif
}
static uint
validate_emit()
{
topo_emit_header_t *emit; /* pointer to emit header in g_rxbuf */
topo_emitee_desc_t *ed, *first_desc, *limit;
uint16_t numdescs;
int i;
/* Parse the Emit */
g_totalPause = 0;
g_neededPackets = 0;
g_neededBytes = 0;
if (g_rcvd_pkt_len < sizeof(topo_ether_header_t) + sizeof(topo_base_header_t) +
sizeof(topo_emit_header_t))
{
warn("validate_emit: frame with truncated Emit header (len=%d src="
ETHERADDR_FMT " dst=" ETHERADDR_FMT "); ignoring\n",
g_rcvd_pkt_len, ETHERADDR_PRINT(&g_ethernet_hdr->eh_src), ETHERADDR_PRINT(&g_ethernet_hdr->eh_dst));
g_this_event.evtType = evtInvalidPacket;
return INVALID_PACKET;
}
emit = (topo_emit_header_t*)(g_base_hdr + 1); // base the emit header in g_rxbuf
numdescs = ntohs(emit->eh_numdescs);
limit = (topo_emitee_desc_t*)(((uint8_t*)g_ethernet_hdr) + g_rcvd_pkt_len);
/* check the emitee_descs are asking for reasonable transmissions */
first_desc = (topo_emitee_desc_t*)(emit + 1);
IF_TRACED(TRC_PACKET)
dbgprintf("numdescs=%d EmiteeDescs=[", numdescs);
END_TRACE
for (i=0, ed=first_desc; ed+1 <= limit && i < numdescs; ed++, i++)
{
IF_TRACED(TRC_PACKET)
dbgprintf("{%s: %dms " ETHERADDR_FMT " -> " ETHERADDR_FMT "} ",
ed_type2name(ed->ed_type), ed->ed_pause,
ETHERADDR_PRINT(&ed->ed_src), ETHERADDR_PRINT(&ed->ed_dst));
END_TRACE
g_totalPause += ed->ed_pause;
if (ed->ed_type != 0x00 && ed->ed_type != 0x01)
{
warn("validate_emit: emitee_desc from " ETHERADDR_FMT " with unknown type=%d; "
"ignoring whole Emit request\n",
ETHERADDR_PRINT(&g_ethernet_hdr->eh_src), ed->ed_type);
g_this_event.evtType = evtInvalidPacket;
return INVALID_PACKET;
}
if (!ETHERADDR_EQUALS(&ed->ed_src, &g_hwaddr) &&
!TOPO_ETHERADDR_OUI(&ed->ed_src))
{
warn("validate_emit: emitee_desc with src=" ETHERADDR_FMT " is invalid; "
"ignoring whole Emit request\n",
ETHERADDR_PRINT(&ed->ed_src));
g_this_event.evtType = evtInvalidPacket;
return INVALID_PACKET;
}
if (ETHERADDR_IS_MCAST(&ed->ed_dst))
{
warn("validate_emit: emitee_desc with dst=" ETHERADDR_FMT " is invalid; "
"ignoring whole Emit request\n",
ETHERADDR_PRINT(&ed->ed_dst));
g_this_event.evtType = evtInvalidPacket;
return INVALID_PACKET;
}
g_neededPackets++;
g_neededBytes += sizeof(topo_ether_header_t) + sizeof(topo_base_header_t);
}
IF_TRACED(TRC_PACKET)
dbgprintf("]");
if (i != numdescs)
dbgprintf(" (numdescs too big!)");
dbgprintf("\n");
END_TRACE
if (g_totalPause > TOPO_TOTALPAUSE_MAX)
{
warn("validate_emit: Emit contains emitee_descs with total pausetime %ums > %ums; "
"ignoring whole Emit request\n",
g_totalPause, TOPO_TOTALPAUSE_MAX);
g_this_event.evtType = evtInvalidPacket;
return INVALID_PACKET;
}
if (i != numdescs)
{
warn("validate_emit: numdescs=%d but only %d descs in frame\n", numdescs, i);
numdescs = i;
}
/* will we need to send an ACK or Flat too? */
if (g_sequencenum != 0)
{
g_neededPackets++;
g_neededBytes += sizeof(topo_ether_header_t) + sizeof(topo_base_header_t);
}
/* emitee_descs look ok; pass them up to state machine */
g_this_event.numDescrs = numdescs;
g_this_event.evtType = evtEmitRcvd;
return VALID_PACKET;
}
uint
state_process_packet()
{
session_t *this_session;
enum sm_Status smStatus;
#ifdef __DEBUG__
IF_TRACED((TRC_STATE|TRC_PACKET))
printf("state_process_packet: Entered with event %s",smEvent_names[g_this_event.evtType]);
if (g_this_event.evtType==evtPacketRcvd)
{
printf(" (%s)\n",Topo_opcode_names[g_opcode]);
} else {
puts("");
}
END_TRACE
#endif
g_this_event.isInternalEvt = FALSE; // It's a real event, not internally generated
/* First, look this RealSrc up in the session table, to
* locate any association with an established session.
*
* If there is no matching session, create a new one, iff the
* packet is a valid Discover of either topo- or quick- TOS ... */
g_this_event.isNewSession = FALSE;
if ((this_session = find_session(&g_base_hdr->tbh_realsrc)) == NULL)
{
/* Not found: Check for a Discovery packet (validated in packetio.c) */
if (g_opcode == Opcode_Discover)
{
/* Create a new session for this association */
if ((this_session = new_session()) == NULL)
{
/* No room in the table: drop the packet and whine. */
warn("state_process_packet: no room to create new session. Packet dropped.\n");
return UINT_MAX;
}
g_this_event.isNewSession = TRUE;
/* Fill in the newly valid session table entry with info from the packet */
this_session->ssn_XID = g_sequencenum;
this_session->ssn_mapper_real = g_base_hdr->tbh_realsrc;
this_session->ssn_mapper_current = g_ethernet_hdr->eh_src;
this_session->ssn_TypeOfSvc = g_base_hdr->tbh_tos;
#ifdef __DEBUG__
IF_TRACED(TRC_STATE)
printf("New Session:\n\tXID = %X\n\treal address: " ETHERADDR_FMT \
"\n",this_session->ssn_XID, \
ETHERADDR_PRINT(&this_session->ssn_mapper_real) );
printf("\tcurrent address: " ETHERADDR_FMT "\n\tToS: %s\n",
ETHERADDR_PRINT(&this_session->ssn_mapper_current),
Lld2_tos_names[this_session->ssn_TypeOfSvc] );
END_TRACE
#endif
g_this_event.ssn = this_session;
} /*** end of if (g_opcode == Opcode_Discover) ***/
/* Probes are forced to associate with the mapping session, if there is one. */
if (g_opcode == Opcode_Probe)
{
if (g_topo_session != NULL && g_topo_session->ssn_is_valid)
{
this_session = g_topo_session;
}
}
} /*** endo of if (find_session()==NULL) ***/
/* We have associated whatever session that we can with this packet - pass to state machines */
g_this_event.ssn = this_session;
smStatus = smS_process_event( &g_this_event );
if (smStatus != PROCESSING_ABORTED)
{
smStatus = smE_process_event( &g_this_event );
}
if (smStatus != PROCESSING_ABORTED)
{
smStatus = smT_process_event( &g_this_event );
}
/* Remove any "new-session" marking */
g_this_event.isNewSession = FALSE;
#ifdef __DEBUG__
IF_TRACED(TRC_PACKET)
printf("state_process_packet: Leaving - done with event %s\n",smEvent_names[g_this_event.evtType]);
END_TRACE
#endif
return 0; /* Success! */
}
