<commit_before>#include <cachelot/common.h>
#include <cachelot/memalloc.h>
#include <cachelot/random.h>

extern "C" {
#include "tlsf.h"
}

using namespace cachelot;

static const int num_runs = 10;
static const size_t memory_limit = 1024 * Megabyte;
static const size_t page_size = 4 * Megabyte;
static const size_t user_available_memory = memory_limit - memory_limit * .2; // 80%
static const size_t min_allocation_size = 4;
static const size_t max_allocation_size = page_size;
static const size_t max_allocations_num = memory_limit / min_allocation_size;
typedef std::chrono::high_resolution_clock hires_clock;

int main() {
    size_t total_allocated = 0;
    size_t num_allocations = 0;
    std::vector<std::pair<size_t, void *>> allocations;
    std::vector<size_t> allocation_sizes;

    allocations.reserve(max_allocations_num);
    allocation_sizes.reserve(max_allocations_num);
    for (int run = 0; run < num_runs; ++run) {
        std::cout << "### Running test: " << run << std::endl << std::endl;
        static random_int<size_t> calc_rand_size(min_allocation_size, max_allocation_size);
        allocation_sizes.clear();
        for (size_t alloc_no = 0; alloc_no < max_allocations_num; ++alloc_no) {
            allocation_sizes.push_back(calc_rand_size());
        }
////////////////////////////////////////////////////////////////
    std::cout << "## memalloc   ";
////////////////////////////////////////////////////////////////
    {
        memalloc ma(memory_limit, page_size);
        total_allocated = 0;
        num_allocations = 1;
        allocations.clear();

        auto start = hires_clock::now();
        for (auto alloc_size : allocation_sizes) {
            if (total_allocated < user_available_memory) {
                void * ptr = ma.alloc(alloc_size);
                if (ptr) {
                    total_allocated += alloc_size;
                    num_allocations += 1;
                    allocations.push_back(std::make_pair(alloc_size, ptr));
                } else {
                    break;
                }
            } else {
                break;
            }
        }
        for (const auto al : allocations) { ma.free(al.second); }
        auto end = hires_clock::now();
        uint64 took_ns = std::chrono::duration_cast<std::chrono::nanoseconds>((end - start)).count();
        uint64 APS = 1000000000 / (took_ns / num_allocations);
        std::cout << "Num allocations: " << num_allocations << " took: " << took_ns << "ns (" << APS << " APS)" << std::endl;
    }
////////////////////////////////////////////////////////////////
    std::cout << "## OS runtime ";
////////////////////////////////////////////////////////////////
    {
        total_allocated = 0;
        num_allocations = 1;
        allocations.clear();

        auto start = hires_clock::now();
        for (auto alloc_size : allocation_sizes) {
            if (total_allocated < user_available_memory) {
                void * ptr = ::malloc(alloc_size);
                if (ptr) {
                    total_allocated += alloc_size;
                    num_allocations += 1;
                    allocations.push_back(std::make_pair(alloc_size, ptr));
                } else {
                    break;
                }
            } else {
                break;
            }
        }
        for (const auto al : allocations) { ::free(al.second); }
        auto end = hires_clock::now();
        uint64 took_ns = std::chrono::duration_cast<std::chrono::nanoseconds>((end - start)).count();
        uint64 APS = 1000000000 / (took_ns / num_allocations);
        std::cout << "Num allocations: " << num_allocations << " took: " << took_ns << "ns (" << APS << " APS)" << std::endl;
    }
////////////////////////////////////////////////////////////////
    std::cout << "## TLSF       ";
////////////////////////////////////////////////////////////////
    {
        total_allocated = 0;
        num_allocations = 1;
        allocations.clear();
        std::unique_ptr<uint8[]> arena(new uint8 [memory_limit]);
        init_memory_pool(memory_limit, arena.get());

        auto start = hires_clock::now();
        for (auto alloc_size : allocation_sizes) {
            if (total_allocated < user_available_memory) {
                void * ptr = tlsf_malloc(alloc_size);
                if (ptr) {
                    total_allocated += alloc_size;
                    num_allocations += 1;
                    allocations.push_back(std::make_pair(alloc_size, ptr));
                } else {
                    break;
                }
            } else {
                break;
            }
        }
        for (const auto al : allocations) { tlsf_free(al.second); }
        auto end = hires_clock::now();
        uint64 took_ns = std::chrono::duration_cast<std::chrono::nanoseconds>((end - start)).count();
        uint64 APS = 1000000000 / (took_ns / num_allocations);
        std::cout << "Num allocations: " << num_allocations << " took: " << took_ns << "ns (" << APS << " APS)" << std::endl;
    }
}
    return 0;
}

<commit_msg>bench: reduce memalloc page size<commit_after>#include <cachelot/common.h>
#include <cachelot/memalloc.h>
#include <cachelot/random.h>

extern "C" {
#include "tlsf.h"
}

using namespace cachelot;

static const int num_runs = 10;
static const size_t memory_limit = 1024 * Megabyte;
static const size_t page_size = 4 * Kilobyte;
static const size_t user_available_memory = memory_limit - memory_limit * .2; // 80%
static const size_t min_allocation_size = 4;
static const size_t max_allocation_size = page_size;
static const size_t max_allocations_num = memory_limit / min_allocation_size;
typedef std::chrono::high_resolution_clock hires_clock;

int main() {
    size_t total_allocated = 0;
    size_t num_allocations = 0;
    std::vector<std::pair<size_t, void *>> allocations;
    std::vector<size_t> allocation_sizes;

    allocations.reserve(max_allocations_num);
    allocation_sizes.reserve(max_allocations_num);
    for (int run = 0; run < num_runs; ++run) {
        std::cout << "### Running test: " << run << std::endl << std::endl;
        static random_int<size_t> calc_rand_size(min_allocation_size, max_allocation_size);
        allocation_sizes.clear();
        for (size_t alloc_no = 0; alloc_no < max_allocations_num; ++alloc_no) {
            allocation_sizes.push_back(calc_rand_size());
        }
////////////////////////////////////////////////////////////////
    std::cout << "## memalloc   ";
////////////////////////////////////////////////////////////////
    {
        memalloc ma(memory_limit, page_size);
        total_allocated = 0;
        num_allocations = 1;
        allocations.clear();

        auto start = hires_clock::now();
        for (auto alloc_size : allocation_sizes) {
            if (total_allocated < user_available_memory) {
                void * ptr = ma.alloc(alloc_size);
                if (ptr) {
                    total_allocated += alloc_size;
                    num_allocations += 1;
                    allocations.push_back(std::make_pair(alloc_size, ptr));
                } else {
                    break;
                }
            } else {
                break;
            }
        }
        for (const auto al : allocations) { ma.free(al.second); }
        auto end = hires_clock::now();
        uint64 took_ns = std::chrono::duration_cast<std::chrono::nanoseconds>((end - start)).count();
        uint64 APS = 1000000000 / (took_ns / num_allocations);
        std::cout << "Num allocations: " << num_allocations << " took: " << took_ns << "ns (" << APS << " APS)" << std::endl;
    }
////////////////////////////////////////////////////////////////
    std::cout << "## OS runtime ";
////////////////////////////////////////////////////////////////
    {
        total_allocated = 0;
        num_allocations = 1;
        allocations.clear();

        auto start = hires_clock::now();
        for (auto alloc_size : allocation_sizes) {
            if (total_allocated < user_available_memory) {
                void * ptr = ::malloc(alloc_size);
                if (ptr) {
                    total_allocated += alloc_size;
                    num_allocations += 1;
                    allocations.push_back(std::make_pair(alloc_size, ptr));
                } else {
                    break;
                }
            } else {
                break;
            }
        }
        for (const auto al : allocations) { ::free(al.second); }
        auto end = hires_clock::now();
        uint64 took_ns = std::chrono::duration_cast<std::chrono::nanoseconds>((end - start)).count();
        uint64 APS = 1000000000 / (took_ns / num_allocations);
        std::cout << "Num allocations: " << num_allocations << " took: " << took_ns << "ns (" << APS << " APS)" << std::endl;
    }
////////////////////////////////////////////////////////////////
    std::cout << "## TLSF       ";
////////////////////////////////////////////////////////////////
    {
        total_allocated = 0;
        num_allocations = 1;
        allocations.clear();
        std::unique_ptr<uint8[]> arena(new uint8 [memory_limit]);
        init_memory_pool(memory_limit, arena.get());

        auto start = hires_clock::now();
        for (auto alloc_size : allocation_sizes) {
            if (total_allocated < user_available_memory) {
                void * ptr = tlsf_malloc(alloc_size);
                if (ptr) {
                    total_allocated += alloc_size;
                    num_allocations += 1;
                    allocations.push_back(std::make_pair(alloc_size, ptr));
                } else {
                    break;
                }
            } else {
                break;
            }
        }
        for (const auto al : allocations) { tlsf_free(al.second); }
        auto end = hires_clock::now();
        uint64 took_ns = std::chrono::duration_cast<std::chrono::nanoseconds>((end - start)).count();
        uint64 APS = 1000000000 / (took_ns / num_allocations);
        std::cout << "Num allocations: " << num_allocations << " took: " << took_ns << "ns (" << APS << " APS)" << std::endl;
    }
}
    return 0;
}

<|endoftext|>


<commit_before>/****************************************************************************
 *
 *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file camera_trigger.cpp
 *
 * External camera-IMU synchronisation and triggering via FMU auxiliary pins.
 *
 * @author Mohammed Kabir <mhkabir98@gmail.com>
 */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <fcntl.h>
#include <stdbool.h>
#include <nuttx/clock.h>
#include <nuttx/arch.h>
#include <nuttx/wqueue.h>
#include <systemlib/systemlib.h>
#include <systemlib/err.h>
#include <systemlib/param/param.h>
#include <uORB/uORB.h>
#include <uORB/topics/camera_trigger.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_command.h>
#include <poll.h>
#include <drivers/drv_gpio.h>
#include <drivers/drv_hrt.h>
#include <mavlink/mavlink_log.h>
#include <board_config.h>

#define TRIGGER_PIN_DEFAULT 1

extern "C" __EXPORT int camera_trigger_main(int argc, char *argv[]);

class CameraTrigger
{
public:
	/**
	 * Constructor
	 */
	CameraTrigger();

	/**
	 * Destructor, also kills task.
	 */
	~CameraTrigger();

	/**
	 * Set the trigger on / off
	 */
	void		control(bool on);

	/**
	 * Start the task.
	 */
	void		start();

	/**
	 * Stop the task.
	 */
	void		stop();

	/**
	 * Display info.
	 */
	void		info();

	int			_pins[6];

private:

	struct hrt_call		_engagecall;
	struct hrt_call		_disengagecall;
	static struct work_s	_work;

	int 			_gpio_fd;

	int 			_polarity;
	int			_mode;
	float 			_activation_time;
	float  			_interval;
	uint32_t 		_trigger_seq;
	bool	 		_trigger_enabled;

	int			_vcommand_sub;

	orb_advert_t		_trigger_pub;

	param_t _p_polarity;
	param_t _p_mode;
	param_t _p_activation_time;
	param_t _p_interval;
	param_t _p_pin;

	static constexpr uint32_t _gpios[6] = {
		GPIO_GPIO0_OUTPUT,
		GPIO_GPIO1_OUTPUT,
		GPIO_GPIO2_OUTPUT,
		GPIO_GPIO3_OUTPUT,
		GPIO_GPIO4_OUTPUT,
		GPIO_GPIO5_OUTPUT
	};

	/**
	 * Vehicle command handler
	 */
	static void	cycle_trampoline(void *arg);
	/**
	 * Fires trigger
	 */
	static void	engage(void *arg);
	/**
	 * Resets trigger
	 */
	static void	disengage(void *arg);

};

struct work_s CameraTrigger::_work;
constexpr uint32_t CameraTrigger::_gpios[6];

namespace camera_trigger
{

CameraTrigger	*g_camera_trigger;
}

CameraTrigger::CameraTrigger() :
	_pins{},
	_engagecall {},
	_disengagecall {},
	_gpio_fd(-1),
	_polarity(0),
	_mode(0),
	_activation_time(0.5f /* ms */),
	_interval(100.0f /* ms */),
	_trigger_seq(0),
	_trigger_enabled(false),
	_vcommand_sub(-1),
	_trigger_pub(nullptr)
{
	memset(&_work, 0, sizeof(_work));

	// Parameters
	_p_polarity = param_find("TRIG_POLARITY");
	_p_interval = param_find("TRIG_INTERVAL");
	_p_activation_time = param_find("TRIG_ACT_TIME");
	_p_mode = param_find("TRIG_MODE");
	_p_pin = param_find("TRIG_PINS");

	param_get(_p_polarity, &_polarity);
	param_get(_p_activation_time, &_activation_time);
	param_get(_p_interval, &_interval);
	param_get(_p_mode, &_mode);
	int pin_list;
	param_get(_p_pin, &pin_list);

	// Set all pins as invalid
	for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
		_pins[i] = -1;
	}

	// Convert number to individual channels
	unsigned i = 0;
	int single_pin;

	while ((single_pin = pin_list % 10)) {

		_pins[i] = single_pin - 1;

		if (_pins[i] < 0 || _pins[i] >= static_cast<int>(sizeof(_gpios) / sizeof(_gpios[0]))) {
			_pins[i] = -1;
		}

		pin_list /= 10;
		i++;
	}

	struct camera_trigger_s	trigger = {};

	_trigger_pub = orb_advertise(ORB_ID(camera_trigger), &trigger);
}

CameraTrigger::~CameraTrigger()
{
	camera_trigger::g_camera_trigger = nullptr;
}

void
CameraTrigger::control(bool on)
{
	// always execute, even if already on
	// to reset timings if necessary

	if (on) {
		// schedule trigger on and off calls
		hrt_call_every(&_engagecall, 500, (_interval * 1000),
			       (hrt_callout)&CameraTrigger::engage, this);

		// schedule trigger on and off calls
		hrt_call_every(&_disengagecall, 500 + (_activation_time * 1000), (_interval * 1000),
			       (hrt_callout)&CameraTrigger::disengage, this);

	} else {
		// cancel all calls
		hrt_cancel(&_engagecall);
		hrt_cancel(&_disengagecall);
		// ensure that the pin is off
		hrt_call_after(&_disengagecall, 500,
			       (hrt_callout)&CameraTrigger::disengage, this);
	}

	_trigger_enabled = on;
}

void
CameraTrigger::start()
{

	for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
		stm32_configgpio(_gpios[_pins[i]]);
		stm32_gpiowrite(_gpios[_pins[i]], !_polarity);
	}

	// enable immediate if configured that way
	if (_mode > 1) {
		control(true);
	}

	// start to monitor at high rate for trigger enable command
	work_queue(LPWORK, &_work, (worker_t)&CameraTrigger::cycle_trampoline, this, USEC2TICK(1));
}

void
CameraTrigger::stop()
{
	work_cancel(LPWORK, &_work);
	hrt_cancel(&_engagecall);
	hrt_cancel(&_disengagecall);

	if (camera_trigger::g_camera_trigger != nullptr) {
		delete(camera_trigger::g_camera_trigger);
	}
}

void
CameraTrigger::cycle_trampoline(void *arg)
{

	CameraTrigger *trig = reinterpret_cast<CameraTrigger *>(arg);

	if (trig->_vcommand_sub < 0) {
		trig->_vcommand_sub = orb_subscribe(ORB_ID(vehicle_command));
	}

	bool updated;
	orb_check(trig->_vcommand_sub, &updated);

	// while the trigger is inactive it has to be ready
	// to become active instantaneously
	int poll_interval_usec = 5000;

	if (updated) {

		struct vehicle_command_s cmd;

		orb_copy(ORB_ID(vehicle_command), trig->_vcommand_sub, &cmd);

		if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL) {
			// Set trigger rate from command
			if (cmd.param2 > 0) {
				trig->_interval = cmd.param2;
				param_set(trig->_p_interval, &(trig->_interval));
			}

			if (cmd.param1 < 1.0f) {
				trig->control(false);

			} else if (cmd.param1 >= 1.0f) {
				trig->control(true);
				// while the trigger is active there is no
				// need to poll at a very high rate
				poll_interval_usec = 100000;
			}
		}
	}

	work_queue(LPWORK, &_work, (worker_t)&CameraTrigger::cycle_trampoline,
		   camera_trigger::g_camera_trigger, USEC2TICK(poll_interval_usec));
}

void
CameraTrigger::engage(void *arg)
{

	CameraTrigger *trig = reinterpret_cast<CameraTrigger *>(arg);

	struct camera_trigger_s	trigger;

	/* set timestamp the instant before the trigger goes off */
	trigger.timestamp = hrt_absolute_time();

	for (unsigned i = 0; i < sizeof(trig->_pins) / sizeof(trig->_pins[0]); i++) {
		if (trig->_pins[i] >= 0) {
			// ACTIVE_LOW == 0
			stm32_gpiowrite(trig->_gpios[trig->_pins[i]], trig->_polarity);
		}
	}

	trigger.seq = trig->_trigger_seq++;

	orb_publish(ORB_ID(camera_trigger), trig->_trigger_pub, &trigger);
}

void
CameraTrigger::disengage(void *arg)
{

	CameraTrigger *trig = reinterpret_cast<CameraTrigger *>(arg);

	for (unsigned i = 0; i < sizeof(trig->_pins) / sizeof(trig->_pins[0]); i++) {
		if (trig->_pins[i] >= 0) {
			// ACTIVE_LOW == 1
			stm32_gpiowrite(trig->_gpios[trig->_pins[i]], !(trig->_polarity));
		}
	}
}

void
CameraTrigger::info()
{
	warnx("state : %s", _trigger_enabled ? "enabled" : "disabled");
	warnx("pins 1-3 : %d,%d,%d polarity : %s", _pins[0], _pins[1], _pins[2],
	      _polarity ? "ACTIVE_HIGH" : "ACTIVE_LOW");
	warnx("interval : %.2f", (double)_interval);
}

static void usage()
{
	errx(1, "usage: camera_trigger {start|stop|info} [-p <n>]\n");
}

int camera_trigger_main(int argc, char *argv[])
{
	if (argc < 2) {
		usage();
	}

	if (!strcmp(argv[1], "start")) {

		if (camera_trigger::g_camera_trigger != nullptr) {
			errx(0, "already running");
		}

		camera_trigger::g_camera_trigger = new CameraTrigger;

		if (camera_trigger::g_camera_trigger == nullptr) {
			errx(1, "alloc failed");
		}

		camera_trigger::g_camera_trigger->start();

		return 0;
	}

	if (camera_trigger::g_camera_trigger == nullptr) {
		errx(1, "not running");

	} else if (!strcmp(argv[1], "stop")) {
		camera_trigger::g_camera_trigger->stop();

	} else if (!strcmp(argv[1], "info")) {
		camera_trigger::g_camera_trigger->info();

	} else if (!strcmp(argv[1], "enable")) {
		camera_trigger::g_camera_trigger->control(true);

	} else if (!strcmp(argv[1], "disable")) {
		camera_trigger::g_camera_trigger->control(false);

	} else {
		usage();
	}

	return 0;
}

<commit_msg>camera_trigger: Add one-shot operation<commit_after>/****************************************************************************
 *
 *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file camera_trigger.cpp
 *
 * External camera-IMU synchronisation and triggering via FMU auxiliary pins.
 *
 * @author Mohammed Kabir <mhkabir98@gmail.com>
 */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <fcntl.h>
#include <stdbool.h>
#include <nuttx/clock.h>
#include <nuttx/arch.h>
#include <nuttx/wqueue.h>
#include <systemlib/systemlib.h>
#include <systemlib/err.h>
#include <systemlib/param/param.h>
#include <uORB/uORB.h>
#include <uORB/topics/camera_trigger.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_command.h>
#include <poll.h>
#include <drivers/drv_gpio.h>
#include <drivers/drv_hrt.h>
#include <mavlink/mavlink_log.h>
#include <board_config.h>

#define TRIGGER_PIN_DEFAULT 1

extern "C" __EXPORT int camera_trigger_main(int argc, char *argv[]);

class CameraTrigger
{
public:
	/**
	 * Constructor
	 */
	CameraTrigger();

	/**
	 * Destructor, also kills task.
	 */
	~CameraTrigger();

	/**
	 * Set the trigger on / off
	 */
	void		control(bool on);

	/**
	 * Start the task.
	 */
	void		start();

	/**
	 * Stop the task.
	 */
	void		stop();

	/**
	 * Display info.
	 */
	void		info();

	int			_pins[6];

private:

	struct hrt_call		_engagecall;
	struct hrt_call		_disengagecall;
	static struct work_s	_work;

	int 			_gpio_fd;

	int 			_polarity;
	int			_mode;
	float 			_activation_time;
	float  			_interval;
	uint32_t 		_trigger_seq;
	bool	 		_trigger_enabled;

	int			_vcommand_sub;

	orb_advert_t		_trigger_pub;

	param_t _p_polarity;
	param_t _p_mode;
	param_t _p_activation_time;
	param_t _p_interval;
	param_t _p_pin;

	static constexpr uint32_t _gpios[6] = {
		GPIO_GPIO0_OUTPUT,
		GPIO_GPIO1_OUTPUT,
		GPIO_GPIO2_OUTPUT,
		GPIO_GPIO3_OUTPUT,
		GPIO_GPIO4_OUTPUT,
		GPIO_GPIO5_OUTPUT
	};

	/**
	 * Vehicle command handler
	 */
	static void	cycle_trampoline(void *arg);
	/**
	 * Fires trigger
	 */
	static void	engage(void *arg);
	/**
	 * Resets trigger
	 */
	static void	disengage(void *arg);

	static void trigger(CameraTrigger *trig, bool trigger);

};

struct work_s CameraTrigger::_work;
constexpr uint32_t CameraTrigger::_gpios[6];

namespace camera_trigger
{

CameraTrigger	*g_camera_trigger;
}

CameraTrigger::CameraTrigger() :
	_pins{},
	_engagecall {},
	_disengagecall {},
	_gpio_fd(-1),
	_polarity(0),
	_mode(0),
	_activation_time(0.5f /* ms */),
	_interval(100.0f /* ms */),
	_trigger_seq(0),
	_trigger_enabled(false),
	_vcommand_sub(-1),
	_trigger_pub(nullptr)
{
	memset(&_work, 0, sizeof(_work));

	// Parameters
	_p_polarity = param_find("TRIG_POLARITY");
	_p_interval = param_find("TRIG_INTERVAL");
	_p_activation_time = param_find("TRIG_ACT_TIME");
	_p_mode = param_find("TRIG_MODE");
	_p_pin = param_find("TRIG_PINS");

	param_get(_p_polarity, &_polarity);
	param_get(_p_activation_time, &_activation_time);
	param_get(_p_interval, &_interval);
	param_get(_p_mode, &_mode);
	int pin_list;
	param_get(_p_pin, &pin_list);

	// Set all pins as invalid
	for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
		_pins[i] = -1;
	}

	// Convert number to individual channels
	unsigned i = 0;
	int single_pin;

	while ((single_pin = pin_list % 10)) {

		_pins[i] = single_pin - 1;

		if (_pins[i] < 0 || _pins[i] >= static_cast<int>(sizeof(_gpios) / sizeof(_gpios[0]))) {
			_pins[i] = -1;
		}

		pin_list /= 10;
		i++;
	}

	struct camera_trigger_s	trigger = {};

	_trigger_pub = orb_advertise(ORB_ID(camera_trigger), &trigger);
}

CameraTrigger::~CameraTrigger()
{
	camera_trigger::g_camera_trigger = nullptr;
}

void
CameraTrigger::control(bool on)
{
	// always execute, even if already on
	// to reset timings if necessary

	if (on) {
		// schedule trigger on and off calls
		hrt_call_every(&_engagecall, 500, (_interval * 1000),
			       (hrt_callout)&CameraTrigger::engage, this);

		// schedule trigger on and off calls
		hrt_call_every(&_disengagecall, 500 + (_activation_time * 1000), (_interval * 1000),
			       (hrt_callout)&CameraTrigger::disengage, this);

	} else {
		// cancel all calls
		hrt_cancel(&_engagecall);
		hrt_cancel(&_disengagecall);
		// ensure that the pin is off
		hrt_call_after(&_disengagecall, 500,
			       (hrt_callout)&CameraTrigger::disengage, this);
	}

	_trigger_enabled = on;
}

void
CameraTrigger::start()
{

	for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) {
		stm32_configgpio(_gpios[_pins[i]]);
		stm32_gpiowrite(_gpios[_pins[i]], !_polarity);
	}

	// enable immediate if configured that way
	if (_mode > 1) {
		control(true);
	}

	// start to monitor at high rate for trigger enable command
	work_queue(LPWORK, &_work, (worker_t)&CameraTrigger::cycle_trampoline, this, USEC2TICK(1));
}

void
CameraTrigger::stop()
{
	work_cancel(LPWORK, &_work);
	hrt_cancel(&_engagecall);
	hrt_cancel(&_disengagecall);

	if (camera_trigger::g_camera_trigger != nullptr) {
		delete(camera_trigger::g_camera_trigger);
	}
}

void
CameraTrigger::cycle_trampoline(void *arg)
{

	CameraTrigger *trig = reinterpret_cast<CameraTrigger *>(arg);

	if (trig->_vcommand_sub < 0) {
		trig->_vcommand_sub = orb_subscribe(ORB_ID(vehicle_command));
	}

	bool updated;
	orb_check(trig->_vcommand_sub, &updated);

	// while the trigger is inactive it has to be ready
	// to become active instantaneously
	int poll_interval_usec = 5000;

	if (updated) {

		struct vehicle_command_s cmd;

		orb_copy(ORB_ID(vehicle_command), trig->_vcommand_sub, &cmd);

		if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL) {
			// Set trigger rate from command
			if (cmd.param2 > 0) {
				trig->_interval = cmd.param2;
				param_set(trig->_p_interval, &(trig->_interval));
			}

			if (cmd.param1 < 1.0f) {
				trig->control(false);

			} else if (cmd.param1 >= 1.0f) {
				trig->control(true);
				// while the trigger is active there is no
				// need to poll at a very high rate
				poll_interval_usec = 100000;
			}
		} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL) {
			if (cmd.param5 > 0) {
				// One-shot trigger, default 1 ms interval
				trig->_interval = 1000;
				trig->control(true);
			}
		}
	}

	work_queue(LPWORK, &_work, (worker_t)&CameraTrigger::cycle_trampoline,
		   camera_trigger::g_camera_trigger, USEC2TICK(poll_interval_usec));
}

void
CameraTrigger::engage(void *arg)
{

	CameraTrigger *trig = reinterpret_cast<CameraTrigger *>(arg);

	struct camera_trigger_s	report = {};

	/* set timestamp the instant before the trigger goes off */
	report.timestamp = hrt_absolute_time();

	CameraTrigger::trigger(trig, trig->_polarity);

	report.seq = trig->_trigger_seq++;

	orb_publish(ORB_ID(camera_trigger), trig->_trigger_pub, &report);
}

void
CameraTrigger::disengage(void *arg)
{

	CameraTrigger *trig = reinterpret_cast<CameraTrigger *>(arg);

	CameraTrigger::trigger(trig, !(trig->_polarity));
}

void
CameraTrigger::trigger(CameraTrigger *trig, bool trigger)
{
	for (unsigned i = 0; i < sizeof(trig->_pins) / sizeof(trig->_pins[0]); i++) {
		if (trig->_pins[i] >= 0) {
			// ACTIVE_LOW == 1
			stm32_gpiowrite(trig->_gpios[trig->_pins[i]], trigger);
		}
	}
}

void
CameraTrigger::info()
{
	warnx("state : %s", _trigger_enabled ? "enabled" : "disabled");
	warnx("pins 1-3 : %d,%d,%d polarity : %s", _pins[0], _pins[1], _pins[2],
	      _polarity ? "ACTIVE_HIGH" : "ACTIVE_LOW");
	warnx("interval : %.2f", (double)_interval);
}

static void usage()
{
	errx(1, "usage: camera_trigger {start|stop|info} [-p <n>]\n");
}

int camera_trigger_main(int argc, char *argv[])
{
	if (argc < 2) {
		usage();
	}

	if (!strcmp(argv[1], "start")) {

		if (camera_trigger::g_camera_trigger != nullptr) {
			errx(0, "already running");
		}

		camera_trigger::g_camera_trigger = new CameraTrigger;

		if (camera_trigger::g_camera_trigger == nullptr) {
			errx(1, "alloc failed");
		}

		camera_trigger::g_camera_trigger->start();

		return 0;
	}

	if (camera_trigger::g_camera_trigger == nullptr) {
		errx(1, "not running");

	} else if (!strcmp(argv[1], "stop")) {
		camera_trigger::g_camera_trigger->stop();

	} else if (!strcmp(argv[1], "info")) {
		camera_trigger::g_camera_trigger->info();

	} else if (!strcmp(argv[1], "enable")) {
		camera_trigger::g_camera_trigger->control(true);

	} else if (!strcmp(argv[1], "disable")) {
		camera_trigger::g_camera_trigger->control(false);

	} else {
		usage();
	}

	return 0;
}

<|endoftext|>


<commit_before>#include "SteerAction.h"
#include <Game/ECS/Components/ActionQueueComponent.h>
#include <AI/Navigation/NavAgentComponent.h>
#include <Math/Vector2.h>
#include <Core/Factory.h>

namespace DEM::AI
{
FACTORY_CLASS_IMPL(DEM::AI::CSteerAction, 'STRA', CTraversalAction);

bool CSteerAction::GenerateAction(Game::CGameWorld& World, CNavAgentComponent& Agent, Game::HEntity Controller, Game::CActionQueueComponent& Queue,
	Game::HAction NavAction, const vector3& Pos)
{
	// If not on navmesh, recover to the nearest valid position
	if (Agent.Mode == ENavigationMode::Recovery)
		return !!Queue.PushOrUpdateChild<Steer>(NavAction, Agent.Corridor.getPos(), Agent.Corridor.getPos(), -0.f);

	const float* pPos = (Agent.Mode == ENavigationMode::Offmesh) ? Pos.v : Agent.Corridor.getPos();

	//???add shortcut method to corridor? Agent.Corridor.initStraightPathSearch(Agent.pNavQuery, Ctx);
	dtStraightPathContext Ctx;
	if (dtStatusFailed(Agent.pNavQuery->initStraightPathSearch(
		pPos, Agent.Corridor.getTarget(), Agent.Corridor.getPath(), Agent.Corridor.getPathCount(), Ctx)))
		return false;

	const int Options = Agent.Settings->IsAreaControllable(Agent.CurrAreaType) ? DT_STRAIGHTPATH_ALL_CROSSINGS : DT_STRAIGHTPATH_AREA_CROSSINGS;

	vector3 Dest;
	unsigned char Flags;
	unsigned char AreaType;
	dtPolyRef PolyRef;
	dtStatus Status = Agent.pNavQuery->findNextStraightPathPoint(Ctx, Dest.v, &Flags, &AreaType, &PolyRef, Options);
	if (dtStatusFailed(Status)) return false;

	Agent.IsTraversingLastEdge = dtStatusSucceed(Status);

	vector3 NextDest = Dest;
	bool ActionChanged = false;
	while (!Agent.IsTraversingLastEdge)
	{
		// Check if Dest is close enough to be considered reached
		const bool DestReached =
			std::abs(Pos.y - Dest.y) < Agent.Height &&
			vector3::SqDistance2D(Pos, Dest) < Steer::SqLinearTolerance;

		// Get the next edge traversal action
		Game::HEntity Controller;
		auto pNextAction = Agent.Settings->FindAction(World, Agent, AreaType, PolyRef, &Controller);
		ActionChanged = (!pNextAction || &CSteerAction::RTTI != pNextAction->GetRTTI());
		if (ActionChanged)
		{
			// Action changed, we must finish processing. If we are close enough to the next edge start, let the
			// new action start immediately to avoid regenerating already finished Steer action again and again.
			// Don't trigger offmesh connections. If it was possible, navigation system would do that.
			if (pNextAction && DestReached && !(Flags & DT_STRAIGHTPATH_OFFMESH_CONNECTION))
				return pNextAction->GenerateAction(World, Agent, Controller, Queue, NavAction, Pos);

			break;
		}

		// Get the next path edge end point
		const int Options = Agent.Settings->IsAreaControllable(AreaType) ? DT_STRAIGHTPATH_ALL_CROSSINGS : DT_STRAIGHTPATH_AREA_CROSSINGS;
		Status = Agent.pNavQuery->findNextStraightPathPoint(Ctx, NextDest.v, &Flags, &AreaType, &PolyRef, Options);
		if (dtStatusFailed(Status)) break;

		// If next edge changes direction, use its end position for smooth turning and finish
		if (!DestReached)
		{
			constexpr float COS_SMALL_ANGLE_SQ = 0.99999f;
			const vector2 ToCurr(Dest.x - Pos.x, Dest.z - Pos.z);
			const vector2 ToNext(NextDest.x - Pos.x, NextDest.z - Pos.z);
			const float Dot = ToNext.dot(ToCurr);
			if (Dot * Dot < ToNext.SqLength() * ToCurr.SqLength() * COS_SMALL_ANGLE_SQ)
				break;
		}

		// Same action and direction, elongate path edge
		Dest = NextDest;

		// That was the last edge, its successful traversal will finish the navigate action
		if (dtStatusSucceed(Status)) Agent.IsTraversingLastEdge = true;
	}

	// Calculate distance from Dest to next action change point. Used for arrival slowdown.
	float AdditionalDistance = 0.f;

	// No need to calculate if action changed at Dest
	if (!ActionChanged && !Agent.IsTraversingLastEdge)
	{
		// Try to get existing sub-action of required type
		auto SteerAction = Queue.GetChild(NavAction);
		auto pSteer = SteerAction.As<Steer>();

		// Recalculate only when destination changes
		// TODO: can add other criteria, like final navigation target change
		if (!pSteer || pSteer->_Dest != Dest)
		{
			vector3 Prev = Dest;
			vector3 Curr = NextDest;
			while (true)
			{
				AdditionalDistance += vector3::Distance(Prev, Curr);

				// Stop if path ends at Curr
				if (!dtStatusInProgress(Status)) break;

				// Stop if action changes at Curr
				Game::HEntity Controller;
				auto pNextAction = Agent.Settings->FindAction(World, Agent, AreaType, PolyRef, &Controller);
				if (!pNextAction || &CSteerAction::RTTI != pNextAction->GetRTTI()) break;

				// Get end point of the next edge with the same action
				Prev = Curr;
				const int Options = Agent.Settings->IsAreaControllable(AreaType) ? DT_STRAIGHTPATH_ALL_CROSSINGS : DT_STRAIGHTPATH_AREA_CROSSINGS;
				Status = Agent.pNavQuery->findNextStraightPathPoint(Ctx, Curr.v, nullptr, &AreaType, &PolyRef, Options);
				if (dtStatusFailed(Status)) break;
			}
		}
		else
		{
			// Use value cached in the action
			AdditionalDistance = pSteer->_AdditionalDistance;
		}
	}

	// At the last path edge consider desired final facing
	if (Agent.IsTraversingLastEdge) NextDest = Dest + NavAction.As<Navigate>()->_FinalFacing;

	// Update existing action or push the new one
	return !!Queue.PushOrUpdateChild<Steer>(NavAction, Dest, NextDest, AdditionalDistance);
}
//---------------------------------------------------------------------

}
<commit_msg>Disable slowdown from individual traversal actions<commit_after>#include "SteerAction.h"
#include <Game/ECS/Components/ActionQueueComponent.h>
#include <AI/Navigation/NavAgentComponent.h>
#include <Math/Vector2.h>
#include <Core/Factory.h>

namespace DEM::AI
{
FACTORY_CLASS_IMPL(DEM::AI::CSteerAction, 'STRA', CTraversalAction);

bool CSteerAction::GenerateAction(Game::CGameWorld& World, CNavAgentComponent& Agent, Game::HEntity Controller, Game::CActionQueueComponent& Queue,
	Game::HAction NavAction, const vector3& Pos)
{
	// If not on navmesh, recover to the nearest valid position
	if (Agent.Mode == ENavigationMode::Recovery)
		return !!Queue.PushOrUpdateChild<Steer>(NavAction, Agent.Corridor.getPos(), Agent.Corridor.getPos(), -0.f);

	const float* pPos = (Agent.Mode == ENavigationMode::Offmesh) ? Pos.v : Agent.Corridor.getPos();

	//???add shortcut method to corridor? Agent.Corridor.initStraightPathSearch(Agent.pNavQuery, Ctx);
	dtStraightPathContext Ctx;
	if (dtStatusFailed(Agent.pNavQuery->initStraightPathSearch(
		pPos, Agent.Corridor.getTarget(), Agent.Corridor.getPath(), Agent.Corridor.getPathCount(), Ctx)))
		return false;

	const int Options = Agent.Settings->IsAreaControllable(Agent.CurrAreaType) ? DT_STRAIGHTPATH_ALL_CROSSINGS : DT_STRAIGHTPATH_AREA_CROSSINGS;

	vector3 Dest;
	unsigned char Flags;
	unsigned char AreaType;
	dtPolyRef PolyRef;
	dtStatus Status = Agent.pNavQuery->findNextStraightPathPoint(Ctx, Dest.v, &Flags, &AreaType, &PolyRef, Options);
	if (dtStatusFailed(Status)) return false;

	Agent.IsTraversingLastEdge = dtStatusSucceed(Status);

	vector3 NextDest = Dest;
	bool ActionChanged = false;
	bool NeedSlowdown = true;
	while (!Agent.IsTraversingLastEdge)
	{
		// Check if Dest is close enough to be considered reached
		const bool DestReached =
			std::abs(Pos.y - Dest.y) < Agent.Height &&
			vector3::SqDistance2D(Pos, Dest) < Steer::SqLinearTolerance;

		// Get the next edge traversal action
		Game::HEntity Controller;
		auto pNextAction = Agent.Settings->FindAction(World, Agent, AreaType, PolyRef, &Controller);
		ActionChanged = (!pNextAction || &CSteerAction::RTTI != pNextAction->GetRTTI());
		if (ActionChanged)
		{
			// Action changed, we must finish processing. If we are close enough to the next edge start, let the
			// new action start immediately to avoid regenerating already finished Steer action again and again.
			// Don't trigger offmesh connections. If it was possible, navigation system would do that.
			if (pNextAction && DestReached && !(Flags & DT_STRAIGHTPATH_OFFMESH_CONNECTION))
				return pNextAction->GenerateAction(World, Agent, Controller, Queue, NavAction, Pos);

			// Some actions require no arrival slowdown when approaching action change point
			NeedSlowdown = !pNextAction || pNextAction->NeedSlowdownBeforeStart(Agent);
			break;
		}

		// Get the next path edge end point
		const int Options = Agent.Settings->IsAreaControllable(AreaType) ? DT_STRAIGHTPATH_ALL_CROSSINGS : DT_STRAIGHTPATH_AREA_CROSSINGS;
		Status = Agent.pNavQuery->findNextStraightPathPoint(Ctx, NextDest.v, &Flags, &AreaType, &PolyRef, Options);
		if (dtStatusFailed(Status)) break;

		// If next edge changes direction, use its end position for smooth turning and finish
		if (!DestReached)
		{
			constexpr float COS_SMALL_ANGLE_SQ = 0.99999f;
			const vector2 ToCurr(Dest.x - Pos.x, Dest.z - Pos.z);
			const vector2 ToNext(NextDest.x - Pos.x, NextDest.z - Pos.z);
			const float Dot = ToNext.dot(ToCurr);
			if (Dot * Dot < ToNext.SqLength() * ToCurr.SqLength() * COS_SMALL_ANGLE_SQ)
				break;
		}

		// Same action and direction, elongate path edge
		Dest = NextDest;

		// That was the last edge, its successful traversal will finish the navigate action
		if (dtStatusSucceed(Status)) Agent.IsTraversingLastEdge = true;
	}

	// Calculate distance from Dest to next action change point. Used for arrival slowdown.
	float AdditionalDistance = NeedSlowdown ? 0.f : -0.f;

	// No need to calculate if action changed at Dest
	if (!ActionChanged && !Agent.IsTraversingLastEdge)
	{
		// Try to get existing sub-action of required type
		auto SteerAction = Queue.GetChild(NavAction);
		auto pSteer = SteerAction.As<Steer>();

		// Recalculate only when destination changes
		// TODO: can add other criteria, like final navigation target change
		if (!pSteer || pSteer->_Dest != Dest)
		{
			vector3 Prev = Dest;
			vector3 Curr = NextDest;
			while (true)
			{
				AdditionalDistance += vector3::Distance(Prev, Curr);

				// Stop if path ends at Curr
				if (!dtStatusInProgress(Status)) break;

				// Stop if action changes at Curr
				Game::HEntity Controller;
				auto pNextAction = Agent.Settings->FindAction(World, Agent, AreaType, PolyRef, &Controller);
				if (!pNextAction || &CSteerAction::RTTI != pNextAction->GetRTTI()) break;

				// Get end point of the next edge with the same action
				Prev = Curr;
				const int Options = Agent.Settings->IsAreaControllable(AreaType) ? DT_STRAIGHTPATH_ALL_CROSSINGS : DT_STRAIGHTPATH_AREA_CROSSINGS;
				Status = Agent.pNavQuery->findNextStraightPathPoint(Ctx, Curr.v, nullptr, &AreaType, &PolyRef, Options);
				if (dtStatusFailed(Status)) break;
			}
		}
		else
		{
			// Use value cached in the action
			AdditionalDistance = pSteer->_AdditionalDistance;
		}
	}

	// At the last path edge consider desired final facing
	if (Agent.IsTraversingLastEdge) NextDest = Dest + NavAction.As<Navigate>()->_FinalFacing;

	// Update existing action or push the new one
	return !!Queue.PushOrUpdateChild<Steer>(NavAction, Dest, NextDest, AdditionalDistance);
}
//---------------------------------------------------------------------

}
<|endoftext|>


<commit_before>// Support for Python oword subroutines
// Michael Haberler 4/2011
//
// FIXME mah: proper dereferencing!!

#include "Python.h"
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <ctype.h>
#include <sys/types.h>
#include <sys/stat.h>
#include "rs274ngc.hh"
#include "interp_return.hh"
#include "interp_internal.hh"
#include "rs274ngc_interp.hh"
#include "units.h"

#define HANDLENAME "_this"

#define PYCHK(bad, fmt, ...)				       \
    do {                                                       \
        if (bad) {                                             \
	    ERS(fmt, ## __VA_ARGS__);                          \
	    goto error;					       \
        }                                                      \
    } while(0)

int Interp::init_python(setup_pointer settings)
{
    char cmd[LINELEN];
    PyObject *py_this,*handle,*exc, *val, *tbk;

    fprintf(stderr,"--- init_python(this=%lx) %lx\n",(unsigned long)this,(unsigned long)&this->_setup);
    if (settings->pymodule_ok)
	return INTERP_OK;
    if (settings->pymodule) {
	Py_Initialize();
	PYCHK((PyRun_SimpleString("import sys") < 0),"init_python: 'import sys' failed");
	if (settings->pydir) {
	    snprintf(cmd,sizeof(cmd),"sys.path.append(\"%s\")",settings->pydir);
	    PYCHK((PyRun_SimpleString(cmd) < 0),"init_python: '%s' failed",cmd);
	}

	PYCHK((settings->pymodname = PyString_FromString(settings->pymodule)) == NULL,"init_python: cant  PyString_FromString()");
	PYCHK((settings->pymod = PyImport_Import(settings->pymodname)) == NULL,"init_python: cant import '%s'",settings->pymodule);
	PYCHK((settings->pymdict = PyModule_GetDict(settings->pymod)) == NULL,"init_python: cant PyModule_GetDict");

	// check for HANDLENAME accidentially already defined
	PYCHK(((handle = PyDict_GetItemString(settings->pymdict,HANDLENAME)) != NULL),
	      "init_python: cannot store global '%s' - object already present",HANDLENAME);

	// pass interpreter instance as first argument wrapped in a long
	// so the interpreter can be called from within Python
	PYCHK(((py_this = PyLong_FromVoidPtr((void *)this)) == NULL),
	      "init_python: cannot convert '%s'",HANDLENAME);

	PYCHK(PyDict_SetItemString(settings->pymdict, HANDLENAME, py_this),
	      "init_python: cannot store global '%s' - PyDict_SetItemString failed ",HANDLENAME);

	if (PyErr_Occurred()) {
	    PyErr_Fetch(&exc, &val, &tbk);
	    ERS("init_python(%s): %s",settings->pymodule,PyString_AsString(val));
	    goto error;
	}

	settings->pymodule_ok = true;
    }
    return INTERP_OK;
 error:
    PyErr_Print();
    Py_XDECREF(settings->pymodname);
    Py_XDECREF(settings->pymod);
    Py_XDECREF(settings->pymdict);
    Py_XDECREF(py_this);
    Py_XDECREF(handle);
    Py_XDECREF(exc);
    Py_XDECREF(val);
    Py_XDECREF(tbk);
    PyErr_Clear();
    return INTERP_ERROR;
}

bool Interp::is_pycallable(setup_pointer settings,const char *funcname)
{
    PyObject *func;
    bool result;

    result =  ((settings->pymdict != NULL) &&
	    ((func = PyDict_GetItemString(settings->pymdict, funcname)) != NULL) &&  /* borrowed reference */
	    (PyCallable_Check(func)));
    fprintf(stderr,"---- py_iscallable(%s) = %s\n",funcname,result ? "TRUE":"FALSE");
    return result;
}

int Interp::pycall(setup_pointer settings,
		   const char *funcname,
		   double params[])
{
    PyObject *func,*result,*tuple,
	*exc, *val, *tbk, *exc_str,
	*res_str = NULL;;

    fprintf(stderr,"---- pycall %s [%f] [%f] this=%lx\n",
	    funcname,params[0],params[1],(unsigned long)this);

    // borrowed reference
    PYCHK(((func = PyDict_GetItemString(settings->pymdict,
					funcname)) == NULL),
	  "pycall: didnt find '%s' in module '%s'",
	  funcname,settings->pymodule);

    // there must be a better way to do this
    PYCHK(((tuple = Py_BuildValue("([dddddddddddddddddddddddddddddd])",
				 params[0],params[1],params[2],params[3],params[4],params[5],params[6],
				 params[7],params[8],params[9],params[10],params[11],params[12],
				 params[13],params[14],params[15],params[16],params[17],params[18],
				 params[19],params[20],params[21],params[22],params[23],params[24],
				 params[25],params[26],params[27],params[28],params[29])) == NULL),
	  "pycall: cant build positional arguments for  '%s.%s'",
	  settings->pymodule,funcname);
    result = PyObject_Call(func, tuple,settings->kwargs);
    if (PyErr_Occurred()) {
	PyErr_Fetch(&exc, &val, &tbk);
	ERS("exception calling '%s.%s': %s",settings->pymodule,funcname,PyString_AsString(val));
	goto error;
    }

    if ((result != Py_None) && !PyFloat_Check(result)) {
	res_str = PyObject_Str(result);
	ERS("function '%s.%s' returned '%s' - expected float, got %s",
	    settings->pymodule,funcname, PyString_AsString(res_str),
	    result->ob_type->tp_name);
	goto error;
    }
    if (result != Py_None)
	settings->return_value = PyFloat_AsDouble(result);

    return INTERP_OK;

error:
    // PyErr_Print();
    Py_XDECREF(func);
    Py_XDECREF(tuple);
    Py_XDECREF(result);
    Py_XDECREF(res_str);
    Py_XDECREF(exc_str);
    Py_XDECREF(exc);
    Py_XDECREF(val);
    Py_XDECREF(tbk);
    PyErr_Clear();
    return INTERP_ERROR;
}
<commit_msg>cleanup, prepare to use boost.python<commit_after>// Support for Python oword subroutines
// Michael Haberler 4/2011
//
// FIXME mah: proper dereferencing!!
#include <boost/python.hpp>
#include <boost/make_shared.hpp>
using namespace boost::python;

#include "Python.h"
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <ctype.h>
#include <sys/types.h>
#include <sys/stat.h>
#include "rs274ngc.hh"
#include "interp_return.hh"
#include "interp_internal.hh"
#include "rs274ngc_interp.hh"
#include "units.h"

#define HANDLENAME "_this"

typedef boost::shared_ptr< Interp > interp_ptr;

#define PYCHK(bad, fmt, ...)				       \
    do {                                                       \
        if (bad) {                                             \
	    ERS(fmt, ## __VA_ARGS__);                          \
	    goto error;					       \
        }                                                      \
    } while(0)

int Interp::init_python(setup_pointer settings)
{
    char cmd[LINELEN];
    PyObject *py_this,*handle,*exc, *val, *tbk;

    //boost::python::object  test = boost::python::object::int_obj(42);

    fprintf(stderr,"--- init_python(this=%lx) %lx\n",
	    (unsigned long)this,
	    (unsigned long)&this->_setup);
    if (settings->pymodule_ok)
	return INTERP_OK;
    if (settings->pymodule) {
	Py_Initialize();
	PYCHK((PyRun_SimpleString("import sys") < 0),
	      "init_python: 'import sys' failed");
	if (settings->pydir) {
	    snprintf(cmd,sizeof(cmd),"sys.path.append(\"%s\")",
		     settings->pydir);
	    PYCHK((PyRun_SimpleString(cmd) < 0),
		  "init_python: '%s' failed",cmd);
	}

	PYCHK((settings->pymodname = PyString_FromString(settings->pymodule)) == NULL,
	      "init_python: cant  PyString_FromString()");
	PYCHK((settings->pymod = PyImport_Import(settings->pymodname)) == NULL,
	      "init_python: cant import '%s'",settings->pymodule);
	PYCHK((settings->pymdict = PyModule_GetDict(settings->pymod)) == NULL,
	      "init_python: cant PyModule_GetDict");

	// check for HANDLENAME accidentially already defined
	PYCHK(((handle = PyDict_GetItemString(settings->pymdict,HANDLENAME)) != NULL),
	      "init_python: cannot store global '%s' - object already present",HANDLENAME);

	// pass interpreter instance as first argument wrapped in a long
	// so the interpreter can be called from within Python
	PYCHK(((py_this = PyLong_FromVoidPtr((void *)this)) == NULL),
	      "init_python: cannot convert '%s'",HANDLENAME);

	PYCHK(PyDict_SetItemString(settings->pymdict, HANDLENAME, py_this),
	      "init_python: cannot store global '%s' - PyDict_SetItemString failed ",HANDLENAME);

	if (PyErr_Occurred()) {
	    PyErr_Fetch(&exc, &val, &tbk);
	    ERS("init_python(%s): %s",settings->pymodule,PyString_AsString(val));
	    goto error;
	}

	settings->pymodule_ok = true;
    }
    return INTERP_OK;
 error:
    PyErr_Print();
    Py_XDECREF(settings->pymodname);
    Py_XDECREF(settings->pymod);
    Py_XDECREF(settings->pymdict);
    Py_XDECREF(py_this);
    Py_XDECREF(handle);
    Py_XDECREF(exc);
    Py_XDECREF(val);
    Py_XDECREF(tbk);
    PyErr_Clear();
    return INTERP_ERROR;
}

bool Interp::is_pycallable(setup_pointer settings,const char *funcname)
{
    PyObject *func;
    bool result;

    result =  ((settings->pymdict != NULL) &&
	    ((func = PyDict_GetItemString(settings->pymdict, funcname)) != NULL) &&  /* borrowed reference */
	    (PyCallable_Check(func)));
    fprintf(stderr,"---- py_iscallable(%s) = %s\n",funcname,result ? "TRUE":"FALSE");
    return result;
}

int Interp::pycall(setup_pointer settings,
		   const char *funcname,
		   double params[])
{
    PyObject *func,*result,*tuple,
	*exc, *val, *tbk, *exc_str,
	*res_str = NULL;;

    fprintf(stderr,"---- pycall %s [%f] [%f] this=%lx\n",
	    funcname,params[0],params[1],(unsigned long)this);

    // borrowed reference
    PYCHK(((func = PyDict_GetItemString(settings->pymdict,
					funcname)) == NULL),
	  "pycall: didnt find '%s' in module '%s'",
	  funcname,settings->pymodule);

    // there must be a better way to do this
    PYCHK(((tuple = Py_BuildValue("([dddddddddddddddddddddddddddddd])",
				 params[0],params[1],params[2],params[3],params[4],params[5],params[6],
				 params[7],params[8],params[9],params[10],params[11],params[12],
				 params[13],params[14],params[15],params[16],params[17],params[18],
				 params[19],params[20],params[21],params[22],params[23],params[24],
				 params[25],params[26],params[27],params[28],params[29])) == NULL),
	  "pycall: cant build positional arguments for  '%s.%s'",
	  settings->pymodule,funcname);
    result = PyObject_Call(func, tuple,settings->kwargs);
    if (PyErr_Occurred()) {
	PyErr_Fetch(&exc, &val, &tbk);
	ERS("exception calling '%s.%s': %s",settings->pymodule,funcname,PyString_AsString(val));
	goto error;
    }

    if ((result != Py_None) && !PyFloat_Check(result)) {
	res_str = PyObject_Str(result);
	ERS("function '%s.%s' returned '%s' - expected float, got %s",
	    settings->pymodule,funcname, PyString_AsString(res_str),
	    result->ob_type->tp_name);
	goto error;
    }
    if (result != Py_None)
	settings->return_value = PyFloat_AsDouble(result);

    return INTERP_OK;

error:
    // PyErr_Print();
    Py_XDECREF(func);
    Py_XDECREF(tuple);
    Py_XDECREF(result);
    Py_XDECREF(res_str);
    Py_XDECREF(exc_str);
    Py_XDECREF(exc);
    Py_XDECREF(val);
    Py_XDECREF(tbk);
    PyErr_Clear();
    return INTERP_ERROR;
}
<|endoftext|>


<commit_before>// Copyright (c) 2016, Dmitry Koplyarov <koplyarov.da@gmail.com>
//
// Permission to use, copy, modify, and/or distribute this software for any purpose with or without fee is hereby granted,
// provided that the above copyright notice and this permission notice appear in all copies.
//
// THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS.
// IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS,
// WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.

#include <benchmarks/core/BenchmarkApp.hpp>
#include <benchmarks/core/ipc/MessageQueue.hpp>
#include <benchmarks/core/utils/ThreadPriority.hpp>

#include <boost/program_options.hpp>
#include <boost/regex.hpp>

namespace benchmarks
{

	BenchmarkApp::BenchmarkApp(const BenchmarkSuite& suite)
		: _suite(suite)
	{ }

	namespace
	{
		struct CmdLineException : public std::runtime_error
		{ CmdLineException(const std::string& msg) : std::runtime_error(msg) { } };
	}

	int BenchmarkApp::Run(int argc, char* argv[])
	{
		try
		{
			using namespace boost::program_options;

			std::string subtask, queue_name = "wigwagMessageQueue", benchmark;
			int64_t num_iterations = 0;
			std::vector<std::string> params_vec;

			options_description od;
			od.add_options()
				("help", "Show help")
				("subtask", value<std::string>(&subtask), "Internal option")
				("queue", value<std::string>(&queue_name), "Internal option")
				("iterations", value<int64_t>(&num_iterations), "Internal option")
				("benchmark", value<std::string>(&benchmark), "Benchmark id")
				("params", value<std::vector<std::string>>(&params_vec)->multitoken(), "Benchmark id")
				;

			positional_options_description pd;
			pd.add("benchmark", 1).add("params", -1);

			parsed_options parsed = command_line_parser(argc, argv).options(od).positional(pd).run();

			variables_map vm;
			store(parsed, vm);
			notify(vm);

			if (benchmark.empty() || vm.count("help"))
			{
				std::cerr << boost::lexical_cast<std::string>(od) << std::endl;
				return 0;
			}

			boost::regex benchmark_re(R"(([^.]+)\.([^.]+)\.([^.]+))");
			boost::smatch m;
			if (!boost::regex_match(benchmark, m, benchmark_re))
				throw CmdLineException("Could not parse benchmark id!");

			boost::regex param_re(R"(([^.]+):([^.]+))");
			std::map<std::string, SerializedParam> params;
			for (auto&& param_str : params_vec)
			{
				boost::smatch m;
				if (!boost::regex_match(param_str, m, param_re))
					throw CmdLineException("Could not parse parameter!");
				params[m[1]] = m[2];
			}

			if (subtask == "measureIterationsCount")
			{
				MessageQueue mq(queue_name);
				mq.SendMessage(std::make_shared<IterationsCountMessage>(_suite.MeasureIterationsCount({m[1], m[2], m[3]}, params)));
				return 0;
			}
			else if (subtask == "invokeBenchmark")
			{
				if (vm.count("iterations") == 0)
					throw CmdLineException("Number of iterations is not specified!");
				MessageQueue mq(queue_name);
				SetMaxThreadPriority();
				_suite.InvokeBenchmark(num_iterations, {m[1], m[2], m[3]}, params);
				return 0;
			}

			if (!subtask.empty())
				throw CmdLineException("Unknown subtask!");

			MessageQueue::Remove(queue_name);
			MessageQueue mq(queue_name);

			{
				std::stringstream cmd;
				cmd << argv[0] << " --subtask measureIterationsCount --queue " << queue_name << " " << benchmark;
				for (auto&& p : params_vec)
					cmd << " " << p;

				if (system(cmd.str().c_str()) != 0)
					throw std::runtime_error(cmd.str() + " failed!");
			}

			{
				auto it_msg = mq.ReceiveMessage<IterationsCountMessage>();
				std::stringstream cmd;
				cmd << argv[0] << " --subtask invokeBenchmark --queue " << queue_name << " --iterations " << it_msg->GetCount() << " " << benchmark;
				for (auto&& p : params_vec)
					cmd << " " << p;

				if (system(cmd.str().c_str()) != 0)
					throw std::runtime_error(cmd.str() + " failed!");
			}

			return 0;
		}
		catch (const CmdLineException& ex)
		{
			std::cerr << ex.what() << std::endl;
			return 1;
		}
		catch (const std::exception& ex)
		{
			std::cerr << "Uncaught exception: " << ex.what() << std::endl;
			return 1;
		}
	}

}
<commit_msg>Removing IPC queue at scope exit<commit_after>// Copyright (c) 2016, Dmitry Koplyarov <koplyarov.da@gmail.com>
//
// Permission to use, copy, modify, and/or distribute this software for any purpose with or without fee is hereby granted,
// provided that the above copyright notice and this permission notice appear in all copies.
//
// THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS.
// IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS,
// WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.

#include <benchmarks/core/BenchmarkApp.hpp>
#include <benchmarks/core/ipc/MessageQueue.hpp>
#include <benchmarks/core/utils/ThreadPriority.hpp>

#include <boost/program_options.hpp>
#include <boost/regex.hpp>
#include <boost/scope_exit.hpp>

namespace benchmarks
{

	BenchmarkApp::BenchmarkApp(const BenchmarkSuite& suite)
		: _suite(suite)
	{ }

	namespace
	{
		struct CmdLineException : public std::runtime_error
		{ CmdLineException(const std::string& msg) : std::runtime_error(msg) { } };
	}

	int BenchmarkApp::Run(int argc, char* argv[])
	{
		try
		{
			using namespace boost::program_options;

			std::string subtask, queue_name = "wigwagMessageQueue", benchmark;
			int64_t num_iterations = 0;
			std::vector<std::string> params_vec;

			options_description od;
			od.add_options()
				("help", "Show help")
				("subtask", value<std::string>(&subtask), "Internal option")
				("queue", value<std::string>(&queue_name), "Internal option")
				("iterations", value<int64_t>(&num_iterations), "Internal option")
				("benchmark", value<std::string>(&benchmark), "Benchmark id")
				("params", value<std::vector<std::string>>(&params_vec)->multitoken(), "Benchmark id")
				;

			positional_options_description pd;
			pd.add("benchmark", 1).add("params", -1);

			parsed_options parsed = command_line_parser(argc, argv).options(od).positional(pd).run();

			variables_map vm;
			store(parsed, vm);
			notify(vm);

			if (benchmark.empty() || vm.count("help"))
			{
				std::cerr << boost::lexical_cast<std::string>(od) << std::endl;
				return 0;
			}

			boost::regex benchmark_re(R"(([^.]+)\.([^.]+)\.([^.]+))");
			boost::smatch m;
			if (!boost::regex_match(benchmark, m, benchmark_re))
				throw CmdLineException("Could not parse benchmark id!");

			boost::regex param_re(R"(([^.]+):([^.]+))");
			std::map<std::string, SerializedParam> params;
			for (auto&& param_str : params_vec)
			{
				boost::smatch m;
				if (!boost::regex_match(param_str, m, param_re))
					throw CmdLineException("Could not parse parameter!");
				params[m[1]] = m[2];
			}

			if (subtask == "measureIterationsCount")
			{
				MessageQueue mq(queue_name);
				mq.SendMessage(std::make_shared<IterationsCountMessage>(_suite.MeasureIterationsCount({m[1], m[2], m[3]}, params)));
				return 0;
			}
			else if (subtask == "invokeBenchmark")
			{
				if (vm.count("iterations") == 0)
					throw CmdLineException("Number of iterations is not specified!");
				MessageQueue mq(queue_name);
				SetMaxThreadPriority();
				_suite.InvokeBenchmark(num_iterations, {m[1], m[2], m[3]}, params);
				return 0;
			}

			if (!subtask.empty())
				throw CmdLineException("Unknown subtask!");

			MessageQueue mq(queue_name);
			BOOST_SCOPE_EXIT_ALL(&) { MessageQueue::Remove(queue_name); };

			{
				std::stringstream cmd;
				cmd << argv[0] << " --subtask measureIterationsCount --queue " << queue_name << " " << benchmark;
				for (auto&& p : params_vec)
					cmd << " " << p;

				if (system(cmd.str().c_str()) != 0)
					throw std::runtime_error(cmd.str() + " failed!");
			}

			{
				auto it_msg = mq.ReceiveMessage<IterationsCountMessage>();
				std::stringstream cmd;
				cmd << argv[0] << " --subtask invokeBenchmark --queue " << queue_name << " --iterations " << it_msg->GetCount() << " " << benchmark;
				for (auto&& p : params_vec)
					cmd << " " << p;

				if (system(cmd.str().c_str()) != 0)
					throw std::runtime_error(cmd.str() + " failed!");
			}

			return 0;
		}
		catch (const CmdLineException& ex)
		{
			std::cerr << ex.what() << std::endl;
			return 1;
		}
		catch (const std::exception& ex)
		{
			std::cerr << "Uncaught exception: " << ex.what() << std::endl;
			return 1;
		}
	}

}
<|endoftext|>


<commit_before>/*
 *  yosys -- Yosys Open SYnthesis Suite
 *
 *  Copyright (C) 2012  Clifford Wolf <clifford@clifford.at>
 *
 *  Permission to use, copy, modify, and/or distribute this software for any
 *  purpose with or without fee is hereby granted, provided that the above
 *  copyright notice and this permission notice appear in all copies.
 *
 *  THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 *  WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 *  MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
 *  ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 *  WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
 *  ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
 *  OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 *
 */

#include "kernel/register.h"
#include "kernel/rtlil.h"
#include "kernel/log.h"

USING_YOSYS_NAMESPACE
PRIVATE_NAMESPACE_BEGIN

void split_portname_pair(std::string &port1, std::string &port2)
{
	size_t pos = port1.find_first_of(':');
	if (pos != std::string::npos) {
		port2 = port1.substr(pos+1);
		port1 = port1.substr(0, pos);
	}
}

struct IopadmapPass : public Pass {
	IopadmapPass() : Pass("iopadmap", "technology mapping of i/o pads (or buffers)") { }
	virtual void help()
	{
		log("\n");
		log("    iopadmap [options] [selection]\n");
		log("\n");
		log("Map module inputs/outputs to PAD cells from a library. This pass\n");
		log("can only map to very simple PAD cells. Use 'techmap' to further map\n");
		log("the resulting cells to more sophisticated PAD cells.\n");
		log("\n");
		log("    -inpad <celltype> <portname>[:<portname>]\n");
		log("        Map module input ports to the given cell type with the\n");
		log("        given output port name. if a 2nd portname is given, the\n");
		log("        signal is passed through the pad call, using the 2nd\n");
		log("        portname as input.\n");
		log("\n");
		log("    -outpad <celltype> <portname>[:<portname>]\n");
		log("    -inoutpad <celltype> <portname>[:<portname>]\n");
		log("        Similar to -inpad, but for output and inout ports.\n");
		log("\n");
		log("    -widthparam <param_name>\n");
		log("        Use the specified parameter name to set the port width.\n");
		log("\n");
		log("    -nameparam <param_name>\n");
		log("        Use the specified parameter to set the port name.\n");
		log("\n");
		log("    -bits\n");
		log("        create individual bit-wide buffers even for ports that\n");
		log("        are wider. (the default behavior is to create word-wide\n");
		log("        buffers using -widthparam to set the word size on the cell.)\n");
		log("\n");
	}
	virtual void execute(std::vector<std::string> args, RTLIL::Design *design)
	{
		log_header(design, "Executing IOPADMAP pass (mapping inputs/outputs to IO-PAD cells).\n");

		std::string inpad_celltype, inpad_portname, inpad_portname2;
		std::string outpad_celltype, outpad_portname, outpad_portname2;
		std::string inoutpad_celltype, inoutpad_portname, inoutpad_portname2;
		std::string widthparam, nameparam;
		bool flag_bits = false;

		size_t argidx;
		for (argidx = 1; argidx < args.size(); argidx++)
		{
			std::string arg = args[argidx];
			if (arg == "-inpad" && argidx+2 < args.size()) {
				inpad_celltype = args[++argidx];
				inpad_portname = args[++argidx];
				split_portname_pair(inpad_portname, inpad_portname2);
				continue;
			}
			if (arg == "-outpad" && argidx+2 < args.size()) {
				outpad_celltype = args[++argidx];
				outpad_portname = args[++argidx];
				split_portname_pair(outpad_portname, outpad_portname2);
				continue;
			}
			if (arg == "-inoutpad" && argidx+2 < args.size()) {
				inoutpad_celltype = args[++argidx];
				inoutpad_portname = args[++argidx];
				split_portname_pair(inoutpad_portname, inoutpad_portname2);
				continue;
			}
			if (arg == "-widthparam" && argidx+1 < args.size()) {
				widthparam = args[++argidx];
				continue;
			}
			if (arg == "-nameparam" && argidx+1 < args.size()) {
				nameparam = args[++argidx];
				continue;
			}
			if (arg == "-bits") {
				flag_bits = true;
				continue;
			}
			break;
		}
		extra_args(args, argidx, design);

		for (auto module : design->selected_modules())
		{
			for (auto wire : module->selected_wires())
			{
				if (!wire->port_id)
					continue;

				std::string celltype, portname, portname2;

				if (wire->port_input && !wire->port_output) {
					if (inpad_celltype.empty()) {
						log("Don't map input port %s.%s: Missing option -inpad.\n", RTLIL::id2cstr(module->name), RTLIL::id2cstr(wire->name));
						continue;
					}
					celltype = inpad_celltype;
					portname = inpad_portname;
					portname2 = inpad_portname2;
				} else
				if (!wire->port_input && wire->port_output) {
					if (outpad_celltype.empty()) {
						log("Don't map output port %s.%s: Missing option -outpad.\n", RTLIL::id2cstr(module->name), RTLIL::id2cstr(wire->name));
						continue;
					}
					celltype = outpad_celltype;
					portname = outpad_portname;
					portname2 = outpad_portname2;
				} else
				if (wire->port_input && wire->port_output) {
					if (inoutpad_celltype.empty()) {
						log("Don't map inout port %s.%s: Missing option -inoutpad.\n", RTLIL::id2cstr(module->name), RTLIL::id2cstr(wire->name));
						continue;
					}
					celltype = inoutpad_celltype;
					portname = inoutpad_portname;
					portname2 = inoutpad_portname2;
				} else
					log_abort();

				if (!flag_bits && wire->width != 1 && widthparam.empty()) {
					log("Don't map multi-bit port %s.%s: Missing option -widthparam or -bits.\n", RTLIL::id2cstr(module->name), RTLIL::id2cstr(wire->name));
					continue;
				}

				log("Mapping port %s.%s using %s.\n", RTLIL::id2cstr(module->name), RTLIL::id2cstr(wire->name), celltype.c_str());

				RTLIL::Wire *new_wire = NULL;
				if (!portname2.empty()) {
					new_wire = module->addWire(NEW_ID, wire);
					module->swap_names(new_wire, wire);
				}

				if (flag_bits)
				{
					for (int i = 0; i < wire->width; i++)
					{
						RTLIL::Cell *cell = module->addCell(NEW_ID, RTLIL::escape_id(celltype));
						cell->setPort(RTLIL::escape_id(portname), RTLIL::SigSpec(wire, i));
						if (!portname2.empty())
							cell->setPort(RTLIL::escape_id(portname2), RTLIL::SigSpec(new_wire, i));
						if (!widthparam.empty())
							cell->parameters[RTLIL::escape_id(widthparam)] = RTLIL::Const(1);
						if (!nameparam.empty())
							cell->parameters[RTLIL::escape_id(nameparam)] = RTLIL::Const(stringf("%s[%d]", RTLIL::id2cstr(wire->name), i));
						cell->attributes["\\keep"] = RTLIL::Const(1);
					}
				}
				else
				{
					RTLIL::Cell *cell = module->addCell(NEW_ID, RTLIL::escape_id(celltype));
					cell->setPort(RTLIL::escape_id(portname), RTLIL::SigSpec(wire));
					if (!portname2.empty())
						cell->setPort(RTLIL::escape_id(portname2), RTLIL::SigSpec(new_wire));
					if (!widthparam.empty())
						cell->parameters[RTLIL::escape_id(widthparam)] = RTLIL::Const(wire->width);
					if (!nameparam.empty())
						cell->parameters[RTLIL::escape_id(nameparam)] = RTLIL::Const(RTLIL::id2cstr(wire->name));
					cell->attributes["\\keep"] = RTLIL::Const(1);
				}

				wire->port_id = 0;
				wire->port_input = false;
				wire->port_output = false;
			}

			module->fixup_ports();
		}
	}
} IopadmapPass;

PRIVATE_NAMESPACE_END
<commit_msg>Fixed iopadmap attribute handling<commit_after>/*
 *  yosys -- Yosys Open SYnthesis Suite
 *
 *  Copyright (C) 2012  Clifford Wolf <clifford@clifford.at>
 *
 *  Permission to use, copy, modify, and/or distribute this software for any
 *  purpose with or without fee is hereby granted, provided that the above
 *  copyright notice and this permission notice appear in all copies.
 *
 *  THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 *  WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 *  MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
 *  ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 *  WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
 *  ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
 *  OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 *
 */

#include "kernel/register.h"
#include "kernel/rtlil.h"
#include "kernel/log.h"

USING_YOSYS_NAMESPACE
PRIVATE_NAMESPACE_BEGIN

void split_portname_pair(std::string &port1, std::string &port2)
{
	size_t pos = port1.find_first_of(':');
	if (pos != std::string::npos) {
		port2 = port1.substr(pos+1);
		port1 = port1.substr(0, pos);
	}
}

struct IopadmapPass : public Pass {
	IopadmapPass() : Pass("iopadmap", "technology mapping of i/o pads (or buffers)") { }
	virtual void help()
	{
		log("\n");
		log("    iopadmap [options] [selection]\n");
		log("\n");
		log("Map module inputs/outputs to PAD cells from a library. This pass\n");
		log("can only map to very simple PAD cells. Use 'techmap' to further map\n");
		log("the resulting cells to more sophisticated PAD cells.\n");
		log("\n");
		log("    -inpad <celltype> <portname>[:<portname>]\n");
		log("        Map module input ports to the given cell type with the\n");
		log("        given output port name. if a 2nd portname is given, the\n");
		log("        signal is passed through the pad call, using the 2nd\n");
		log("        portname as input.\n");
		log("\n");
		log("    -outpad <celltype> <portname>[:<portname>]\n");
		log("    -inoutpad <celltype> <portname>[:<portname>]\n");
		log("        Similar to -inpad, but for output and inout ports.\n");
		log("\n");
		log("    -widthparam <param_name>\n");
		log("        Use the specified parameter name to set the port width.\n");
		log("\n");
		log("    -nameparam <param_name>\n");
		log("        Use the specified parameter to set the port name.\n");
		log("\n");
		log("    -bits\n");
		log("        create individual bit-wide buffers even for ports that\n");
		log("        are wider. (the default behavior is to create word-wide\n");
		log("        buffers using -widthparam to set the word size on the cell.)\n");
		log("\n");
	}
	virtual void execute(std::vector<std::string> args, RTLIL::Design *design)
	{
		log_header(design, "Executing IOPADMAP pass (mapping inputs/outputs to IO-PAD cells).\n");

		std::string inpad_celltype, inpad_portname, inpad_portname2;
		std::string outpad_celltype, outpad_portname, outpad_portname2;
		std::string inoutpad_celltype, inoutpad_portname, inoutpad_portname2;
		std::string widthparam, nameparam;
		bool flag_bits = false;

		size_t argidx;
		for (argidx = 1; argidx < args.size(); argidx++)
		{
			std::string arg = args[argidx];
			if (arg == "-inpad" && argidx+2 < args.size()) {
				inpad_celltype = args[++argidx];
				inpad_portname = args[++argidx];
				split_portname_pair(inpad_portname, inpad_portname2);
				continue;
			}
			if (arg == "-outpad" && argidx+2 < args.size()) {
				outpad_celltype = args[++argidx];
				outpad_portname = args[++argidx];
				split_portname_pair(outpad_portname, outpad_portname2);
				continue;
			}
			if (arg == "-inoutpad" && argidx+2 < args.size()) {
				inoutpad_celltype = args[++argidx];
				inoutpad_portname = args[++argidx];
				split_portname_pair(inoutpad_portname, inoutpad_portname2);
				continue;
			}
			if (arg == "-widthparam" && argidx+1 < args.size()) {
				widthparam = args[++argidx];
				continue;
			}
			if (arg == "-nameparam" && argidx+1 < args.size()) {
				nameparam = args[++argidx];
				continue;
			}
			if (arg == "-bits") {
				flag_bits = true;
				continue;
			}
			break;
		}
		extra_args(args, argidx, design);

		for (auto module : design->selected_modules())
		{
			for (auto wire : module->selected_wires())
			{
				if (!wire->port_id)
					continue;

				std::string celltype, portname, portname2;

				if (wire->port_input && !wire->port_output) {
					if (inpad_celltype.empty()) {
						log("Don't map input port %s.%s: Missing option -inpad.\n", RTLIL::id2cstr(module->name), RTLIL::id2cstr(wire->name));
						continue;
					}
					celltype = inpad_celltype;
					portname = inpad_portname;
					portname2 = inpad_portname2;
				} else
				if (!wire->port_input && wire->port_output) {
					if (outpad_celltype.empty()) {
						log("Don't map output port %s.%s: Missing option -outpad.\n", RTLIL::id2cstr(module->name), RTLIL::id2cstr(wire->name));
						continue;
					}
					celltype = outpad_celltype;
					portname = outpad_portname;
					portname2 = outpad_portname2;
				} else
				if (wire->port_input && wire->port_output) {
					if (inoutpad_celltype.empty()) {
						log("Don't map inout port %s.%s: Missing option -inoutpad.\n", RTLIL::id2cstr(module->name), RTLIL::id2cstr(wire->name));
						continue;
					}
					celltype = inoutpad_celltype;
					portname = inoutpad_portname;
					portname2 = inoutpad_portname2;
				} else
					log_abort();

				if (!flag_bits && wire->width != 1 && widthparam.empty()) {
					log("Don't map multi-bit port %s.%s: Missing option -widthparam or -bits.\n", RTLIL::id2cstr(module->name), RTLIL::id2cstr(wire->name));
					continue;
				}

				log("Mapping port %s.%s using %s.\n", RTLIL::id2cstr(module->name), RTLIL::id2cstr(wire->name), celltype.c_str());

				RTLIL::Wire *new_wire = NULL;
				if (!portname2.empty()) {
					new_wire = module->addWire(NEW_ID, wire);
					module->swap_names(new_wire, wire);
					wire->attributes.clear();
				}

				if (flag_bits)
				{
					for (int i = 0; i < wire->width; i++)
					{
						RTLIL::Cell *cell = module->addCell(NEW_ID, RTLIL::escape_id(celltype));
						cell->setPort(RTLIL::escape_id(portname), RTLIL::SigSpec(wire, i));
						if (!portname2.empty())
							cell->setPort(RTLIL::escape_id(portname2), RTLIL::SigSpec(new_wire, i));
						if (!widthparam.empty())
							cell->parameters[RTLIL::escape_id(widthparam)] = RTLIL::Const(1);
						if (!nameparam.empty())
							cell->parameters[RTLIL::escape_id(nameparam)] = RTLIL::Const(stringf("%s[%d]", RTLIL::id2cstr(wire->name), i));
						cell->attributes["\\keep"] = RTLIL::Const(1);
					}
				}
				else
				{
					RTLIL::Cell *cell = module->addCell(NEW_ID, RTLIL::escape_id(celltype));
					cell->setPort(RTLIL::escape_id(portname), RTLIL::SigSpec(wire));
					if (!portname2.empty())
						cell->setPort(RTLIL::escape_id(portname2), RTLIL::SigSpec(new_wire));
					if (!widthparam.empty())
						cell->parameters[RTLIL::escape_id(widthparam)] = RTLIL::Const(wire->width);
					if (!nameparam.empty())
						cell->parameters[RTLIL::escape_id(nameparam)] = RTLIL::Const(RTLIL::id2cstr(wire->name));
					cell->attributes["\\keep"] = RTLIL::Const(1);
				}

				wire->port_id = 0;
				wire->port_input = false;
				wire->port_output = false;
			}

			module->fixup_ports();
		}
	}
} IopadmapPass;

PRIVATE_NAMESPACE_END
<|endoftext|>