/*
* Handle a packet. Returns "false" if we encounter a connection-fatal error.
*/
static bool handlePacket(Peer* pDst, Peer* pSrc)
{
const unsigned char* buf = pSrc->inputBuffer;
u4 length;
u1 flags;
int cc;
length = get4BE(buf+0);
flags = get1(buf+9);
assert((int) length <= pSrc->inputCount);
dumpPacket(buf, pSrc->label, pDst->label);
cc = write(pDst->sock, buf, length);
if (cc != (int) length) {
fprintf(stderr, "Failed sending packet: %s\n", strerror(errno));
return false;
}
/*printf("*** wrote %d bytes from %c to %c\n",
cc, pSrc->label[0], pDst->label[0]);*/
consumeBytes(pSrc, length);
return true;
}
AdpErrs DevSW_Write(const DeviceDescr *device, Packet *packet, DevChanID type)
{
struct DriverCall *dc;
struct data_packet *dp;
dc = &((DevSWState *)(device->SwitcherState))->ds_activewrite;
dp = &dc->dc_packet;
if (illegalDevChanID(type))
return adp_illegal_args;
/*
* try to flush any packet that is still being written
*/
if (DevSW_FlushPendingWrite(device) != adp_ok)
return adp_write_busy;
/*
* we can take this packet - set things up, then try to get rid of it
*/
initialise_write(dc, packet, type);
if (angelDebugLogEnable)
dumpPacket(angelDebugLogFile,"tx:",&dc->dc_packet);
flush_packet(device, dc);
return adp_ok;
}
int main(int argc, char **argv) {
if(argc < 3) {
printf("Usage: packet_gen <number of packets overall> <number of senders>\n");
return 0;
}
int numPackets = atoi(argv[1]);
int numSatellites = atoi(argv[2]);
qb_tc_packet *p = malloc(sizeof(qb_tc_packet));
p->primary_header = malloc(sizeof(qb_tc_packet_header));
p->primary_header->version = 0;
p->primary_header->type = 1;
p->primary_header->sh_present = 0;
p->primary_header->apid = 2047; //all 11 bit are set to 1.
p->primary_header->seq_flags = PACKET_SEQ_STANDALONE;
p->primary_header->seq_count = 16383; //all 14 bit are set to1.
p->primary_header->length = 13;
p->data = "Hello world!";
dumpPacket(p);
return 0;
}
/***********************************************************************
*%FUNCTION: sendPADI
*%ARGUMENTS:
* conn -- PPPoEConnection structure
*%RETURNS:
* Nothing
*%DESCRIPTION:
* Sends a PADI packet
***********************************************************************/
void
sendPADI(PPPoEConnection *conn)
{
PPPoEPacket packet;
unsigned char *cursor = packet.payload;
PPPoETag *svc = (PPPoETag *) (&packet.payload);
UINT16_t namelen = 0;
UINT16_t plen;
if (conn->serviceName) {
namelen = (UINT16_t) strlen(conn->serviceName);
}
plen = TAG_HDR_SIZE + namelen;
CHECK_ROOM(cursor, packet.payload, plen);
/* Set destination to Ethernet broadcast address */
memset(packet.ethHdr.h_dest, 0xFF, ETH_ALEN);
memcpy(packet.ethHdr.h_source, conn->myEth, ETH_ALEN);
packet.ethHdr.h_proto = htons(Eth_PPPOE_Discovery);
packet.ver = 1;
packet.type = 1;
packet.code = CODE_PADI;
packet.session = 0;
svc->type = TAG_SERVICE_NAME;
svc->length = htons(namelen);
CHECK_ROOM(cursor, packet.payload, namelen+TAG_HDR_SIZE);
if (conn->serviceName) {
memcpy(svc->payload, conn->serviceName, strlen(conn->serviceName));
}
cursor += namelen + TAG_HDR_SIZE;
/* If we're using Host-Uniq, copy it over */
if (conn->useHostUniq) {
PPPoETag hostUniq;
pid_t pid = getpid();
hostUniq.type = htons(TAG_HOST_UNIQ);
hostUniq.length = htons(sizeof(pid));
memcpy(hostUniq.payload, &pid, sizeof(pid));
CHECK_ROOM(cursor, packet.payload, sizeof(pid) + TAG_HDR_SIZE);
memcpy(cursor, &hostUniq, sizeof(pid) + TAG_HDR_SIZE);
cursor += sizeof(pid) + TAG_HDR_SIZE;
plen += sizeof(pid) + TAG_HDR_SIZE;
}
packet.length = htons(plen);
sendPacket(conn, conn->discoverySocket, &packet, (int) (plen + HDR_SIZE));
if (conn->debugFile) {
dumpPacket(conn->debugFile, &packet, "SENT");
fprintf(conn->debugFile, "\n");
fflush(conn->debugFile);
}
}
void Proxy::read(unsigned size, unsigned minsize)
{
bIn.init(size);
int readn = sock->read(bIn.Data(0), size);
if ((readn != (int)size) || (minsize && (readn < (int)minsize))){
if (notify) notify->error_state(ErrorProxyConnect);
return;
}
dumpPacket(bIn, 0, "Proxy read");
}
/*
This is called when a configuration is loaded, and updates the plugin's settings.
Careful: the plugin is already drawn before the loadConfiguration method is called the
first time, so you have to be careful not to assume all the plugin values are initialized
the first time you use them
*/
void AntennaTrackGadget::loadConfiguration(IUAVGadgetConfiguration *config)
{
// Delete the (old)port, this also closes it.
if (port) {
delete port;
}
// Delete the (old)parser, this also disconnects all signals.
if (parser) {
delete parser;
}
AntennaTrackGadgetConfiguration *AntennaTrackConfig = qobject_cast< AntennaTrackGadgetConfiguration *>(config);
PortSettings portsettings;
portsettings.BaudRate = AntennaTrackConfig->speed();
portsettings.DataBits = AntennaTrackConfig->dataBits();
portsettings.FlowControl = AntennaTrackConfig->flow();
portsettings.Parity = AntennaTrackConfig->parity();
portsettings.StopBits = AntennaTrackConfig->stopBits();
portsettings.Timeout_Millisec = AntennaTrackConfig->timeOut();
// In case we find no port, buttons disabled
m_widget->connectButton->setEnabled(false);
m_widget->disconnectButton->setEnabled(false);
QList<QextPortInfo> ports = QextSerialEnumerator::getPorts();
foreach(QextPortInfo nport, ports) {
if (nport.friendName == AntennaTrackConfig->port()) {
qDebug() << "Using Serial port";
// parser = new NMEAParser();
#ifdef Q_OS_WIN
port = new QextSerialPort(nport.portName, portsettings, QextSerialPort::EventDriven);
#else
port = new QextSerialPort(nport.physName, portsettings, QextSerialPort::EventDriven);
#endif
m_widget->setPort(port);
m_widget->connectButton->setEnabled(true);
m_widget->disconnectButton->setEnabled(false);
m_widget->connectButton->setHidden(false);
m_widget->disconnectButton->setHidden(false);
connect(port, SIGNAL(readyRead()), this, SLOT(onDataAvailable()));
}
}
m_widget->dataStreamGroupBox->setHidden(false);
qDebug() << "Using Telemetry parser";
parser = new TelemetryParser();
connect(parser, SIGNAL(position(double, double, double)), m_widget, SLOT(setPosition(double, double, double)));
connect(parser, SIGNAL(home(double, double, double)), m_widget, SLOT(setHomePosition(double, double, double)));
connect(parser, SIGNAL(packet(QString)), m_widget, SLOT(dumpPacket(QString)));
}
void ICQClient::sendPacket()
{
char *packet = writeBuffer.Data(m_nPacketStart);
*((unsigned short*)(packet + 4)) = htons(writeBuffer.size() - m_nPacketStart - 6);
dumpPacket(writeBuffer, m_nPacketStart, "Write");
time(&m_lastTime);
if ((m_fd == -1) || (m_connecting != Connected)) return;
fd_set wf;
FD_ZERO(&wf);
FD_SET(m_fd, &wf);
struct timeval tv;
tv.tv_sec = 0;
tv.tv_usec = 0;
int res = select(m_fd + 1, NULL, &wf, NULL, &tv);
if (res <= 0) return;
if (!FD_ISSET(m_fd, &wf)) return;
write_ready();
}
void HttpRequest::read_ready()
{
if (state == Connected){
string s;
if (!readLine(s)) return;
if (s.length() < strlen(HTTP)){
error_state(ErrorRead);
return;
}
const char *r = strchr(s.c_str(), ' ');
if (r == NULL){
error_state(ErrorRead);
return;
}
r++;
int code = atoi(r);
if (code == 401){
dumpPacket(bIn, 0, "Proxy read");
error_state(ErrorProxyAuth);
return;
}
if (code == 502){
dumpPacket(bIn, 0, "Proxy read");
error_state(ErrorRead);
return;
}
if (code != 200){
dumpPacket(bIn, 0, "Proxy read");
error_state(ErrorProxyConnect);
return;
}
state = ReadHeader;
}
if (state == ReadHeader){
for (;;){
string s;
if (!readLine(s)) return;
if (s.length() == 0){
state = ReadData;
break;
}
string h = s.substr(0, strlen(CONTENT_LENGTH));
if (!strcasecmp(h.c_str(), CONTENT_LENGTH)){
h = s.substr(strlen(CONTENT_LENGTH), s.size());
for (const char *p = h.c_str(); *p; p++){
if ((*p >= '0') && (*p <= '9')){
data_size = atol(p);
break;
}
}
}
}
}
if (state == ReadData){
while (data_size > 0){
char b[2048];
unsigned tail = data_size;
if (tail > sizeof(b)) tail = sizeof(b);
int n = m_sock->read(b, tail);
if (n < 0){
dumpPacket(bIn, 0, "Proxy read");
error_state(ErrorProxyConnect);
return;
}
if (n == 0) break;
bIn.pack(b, n);
data_size -= n;
}
if (data_size == 0){
state = None;
dumpPacket(bIn, 0, "Proxy read");
data_ready();
}
}
}
void HttpRequest::connect_ready()
{
if (state == WaitConnect){
state = Connected;
#ifdef USE_OPENSSL
bool isSecure = m_proxy->isSecure();
if (isSecure){
SSLClient *ssl = new SSLClient(m_sock);
if (!ssl->initHTTPS()){
delete ssl;
ssl = NULL;
error_state(ErrorProxyConnect);
return;
}
ssl->setNotify(this);
ssl->connect();
ssl->process();
m_sock = ssl;
return;
}
#endif
}
const char *h = host();
HttpPacket *p = packet();
Buffer bOut;
bOut << (p ? "POST" : "GET") << " http://" << h << uri() << " HTTP/1.1\r\n" <<
"Host: " << h << "\r\n"
"User-agent: Mozilla/4.08 [en] (WinNT; U ;Nav)\r\n"
"Cache-control: no-store, no-cache\r\n"
"Connection: close\r\n"
"Pragma: no-cache\r\n";
if (p){
char b[15];
snprintf(b, sizeof(b), "%u", p->size + 14);
bOut << "Content-Length: " << b << "\r\n";
}
if (m_proxy->m_user.length()){
string s;
s = m_proxy->m_user.c_str();
s += ":";
s += m_proxy->m_passwd.c_str();
s = tobase64(s.c_str());
bOut << "Proxy-Auth: basic ";
bOut << s.c_str();
bOut << "\r\n";
}
bOut << "\r\n";
if (p){
unsigned short len = p->size + 12;
bOut
<< len
<< HTTP_PROXY_VERSION
<< p->type
<< 0x00000000L
<< p->nSock;
if (p->size)
bOut.pack(p->data, p->size);
m_proxy->queue.remove(p);
delete p;
}
dumpPacket(bOut, 0, "Proxy write");
m_sock->write(bOut.Data(0), bOut.size());
bOut.init(0);
}
/**********************************************************************
*%FUNCTION: waitForPADO
*%ARGUMENTS:
* conn -- PPPoEConnection structure
* timeout -- how long to wait (in seconds)
*%RETURNS:
* Nothing
*%DESCRIPTION:
* Waits for a PADO packet and copies useful information
***********************************************************************/
static void
waitForPADO(PPPoEConnection *conn, int timeout)
{
fd_set readable;
int r;
struct timeval tv;
struct timeval expire_at;
struct timeval now;
PPPoEPacket packet;
int len;
struct PacketCriteria pc;
pc.conn = conn;
#ifdef PLUGIN
conn->seenMaxPayload = 0;
#endif
if (gettimeofday(&expire_at, NULL) < 0) {
fatalSys("gettimeofday (waitForPADO)");
}
expire_at.tv_sec += timeout;
do {
#ifdef PLUGIN
if (got_sigterm || got_sighup) return;
#endif
if (BPF_BUFFER_IS_EMPTY) {
if (gettimeofday(&now, NULL) < 0) {
fatalSys("gettimeofday (waitForPADO)");
}
tv.tv_sec = expire_at.tv_sec - now.tv_sec;
tv.tv_usec = expire_at.tv_usec - now.tv_usec;
if (tv.tv_usec < 0) {
tv.tv_usec += 1000000;
if (tv.tv_sec) {
tv.tv_sec--;
} else {
/* Timed out */
return;
}
}
if (tv.tv_sec <= 0 && tv.tv_usec <= 0) {
/* Timed out */
return;
}
FD_ZERO(&readable);
FD_SET(conn->discoverySocket, &readable);
while(1) {
r = select(conn->discoverySocket+1, &readable, NULL, NULL, &tv);
if (r >= 0 || errno != EINTR) break;
#ifdef PLUGIN
if (got_sigterm || got_sighup) return;
#endif
}
if (r < 0) {
fatalSys("select (waitForPADO)");
}
if (r == 0) {
/* Timed out */
return;
}
}
/* Get the packet */
receivePacket(conn->discoverySocket, &packet, &len);
/* Check length */
if (ntohs(packet.length) + HDR_SIZE > len) {
syslog(LOG_ERR, "Bogus PPPoE length field (%u)",
(unsigned int) ntohs(packet.length));
continue;
}
#ifdef USE_BPF
/* If it's not a Discovery packet, loop again */
if (etherType(&packet) != Eth_PPPOE_Discovery) continue;
#endif
#ifdef DEBUGGING_ENABLED
if (conn->debugFile) {
dumpPacket(conn->debugFile, &packet, "RCVD");
fprintf(conn->debugFile, "\n");
fflush(conn->debugFile);
}
#endif
/* If it's not for us, loop again */
if (!packetIsForMe(conn, &packet)) continue;
if (packet.code == CODE_PADO) {
if (BROADCAST(packet.ethHdr.h_source)) {
printErr("Ignoring PADO packet from broadcast MAC address");
continue;
}
#ifdef PLUGIN
if (conn->req_peer
&& memcmp(packet.ethHdr.h_source, conn->req_peer_mac, ETH_ALEN) != 0) {
warn("Ignoring PADO packet from wrong MAC address");
continue;
}
#endif
pc.gotError = 0;
pc.seenACName = 0;
pc.seenServiceName = 0;
pc.acNameOK = (conn->acName) ? 0 : 1;
pc.serviceNameOK = (conn->serviceName) ? 0 : 1;
parsePacket(&packet, parsePADOTags, &pc);
if (pc.gotError) {
printErr("Error in PADO packet");
continue;
}
if (!pc.seenACName) {
printErr("Ignoring PADO packet with no AC-Name tag");
continue;
}
if (!pc.seenServiceName) {
printErr("Ignoring PADO packet with no Service-Name tag");
continue;
}
conn->numPADOs++;
if (pc.acNameOK && pc.serviceNameOK) {
memcpy(conn->peerEth, packet.ethHdr.h_source, ETH_ALEN);
if (conn->printACNames) {
printf("AC-Ethernet-Address: %02x:%02x:%02x:%02x:%02x:%02x\n",
(unsigned) conn->peerEth[0],
(unsigned) conn->peerEth[1],
(unsigned) conn->peerEth[2],
(unsigned) conn->peerEth[3],
(unsigned) conn->peerEth[4],
(unsigned) conn->peerEth[5]);
printf("--------------------------------------------------\n");
continue;
}
conn->discoveryState = STATE_RECEIVED_PADO;
break;
}
}
} while (conn->discoveryState != STATE_RECEIVED_PADO);
}
/***********************************************************************
*%FUNCTION: sendPADR
*%ARGUMENTS:
* conn -- PPPoE connection structur
*%RETURNS:
* Nothing
*%DESCRIPTION:
* Sends a PADR packet
***********************************************************************/
void
sendPADR(PPPoEConnection *conn)
{
PPPoEPacket packet;
PPPoETag *svc = (PPPoETag *) packet.payload;
unsigned char *cursor = packet.payload;
UINT16_t namelen = 0;
UINT16_t plen;
if (conn->serviceName) {
namelen = (UINT16_t) strlen(conn->serviceName);
}
plen = TAG_HDR_SIZE + namelen;
CHECK_ROOM(cursor, packet.payload, plen);
memcpy(packet.ethHdr.h_dest, conn->peerEth, ETH_ALEN);
memcpy(packet.ethHdr.h_source, conn->myEth, ETH_ALEN);
packet.ethHdr.h_proto = htons(Eth_PPPOE_Discovery);
packet.ver = 1;
packet.type = 1;
packet.code = CODE_PADR;
packet.session = 0;
svc->type = TAG_SERVICE_NAME;
svc->length = htons(namelen);
if (conn->serviceName) {
memcpy(svc->payload, conn->serviceName, namelen);
}
cursor += namelen + TAG_HDR_SIZE;
/* If we're using Host-Uniq, copy it over */
if (conn->useHostUniq) {
PPPoETag hostUniq;
pid_t pid = getpid();
hostUniq.type = htons(TAG_HOST_UNIQ);
hostUniq.length = htons(sizeof(pid));
memcpy(hostUniq.payload, &pid, sizeof(pid));
CHECK_ROOM(cursor, packet.payload, sizeof(pid)+TAG_HDR_SIZE);
memcpy(cursor, &hostUniq, sizeof(pid) + TAG_HDR_SIZE);
cursor += sizeof(pid) + TAG_HDR_SIZE;
plen += sizeof(pid) + TAG_HDR_SIZE;
}
/* Copy cookie and relay-ID if needed */
if (conn->cookie.type) {
CHECK_ROOM(cursor, packet.payload,
ntohs(conn->cookie.length) + TAG_HDR_SIZE);
memcpy(cursor, &conn->cookie, ntohs(conn->cookie.length) + TAG_HDR_SIZE);
cursor += ntohs(conn->cookie.length) + TAG_HDR_SIZE;
plen += ntohs(conn->cookie.length) + TAG_HDR_SIZE;
}
if (conn->relayId.type) {
CHECK_ROOM(cursor, packet.payload,
ntohs(conn->relayId.length) + TAG_HDR_SIZE);
memcpy(cursor, &conn->relayId, ntohs(conn->relayId.length) + TAG_HDR_SIZE);
cursor += ntohs(conn->relayId.length) + TAG_HDR_SIZE;
plen += ntohs(conn->relayId.length) + TAG_HDR_SIZE;
}
packet.length = htons(plen);
sendPacket(conn, conn->discoverySocket, &packet, (int) (plen + HDR_SIZE));
if (conn->debugFile) {
dumpPacket(conn->debugFile, &packet, "SENT");
fprintf(conn->debugFile, "\n");
fflush(conn->debugFile);
}
}
void ICQClient::packet_ready()
{
time(&m_lastTime);
if (m_bHeader){
char c;
readBuffer >> c;
if (c != 0x2A){
log(L_ERROR, "Server send bad packet start code: %02X", c);
error(ErrorProtocol);
return;
}
readBuffer >> m_nChannel;
unsigned short sequence, size;
readBuffer >> sequence >> size;
if (size){
readBuffer.add(size);
m_bHeader = false;
return;
}
}
dumpPacket(readBuffer, 0,"Read");
switch (m_nChannel){
case ICQ_CHNxNEW:
chn_login();
break;
case ICQ_CHNxCLOSE:
chn_close();
break;
case ICQ_CHNxDATA:{
unsigned short fam, type;
unsigned short flags, seq, cmd;
readBuffer >> fam >> type >> flags >> seq >> cmd;
switch (fam){
case ICQ_SNACxFAM_SERVICE:
snac_service(type, seq);
break;
case ICQ_SNACxFAM_LOCATION:
snac_location(type, seq);
break;
case ICQ_SNACxFAM_BUDDY:
snac_buddy(type, seq);
break;
case ICQ_SNACxFAM_MESSAGE:
snac_message(type, seq);
break;
case ICQ_SNACxFAM_BOS:
snac_bos(type, seq);
break;
case ICQ_SNACxFAM_PING:
snac_ping(type, seq);
break;
case ICQ_SNACxFAM_LISTS:
snac_lists(type, seq);
break;
case ICQ_SNACxFAM_VARIOUS:
snac_various(type, seq);
break;
case ICQ_SNACxFAM_LOGIN:
snac_login(type, seq);
break;
default:
log(L_WARN, "Unknown family %02X", fam);
}
break;
}
default:
log(L_ERROR, "Unknown channel %u", m_nChannel & 0xFF);
}
processInfoRequestQueue();
processPhoneRequestQueue(0);
processResponseRequestQueue(0);
readBuffer.init(6);
m_bHeader = true;
}
void Proxy::write()
{
dumpPacket(bOut, 0, "Proxy write");
sock->write(bOut.Data(0), bOut.size());
bOut.init(0);
}
AdpErrs DevSW_Read(const DeviceDescr *device, const DevChanID type,
Packet **packet, bool block)
{
int read_err;
DevSWState *ds = device->SwitcherState;
/*
* To try to get information out of the device driver as
* quickly as possible, we try and read more packets, even
* if a completed packet is already available.
*/
/*
* have we got a packet currently pending?
*/
if (ds->ds_nextreadpacket == NULL)
/*
* no - set things up
*/
if (initialise_read(ds) < 0) {
/*
* we failed to initialise the next packet, but can
* still return a packet that has already arrived.
*/
*packet = Adp_removeFromQueue(&ds->ds_readqueue[type]);
return adp_ok;
}
read_err = device->DeviceRead(&ds->ds_activeread, block);
switch (read_err) {
case 1:
/*
* driver has pulled in a complete packet, queue it up
*/
#ifdef RET_DEBUG
printf("got a complete packet\n");
#endif
if (angelDebugLogEnable)
dumpPacket(angelDebugLogFile,"rx:",&ds->ds_activeread.dc_packet);
enqueue_packet(ds);
*packet = Adp_removeFromQueue(&ds->ds_readqueue[type]);
return adp_ok;
case 0:
/*
* OK, return the head of the read queue for the given type
*/
/* enqueue_packet(ds); */
*packet = Adp_removeFromQueue(&ds->ds_readqueue[type]);
return adp_ok;
case -1:
#ifdef RET_DEBUG
printf("got a bad packet\n");
#endif
/* bad packet */
*packet = NULL;
return adp_bad_packet;
default:
panic("DevSW_Read: bad read status %d", read_err);
}
return 0; /* get rid of a potential compiler warning */
}
void ReadInterfaceNamesBulk()
{
BOOL Done=FALSE;
char *OIDString;
OidVarbind* oid;
VbPair* vbp;
const char *RawRetP=NULL;
Packet* retP;
DWORD Retries;
DWORD x; /* Counter */
Packet p(GETBULK, 1); // create an SNMP v2c GETBULK command
int vbLen;
int vbCounter;
int len;
unsigned int IntNumber;
/***************/
/* Set up OIDs */
/***************/
OIDString = (char *)malloc(1000);
sprintf(OIDString, "1.3.6.1.2.1.2.2.1.2");
/*************************/
/* Create GETBULK packet */
/*************************/
p.Community(Community); // set the community string
p.RequestId(123); // use device number to track the request
p.ErrorStatus(0); // set non-repeaters to zero
p.ErrorIndex(5); // set number of max-repititions
oid = new OidVarbind(OIDString); // OID for first MAC address
vbp = new VbPair(oid, NULL); // Create vbpairs
p.Add(vbp); // Add vbpair to the packet
UdpClient udp(161, Agent); // Set to port 161 and IP address (or host name)
udp.Timeout(5); // Set timeout to PollDeviceTimeout
udp.ReadBufferSize(65535); // set buffer size to be large enough to handle response
/*******************/
/* Collection loop */
/*******************/
IntNumber=0;
Done = FALSE;
while (!Done)
{
Retries=0;
do
{
dumpPacket(&p);
udp.Send(&p); // Send the packet
Retries++;
RawRetP = udp.Receive(); // Receive the reply
} while ((!RawRetP) && (Retries < 3));
/*************************************/
/* If the packet is valid, save info */
/*************************************/
if ( RawRetP )
{
// Determine length and parse packet
len = udp.ReadLength();
SnmpParser parser((unsigned char*)RawRetP, len);
retP = parser.packet();
if (retP != NULL)
{
// Packet was parsed successfully
if ( retP->RequestId() == 123 )
{
// RequestID matches
if ( retP->ErrorStatus() == NOERROR )
{
// No SNMP error found, learn how many OIDs were returned
vbLen = retP->VbListLength();
// Loop through for each returned OID
for (vbCounter=1 ; vbCounter <= vbLen ; vbCounter++)
{
sprintf(OIDString, "%s", retP->VbOID(vbCounter));
if (strncmp(OIDString, "1.3.6.1.2.1.2.2.1.2.", 20) == 0)
{
// OID is still correct, print the results
printf("%s: %s\n", retP->VbOID(vbCounter), retP->VbData(vbCounter));
}
else
{
// OID does not match, we're done
Done=TRUE;
break;
}
} // Loop through all OIDs
// Set up next packet
delete oid;
printf("assigning new oid of <%s>\r\n", OIDString);
oid = new OidVarbind(OIDString);
p.VbList()->FirstVbPair()->OIDVarbind(oid);
}
else
{
// ErrorStatus returned an SNMP error
printf("ERROR: ErrorStatus returned an SNMP error.\n");
break;
}
}
else
{
// RequestID does not match
printf("ERROR: RequestID does not match.\n");
break;
}
}
else
{
// Packet could not be parsed correctly, some error somewhere
printf("ERROR: Packet could not be parsed. Error code: %u\n", udp.ErrorCode());
break;
}
/******************************/
/* Delete packet if it exists */
/******************************/
if ( retP != NULL )
{
delete retP;
retP=NULL;
}
}
else
{
// No packet received
printf("ERROR: No packet returned. Error code: %u\n", udp.ErrorCode());
break;
}
}
free(OIDString);
return;
}
/**********************************************************************
*%FUNCTION: waitForPADO
*%ARGUMENTS:
* conn -- PPPoEConnection structure
* timeout -- how long to wait (in seconds)
*%RETURNS:
* Nothing
*%DESCRIPTION:
* Waits for a PADO packet and copies useful information
***********************************************************************/
void
waitForPADO(PPPoEConnection *conn, int timeout)
{
fd_set readable;
int r;
struct timeval tv;
PPPoEPacket packet;
int len;
struct PacketCriteria pc;
pc.conn = conn;
pc.acNameOK = (conn->acName) ? 0 : 1;
pc.serviceNameOK = (conn->serviceName) ? 0 : 1;
pc.seenACName = 0;
pc.seenServiceName = 0;
do {
if (BPF_BUFFER_IS_EMPTY) {
tv.tv_sec = timeout;
tv.tv_usec = 0;
FD_ZERO(&readable);
FD_SET(conn->discoverySocket, &readable);
while(1) {
r = select(conn->discoverySocket+1, &readable, NULL, NULL, &tv);
if (r >= 0 || errno != EINTR) break;
}
if (r < 0) {
fatalSys("select (waitForPADO)");
}
if (r == 0) return; /* Timed out */
}
/* Get the packet */
receivePacket(conn->discoverySocket, &packet, &len);
/* Check length */
if (ntohs(packet.length) + HDR_SIZE > len) {
syslog(LOG_ERR, "Bogus PPPoE length field (%u)",
(unsigned int) ntohs(packet.length));
continue;
}
#ifdef USE_BPF
/* If it's not a Discovery packet, loop again */
if (etherType(&packet) != Eth_PPPOE_Discovery) continue;
#endif
if (conn->debugFile) {
dumpPacket(conn->debugFile, &packet, "RCVD");
fprintf(conn->debugFile, "\n");
fflush(conn->debugFile);
}
/* If it's not for us, loop again */
if (!packetIsForMe(conn, &packet)) continue;
if (packet.code == CODE_PADO) {
if (NOT_UNICAST(packet.ethHdr.h_source)) {
printErr("Ignoring PADO packet from non-unicast MAC address");
continue;
}
parsePacket(&packet, parsePADOTags, &pc);
if (!pc.seenACName) {
printErr("Ignoring PADO packet with no AC-Name tag");
continue;
}
if (!pc.seenServiceName) {
printErr("Ignoring PADO packet with no Service-Name tag");
continue;
}
conn->numPADOs++;
if (conn->printACNames) {
printf("--------------------------------------------------\n");
}
if (pc.acNameOK && pc.serviceNameOK) {
memcpy(conn->peerEth, packet.ethHdr.h_source, ETH_ALEN);
if (conn->printACNames) {
printf("AC-Ethernet-Address: %02x:%02x:%02x:%02x:%02x:%02x\n",
(unsigned) conn->peerEth[0],
(unsigned) conn->peerEth[1],
(unsigned) conn->peerEth[2],
(unsigned) conn->peerEth[3],
(unsigned) conn->peerEth[4],
(unsigned) conn->peerEth[5]);
continue;
}
conn->discoveryState = STATE_RECEIVED_PADO;
break;
}
}
} while (conn->discoveryState != STATE_RECEIVED_PADO);
}
static int sendAllListPackets() {
// send packet from tail to head and remove sent ones
int sendCount = 0;
UINT sendLen;
PacketNode *pnode;
#ifdef _DEBUG
// check the list is good
// might go into dead loop but it's better for debugging
PacketNode *p = head;
do {
p = p->next;
} while (p->next);
assert(p == tail);
#endif
while (!isListEmpty()) {
pnode = popNode(tail->prev);
sendLen = 0;
assert(pnode != head);
// FIXME inbound injection on any kind of packet is failing with a very high percentage
// need to contact windivert auther and wait for next release
if (!WinDivertSend(divertHandle, pnode->packet, pnode->packetLen, &(pnode->addr), &sendLen)) {
PWINDIVERT_ICMPHDR icmp_header;
PWINDIVERT_ICMPV6HDR icmpv6_header;
PWINDIVERT_IPHDR ip_header;
PWINDIVERT_IPV6HDR ipv6_header;
LOG("Failed to send a packet. (%lu)", GetLastError());
dumpPacket(pnode->packet, pnode->packetLen, &(pnode->addr));
// as noted in windivert help, reinject inbound icmp packets some times would fail
// workaround this by resend them as outbound
// TODO not sure is this even working as can't find a way to test
// need to document about this
WinDivertHelperParsePacket(pnode->packet, pnode->packetLen, &ip_header, &ipv6_header,
&icmp_header, &icmpv6_header, NULL, NULL, NULL, NULL);
if ((icmp_header || icmpv6_header) && IS_INBOUND(pnode->addr.Direction)) {
BOOL resent;
pnode->addr.Direction = WINDIVERT_DIRECTION_OUTBOUND;
if (ip_header) {
UINT32 tmp = ip_header->SrcAddr;
ip_header->SrcAddr = ip_header->DstAddr;
ip_header->DstAddr = tmp;
} else if (ipv6_header) {
UINT32 tmpArr[4];
memcpy(tmpArr, ipv6_header->SrcAddr, sizeof(tmpArr));
memcpy(ipv6_header->SrcAddr, ipv6_header->DstAddr, sizeof(tmpArr));
memcpy(ipv6_header->DstAddr, tmpArr, sizeof(tmpArr));
}
resent = WinDivertSend(divertHandle, pnode->packet, pnode->packetLen, &(pnode->addr), &sendLen);
LOG("Resend failed inbound ICMP packets as outbound: %s", resent ? "SUCCESS" : "FAIL");
InterlockedExchange16(&sendState, SEND_STATUS_SEND);
} else {
InterlockedExchange16(&sendState, SEND_STATUS_FAIL);
}
} else {
if (sendLen < pnode->packetLen) {
// TODO don't know how this can happen, or it needs to be resent like good old UDP packet
LOG("Internal Error: DivertSend truncated send packet.");
InterlockedExchange16(&sendState, SEND_STATUS_FAIL);
} else {
InterlockedExchange16(&sendState, SEND_STATUS_SEND);
}
}
freeNode(pnode);
++sendCount;
}
assert(isListEmpty()); // all packets should be sent by now
return sendCount;
}
/**********************************************************************
*%FUNCTION: waitForPADS
*%ARGUMENTS:
* conn -- PPPoE connection info
* timeout -- how long to wait (in seconds)
*%RETURNS:
* Nothing
*%DESCRIPTION:
* Waits for a PADS packet and copies useful information
***********************************************************************/
void
waitForPADS(PPPoEConnection *conn, int timeout)
{
fd_set readable;
int r;
struct timeval tv;
PPPoEPacket packet;
int len;
do {
if (BPF_BUFFER_IS_EMPTY) {
tv.tv_sec = timeout;
tv.tv_usec = 0;
FD_ZERO(&readable);
FD_SET(conn->discoverySocket, &readable);
while(1) {
r = select(conn->discoverySocket+1, &readable, NULL, NULL, &tv);
if (r >= 0 || errno != EINTR) break;
}
if (r < 0) {
fatalSys("select (waitForPADS)");
}
if (r == 0) return;
}
/* Get the packet */
receivePacket(conn->discoverySocket, &packet, &len);
/* Check length */
if (ntohs(packet.length) + HDR_SIZE > len) {
syslog(LOG_ERR, "Bogus PPPoE length field (%u)",
(unsigned int) ntohs(packet.length));
continue;
}
#ifdef USE_BPF
/* If it's not a Discovery packet, loop again */
if (etherType(&packet) != Eth_PPPOE_Discovery) continue;
#endif
if (conn->debugFile) {
dumpPacket(conn->debugFile, &packet, "RCVD");
fprintf(conn->debugFile, "\n");
fflush(conn->debugFile);
}
/* If it's not from the AC, it's not for me */
if (memcmp(packet.ethHdr.h_source, conn->peerEth, ETH_ALEN)) continue;
/* If it's not for us, loop again */
if (!packetIsForMe(conn, &packet)) continue;
/* Is it PADS? */
if (packet.code == CODE_PADS) {
/* Parse for goodies */
parsePacket(&packet, parsePADSTags, conn);
conn->discoveryState = STATE_SESSION;
break;
}
} while (conn->discoveryState != STATE_SESSION);
/* Don't bother with ntohs; we'll just end up converting it back... */
conn->session = packet.session;
syslog(LOG_INFO, "PPP session is %d", (int) ntohs(conn->session));
/* RFC 2516 says session id MUST NOT be zero or 0xFFFF */
if (ntohs(conn->session) == 0 || ntohs(conn->session) == 0xFFFF) {
syslog(LOG_ERR, "Access concentrator used a session value of %x -- the AC is violating RFC 2516", (unsigned int) ntohs(conn->session));
}
}
/***********************************************************************
*%FUNCTION: sendPADI
*%ARGUMENTS:
* conn -- PPPoEConnection structure
*%RETURNS:
* Nothing
*%DESCRIPTION:
* Sends a PADI packet
***********************************************************************/
static void
sendPADI(PPPoEConnection *conn)
{
PPPoEPacket packet;
unsigned char *cursor = packet.payload;
PPPoETag *svc = (PPPoETag *) (&packet.payload);
UINT16_t namelen = 0;
UINT16_t plen;
int omit_service_name = 0;
if (conn->serviceName) {
namelen = (UINT16_t) strlen(conn->serviceName);
if (!strcmp(conn->serviceName, "NO-SERVICE-NAME-NON-RFC-COMPLIANT")) {
omit_service_name = 1;
}
}
/* Set destination to Ethernet broadcast address */
memset(packet.ethHdr.h_dest, 0xFF, ETH_ALEN);
memcpy(packet.ethHdr.h_source, conn->myEth, ETH_ALEN);
packet.ethHdr.h_proto = htons(Eth_PPPOE_Discovery);
packet.ver = 1;
packet.type = 1;
packet.code = CODE_PADI;
packet.session = 0;
if (!omit_service_name) {
plen = TAG_HDR_SIZE + namelen;
CHECK_ROOM(cursor, packet.payload, plen);
svc->type = TAG_SERVICE_NAME;
svc->length = htons(namelen);
if (conn->serviceName) {
memcpy(svc->payload, conn->serviceName, strlen(conn->serviceName));
}
cursor += namelen + TAG_HDR_SIZE;
} else {
plen = 0;
}
/* If we're using Host-Uniq, copy it over */
if (conn->useHostUniq) {
PPPoETag hostUniq;
pid_t pid = getpid();
hostUniq.type = htons(TAG_HOST_UNIQ);
hostUniq.length = htons(sizeof(pid));
memcpy(hostUniq.payload, &pid, sizeof(pid));
CHECK_ROOM(cursor, packet.payload, sizeof(pid) + TAG_HDR_SIZE);
memcpy(cursor, &hostUniq, sizeof(pid) + TAG_HDR_SIZE);
cursor += sizeof(pid) + TAG_HDR_SIZE;
plen += sizeof(pid) + TAG_HDR_SIZE;
}
#ifdef PLUGIN
/* Add our maximum MTU/MRU */
if (MIN(lcp_allowoptions[0].mru, lcp_wantoptions[0].mru) > ETH_PPPOE_MTU) {
PPPoETag maxPayload;
UINT16_t mru = htons(MIN(lcp_allowoptions[0].mru, lcp_wantoptions[0].mru));
maxPayload.type = htons(TAG_PPP_MAX_PAYLOAD);
maxPayload.length = htons(sizeof(mru));
memcpy(maxPayload.payload, &mru, sizeof(mru));
CHECK_ROOM(cursor, packet.payload, sizeof(mru) + TAG_HDR_SIZE);
memcpy(cursor, &maxPayload, sizeof(mru) + TAG_HDR_SIZE);
cursor += sizeof(mru) + TAG_HDR_SIZE;
plen += sizeof(mru) + TAG_HDR_SIZE;
}
#endif
packet.length = htons(plen);
sendPacket(conn, conn->discoverySocket, &packet, (int) (plen + HDR_SIZE));
#ifdef DEBUGGING_ENABLED
if (conn->debugFile) {
dumpPacket(conn->debugFile, &packet, "SENT");
fprintf(conn->debugFile, "\n");
fflush(conn->debugFile);
}
#endif
}
