On Windows I am able to enumerate removable drives using FindFirstVolume, FindNextVolume, GetVolumePathNamesForVolumeName and the GetDriveType functions. How can I achieve this on Linux and macOS?
On macOS you need to work with IOKit: https://developer.apple.com/documentation/iokit
It can looks like the following code:
#include <cstdlib>
#include <iostream>
#include <mach/mach_port.h>
#include <IOKit/IOKitLib.h>
#include <IOKit/IOTypes.h>
#include <IOKit/IOKitKeys.h>
#include <IOKit/IOCFPlugIn.h>
#include <IOKit/usb/IOUSBLib.h>
std::string get_descriptor_idx(IOUSBDeviceInterface **dev, UInt8 idx);
int main(int argc, char *argv[]) {
/*
* Get the IO registry
*/
auto entry = IORegistryGetRootEntry(kIOMasterPortDefault);
if (entry == 0)
return EXIT_FAILURE;
io_iterator_t iter{};
auto kret = IORegistryEntryCreateIterator(entry, kIOUSBPlane, kIORegistryIterateRecursively, &iter);
if (kret != KERN_SUCCESS || iter == 0)
return EXIT_FAILURE;
io_service_t service {};
std::cout << std::endl;
while ((service = IOIteratorNext(iter))) {
IOCFPlugInInterface **plug = nullptr;
IOUSBDeviceInterface **dev = nullptr;
io_string_t path;
SInt32 score = 0;
IOReturn ioret;
kret = IOCreatePlugInInterfaceForService(service, kIOUSBDeviceUserClientTypeID, kIOCFPlugInInterfaceID, &plug, &score);
IOObjectRelease(service);
if (kret != KERN_SUCCESS || plug == nullptr) {
continue;
}
/*
* USB
*/
ioret = (*plug)->QueryInterface(plug, CFUUIDGetUUIDBytes(kIOUSBDeviceInterfaceID),
static_cast<LPVOID *>((void *) &dev));
(*plug)->Release(plug);
if (ioret != kIOReturnSuccess || dev == nullptr) {
continue;
}
if (IORegistryEntryGetPath(service, kIOServicePlane, path) != KERN_SUCCESS) {
(*dev)->Release(dev);
continue;
}
std::cout << "USB Path: " << path << std::endl;
UInt8 si;
UInt16 u16v;
if ((*dev)->GetDeviceVendor(dev, &u16v) == kIOReturnSuccess)
std::cout << "VID: "<< u16v << std::endl;
if ((*dev)->GetDeviceProduct(dev, &u16v) == kIOReturnSuccess)
std::cout << "PID: "<< u16v << std::endl;
if ((*dev)->USBGetManufacturerStringIndex(dev, &si) == kIOReturnSuccess) {
std::cout << "Manufacturer: " << get_descriptor_idx(dev, si) << std::endl;
}
if ((*dev)->USBGetProductStringIndex(dev, &si) == kIOReturnSuccess) {
std::cout << "Product: " << get_descriptor_idx(dev, si) << std::endl;
}
(*dev)->Release(dev);
std::cout << std::endl;
}
IOObjectRelease(iter);
return 0;
}
/* a quick version */
std::string get_descriptor_idx(IOUSBDeviceInterface **dev, UInt8 idx)
{
IOUSBDevRequest request;
IOReturn ioret;
char buffer[4086] = { 0 };
CFStringRef cfstr;
CFIndex len;
request.bmRequestType = USBmakebmRequestType(kUSBIn, kUSBStandard, kUSBDevice);
request.bRequest = kUSBRqGetDescriptor;
request.wValue = (kUSBStringDesc << 8) | idx;
request.wIndex = 0x409;
request.wLength = sizeof(buffer);
request.pData = buffer;
ioret = (*dev)->DeviceRequest(dev, &request);
if (ioret != kIOReturnSuccess)
return "n/a";
if (request.wLenDone <= 2)
return "n/a";
cfstr = CFStringCreateWithBytes(nullptr, (const UInt8 *)buffer+2, request.wLenDone-2, kCFStringEncodingUTF16LE, 0);
len = CFStringGetMaximumSizeForEncoding(CFStringGetLength(cfstr), kCFStringEncodingUTF8) + 1;
if (len < 0) {
CFRelease(cfstr);
return "n/a";
}
std::string str; str.resize(len);
CFStringGetCString(cfstr, str.data(), len, kCFStringEncodingUTF8);
CFRelease(cfstr);
return str;
}
In my case the result ıs:
USB Path: IOService:/AppleARMPE/arm-io/xxxx/USB2.1 Hub#01100000
VID: 1507
PID: 1552
Manufacturer: GenesysLogic
Product: USB2.1 USB
Related
I'm trying to figure out how ICMP and Boost Asio work. There was a problem sending the packet to the endpoint. The entered url is translated into ip and a socket connection is made. The problem is that when a packet is sent via socket.send (...), an exception is thrown.
Exception: send: Bad address
#include <algorithm>
#include <chrono>
#include <functional>
#include <iostream>
#include <memory>
#include <tuple>
//BOOST
#include <boost/asio.hpp>
#include <boost/program_options.hpp>
#include <boost/log/trivial.hpp>
//CONSTANTS
#define BUFFER_SIZE_64KB 65536
#define TTL_DEFAULT 64
#define ICMP_HDR_SIZE 8
#define LINUX_PAYLOAD_SIZE 56
#define TIME_BYTE_SIZE 4
#define FILL_BYTE 0X8
template <typename T, typename flag_type = int>
using flagged = std::tuple<flag_type, T>;
using namespace boost::asio;
typedef boost::system::error_code error_code;
typedef unsigned char byte;
enum ICMP : uint8_t {
ECHO_REPLY = 0,
UNREACH = 3,
TIME_EXCEEDED = 11,
ECHO_REQUEST = 8
};
enum class IPtype {IPV4, IPV6, BOTH};
struct icmp_header_t {
uint8_t type;
uint8_t code;
uint16_t checksum;
uint16_t id;
uint16_t seq_num;
};
struct ip_header_t {
uint8_t ver_ihl;
uint8_t tos;
uint16_t total_length;
uint16_t id;
uint16_t flags_fo;
uint8_t ttl;
uint8_t protocol;
uint16_t checksum;
uint32_t src_addr;
uint32_t dst_addr;
};
ip_header_t ip_load(std::istream& stream, bool ntoh ) {
ip_header_t header;
stream.read((char*)&header.ver_ihl, sizeof(header.ver_ihl));
stream.read((char*)&header.tos, sizeof(header.tos));
stream.read((char*)&header.total_length, sizeof(header.total_length));
stream.read((char*)&header.id, sizeof(header.id));
stream.read((char*)&header.flags_fo, sizeof(header.flags_fo));
stream.read((char*)&header.ttl, sizeof(header.ttl));
stream.read((char*)&header.protocol, sizeof(header.protocol));
stream.read((char*)&header.checksum, sizeof(header.checksum));
stream.read((char*)&header.src_addr, sizeof(header.src_addr));
stream.read((char*)&header.dst_addr, sizeof(header.dst_addr));
if (ntoh) {
header.total_length = ntohs(header.total_length);
header.id = ntohs(header.id);
header.flags_fo = ntohs(header.flags_fo);
header.checksum = ntohs(header.checksum);
header.src_addr = ntohl(header.src_addr);
header.dst_addr = ntohl(header.dst_addr);
}
return header;
}
icmp_header_t icmp_load(std::istream& stream) {
icmp_header_t header;
stream.read((char*)&header.type, sizeof(header.type));
stream.read((char*)&header.code, sizeof(header.code));
stream.read((char*)&header.checksum, sizeof(header.checksum));
stream.read((char*)&header.id, sizeof(header.id));
stream.read((char*)&header.seq_num, sizeof(header.seq_num));
return header;
}
flagged<ip::icmp::endpoint> sync_icmp_solver(io_service& ios, std::string host,
IPtype type = IPtype::BOTH) noexcept {
ip::icmp::resolver::query query(host, "");
ip::icmp::resolver resl(ios);
ip::icmp::endpoint ep;
error_code ec;
auto it = resl.resolve(query, ec);
if (ec != boost::system::errc::errc_t::success) {
std::cerr << "Error message = " << ec.message() << std::endl;
return std::make_tuple(ec.value(), ep);
}
ip::icmp::resolver::iterator it_end;
//Finds first available ip.
while (it != it_end) {
ip::icmp::endpoint ep = (it++)->endpoint();
auto addr = ep.address();
switch(type) {
case IPtype::IPV4:
if (addr.is_v4()) return std::make_tuple(0, ep);
break;
case IPtype::IPV6:
if(addr.is_v6()) return std::make_tuple(0, ep);
break;
case IPtype::BOTH:
return std::make_tuple(0, ep);
break;
}
}
return std::make_tuple(-1, ep);
}
unsigned short checksum(void *b, int len) {
unsigned short* buf = reinterpret_cast<unsigned short*>(b);
unsigned int sum = 0;
unsigned short result;
for (sum = 0; len > 1; len -= 2 ) {
sum += *buf++;
}
if (len == 1) sum += *(byte*) buf;
sum = (sum >> 16) + (sum & 0xFFFF);
sum += (sum >> 16);
result = ~sum;
return result;
}
unsigned short get_identifier() {
#if defined(BOOST_WINDOWS)
return static_cast<unsigned short>(::GetCurrentProcessId());
#else
return static_cast<unsigned short>(::getpid());
#endif
}
struct PingInfo {
unsigned short seq_num = 0;
size_t time_out;
size_t reply_time = 1;
size_t payload_size = LINUX_PAYLOAD_SIZE;
size_t packets_rec = 0;
size_t packets_trs = 0;
size_t reps = 0;
};
class PingConnection {
private:
ip::icmp::socket sock;
io_service* ios_ptr;
PingInfo* pi_ptr;
ip::icmp::endpoint dst;
boost::posix_time::ptime timestamp;
streambuf input_buf;
deadline_timer deadtime;
//TODO: Check for memleaks.
void write_icmp_req(std::ostream& os) {
byte* pckt = new byte[ICMP_HDR_SIZE + pi_ptr->payload_size];
unsigned short pid = get_identifier();
pckt[0] = 0x8;
pckt[1] = 0x0;
pckt[2] = 0x0;
pckt[3] = 0x0;
pckt[4] = (byte)((pid & 0xF0) >> 4);
pckt[5] = (byte)(pid & 0x0F);
for (size_t i = ICMP_HDR_SIZE; i < ICMP_HDR_SIZE + pi_ptr->payload_size; i++) {
pckt[i] = FILL_BYTE;
}
pckt[6] = (byte)((pi_ptr->seq_num & 0xF0) >> 4);
pckt[7] = (byte)((pi_ptr->seq_num)++ & 0x0F);
unsigned short cs = checksum(pckt, ICMP_HDR_SIZE);
pckt[2] = (byte)((cs & 0xF0) >> 4);
pckt[3] = (byte)(cs & 0x0F);
os << pckt;
delete [] pckt;
}
void pckt_send() {
streambuf buf;
std::ostream os(&buf);
write_icmp_req(os);
timestamp = boost::posix_time::microsec_clock::universal_time();
std::cout << "begin" << std::endl;
sock.send(buf.data());
std::cout << "sock.send(buf.data())" << std::endl;
deadtime.expires_at(timestamp + boost::posix_time::seconds(pi_ptr->time_out));
deadtime.async_wait(std::bind(&PingConnection::req_timeout_callback, this));
}
void req_timeout_callback() {
if (pi_ptr->reps == 0) {
std::cout << "Time Out:echo req" << std::endl;
}
deadtime.expires_at(timestamp + boost::posix_time::seconds(pi_ptr->reply_time));
deadtime.async_wait(std::bind(&PingConnection::pckt_send, this));
}
void pckt_recv() {
std::cout << "pckt_recv" << std::endl;
input_buf.consume(input_buf.size());
sock.async_receive(input_buf.prepare(BUFFER_SIZE_64KB),
std::bind(&PingConnection::recv_timeout_callback, this, std::placeholders::_2));
}
void recv_timeout_callback(size_t sz) {
std::cout << "recv_timeout_callback" << std::endl;
input_buf.commit(sz);
std::istream is(&input_buf);
ip_header_t iph = ip_load(is, false);
icmp_header_t icmph = icmp_load(is);
if (is &&
icmph.type == ECHO_REQUEST &&
icmph.id == get_identifier() &&
icmph.seq_num == pi_ptr->seq_num) {
// If this is the first reply, interrupt the five second timeout.
if (pi_ptr->reps++ == 0) deadtime.cancel();
boost::posix_time::ptime now = boost::posix_time::microsec_clock::universal_time();
std::cout << sz - iph.total_length
<< " bytes from " << iph.src_addr
<< ": icmp_seq=" << icmph.seq_num
<< ", ttl=" << iph.ttl
<< ", time=" << (now - timestamp).total_milliseconds() << " ms"
<< std::endl;
}
pckt_recv();
}
public:
PingConnection(io_service& ios, PingInfo& pi_add) : deadtime(ios), sock(ios) {
pi_ptr = &pi_add;
ios_ptr = &ios;
}
void ping(std::string host) {
int err_flag;
error_code error;
std::tie(err_flag, dst) = sync_icmp_solver(*ios_ptr, host);
if (err_flag) return;
std::cout << dst << std::endl;
sock.connect(dst, error);
if(error) {
return;
}
std::cout << "sock.connect(dst)" << error.message() <<std::endl;
pckt_send();
pckt_recv();
}
};
int main(int argc, char** argv) {
try
{
if (argc < 2) {
std::cerr << "Usage: ping [args]* destination\n";
return -1;
}
io_service ios;
PingInfo pi;
pi.time_out = 56;
PingConnection ping(ios, pi);
ping.ping(argv[1]);
ios.run();
} catch(std::exception& e) {
std::cerr << "Exception: " << e.what() << std::endl;
}
}
socket.send() is called in pckt_send()
For development I use WSL2 and Ubuntu image.
I'm studying ZeroMQ with myself.
I tested PUB as a server(bind), SUB as a client(connect) and worked fine. Opposite (PUB as a client(connect), SUB as a server(bind)) also works fine.
When I connect a another SUB socket as client something goes wrong without any exception or errors.
here's my example code.
#include <zmq.hpp>
#include <string>
#include <iostream>
#include <unistd.h>
#include <thread>
class ZMQSock
{
public:
ZMQSock(const char* addr)
{
if (addr != NULL)
{
mctx = new zmq::context_t(1);
mszAddr = new char[strlen(addr) + 1];
snprintf(mszAddr, strlen(addr) + 1, "%s", addr);
}
}
virtual ~ZMQSock()
{
if (msock != nullptr)
delete msock;
if (mctx != nullptr)
delete mctx;
if (mszAddr != nullptr)
delete [] mszAddr;
}
int zbind()
{
if (msock != nullptr)
msock->bind(mszAddr);
else return -1;
return 0;
}
int zconnect()
{
if (msock != nullptr)
msock->connect(mszAddr);
else return -1;
return 0;
}
void start()
{
if (mbthread != false)
return ;
mbthread = true;
mhthread = std::thread(std::bind(&ZMQSock::run, this));
}
virtual void stop()
{
if (mbthread == false)
return ;
mbthread = false;
if (mhthread.joinable())
mhthread.join();
}
virtual void run() = 0;
protected:
char* mszAddr{nullptr};
zmq::context_t* mctx{nullptr};
zmq::socket_t* msock{nullptr};
bool mbthread{false};
std::thread mhthread;
};
class ZPublisher : public ZMQSock
{
public:
ZPublisher(const char* addr) : ZMQSock(addr)
{
if (msock == nullptr)
{
msock = new zmq::socket_t(*mctx, ZMQ_PUB);
}
}
virtual ~ZPublisher()
{
}
bool zsend(const char* data, const unsigned int length, bool sendmore=false)
{
zmq::message_t msg(length);
memcpy(msg.data(), data, length);
if (sendmore)
return msock->send(msg, ZMQ_SNDMORE);
return msock->send(msg);
}
void run()
{
if (mszAddr == nullptr)
return ;
if (strlen(mszAddr) < 6)
return ;
const char* fdelim = "1";
const char* first = "it sends to first. two can not recv this sentence!\0";
const char* sdelim = "2";
const char* second = "it sends to second. one can not recv this sentence!\0";
while (mbthread)
{
zsend(fdelim, 1, true);
zsend(first, strlen(first));
zsend(sdelim, 1, true);
zsend(second, strlen(second));
usleep(1000 * 1000);
}
}
};
class ZSubscriber : public ZMQSock
{
public:
ZSubscriber(const char* addr) : ZMQSock(addr)
{
if (msock == nullptr)
{
msock = new zmq::socket_t(*mctx, ZMQ_SUB);
}
}
virtual ~ZSubscriber()
{
}
void setScriberDelim(const char* delim, const int length)
{
msock->setsockopt(ZMQ_SUBSCRIBE, delim, length);
mdelim = std::string(delim, length);
}
std::string zrecv()
{
zmq::message_t msg;
msock->recv(&msg);
return std::string(static_cast<char*>(msg.data()), msg.size());
}
void run()
{
if (mszAddr == nullptr)
return ;
if (strlen(mszAddr) < 6)
return ;
while (mbthread)
{
std::cout << "MY DELIM IS [" << mdelim << "] - MSG : ";
std::cout << zrecv() << std::endl;
usleep(1000 * 1000);
}
}
private:
std::string mdelim;
};
int main ()
{
ZPublisher pub("tcp://localhost:5252");
ZSubscriber sub1("tcp://localhost:5252");
ZSubscriber sub2("tcp://*:5252");
pub.zconnect();
sub1.zconnect();
sub2.zbind();
sub1.setScriberDelim("1", 1);
sub2.setScriberDelim("2", 1);
pub.start();
std::cout << "PUB Server has been started.." << std::endl;
usleep(1000 * 1000);
sub1.start();
std::cout << "SUB1 Start." << std::endl;
sub2.start();
std::cout << "SUB2 Start." << std::endl;
int i = 0;
std::cout << "< Press any key to exit program. >" << std::endl;
std::cin >> i;
std::cout << "SUB1 STOP START" << std::endl;
sub1.stop();
std::cout << "SUB2 STOP START" << std::endl;
sub2.stop();
std::cout << "PUB STOP START" << std::endl;
pub.stop();
std::cout << "ALL DONE" << std::endl;
return 0;
}
What causes this? or Am I using PUB/SUB illegally?
You are connecting a SUB socket to a SUB socket, that is an invalid connection. In your case the PUB should bind and the SUBs should connect.
If I plug in a device, say /dev/ttyUSB0 and I want to get the number 0 based on its VID:PID (found with lsusb), how could I do that in C++ Linux? I have this code to find one printer device, if it's helpful at all:
int printer_open (void)
{
char printer_location[] = "/dev/usb/lpX";
struct stat buf;
// continuously try all numbers until stat returns true for the connected printer
for (int i = 0; i < 10; i++)
{
printer_location[11] = '0' + i;
if (!stat (printer_location, &buf))
break;
}
return 0;
}
You could use libusb
apt-get install build-essential libudev-dev
Here is a good example: http://www.dreamincode.net/forums/topic/148707-introduction-to-using-libusb-10/
and here is the lib description: http://libusb.sourceforge.net/api-1.0/
int main() {
libusb_context *context = NULL;
libusb_device **list = NULL;
int rc = 0;
ssize_t count = 0;
rc = libusb_init(&context);
assert(rc == 0);
count = libusb_get_device_list(context, &list);
assert(count > 0);
for (size_t idx = 0; idx < count; ++idx) {
libusb_device *device = list[idx];
libusb_device_descriptor desc = {0};
rc = libusb_get_device_descriptor(device, &desc);
assert(rc == 0);
printf("Vendor:Device = %04x:%04x\n", desc.idVendor, desc.idProduct);
}
}
And if you compile your code don't forget to add the lib reference -I/usr/include/libusb-1.0/ and - lusb-1.0
libusb can't get it actually. So look at this file instead: /proc/bus/input/devices
Example line from the file:
I: Bus=0003 Vendor=1a2c Product=0c23 Version=0110
N: Name="USB USB Keyboard"
P: Phys=usb-0000:00:14.0-3/input0
S: Sysfs=/devices/pci0000:00/0000:00:14.0/usb1/1-3/1-3:1.0/0003:1A2C:0C23.0015/input/input30
U: Uniq=
H: Handlers=sysrq kbd event10 leds
B: PROP=0
B: EV=120013
B: KEY=1000000000007 ff800000000007ff febeffdff3cfffff fffffffffffffffe
B: MSC=10
B: LED=7
This function gets the event number from the device with the matching VID:PID:
#include <string>
#include <iostream>
#include <fstream>
void open_device (std::string device_vid, std::string device_pid)
{
try
{
std::ifstream file_input;
std::size_t pos;
std::string device_path, current_line, search_str, event_str;
std::string device_list_file = "/proc/bus/input/devices";
bool vid_pid_found = false;
int fd = 0;
bool debug = true;
// 1. open device list file
file_input.open(device_list_file.c_str());
if (!file_input.is_open())
{
std::cerr << "file_input.open >> " << std::strerror(errno) << std::endl;
throw -2;
}
// 2. search for first VID:PID and get event number
search_str = "Vendor=" + device_vid + " Product=" + device_pid;
while (getline(file_input, current_line))
{
if (!vid_pid_found)
{
pos = current_line.find(search_str, 0);
if (pos != std::string::npos)
{
vid_pid_found = true;
search_str = "event";
}
}
else
{
pos = current_line.find(search_str, 0);
if (pos != std::string::npos)
{
event_str = current_line.substr(pos);
// find space and substring event##
pos = event_str.find(' ', 0);
event_str = event_str.substr(0, pos);
break;
}
}
}
// 3. build device path
device_path = "/dev/input/" + event_str;
if (debug) std::cout << "device_path = " << device_path << std::endl;
// 4. connect to device
fd = open (device_path.c_str(), O_RDONLY);
if (fd < 0)
{
std::cerr << "open >> errno = " << std::strerror(errno) << std::endl;
throw -3;
}
}
catch (const std::exception &e)
{
std::cerr << "e.what() = " << e.what() << std::endl;
throw -1;
}
return;
}
Closed. This question needs debugging details. It is not currently accepting answers.
Edit the question to include desired behavior, a specific problem or error, and the shortest code necessary to reproduce the problem. This will help others answer the question.
Closed 7 years ago.
Improve this question
I am trying to write a c++ program that assembles MIPS instructions. While debugging, it keeps throwing an error at line 74 of my main:
myassembler.add(lexed[i].labels[0], lexed[i].name, tokens, i);
my main is here:
#include <fstream>
#include <iostream>
#include <iomanip>
#include <memory>
#include <stdexcept>
#include <string>
#include <sstream>
#include <vector>
#include "exceptions.h"
#include "lexer.h"
#include "util.h"
#include "assembler.h"
std::string read_file(const std::string& name) {
std::ifstream file(name);
if (!file.is_open()) {
std::string error = "Could not open file: ";
error += name;
throw std::runtime_error(error);
}
std::stringstream stream;
stream << file.rdbuf();
return std::move(stream.str());
}
int main(int argc, char** argv) {
// Adjusting -- argv[0] is always filename.
--argc;
++argv;
if (argc == 0) {
std::cerr << "Need a file" << std::endl;
return 1;
}
assembler myassembler;
for (int i = 0; i < argc; ++i) {
std::string asmName(argv[i]);
if (!util::ends_with_subseq(asmName, std::string(".asm"))) {
std::cerr << "Need a valid file name (that ends in .asm)" << std::endl;
std::cerr << "(Bad name: " << asmName << ")" << std::endl;
return 1;
}
// 4 is len(".asm")
auto length = asmName.size() - string_length(".asm");
std::string baseName(asmName.begin(), asmName.begin() + length);
std::string objName = baseName + ".obj";
try {
auto text = read_file(asmName);
try {
auto lexed = lexer::analyze(text); // Parses the entire file and returns a vector of instructions
for (int i =0; i < (int)lexed.size(); i++){
if(lexed[i].labels.size() > 0) // Checking if there is a label in the current instruction
std::cout << "label = " << lexed[i].labels[0] << "\n"; // Prints the label
std::cout<< "instruction name = " << lexed[i].name<< "\n"; // Prints the name of instruction
std::cout << "tokens = ";
std::vector<lexer::token> tokens = lexed[i].args;
for(int j=0; j < (int)tokens.size(); j++){ // Prints all the tokens of this instruction like $t1, $t2, $t3
if (tokens[j].type == lexer::token::Integer)
std::cout << tokens[j].integer() << " ";
else
std::cout << tokens[j].string() << " ";
}
myassembler.add(lexed[i].labels[0], lexed[i].name, tokens, i);
myassembler.p();
std::cout << "\n\n\n";
}
} catch(const bad_asm& e) {
std::stringstream error;
error << "Cannot assemble the assembly code at line " << e.line;
throw std::runtime_error(error.str());
} catch(const bad_label& e) {
std::stringstream error;
error << "Undefined label " << e.what() << " at line " << e.line;
throw std::runtime_error(error.str());
}
} catch (const std::runtime_error& err) {
std::cout << err.what() << std::endl;
return 1;
}
}
/*getchar();*/
return 0;
}
assembler.h:
#include "lexer.h"
#include <fstream>
#include <vector>
#include <string>
struct symbol
{
std::string label = "";
int slinenum;
};
struct relocation
{
std::string instruct = "";
std::string label = "";
int rlinenum;
int rt = 0;
int rs = 0;
};
struct opcode
{
std::string instruct = "";
int opc = 0;
bool isloadstore = false;
int extType = 0;
bool isbranch = false;
};
struct function
{
std::string instruct = "";
int funct = 0;
bool isjr = false;
bool isshift = false;
};
struct regs
{
std::string name;
int num;
};
enum instrtype
{
R, I, neither
};
class assembler
{
public:
assembler();
void oinit(void);
void finit(void);
void rinit(void);
void printToFile(std::fstream &file);
void savesymb(std::string label, int line);
void saverel(std::string instr, std::string label, int line, int rt, int rs);
std::vector<int> formatr(std::string instr, lexer::token toke1, lexer::token toke2, lexer::token toke3, int line);
int formatr(std::string instr, lexer::token toke, int line);
std::vector<int> formati(std::string instr, lexer::token toke1, lexer::token toke2, lexer::token toke3, int line);
std::vector<int> formati(std::string instr, lexer::token toke1, lexer::token toke2, int line);
int findnum(std::string regname);
void add(std::string label, std::string instr, const std::vector<lexer::token> &tokens, int linen);
void secAdd(void);
int rassemble(std::string instr, int rd, int rs, int rt, int shamt);
int iassemble(std::string instr, int rt, int rs, int imm);
void p();
private:
std::vector<int> results;
std::vector<symbol> symbtable;
std::vector<relocation> reloctable;
std::vector<opcode> ops;
std::vector<function> functions;
std::vector<regs> registers;
instrtype type = neither;
};
and assembler.cpp:
// ECE 2500
// Project 1: myAssembler
// assembler.cpp
// Sheila Zhu
#include "lexer.h"
#include <iostream>
#include <fstream>
#include <vector>
#include <string>
#include "assembler.h"
assembler::assembler()
{
oinit();
finit();
rinit();
}
void assembler::oinit()
{
opcode myop;
myop.instruct = "addi";
myop.opc = 8;
myop.extType = 1;
ops.push_back(myop);
// more of the same
}
void assembler::finit()
{
function myfunc;
myfunc.instruct = "add";
myfunc.funct = 32;
functions.push_back(myfunc);
// more of the same
}
void assembler::rinit()
{
regs myreg;
myreg.name = "$zero";
myreg.num = 0;
registers.push_back(myreg);
//more of the same
}
void assembler::printToFile(std::fstream &file)
{
for (int i = 0; i < (int)results.size(); i++)
file << results.at(i) << std::endl;
}
void assembler::savesymb(std::string label, int line)
{
symbol symb;
symb.label = label;
symb.slinenum = line * 4;
symbtable.push_back(symb);
}
void assembler::saverel(std::string instr, std::string label, int line, int rt, int rs)
{
relocation re;
re.instruct = instr;
re.label = label;
re.rlinenum = line;
re.rt = rt;
re.rs = rs;
}
int assembler::findnum(std::string regname)
{
for (int i = 0; i < (int)registers.size(); i++)
{
if (regname == registers.at(i).name)
return registers.at(i).num;
}
return -1;
}
std::vector<int> assembler::formatr(std::string instr, lexer::token toke1, lexer::token toke2, lexer::token toke3, int line)
{
int rd = 0, rs = 0, rt = 0, shamt = 0;
std::vector<int> x;
function currf;
for (int i = 0; i < (int)functions.size(); i++)
{
if (instr == functions.at(i).instruct)
currf = functions.at(i);
}
try
{
if (currf.isshift)
{
if (toke1.type == lexer::token::Integer)
throw 1;
else
{
rd = findnum(toke1.string());
if (rd == -1)
throw 2;
}
if (toke2.type == lexer::token::Integer)
throw 1;
else
{
rs = findnum(toke2.string());
if (rs == -1)
throw 2;
}
if (toke3.type == lexer::token::Integer)
{
shamt = toke3.integer();
if (shamt < 0)
throw 3;
}
else
throw 1;
}
else
{
if (toke1.type == lexer::token::Integer)
throw 1;
else
{
rd = findnum(toke1.string());
if (rd == -1)
throw 2;
}
if (toke2.type == lexer::token::Integer)
throw 1;
else
{
rs = findnum(toke2.string());
if (rs == -1)
throw 2;
}
if (toke3.type == lexer::token::Integer)
throw 1;
else
{
rt = findnum(toke3.string());
if (rt == -1)
throw 2;
}
}
}
catch (int e)
{
if (e == 1)
std::cerr << "Wrong argument in line " << line << std::endl;
else if (e == 2)
std::cerr << "Invalid register name in line " << line << std::endl;
else
std::cerr << "Shift amount cannot be negative in line " << line << std::endl;
}
x.push_back(rd);
x.push_back(rs);
x.push_back(rt);
x.push_back(shamt);
return x;
}
int assembler::formatr(std::string instr, lexer::token toke, int line)
{
int rs = 0;
try
{
if (toke.type == lexer::token::Integer)
throw 1;
else
{
rs = findnum(toke.string());
if (rs == -1)
throw 2;
}
}
catch (int e)
{
if (e == 1)
std::cerr << "Wrong argument in line " << line << std::endl;
else
std::cerr << "Invalid register name in line " << line << std::endl;
}
return rs;
}
std::vector<int> assembler::formati(std::string instr, lexer::token toke1, lexer::token toke2, lexer::token toke3, int line)
{
int rt = 0, rs = 0, imm = 0;
std::vector<int> x;
opcode currop;
for (int i = 0; i < (int)ops.size(); i++)
{
if (instr == ops.at(i).instruct)
currop = ops.at(i);
}
try
{
if (currop.isbranch)
{
if (toke1.type == lexer::token::Integer)
throw 1;
else
{
rt = findnum(toke1.string());
if (rt == -1)
throw 2;
}
if (toke2.type == lexer::token::Integer)
throw 1;
else
{
rs = findnum(toke2.string());
if (rs == -1)
throw 2;
}
if (toke3.type == lexer::token::Integer)
imm = toke3.integer();
else
saverel(instr, toke3.string(), line, rt, rs);
}
else if (currop.isloadstore)
{
if ((instr == "lbu") || (instr == "sb"))
{
if (toke2.type == lexer::token::String)
throw 1;
else
{
if (toke2.integer() < 0)
imm = (0xFFFF << 16) + (0xFF << 8) + toke2.integer();
else
imm = toke2.integer();
}
if (toke1.type == lexer::token::Integer)
throw 1;
else
{
rt = findnum(toke1.string());
if (rt == -1)
throw 2;
}
if (toke3.type == lexer::token::Integer)
throw 1;
else
{
rs = findnum(toke2.string());
if (rs == -1)
throw 2;
}
}
else
{
if (toke2.type == lexer::token::String)
throw 1;
else
{
if (toke2.integer() < 0)
imm = (0xFFFF << 16) + toke2.integer();
else
imm = toke2.integer();
}
if (toke1.type == lexer::token::Integer)
throw 1;
else
{
rt = findnum(toke1.string());
if (rt == -1)
throw 2;
}
if (toke3.type == lexer::token::Integer)
throw 1;
else
{
rs = findnum(toke2.string());
if (rs == -1)
throw 2;
}
}
}
else
{
if ((instr == "andi") || (instr == "ori"))
{
if (toke1.type == lexer::token::Integer)
throw 1;
else
{
rt = findnum(toke1.string());
if (rt == -1)
throw 2;
}
if (toke2.type == lexer::token::Integer)
throw 1;
else
{
rs = findnum(toke2.string());
if (rs == -1)
throw 2;
}
if (toke3.type == lexer::token::Integer)
imm = toke3.integer();
else
throw 1;
}
else
{
if (toke1.type == lexer::token::Integer)
throw 1;
else
{
rt = findnum(toke1.string());
if (rt == -1)
throw 2;
}
if (toke2.type == lexer::token::Integer)
throw 1;
else
{
rs = findnum(toke2.string());
if (rs == -1)
throw 2;
}
if (toke3.type == lexer::token::Integer)
{
if (toke3.integer() < 0)
imm = (0xFFFF << 16) + toke2.integer();
else
imm = toke3.integer();
}
else
throw 1;
}
}
}
catch (int e)
{
if (e == 1)
std::cerr << "Wrong argument in line " << line << std::endl;
else
std::cerr << "Invalid register name in line " << line << std::endl;
}
x.push_back(rt);
x.push_back(rs);
x.push_back(imm);
return x;
}
std::vector<int> assembler::formati(std::string instr, lexer::token toke1, lexer::token toke2, int line)
{
int rt = 0, imm = 0;
std::vector<int> rval;
try
{
if (toke1.type == lexer::token::Integer)
throw 1;
else
{
rt = findnum(toke1.string());
if (rt == -1)
throw 2;
}
if (toke2.type == lexer::token::String)
throw 1;
else
imm = toke2.integer();
}
catch (int e)
{
if (e == 1)
std::cerr << "Wrong argument in line " << line << std::endl;
else
std::cerr << "Invalid register name in line " << line << std::endl;
}
rval.push_back(rt);
rval.push_back(imm);
return rval;
}
void assembler::add(std::string label, std::string instr, const std::vector<lexer::token> &token, int linen)
{
int assembled = 0, rd = 0, rt = 0;
std::vector<int> argh;
int arg;
if (label.length() > 0)
savesymb(label, linen);
for (int i = 0; i < (int)functions.size(); i++)
{
if (instr == functions.at(i).instruct)
type = R;
}
for (int i = 0; i < (int)ops.size(); i++)
{
if (instr == ops.at(i).instruct)
type = I;
}
if (type == R)
{
try
{
if (instr == "jr")
{
if ((int)token.size() == 1)
{
arg = formatr(instr, token.at(0), linen);
assembled = rassemble(instr, rd, arg, rt, 0);
}
else
throw 1;
}
else
{
if ((int)token.size() == 3)
{
argh = formatr(instr, token.at(0), token.at(2), token.at(3), linen);
assembled = rassemble(instr, argh[0], argh[1], argh[2], argh[3]);
}
else
throw 1;
}
}
catch (int e)
{
if (e == 1)
std::cerr << "Wrong number of arguments at line " << linen << std::endl;
}
}
else if (type == I)
{
try
{
if (instr == "lui")
{
if ((int)token.size() == 2)
{
argh = formati(instr, token.at(0), token.at(1), linen);
assembled = iassemble(instr, argh[0], 0, argh[1]);
}
else
throw 1;
}
else
{
if ((int)token.size() == 3)
{
argh = formati(instr, token.at(0), token.at(1), token.at(2), linen);
assembled = iassemble(instr, argh[0], argh[1], argh[2]);
}
else
throw 1;
}
}
catch (int e)
{
if (e == 1)
std::cout << "Wrong number of arguments at line " << linen << std::endl;
}
}
else
std::cerr << "Instruction not recognized at line " << linen << std::endl;
results.push_back(assembled);
}
void assembler::secAdd(void)
{
std::vector<int>::iterator iter = results.begin();
for (int i = 0; i < (int)reloctable.size(); i++)
{
for (unsigned int j = 0; j < symbtable.size(); j++)
{
if (reloctable.at(i).label == symbtable.at(j).label)
{
int assembled = 0;
iter += (reloctable.at(i).rlinenum / 4);
for (unsigned int k = 0; k < ops.size(); k++)
{
if (reloctable.at(i).instruct == ops.at(k).instruct)
type = I;
}
if (type == I)
assembled = iassemble(reloctable.at(i).instruct, reloctable.at(i).rt, reloctable.at(i).rs, symbtable.at(i).slinenum);
else
std::cerr << "Instruction not recognized at line " << reloctable.at(i).rlinenum << std::endl;
results.erase(iter);
results.insert(iter, assembled);
}
}
}
}
int assembler::rassemble(std::string instr, int rd, int rs, int rt, int shamt)
{
int func = 0;
int code = 0;
for (int i = 0; i < (int)functions.size(); i++)
{
if (instr == functions.at(i).instruct)
{
func = functions.at(i).funct;
break;
}
else
{
if (i == (functions.size() - 1))
return -1;
}
}
code = (rs << 21) + (rt << 16) + (rd << 11) + (shamt << 6) + func;
return code;
}
int assembler::iassemble(std::string instr, int rt, int rs, int imm)
{
int op = 0;
int code = 0;
for (int i = 0; i < (int)ops.size(); i++)
{
if (instr == ops.at(i).instruct)
{
op = ops.at(i).opc;
break;
}
else
{
if (i == (ops.size() - 1))
return -1;
}
}
code = (op << 26) + (rs << 21) + (rt << 16) + imm;
return code;
}
void assembler::p()
{
for (int i = 0; i < (int)results.size(); i++)
std::cout << results.at(i) << " ";
std::cout << std::endl;
}
When debugging, the tokens parameter triggers the error, and the this pointer in the vector code shows that the vector size changes to 0 at these lines:
#if _ITERATOR_DEBUG_LEVEL == 2
if (size() <= _Pos)
What exactly is happening?
Sorry if my formatting is bad/wrong, etc., and please let me know if I should make any edits/provide more code.
Thanks in advance.
The error is caused by accessing by index vector element that does not exist.
In you case lexed[i] should be valid. So, the only possible issue may be with empty labels vector. Validate this vector before accessing its elements, for example
myassembler.add(lexed[i].labels.empty() ? "" : lexed[i].labels[0],
lexed[i].name, tokens, i);
Actually there is one more bug for very large lexed arrays when integer index may overflow. You should not cast result of .size() to int. Instead proper type should be used for i:
for (size_t i = 0; i < lexed.size(); i++)
I have a problem about the usage of this TPACKET_V2 .
My problem is that after setting of this type of packet on socket, when I try to receive some packets I can't read the vlan id from the packet (of course from the header of the packet) the vlan_tci is ever 0.
Now I'm using open suse sp1 and when I run my program on sless sp2 I 'm able to get the vlan id with the same program that doesn't work on sless sp1 but the weird thing is that tcpdump is able to get the vlan id (on this sless) and tcpdump set the TPACKET_V2 (so this means that TPACKET_2 is supported)
My simple project is based on these functions , all called by the function createSocket , then there is a simple method that is reading packets on the socket and there I try to get informations on vlan id (there there is also the relative part used before with the TPACKET_V1)
#include <linux/if_packet.h>
#include <linux/if_ether.h>
#include <netinet/in.h>
#include <netinet/if_ether.h>
#include <netinet/ether.h>
#include <linux/filter.h>
#include <net/if.h>
#include <arpa/inet.h>
enum INTERFACE_T
{
RX_INTERFACE,
TX_INTERFACE
};
static const char* PKT_TYPE[];
// BLOCK_NUM*BLOCK_SIZE = FRAME_NUM*FRAME_SIZE
enum { RX_BLOCK_SIZE = 8192,
RX_BLOCK_NUM = 256,
RX_FRAME_SIZE = 2048,
RX_FRAME_NUM = 1024
};
enum { TX_BLOCK_SIZE = 8192,
TX_BLOCK_NUM = 256,
TX_FRAME_SIZE = 2048,
TX_FRAME_NUM = 1024
};
struct RxFrame {
struct tpacket2_hdr tp_h; // Packet header
uint8_t tp_pad[TPACKET_ALIGN(sizeof(tpacket2_hdr))-sizeof(tpacket2_hdr)];
struct sockaddr_ll sa_ll; // Link level address information
uint8_t sa_ll_pad[14]; //Alignment padding
struct ethhdr eth_h;
} __attribute__((packed));
struct TxFrame
{
struct tpacket_hdr tp_h; // Packet header
uint8_t tp_pad[TPACKET_ALIGN(sizeof(tpacket_hdr))-sizeof(tpacket_hdr)];
// struct vlan_ethhdr vlan_eth_h;
// struct arp arp;
} __attribute__((packed));
struct ring_buff {
struct tpacket_req req;
size_t size; // mmap size
size_t cur_frame;
struct iovec *ring_buffer_;
void *buffer; // mmap
};
int setIfFlags(short int flags)
{
struct ifreq ifr;
memset(&ifr, 0, sizeof(ifr));
strncpy(ifr.ifr_name, if_name_.c_str(), sizeof(ifr.ifr_name));
ifr.ifr_hwaddr.sa_family=getIfArptype();
ifr.ifr_flags |= flags;
if ( ioctl(socket_, SIOCSIFFLAGS, &ifr) == -1)
{
std::cout << "Error: ioctl(SIOSIFFLAGS) failed!" << std::endl;
return 1;
}
return 0;
}
int bindSocket()
{
struct sockaddr_ll sll;
memset(&sll, 0, sizeof(sll));
sll.sll_family = AF_PACKET;
sll.sll_protocol = htons(ETH_P_ALL);
sll.sll_ifindex = ifIndex_;
sll.sll_hatype = 0;
sll.sll_pkttype = 0;
sll.sll_halen = 0;
if (bind(socket_, (struct sockaddr *)&sll, sizeof(sll)) < 0) {
std::cout << "Error: bind() failed!" << std::endl;
return 1;
}
return 0;
}
int packetMmap(ring_buff * rb)
{
assert(rb);
rb->buffer = mmap(0, rb->size, PROT_READ | PROT_WRITE, MAP_SHARED, socket_, 0);
if (rb->buffer == MAP_FAILED) {
std::cout << "Error: mmap() failed!" << std::endl;
return 1;
}
return 0;
}
void packetMunmap(ring_buff * rb)
{
assert(rb);
if (rb->buffer)
{
munmap(rb->buffer, rb->size);
rb->buffer = NULL;
rb->size = 0;
}
}
int frameBufferCreate(ring_buff * rb)
{
assert(rb);
rb->ring_buffer_ = (struct iovec*) malloc(rb->req.tp_frame_nr * sizeof(*rb->ring_buffer_));
if (!rb->ring_buffer_) {
std::cout << "No memory available !!!" << std::endl;
return 1;
}
memset(rb->ring_buffer_, 0, rb->req.tp_frame_nr * sizeof(*rb->ring_buffer_));
for (unsigned int i = 0; i < rb->req.tp_frame_nr; i++) {
rb->ring_buffer_[i].iov_base = static_cast<void *>(static_cast<char *>(rb->buffer)+(i*rb->req.tp_frame_size));
rb->ring_buffer_[i].iov_len = rb->req.tp_frame_size;
}
return 0;
}
void setRingBuffer(struct ring_buff *ringbuf) { rb_ = ringbuf; }
int setVlanTaggingStripping()
{
socklen_t len;
int val;
unsigned int sk_type, tp_reserve, maclen, tp_hdrlen, netoff, macoff;
unsigned int tp_hdr_len;
unsigned int frame_size = RX_FRAME_SIZE;
val = TPACKET_V2;
len = sizeof(val);
if (getsockopt(socket_, SOL_PACKET, PACKET_HDRLEN, &val, &len) < 0) {
std::cout << "Error: getsockopt(SOL_PACKET, PACKET_HDRLEN) failed (can't get TPACKET_V2 header len on packet)" << std::endl;
return 1;
}
tp_hdr_len = val;
std::cout << "TPACKET_V2 header is supported (hdr len is " << val << ")"<< std::endl;
std::cout << "tpacket2_hdrs header is supported (hdr len is " << sizeof(tpacket2_hdr) << ")"<< std::endl;
val = TPACKET_V2;
if (setsockopt(socket_, SOL_PACKET, PACKET_VERSION, &val, sizeof(val)) < 0) {
std::cout << "Error: setsockopt(SOL_PACKET, PACKET_VERSION) failed (can't activate TPACKET_V2 on packet)" << std::endl;
return 1;
}
std::cout << "TPACKET_V2 version is configured !!! " << std::endl;
/* Reserve space for VLAN tag reconstruction */
val = VLAN_TAG_LEN;
if (setsockopt(socket_, SOL_PACKET, PACKET_RESERVE, &val, sizeof(val)) < 0) {
std::cout << "Error: setsockopt(SOL_PACKET, PACKET_RESERVE) failed (can't set up reserve on packet)" << std::endl;
return 1;
}
std::cout<< "Reserve space for VLAN tag reconstruction is configured !!! " << std::endl;
return 0;
}
int setSoBufforce(int optname, int buffSize)
{
if (setsockopt(socket_, SOL_SOCKET, SO_SNDBUFFORCE, &buffSize, sizeof(buffSize)) == -1)
{
std::cout << "Error: setsocketopt("<< (optname == SO_SNDBUFFORCE ? "SO_SNDBUFFORCE" : "SO_RCVBUFFORCE") << ") failed!" << std::endl;
return 1;
}
return 0;
}
createSocket(std::string ifName, INTERFACE_T ifType)
{
if (ifName.empty())
{
std::cout << "Error: interface is empty!" << std::endl;;
return NULL;
}
//Create the socket
if ( (socket_ = socket(AF_PACKET, SOCK_RAW, htons(ETH_P_ALL))) == -1 )
{
std::cout << "Error: calling socket(AF_PACKET, SOCK_RAW, htons(ETH_P_ALL)) failed!" << std::endl;
}
std::cout << "Creating Socket on interface= " << ifName << " to listen to ETH_P_ALL"<<std::endl;;
s->setIfFlags(IFF_PROMISC|IFF_BROADCAST);
//allocate space for ring buffer
ring_buff *rb = (ring_buff *) malloc(sizeof(ring_buff));
// use the same size for RX/TX ring
//set the version , here I insert the use of TPACKET_V2!
setVlanTaggingStripping();
rb->req.tp_block_size = RX_BLOCK_SIZE;
rb->req.tp_block_nr = RX_BLOCK_NUM;
rb->req.tp_frame_size = RX_FRAME_SIZE;
rb->req.tp_frame_nr = RX_FRAME_NUM;
setPacketRing(PACKET_RX_RING,&rb->req);
rb->size = (rb->req.tp_block_size)*(rb->req.tp_block_nr);
rb->cur_frame = 0;
// Tweak send/rcv buffer size
int sndBufSz = 4194304; // Send buffer in bytes
int rcvBufSz = 4194304; // Receive buffer in bytes
if (setSoBufforce(SO_SNDBUFFORCE, sndBufSz))
{
//close socket
}
if (setSoBufforce(SO_RCVBUFFORCE, rcvBufSz))
{
//close socket
}
// Add ARP filter so we will only receive ARP packet on this socket
struct sock_filter BPF_code[6];
struct sock_fprog filter;
bindSocket();
if (packetMmap(rb))
{
std::cout << "Error: mmap() failed!" << std::endl;
//close socket
}
frameBufferCreate(rb);
setRingBuffer(rb);
}
and in my function for receive packets and I try to read informations and in particular h_vlan_TCI from but I receive ever 0x00 !!! Any suggestions?
struct vlan_ethhdr* vlan_eth_h = (struct vlan_ethhdr*)&frame->eth_h
void readRawSocket(socket_)
{
while (*(unsigned long*)rb->ring_buffer_[rb->cur_frame].iov_base)
{
RxFrame* frame = (RxFrame *)rb->ring_buffer_[rb->cur_frame].iov_base;
#if 0
tpacket_hdr* h = &frame->tp_h;
char buffer[256];
sprintf (buffer, " -tpacket(v1): status=%ld,len=%d,snaplen=%d,mac=%d,net=%d,sec=%d,usec=%d",
h->tp_status, h->tp_len, h->tp_snaplen, h->tp_mac,h->tp_net, h->tp_sec, h->tp_usec);
std::cout << std::string(buffer) << std::endl;
#else
tpacket2_hdr* h = &frame->tp_h;
char buffer[512];
sprintf (buffer, " -tpacket(v2): status=%d,len=%d,snaplen=%d,mac=%d,net=%d,sec=%d,nsec=%d,vlan_tci=%d (vlan_tci=0x%04x)",
h->tp_status, h->tp_len, h->tp_snaplen, h->tp_mac, h->tp_net, h->tp_sec, h->tp_nsec, h->tp_vlan_tci, ntohs(h->tp_vlan_tci));
std::cout << std::string(buffer) << std::endl;
#endif
if ( ETH_P_8021Q == ntohs(frame->eth_h.h_proto) )
{
struct vlan_ethhdr* vlan_eth_h = (struct vlan_ethhdr*)&frame->eth_h;
int vlan_tag = VLAN_TAG(ntohs(vlan_eth_h->h_vlan_TCI));
std::cout << " -Vlan " << vlan_tag << " packet to this host received";
}
rb->cur_frame = ( rb->cur_frame+1) % rx_socket_->getFrameNum();
} // while()
}
When the kernel removes the vlan it also changes eth_h.h_proto to the protocol after de vlan tag so ETH_P_8021Q == ntohs(frame->eth_h.h_proto) will most probably be false.
Also, if you are listening in the tagged interface (ie. eth0.100) instead of the physical interface (eth0) you will not see the tags.