I am receiving data following a certain format over a TCP server by serializing them.
the class of the object:
class Command {
private:
char letter;
int x;
int y;
std::string button;
public:
Command(char _letter, int _x, int _y, std::string _button) {
letter = _letter;
x = _x;
y = _y;
button = _button;
}
Command(std::string serializedCmd)
{
size_t delimPos = 0;
std::vector<std::string> parts;
while ((delimPos = serializedCmd.find(SERIALIZE_DELIM)) != std::string::npos)
{
parts.push_back(serializedCmd.substr(0, delimPos));
serializedCmd.erase(0, delimPos + 1);
}
if (parts.empty()) {
this->letter = '$';
this->x = -1;
this->y = -1;
this->button = "nan";
return;
}
this->letter = (char)atoi(parts.at(0).data());
this->x = atoi(parts.at(1).data());
this->y = atoi(parts.at(2).data());
this->button = parts.at(3);
}
Command() {}
~Command() {}
std::string serialize()
{
return std::to_string(letter) + SERIALIZE_DELIM + std::to_string(x) + SERIALIZE_DELIM + std::to_string(y) + SERIALIZE_DELIM + button + SERIALIZE_DELIM;
}
char getLetter() { return letter; }
int getX() { return x; }
int getY() { return y; }
std::string getButton() { return button; }
bool isEmpty() {
return((this->letter == '$') && (this->x == -1) && (this->y == -1) && (this->button == "nan"));
}
void printCommand() {
std::cout << "letter: " << letter << std::endl;
std::cout << "x : " << x << std::endl;
std::cout << "y : " << y << std::endl;
std::cout << "button: " << button << std::endl;
std::cout << "================" << std::endl;
}
};
The data after being DeSerialized at the clients end follows this format:
||{key}|{x}|{y}|{button}||
Example: ||$|20|40|nan||
The problem is that when using recv to get the data, it seems that I'm picking up some noise around the command.
Example:
Sending:
||$|301|386|nan||
Reciving:
(¿ⁿ8T√|301|386|╠╠↕▼
The command is there although it's crowded with noise for some reason.
The code I'm using to receive the data:
char buf[4096];
Command c;
std::string commandTemp = "";
while (true) {
memset(buf, '\0', 4096);
const int size = recv(sock, buf, sizeof(buf), 0);
std::string s(buf,size);
std::cout << s << std::endl;
buf[size] = 0;
commandTemp = buf;
if (!commandTemp.empty()) {
try {
c = Command(commandTemp);
exe(c); //executes command (unrelated)
}
catch (const std::exception& err) {
std::cout << "Couldn't execute!!!!!!!!" << std::endl;
}
}
else {
std::cout << "Error empty command!\n";
}
}
If I am missing any information I will happily provide it.
Can someone maybe tell what the problem is?
You have to loop on the recv till you get the entire message
This may not be the immediate cause of you problem but you will hit it eventually.
TCP is a stream protocol, not a message protocol. All that TCP guarantees is that the bytes you send are received once and in order. But you might send one 100 byte message and receive 20 5 byte messages. You will say "but it works now", true if on the same machine or the messages are small but not true with larger message over a real netwrok so you must do this
char buf[4096];
Command c;
std::string commandTemp = "";
while (true) {
memset(buf, '\0', 4096);
int offset = 0;
int len = ??;
while(len > 0){
const int size = recv(sock, buf + offset, sizeof(buf) - offset, 0);
if (size == 0)
break; // record that we got incomplete message
offset += size;
len -= size;
}
....
Note that you need to know the length in advance too. So either send fixed length message or prepend a fixed size length to each message and read that first
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;
}
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.
I have an issue where my Arduino program hangs for no reason. I run my program, and at some undetermined point, the Serial Monitor stops printing output. Here's what I've tested so far:
In my "com.init_drone()" method, I've commented everything out except for the last line, which signals that the method returned. When I do this, my program hangs somewhere else but still it doesn't get to the infinite while loop.
I've been outputting memory usage, and I'm getting numbers no lower than 450 -- this tells me that I'm not using an absurd amount of memory.
I've tried removing the Timer1 instantiation, interrupt attach/detach, bu that has had no effect on the program.
My .ino file(and Command) is located here for anyone that wants a fuller picture and doesn't want to scroll through all this code I'm going to post below.
Here's my log output so far. Notice the truncation!:
AT&F
AT+NMAC=00:1d:c9:10:39:6f
AT+WM=0
AT+NDHCP=1
AT+WA=ardrone_279440
AT+NCUDP=192.168.1.1,5556
S0AT*CONFIG=1,"general:navdata_demo","TRUE"
EAT*CONFIG=1,"general:navdata_demo","TRUE"
638
S0AT*CONFIG=2,"control:altitude_max","2000"
EAT*CONFIG=2,"control:altitude_max","2000"
638
S0AT*CONFIG=3,"control:euler_angle_max","0.35"
EAT*CONFIG=3,"control:euler_angle_max","0.35"
586
S0AT*CONFIG=4,"control:outdoor","FALSE"
EAT*CONFIG=4,"control:outdoor","FALSE"
635
S0AT*CONFIG=5,"control:flight_without_shell","FALSE"
EAT*CONFIG=5,"control:flight_without_shell","FALSE"
574
S0AT*CTRL=6,4,0
EAT*CTRL=6,4,0
629
S0AT*CTRL=7,0,0
EAT*CTRL=7,0,0
629
S0AT*CTRL=8,4,0
EAT*CTRL=8,4,0
629
S0AT*COMWDG=9
EAT*COMWDG=9
629
S0AT*COMWDG=10
EAT*COMWDG=10
629
S0AT*COMWDG=11
EAT*COMWDG=11
629
S0AT*COMWDG=12
EAT*COMWDG=12
629
S0AT*COMWDG=13
EAT*COMWDG=13
629
S0AT*FTRIM=14
EAT*FTRIM=14
629
Here is my .ino file:
#include "Command.h"
#include "Streaming.h"
int debug = 1;
extern ring_buffer rx_buf;
extern resultint_ resultint;
Command com;
int sequenceNumber = 1;
String atcmd = "";
#include "TimerOne.h"
#define LEDpin 13
void setup()
{
PCsrl.begin(9600);
com.start_wifi_connection();
com.drone_is_init = com.init_drone();
Timer1.initialize(COMWDG_INTERVAL_USEC);
Timer1.attachInterrupt(watchdog_timer);
}
void watchdog_timer() {
com.sendwifi(com.makeComwdg());
}
void loop()
{
if (com.drone_is_init == 0) {
if (debug) {
// never use three ! together in arduino code
PCsrl.println("Drone wasn't initlized before loop() was called. Initalizing now.\r\n");
}
} else {
com.drone_takeoff();
com.drone_takeoff();
com.sendwifi(com.makePcmd(1,0,0,0,0));
com.sendwifi(com.makePcmd(1,0,0,0,0));
delay(5000);
com.moveForward(1);
com.moveRotate(180);
com.moveForward(1);
com.moveRotate(180);
delay(500);
com.drone_landing();
com.drone_landing();
delay(500);
//end of program
Timer1.detachInterrupt();
PCsrl.println("Program finished");
while (1){};
}
}
And my Command.cpp
#ifndef GAINSPAN
#define GAINSPAN
#include "Command.h"
extern int sequenceNumber;
extern int debug;
ring_buffer rx_buf= {{0}, 0, 0};
resultint_ resultint;
Command::Command()
{
at = "";
command = "";
s2ip_running = 0;
drone_is_init = 0;
drone_is_hover = 0;
emergency = 0;
}
void Command::sendwifi(String s) {
WIFIsrl.write(27); //esc
WIFIsrl.print("S0"); //choose connection CID 0
WIFIsrl.print(s);
WIFIsrl.write(27);
WIFIsrl.print("E");
if(debug) PCsrl.println(s);
WIFIsrl.println(memoryTest());
}
int Command::start_wifi_connection()
{
WIFIsrl.begin(9600);
WIFIsrl.println("");
WIFIsrl.println("AT&F");
//WIFIsrl.println("ATE0"); //turn off echo
WIFIsrl.print("AT+NMAC=00:1d:c9:10:39:6f\r"); //set MAC address
WIFIsrl.println("AT+WM=0");
WIFIsrl.println("AT+NDHCP=1");
/* drone's network profile, change if needed*/
WIFIsrl.println("AT+WA=ardrone_279440");
WIFIsrl.println("AT+NCUDP=192.168.1.1,5556");
readARsrl();
delay(3000); //need 3 seconds for connection to establish
return 0;
}
String Command::makeComwdg()
{
at = "AT*COMWDG=";
command = at + getSequenceNumber() + "\r\n";
return command;
}
void Command::sendComwdg_t(int msec)
{
for (int i = 0; i < msec; i+=20) {
sendwifi(makeComwdg());
delay(20);
}
}
void Command::sendFtrim()
{
at = "AT*FTRIM=";
command = at + getSequenceNumber() + "\r\n";
sendwifi(command);
}
void Command::sendConfig(String option, String value)
{
at = "AT*CONFIG=";
command = at + getSequenceNumber() + ",\"" + option + "\",\"" + value + "\"\r\n";
sendwifi(command);
}
void Command::sendRef(flying_status fs)
{
at = "AT*REF=";
if(fs == TAKEOFF){
command = at + getSequenceNumber() + ",290718208\r\n"; //takeoff
}
else if(fs == LANDING){
command = at + getSequenceNumber() + ",290717696\r\n"; //landing
} else if (fs == EMERGENCY_TOGGLE){
command = at + getSequenceNumber() + ",290717952\r\n"; //landing
}
// emergency -> 290717952
sendwifi(command);
}
void Command::send_control_commands(){
at = "AT*CTRL=";
sendwifi(at+getSequenceNumber()+",4,0\r\n");
sendwifi(at+getSequenceNumber()+",0,0\r\n");
sendwifi(at+getSequenceNumber()+",4,0\r\n");
}
void Command::drone_emergency_reset()
{
at = "AT*REF=";
command = at + getSequenceNumber() + ",290717952\r\n";
sendwifi(command);
}
/** Movement functions **/
int Command::moveForward(float distanceInMeters)
{
float i = 0;
String moveForward = makePcmd(1, 0, -.855, 0, 0);
delay(1000*distanceInMeters);
sendPcmd(moveForward);
return 1;
}
int Command::moveRotate(float yawInDegrees)
{
int i = 0;
while (i < yawInDegrees) {
String stayRotate = makePcmd(1, 0, 0, 0, 0.17);
sendPcmd(stayRotate);
delay(150);
i += 8;
}
return 1;
}
String Command::makePcmd(int enable, float roll, float pitch, float gaz, float yaw)
{
at = "AT*PCMD=";
command = at + getSequenceNumber() + "," + enable + "," + fl2int(roll) + "," + fl2int(pitch) + "," + fl2int(gaz) + "," + fl2int(yaw) + "\r";
return command;
}
void Command::sendPcmd(String command)
{
previousCommand = command;
sendwifi(command);
}
void Command::sendPcmd(int enable, float roll, float pitch, float gaz, float yaw)
{
at = "AT*PCMD=";
command = at + getSequenceNumber() + "," + enable + "," + fl2int(roll) + "," + fl2int(pitch) + "," + fl2int(gaz) + "," + fl2int(yaw) + "\r";
sendwifi(command);
}
String Command::makeAnim(anim_mayday_t anim, int time)
{
at = "AT*ANIM=";
command = at + getSequenceNumber() + "," + anim + "," + time + "\r\n";
return command;
}
void Command::doLEDAnim(int animseq, int duration)
{
PCsrl << "calling LEDAnim" << endl;
at = "AT*LED=";
command = at + getSequenceNumber() + "," + animseq + ",1073741824," + duration + "\r\n";
sendwifi(command);
}
int Command::start_s2ip()
{
char temp;
//delay(20000); //wait for drone to start
readARsrl();
if (debug) {
PCsrl << "trying to start s2ip" << endl;
}
ARsrl.print("\r\n");
delay(500);
ARsrl.print("\r\n");
delay(500);
ARsrl << "cd ~" << endl;
if (debug) {
readARsrl();
}
delay(500);
ARsrl << "cd data/video/apps/" << endl;
delay(500);
ARsrl << "./s2ip.arm" << endl;
while ((int) temp != 2) {
temp = ARsrl.read();
if (temp == 2) {
PCsrl << "s2ip is running" << endl;
ARsrl << "bullshit\r\n"; //to fix a delay bug
break;
}
//PCsrl << "s2ip not running" << endl;
}
if (debug) {
while (ARsrl.available()) {
PCsrl.write(ARsrl.read());
}
}
return 1;
}
void Command::quit_s2ip()
{
ARsrl.println("EXIT");
while (ARsrl.available()) {
PCsrl.write(ARsrl.read());
}
}
int Command::init_drone()
{
sendConfig("general:navdata_demo","TRUE");
sendConfig("control:altitude_max","2000");
sendConfig("control:euler_angle_max","0.35");
sendConfig("control:outdoor","FALSE");
sendConfig("control:flight_without_shell","FALSE");
send_control_commands();
sendComwdg_t(90);
sendFtrim();
drone_emergency_reset(); //clear emergency flag
return 1;
}
int Command::drone_takeoff()
{
sendRef(TAKEOFF);
int i = 0;
return 1;
}
int Command::drone_hover(int msec)
{
int i = 0;
while (i < msec) {
sendwifi(makePcmd(1, 0, 0, 0, 0));
delay(100);
i += 100;
}
return 1;
}
int Command::drone_landing()
{
sendRef(LANDING);
return 1;
}
int Command::drone_move_up(int centimeter)
{
int i = 0;
while (i < centimeter) {
ARsrl << makePcmd(1, 0, 0, 0.6, 0);
delay(100);
i += 10;
}
return 1;
}
int Command::drone_move_down(int centimeter)
{
int i = 0;
while (i < centimeter) {
sendwifi(makePcmd(1, 0, 0, -0.5, 0));
delay(100);
i += 10;
}
return 1;
}
long Command::fl2int(float value)
{
resultint.i = 0;
if (value < -1 || value > 1) {
resultint.f = 1;
} else {
resultint.f=value;
}
return resultint.i;
}
void Command::readARsrl()
{
while (ARsrl.available()) {
if (debug) {
PCsrl.write(ARsrl.read());
}
}
}
//Memory test code from : http://www.faludi.com/2007/04/18/arduino-available-memory-test/
int Command::memoryTest() {
int byteCounter = 0; // initialize a counter
byte *byteArray; // create a pointer to a byte array
// More on pointers here: http://en.wikipedia.org/wiki/Pointer#C_pointers
// use the malloc function to repeatedly attempt
// allocating a certain number of bytes to memory
// More on malloc here: http://en.wikipedia.org/wiki/Malloc
while ( (byteArray = (byte*) malloc (byteCounter * sizeof(byte))) != NULL ) {
byteCounter++; // if allocation was successful, then up the count for the next try
free(byteArray); // free memory after allocating it
}
free(byteArray); // also free memory after the function finishes
return byteCounter; // send back the highest number of bytes successfully allocated
}
int Command::getSequenceNumber(){
return sequenceNumber++;
}
// Volatile, since it is modified in an ISR.
volatile boolean inService = false;
void SrlRead()
{
if (inService) {
PCsrl.println("timer kicked too fast");
return;
}
interrupts();
inService = true;
while(ARsrl.available()) {
unsigned char k = ARsrl.read();
store_char(k, &rx_buf);
}
inService = false;
}
void read_rx_buf()
{
while (rx_buf.tail != rx_buf.head) {
if (debug) {
PCsrl.write(rx_buf.buffer[rx_buf.tail]);
}
rx_buf.tail = (unsigned int) (rx_buf.tail+ 1) % SERIAL_BUFFER_SIZE;
}
}
inline void store_char(unsigned char c, ring_buffer *buffer)
{
int i = (unsigned int)(buffer->head + 1) % SERIAL_BUFFER_SIZE;
// if we should be storing the received character into the location
// just before the tail (meaning that the head would advance to the
// current location of the tail), we're about to overflow the buffer
// and so we don't write the character or advance the head.
if (i != buffer->tail) {
buffer->buffer[buffer->head] = c;
buffer->head = i;
}
else {
Serial.println("ring buffer is too small");
}
}
#endif
I know it sounds weird but, sometimes this happens when arduino is not getting enough power supply. Try connecting the arduino to a power source different from USB.
As soon as I started to put things in prog memory my program started to clear up it's hiccups. It seems it was a memory issue.
I had the same problem, but the issue was with Timer1.initialize(). Try this:
com.drone_is_init = com.init_drone();
Serial.println("One");
Serial.println("Two");
Serial.println("Three");
Timer1.initialize(COMWDG_INTERVAL_USEC);
Timer1.attachInterrupt(watchdog_timer);
Open serial monitor and see... it will show until "Two", and then Arduino will hang.
The issue was I calling some functions of the LiquidCrystal_I2C library, that need interrupt routines. Check if your timer ISR is using some interrupts. If so, you should move this code to another place in your project.