<commit_before>#include <iostream>                                         /* allows to perform standard input and output operations */
#include <stdio.h>                                          /* Standard input/output definitions */
#include <stdint.h>                                         /* Standard input/output definitions */
#include <stdlib.h>                                         /* defines several general purpose functions */
//#include <string>                                         /* String function definitions */
#include <unistd.h>                                         /* UNIX standard function definitions */
#include <fcntl.h>                                          /* File control definitions */
#include <errno.h>                                          /* Error number definitions */
#include <termios.h>                                        /* POSIX terminal control definitions */
#include <ctype.h>                                        /* isxxx() */
#include <ros/ros.h>                                        /* ROS */
#include <geometry_msgs/Twist.h>                            /* ROS Twist message */
#include <base_controller/encoders.h>                       /* Custom message /encoders */

const char* serialport="/dev/ttyAMA0";                      /* defines used serialport */
int serialport_bps=B38400;                                  /* defines baudrate od serialport */
int32_t EncoderL;                                           /* stores encoder value left read from md49 */
int32_t EncoderR;                                           /* stores encoder value right read from md49 */
unsigned char speed_l=128, speed_r=128;                               /* speed to set for MD49 */
bool cmd_vel_received=true;
double vr = 0.0;
double vl = 0.0;
double max_vr = 0.2;
double max_vl = 0.2;
double min_vr = 0.2;
double min_vl = 0.2;
double base_width = 0.4;                                    /* Base width in meters */

int filedesc;                                               /* filedescriptor serialport */
int fd;                                                     /* serial port file descriptor */
unsigned char serialBuffer[16];                             /* Serial buffer to store uart data */
struct termios orig;                                        /* stores original settings */

int openSerialPort(const char * device, int bps);
void writeBytes(int descriptor, int count);
void readBytes(int descriptor, int count);
void read_MD49_Data (void);
void set_MD49_speed (unsigned char speed_l, unsigned char speed_r);

void cmd_vel_callback(const geometry_msgs::Twist& vel_cmd){

    ROS_DEBUG("geometry_msgs/Twist received: linear.x= %f angular.z= %f", vel_cmd.linear.x, vel_cmd.angular.z);

    //ANFANG Alternative
    if (vel_cmd.linear.x==0 && vel_cmd.angular.z==0){vl=0;vr=0;}
    else if(vel_cmd.linear.x == 0){
        // turning
        vr = vel_cmd.angular.z * base_width / 2.0;
        vl = (-1) * vr;
    }
    else if(vel_cmd.angular.z == 0){
        // forward / backward
        vl = vr = vel_cmd.linear.x;
    }
    else{
        // moving doing arcs
        vl = vel_cmd.linear.x - vel_cmd.angular.z * base_width / 2.0;
        if (vl > max_vl) {vl=max_vl;}
        if (vl < min_vl) {vl=min_vl;}
        vr = vel_cmd.linear.x + vel_cmd.angular.z * base_width / 2.0;
        if (vr > max_vr) {vr=max_vr;}
        if (vr < min_vr) {vr=min_vr;}
    }
    //ENDE Alternative

    if (vel_cmd.linear.x>0){
        speed_l = 255;
        speed_r = 255;
        //serialBuffer[0] = 88;					// 88 =X Steuerbyte um Commands an MD49 zu senden
        //serialBuffer[1] = 115;					// 115=s Steuerbyte setSpeed
        //serialBuffer[2] = 255;					// speed1
        //serialBuffer[3] = 255;					// speed2
        //writeBytes(fd, 4);
    }
    if (vel_cmd.linear.x<0){
        speed_l = 0;
        speed_r = 0;
    }
    if (vel_cmd.linear.x==0 && vel_cmd.angular.z==0){
        speed_l = 128;
        speed_r = 128;
    }
    if (vel_cmd.angular.z>0){
        speed_l = 0;
        speed_r = 255;
    }
    if (vel_cmd.angular.z<0){
        speed_l = 255;
        speed_r = 0;
    }

    cmd_vel_received=true;
}

int main( int argc, char* argv[] ){

    ros::init(argc, argv, "base_controller" );
    ros::NodeHandle n;
    ros::Subscriber sub = n.subscribe("/cmd_vel", 100, cmd_vel_callback);
    ros::Publisher encoders_pub = n.advertise<base_controller::encoders>("encoders",100);

    // Open serial port
    // ****************
    filedesc = openSerialPort("/dev/ttyAMA0", B38400);
    if (filedesc == -1) exit(1);
    usleep(40000);                                      // Sleep for UART to power up and set options
    ROS_DEBUG("Serial Port opened \n");

    // Set nodes looprate 10Hz
    // ***********************
    ros::Rate loop_rate(50);

    while( n.ok() )
    {
            // Read encoder and other data from MD49
            // *************************************
            read_MD49_Data();

            // Set speed left and right for MD49
            // ********************************
            if (cmd_vel_received==true) {
                set_MD49_speed(speed_l,speed_r);
                //set_MD49_speed(speed_l,speed_r);
                cmd_vel_received=false;
            }
            //set_MD49_speed(speed_l,speed_r);

            // Publish encoder values to topic /encoders (custom message)
            // ********************************************************************
            base_controller::encoders encoders;
            encoders.encoder_l=EncoderL;
            encoders.encoder_r=EncoderR;
            encoders_pub.publish(encoders);

            // Loop
            // ****
            ros::spinOnce();
            loop_rate.sleep();
    }// end.mainloop

    return 1;
} // end.main

int openSerialPort(const char * device, int bps){
   struct termios neu;
   char buf[128];

   //fd = open(device, O_RDWR | O_NOCTTY | O_NDELAY | O_NONBLOCK);
   fd = open(device, O_RDWR | O_NOCTTY);

   if (fd == -1)
   {
      sprintf(buf, "openSerialPort %s error", device);
      perror(buf);
   }
   else
   {
      tcgetattr(fd, &orig); 						/* save current serial settings */
      tcgetattr(fd, &neu);
      cfmakeraw(&neu);
      //fprintf(stderr, "speed=%d\n", bps);
      cfsetispeed(&neu, bps);
      cfsetospeed(&neu, bps);
      tcsetattr(fd, TCSANOW, &neu); 				/* set new serial settings */
      fcntl (fd, F_SETFL, O_RDWR);
   }
   return fd;
}

void writeBytes(int descriptor, int count) {
    if ((write(descriptor, serialBuffer, count)) == -1) {	// Send data out
        perror("Error writing");
        close(descriptor);				// Close port if there is an error
        exit(1);
    }
    //write(fd,serialBuffer, count);

}

void readBytes(int descriptor, int count) {
    if (read(descriptor, serialBuffer, count) == -1) {	// Read back data into buf[]
        perror("Error reading ");
        close(descriptor);				// Close port if there is an error
        exit(1);
    }
}

void read_MD49_Data (void){
    serialBuffer[0] = 82;							// 82=R Steuerbyte um alle Daten vom MD49 zu lesen
    writeBytes(fd, 1);
    //Daten lesen und in Array schreiben
    readBytes(fd, 18);

    printf("\033[2J");        /*  clear the screen  */
    printf("\033[H");         /*  position cursor at top-left corner */
    printf ("MD49-Data read from AVR-Master: \n");
    printf("====================================================== \n");
    //printf("Encoder1 Byte1: %i ",serialBuffer[0]);
    //printf("Byte2: %i ",serialBuffer[1]);
    //printf("Byte3: % i ",serialBuffer[2]);
    //printf("Byte4: %i \n",serialBuffer[3]);
    //printf("Encoder2 Byte1: %i ",serialBuffer[4]);
    //printf("Byte2: %i ",serialBuffer[5]);
    //printf("Byte3: %i ",serialBuffer[6]);
    //printf("Byte4: %i \n",serialBuffer[7]);
    printf("EncoderL: %i ",EncoderL);
    printf("EncoderR: %i \n",EncoderR);
    printf("====================================================== \n");
    printf("Speed1: %i ",serialBuffer[8]);
    printf("Speed2: %i \n",serialBuffer[9]);
    printf("Volts: %i \n",serialBuffer[10]);
    printf("Current1: %i ",serialBuffer[11]);
    printf("Current2: %i \n",serialBuffer[12]);
    printf("Error: %i \n",serialBuffer[13]);
    printf("Acceleration: %i \n",serialBuffer[14]);
    printf("Mode: %i \n",serialBuffer[15]);
    printf("Regulator: %i \n",serialBuffer[16]);
    printf("Timeout: %i \n",serialBuffer[17]);

    printf("vl= %f \n", vl);
    printf("vr= %f \n", vr);
    EncoderL = serialBuffer[0] << 24;                        // Put together first encoder value
    EncoderL |= (serialBuffer[1] << 16);
    EncoderL |= (serialBuffer[2] << 8);
    EncoderL |= (serialBuffer[3]);
    EncoderR = serialBuffer[4] << 24;                        // Put together second encoder value
    EncoderR |= (serialBuffer[5] << 16);
    EncoderR |= (serialBuffer[6] << 8);
    EncoderR |= (serialBuffer[7]);
}

void set_MD49_speed (unsigned char speed_l, unsigned char speed_r){
    serialBuffer[0] = 88;					// 88 =X Steuerbyte um Commands an MD49 zu senden
    serialBuffer[1] = 115;					// 115=s Steuerbyte setSpeed
    serialBuffer[2] = speed_l;					// speed1
    serialBuffer[3] = speed_r;					// speed2
    writeBytes(fd, 4);
}
<commit_msg>Update code<commit_after>#include <iostream>                                         /* allows to perform standard input and output operations */
#include <stdio.h>                                          /* Standard input/output definitions */
#include <stdint.h>                                         /* Standard input/output definitions */
#include <stdlib.h>                                         /* defines several general purpose functions */
//#include <string>                                         /* String function definitions */
#include <unistd.h>                                         /* UNIX standard function definitions */
#include <fcntl.h>                                          /* File control definitions */
#include <errno.h>                                          /* Error number definitions */
#include <termios.h>                                        /* POSIX terminal control definitions */
#include <ctype.h>                                        /* isxxx() */
#include <ros/ros.h>                                        /* ROS */
#include <geometry_msgs/Twist.h>                            /* ROS Twist message */
#include <base_controller/encoders.h>                       /* Custom message /encoders */

const char* serialport="/dev/ttyAMA0";                      /* defines used serialport */
int serialport_bps=B38400;                                  /* defines baudrate od serialport */
int32_t EncoderL;                                           /* stores encoder value left read from md49 */
int32_t EncoderR;                                           /* stores encoder value right read from md49 */
unsigned char speed_l=128, speed_r=128;                               /* speed to set for MD49 */
bool cmd_vel_received=true;
double vr = 0.0;
double vl = 0.0;
double max_vr = 0.2;
double max_vl = 0.2;
double min_vr = 0.2;
double min_vl = 0.2;
double base_width = 0.4;                                    /* Base width in meters */

int filedesc;                                               /* filedescriptor serialport */
int fd;                                                     /* serial port file descriptor */
unsigned char serialBuffer[16];                             /* Serial buffer to store uart data */
struct termios orig;                                        /* stores original settings */

int openSerialPort(const char * device, int bps);
void writeBytes(int descriptor, int count);
void readBytes(int descriptor, int count);
void read_MD49_Data (void);
void set_MD49_speed (unsigned char speed_l, unsigned char speed_r);

void cmd_vel_callback(const geometry_msgs::Twist& vel_cmd){

    ROS_DEBUG("geometry_msgs/Twist received: linear.x= %f angular.z= %f", vel_cmd.linear.x, vel_cmd.angular.z);

    //ANFANG Alternative
    if (vel_cmd.linear.x==0 && vel_cmd.angular.z==0){vl=0;vr=0;}
    else if(vel_cmd.linear.x == 0){
        // turning
        vr = vel_cmd.angular.z * base_width / 2.0;
        vl = (-1) * vr;
    }
    else if(vel_cmd.angular.z == 0){
        // forward / backward
        vl = vr = vel_cmd.linear.x;
    }
    else{
        // moving doing arcs
        vl = vel_cmd.linear.x - vel_cmd.angular.z * base_width / 2.0;
        if (vl > max_vl) {vl=max_vl;}
        if (vl < min_vl) {vl=min_vl;}
        vr = vel_cmd.linear.x + vel_cmd.angular.z * base_width / 2.0;
        if (vr > max_vr) {vr=max_vr;}
        if (vr < min_vr) {vr=min_vr;}
    }
    //ENDE Alternative

    if (vel_cmd.linear.x>0){
        speed_l = 255;
        speed_r = 255;
        //serialBuffer[0] = 88;					// 88 =X Steuerbyte um Commands an MD49 zu senden
        //serialBuffer[1] = 115;					// 115=s Steuerbyte setSpeed
        //serialBuffer[2] = 255;					// speed1
        //serialBuffer[3] = 255;					// speed2
        //writeBytes(fd, 4);
    }
    if (vel_cmd.linear.x<0){
        speed_l = 0;
        speed_r = 0;
    }
    if (vel_cmd.linear.x==0 && vel_cmd.angular.z==0){
        speed_l = 128;
        speed_r = 128;
    }
    if (vel_cmd.angular.z>0){
        speed_l = 0;
        speed_r = 255;
    }
    if (vel_cmd.angular.z<0){
        speed_l = 255;
        speed_r = 0;
    }

    cmd_vel_received=true;
}

int main( int argc, char* argv[] ){

    ros::init(argc, argv, "base_controller" );
    ros::NodeHandle n;
    ros::Subscriber sub = n.subscribe("/cmd_vel", 100, cmd_vel_callback);
    ros::Publisher encoders_pub = n.advertise<base_controller::encoders>("encoders",100);

    // Open serial port
    // ****************
    filedesc = openSerialPort("/dev/ttyAMA0", B38400);
    if (filedesc == -1) exit(1);
    usleep(40000);                                      // Sleep for UART to power up and set options
    ROS_DEBUG("Serial Port opened \n");

    // Set nodes looprate 10Hz
    // ***********************
    ros::Rate loop_rate(5);

    while( n.ok() )
    {
            // Read encoder and other data from MD49
            // *************************************
            read_MD49_Data();

            // Set speed left and right for MD49
            // ********************************
            if (cmd_vel_received==true) {
                set_MD49_speed(speed_l,speed_r);
                //set_MD49_speed(speed_l,speed_r);
                cmd_vel_received=false;
            }
            //set_MD49_speed(speed_l,speed_r);

            // Publish encoder values to topic /encoders (custom message)
            // ********************************************************************
            base_controller::encoders encoders;
            encoders.encoder_l=EncoderL;
            encoders.encoder_r=EncoderR;
            encoders_pub.publish(encoders);

            // Loop
            // ****
            ros::spinOnce();
            loop_rate.sleep();
    }// end.mainloop

    return 1;
} // end.main

int openSerialPort(const char * device, int bps){
   struct termios neu;
   char buf[128];

   //fd = open(device, O_RDWR | O_NOCTTY | O_NDELAY | O_NONBLOCK);
   fd = open(device, O_RDWR | O_NOCTTY);

   if (fd == -1)
   {
      sprintf(buf, "openSerialPort %s error", device);
      perror(buf);
   }
   else
   {
      tcgetattr(fd, &orig); 						/* save current serial settings */
      tcgetattr(fd, &neu);
      cfmakeraw(&neu);
      //fprintf(stderr, "speed=%d\n", bps);
      cfsetispeed(&neu, bps);
      cfsetospeed(&neu, bps);
      tcsetattr(fd, TCSANOW, &neu); 				/* set new serial settings */
      fcntl (fd, F_SETFL, O_RDWR);
   }
   return fd;
}

void writeBytes(int descriptor, int count) {
    if ((write(descriptor, serialBuffer, count)) == -1) {	// Send data out
        perror("Error writing");
        close(descriptor);				// Close port if there is an error
        exit(1);
    }
    //write(fd,serialBuffer, count);

}

void readBytes(int descriptor, int count) {
    if (read(descriptor, serialBuffer, count) == -1) {	// Read back data into buf[]
        perror("Error reading ");
        close(descriptor);				// Close port if there is an error
        exit(1);
    }
}

void read_MD49_Data (void){
    serialBuffer[0] = 82;							// 82=R Steuerbyte um alle Daten vom MD49 zu lesen
    writeBytes(fd, 1);
    //Daten lesen und in Array schreiben
    readBytes(fd, 18);

    printf("\033[2J");        /*  clear the screen  */
    printf("\033[H");         /*  position cursor at top-left corner */
    printf ("MD49-Data read from AVR-Master: \n");
    printf("====================================================== \n");
    //printf("Encoder1 Byte1: %i ",serialBuffer[0]);
    //printf("Byte2: %i ",serialBuffer[1]);
    //printf("Byte3: % i ",serialBuffer[2]);
    //printf("Byte4: %i \n",serialBuffer[3]);
    //printf("Encoder2 Byte1: %i ",serialBuffer[4]);
    //printf("Byte2: %i ",serialBuffer[5]);
    //printf("Byte3: %i ",serialBuffer[6]);
    //printf("Byte4: %i \n",serialBuffer[7]);
    printf("EncoderL: %i ",EncoderL);
    printf("EncoderR: %i \n",EncoderR);
    printf("====================================================== \n");
    printf("Speed1: %i ",serialBuffer[8]);
    printf("Speed2: %i \n",serialBuffer[9]);
    printf("Volts: %i \n",serialBuffer[10]);
    printf("Current1: %i ",serialBuffer[11]);
    printf("Current2: %i \n",serialBuffer[12]);
    printf("Error: %i \n",serialBuffer[13]);
    printf("Acceleration: %i \n",serialBuffer[14]);
    printf("Mode: %i \n",serialBuffer[15]);
    printf("Regulator: %i \n",serialBuffer[16]);
    printf("Timeout: %i \n",serialBuffer[17]);

    printf("vl= %f \n", vl);
    printf("vr= %f \n", vr);
    EncoderL = serialBuffer[0] << 24;                        // Put together first encoder value
    EncoderL |= (serialBuffer[1] << 16);
    EncoderL |= (serialBuffer[2] << 8);
    EncoderL |= (serialBuffer[3]);
    EncoderR = serialBuffer[4] << 24;                        // Put together second encoder value
    EncoderR |= (serialBuffer[5] << 16);
    EncoderR |= (serialBuffer[6] << 8);
    EncoderR |= (serialBuffer[7]);
}

void set_MD49_speed (unsigned char speed_l, unsigned char speed_r){
    serialBuffer[0] = 88;					// 88 =X Steuerbyte um Commands an MD49 zu senden
    serialBuffer[1] = 115;					// 115=s Steuerbyte setSpeed
    serialBuffer[2] = speed_l;					// speed1
    serialBuffer[3] = speed_r;					// speed2
    writeBytes(fd, 4);
}
<|endoftext|>


<commit_before>/* <x0/HttpConnection.cpp>
 *
 * This file is part of the x0 web server project and is released under LGPL-3.
 * http://www.xzero.ws/
 *
 * (c) 2009-2010 Christian Parpart <trapni@gentoo.org>
 */

#include <x0/http/HttpConnection.h>
#include <x0/http/HttpListener.h>
#include <x0/http/HttpRequest.h>
#include <x0/SocketDriver.h>
#include <x0/StackTrace.h>
#include <x0/Socket.h>
#include <x0/Types.h>
#include <x0/sysconfig.h>

#include <functional>

#include <sys/socket.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <arpa/inet.h>

#if !defined(NDEBUG)
#	define TRACE(msg...) this->debug(msg)
#else
#	define TRACE(msg...) do { } while (0)
#endif

#define X0_HTTP_STRICT 1

namespace x0 {

/**
 * \class HttpConnection
 * \brief represents an HTTP connection handling incoming requests.
 *
 * The \p HttpConnection object is to be allocated once an HTTP client connects
 * to the HTTP server and was accepted by the \p HttpListener.
 * It will also own the respective request and response objects created to serve
 * the requests passed through this connection.
 */

/** initializes a new connection object, created by given listener.
 * \param lst the listener object that created this connection.
 * \note This triggers the onConnectionOpen event.
 */
HttpConnection::HttpConnection(HttpListener& lst, HttpWorker& w, int fd) :
	HttpMessageProcessor(HttpMessageProcessor::REQUEST),
	secure(false),
	listener_(lst),
	worker_(w),
	socket_(0),
	active_(true), // when this is constricuted, it *must* be active right now :)
	buffer_(8192),
	offset_(0),
	request_count_(0),
	request_(0),
	abortHandler_(nullptr),
	abortData_(nullptr),
	source_(),
	sink_(nullptr),
	onWriteComplete_(),
	bytesTransferred_(0)
#if !defined(NDEBUG)
	, ctime_(ev_now(loop()))
#endif
{
	socket_ = listener_.socketDriver()->create(loop(), fd, lst.addressFamily());
	sink_.setSocket(socket_);

#if !defined(NDEBUG)
	debug(true);
	static std::atomic<unsigned long long> id(0);
	setLoggingPrefix("Connection[%d,%s:%d]", ++id, remoteIP().c_str(), remotePort());
#endif

	TRACE("fd=%d", socket_->handle());

#if defined(TCP_NODELAY)
	if (worker_.server_.tcp_nodelay())
		socket_->setTcpNoDelay(true);
#endif

	worker_.server_.onConnectionOpen(this);
}

/** releases all connection resources  and triggers the onConnectionClose event.
 */
HttpConnection::~HttpConnection()
{
	delete request_;
	request_ = nullptr;

	clearCustomData();

	TRACE("destructing");
	//TRACE("Stack Trace:\n%s", StackTrace().c_str());

	worker_.release(this);

	try
	{
		worker_.server_.onConnectionClose(this); // we cannot pass a shared pointer here as use_count is already zero and it would just lead into an exception though
	}
	catch (...)
	{
		TRACE("unexpected exception");
	}

	if (socket_)
	{
		delete socket_;
#if !defined(NDEBUG)
		socket_ = 0;
#endif
	}
}

void HttpConnection::io(Socket *, int revents)
{
	TRACE("io(mode=%s)", socket_->mode_str());
	active_ = true;

	if (revents & Socket::Read)
		processInput();

	if (revents & Socket::Write)
		processOutput();

	if (isClosed())
		delete this;
	else
		active_ = false;
}

void HttpConnection::timeout(Socket *)
{
	TRACE("timed out");

//	ev_unloop(loop(), EVUNLOOP_ONE);

	delete this;
}

#if defined(WITH_SSL)
bool HttpConnection::isSecure() const
{
	return listener_.isSecure();
}
#endif

/** start first async operation for this HttpConnection.
 *
 * This is done by simply registering the underlying socket to the the I/O service
 * to watch for available input.
 * \see stop()
 * :
 */
void HttpConnection::start()
{
	if (socket_->state() == Socket::Handshake)
	{
		TRACE("start: handshake.");
		socket_->handshake<HttpConnection, &HttpConnection::handshakeComplete>(this);
	}
	else
	{
#if defined(TCP_DEFER_ACCEPT)
		TRACE("start: processing input");
		// it is ensured, that we have data pending, so directly start reading
		processInput();
		TRACE("start: processing input done");

		// destroy connection in case the above caused connection-close
		// XXX this is usually done within HttpConnection::io(), but we are not.
		if (isClosed())
			delete this;
		else
			active_ = false;
#else
		TRACE("start: startRead.");
		// client connected, but we do not yet know if we have data pending
		startRead();
#endif
	}
}

void HttpConnection::handshakeComplete(Socket *)
{
	TRACE("handshakeComplete() socketState=%s", socket_->state_str());

	if (socket_->state() == Socket::Operational)
		startRead();
	else
	{
		TRACE("handshakeComplete(): handshake failed\n%s", StackTrace().c_str());
		close();
	}
}

inline bool url_decode(BufferRef& url)
{
	std::size_t left = url.offset();
	std::size_t right = left + url.size();
	std::size_t i = left; // read pos
	std::size_t d = left; // write pos
	Buffer& value = url.buffer();

	while (i != right)
	{
		if (value[i] == '%')
		{
			if (i + 3 <= right)
			{
				int ival;
				if (hex2int(value.begin() + i + 1, value.begin() + i + 3, ival))
				{
					value[d++] = static_cast<char>(ival);
					i += 3;
				}
				else
				{
					return false;
				}
			}
			else
			{
				return false;
			}
		}
		else if (value[i] == '+')
		{
			value[d++] = ' ';
			++i;
		}
		else if (d != i)
		{
			value[d++] = value[i++];
		}
		else
		{
			++d;
			++i;
		}
	}

	url = value.ref(left, d - left);
	return true;
}

void HttpConnection::messageBegin(BufferRef&& method, BufferRef&& uri, int version_major, int version_minor)
{
	TRACE("messageBegin('%s', '%s', HTTP/%d.%d)", method.str().c_str(), uri.str().c_str(), version_major, version_minor);

	request_ = new HttpRequest(*this);

	request_->method = std::move(method);

	request_->uri = std::move(uri);
	url_decode(request_->uri);

	std::size_t n = request_->uri.find("?");
	if (n != std::string::npos)
	{
		request_->path = request_->uri.ref(0, n);
		request_->query = request_->uri.ref(n + 1);
	}
	else
	{
		request_->path = request_->uri;
	}

	request_->httpVersionMajor = version_major;
	request_->httpVersionMinor = version_minor;
}

void HttpConnection::messageHeader(BufferRef&& name, BufferRef&& value)
{
	if (iequals(name, "Host"))
	{
		auto i = value.find(':');
		if (i != BufferRef::npos)
			request_->hostname = value.ref(0, i);
		else
			request_->hostname = value;
	}

	request_->requestHeaders.push_back(HttpRequestHeader(std::move(name), std::move(value)));
}

bool HttpConnection::messageHeaderEnd()
{
	TRACE("messageHeaderEnd()");

#if X0_HTTP_STRICT
	BufferRef expectHeader = request_->requestHeader("Expect");
	bool content_required = request_->method == "POST" || request_->method == "PUT";

	if (content_required && request_->connection.contentLength() == -1)
	{
		request_->status = HttpError::LengthRequired;
		request_->finish();
	}
	else if (!content_required && request_->contentAvailable())
	{
		request_->status = HttpError::BadRequest; // FIXME do we have a better status code?
		request_->finish();
	}
	else if (expectHeader)
	{
		request_->expectingContinue = equals(expectHeader, "100-continue");

		if (!request_->expectingContinue || !request_->supportsProtocol(1, 1))
		{
			request_->status = HttpError::ExpectationFailed;
			request_->finish();
		}
		else
			worker_.handleRequest(request_);
	}
	else
		worker_.handleRequest(request_);
#else
	worker_.handleRequest(request_);
#endif

	return true;
}

bool HttpConnection::messageContent(BufferRef&& chunk)
{
	TRACE("messageContent(#%ld)", chunk.size());

	if (request_)
		request_->onRequestContent(std::move(chunk));

	return true;
}

bool HttpConnection::messageEnd()
{
	TRACE("messageEnd()");

	// increment the number of fully processed requests
	++request_count_;

	// XXX is this really required? (meant to mark the request-content EOS)
	if (request_)
		request_->onRequestContent(BufferRef());

	// allow continueing processing possible further requests
	return true;
}

/** Resumes processing the <b>next</b> HTTP request message within this connection.
 *
 * This method may only be invoked when the current/old request has been fully processed,
 * and is though only be invoked from within the finish handler of \p HttpRequest.
 *
 * \see HttpRequest::finish()
 */
void HttpConnection::resume()
{
	if (socket()->tcpCork())
		socket()->setTcpCork(false);

	assert(request_ != nullptr);
	delete request_;
	request_ = nullptr;

	// wait for new request message, if nothing in buffer
	if (offset_ == buffer_.size())
		startRead();
}

void HttpConnection::startRead()
{
	int timeout = request_count_ && state() == MESSAGE_BEGIN
		? worker_.server_.max_keep_alive_idle()
		: worker_.server_.max_read_idle();

	if (timeout > 0)
		socket_->setTimeout<HttpConnection, &HttpConnection::timeout>(this, timeout);

	socket_->setReadyCallback<HttpConnection, &HttpConnection::io>(this);
	socket_->setMode(Socket::Read);
}

/**
 * This method gets invoked when there is data in our HttpConnection ready to read.
 *
 * We assume, that we are in request-parsing state.
 */
void HttpConnection::processInput()
{
	TRACE("processInput()");

	ssize_t rv = socket_->read(buffer_);

	if (rv < 0) // error
	{
		if (errno == EAGAIN || errno == EINTR)
		{
			startRead();
			ev_unloop(loop(), EVUNLOOP_ONE);
		}
		else
		{
			TRACE("processInput(): %s", strerror(errno));
			close();
		}
	}
	else if (rv == 0) // EOF
	{
		TRACE("processInput(): (EOF)");

		if (abortHandler_) {
			socket_->setMode(Socket::None);
			abortHandler_(abortData_);
		} else
			close();
	}
	else
	{
		TRACE("processInput(): read %ld bytes", rv);
		TRACE("%s", buffer_.ref(buffer_.size() - rv).str().c_str());

		process();

		TRACE("processInput(): done process()ing; mode=%s, fd=%d, request=%p",
			socket_->mode_str(), socket_->handle(), request_);
	}
}

/**
 * Writes as much as it wouldn't block of the response stream into the underlying socket.
 *
 */
void HttpConnection::processOutput()
{
	TRACE("processOutput()");

	for (;;)
	{
		ssize_t rv = source_.sendto(sink_);

		TRACE("processOutput(): sendto().rv=%ld %s", rv, rv < 0 ? strerror(errno) : "");
		// TODO make use of source_->eof()

		if (rv > 0) // source (partially?) written
		{
			bytesTransferred_ += rv;
		}
		else if (rv == 0) // source fully written
		{
			TRACE("processOutput(): source fully written");
			source_.reset();

			if (onWriteComplete_)
				onWriteComplete_(0, bytesTransferred_);

			//onWriteComplete_ = CompletionHandlerType();
			break;
		}
		else if (errno == EAGAIN || errno == EINTR) // completing write would block
		{
			socket_->setReadyCallback<HttpConnection, &HttpConnection::io>(this); // XXX should be same
			socket_->setMode(Socket::Write);
			break;
		}
		else // an error occurred
		{
			TRACE("processOutput(): write error (%d): %s", errno, strerror(errno));
			source_.reset();

			if (onWriteComplete_)
				onWriteComplete_(errno, bytesTransferred_);

			//onWriteComplete_ = CompletionHandlerType();
			close();
			break;
		}
	}
}

/** closes this HttpConnection, possibly deleting this object (or propagating delayed delete).
 */
void HttpConnection::close()
{
	TRACE("(%p).close() (active=%d)", this, active_);
	//TRACE("Stack Trace:%s\n", StackTrace().c_str());

	socket_->close();

	if (request_) {
		delete request_;
		request_ = nullptr;
	}

	if (!active_) {
		delete this;
	}
}

/** processes a (partial) request from buffer's given \p offset of \p count bytes.
 */
void HttpConnection::process()
{
	TRACE("process: offset=%ld, size=%ld (before processing)", offset_, buffer_.size());

	HttpMessageError ec = HttpMessageProcessor::process(
			buffer_.ref(offset_, buffer_.size() - offset_),
			offset_);

	TRACE("process: offset=%ld, bs=%ld, ec=%s, state=%s (after processing)",
			offset_, buffer_.size(), ec.message().c_str(), state_str());

#if 0
	if (state() == HttpMessageProcessor::MESSAGE_BEGIN) {
		// TODO reenable buffer reset (or reuse another for content! to be more huge-body friendly)
		offset_ = 0;
		buffer_.clear();
	}
#endif

	if (ec == HttpMessageError::Partial) {
		startRead();
	else if (!request_)
		return;
	else if (ec && ec != HttpMessageError::Aborted)
	{
		// -> send stock response: BAD_REQUEST
		request_->status = HttpError::BadRequest;
		request_->finish();
	}
}

std::string HttpConnection::remoteIP() const
{
	return socket_->remoteIP();
}

unsigned int HttpConnection::remotePort() const
{
	return socket_->remotePort();
}

std::string HttpConnection::localIP() const
{
	return listener_.address();
}

unsigned int HttpConnection::localPort() const
{
	return socket_->localPort();
}

} // namespace x0
<commit_msg>[http] HttpConnection: fixes crash on HTTP request syntax errors.<commit_after>/* <x0/HttpConnection.cpp>
 *
 * This file is part of the x0 web server project and is released under LGPL-3.
 * http://www.xzero.ws/
 *
 * (c) 2009-2010 Christian Parpart <trapni@gentoo.org>
 */

#include <x0/http/HttpConnection.h>
#include <x0/http/HttpListener.h>
#include <x0/http/HttpRequest.h>
#include <x0/SocketDriver.h>
#include <x0/StackTrace.h>
#include <x0/Socket.h>
#include <x0/Types.h>
#include <x0/sysconfig.h>

#include <functional>

#include <sys/socket.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <arpa/inet.h>

#if !defined(NDEBUG)
#	define TRACE(msg...) this->debug(msg)
#else
#	define TRACE(msg...) do { } while (0)
#endif

#define X0_HTTP_STRICT 1

namespace x0 {

/**
 * \class HttpConnection
 * \brief represents an HTTP connection handling incoming requests.
 *
 * The \p HttpConnection object is to be allocated once an HTTP client connects
 * to the HTTP server and was accepted by the \p HttpListener.
 * It will also own the respective request and response objects created to serve
 * the requests passed through this connection.
 */

/** initializes a new connection object, created by given listener.
 * \param lst the listener object that created this connection.
 * \note This triggers the onConnectionOpen event.
 */
HttpConnection::HttpConnection(HttpListener& lst, HttpWorker& w, int fd) :
	HttpMessageProcessor(HttpMessageProcessor::REQUEST),
	secure(false),
	listener_(lst),
	worker_(w),
	socket_(0),
	active_(true), // when this is constricuted, it *must* be active right now :)
	buffer_(8192),
	offset_(0),
	request_count_(0),
	request_(0),
	abortHandler_(nullptr),
	abortData_(nullptr),
	source_(),
	sink_(nullptr),
	onWriteComplete_(),
	bytesTransferred_(0)
#if !defined(NDEBUG)
	, ctime_(ev_now(loop()))
#endif
{
	socket_ = listener_.socketDriver()->create(loop(), fd, lst.addressFamily());
	sink_.setSocket(socket_);

#if !defined(NDEBUG)
	debug(false);
	static std::atomic<unsigned long long> id(0);
	setLoggingPrefix("Connection[%d,%s:%d]", ++id, remoteIP().c_str(), remotePort());
#endif

	TRACE("fd=%d", socket_->handle());

#if defined(TCP_NODELAY)
	if (worker_.server_.tcp_nodelay())
		socket_->setTcpNoDelay(true);
#endif

	worker_.server_.onConnectionOpen(this);
}

/** releases all connection resources  and triggers the onConnectionClose event.
 */
HttpConnection::~HttpConnection()
{
	delete request_;
	request_ = nullptr;

	clearCustomData();

	TRACE("destructing");
	//TRACE("Stack Trace:\n%s", StackTrace().c_str());

	worker_.release(this);

	try
	{
		worker_.server_.onConnectionClose(this); // we cannot pass a shared pointer here as use_count is already zero and it would just lead into an exception though
	}
	catch (...)
	{
		TRACE("unexpected exception");
	}

	if (socket_)
	{
		delete socket_;
#if !defined(NDEBUG)
		socket_ = 0;
#endif
	}
}

void HttpConnection::io(Socket *, int revents)
{
	TRACE("io(mode=%s)", socket_->mode_str());
	active_ = true;

	if (revents & Socket::Read)
		processInput();

	if (revents & Socket::Write)
		processOutput();

	if (isClosed())
		delete this;
	else
		active_ = false;
}

void HttpConnection::timeout(Socket *)
{
	TRACE("timed out");

//	ev_unloop(loop(), EVUNLOOP_ONE);

	delete this;
}

#if defined(WITH_SSL)
bool HttpConnection::isSecure() const
{
	return listener_.isSecure();
}
#endif

/** start first async operation for this HttpConnection.
 *
 * This is done by simply registering the underlying socket to the the I/O service
 * to watch for available input.
 * \see stop()
 * :
 */
void HttpConnection::start()
{
	if (socket_->state() == Socket::Handshake)
	{
		TRACE("start: handshake.");
		socket_->handshake<HttpConnection, &HttpConnection::handshakeComplete>(this);
	}
	else
	{
#if defined(TCP_DEFER_ACCEPT)
		TRACE("start: processing input");
		// it is ensured, that we have data pending, so directly start reading
		processInput();
		TRACE("start: processing input done");

		// destroy connection in case the above caused connection-close
		// XXX this is usually done within HttpConnection::io(), but we are not.
		if (isClosed())
			delete this;
		else
			active_ = false;
#else
		TRACE("start: startRead.");
		// client connected, but we do not yet know if we have data pending
		startRead();
#endif
	}
}

void HttpConnection::handshakeComplete(Socket *)
{
	TRACE("handshakeComplete() socketState=%s", socket_->state_str());

	if (socket_->state() == Socket::Operational)
		startRead();
	else
	{
		TRACE("handshakeComplete(): handshake failed\n%s", StackTrace().c_str());
		close();
	}
}

inline bool url_decode(BufferRef& url)
{
	std::size_t left = url.offset();
	std::size_t right = left + url.size();
	std::size_t i = left; // read pos
	std::size_t d = left; // write pos
	Buffer& value = url.buffer();

	while (i != right)
	{
		if (value[i] == '%')
		{
			if (i + 3 <= right)
			{
				int ival;
				if (hex2int(value.begin() + i + 1, value.begin() + i + 3, ival))
				{
					value[d++] = static_cast<char>(ival);
					i += 3;
				}
				else
				{
					return false;
				}
			}
			else
			{
				return false;
			}
		}
		else if (value[i] == '+')
		{
			value[d++] = ' ';
			++i;
		}
		else if (d != i)
		{
			value[d++] = value[i++];
		}
		else
		{
			++d;
			++i;
		}
	}

	url = value.ref(left, d - left);
	return true;
}

void HttpConnection::messageBegin(BufferRef&& method, BufferRef&& uri, int version_major, int version_minor)
{
	TRACE("messageBegin('%s', '%s', HTTP/%d.%d)", method.str().c_str(), uri.str().c_str(), version_major, version_minor);

	request_ = new HttpRequest(*this);

	request_->method = std::move(method);

	request_->uri = std::move(uri);
	url_decode(request_->uri);

	std::size_t n = request_->uri.find("?");
	if (n != std::string::npos)
	{
		request_->path = request_->uri.ref(0, n);
		request_->query = request_->uri.ref(n + 1);
	}
	else
	{
		request_->path = request_->uri;
	}

	request_->httpVersionMajor = version_major;
	request_->httpVersionMinor = version_minor;
}

void HttpConnection::messageHeader(BufferRef&& name, BufferRef&& value)
{
	if (iequals(name, "Host"))
	{
		auto i = value.find(':');
		if (i != BufferRef::npos)
			request_->hostname = value.ref(0, i);
		else
			request_->hostname = value;
	}

	request_->requestHeaders.push_back(HttpRequestHeader(std::move(name), std::move(value)));
}

bool HttpConnection::messageHeaderEnd()
{
	TRACE("messageHeaderEnd()");

#if X0_HTTP_STRICT
	BufferRef expectHeader = request_->requestHeader("Expect");
	bool content_required = request_->method == "POST" || request_->method == "PUT";

	if (content_required && request_->connection.contentLength() == -1)
	{
		request_->status = HttpError::LengthRequired;
		request_->finish();
	}
	else if (!content_required && request_->contentAvailable())
	{
		request_->status = HttpError::BadRequest; // FIXME do we have a better status code?
		request_->finish();
	}
	else if (expectHeader)
	{
		request_->expectingContinue = equals(expectHeader, "100-continue");

		if (!request_->expectingContinue || !request_->supportsProtocol(1, 1))
		{
			request_->status = HttpError::ExpectationFailed;
			request_->finish();
		}
		else
			worker_.handleRequest(request_);
	}
	else
		worker_.handleRequest(request_);
#else
	worker_.handleRequest(request_);
#endif

	return true;
}

bool HttpConnection::messageContent(BufferRef&& chunk)
{
	TRACE("messageContent(#%ld)", chunk.size());

	if (request_)
		request_->onRequestContent(std::move(chunk));

	return true;
}

bool HttpConnection::messageEnd()
{
	TRACE("messageEnd()");

	// increment the number of fully processed requests
	++request_count_;

	// XXX is this really required? (meant to mark the request-content EOS)
	if (request_)
		request_->onRequestContent(BufferRef());

	// allow continueing processing possible further requests
	return true;
}

/** Resumes processing the <b>next</b> HTTP request message within this connection.
 *
 * This method may only be invoked when the current/old request has been fully processed,
 * and is though only be invoked from within the finish handler of \p HttpRequest.
 *
 * \see HttpRequest::finish()
 */
void HttpConnection::resume()
{
	if (socket()->tcpCork())
		socket()->setTcpCork(false);

	assert(request_ != nullptr);
	delete request_;
	request_ = nullptr;

	// wait for new request message, if nothing in buffer
	if (offset_ == buffer_.size())
		startRead();
}

void HttpConnection::startRead()
{
	int timeout = request_count_ && state() == MESSAGE_BEGIN
		? worker_.server_.max_keep_alive_idle()
		: worker_.server_.max_read_idle();

	if (timeout > 0)
		socket_->setTimeout<HttpConnection, &HttpConnection::timeout>(this, timeout);

	socket_->setReadyCallback<HttpConnection, &HttpConnection::io>(this);
	socket_->setMode(Socket::Read);
}

/**
 * This method gets invoked when there is data in our HttpConnection ready to read.
 *
 * We assume, that we are in request-parsing state.
 */
void HttpConnection::processInput()
{
	TRACE("processInput()");

	ssize_t rv = socket_->read(buffer_);

	if (rv < 0) // error
	{
		if (errno == EAGAIN || errno == EINTR)
		{
			startRead();
			ev_unloop(loop(), EVUNLOOP_ONE);
		}
		else
		{
			TRACE("processInput(): %s", strerror(errno));
			close();
		}
	}
	else if (rv == 0) // EOF
	{
		TRACE("processInput(): (EOF)");

		if (abortHandler_) {
			socket_->setMode(Socket::None);
			abortHandler_(abortData_);
		} else
			close();
	}
	else
	{
		TRACE("processInput(): read %ld bytes", rv);
		TRACE("%s", buffer_.ref(buffer_.size() - rv).str().c_str());

		process();

		TRACE("processInput(): done process()ing; mode=%s, fd=%d, request=%p",
			socket_->mode_str(), socket_->handle(), request_);
	}
}

/**
 * Writes as much as it wouldn't block of the response stream into the underlying socket.
 *
 */
void HttpConnection::processOutput()
{
	TRACE("processOutput()");

	for (;;)
	{
		ssize_t rv = source_.sendto(sink_);

		TRACE("processOutput(): sendto().rv=%ld %s", rv, rv < 0 ? strerror(errno) : "");
		// TODO make use of source_->eof()

		if (rv > 0) // source (partially?) written
		{
			bytesTransferred_ += rv;
		}
		else if (rv == 0) // source fully written
		{
			TRACE("processOutput(): source fully written");
			source_.reset();

			if (onWriteComplete_)
				onWriteComplete_(0, bytesTransferred_);

			//onWriteComplete_ = CompletionHandlerType();
			break;
		}
		else if (errno == EAGAIN || errno == EINTR) // completing write would block
		{
			socket_->setReadyCallback<HttpConnection, &HttpConnection::io>(this); // XXX should be same
			socket_->setMode(Socket::Write);
			break;
		}
		else // an error occurred
		{
			TRACE("processOutput(): write error (%d): %s", errno, strerror(errno));
			source_.reset();

			if (onWriteComplete_)
				onWriteComplete_(errno, bytesTransferred_);

			//onWriteComplete_ = CompletionHandlerType();
			close();
			break;
		}
	}
}

/** closes this HttpConnection, possibly deleting this object (or propagating delayed delete).
 */
void HttpConnection::close()
{
	TRACE("(%p).close() (active=%d)", this, active_);
	//TRACE("Stack Trace:%s\n", StackTrace().c_str());

	socket_->close();

	if (request_) {
		delete request_;
		request_ = nullptr;
	}

	if (!active_) {
		delete this;
	}
}

/** processes a (partial) request from buffer's given \p offset of \p count bytes.
 */
void HttpConnection::process()
{
	TRACE("process: offset=%ld, size=%ld (before processing)", offset_, buffer_.size());

	HttpMessageError ec = HttpMessageProcessor::process(
			buffer_.ref(offset_, buffer_.size() - offset_),
			offset_);

	TRACE("process: offset=%ld, bs=%ld, ec=%s, state=%s (after processing)",
			offset_, buffer_.size(), std::error_code(ec).message().c_str(), state_str());

	if (isClosed())
		return;

	if (state() == SYNTAX_ERROR) {
		if (!request_)
			// in case the syntax error occured already in the request line, no request object has been instanciated yet.
			request_ = new HttpRequest(*this);

		request_->status = HttpError::BadRequest;
		request_->finish();
		return;
	}

#if 0
	if (state() == HttpMessageProcessor::MESSAGE_BEGIN) {
		// TODO reenable buffer reset (or reuse another for content! to be more huge-body friendly)
		offset_ = 0;
		buffer_.clear();
	}
#endif

	if (ec == HttpMessageError::Partial) {
		startRead();
	}
}

std::string HttpConnection::remoteIP() const
{
	return socket_->remoteIP();
}

unsigned int HttpConnection::remotePort() const
{
	return socket_->remotePort();
}

std::string HttpConnection::localIP() const
{
	return listener_.address();
}

unsigned int HttpConnection::localPort() const
{
	return socket_->localPort();
}

} // namespace x0
<|endoftext|>


<commit_before>// Copyright (C) 2018 Egor Pugin <egor.pugin@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include <primitives/executor.h>

#include <primitives/debug.h>
#include <primitives/exceptions.h>
#include <primitives/thread.h>

#include <iostream>
#include <string>

#ifdef _WIN32
#include <windows.h>
#endif

//#include "log.h"
//DECLARE_STATIC_LOGGER(logger, "executor");

size_t get_max_threads(int N)
{
    return std::max<size_t>(N, std::thread::hardware_concurrency());
}

size_t select_number_of_threads(int N)
{
    if (N > 0)
        return N;
    N = std::thread::hardware_concurrency();
    if (N == 1)
        N = 2;
    else if (N <= 8)
        N += 2;
    else if (N <= 64)
        N += 4;
    else
        N += 8;
    return N;
}

SW_DEFINE_GLOBAL_STATIC_FUNCTION(Executor, getExecutor)

Executor::Executor(const std::string &name, size_t nThreads)
    : Executor(nThreads, name)
{
}

Executor::Executor(size_t nThreads, const std::string &name)
    : nThreads(nThreads)
{
    thread_pool.resize(nThreads);
    for (size_t i = 0; i < nThreads; i++)
    {
        thread_pool[i].t = make_thread([this, i, name = name]() mutable
        {
            run(i, name);
        });
    }
}

Executor::~Executor()
{
    join();
}

size_t Executor::get_n() const
{
    return thread_ids.find(std::this_thread::get_id())->second;
}

bool Executor::is_in_executor() const
{
    return thread_ids.find(std::this_thread::get_id()) != thread_ids.end();
}

bool Executor::try_run_one()
{
    auto t = try_pop();
    if (t)
        run_task(t);
    return !!t;
}

void Executor::run(size_t i, const std::string &name)
{
    auto n = name;
    if (!n.empty())
        n += " ";
    n += std::to_string(i);
    primitives::setThreadName(n);

    {
        std::unique_lock<std::mutex> lk(m);
        thread_ids[std::this_thread::get_id()] = i;
    }

    while (!stopped_)
    {
        if (!run_task())
            break;
    }
}

bool Executor::run_task()
{
    return run_task(get_n());
}

bool Executor::run_task(Task &t)
{
    run_task(get_n(), t);
    return true;
}

bool Executor::run_task(size_t i)
{
    auto t = get_task(i);

    // double check
    if (stopped_)
        return false;

    run_task(i, t);
    return true;
}

void Executor::run_task(size_t i, Task &t) noexcept
{
    primitives::ScopedThreadName stn(std::to_string(i) + " busy");

    auto &thr = thread_pool[i];
    t();
    thr.busy = false;
}

Task Executor::get_task()
{
    return get_task(get_n());
}

Task Executor::get_task(size_t i)
{
    auto &thr = thread_pool[i];
    Task t = try_pop(i);
    // no task popped, probably shutdown command was issues
    if (!t && !thr.q.pop(t, thr.busy))
        return Task();

    return t;
}

Task Executor::get_task_non_stealing()
{
    return get_task_non_stealing(get_n());
}

Task Executor::get_task_non_stealing(size_t i)
{
    auto &thr = thread_pool[i];
    Task t;
    if (thr.q.pop(t, thr.busy))
        return t;
    return Task();
}

void Executor::join()
{
    wait();
    stop();
    for (auto &t : thread_pool)
    {
        if (t.t.joinable())
            t.t.join();
    }
}

void Executor::stop()
{
    stopped_ = true;
    for (auto &t : thread_pool)
        t.q.done();
}

void Executor::wait(WaitStatus p)
{
    std::unique_lock<std::mutex> lk(m_wait, std::try_to_lock);
    if (!lk.owns_lock())
    {
        std::unique_lock<std::mutex> lk(m_wait);
        return;
    }

    waiting_ = p;

    bool run_again = is_in_executor();

    // wait for empty queues
    for (auto &t : thread_pool)
    {
        while (!t.q.empty() && !stopped_)
        {
            if (run_again)
            {
                // finish everything
                for (auto &t : thread_pool)
                {
                    Task ta;
                    if (t.q.try_pop(ta, thread_pool[get_n()].busy))
                        run_task(get_n(), ta);
                }
            }
            else
                std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }
    }

    // free self
    if (run_again)
        thread_pool[get_n()].busy = false;

    // wait for end of execution
    for (auto &t : thread_pool)
    {
        while (t.busy && !stopped_)
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }

    waiting_ = WaitStatus::Running;
    cv_wait.notify_all();
}

void Executor::push(Task &&t)
{
    if (waiting_ == WaitStatus::RejectIncoming)
        throw SW_RUNTIME_ERROR("Executor is in the wait state and rejects new jobs");
    if (waiting_ == WaitStatus::BlockIncoming)
    {
        std::unique_lock<std::mutex> lk(m_wait);
        cv_wait.wait(lk, [this] { return waiting_ == WaitStatus::Running; });
    }

    auto i = index++;
    for (size_t n = 0; n != nThreads; ++n)
    {
        if (thread_pool[(i + n) % nThreads].q.try_push(std::move(t)))
            return;
    }
    thread_pool[i % nThreads].q.push(std::move(t));
}

Task Executor::try_pop()
{
    return try_pop(get_n());
}

Task Executor::try_pop(size_t i)
{
    Task t;
    const size_t spin_count = nThreads * 4;
    for (auto n = 0; n != spin_count; ++n)
    {
        if (thread_pool[(i + n) % nThreads].q.try_pop(t, thread_pool[i].busy))
            break;
    }
    return t;
}

bool Executor::empty() const
{
    return std::all_of(thread_pool.begin(), thread_pool.end(), [](const auto &t)
    {
        return t.empty();
    });
}
<commit_msg>[executor] Prevent data races on start. This was caused by containers access in threads.<commit_after>// Copyright (C) 2018 Egor Pugin <egor.pugin@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include <primitives/executor.h>

#include <primitives/debug.h>
#include <primitives/exceptions.h>
#include <primitives/thread.h>

#include <atomic>
#include <iostream>
#include <string>

#ifdef _WIN32
#include <windows.h>
#endif

//#include "log.h"
//DECLARE_STATIC_LOGGER(logger, "executor");

size_t get_max_threads(int N)
{
    return std::max<size_t>(N, std::thread::hardware_concurrency());
}

size_t select_number_of_threads(int N)
{
    if (N > 0)
        return N;
    N = std::thread::hardware_concurrency();
    if (N == 1)
        N = 2;
    else if (N <= 8)
        N += 2;
    else if (N <= 64)
        N += 4;
    else
        N += 8;
    return N;
}

SW_DEFINE_GLOBAL_STATIC_FUNCTION(Executor, getExecutor)

Executor::Executor(const std::string &name, size_t nThreads)
    : Executor(nThreads, name)
{
}

Executor::Executor(size_t nThreads, const std::string &name)
    : nThreads(nThreads)
{
    // we keep this lock until all threads created and assigned to thread_pool var
    // this is to prevent races on data objects
    std::unique_lock<std::mutex> lk(m);

    std::atomic<decltype(nThreads)> barrier{ 0 };

    thread_pool.resize(nThreads);
    for (size_t i = 0; i < nThreads; i++)
    {
        thread_pool[i].t = make_thread([this, i, name = name, &barrier, nThreads]() mutable
        {
            // set tids early
            {
                std::unique_lock<std::mutex> lk(m);
                thread_ids[std::this_thread::get_id()] = i;
                ++barrier;
            }

            // wait when all threads set their tids
            while (barrier != nThreads)
                std::this_thread::sleep_for(std::chrono::microseconds(1));

            // proceed
            run(i, name);
        });
    }

    // allow threads to run
    lk.unlock();

    // wait when all threads set their tids
    while (barrier != nThreads)
        std::this_thread::sleep_for(std::chrono::microseconds(1));
}

Executor::~Executor()
{
    join();
}

size_t Executor::get_n() const
{
    return thread_ids.find(std::this_thread::get_id())->second;
}

bool Executor::is_in_executor() const
{
    return thread_ids.find(std::this_thread::get_id()) != thread_ids.end();
}

bool Executor::try_run_one()
{
    auto t = try_pop();
    if (t)
        run_task(t);
    return !!t;
}

void Executor::run(size_t i, const std::string &name)
{
    auto n = name;
    if (!n.empty())
        n += " ";
    n += std::to_string(i);
    primitives::setThreadName(n);

    while (!stopped_)
    {
        if (!run_task())
            break;
    }
}

bool Executor::run_task()
{
    return run_task(get_n());
}

bool Executor::run_task(Task &t)
{
    run_task(get_n(), t);
    return true;
}

bool Executor::run_task(size_t i)
{
    auto t = get_task(i);

    // double check
    if (stopped_)
        return false;

    run_task(i, t);
    return true;
}

void Executor::run_task(size_t i, Task &t) noexcept
{
    primitives::ScopedThreadName stn(std::to_string(i) + " busy");

    auto &thr = thread_pool[i];
    t();
    thr.busy = false;
}

Task Executor::get_task()
{
    return get_task(get_n());
}

Task Executor::get_task(size_t i)
{
    auto &thr = thread_pool[i];
    Task t = try_pop(i);
    // no task popped, probably shutdown command was issues
    if (!t && !thr.q.pop(t, thr.busy))
        return Task();

    return t;
}

Task Executor::get_task_non_stealing()
{
    return get_task_non_stealing(get_n());
}

Task Executor::get_task_non_stealing(size_t i)
{
    auto &thr = thread_pool[i];
    Task t;
    if (thr.q.pop(t, thr.busy))
        return t;
    return Task();
}

void Executor::join()
{
    wait();
    stop();
    for (auto &t : thread_pool)
    {
        if (t.t.joinable())
            t.t.join();
    }
}

void Executor::stop()
{
    stopped_ = true;
    for (auto &t : thread_pool)
        t.q.done();
}

void Executor::wait(WaitStatus p)
{
    std::unique_lock<std::mutex> lk(m_wait, std::try_to_lock);
    if (!lk.owns_lock())
    {
        std::unique_lock<std::mutex> lk(m_wait);
        return;
    }

    waiting_ = p;

    bool run_again = is_in_executor();

    // wait for empty queues
    for (auto &t : thread_pool)
    {
        while (!t.q.empty() && !stopped_)
        {
            if (run_again)
            {
                // finish everything
                for (auto &t : thread_pool)
                {
                    Task ta;
                    if (t.q.try_pop(ta, thread_pool[get_n()].busy))
                        run_task(get_n(), ta);
                }
            }
            else
                std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }
    }

    // free self
    if (run_again)
        thread_pool[get_n()].busy = false;

    // wait for end of execution
    for (auto &t : thread_pool)
    {
        while (t.busy && !stopped_)
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }

    waiting_ = WaitStatus::Running;
    cv_wait.notify_all();
}

void Executor::push(Task &&t)
{
    if (waiting_ == WaitStatus::RejectIncoming)
        throw SW_RUNTIME_ERROR("Executor is in the wait state and rejects new jobs");
    if (waiting_ == WaitStatus::BlockIncoming)
    {
        std::unique_lock<std::mutex> lk(m_wait);
        cv_wait.wait(lk, [this] { return waiting_ == WaitStatus::Running; });
    }

    auto i = index++;
    for (size_t n = 0; n != nThreads; ++n)
    {
        if (thread_pool[(i + n) % nThreads].q.try_push(std::move(t)))
            return;
    }
    thread_pool[i % nThreads].q.push(std::move(t));
}

Task Executor::try_pop()
{
    return try_pop(get_n());
}

Task Executor::try_pop(size_t i)
{
    Task t;
    const size_t spin_count = nThreads * 4;
    for (auto n = 0; n != spin_count; ++n)
    {
        if (thread_pool[(i + n) % nThreads].q.try_pop(t, thread_pool[i].busy))
            break;
    }
    return t;
}

bool Executor::empty() const
{
    return std::all_of(thread_pool.begin(), thread_pool.end(), [](const auto &t)
    {
        return t.empty();
    });
}
<|endoftext|>


<commit_before>///
/// @file  balance_S2_load.cpp
/// @brief Dynamically increase or decrease the segment_size or the
///        segments_per_thread in order to improve the load balancing
///        in the computation of the special leaves. The algorithm
///        calculates the relative standard deviation of the timings
///        of the individual threads and then decides whether to
///        assign more work (low relative standard deviation) or less
///        work (large relative standard deviation) to the threads.
///
/// Copyright (C) 2014 Kim Walisch, <kim.walisch@gmail.com>
///
/// This file is distributed under the BSD License. See the COPYING
/// file in the top level directory.
///

#include <balance_S2_load.hpp>
#include <aligned_vector.hpp>
#include <pmath.hpp>

#include <stdint.h>
#include <algorithm>
#include <cmath>

using namespace std;
using namespace primecount;

namespace {

double get_average(aligned_vector<double>& timings)
{
  size_t n = timings.size();
  double sum = 0;

  for (size_t i = 0; i < n; i++)
    sum += timings[i];

  return sum / n;
}

double relative_standard_deviation(aligned_vector<double>& timings)
{
  size_t n = timings.size();
  double average = get_average(timings);
  double sum_mean_squared = 0;

  if (average == 0)
    return 0;

  for (size_t i = 0; i < n; i++)
  {
    double mean = timings[i] - average;
    sum_mean_squared += mean * mean;
  }

  double divisor = max(1.0, (double) n - 1);
  double standard_deviation = sqrt(sum_mean_squared / divisor);
  double rsd = 100 * standard_deviation / average;

  return rsd;
}

/// @param rsd  Relative standard deviation
bool increase_size(double rsd,
                   double old_rsd,
                   double seconds)
{
  return seconds < 10 && (seconds < 0.01 || rsd < old_rsd);
}

/// @param rsd  Relative standard deviation
bool decrease_size(double rsd,
                   double old_rsd,
                   double seconds)
{
  return seconds > 0.01 && rsd > old_rsd;
}

bool adjust_segments(double segments,
                     int64_t old_segments,
                     double seconds)
{
  return (segments < old_segments && seconds > 0.01) ||
         (segments > old_segments && seconds < 10);
}

} // namespace

namespace primecount {

/// Balance the load in the computation of the special leaves
/// by dynamically ajusting the segment_size and segments_per_thread.
/// @param old_rsd  Previous relative standard deviation.
/// @param timings  Timings of the threads.
///
void balance_S2_load(int64_t* segment_size,
                     int64_t* segments_per_thread,
                     int64_t min_segment_size,
                     int64_t max_segment_size,
                     double* old_rsd,
                     aligned_vector<double>& timings)
{
  double seconds = get_average(timings);
  double rsd = relative_standard_deviation(timings);

  rsd = in_between(max(1.0, *old_rsd / 5), rsd, *old_rsd * 5);

  if (*segment_size < max_segment_size)
  {
    if (increase_size(rsd, *old_rsd, seconds))
      *segment_size <<= 1;
    else if (decrease_size(rsd, *old_rsd, seconds))
      if (*segment_size > min_segment_size)
        *segment_size >>= 1;
  }
  else
  {
    double segments = max(1.0, *segments_per_thread * *old_rsd / rsd);
    if (adjust_segments(segments, *segments_per_thread, seconds))
      *segments_per_thread = (int) segments;
  }

  *old_rsd = rsd;
}

} //namespace
<commit_msg>Improve scaling<commit_after>///
/// @file  balance_S2_load.cpp
/// @brief Dynamically increase or decrease the segment_size or the
///        segments_per_thread in order to improve the load balancing
///        in the computation of the special leaves. The algorithm
///        calculates the relative standard deviation of the timings
///        of the individual threads and then decides whether to
///        assign more work (low relative standard deviation) or less
///        work (large relative standard deviation) to the threads.
///
/// Copyright (C) 2014 Kim Walisch, <kim.walisch@gmail.com>
///
/// This file is distributed under the BSD License. See the COPYING
/// file in the top level directory.
///

#include <balance_S2_load.hpp>
#include <aligned_vector.hpp>
#include <pmath.hpp>

#include <stdint.h>
#include <algorithm>
#include <cmath>

using namespace std;
using namespace primecount;

namespace {

double get_average(aligned_vector<double>& timings)
{
  size_t n = timings.size();
  double sum = 0;

  for (size_t i = 0; i < n; i++)
    sum += timings[i];

  return sum / n;
}

double relative_standard_deviation(aligned_vector<double>& timings)
{
  size_t n = timings.size();
  double average = get_average(timings);
  double sum_mean_squared = 0;

  if (average == 0)
    return 0;

  for (size_t i = 0; i < n; i++)
  {
    double mean = timings[i] - average;
    sum_mean_squared += mean * mean;
  }

  double divisor = max(1.0, (double) n - 1);
  double standard_deviation = sqrt(sum_mean_squared / divisor);
  double rsd = 100 * standard_deviation / average;

  return rsd;
}

/// @param rsd  Relative standard deviation
bool increase_size(double rsd,
                   double decrease_threshold,
                   double seconds)
{
  return seconds < 10 && (seconds < 0.01 || rsd < decrease_threshold);
}

/// @param rsd  Relative standard deviation
bool decrease_size(double rsd,
                   double decrease_threshold,
                   double seconds)
{
  return seconds > 0.01 && rsd > decrease_threshold;
}

bool adjust_segments(double segments,
                     int64_t old_segments,
                     double seconds)
{
  return (segments < old_segments && seconds > 0.01) ||
         (segments > old_segments && seconds < 10);
}

} // namespace

namespace primecount {

/// Balance the load in the computation of the special leaves
/// by dynamically adjusting the segment_size and segments_per_thread.
/// @param old_rsd  Previous relative standard deviation.
/// @param timings  Timings of the threads.
///
void balance_S2_load(int64_t* segment_size,
                     int64_t* segments_per_thread,
                     int64_t min_segment_size,
                     int64_t max_segment_size,
                     double* old_rsd,
                     aligned_vector<double>& timings)
{
  double seconds = get_average(timings);
  double rsd = max(0.1, relative_standard_deviation(timings));
  double num_threads = timings.size();
  double decrease_threshold = *old_rsd + min(log(num_threads), 1.0 / seconds);

  if (*segment_size < max_segment_size)
  {
    if (increase_size(rsd, decrease_threshold, seconds))
      *segment_size <<= 1;
    else if (decrease_size(rsd, decrease_threshold, seconds))
      if (*segment_size > min_segment_size)
        *segment_size >>= 1;
  }
  else
  {
    double factor = in_between(0.2, decrease_threshold / rsd, 10.0);
    double segments = max(1.0, *segments_per_thread * factor);

    if (adjust_segments(segments, *segments_per_thread, seconds))
      *segments_per_thread = (int) segments;
  }

  *old_rsd = rsd;
}

} //namespace
<|endoftext|>


<commit_before>/**************************************************************************
**
** This file is part of the Qt Build Suite
**
** Copyright (c) 2012 Nokia Corporation and/or its subsidiary(-ies).
**
** Contact: Nokia Corporation (info@qt.nokia.com)
**
**
** GNU Lesser General Public License Usage
**
** This file may be used under the terms of the GNU Lesser General Public
** License version 2.1 as published by the Free Software Foundation and
** appearing in the file LICENSE.LGPL included in the packaging of this file.
** Please review the following information to ensure the GNU Lesser General
** Public License version 2.1 requirements will be met:
** http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html.
**
** In addition, as a special exception, Nokia gives you certain additional
** rights. These rights are described in the Nokia Qt LGPL Exception
** version 1.1, included in the file LGPL_EXCEPTION.txt in this package.
**
** GNU General Public License Usage
** Alternatively, this file may be used under the terms of the GNU General
** Public License version 3.0 as published by the Free Software Foundation
** and appearing in the file LICENSE.GPL included in the packaging of this
** file.
** Please review the following information to ensure the GNU General
** Public License version 3.0 requirements will be met:
** http://www.gnu.org/copyleft/gpl.html.
**
** Other Usage
** Alternatively, this file may be used in accordance with the terms and
** conditions contained in a signed written agreement between you and Nokia.
**
**************************************************************************/

#include "fileinfo.h"
#include <QtCore/QCoreApplication>
#include <QDir>
#include <QFileInfo>
#include <QDebug>
#include <cassert>

#ifdef Q_OS_UNIX
#include <sys/stat.h>
#endif

namespace qbs {

QString FileInfo::fileName(const QString &fp)
{
    int last = fp.lastIndexOf('/');
    if (last < 0)
        return fp;
    return fp.mid(last + 1);
}

QString FileInfo::baseName(const QString &fp)
{
    QString fn = fileName(fp);
    int dot = fn.indexOf('.');
    if (dot < 0)
        return fp;
    return fn.mid(0, dot);
}

QString FileInfo::completeBaseName(const QString &fp)
{
    QString fn = fileName(fp);
    int dot = fn.lastIndexOf('.');
    if (dot < 0)
        return fp;
    return fn.mid(0, dot);
}

QString FileInfo::path(const QString &fp)
{
    if (fp.isEmpty())
        return QString();
    if (fp.at(fp.size() - 1) == '/')
        return fp;
    int last = fp.lastIndexOf('/');
    if (last < 0)
        return ".";
    return QDir::cleanPath(fp.mid(0, last));
}

bool FileInfo::exists(const QString &fp)
{
    return FileInfo(fp).exists();
}

// from creator/src/shared/proparser/ioutils.cpp
bool FileInfo::isAbsolute(const QString &path)
{
    if (path.startsWith(QLatin1Char('/')))
        return true;
#ifdef Q_OS_WIN
    if (path.startsWith(QLatin1Char('\\')))
        return true;
    // Unlike QFileInfo, this won't accept a relative path with a drive letter.
    // Such paths result in a royal mess anyway ...
    if (path.length() >= 3 && path.at(1) == QLatin1Char(':') && path.at(0).isLetter()
            && (path.at(2) == QLatin1Char('/') || path.at(2) == QLatin1Char('\\')))
        return true;
#endif
    return false;
}

QString FileInfo::resolvePath(const QString &base, const QString &rel)
{
    if (isAbsolute(rel))
        return rel;
    if (rel.size() == 1 && rel.at(0) == QLatin1Char('.'))
        return base;

    QString r = base;
    if (!r.endsWith(QLatin1Char('/')))
        r.append(QLatin1Char('/'));
    r.append(rel);

    return r;
}

bool FileInfo::globMatches(const QString &pattern, const QString &subject)
{
    // ### the QRegExp wildcard matcher is slow!
    //QRegExp rex(pattern, Qt::CaseSensitive, QRegExp::Wildcard);
    //return rex.exactMatch(subject);
    return subject.endsWith(pattern.mid(1), Qt::CaseInsensitive);
}

static QString resolveSymlinks(const QString &fileName)
{
    QFileInfo fi(fileName);
    while (fi.isSymLink())
        fi.setFile(fi.symLinkTarget());
    return fi.absoluteFilePath();
}

#if defined(Q_OS_WIN)

#include <qt_windows.h>

#define z(x) reinterpret_cast<WIN32_FILE_ATTRIBUTE_DATA*>(const_cast<FileInfo::InternalStatType*>(&x))

template<bool> struct CompileTimeAssert;
template<> struct CompileTimeAssert<true> {};

FileInfo::FileInfo(const QString &fileName)
{
    static CompileTimeAssert<
        sizeof(FileInfo::InternalStatType) == sizeof(WIN32_FILE_ATTRIBUTE_DATA)
            > internal_type_has_wrong_size;
    if (!GetFileAttributesEx(reinterpret_cast<const WCHAR*>(fileName.utf16()),
                             GetFileExInfoStandard, &m_stat))
    {
        z(m_stat)->dwFileAttributes = INVALID_FILE_ATTRIBUTES;
    }
}

bool FileInfo::exists() const
{
    return z(m_stat)->dwFileAttributes != INVALID_FILE_ATTRIBUTES;
}

FileTime FileInfo::lastModified() const
{
    return FileTime(*reinterpret_cast<const FileTime::InternalType*>(
        &z(m_stat)->ftLastWriteTime));
}

QString applicationDirPath()
{
    static const QString appDirPath = FileInfo::path(resolveSymlinks(QCoreApplication::applicationFilePath()));
    return appDirPath;
}

#elif defined(Q_OS_UNIX)

FileInfo::FileInfo(const QString &fileName)
{
    if (stat(fileName.toLocal8Bit(), &m_stat) == -1)
        m_stat.st_mtime = 0;
}

bool FileInfo::exists() const
{
    return m_stat.st_mtime != 0;
}

FileTime FileInfo::lastModified() const
{
    return m_stat.st_mtime;
}

QString applicationDirPath()
{
    return QCoreApplication::applicationDirPath();
}

#endif

// adapted from qtc/plugins/vcsbase/cleandialog.cpp
static bool removeFileRecursion(const QFileInfo &f, QString *errorMessage)
{
    if (!f.exists())
        return true;
    if (f.isDir()) {
        const QDir dir(f.absoluteFilePath());
        foreach(const QFileInfo &fi, dir.entryInfoList(QDir::AllEntries|QDir::NoDotAndDotDot|QDir::Hidden))
            removeFileRecursion(fi, errorMessage);
        QDir parent = f.absoluteDir();
        if (!parent.rmdir(f.fileName())) {
            errorMessage->append(FileInfo::tr("The directory %1 could not be deleted.").
                                 arg(QDir::toNativeSeparators(f.absoluteFilePath())));
            return false;
        }
    } else if (!QFile::remove(f.absoluteFilePath())) {
        if (!errorMessage->isEmpty())
            errorMessage->append(QLatin1Char('\n'));
        errorMessage->append(FileInfo::tr("The file %1 could not be deleted.").
                             arg(QDir::toNativeSeparators(f.absoluteFilePath())));
        return false;
    }
    return true;
}

bool removeDirectoryWithContents(const QString &path, QString *errorMessage)
{
    QFileInfo f(path);
    if (f.exists() && !f.isDir()) {
        *errorMessage = FileInfo::tr("%1 is not a directory.").arg(QDir::toNativeSeparators(path));
        return false;
    }
    return removeFileRecursion(f, errorMessage);
}

QString qbsRootPath()
{
    return QDir::cleanPath(applicationDirPath() + QLatin1String("/../"));
}

}
<commit_msg>fileinfo.cpp: a few micro optimnizations<commit_after>/**************************************************************************
**
** This file is part of the Qt Build Suite
**
** Copyright (c) 2012 Nokia Corporation and/or its subsidiary(-ies).
**
** Contact: Nokia Corporation (info@qt.nokia.com)
**
**
** GNU Lesser General Public License Usage
**
** This file may be used under the terms of the GNU Lesser General Public
** License version 2.1 as published by the Free Software Foundation and
** appearing in the file LICENSE.LGPL included in the packaging of this file.
** Please review the following information to ensure the GNU Lesser General
** Public License version 2.1 requirements will be met:
** http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html.
**
** In addition, as a special exception, Nokia gives you certain additional
** rights. These rights are described in the Nokia Qt LGPL Exception
** version 1.1, included in the file LGPL_EXCEPTION.txt in this package.
**
** GNU General Public License Usage
** Alternatively, this file may be used under the terms of the GNU General
** Public License version 3.0 as published by the Free Software Foundation
** and appearing in the file LICENSE.GPL included in the packaging of this
** file.
** Please review the following information to ensure the GNU General
** Public License version 3.0 requirements will be met:
** http://www.gnu.org/copyleft/gpl.html.
**
** Other Usage
** Alternatively, this file may be used in accordance with the terms and
** conditions contained in a signed written agreement between you and Nokia.
**
**************************************************************************/

#include "fileinfo.h"
#include <QtCore/QCoreApplication>
#include <QDir>
#include <QFileInfo>
#include <QDebug>
#include <cassert>

#ifdef Q_OS_UNIX
#include <sys/stat.h>
#endif

namespace qbs {

QString FileInfo::fileName(const QString &fp)
{
    int last = fp.lastIndexOf(QLatin1Char('/'));
    if (last < 0)
        return fp;
    return fp.mid(last + 1);
}

QString FileInfo::baseName(const QString &fp)
{
    QString fn = fileName(fp);
    int dot = fn.indexOf(QLatin1Char('.'));
    if (dot < 0)
        return fp;
    return fn.mid(0, dot);
}

QString FileInfo::completeBaseName(const QString &fp)
{
    QString fn = fileName(fp);
    int dot = fn.lastIndexOf(QLatin1Char('.'));
    if (dot < 0)
        return fp;
    return fn.mid(0, dot);
}

QString FileInfo::path(const QString &fp)
{
    if (fp.isEmpty())
        return QString();
    if (fp.at(fp.size() - 1) == QLatin1Char('/'))
        return fp;
    int last = fp.lastIndexOf(QLatin1Char('/'));
    if (last < 0)
        return QLatin1String(".");
    return QDir::cleanPath(fp.mid(0, last));
}

bool FileInfo::exists(const QString &fp)
{
    return FileInfo(fp).exists();
}

// from creator/src/shared/proparser/ioutils.cpp
bool FileInfo::isAbsolute(const QString &path)
{
    const int n = path.size();
    if (n == 0)
        return false;
    const QChar at0 = path.at(0);
    if (at0 == QLatin1Char('/'))
        return true;
#ifdef Q_OS_WIN
    if (at0 == QLatin1Char('\\'))
        return true;
    // Unlike QFileInfo, this won't accept a relative path with a drive letter.
    // Such paths result in a royal mess anyway ...
    if (n >= 3 && path.at(1) == QLatin1Char(':') && at0.isLetter()
            && (path.at(2) == QLatin1Char('/') || path.at(2) == QLatin1Char('\\')))
        return true;
#endif
    return false;
}

QString FileInfo::resolvePath(const QString &base, const QString &rel)
{
    if (isAbsolute(rel))
        return rel;
    if (rel.size() == 1 && rel.at(0) == QLatin1Char('.'))
        return base;

    QString r = base;
    if (!r.endsWith(QLatin1Char('/')))
        r.append(QLatin1Char('/'));
    r.append(rel);

    return r;
}

bool FileInfo::globMatches(const QString &pattern, const QString &subject)
{
    // ### the QRegExp wildcard matcher is slow!
    //QRegExp rex(pattern, Qt::CaseSensitive, QRegExp::Wildcard);
    //return rex.exactMatch(subject);
    return subject.endsWith(pattern.mid(1), Qt::CaseInsensitive);
}

static QString resolveSymlinks(const QString &fileName)
{
    QFileInfo fi(fileName);
    while (fi.isSymLink())
        fi.setFile(fi.symLinkTarget());
    return fi.absoluteFilePath();
}

#if defined(Q_OS_WIN)

#include <qt_windows.h>

#define z(x) reinterpret_cast<WIN32_FILE_ATTRIBUTE_DATA*>(const_cast<FileInfo::InternalStatType*>(&x))

template<bool> struct CompileTimeAssert;
template<> struct CompileTimeAssert<true> {};

FileInfo::FileInfo(const QString &fileName)
{
    static CompileTimeAssert<
        sizeof(FileInfo::InternalStatType) == sizeof(WIN32_FILE_ATTRIBUTE_DATA)
            > internal_type_has_wrong_size;
    if (!GetFileAttributesEx(reinterpret_cast<const WCHAR*>(fileName.utf16()),
                             GetFileExInfoStandard, &m_stat))
    {
        z(m_stat)->dwFileAttributes = INVALID_FILE_ATTRIBUTES;
    }
}

bool FileInfo::exists() const
{
    return z(m_stat)->dwFileAttributes != INVALID_FILE_ATTRIBUTES;
}

FileTime FileInfo::lastModified() const
{
    return FileTime(*reinterpret_cast<const FileTime::InternalType*>(
        &z(m_stat)->ftLastWriteTime));
}

QString applicationDirPath()
{
    static const QString appDirPath = FileInfo::path(resolveSymlinks(QCoreApplication::applicationFilePath()));
    return appDirPath;
}

#elif defined(Q_OS_UNIX)

FileInfo::FileInfo(const QString &fileName)
{
    if (stat(fileName.toLocal8Bit(), &m_stat) == -1)
        m_stat.st_mtime = 0;
}

bool FileInfo::exists() const
{
    return m_stat.st_mtime != 0;
}

FileTime FileInfo::lastModified() const
{
    return m_stat.st_mtime;
}

QString applicationDirPath()
{
    return QCoreApplication::applicationDirPath();
}

#endif

// adapted from qtc/plugins/vcsbase/cleandialog.cpp
static bool removeFileRecursion(const QFileInfo &f, QString *errorMessage)
{
    if (!f.exists())
        return true;
    if (f.isDir()) {
        const QDir dir(f.absoluteFilePath());
        foreach(const QFileInfo &fi, dir.entryInfoList(QDir::AllEntries|QDir::NoDotAndDotDot|QDir::Hidden))
            removeFileRecursion(fi, errorMessage);
        QDir parent = f.absoluteDir();
        if (!parent.rmdir(f.fileName())) {
            errorMessage->append(FileInfo::tr("The directory %1 could not be deleted.").
                                 arg(QDir::toNativeSeparators(f.absoluteFilePath())));
            return false;
        }
    } else if (!QFile::remove(f.absoluteFilePath())) {
        if (!errorMessage->isEmpty())
            errorMessage->append(QLatin1Char('\n'));
        errorMessage->append(FileInfo::tr("The file %1 could not be deleted.").
                             arg(QDir::toNativeSeparators(f.absoluteFilePath())));
        return false;
    }
    return true;
}

bool removeDirectoryWithContents(const QString &path, QString *errorMessage)
{
    QFileInfo f(path);
    if (f.exists() && !f.isDir()) {
        *errorMessage = FileInfo::tr("%1 is not a directory.").arg(QDir::toNativeSeparators(path));
        return false;
    }
    return removeFileRecursion(f, errorMessage);
}

QString qbsRootPath()
{
    return QDir::cleanPath(applicationDirPath() + QLatin1String("/../"));
}

}
<|endoftext|>


<commit_before>/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#include "velodyne_driver/input.h"

namespace apollo {
namespace drivers {
namespace velodyne {

bool Input::exract_nmea_time_from_packet(const NMEATimePtr& nmea_time,
                                         const uint8_t* bytes) {
  int gprmc_index = 206;

  int field_count = 0;
  int time_field_index = 0;
  int validity_field_index = 0;
  int date_field_index = 0;
  while (bytes[++gprmc_index] != '*') {
    if (bytes[gprmc_index] == ',') {
      ++field_count;
      if (field_count == 1 && time_field_index == 0) {
        time_field_index = gprmc_index + 1;
      } else if (field_count == 2 && validity_field_index == 0) {
        validity_field_index = gprmc_index + 1;
        if (bytes[validity_field_index] == 'V') {
          ROS_ERROR("NAV receiver warning, GPS info is invalid!");
          return false;
        }
      } else if (field_count == 9 && date_field_index == 0) {
        date_field_index = gprmc_index + 1;
        break;
      }
    }
  }

  nmea_time->year = (bytes[date_field_index + 4] - '0') * 10 +
                    (bytes[date_field_index + 5] - '0');
  nmea_time->mon = (bytes[date_field_index + 2] - '0') * 10 +
                   (bytes[date_field_index + 3] - '0');
  nmea_time->day = (bytes[date_field_index] - '0') * 10 +
                   (bytes[date_field_index + 1] - '0');
  nmea_time->hour = (bytes[time_field_index] - '0') * 10 +
                    (bytes[time_field_index + 1] - '0');
  nmea_time->min = (bytes[time_field_index + 2] - '0') * 10 +
                   (bytes[time_field_index + 3] - '0');
  nmea_time->sec = (bytes[time_field_index + 4] - '0') * 10 +
                   (bytes[time_field_index + 5] - '0');

  if (nmea_time->year < 0 || nmea_time->year > 99 || nmea_time->mon > 12 ||
      nmea_time->mon < 1 || nmea_time->day > 31 || nmea_time->day < 1 ||
      nmea_time->hour > 23 || nmea_time->hour < 0 || nmea_time->min > 59 ||
      nmea_time->min < 0 || nmea_time->sec > 59 || nmea_time->sec < 0) {
    ROS_ERROR(
        "Invalid GPS time:  %d-%d-%d %d:%d:%d, make sure have connected to GPS "
        "device",
        nmea_time->year, nmea_time->mon, nmea_time->day, nmea_time->hour,
        nmea_time->min, nmea_time->sec);
    return false;
  }
  return true;
}

}  // namespace velodyne
}  // namespace drivers
}  // namespace apollo
<commit_msg>drivers: velodyne fix index bounds.<commit_after>/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#include "velodyne_driver/input.h"

namespace apollo {
namespace drivers {
namespace velodyne {

bool Input::exract_nmea_time_from_packet(const NMEATimePtr& nmea_time,
                                         const uint8_t* bytes) {
  int gprmc_index = 206;

  int field_count = 0;
  int time_field_index = 0;
  int validity_field_index = 0;
  int date_field_index = 0;
  while (bytes[++gprmc_index] != '*' && gprmc_index < POSITIONING_DATA_PACKET_SIZE) {
    if (bytes[gprmc_index] == ',') {
      ++field_count;
      if (field_count == 1 && time_field_index == 0) {
        time_field_index = gprmc_index + 1;
      } else if (field_count == 2 && validity_field_index == 0) {
        validity_field_index = gprmc_index + 1;
        if (bytes[validity_field_index] == 'V') {
          ROS_ERROR("NAV receiver warning, GPS info is invalid!");
          return false;
        }
      } else if (field_count == 9 && date_field_index == 0) {
        date_field_index = gprmc_index + 1;
        break;
      }
    }
  }

  nmea_time->year = (bytes[date_field_index + 4] - '0') * 10 +
                    (bytes[date_field_index + 5] - '0');
  nmea_time->mon = (bytes[date_field_index + 2] - '0') * 10 +
                   (bytes[date_field_index + 3] - '0');
  nmea_time->day = (bytes[date_field_index] - '0') * 10 +
                   (bytes[date_field_index + 1] - '0');
  nmea_time->hour = (bytes[time_field_index] - '0') * 10 +
                    (bytes[time_field_index + 1] - '0');
  nmea_time->min = (bytes[time_field_index + 2] - '0') * 10 +
                   (bytes[time_field_index + 3] - '0');
  nmea_time->sec = (bytes[time_field_index + 4] - '0') * 10 +
                   (bytes[time_field_index + 5] - '0');

  if (nmea_time->year < 0 || nmea_time->year > 99 || nmea_time->mon > 12 ||
      nmea_time->mon < 1 || nmea_time->day > 31 || nmea_time->day < 1 ||
      nmea_time->hour > 23 || nmea_time->hour < 0 || nmea_time->min > 59 ||
      nmea_time->min < 0 || nmea_time->sec > 59 || nmea_time->sec < 0) {
    ROS_ERROR(
        "Invalid GPS time:  %d-%d-%d %d:%d:%d, make sure have connected to GPS "
        "device",
        nmea_time->year, nmea_time->mon, nmea_time->day, nmea_time->hour,
        nmea_time->min, nmea_time->sec);
    return false;
  }
  return true;
}

}  // namespace velodyne
}  // namespace drivers
}  // namespace apollo
<|endoftext|>


<commit_before>//* This file is part of the MOOSE framework
//* https://www.mooseframework.org
//*
//* All rights reserved, see COPYRIGHT for full restrictions
//* https://github.com/idaholab/moose/blob/master/COPYRIGHT
//*
//* Licensed under LGPL 2.1, please see LICENSE for details
//* https://www.gnu.org/licenses/lgpl-2.1.html

#include "INSFVAdvectionKernel.h"
#include "NS.h"
#include "MooseVariableFV.h"
#include "RelationshipManager.h"
#include "NSFVUtils.h"

InputParameters
INSFVAdvectionKernel::validParams()
{
  InputParameters params = FVFluxKernel::validParams();
  params += Moose::FV::interpolationParameters();
  params.addRequiredParam<UserObjectName>("rhie_chow_user_object", "The rhie-chow user-object");
  // We need 2 ghost layers for the Rhie-Chow interpolation
  params.set<unsigned short>("ghost_layers") = 2;
  params.addRequiredParam<UserObjectName>("rhie_chow_user_object", "The rhie-chow user-object");
  return params;
}

INSFVAdvectionKernel::INSFVAdvectionKernel(const InputParameters & params)
  : FVFluxKernel(params),
    _rc_vel_provider(getUserObject<INSFVRhieChowInterpolator>("rhie_chow_user_object"))
{
#ifndef MOOSE_GLOBAL_AD_INDEXING
  mooseError("INSFV is not supported by local AD indexing. In order to use INSFV, please run the "
             "configure script in the root MOOSE directory with the configure option "
             "'--with-ad-indexing-type=global'");
#endif
  const bool need_more_ghosting =
      Moose::FV::setInterpolationMethods(*this, _advected_interp_method, _velocity_interp_method);
  if (need_more_ghosting && _tid == 0)
  {
    auto & factory = _app.getFactory();

    auto rm_params = factory.getValidParams("ElementSideNeighborLayers");

    rm_params.set<std::string>("for_whom") = name();
    rm_params.set<MooseMesh *>("mesh") = &const_cast<MooseMesh &>(_mesh);
    rm_params.set<Moose::RelationshipManagerType>("rm_type") =
        Moose::RelationshipManagerType::GEOMETRIC | Moose::RelationshipManagerType::ALGEBRAIC |
        Moose::RelationshipManagerType::COUPLING;
    FVKernel::setRMParams(
        _pars, rm_params, std::max((unsigned short)(3), _pars.get<unsigned short>("ghost_layers")));
    mooseAssert(rm_params.areAllRequiredParamsValid(),
                "All relationship manager parameters should be valid.");

    auto rm_obj = factory.create<RelationshipManager>(
        "ElementSideNeighborLayers", name() + "_skew_correction", rm_params);

    // Delete the resources created on behalf of the RM if it ends up not being added to the
    // App.
    if (!_app.addRelationshipManager(rm_obj))
      factory.releaseSharedObjects(*rm_obj);

    // If we need more ghosting, then we are a second-order nonlinear limiting scheme whose stencil
    // is liable to change upon wind-direction change. Consequently we need to tell our problem that
    // it's ok to have new nonzeros which may crop-up after PETSc has shrunk the matrix memory
    getCheckedPointerParam<FEProblemBase *>("_fe_problem_base")
        ->setErrorOnJacobianNonzeroReallocation(false);
  }

  auto param_check = [&params, this](const auto & param_name)
  {
    if (params.isParamSetByUser(param_name))
      paramError(
          param_name, "This parameter is not honored by INSFVAdvectionKernels like '", name(), "'");
  };

  param_check("force_boundary_execution");
  param_check("boundaries_to_force");
}

void
INSFVAdvectionKernel::initialSetup()
{
  INSFVBCInterface::initialSetup(*this);
}

bool
INSFVAdvectionKernel::skipForBoundary(const FaceInfo & fi) const
{
  if (!onBoundary(fi))
    return false;

  // If we have flux bcs then we do skip
  const auto & flux_pr = _var.getFluxBCs(fi);
  if (flux_pr.first)
    return true;

  // If we have a flow boundary without a replacement flux BC, then we must not skip. Mass and
  // momentum are transported via advection across boundaries
  for (const auto bc_id : fi.boundaryIDs())
    if (_flow_boundaries.find(bc_id) != _flow_boundaries.end())
      return false;

  // If not a flow boundary, then there should be no advection/flow in the normal direction, e.g. we
  // should not contribute any advective flux
  return true;
}
<commit_msg>Use option to skip internal sidesets in INSFVAdvection refs #20695<commit_after>//* This file is part of the MOOSE framework
//* https://www.mooseframework.org
//*
//* All rights reserved, see COPYRIGHT for full restrictions
//* https://github.com/idaholab/moose/blob/master/COPYRIGHT
//*
//* Licensed under LGPL 2.1, please see LICENSE for details
//* https://www.gnu.org/licenses/lgpl-2.1.html

#include "INSFVAdvectionKernel.h"
#include "NS.h"
#include "MooseVariableFV.h"
#include "RelationshipManager.h"
#include "NSFVUtils.h"

InputParameters
INSFVAdvectionKernel::validParams()
{
  InputParameters params = FVFluxKernel::validParams();
  params += Moose::FV::interpolationParameters();
  params.addRequiredParam<UserObjectName>("rhie_chow_user_object", "The rhie-chow user-object");
  // We need 2 ghost layers for the Rhie-Chow interpolation
  params.set<unsigned short>("ghost_layers") = 2;
  params.addRequiredParam<UserObjectName>("rhie_chow_user_object", "The rhie-chow user-object");
  return params;
}

INSFVAdvectionKernel::INSFVAdvectionKernel(const InputParameters & params)
  : FVFluxKernel(params),
    _rc_vel_provider(getUserObject<INSFVRhieChowInterpolator>("rhie_chow_user_object"))
{
#ifndef MOOSE_GLOBAL_AD_INDEXING
  mooseError("INSFV is not supported by local AD indexing. In order to use INSFV, please run the "
             "configure script in the root MOOSE directory with the configure option "
             "'--with-ad-indexing-type=global'");
#endif
  const bool need_more_ghosting =
      Moose::FV::setInterpolationMethods(*this, _advected_interp_method, _velocity_interp_method);
  if (need_more_ghosting && _tid == 0)
  {
    auto & factory = _app.getFactory();

    auto rm_params = factory.getValidParams("ElementSideNeighborLayers");

    rm_params.set<std::string>("for_whom") = name();
    rm_params.set<MooseMesh *>("mesh") = &const_cast<MooseMesh &>(_mesh);
    rm_params.set<Moose::RelationshipManagerType>("rm_type") =
        Moose::RelationshipManagerType::GEOMETRIC | Moose::RelationshipManagerType::ALGEBRAIC |
        Moose::RelationshipManagerType::COUPLING;
    FVKernel::setRMParams(
        _pars, rm_params, std::max((unsigned short)(3), _pars.get<unsigned short>("ghost_layers")));
    mooseAssert(rm_params.areAllRequiredParamsValid(),
                "All relationship manager parameters should be valid.");

    auto rm_obj = factory.create<RelationshipManager>(
        "ElementSideNeighborLayers", name() + "_skew_correction", rm_params);

    // Delete the resources created on behalf of the RM if it ends up not being added to the
    // App.
    if (!_app.addRelationshipManager(rm_obj))
      factory.releaseSharedObjects(*rm_obj);

    // If we need more ghosting, then we are a second-order nonlinear limiting scheme whose stencil
    // is liable to change upon wind-direction change. Consequently we need to tell our problem that
    // it's ok to have new nonzeros which may crop-up after PETSc has shrunk the matrix memory
    getCheckedPointerParam<FEProblemBase *>("_fe_problem_base")
        ->setErrorOnJacobianNonzeroReallocation(false);
  }

  auto param_check = [&params, this](const auto & param_name)
  {
    if (params.isParamSetByUser(param_name))
      paramError(
          param_name, "This parameter is not honored by INSFVAdvectionKernels like '", name(), "'");
  };

  param_check("force_boundary_execution");
  param_check("boundaries_to_force");
}

void
INSFVAdvectionKernel::initialSetup()
{
  INSFVBCInterface::initialSetup(*this);
}

bool
INSFVAdvectionKernel::skipForBoundary(const FaceInfo & fi) const
{
  for (const auto bnd_id : fi.boundaryIDs())
    if (_boundaries_to_avoid.find(bnd_id) != _boundaries_to_avoid.end())
    {
      return true;
    }

  if (!onBoundary(fi))
    return false;

  // If we have flux bcs then we do skip
  const auto & flux_pr = _var.getFluxBCs(fi);
  if (flux_pr.first)
    return true;

  // If we have a flow boundary without a replacement flux BC, then we must not skip. Mass and
  // momentum are transported via advection across boundaries
  for (const auto bc_id : fi.boundaryIDs())
    if (_flow_boundaries.find(bc_id) != _flow_boundaries.end())
      return false;

  // If not a flow boundary, then there should be no advection/flow in the normal direction, e.g. we
  // should not contribute any advective flux
  return true;
}
<|endoftext|>