text:
// Copyright (c) 2016, Huang-Ming Huang,  Object Computing, Inc.
// All rights reserved.
//
// This file is part of mFAST.
// See the file license.txt for licensing information.
#include <boost/container/map.hpp>
#include "../mfast_coder_export.h"
#include "../fast_decoder.h"
#include "../../field_visitor.h"
#include "../../sequence_ref.h"
#include "../../malloc_allocator.h"
#include "../../output.h"
#include "../../composite_type.h"
#include "../common/exceptions.h"
#include "../common/debug_stream.h"
#include "../common/template_repo.h"
#include "../common/codec_helper.h"
#include "decoder_presence_map.h"
#include "decoder_field_operator.h"
#include "fast_istream.h"
#include "mfast/vector_ref.h"

namespace mfast {

struct fast_decoder_impl;

struct fast_decoder_impl {
  fast_decoder_impl(mfast::allocator *alloc);
  ~fast_decoder_impl();
  decoder_presence_map &current_pmap();

  struct pmap_state {
    decoder_presence_map pmap_;
    decoder_presence_map *prev_pmap_;

    pmap_state() : prev_pmap_(nullptr) {}
  };

  void decode_pmap(pmap_state &state) {
    state.prev_pmap_ = this->current_;
    this->current_ = &state.pmap_;
    this->strm_.decode(state.pmap_);
  }

  void restore_pmap(pmap_state &state) {
    if (state.prev_pmap_)
      this->current_ = state.prev_pmap_;
  }

  void visit(enum_mref &mref) {
    this->visit(reinterpret_cast<uint64_mref &>(mref));
  }

  void visit(set_mref &mref) {
    this->visit(reinterpret_cast<uint64_mref &>(mref));
  }

  template <typename SimpleMRef> void visit(const SimpleMRef &mref);

  template <typename IntType> void visit(const int_vector_mref<IntType> &mref);

  void visit(const group_mref &mref, int);
  void visit(const sequence_mref &mref, int);
  void visit(const nested_message_mref &mref, int);
  void visit(const sequence_element_mref &mref, int);

  message_type *decode_segment(fast_istreambuf &sb);

  typedef message_type info_entry;

  struct info_entry_converter {
    typedef info_entry repo_mapped_type;

    info_entry_converter(mfast::allocator *alloc) : alloc_(alloc) {}

    template <typename Message>
    std::pair<mfast::allocator *, const template_instruction *>
    to_repo_entry(const template_instruction *inst, Message *) {
      return std::make_pair(alloc_, inst);
    }

    template_instruction *to_instruction(const repo_mapped_type &entry) {
      return const_cast<template_instruction *>(entry.instruction());
    }

    mfast::allocator *alloc_;
  };

  template_repo<info_entry_converter> repo_;
  allocator *message_alloc_;
  fast_istream strm_;
  message_type *active_message_;
  bool force_reset_;
  debug_stream debug_;
  decoder_presence_map *current_;
  std::ostream *warning_log_;
};

inline fast_decoder_impl::fast_decoder_impl(mfast::allocator *alloc)
    : repo_(info_entry_converter(alloc)), message_alloc_(alloc), strm_(nullptr),
      warning_log_(nullptr) {}

fast_decoder_impl::~fast_decoder_impl() {}

inline decoder_presence_map &fast_decoder_impl::current_pmap() {
  return *current_;
}

template <typename SimpleMRef>
inline void fast_decoder_impl::visit(const SimpleMRef &mref) {
  debug_ << "   decoding " << mref.name() << ": pmap -> " << current_pmap()
         << "\n"
         << "               stream -> " << strm_ << "\n";

  const decoder_field_operator *field_operator =
      decoder_operators[mref.instruction()->field_operator()];
  field_operator->decode(mref, strm_, current_pmap());

  if (mref.present())
    debug_ << "   decoded " << mref.name() << " = " << mref << "\n";
  else
    debug_ << "   decoded " << mref.name() << " is absent\n";
}

template <typename IntType>
void fast_decoder_impl::visit(const int_vector_mref<IntType> &mref) {
  debug_ << "decoding int vector " << mref.name();

  uint32_t length = 0;
  if (!strm_.decode(length, mref.optional())) {
    mref.omit();
    return;
  }

  mref.resize(length);
  for (uint32_t i = 0; i < length; ++i) {
    strm_.decode(mref[i], false);
  }
}

inline void fast_decoder_impl::visit(const group_mref &mref, int) {
  debug_ << "decoding group " << mref.name();

  // If a group field is optional, it will occupy a single bit in the presence
  // map.
  // The contents of the group may appear in the stream iff the bit is set.
  if (mref.optional()) {
    debug_ << " : current pmap = " << current_pmap() << "\n";
    if (!current_pmap().is_next_bit_set()) {
      debug_ << "        " << mref.name() << " is absent\n";
      mref.omit();
      return;
    }
  } else {
    debug_ << " : mandatory\n";
  }

  pmap_state state;

  if (mref.instruction()->segment_pmap_size() > 0) {
    decode_pmap(state);
    debug_ << "        " << mref.name() << " has group pmap -> "
           << current_pmap() << "\n";
  }

  for (auto &&field : aggregate_mref(mref)) {
    apply_mutator(*this, field);
  }

  restore_pmap(state);
}

inline void fast_decoder_impl::visit(const sequence_mref &mref, int) {
  debug_ << "decoding sequence " << mref.name() << " ---\n";

  const uint32_field_instruction *length_instruction =
      mref.instruction()->length_instruction();
  value_storage storage;

  debug_ << "  decoding sequence length " << mref.name() << " : stream -> "
         << strm_ << "\n";
  uint32_mref length_mref(nullptr, &storage, length_instruction);
  this->visit(length_mref);

  if (length_mref.present()) {
    debug_ << "  decoded sequence length " << length_mref.value() << "\n";
    mref.resize(length_mref.value());
  } else {
    debug_ << "  " << mref.name() << "is absent\n";
    mref.omit();
  }
  if (length_mref.present() && length_mref.value() > 0) {
    int i = 0;
    for (auto &&elem : mref) {
      this->visit(elem, i++);
    }
  }
}

inline void fast_decoder_impl::visit(const sequence_element_mref &mref,
                                     int index) {
  debug_ << "decoding  element[" << index << "] : segment pmap size = "
         << mref.instruction()->segment_pmap_size() << "\n";

  pmap_state state;
  if (mref.instruction()->segment_pmap_size() > 0) {
    decode_pmap(state);
    debug_ << "    decoded pmap -> " << current_pmap() << "\n";
  }

  for (auto &&field : mref) {
    apply_mutator(*this, field);
  }

  restore_pmap(state);
}

inline void fast_decoder_impl::visit(const nested_message_mref &mref, int) {
  pmap_state state;
  message_type *saved_active_message = active_message_;

  debug_ << "decoding dynamic templateRef ...\n";

  decode_pmap(state);

  debug_ << "   decoded pmap -> " << current_pmap() << "\n";

  decoder_presence_map &pmap = current_pmap();

  if (pmap.is_next_bit_set()) {
    uint32_t template_id;

    strm_.decode(template_id, false);
    debug_ << "   decoded template id -> " << template_id << "\n";

    // find the message with corresponding template id
    active_message_ = repo_.find(template_id);
    if (active_message_ == nullptr) {
      using namespace coder;

      BOOST_THROW_EXCEPTION(fast_dynamic_error("D9")
                            << template_id_info(template_id));
    }
    mref.set_target_instruction(active_message_->instruction(), false);
  }

  for (auto &&field : mref.target()) {
    apply_mutator(*this, field);
  }

  restore_pmap(state);
  active_message_ = saved_active_message;
}

message_type *fast_decoder_impl::decode_segment(fast_istreambuf &sb) {

  strm_.reset(&sb);

  decoder_presence_map pmap;
  this->current_ = &pmap;
  strm_.decode(pmap);

  debug_ << "decoding segment : pmap -> " << pmap << "\n"
         << "                   entity -> " << strm_ << "\n";

  uint32_t template_id = 0;

  if (pmap.is_next_bit_set()) {

    strm_.decode(template_id, false);

    debug_ << "decoded template id = " << template_id << "\n";

    // find the message with corresponding template id
    active_message_ = repo_.find(template_id);
  }

  if (active_message_ == nullptr) {
    BOOST_THROW_EXCEPTION(fast_dynamic_error("D9")
                          << coder::template_id_info(template_id));
  }

  if (force_reset_ || active_message_->instruction()->has_reset_attribute()) {
    repo_.reset_dictionary();
  }

  // we have to keep the active_message_ in a new variable
  // because after the accept_mutator(), the active_message_
  // may change because of the decoding of dynamic template reference
  message_type *message = active_message_;
  // message->ensure_valid();
  // message->ref().accept_mutator(*this);

  for (auto &&field : message->ref()) {
    apply_mutator(*this, field);
  }
  return message;
}

fast_decoder::fast_decoder(allocator *alloc)
    : impl_(new fast_decoder_impl(alloc)) {}

fast_decoder::~fast_decoder() { delete impl_; }

void fast_decoder::include(const templates_description *const *descriptions,
                           std::size_t description_count) {
  impl_->repo_.build(descriptions, description_count);
  impl_->active_message_ = impl_->repo_.unique_entry();
}

message_cref fast_decoder::decode(const char *&first, const char *last,
                                  bool force_reset) {
  assert(first < last);
  fast_istreambuf sb(first, last - first);
  impl_->force_reset_ = force_reset;
  message_cref result = impl_->decode_segment(sb)->cref();
  first = sb.gptr();
  return result;
}

void fast_decoder::debug_log(std::ostream *log) { impl_->debug_.set(log); }

void fast_decoder::warning_log(std::ostream *os) {
  impl_->strm_.warning_log(os);
}
}

max_stars_repo_name: SBAM/mFAST

max_issues_repo_name: SBAM/mFAST

max_forks_repo_name: SBAM/mFAST


text:
/*
MIT License

Copyright (c) 2022 Lou Amadio

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/

#include <memory>
#include <chrono>
#include "rclcpp/rclcpp.hpp"
#include <boost/asio.hpp>
#include <geometry_msgs/msg/twist.hpp>

#ifdef _WIN32
#pragma optimize( "", off )
#else
#include "i2c/i2c.h"
#pragma GCC optimize ("O0")
#endif

using namespace std::chrono_literals;
using std::placeholders::_1;

uint8_t kMotorNeutral = 128;

typedef enum : uint8_t
{
   MotorCommand_Id = 0x01,
   MotorCommand_Status = 0x77,
   MotorCommand_Enable = 0x70,
   MotorCommand_Drive0 = 0x20,
   //..
   //MotorCommand_DriveX = 0x41,

} QwiicMotorCommand;

typedef enum : uint8_t
{
    MotorStatusBit_Enum = 0x01,
    MotorStatusBit_Busy = 0x02,
    MotorStatusBit_Read = 0x04,
    MotorStatusBit_Write = 0x08,
    MotorStatusBit_Enable = 0x10

} QwiicMotorStatusBit;

class MotorSubscriber : public rclcpp::Node
{

  public:
    MotorSubscriber()
    : Node("ros_qwiic_motor")
    , _wheelSeparation(0.1)
    , _wheelRadius(0.03)
    , _powerScale(.05) // power -> RPM
    , _leftInvert(false)
    , _rightInvert(false)
    , _id(0x5D)

    {
    }

    void start()
    {
        #ifndef _WIN32
        get_parameter_or<uint8_t>("id", _id, 0x5D);

        if ((_i2cFileDescriptor = i2c_open("/dev/i2c-1")) == -1)
        {
            return;
        }

        i2c_init_device(&_i2cDevice);

        _i2cDevice.bus = _i2cFileDescriptor;
        _i2cDevice.addr = _id;
        #endif

        rclcpp::Rate loop_rate(1);
        uint8_t id = getId();
        RCLCPP_INFO(rclcpp::get_logger("motor"), "Communicating with motor id: [%d]", id);

        while (!ready())
        {
            RCLCPP_INFO(rclcpp::get_logger("motor"), "Waiting for Motor Controller to be ready");
            loop_rate.sleep();
        }

        while (busy())
        {
            RCLCPP_INFO(rclcpp::get_logger("motor"), "Waiting for Motor Controller, to not be busy...");
            loop_rate.sleep();
        }

        enable();

        get_parameter_or<float>("wheelSeparation", _wheelSeparation, .10);
        get_parameter_or<float>("wheelRadius", _wheelRadius, 0.03);
        get_parameter_or<float>("powerScale", _powerScale, 0.05);
        get_parameter_or<bool>("leftInverted", _leftInvert, false);
        get_parameter_or<bool>("rightInverted", _rightInvert, false);

        _subscription = this->create_subscription<geometry_msgs::msg::Twist>(
            "cmd_vel", 10, std::bind(&MotorSubscriber::cmdVelCallback, this, _1));
    }

  private:
    uint8_t getId()
    {
        uint8_t id = 0;
        int ret = i2c_read(&_i2cDevice, MotorCommand_Id, &id, 1);
        if (ret == -1 || (size_t)ret != 1)
        {
            RCLCPP_INFO(rclcpp::get_logger("motor"), "failed to read motor id: [%d]", ret);
        }

        return id;
    }

    // interesting that ready != busy
    bool ready()
    {
        uint8_t status = 0;
        int ret = i2c_read(&_i2cDevice, MotorCommand_Status, &status, 1);
        if (ret == -1 || (size_t)ret != 1)
        {
            RCLCPP_INFO(rclcpp::get_logger("motor"), "failed to read motor status: [%d]", ret);
        }

        if (status != 0xFF &&
            (status & MotorStatusBit_Enum) == MotorStatusBit_Enum)
        {
            return true;
        }
        else
        {
            return false;
        }
    }

    bool busy()
    {
        uint8_t status = 0;
        int ret = i2c_read(&_i2cDevice, MotorCommand_Status, &status, 1);
        if (ret == -1 || (size_t)ret != 1)
        {
            RCLCPP_INFO(rclcpp::get_logger("motor"), "failed to read motor status: [%d]", ret);
        }

        if (status & (MotorStatusBit_Busy | MotorStatusBit_Read | MotorStatusBit_Write))
        {
            return true;
        }
        else
        {
            return false;
        }
    }

    void enable()
    {
        command(MotorCommand_Enable, 0x01);
    }

    void disable()
    {
        command(MotorCommand_Enable, 0x00);
    }

    void cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg)
    {
        //enable();

        double speedRight = msg->linear.x + msg->angular.z;
        double speedLeft = msg->linear.x - msg->angular.z;

        motor(0, (_rightInvert? -1.0 : 1.0) * (speedRight / _wheelRadius) * _powerScale);
        motor(1, (_leftInvert? -1.0 : 1.0) * (speedLeft / _wheelRadius) * _powerScale);
    }

    void motor(uint8_t channel, double power)
    {
        double powerAdjustment = std::abs(power);
        if (powerAdjustment > 1.0)
        {
            powerAdjustment = 1.0;
        }

        // Motor controller does 0 +/- 255, with 128 as median
        uint8_t powerLevel = (uint8_t)(powerAdjustment * 127.0);

        if (power < 0.0)
        {
            command(MotorCommand_Drive0 + channel, kMotorNeutral - powerLevel);
        }
        else
        {
            command(MotorCommand_Drive0 + channel, kMotorNeutral + powerLevel);
        }

    }

    void command(uint8_t command, uint8_t value)
    {
        #ifndef _WIN32
        int ret = i2c_ioctl_write(&_i2cDevice, command, &value, 1);
        if (ret == -1 || (size_t)ret != 1)
        {
            RCLCPP_INFO(rclcpp::get_logger("motor"), "failed to write to motor controller: [%d]", ret);
        }
        #endif
    }

    rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr _subscription;

    #ifndef _WIN32
    int _i2cFileDescriptor;
    I2CDevice _i2cDevice;
    #endif

    float _wheelSeparation;
    float _wheelRadius;
    float _powerScale;
    bool _leftInvert;
    bool _rightInvert;
    uint8_t _id;
};

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);

    auto node = std::make_shared<MotorSubscriber>();

    node->declare_parameter("wheelSeparation");
    node->declare_parameter("wheelRadius");

    node->start();

    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

ext: cpp


text:
//==============================================================================
//         Copyright 2003 - 2012 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_SWAR_FUNCTIONS_SIMD_COMMON_INTERLEAVE_SECOND_HPP_INCLUDED
#define BOOST_SIMD_SWAR_FUNCTIONS_SIMD_COMMON_INTERLEAVE_SECOND_HPP_INCLUDED

#include <boost/simd/swar/functions/interleave_second.hpp>
#include <boost/simd/include/functions/simd/insert.hpp>
#include <boost/simd/include/functions/simd/extract.hpp>
#include <boost/simd/include/functions/simd/bitwise_cast.hpp>
#include <boost/simd/sdk/meta/is_bitwise_logical.hpp>
#include <boost/simd/sdk/meta/as_arithmetic.hpp>
#include <boost/simd/sdk/meta/cardinal_of.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( interleave_second_, tag::cpu_
                                    , (A0)(A1)(X)
                                    , ((simd_< unspecified_<A0>, X>))
                                      ((simd_< unspecified_<A1>, X>))
                                    )
  {
    typedef A0 result_type;

    result_type operator()(A0 const& a0, A1 const& a1) const
    {
      result_type that;
      const std::size_t n = meta::cardinal_of<A0>::value;
      std::size_t  middle = meta::cardinal_of<A0>::value/2;
      for(std::size_t i=0;i<n/2;++i)
      {
        that[i*2]   = a0[middle];
        that[i*2+1] = a1[middle];
        middle++;
      }
      return that;
    }
  };

  BOOST_DISPATCH_IMPLEMENT_IF         ( interleave_second_, tag::cpu_
                                      , (A0)(X)
                                      , ( boost::simd::meta::is_bitwise_logical<A0> )
                                      , ((simd_< logical_<A0>, X>))
                                        ((simd_< logical_<A0>, X>))
                                      )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename meta::as_arithmetic<A0>::type type;
      return bitwise_cast<result_type>(
        interleave_second( bitwise_cast<type>(a0), bitwise_cast<type>(a1) )
      );
    }
  };
} } }

#endif

ext: hpp


text:
//
//  Copyright (C) 2016  Clifford Wolf <clifford@clifford.at>
//  Copyright (C) 2019  Sylvain Munaut <tnt@246tNt.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 <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <stdint.h>
#ifdef _WIN32
#define NOMINMAX
#include "Windows.h"
#undef NOMINMAX
#else
#include <unistd.h>
#include <sys/time.h>
#endif

#include <map>
#include <vector>
#include <string>
#include <fstream>
#include <iostream>

#include <boost/program_options.hpp>

#include "ChipConfig.hpp"
#include "Chip.hpp"
#include "Database.hpp"
#include "DatabasePath.hpp"

using std::map;
using std::pair;
using std::vector;
using std::string;
using std::ifstream;
using std::getline;

uint64_t x;
uint64_t xorshift64star(void) {
    x ^= x >> 12; // a
    x ^= x << 25; // b
    x ^= x >> 27; // c
    return x * UINT64_C(2685821657736338717);
}

void push_back_bitvector(vector<vector<bool>> &hexfile, const vector<int> &digits)
{
    if (digits.empty())
        return;

    hexfile.push_back(vector<bool>(digits.size() * 4));

    for (int i = 0; i < int(digits.size()) * 4; i++)
        if ((digits.at(digits.size() - i/4 -1) & (1 << (i%4))) != 0)
            hexfile.back().at(i) = true;
}

void parse_hexfile_line(const char *filename, int linenr, vector<vector<bool>> &hexfile, string &line, int &address)
{
    vector<int> digits;
    bool reading_address = false;

    for (char c : line) {
        if ('0' <= c && c <= '9')
            digits.push_back(c - '0');
        else if ('a' <= c && c <= 'f')
            digits.push_back(10 + c - 'a');
        else if ('A' <= c && c <= 'F')
            digits.push_back(10 + c - 'A');
        else if ('x' == c || 'X' == c ||
             'z' == c || 'Z' == c)
            digits.push_back(0);
        else if ('_' == c)
            ;
	else if ('@' == c) {
	    if (reading_address || !digits.empty())
		goto error;
	    else
		reading_address = true;
        } else if (' ' == c || '\t' == c || '\r' == c) {
	    if (reading_address) {
		int file_address = 0;
		for (int i = 0; i < int(digits.size()); i++ ) {
		    file_address <<= 4;
		    file_address |= digits.at(i);
		}
		if (file_address != address) {
		    fprintf(stderr, "Non-contiguous address (expected @%X) at line %d of %s: %s\n", address, linenr, filename, line.c_str());
		    exit(1);
		}
	    } else {
		push_back_bitvector(hexfile, digits);
		if( !digits.empty() )
		    address++;
	    }
	    digits.clear();
	    reading_address = false;
        } else goto error;
    }

    push_back_bitvector(hexfile, digits);

    return;

error:
    fprintf(stderr, "Can't parse line %d of %s: %s\n", linenr, filename, line.c_str());
    exit(1);
}

int main(int argc, char **argv)
{
    bool verbose = false;
#ifdef _WIN32
    uint32_t seed_nr = GetCurrentProcessId();
#else
    uint32_t seed_nr = getpid();
#endif
    namespace po = boost::program_options;

    std::string database_folder = get_database_path();;

    po::options_description options("Allowed options");

    po::options_description options_any("Generic options");
    options_any.add_options()("help,h", "show help");
    options_any.add_options()("verbose,v", "verbose output");

    po::options_description options_init("Initialize options");
    options_init.add_options()("input,i", po::value<std::string>(), "input configuration file");
    options_init.add_options()("output,o", po::value<std::string>(), "output configuration file");
    options_init.add_options()("from,f", po::value<std::string>(), "original content hex file");
    options_init.add_options()("to,t", po::value<std::string>(), "new content hex file");

    po::options_description options_gen("Generate options");
    options_gen.add_options()("generate,g", po::value<std::string>(), "Generate random hex of given geometry into given file");
    options_gen.add_options()("seed,s", po::value<int>(), "seed random generator with fixed value");
    options_gen.add_options()("width,w", po::value<int>(), "width of the BRAM content (in bits)");
    options_gen.add_options()("depth,d", po::value<int>(), "idepth of the BRAM content (in # of words)");

    options.add(options_any).add(options_init).add(options_gen);

    po::variables_map vm;
    try {
        po::parsed_options parsed = po::command_line_parser(argc, argv).options(options).run();
        po::store(parsed, vm);
        po::notify(vm);
    }
    catch (std::exception &e) {
        cerr << e.what() << endl << endl;
        goto help;
    }

    if (vm.count("help")) {
    help:
        cerr << "Project Trellis - Open Source Tools for ECP5 FPGAs" << endl;
        cerr << argv[0] << ": ECP5 BRAM content initialization tool" << endl;
        cerr << endl;
        cerr << "Copyright (C) 2019  Sylvain Munaut <tnt@246tNt.com>" << endl;
        cerr << "Copyright (C) 2016  Clifford Wolf <clifford@clifford.at>" << endl;
        cerr << endl;
        cerr << options << endl;
        return vm.count("help") ? 0 : 1;
    }

    if (vm.count("verbose"))
        verbose = true;

    if (vm.count("generate"))
    {
        if (!vm.count("width") || !vm.count("depth"))
            goto help;

        int width = vm.at("width").as<int>();
        int depth = vm.at("depth").as<int>();

        if (width <= 0 || width % 4 != 0) {
            fprintf(stderr, "Hexfile width (%d bits) is not divisible by 4 or nonpositive!\n", width);
            return 1;
        }

        if (depth <= 0 || depth % 512 != 0) {
            fprintf(stderr, "Hexfile number of words (%d) is not divisible by 512 or nonpositive!\n", depth);
            return 1;
        }

        if (vm.count("seed")) {
            seed_nr = vm.at("seed").as<int>();

            if (verbose)
                fprintf(stderr, "Seed: %d\n", seed_nr);
        }

        x  =  uint64_t(seed_nr) << 32;
        x ^= uint64_t(seed_nr) << 20;
        x ^= uint64_t(seed_nr);

        x ^= uint64_t(depth) << 16;
        x ^= uint64_t(width) << 10;

        xorshift64star();
        xorshift64star();
        xorshift64star();

        if (!vm.count("seed")) {
#ifdef _WIN32
            SYSTEMTIME system_time;
            FILETIME file_time;
            uint64_t time;
            GetSystemTime(&system_time);
            SystemTimeToFileTime(&system_time, &file_time);
            x ^= uint64_t(file_time.dwLowDateTime);
            x ^= uint64_t(file_time.dwHighDateTime) << 32;
#else
            struct timeval tv;
            gettimeofday(&tv, NULL);
            x ^= uint64_t(tv.tv_sec) << 20;
            x ^= uint64_t(tv.tv_usec);
#endif
        }

        xorshift64star();
        xorshift64star();
        xorshift64star();

        ofstream romfile(vm.at("generate").as<std::string>());

        for (int i = 0; i < depth; i++) {
            for (int j = 0; j < width / 4; j++) {
                int digit = xorshift64star() & 15;
                romfile << "0123456789abcdef"[digit];
            }
            romfile << std::endl;
        }

        return 0;
    }

    if (!vm.count("input") || !vm.count("output") || !vm.count("from") || !vm.count("to"))
        goto help;

    // -------------------------------------------------------
    // Load from_hexfile and to_hexfile

    const char *from_hexfile_n = vm.at("from").as<std::string>().c_str();
    ifstream from_hexfile_f(from_hexfile_n);
    vector<vector<bool>> from_hexfile;

    const char *to_hexfile_n = vm.at("to").as<std::string>().c_str();
    ifstream to_hexfile_f(to_hexfile_n);
    vector<vector<bool>> to_hexfile;

    string line;

    for (int i = 1, address = 0; getline(from_hexfile_f, line); i++)
        parse_hexfile_line(from_hexfile_n, i, from_hexfile, line, address);

    for (int i = 1, address = 0; getline(to_hexfile_f, line); i++)
        parse_hexfile_line(to_hexfile_n, i, to_hexfile, line, address);

    if (to_hexfile.size() > 0 && from_hexfile.size() > to_hexfile.size()) {
        if (verbose)
            fprintf(stderr, "Padding to_hexfile from %d words to %d\n",
                int(to_hexfile.size()), int(from_hexfile.size()));
        do
            to_hexfile.push_back(vector<bool>(to_hexfile.at(0).size()));
        while (from_hexfile.size() > to_hexfile.size());
    }

    if (from_hexfile.size() != to_hexfile.size()) {
        fprintf(stderr, "Hexfiles have different number of words! (%d vs. %d)\n", int(from_hexfile.size()), int(to_hexfile.size()));
        return 1;
    }

    if (from_hexfile.size() % 512 != 0) {
        fprintf(stderr, "Hexfile number of words (%d) is not divisible by 512!\n", int(from_hexfile.size()));
        return 1;
    }

    for (size_t i = 1; i < from_hexfile.size(); i++)
        if (from_hexfile.at(i-1).size() != from_hexfile.at(i).size()) {
            fprintf(stderr, "Inconsistent word width at line %d of %s!\n", int(i), from_hexfile_n);
            return 1;
        }

    for (size_t i = 1; i < to_hexfile.size(); i++) {
        while (to_hexfile.at(i-1).size() > to_hexfile.at(i).size())
            to_hexfile.at(i).push_back(false);
        if (to_hexfile.at(i-1).size() != to_hexfile.at(i).size()) {
            fprintf(stderr, "Inconsistent word width at line %d of %s!\n", int(i+1), to_hexfile_n);
            return 1;
        }
    }

    if (from_hexfile.size() == 0 || from_hexfile.at(0).size() == 0) {
        fprintf(stderr, "Empty from/to hexfiles!\n");
        return 1;
    }

    if (verbose)
        fprintf(stderr, "Loaded pattern for %d bits wide and %d words deep memory.\n", int(from_hexfile.at(0).size()), int(from_hexfile.size()));

    // -------------------------------------------------------
    // Create bitslices from pattern data

    map<vector<bool>, pair<vector<bool>, int>> pattern;

    for (int i = 0; i < int(from_hexfile.at(0).size()); i++)
    {
        vector<bool> pattern_from, pattern_to;

        for (int j = 0; j < int(from_hexfile.size()); j++)
        {
            pattern_from.push_back(from_hexfile.at(j).at(i));
            pattern_to.push_back(to_hexfile.at(j).at(i));

            if (pattern_from.size() == 512) {
                if (pattern.count(pattern_from)) {
                    fprintf(stderr, "Conflicting from pattern for bit slice from_hexfile[%d:%d][%d]!\n", j, j-255, i);
                    return 1;
                }
                pattern[pattern_from] = std::make_pair(pattern_to, 0);
                pattern_from.clear(), pattern_to.clear();
            }
        }

        assert(pattern_from.empty());
        assert(pattern_to.empty());
    }

    if (verbose)
        fprintf(stderr, "Extracted %d bit slices from from/to hexfile data.\n", int(pattern.size()));

    // -------------------------------------------------------
    // Load database and config

    ifstream config_file(vm.at("input").as<string>());

    try {
        Trellis::load_database(database_folder);
    } catch (runtime_error &e) {
        cerr << "Failed to load Trellis database: " << e.what() << endl;
        return 1;
    }

    string textcfg((std::istreambuf_iterator<char>(config_file)), std::istreambuf_iterator<char>());

    Trellis::ChipConfig cc;
    try {
        cc = Trellis::ChipConfig::from_string(textcfg);
    } catch (runtime_error &e) {
        cerr << "Failed to process input config: " << e.what() << endl;
        return 1;
    }

    // -------------------------------------------------------
    // Replace bram data

    int max_replace_cnt = 0;

    for (auto &bram_it : cc.bram_data)
    {
        auto &bram_data = bram_it.second;

        const int W[] =  {  1,  2,  4,  9, 18, 36 };
        const int NW[] = {  8,  8,  8,  9,  9,  9 };
        const int B[]  = { 32, 32, 32, 36, 36, 36 };

        for (int i = 0; i < 6; i++)
        {
            for (int j = 0; j < B[i]; j++)
            {
                vector<bool> from_bitslice;

                for (int k = 0; k < 512; k++)
                {
                    int bn = (k * W[i]) + (j % W[i]) + ((j/W[i]) * 512 * W[i]);
                    int word  = bn / NW[i];
                    int bit   = bn % NW[i];

                    from_bitslice.push_back((bram_data.at(word) & (1 << bit)) != 0);
                }

                auto p = pattern.find(from_bitslice);
                if (p != pattern.end())
                {
                    auto &to_bitslice = p->second.first;

                    for (int k = 0; k < 512; k++)
                    {
                        int bn = (k * W[i]) + (j % W[i]) + ((j/W[i]) * 512 * W[i]);
                        int word  = bn / NW[i];
                        int bit   = bn % NW[i];

                        if (to_bitslice.at(k))
                            bram_data.at(word) |=  (1 << bit);
                        else
                            bram_data.at(word) &= ~(1 << bit);
                    }

                    max_replace_cnt = std::max(++p->second.second, max_replace_cnt);
                }
            }
        }
    }

    // -------------------------------------------------------
    // Save the new config

    ofstream new_config_file(vm.at("output").as<std::string>());
    new_config_file << cc.to_string();

    return 0;
}