<commit_before>/**
 * @file DartLoader.cpp
 */

#include "DartLoader.h"
#include <map>

#include <urdf_parser/urdf_parser.h>
#include <urdf_world/world.h>
#include "urdf_world_parser.h"

#include <iostream>
#include <fstream>
#include "dart/dynamics/Skeleton.h"
#include "dart/dynamics/BodyNode.h"
#include "dart/dynamics/Joint.h"
#include "dart/dynamics/RevoluteJoint.h"
#include "dart/dynamics/PrismaticJoint.h"
#include "dart/dynamics/WeldJoint.h"
#include "dart/dynamics/FreeJoint.h"
#include "dart/dynamics/GenCoord.h"
#include "dart/dynamics/Shape.h"
#include "dart/dynamics/BoxShape.h"
#include "dart/dynamics/EllipsoidShape.h"
#include "dart/dynamics/CylinderShape.h"
#include "dart/dynamics/MeshShape.h"
#include "dart/simulation/World.h"

namespace dart {
namespace utils {

/**
 * @function parseSkeleton
 */
dynamics::Skeleton* DartLoader::parseSkeleton(std::string _urdfFileName) {

    urdf::ModelInterface* skeletonModel = urdf::parseURDF(readFileToString(_urdfFileName)).get();
    if(!skeletonModel)
        return NULL;

    // Change path to a Unix-style path if given a Windows one
    // Windows can handle Unix-style paths (apparently)
    std::replace(_urdfFileName.begin(), _urdfFileName.end(), '\\' , '/' );
    std::string skelDirectory = _urdfFileName.substr(0, _urdfFileName.rfind("/") + 1);

    return modelInterfaceToSkeleton(skeletonModel, skelDirectory);
}

/**
 * @function parseWorld
 */
simulation::World* DartLoader::parseWorld(std::string _urdfFileName) {

    std::string xmlString = readFileToString(_urdfFileName);

    // Change path to a Unix-style path if given a Windows one
    // Windows can handle Unix-style paths (apparently)
    std::replace(_urdfFileName.begin(), _urdfFileName.end(), '\\' , '/');
    std::string worldDirectory = _urdfFileName.substr(0, _urdfFileName.rfind("/") + 1);

    urdf::World* worldInterface = urdf::parseWorldURDF(xmlString, worldDirectory);
    if(!worldInterface)
        return NULL;

    // Store paths from world to entities
    parseWorldToEntityPaths(xmlString);

    simulation::World* world = new simulation::World();

    for(unsigned int i = 0; i < worldInterface->models.size(); ++i) {
      std::string skeletonDirectory = worldDirectory + mWorld_To_Entity_Paths.find(worldInterface->models[i].model->getName())->second;
      dynamics::Skeleton* skeleton = modelInterfaceToSkeleton(worldInterface->models[i].model.get(), skeletonDirectory);

      if(!skeleton) {
        std::cout << "[ERROR] Robot " << worldInterface->models[i].model->getName() << " was not correctly parsed. World is not loaded. Exiting!" << std::endl;
        return NULL;
      }

      // Initialize position and RPY
      dynamics::Joint* rootJoint = skeleton->getRootBodyNode()->getParentJoint();
      Eigen::Isometry3d transform = toEigen(worldInterface->models[i].origin);

      if(dynamic_cast<dynamics::FreeJoint*>(rootJoint)) {
          Eigen::Vector6d coordinates;
          coordinates << math::logMap(transform.linear()), transform.translation();
          rootJoint->set_q(coordinates);
      }
      else {
          rootJoint->setTransformFromParentBodyNode(transform);
      }

      world->addSkeleton(skeleton);
    }
    return world;
}

/**
 * @function parseWorldToEntityPaths
 */
void DartLoader::parseWorldToEntityPaths(const std::string &_xml_string) {

    TiXmlDocument xml_doc;
    xml_doc.Parse(_xml_string.c_str());

    TiXmlElement *world_xml = xml_doc.FirstChildElement("world");

    if( !world_xml ) {
      return;
    }

    // Get all include filenames
    std::map<std::string, std::string> includedFiles;

    for( TiXmlElement* include_xml = world_xml->FirstChildElement("include");
      include_xml; include_xml = include_xml->NextSiblingElement("include") ) {

      const char *filename = include_xml->Attribute("filename");
      const char *model_name = include_xml->Attribute("model_name");
      std::string string_filename( filename );
      std::string string_filepath = string_filename.substr( 0, string_filename.rfind("/") + 1 );
      std::string string_model_name( model_name );

      includedFiles[string_model_name] = string_filepath;
    }

    // Get all entities
    for( TiXmlElement* entity_xml = world_xml->FirstChildElement("entity");
      entity_xml; entity_xml = entity_xml->NextSiblingElement("entity") ) {

      // Find model and name for entity, if not, error
      const char* entity_model = entity_xml->Attribute("model");
      const char* entity_name = entity_xml->Attribute("name");

      if( entity_name && entity_model ) {
        std::string string_entity_model( entity_model );
        std::string string_entity_name( entity_name );
        // Find the model
        if( includedFiles.find( string_entity_model ) == includedFiles.end() ) {
          std::cout<<"[!] Did not find entity model included. Weird things may happen"<<std::endl;
          return;
        }
        // Add it
        else {
          std::string string_entity_filepath = includedFiles.find( string_entity_model )->second;
          mWorld_To_Entity_Paths[string_entity_name] = string_entity_filepath;
        }
      }
      // If no name or model is defined
      else {
        std::cout<< "[!] Entity was not defined. Weird things will happen" <<std::endl;
      }

    } // for all entities

}

/**
 * @function modelInterfaceToSkeleton
 * @brief Read the ModelInterface and spits out a Skeleton object
 */
dynamics::Skeleton* DartLoader::modelInterfaceToSkeleton(const urdf::ModelInterface* _model, std::string _rootToSkelPath) {

    if( _rootToSkelPath.empty() ) {
        std::cout<< "[DartLoader] Absolute path to skeleton "<<_model->getName()<<" is not set. Probably I will crash!"<<std::endl;
    }

    dynamics::Skeleton* skeleton = new dynamics::Skeleton(_model->getName());
    dynamics::BodyNode* rootNode;
    dynamics::Joint* rootJoint;

    const urdf::Link* root = _model->getRoot().get();
    if(root->name == "world") {
        if(_model->getRoot()->child_links.size() != 1) {
            std::cout<< "[ERROR] Not unique link attached to world." << std::endl;
        }
        else {
            root = root->child_links[0].get();
            rootNode = createDartNode(root, _rootToSkelPath);
            rootJoint = createDartJoint(root->parent_joint.get());
            if(!rootJoint)
                return NULL;
        }
    }
    else {
        rootNode = createDartNode(root, _rootToSkelPath);
        rootJoint = new dynamics::FreeJoint();
        rootJoint->setName("rootJoint");
        rootJoint->setTransformFromParentBodyNode(Eigen::Isometry3d::Identity());
        rootJoint->setTransformFromChildBodyNode(Eigen::Isometry3d::Identity());
    }

    rootNode->setParentJoint(rootJoint);
    skeleton->addBodyNode(rootNode);

    for(unsigned int i = 0; i < root->child_links.size(); i++) {
        createSkeletonRecursive(skeleton, root->child_links[i].get(), rootNode, _rootToSkelPath);
    }

    return skeleton;
}

void DartLoader::createSkeletonRecursive(dynamics::Skeleton* _skel, const urdf::Link* _lk, dynamics::BodyNode* _parentNode, std::string _rootToSkelPath) {
  dynamics::BodyNode* node = createDartNode(_lk, _rootToSkelPath);
  dynamics::Joint* joint = createDartJoint(_lk->parent_joint.get());
  node->setParentJoint(joint);
  _parentNode->addChildBodyNode(node);
  _skel->addBodyNode(node);

  for(unsigned int i = 0; i < _lk->child_links.size(); i++) {
      createSkeletonRecursive(_skel, _lk->child_links[i].get(), node, _rootToSkelPath);
  }
}

/**
 * @function readXml
 */
std::string  DartLoader::readFileToString(std::string _xmlFile) {

  std::string xml_string;
  std::ifstream xml_file(_xmlFile.c_str());

  // Read xml
  while(xml_file.good()) {
    std::string line;
    std::getline(xml_file, line);
    xml_string += (line + "\n");
  }
  xml_file.close();

  return xml_string;
}

/**
 * @function createDartJoint
 */
dynamics::Joint* DartLoader::createDartJoint(const urdf::Joint* _jt)
{
  dynamics::Joint* joint;
  switch(_jt->type) {
  case urdf::Joint::REVOLUTE:
      joint = new dynamics::RevoluteJoint(toEigen(_jt->axis));
      joint->getGenCoord(0)->set_qMin(_jt->limits->lower);
      joint->getGenCoord(0)->set_qMax(_jt->limits->upper);
      break;
  case urdf::Joint::CONTINUOUS:
      joint = new dynamics::RevoluteJoint(toEigen(_jt->axis));
      break;
  case urdf::Joint::PRISMATIC:
      joint = new dynamics::PrismaticJoint(toEigen(_jt->axis));
      joint->getGenCoord(0)->set_qMin(_jt->limits->lower);
      joint->getGenCoord(0)->set_qMax(_jt->limits->upper);
      break;
  case urdf::Joint::FIXED:
      joint = new dynamics::WeldJoint();
      break;
  case urdf::Joint::FLOATING:
      joint = new dynamics::FreeJoint();
      break;
  case urdf::Joint::PLANAR:
      std::cout << "Planar joint not supported." << std::endl;
      assert(false);
      return NULL;
  default:
      std::cout << "Unsupported joint type." << std::endl;
      assert(false);
      return NULL;
  }
  joint->setName(_jt->name);
  joint->setTransformFromParentBodyNode(toEigen(_jt->parent_to_joint_origin_transform));
  joint->setTransformFromChildBodyNode(Eigen::Isometry3d::Identity());
  if(joint->getNumGenCoords() == 1 && _jt->limits) {
    joint->getGenCoord(0)->set_dqMin(-_jt->limits->velocity);
    joint->getGenCoord(0)->set_dqMax(_jt->limits->velocity);
    joint->getGenCoord(0)->set_tauMin(-_jt->limits->effort);
    joint->getGenCoord(0)->set_tauMax(_jt->limits->effort);
  }
  return joint;
}

/**
 * @function createDartNode
 */
dynamics::BodyNode* DartLoader::createDartNode(const urdf::Link* _lk, std::string _rootToSkelPath) {

  dynamics::BodyNode* node = new dynamics::BodyNode(_lk->name);

  // Mesh Loading
  //FIXME: Shouldn't mass and inertia default to 0?
  double mass = 0.1;
  Eigen::Matrix3d inertia = Eigen::MatrixXd::Identity(3,3);
  inertia *= 0.1;

  // Load Inertial information
  if(_lk->inertial) {
      node->setLocalCOM(toEigen(_lk->inertial->origin.position));
      node->setMass(_lk->inertial->mass);
      node->setInertia(_lk->inertial->ixx, _lk->inertial->iyy, _lk->inertial->izz,
                               _lk->inertial->ixy, _lk->inertial->ixz, _lk->inertial->iyz);
  }

  // Set visual information
  for(unsigned int i = 0; i < _lk->visual_array.size(); i++) {
    if(dynamics::Shape* shape = createShape(_lk->visual_array[i].get(), _rootToSkelPath)) {
      node->addVisualizationShape(shape);
    }
  }

  // Set collision information
  for(unsigned int i = 0; i < _lk->collision_array.size(); i++) {
    if(dynamics::Shape* shape = createShape(_lk->collision_array[i].get(), _rootToSkelPath)) {
      node->addCollisionShape(shape);
    }
  }

  return node;
}

void setMaterial(dynamics::Shape* _shape, const urdf::Visual* _viz) {
  if(_viz->material) {
    _shape->setColor(Eigen::Vector3d(_viz->material->color.r, _viz->material->color.g, _viz->material->color.b));
  }
}

void setMaterial(dynamics::Shape* _shape, const urdf::Collision* _col) {
}

/**
 * @function createShape
 */
template <class VisualOrCollision>
dynamics::Shape* DartLoader::createShape(const VisualOrCollision* _vizOrCol, std::string _rootToSkelPath) {
  dynamics::Shape* shape;

  // Sphere
  if(urdf::Sphere* sphere = dynamic_cast<urdf::Sphere*>(_vizOrCol->geometry.get())) {
    shape = new dynamics::EllipsoidShape(2.0 * sphere->radius * Eigen::Vector3d::Ones());
  }

  // Box
  else if(urdf::Box* box = dynamic_cast<urdf::Box*>(_vizOrCol->geometry.get())) {
    shape = new dynamics::BoxShape(Eigen::Vector3d(box->dim.x, box->dim.y, box->dim.z));
  }

  // Cylinder
  else if(urdf::Cylinder* cylinder = dynamic_cast<urdf::Cylinder*>(_vizOrCol->geometry.get())) {
    shape = new dynamics::CylinderShape(cylinder->radius, cylinder->length);
  }

  // Mesh
  else if(urdf::Mesh* mesh = dynamic_cast<urdf::Mesh*>(_vizOrCol->geometry.get())) {
    std::string fullPath = _rootToSkelPath + mesh->filename;
    const aiScene* model = dynamics::MeshShape::loadMesh( fullPath );

    if(!model) {
      std::cout<< "[add_Shape] [ERROR] Not loading model " << fullPath << " (NULL) \n";
      shape = NULL;
    }
    else {
      shape = new dynamics::MeshShape(Eigen::Vector3d(mesh->scale.x, mesh->scale.y, mesh->scale.z), model);
    }
  }

  // Unknown geometry type
  else {
    std::cout << "[add_Shape] No MESH, BOX, CYLINDER OR SPHERE! Not loading this shape." << std::endl;
    return NULL;
  }

  shape->setLocalTransform(toEigen(_vizOrCol->origin));
  setMaterial(shape, _vizOrCol);
  return shape;
}

template dynamics::Shape* DartLoader::createShape<urdf::Visual>(const urdf::Visual* _vizOrCol, std::string _rootToSkelPath);
template dynamics::Shape* DartLoader::createShape<urdf::Collision>(const urdf::Collision* _vizOrCol, std::string _rootToSkelPath);

/**
 * @function pose2Affine3d
 */
Eigen::Isometry3d DartLoader::toEigen(const urdf::Pose& _pose) {
    Eigen::Quaterniond quat;
    _pose.rotation.getQuaternion(quat.x(), quat.y(), quat.z(), quat.w());
    Eigen::Isometry3d transform(quat);
    transform.translation() = Eigen::Vector3d(_pose.position.x, _pose.position.y, _pose.position.z);
    return transform;
}

Eigen::Vector3d DartLoader::toEigen(const urdf::Vector3& _vector) {
    return Eigen::Vector3d(_vector.x, _vector.y, _vector.z);
}

} // namespace utils
} // namespace dart
<commit_msg>Fix use of boost::shared_ptr in DartLoader::parseSkeleton()<commit_after>/**
 * @file DartLoader.cpp
 */

#include "DartLoader.h"
#include <map>

#include <urdf_parser/urdf_parser.h>
#include <urdf_world/world.h>
#include "urdf_world_parser.h"

#include <iostream>
#include <fstream>
#include "dart/dynamics/Skeleton.h"
#include "dart/dynamics/BodyNode.h"
#include "dart/dynamics/Joint.h"
#include "dart/dynamics/RevoluteJoint.h"
#include "dart/dynamics/PrismaticJoint.h"
#include "dart/dynamics/WeldJoint.h"
#include "dart/dynamics/FreeJoint.h"
#include "dart/dynamics/GenCoord.h"
#include "dart/dynamics/Shape.h"
#include "dart/dynamics/BoxShape.h"
#include "dart/dynamics/EllipsoidShape.h"
#include "dart/dynamics/CylinderShape.h"
#include "dart/dynamics/MeshShape.h"
#include "dart/simulation/World.h"

namespace dart {
namespace utils {

/**
 * @function parseSkeleton
 */
dynamics::Skeleton* DartLoader::parseSkeleton(std::string _urdfFileName) {

    boost::shared_ptr<urdf::ModelInterface> skeletonModelPtr = urdf::parseURDF(readFileToString(_urdfFileName));
    if(!skeletonModelPtr)
        return NULL;

    // Change path to a Unix-style path if given a Windows one
    // Windows can handle Unix-style paths (apparently)
    std::replace(_urdfFileName.begin(), _urdfFileName.end(), '\\' , '/' );
    std::string skelDirectory = _urdfFileName.substr(0, _urdfFileName.rfind("/") + 1);

    return modelInterfaceToSkeleton(skeletonModelPtr.get(), skelDirectory);
}

/**
 * @function parseWorld
 */
simulation::World* DartLoader::parseWorld(std::string _urdfFileName) {

    std::string xmlString = readFileToString(_urdfFileName);

    // Change path to a Unix-style path if given a Windows one
    // Windows can handle Unix-style paths (apparently)
    std::replace(_urdfFileName.begin(), _urdfFileName.end(), '\\' , '/');
    std::string worldDirectory = _urdfFileName.substr(0, _urdfFileName.rfind("/") + 1);

    urdf::World* worldInterface = urdf::parseWorldURDF(xmlString, worldDirectory);
    if(!worldInterface)
        return NULL;

    // Store paths from world to entities
    parseWorldToEntityPaths(xmlString);

    simulation::World* world = new simulation::World();

    for(unsigned int i = 0; i < worldInterface->models.size(); ++i) {
      std::string skeletonDirectory = worldDirectory + mWorld_To_Entity_Paths.find(worldInterface->models[i].model->getName())->second;
      dynamics::Skeleton* skeleton = modelInterfaceToSkeleton(worldInterface->models[i].model.get(), skeletonDirectory);

      if(!skeleton) {
        std::cout << "[ERROR] Robot " << worldInterface->models[i].model->getName() << " was not correctly parsed. World is not loaded. Exiting!" << std::endl;
        return NULL;
      }

      // Initialize position and RPY
      dynamics::Joint* rootJoint = skeleton->getRootBodyNode()->getParentJoint();
      Eigen::Isometry3d transform = toEigen(worldInterface->models[i].origin);

      if(dynamic_cast<dynamics::FreeJoint*>(rootJoint)) {
          Eigen::Vector6d coordinates;
          coordinates << math::logMap(transform.linear()), transform.translation();
          rootJoint->set_q(coordinates);
      }
      else {
          rootJoint->setTransformFromParentBodyNode(transform);
      }

      world->addSkeleton(skeleton);
    }
    return world;
}

/**
 * @function parseWorldToEntityPaths
 */
void DartLoader::parseWorldToEntityPaths(const std::string &_xml_string) {

    TiXmlDocument xml_doc;
    xml_doc.Parse(_xml_string.c_str());

    TiXmlElement *world_xml = xml_doc.FirstChildElement("world");

    if( !world_xml ) {
      return;
    }

    // Get all include filenames
    std::map<std::string, std::string> includedFiles;

    for( TiXmlElement* include_xml = world_xml->FirstChildElement("include");
      include_xml; include_xml = include_xml->NextSiblingElement("include") ) {

      const char *filename = include_xml->Attribute("filename");
      const char *model_name = include_xml->Attribute("model_name");
      std::string string_filename( filename );
      std::string string_filepath = string_filename.substr( 0, string_filename.rfind("/") + 1 );
      std::string string_model_name( model_name );

      includedFiles[string_model_name] = string_filepath;
    }

    // Get all entities
    for( TiXmlElement* entity_xml = world_xml->FirstChildElement("entity");
      entity_xml; entity_xml = entity_xml->NextSiblingElement("entity") ) {

      // Find model and name for entity, if not, error
      const char* entity_model = entity_xml->Attribute("model");
      const char* entity_name = entity_xml->Attribute("name");

      if( entity_name && entity_model ) {
        std::string string_entity_model( entity_model );
        std::string string_entity_name( entity_name );
        // Find the model
        if( includedFiles.find( string_entity_model ) == includedFiles.end() ) {
          std::cout<<"[!] Did not find entity model included. Weird things may happen"<<std::endl;
          return;
        }
        // Add it
        else {
          std::string string_entity_filepath = includedFiles.find( string_entity_model )->second;
          mWorld_To_Entity_Paths[string_entity_name] = string_entity_filepath;
        }
      }
      // If no name or model is defined
      else {
        std::cout<< "[!] Entity was not defined. Weird things will happen" <<std::endl;
      }

    } // for all entities

}

/**
 * @function modelInterfaceToSkeleton
 * @brief Read the ModelInterface and spits out a Skeleton object
 */
dynamics::Skeleton* DartLoader::modelInterfaceToSkeleton(const urdf::ModelInterface* _model, std::string _rootToSkelPath) {

    if( _rootToSkelPath.empty() ) {
        std::cout<< "[DartLoader] Absolute path to skeleton "<<_model->getName()<<" is not set. Probably I will crash!"<<std::endl;
    }

    dynamics::Skeleton* skeleton = new dynamics::Skeleton(_model->getName());
    dynamics::BodyNode* rootNode;
    dynamics::Joint* rootJoint;

    const urdf::Link* root = _model->getRoot().get();
    if(root->name == "world") {
        if(_model->getRoot()->child_links.size() != 1) {
            std::cout<< "[ERROR] Not unique link attached to world." << std::endl;
        }
        else {
            root = root->child_links[0].get();
            rootNode = createDartNode(root, _rootToSkelPath);
            rootJoint = createDartJoint(root->parent_joint.get());
            if(!rootJoint)
                return NULL;
        }
    }
    else {
        rootNode = createDartNode(root, _rootToSkelPath);
        rootJoint = new dynamics::FreeJoint();
        rootJoint->setName("rootJoint");
        rootJoint->setTransformFromParentBodyNode(Eigen::Isometry3d::Identity());
        rootJoint->setTransformFromChildBodyNode(Eigen::Isometry3d::Identity());
    }

    rootNode->setParentJoint(rootJoint);
    skeleton->addBodyNode(rootNode);

    for(unsigned int i = 0; i < root->child_links.size(); i++) {
        createSkeletonRecursive(skeleton, root->child_links[i].get(), rootNode, _rootToSkelPath);
    }

    return skeleton;
}

void DartLoader::createSkeletonRecursive(dynamics::Skeleton* _skel, const urdf::Link* _lk, dynamics::BodyNode* _parentNode, std::string _rootToSkelPath) {
  dynamics::BodyNode* node = createDartNode(_lk, _rootToSkelPath);
  dynamics::Joint* joint = createDartJoint(_lk->parent_joint.get());
  node->setParentJoint(joint);
  _parentNode->addChildBodyNode(node);
  _skel->addBodyNode(node);

  for(unsigned int i = 0; i < _lk->child_links.size(); i++) {
      createSkeletonRecursive(_skel, _lk->child_links[i].get(), node, _rootToSkelPath);
  }
}

/**
 * @function readXml
 */
std::string  DartLoader::readFileToString(std::string _xmlFile) {

  std::string xml_string;
  std::ifstream xml_file(_xmlFile.c_str());

  // Read xml
  while(xml_file.good()) {
    std::string line;
    std::getline(xml_file, line);
    xml_string += (line + "\n");
  }
  xml_file.close();

  return xml_string;
}

/**
 * @function createDartJoint
 */
dynamics::Joint* DartLoader::createDartJoint(const urdf::Joint* _jt)
{
  dynamics::Joint* joint;
  switch(_jt->type) {
  case urdf::Joint::REVOLUTE:
      joint = new dynamics::RevoluteJoint(toEigen(_jt->axis));
      joint->getGenCoord(0)->set_qMin(_jt->limits->lower);
      joint->getGenCoord(0)->set_qMax(_jt->limits->upper);
      break;
  case urdf::Joint::CONTINUOUS:
      joint = new dynamics::RevoluteJoint(toEigen(_jt->axis));
      break;
  case urdf::Joint::PRISMATIC:
      joint = new dynamics::PrismaticJoint(toEigen(_jt->axis));
      joint->getGenCoord(0)->set_qMin(_jt->limits->lower);
      joint->getGenCoord(0)->set_qMax(_jt->limits->upper);
      break;
  case urdf::Joint::FIXED:
      joint = new dynamics::WeldJoint();
      break;
  case urdf::Joint::FLOATING:
      joint = new dynamics::FreeJoint();
      break;
  case urdf::Joint::PLANAR:
      std::cout << "Planar joint not supported." << std::endl;
      assert(false);
      return NULL;
  default:
      std::cout << "Unsupported joint type." << std::endl;
      assert(false);
      return NULL;
  }
  joint->setName(_jt->name);
  joint->setTransformFromParentBodyNode(toEigen(_jt->parent_to_joint_origin_transform));
  joint->setTransformFromChildBodyNode(Eigen::Isometry3d::Identity());
  if(joint->getNumGenCoords() == 1 && _jt->limits) {
    joint->getGenCoord(0)->set_dqMin(-_jt->limits->velocity);
    joint->getGenCoord(0)->set_dqMax(_jt->limits->velocity);
    joint->getGenCoord(0)->set_tauMin(-_jt->limits->effort);
    joint->getGenCoord(0)->set_tauMax(_jt->limits->effort);
  }
  return joint;
}

/**
 * @function createDartNode
 */
dynamics::BodyNode* DartLoader::createDartNode(const urdf::Link* _lk, std::string _rootToSkelPath) {

  dynamics::BodyNode* node = new dynamics::BodyNode(_lk->name);

  // Mesh Loading
  //FIXME: Shouldn't mass and inertia default to 0?
  double mass = 0.1;
  Eigen::Matrix3d inertia = Eigen::MatrixXd::Identity(3,3);
  inertia *= 0.1;

  // Load Inertial information
  if(_lk->inertial) {
      node->setLocalCOM(toEigen(_lk->inertial->origin.position));
      node->setMass(_lk->inertial->mass);
      node->setInertia(_lk->inertial->ixx, _lk->inertial->iyy, _lk->inertial->izz,
                               _lk->inertial->ixy, _lk->inertial->ixz, _lk->inertial->iyz);
  }

  // Set visual information
  for(unsigned int i = 0; i < _lk->visual_array.size(); i++) {
    if(dynamics::Shape* shape = createShape(_lk->visual_array[i].get(), _rootToSkelPath)) {
      node->addVisualizationShape(shape);
    }
  }

  // Set collision information
  for(unsigned int i = 0; i < _lk->collision_array.size(); i++) {
    if(dynamics::Shape* shape = createShape(_lk->collision_array[i].get(), _rootToSkelPath)) {
      node->addCollisionShape(shape);
    }
  }

  return node;
}

void setMaterial(dynamics::Shape* _shape, const urdf::Visual* _viz) {
  if(_viz->material) {
    _shape->setColor(Eigen::Vector3d(_viz->material->color.r, _viz->material->color.g, _viz->material->color.b));
  }
}

void setMaterial(dynamics::Shape* _shape, const urdf::Collision* _col) {
}

/**
 * @function createShape
 */
template <class VisualOrCollision>
dynamics::Shape* DartLoader::createShape(const VisualOrCollision* _vizOrCol, std::string _rootToSkelPath) {
  dynamics::Shape* shape;

  // Sphere
  if(urdf::Sphere* sphere = dynamic_cast<urdf::Sphere*>(_vizOrCol->geometry.get())) {
    shape = new dynamics::EllipsoidShape(2.0 * sphere->radius * Eigen::Vector3d::Ones());
  }

  // Box
  else if(urdf::Box* box = dynamic_cast<urdf::Box*>(_vizOrCol->geometry.get())) {
    shape = new dynamics::BoxShape(Eigen::Vector3d(box->dim.x, box->dim.y, box->dim.z));
  }

  // Cylinder
  else if(urdf::Cylinder* cylinder = dynamic_cast<urdf::Cylinder*>(_vizOrCol->geometry.get())) {
    shape = new dynamics::CylinderShape(cylinder->radius, cylinder->length);
  }

  // Mesh
  else if(urdf::Mesh* mesh = dynamic_cast<urdf::Mesh*>(_vizOrCol->geometry.get())) {
    std::string fullPath = _rootToSkelPath + mesh->filename;
    const aiScene* model = dynamics::MeshShape::loadMesh( fullPath );

    if(!model) {
      std::cout<< "[add_Shape] [ERROR] Not loading model " << fullPath << " (NULL) \n";
      shape = NULL;
    }
    else {
      shape = new dynamics::MeshShape(Eigen::Vector3d(mesh->scale.x, mesh->scale.y, mesh->scale.z), model);
    }
  }

  // Unknown geometry type
  else {
    std::cout << "[add_Shape] No MESH, BOX, CYLINDER OR SPHERE! Not loading this shape." << std::endl;
    return NULL;
  }

  shape->setLocalTransform(toEigen(_vizOrCol->origin));
  setMaterial(shape, _vizOrCol);
  return shape;
}

template dynamics::Shape* DartLoader::createShape<urdf::Visual>(const urdf::Visual* _vizOrCol, std::string _rootToSkelPath);
template dynamics::Shape* DartLoader::createShape<urdf::Collision>(const urdf::Collision* _vizOrCol, std::string _rootToSkelPath);

/**
 * @function pose2Affine3d
 */
Eigen::Isometry3d DartLoader::toEigen(const urdf::Pose& _pose) {
    Eigen::Quaterniond quat;
    _pose.rotation.getQuaternion(quat.x(), quat.y(), quat.z(), quat.w());
    Eigen::Isometry3d transform(quat);
    transform.translation() = Eigen::Vector3d(_pose.position.x, _pose.position.y, _pose.position.z);
    return transform;
}

Eigen::Vector3d DartLoader::toEigen(const urdf::Vector3& _vector) {
    return Eigen::Vector3d(_vector.x, _vector.y, _vector.z);
}

} // namespace utils
} // namespace dart
<|endoftext|>


<commit_before>/* IBM_PROLOG_BEGIN_TAG                                                   */
/* This is an automatically generated prolog.                             */
/*                                                                        */
/* $Source: src/import/chips/p9/procedures/hwp/pm/p9_pm_ppm_firinit.C $   */
/*                                                                        */
/* OpenPOWER HostBoot Project                                             */
/*                                                                        */
/* Contributors Listed Below - COPYRIGHT 2015,2019                        */
/* [+] International Business Machines Corp.                              */
/*                                                                        */
/*                                                                        */
/* Licensed under the Apache License, Version 2.0 (the "License");        */
/* you may not use this file except in compliance with the License.       */
/* You may obtain a copy of the License at                                */
/*                                                                        */
/*     http://www.apache.org/licenses/LICENSE-2.0                         */
/*                                                                        */
/* Unless required by applicable law or agreed to in writing, software    */
/* distributed under the License is distributed on an "AS IS" BASIS,      */
/* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or        */
/* implied. See the License for the specific language governing           */
/* permissions and limitations under the License.                         */
/*                                                                        */
/* IBM_PROLOG_END_TAG                                                     */
///
/// @file p9_pm_ppm_firinit.C
/// @brief Configures the PPM FIR errors
///
// *HWP HW Owner: Amit Kumar <akumar3@us.ibm.com>
// *HWP Backup HWP Owner: Greg Still <stillgs@us.ibm.com>
// *HWP FW Owner: Sangeetha T S <sangeet2@in.ibm.com>
// *HWP Team: PM
// *HWP Level: 1
// *HWP Consumed by: FSP:HS

///
/// \verbatim
///
///  Procedure Prereq:
///  - completed istep procedure
///
/// High-level procedure flow:
///
///  get all functional child chiplets
///
///  loop over all functional chiplets {
///     calculate address
///     set the error mask in order to mask all errors
///
///  }
///
/// \endverbatim

// ----------------------------------------------------------------------
// Includes
// ----------------------------------------------------------------------
#include <p9_pm_ppm_firinit.H>

// ----------------------------------------------------------------------
// Function definitions
// ----------------------------------------------------------------------

fapi2::ReturnCode p9_pm_ppm_firinit(
    const fapi2::Target<fapi2::TARGET_TYPE_PROC_CHIP>& i_target,
    const p9pm::PM_FLOW_MODE i_mode)
{
    FAPI_IMP("p9_pm_ppm_firinit start");

    FAPI_INF("p9_pm_ppm_firinit end");
    return fapi2::current_err;
}
<commit_msg>p9_pm_ppm_firinit & p9_pm_cme_firinit L2 delivery<commit_after>/* IBM_PROLOG_BEGIN_TAG                                                   */
/* This is an automatically generated prolog.                             */
/*                                                                        */
/* $Source: src/import/chips/p9/procedures/hwp/pm/p9_pm_ppm_firinit.C $   */
/*                                                                        */
/* OpenPOWER HostBoot Project                                             */
/*                                                                        */
/* Contributors Listed Below - COPYRIGHT 2015,2019                        */
/* [+] International Business Machines Corp.                              */
/*                                                                        */
/*                                                                        */
/* Licensed under the Apache License, Version 2.0 (the "License");        */
/* you may not use this file except in compliance with the License.       */
/* You may obtain a copy of the License at                                */
/*                                                                        */
/*     http://www.apache.org/licenses/LICENSE-2.0                         */
/*                                                                        */
/* Unless required by applicable law or agreed to in writing, software    */
/* distributed under the License is distributed on an "AS IS" BASIS,      */
/* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or        */
/* implied. See the License for the specific language governing           */
/* permissions and limitations under the License.                         */
/*                                                                        */
/* IBM_PROLOG_END_TAG                                                     */
///
/// @file p9_pm_ppm_firinit.C
/// @brief Configures the CME FIRs, Mask & Actions
///
// *HWP HW Owner: Amit Kumar <akumar3@us.ibm.com>
// *HWP Backup HWP Owner: Greg Still <stillgs@us.ibm.com>
// *HWP FW Owner: Sangeetha T S <sangeet2@in.ibm.com>
// *HWP Team: PM
// *HWP Level: 2
// *HWP Consumed by: FSP:HS

/// High-level procedure flow:
/// \verbatim
///   if reset:
///      loop over all functional chiplets (core and quad){
///         Mask all bits of error mask
///      }
///   else if init:
///      loop over all functional chiplets (core and quad) {
///         Mask the required bits
///      }
///
/// Procedure Prereq:
///   o System clocks are running
/// \endverbatim

// ----------------------------------------------------------------------
// Includes
// ----------------------------------------------------------------------
#include <p9_pm_ppm_firinit.H>

// ----------------------------------------------------------------------
// Constant Definitions
// ----------------------------------------------------------------------

// ----------------------------------------------------------------------
// Function prototype
// ----------------------------------------------------------------------

/// @brief Initialize the core and quad's error mask registers
///
/// @param[in] i_target   Chip target
///
/// @return FAPI2_RC_SUCCESS if success, else error code.
///
fapi2::ReturnCode pm_ppm_fir_init(
    const fapi2::Target<fapi2::TARGET_TYPE_PROC_CHIP>& i_target);

/// @brief Reset the core and quad's error mask registers
///
/// @param[in] i_target   Chip target
///
/// @return FAPI2_RC_SUCCESS if success, else error code.
///
fapi2::ReturnCode pm_ppm_fir_reset(
    const fapi2::Target<fapi2::TARGET_TYPE_PROC_CHIP>& i_target);

// ----------------------------------------------------------------------
// Function definitions
// ----------------------------------------------------------------------

fapi2::ReturnCode p9_pm_ppm_firinit(
    const fapi2::Target<fapi2::TARGET_TYPE_PROC_CHIP>& i_target,
    const p9pm::PM_FLOW_MODE i_mode)
{
    FAPI_IMP("p9_pm_ppm_firinit start");

    if(i_mode == p9pm::PM_RESET)
    {
        FAPI_TRY(pm_ppm_fir_reset(i_target),
                 "ERROR: Failed to reset the Core and Quad error masks");
    }
    else if(i_mode == p9pm::PM_INIT)
    {
        FAPI_TRY(pm_ppm_fir_init(i_target),
                 "ERROR: Failed to initialize Core and Quad error masks");
    }
    else
    {
        FAPI_ASSERT(false, fapi2::PM_PPM_FIRINIT_BAD_MODE().set_BADMODE(i_mode),
                    "ERROR; Unknown mode passed to p9_pm_ppm_firinit. Mode %x",
                    i_mode);
    }

fapi_try_exit:
    FAPI_INF("p9_pm_ppm_firinit end");
    return fapi2::current_err;
}

fapi2::ReturnCode pm_ppm_fir_init(
    const fapi2::Target<fapi2::TARGET_TYPE_PROC_CHIP>& i_target)
{
    FAPI_IMP("pm_ppm_fir_init start");

    uint8_t l_firinit_done_flag;
    auto l_eqChiplets = i_target.getChildren<fapi2::TARGET_TYPE_EQ>
                        (fapi2::TARGET_STATE_FUNCTIONAL);

    auto l_coreChiplets = i_target.getChildren<fapi2::TARGET_TYPE_CORE>
                          (fapi2::TARGET_STATE_FUNCTIONAL);

    FAPI_TRY(FAPI_ATTR_GET(fapi2::ATTR_PM_FIRINIT_DONE_ONCE_FLAG,
                           i_target, l_firinit_done_flag),
             "ERROR: Failed to fetch the entry status of FIRINIT");

    for (auto l_eq_chplt : l_eqChiplets)
    {
        p9pmFIR::PMFir <p9pmFIR::FIRTYPE_PPM_LFIR> l_qppmFir(l_eq_chplt);

        // As of now I am just masking all the bits.
        // The exact details about which bits to be masked/unmasked
        // is yet to be known
        FAPI_TRY(l_qppmFir.setAllRegBits(p9pmFIR::REG_ERRMASK),
                 "ERROR: Faled to set the Quad Error MASK");

        if (l_firinit_done_flag)
        {
            /* Restore Quad PPM Error Mask */
            FAPI_TRY(l_qppmFir.restoreSavedMask(),
                     "ERROR: Failed to restore the Quad Error mask saved");
        }

        FAPI_TRY(l_qppmFir.put(),
                 "ERROR:Failed to write to the Quad Error mask FIR MASK");
    }

    for (auto l_core_chplt : l_coreChiplets)
    {
        p9pmFIR::PMFir <p9pmFIR::FIRTYPE_PPM_LFIR> l_cppmFir(l_core_chplt);

        // As of now I am just masking all the bits.
        // The exact details about which bits to be masked/unmasked
        // is yet to be known
        FAPI_TRY(l_cppmFir.setAllRegBits(p9pmFIR::REG_ERRMASK),
                 "ERROR: Failed to set the Core Error MASK");

        if (l_firinit_done_flag)
        {
            /* Restore Core PPM Error Mask */
            FAPI_TRY(l_cppmFir.restoreSavedMask(),
                     "ERROR: Failed to restore the Core Error mask saved");
        }

        FAPI_TRY(l_cppmFir.put(),
                 "ERROR:Failed to write to the Core Error mask");
    }

fapi_try_exit:
    return fapi2::current_err;
}

fapi2::ReturnCode pm_ppm_fir_reset(
    const fapi2::Target<fapi2::TARGET_TYPE_PROC_CHIP>& i_target)
{
    FAPI_IMP("pm_ppm_fir_reset start");

    uint8_t l_firinit_done_flag;
    auto l_eqChiplets = i_target.getChildren<fapi2::TARGET_TYPE_EQ>
                        (fapi2::TARGET_STATE_FUNCTIONAL);
    auto l_coreChiplets = i_target.getChildren<fapi2::TARGET_TYPE_CORE>
                          (fapi2::TARGET_STATE_FUNCTIONAL);

    FAPI_TRY(FAPI_ATTR_GET(fapi2::ATTR_PM_FIRINIT_DONE_ONCE_FLAG,
                           i_target, l_firinit_done_flag),
             "ERROR: Failed to fetch the entry status of FIRINIT");

    for (auto l_eq_chplt : l_eqChiplets)
    {
        p9pmFIR::PMFir <p9pmFIR::FIRTYPE_PPM_LFIR> l_ppmFir(l_eq_chplt);

        if (l_firinit_done_flag == 1)
        {
            FAPI_TRY(l_ppmFir.get(p9pmFIR::REG_ERRMASK),
                     "ERROR: Failed to get the QUAD ERROR MASK value");

            /* Fetch the QUAD ERROR MASK; Save it to HWP attribute; clear it */
            FAPI_TRY(l_ppmFir.saveMask(),
                     "ERROR: Failed to save Quad ERROR Mask to the attribute");
        }

        FAPI_TRY(l_ppmFir.setAllRegBits(p9pmFIR::REG_ERRMASK),
                 "ERROR: Faled to set the Quad Error MASK");

        FAPI_TRY(l_ppmFir.put(),
                 "ERROR:Failed to write to the Quad Error MASK");
    }

    for (auto l_c_chplt : l_coreChiplets)
    {
        p9pmFIR::PMFir <p9pmFIR::FIRTYPE_PPM_LFIR> l_cppmFir(l_c_chplt);

        if (l_firinit_done_flag == 1)
        {
            FAPI_TRY(l_cppmFir.get(p9pmFIR::REG_ERRMASK),
                     "ERROR: Failed to get the Core ERROR MASK value");

            /* Fetch the core ERROR MASK; Save it to HWP attribute; clear it */
            FAPI_TRY(l_cppmFir.saveMask(),
                     "ERROR: Failed to save Core ERROR Mask to the attribute");
        }

        FAPI_TRY(l_cppmFir.setAllRegBits(p9pmFIR::REG_ERRMASK),
                 "ERROR: Faled to set the Core Error MASK");

        FAPI_TRY(l_cppmFir.put(),
                 "ERROR:Failed to write to the Core Error MASK");
    }

fapi_try_exit:
    return fapi2::current_err;
}
<|endoftext|>


<commit_before>/* IBM_PROLOG_BEGIN_TAG                                                   */
/* This is an automatically generated prolog.                             */
/*                                                                        */
/* $Source: src/import/chips/p9/procedures/hwp/pm/p9_scan_ring_util.H $   */
/*                                                                        */
/* OpenPOWER HostBoot Project                                             */
/*                                                                        */
/* Contributors Listed Below - COPYRIGHT 2016,2019                        */
/* [+] International Business Machines Corp.                              */
/*                                                                        */
/*                                                                        */
/* Licensed under the Apache License, Version 2.0 (the "License");        */
/* you may not use this file except in compliance with the License.       */
/* You may obtain a copy of the License at                                */
/*                                                                        */
/*     http://www.apache.org/licenses/LICENSE-2.0                         */
/*                                                                        */
/* Unless required by applicable law or agreed to in writing, software    */
/* distributed under the License is distributed on an "AS IS" BASIS,      */
/* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or        */
/* implied. See the License for the specific language governing           */
/* permissions and limitations under the License.                         */
/*                                                                        */
/* IBM_PROLOG_END_TAG                                                     */
#ifndef __P9_SCAN_RING_UTIL_H
#define __P9_SCAN_RING_UTIL_H

#include <fapi2.H>
#include "p9_ringId.H"
#include "p9_stop_util.H"
#include <map>

//#define __CRONUS_VER
///
/// @file   p9_scan_ring_util.H
/// @brief  utility classes and functions for scan ring debug.
///
/// *HWP HWP Owner:      Greg Still <stillgs@us.ibm.com>
/// *HWP FW Owner:       Prem S Jha <premjha2@in.ibm.com>
/// *HWP Team:           PM
/// *HWP Level:          2
/// *HWP Consumed by:    Hostboot:Phyp:Cro
//

namespace p9_hcodeImageBuild
{

/**
 * @brief   enumerates all the platforms associated with hcode image build.
 */
enum PlatId
{
    PLAT_SELF   =   0,
    PLAT_SGPE   =   1,
    PLAT_CME    =   2,
    PLAT_PGPE   =   3,
};

/**
 * @brief   constants associated with fake scan rings.
 */
enum FakeScanRing_t
{
    SCAN_RNG_START      =   0x5363616e20526e67,
    SCAN_RNG_END        =   0x52696e6720456e64,
    MAX_SCAN_RING_SIZE  =   120,
    MAX_FAKE_CMN_RING   =   0x14,
    MAX_FAKE_RING       =   0x1E,
};

/**
 * @brief   debug modes supported for scan rings.
 */
enum RingDebugMode_t
{
    SCAN_RING_NO_DEBUG          =   0x00,
    SCAN_RING_TRACE_DEBUG       =   0x01,
    SCAN_RING_TRACE_DEEP_DEBUG  =   0x02,
};
/**
 * @brief   models a fake ring for debug.
 */
struct FakeRing
{
    uint64_t iv_ringHeader;
    uint64_t iv_ringNumber;
    uint8_t  iv_ringPayload[MAX_SCAN_RING_SIZE];
    uint64_t iv_ringEnd;

#ifdef __CRONUS_VER
    FakeRing( uint8_t i_ringNumber ):
        iv_ringHeader( SWIZZLE_8_BYTE(SCAN_RNG_START) ),
        iv_ringEnd(SWIZZLE_8_BYTE(SCAN_RNG_END))
    {
        memset( &iv_ringPayload, 0xAA, MAX_SCAN_RING_SIZE );
        uint8_t temp1 = i_ringNumber / 100;
        temp1 = temp1 + 0x30;
        iv_ringNumber = temp1 << 16;
        temp1 = i_ringNumber % 100;
        iv_ringNumber |= (( temp1 / 10) + 0x30 ) << 8;
        iv_ringNumber |= ((temp1 % 10) + 0x30 );
        iv_ringNumber = SWIZZLE_8_BYTE(iv_ringNumber);
    }
#else
    FakeRing( uint8_t i_ringNumber ):
        iv_ringHeader( 0 ),
        iv_ringEnd( 0)
    {
        //dummy constructor to avoid compilation issue
        memset( &iv_ringPayload, 0xAA, MAX_SCAN_RING_SIZE );
    }
#endif
};

class RingName
{
    public:
        /**
         * @brief   constructor
         * @param   i_ringName      name associated with the a scan ring.
         * @param   i_debugMode     debug mode.
         */
        RingName( char* i_ringName );

        /**
         * @brief   constructor
         */
        RingName() {};

        /**
         * @brief   destructor
         */
        ~RingName() {};

        /**
         * @brief   returns string Name.
         */
        char* c_str();
    private:
        char iv_ringStr[40];
};

/**
 * @brief   models the profile of a scan ring for debug.
 */
struct RingProfile
{
    RingID iv_ringId;
    uint32_t iv_ringOffset;
    uint32_t iv_ringSize;
    uint32_t iv_chipletPos;

    RingProfile ( RingID i_ringId, uint16_t i_ringOffset, uint16_t i_ringSize ) :
        iv_ringId( i_ringId ),
        iv_ringOffset( i_ringOffset ),
        iv_ringSize( i_ringSize ),
        iv_chipletPos( 0 )
    { }

    RingProfile ( RingID i_ringId, uint8_t i_chipletPos ) :
        iv_ringId( i_ringId ),
        iv_ringOffset( 0 ),
        iv_ringSize( 0 ),
        iv_chipletPos( i_chipletPos )
    { }

    RingProfile():
        iv_ringId(eq_fure),
        iv_ringOffset(0),
        iv_ringSize(0)
    {}
};

/**
 * @brief   populates an input buffer with a fake scan ring.
 * @param   o_pRingBuffer   rings in which scan rings are copied.
 * @param   o_size          size of scan ring.
 * @param   i_ringId        scan ring id.
 * @param   i_plat          platform associated with scan ring.
 */
void getScanRing( void* o_pRingBuffer, uint32_t& o_size,
                  uint32_t i_ringId, PlatId i_plat );

/**
 * @brief   class to suppport debug of scan ring layout in P9 HOMER
 * @note    Intended platform is cronus.
 */
class RingBucket
{
    public:
        /**
         * @brief   constructor
         */
        RingBucket( PlatId i_plat, uint8_t* i_pRingStart, RingDebugMode_t i_debugMode );

        /**
         * @brief   destructor
         */
        ~RingBucket();
        /**
         * @brief   returns the id of common ring from storage index.
         * @param   i_ringPos   storage position of ring.
         * @return  scan ring id.
         */
        RingID getCommonRingId( uint16_t i_ringPos );

        /**
         * @brief   returns the id of chiplet instance specific ring from storage index.
         * @param   i_ringPos   storage position of ring.
         * @return  scan ring id.
         */
        RingID getInstRingId  ( uint16_t  i_ringPos );

        /**
         * @brief   returns the offset associated with scan ring of given id.
         * @param   i_ringId    Id associated with a scan ring.
         * @param   SUCCESS for successful operation erro code otherwise.
         * @return  offset associated with a given ring.
         */
        uint16_t getRingOffset( RingID i_ringId, uint8_t i_chipletPos = 0 );

        /**
         * @brief   sets the offset associated with scan ring of given id.
         * @param   i_pRingPtr  points to start of scan ring.
         * @param   i_ringId    Id associated with a scan ring.
         * @param   SUCCESS for successful operation, error code otherwise.
         */
        void setRingOffset( uint8_t* i_pRingPtr, RingID i_ringId, uint8_t i_chipletPos = 0 );

        /**
         * @brief   sets base of a ring associated with a given platform.
         * @param   i_ringPos   points to start of ring index of CME/SGPE
         */
        void initRingBase( uint8_t* i_pRingStart );

        /**
         * @brief   sets size of a given scan ring.
         * @param   i_ringId    Id associated with a given scan ring.
         * @param   i_ringSize  size of scan ring.
         */
        void setRingSize( RingID i_ringId, uint16_t i_ringSize, uint8_t i_chipletPos = 0 );

        /**
         * @brief   returns size of a given scan ring.
         * @param   i_ringId    Id associated with a given scan ring.
         * @return  size of a given scan ring.
         */
        uint16_t getRingSize( RingID i_ringId, uint8_t i_chipletPos = 0 );

        /**
         * @brief   returns name of scan ring associated with given id.
         * @param   i_ringId    Id associated with a given scan ring.
         * @return  size of a given scan ring.
         */
        const char* getRingName( RingID i_ringId );

        /**
         * @brief   generates a report on layout of CME/SGPE scan rings in HOMER.
         */
        void dumpRings( );

        /**
         * @brief   generates a report on layout of CME/SGPE scan ring overrides in HOMER.
         */
        void dumpOverrideRings();

        /**
         * @brief   prints a given ring for content debug
         * @param   i_ptr       points to a work buffer containing ring.
         * @param   i_ringSize  size of a ring.
         * @param   i_ringId    ring id
         */
        void extractRing( void* i_ptr, uint32_t i_ringSize, uint32_t i_ringId );
    private:
        std::map<uint16_t, RingProfile> iv_cmnRingMap;
        std::map<uint16_t, RingProfile> iv_instRingMap;
        uint8_t* iv_pRingStart;
        uint16_t iv_ringSize;
        PlatId iv_plat;
        RingDebugMode_t iv_debugMode;

        std::map<RingID, RingName> iv_ringName;
};

class P9FuncModel
{
    public:
        /**
         * @brief   constructor
         */
        P9FuncModel( const fapi2::Target<fapi2::TARGET_TYPE_PROC_CHIP >& i_procTgt );
        /**
         * brief    destructor
         */
        ~P9FuncModel( );

        /**
         * @brief   returns functional state of a given core.
         */
        bool isCoreFunctional( uint32_t i_corePos ) const;

        /**
         * @brief   returns functional state of a given ex.
         */
        bool isExFunctional( uint32_t i_exPos )     const;

        /**
         * @brief   returns functional state of a given quad.
         */
        bool isQuadFunctional( uint32_t i_quadPos ) const;

        /**
         * @brief   returns dd level associated with a processor.
         */
        uint8_t getChipLevel()  const;

        /**
         * uint8_t returns an ID pertaining to chip name.
         */

        uint8_t getChipName() const;

        /**
         * @brief   constructor
         */
        P9FuncModel(  );

    private:
        P9FuncModel( const P9FuncModel& );              //Disallow copy constructor
        P9FuncModel& operator = ( const P9FuncModel&); // Disallow assignment operator
        uint32_t iv_funcCores;
        uint16_t iv_funcExes;
        uint8_t  iv_funcQuads;
        uint8_t  iv_ddLevel;
        uint8_t iv_chipName;
        uint8_t iv_reserve[3];
};

}// namesapce p9_hcodeImageBuild ends

#endif //__P9_SCAN_RING_UTIL_H
<commit_msg>UV Support : Augmented STOP API and self restore for enabling ultravisor.<commit_after>/* IBM_PROLOG_BEGIN_TAG                                                   */
/* This is an automatically generated prolog.                             */
/*                                                                        */
/* $Source: src/import/chips/p9/procedures/hwp/pm/p9_scan_ring_util.H $   */
/*                                                                        */
/* OpenPOWER HostBoot Project                                             */
/*                                                                        */
/* Contributors Listed Below - COPYRIGHT 2016,2019                        */
/* [+] International Business Machines Corp.                              */
/*                                                                        */
/*                                                                        */
/* Licensed under the Apache License, Version 2.0 (the "License");        */
/* you may not use this file except in compliance with the License.       */
/* You may obtain a copy of the License at                                */
/*                                                                        */
/*     http://www.apache.org/licenses/LICENSE-2.0                         */
/*                                                                        */
/* Unless required by applicable law or agreed to in writing, software    */
/* distributed under the License is distributed on an "AS IS" BASIS,      */
/* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or        */
/* implied. See the License for the specific language governing           */
/* permissions and limitations under the License.                         */
/*                                                                        */
/* IBM_PROLOG_END_TAG                                                     */
#ifndef __P9_SCAN_RING_UTIL_H
#define __P9_SCAN_RING_UTIL_H

#include <fapi2.H>
#include "p9_ringId.H"
#include "p9_stop_util.H"
#include <map>

//#define __CRONUS_VER
///
/// @file   p9_scan_ring_util.H
/// @brief  utility classes and functions for scan ring debug.
///
/// *HWP HWP Owner:      Greg Still <stillgs@us.ibm.com>
/// *HWP FW Owner:       Prem S Jha <premjha2@in.ibm.com>
/// *HWP Team:           PM
/// *HWP Level:          2
/// *HWP Consumed by:    Hostboot:Phyp:Cro
//

namespace p9_hcodeImageBuild
{

/**
 * @brief   enumerates all the platforms associated with hcode image build.
 */
enum PlatId
{
    PLAT_SELF   =   0,
    PLAT_SGPE   =   1,
    PLAT_CME    =   2,
    PLAT_PGPE   =   3,
};

/**
 * @brief   constants associated with fake scan rings.
 */
enum FakeScanRing_t
{
    SCAN_RNG_START      =   0x5363616e20526e67,
    SCAN_RNG_END        =   0x52696e6720456e64,
    MAX_SCAN_RING_SIZE  =   120,
    MAX_FAKE_CMN_RING   =   0x14,
    MAX_FAKE_RING       =   0x1E,
};

/**
 * @brief   debug modes supported for scan rings.
 */
enum RingDebugMode_t
{
    SCAN_RING_NO_DEBUG          =   0x00,
    SCAN_RING_TRACE_DEBUG       =   0x01,
    SCAN_RING_TRACE_DEEP_DEBUG  =   0x02,
};
/**
 * @brief   models a fake ring for debug.
 */
struct FakeRing
{
    uint64_t iv_ringHeader;
    uint64_t iv_ringNumber;
    uint8_t  iv_ringPayload[MAX_SCAN_RING_SIZE];
    uint64_t iv_ringEnd;

#ifdef __CRONUS_VER
    FakeRing( uint8_t i_ringNumber ):
        iv_ringHeader( SWIZZLE_8_BYTE(SCAN_RNG_START) ),
        iv_ringEnd(SWIZZLE_8_BYTE(SCAN_RNG_END))
    {
        memset( &iv_ringPayload, 0xAA, MAX_SCAN_RING_SIZE );
        uint8_t temp1 = i_ringNumber / 100;
        temp1 = temp1 + 0x30;
        iv_ringNumber = temp1 << 16;
        temp1 = i_ringNumber % 100;
        iv_ringNumber |= (( temp1 / 10) + 0x30 ) << 8;
        iv_ringNumber |= ((temp1 % 10) + 0x30 );
        iv_ringNumber = SWIZZLE_8_BYTE(iv_ringNumber);
    }
#else
    FakeRing( uint8_t i_ringNumber ):
        iv_ringHeader( 0 ),
        iv_ringEnd( 0)
    {
        //dummy constructor to avoid compilation issue
        memset( &iv_ringPayload, 0xAA, MAX_SCAN_RING_SIZE );
    }
#endif
};

class RingName
{
    public:
        /**
         * @brief   constructor
         * @param   i_ringName      name associated with the a scan ring.
         * @param   i_debugMode     debug mode.
         */
        RingName( char* i_ringName );

        /**
         * @brief   constructor
         */
        RingName() {};

        /**
         * @brief   destructor
         */
        ~RingName() {};

        /**
         * @brief   returns string Name.
         */
        char* c_str();
    private:
        char iv_ringStr[40];
};

/**
 * @brief   models the profile of a scan ring for debug.
 */
struct RingProfile
{
    RingID iv_ringId;
    uint32_t iv_ringOffset;
    uint32_t iv_ringSize;
    uint32_t iv_chipletPos;

    RingProfile ( RingID i_ringId, uint16_t i_ringOffset, uint16_t i_ringSize ) :
        iv_ringId( i_ringId ),
        iv_ringOffset( i_ringOffset ),
        iv_ringSize( i_ringSize ),
        iv_chipletPos( 0 )
    { }

    RingProfile ( RingID i_ringId, uint8_t i_chipletPos ) :
        iv_ringId( i_ringId ),
        iv_ringOffset( 0 ),
        iv_ringSize( 0 ),
        iv_chipletPos( i_chipletPos )
    { }

    RingProfile():
        iv_ringId(eq_fure),
        iv_ringOffset(0),
        iv_ringSize(0)
    {}
};

/**
 * @brief   populates an input buffer with a fake scan ring.
 * @param   o_pRingBuffer   rings in which scan rings are copied.
 * @param   o_size          size of scan ring.
 * @param   i_ringId        scan ring id.
 * @param   i_plat          platform associated with scan ring.
 */
void getScanRing( void* o_pRingBuffer, uint32_t& o_size,
                  uint32_t i_ringId, PlatId i_plat );

/**
 * @brief   class to suppport debug of scan ring layout in P9 HOMER
 * @note    Intended platform is cronus.
 */
class RingBucket
{
    public:
        /**
         * @brief   constructor
         */
        RingBucket( PlatId i_plat, uint8_t* i_pRingStart, RingDebugMode_t i_debugMode );

        /**
         * @brief   destructor
         */
        ~RingBucket();
        /**
         * @brief   returns the id of common ring from storage index.
         * @param   i_ringPos   storage position of ring.
         * @return  scan ring id.
         */
        RingID getCommonRingId( uint16_t i_ringPos );

        /**
         * @brief   returns the id of chiplet instance specific ring from storage index.
         * @param   i_ringPos   storage position of ring.
         * @return  scan ring id.
         */
        RingID getInstRingId  ( uint16_t  i_ringPos );

        /**
         * @brief   returns the offset associated with scan ring of given id.
         * @param   i_ringId    Id associated with a scan ring.
         * @param   SUCCESS for successful operation erro code otherwise.
         * @return  offset associated with a given ring.
         */
        uint16_t getRingOffset( RingID i_ringId, uint8_t i_chipletPos = 0 );

        /**
         * @brief   sets the offset associated with scan ring of given id.
         * @param   i_pRingPtr  points to start of scan ring.
         * @param   i_ringId    Id associated with a scan ring.
         * @param   SUCCESS for successful operation, error code otherwise.
         */
        void setRingOffset( uint8_t* i_pRingPtr, RingID i_ringId, uint8_t i_chipletPos = 0 );

        /**
         * @brief   sets base of a ring associated with a given platform.
         * @param   i_ringPos   points to start of ring index of CME/SGPE
         */
        void initRingBase( uint8_t* i_pRingStart );

        /**
         * @brief   sets size of a given scan ring.
         * @param   i_ringId    Id associated with a given scan ring.
         * @param   i_ringSize  size of scan ring.
         */
        void setRingSize( RingID i_ringId, uint16_t i_ringSize, uint8_t i_chipletPos = 0 );

        /**
         * @brief   returns size of a given scan ring.
         * @param   i_ringId    Id associated with a given scan ring.
         * @return  size of a given scan ring.
         */
        uint16_t getRingSize( RingID i_ringId, uint8_t i_chipletPos = 0 );

        /**
         * @brief   returns name of scan ring associated with given id.
         * @param   i_ringId    Id associated with a given scan ring.
         * @return  size of a given scan ring.
         */
        const char* getRingName( RingID i_ringId );

        /**
         * @brief   generates a report on layout of CME/SGPE scan rings in HOMER.
         */
        void dumpRings( );

        /**
         * @brief   generates a report on layout of CME/SGPE scan ring overrides in HOMER.
         */
        void dumpOverrideRings();

        /**
         * @brief   prints a given ring for content debug
         * @param   i_ptr       points to a work buffer containing ring.
         * @param   i_ringSize  size of a ring.
         * @param   i_ringId    ring id
         */
        void extractRing( void* i_ptr, uint32_t i_ringSize, uint32_t i_ringId );
    private:
        std::map<uint16_t, RingProfile> iv_cmnRingMap;
        std::map<uint16_t, RingProfile> iv_instRingMap;
        uint8_t* iv_pRingStart;
        uint16_t iv_ringSize;
        PlatId iv_plat;
        RingDebugMode_t iv_debugMode;

        std::map<RingID, RingName> iv_ringName;
};

class P9FuncModel
{
    public:
        /**
         * @brief   constructor
         */
        P9FuncModel( const fapi2::Target<fapi2::TARGET_TYPE_PROC_CHIP >& i_procTgt );
        /**
         * brief    destructor
         */
        ~P9FuncModel( );

        /**
         * @brief   returns functional state of a given core.
         */
        bool isCoreFunctional( uint32_t i_corePos ) const;

        /**
         * @brief   returns functional state of a given ex.
         */
        bool isExFunctional( uint32_t i_exPos )     const;

        /**
         * @brief   returns functional state of a given quad.
         */
        bool isQuadFunctional( uint32_t i_quadPos ) const;

        /**
         * @brief   returns dd level associated with a processor.
         */
        uint8_t getChipLevel()  const;

        /**
         * uint8_t returns an ID pertaining to chip name.
         */

        uint8_t getChipName() const;

        /**
         * returns true if URMOR has a hw bug else false.
         */
        uint8_t hasUrmorBug() const;

        /**
         * @brief   constructor
         */
        P9FuncModel(  );

    private:
        P9FuncModel( const P9FuncModel& );              //Disallow copy constructor
        P9FuncModel& operator = ( const P9FuncModel&); // Disallow assignment operator
        uint32_t iv_funcCores;
        uint16_t iv_funcExes;
        uint8_t  iv_funcQuads;
        uint8_t  iv_ddLevel;
        uint8_t  iv_chipName;
        uint8_t  iv_urmorBug;
        uint8_t  iv_reserve[2];
};

}// namesapce p9_hcodeImageBuild ends

#endif //__P9_SCAN_RING_UTIL_H
<|endoftext|>


<commit_before>/*
 * Molch, an implementation of the axolotl ratchet based on libsodium
 *
 * ISC License
 *
 * Copyright (C) 2015-2016 Max Bruckner (FSMaxB) <max at maxbruckner dot de>
 *
 * 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.
 */

#ifndef LIB_KEY_HPP
#define LIB_KEY_HPP

#include <array>
#include <memory>
#include <ostream>
#include <sodium.h>
#include <iterator>
#include <algorithm>

#include "buffer.hpp"
#include "constants.h"
#include "molch-exception.hpp"
#include "endianness.hpp"
#include "gsl.hpp"
#include "protobuf-pool.hpp"
#include "protobuf.hpp"

namespace Molch {

	//type of key, this is used to distinguish key types
	//and for example avoid copying a private key to a public key
	enum class KeyType : uint8_t {
		Key,
		MessageKey,
		ChainKey,
		HeaderKey,
		RootKey,
		BackupKey,
		PublicKey,
		PrivateKey,
		PublicSigningKey,
		PrivateSigningKey
	};

	template <size_t length, KeyType keytype>
	class Key : public std::array<gsl::byte,length> {
	private:
		Key& copy(const Key& key) {
			this->empty = key.empty;

			std::copy(std::cbegin(key), std::cend(key), std::begin(*this));

			return *this;
		}

		Key& move(Key&& key) {
			this->empty = key.empty;

			if (key.empty) {
				key.clear();
				return *this;
			}

			for (size_t i{0}; i < length; ++i) {
				(*this)[i] = key[i];
			}

			return *this;
		}

	public:
		bool empty{true};

		Key() = default;
		Key(const Key& key) {
			this->copy(key);
		}

		Key(Key&& key) {
			this->move(std::move(key));
		}

		Key(const ProtobufCKey& key) {
			this->set({uchar_to_byte(key.key.data), key.key.len});
		}

		~Key() {
			this->clear();
		}

		Key& operator=(const Key& key) {
			return this->copy(key);
		}
		Key& operator=(Key&& key) {
			return this->move(std::move(key));
		}

		/*
		 * Constant time comparison of two keys.
		 *
		 * returns:
		 *  -1 if this is less than key
		 *   0 if this is equal to key
		 *  +1 if this is greater than key
		 *
		 *  throws an exception if either is empty.
		 */
		int compare(const Key& key) const {
			Expects(!this->empty && !key.empty);

			return sodium_compare(*this, key);
		}

		//comparison operators
		bool operator>(const Key& key) const {
			return (this->compare(key) == 1);
		}
		bool operator<(const Key& key) const {
			return (this->compare(key) == -1);
		}
		bool operator>=(const Key& key) const {
			return !(*this < key);
		}
		bool operator<=(const Key& key) const {
			return !(*this > key);
		}
		bool operator==(const Key& key) const {
			if (this->empty && key.empty) {
				return true;
			} else if (this->empty || key.empty) {
				return false;
			}

			return (this->compare(key) == 0);
		}
		bool operator!=(const Key& key) const {
			return !(*this == key);
		}

		KeyType type() {
			return type;
		}

		template <size_t derived_length,KeyType derived_type>
		void deriveTo(Key<derived_length,derived_type>& derived_key, const uint32_t subkey_counter) const {
			Expects(!this->empty);

			static_assert(derived_length <= crypto_generichash_blake2b_BYTES_MAX, "The derived length is greater than crypto_generichas_blake2b_BYTES_MAX");
			static_assert(derived_length >= crypto_generichash_blake2b_BYTES_MIN, "The derived length is smaller than crypto_generichash_blake2b_BYTES_MAX");
			static_assert(length <= crypto_generichash_blake2b_KEYBYTES_MAX, "The length to derive from is greater than crypto_generichash_blake2b_KEYBYTES_MAX");
			static_assert(length >= crypto_generichash_blake2b_KEYBYTES_MIN, "The length to derive from is smaller than crypto_generichash_blake2b_KEYBYTES_MIN");

			//create a salt that contains the number of the subkey
			Key<crypto_generichash_blake2b_SALTBYTES,KeyType::Key> salt;
			salt.clear(); //fill with zeroes
			salt.empty = false;

			//fill the salt with a big endian representation of the subkey counter
			to_big_endian(subkey_counter, {salt.data()+ salt.size() - sizeof(uint32_t), sizeof(uint32_t)});

			const unsigned char personal[]{"molch_cryptolib"};
			static_assert(sizeof(personal) == crypto_generichash_blake2b_PERSONALBYTES, "personal string is not crypto_generichash_blake2b_PERSONALBYTES long");

			//set length of output
			crypto_generichash_blake2b_salt_personal(
					derived_key,
					{nullptr}, //input
					*this,
					salt,
					{uchar_to_byte(personal), sizeof(personal)});

			derived_key.empty = false;
		}

		void fillRandom() {
			randombytes_buf(*this);
			this->empty = false;
		}

		//TODO get rid of this
		bool isNone() const {
			if (this->empty) {
				return true;
			}

			return sodium_is_zero(*this);
		}

		//copy from a raw byte array
		void set(const span<const gsl::byte> data) {
			Expects(data.size() == length);

			std::copy(std::cbegin(data), std::cend(data), this->data());
			this->empty = false;
		}

		//copy to a raw byte array
		void copyTo(span<gsl::byte> data) const {
			Expects(data.size() == length);

			std::copy(std::cbegin(*this), std::cend(*this), std::begin(data));
		}

		void clear() {
			sodium_memzero(*this);
			this->empty = true;
		}

		ProtobufCKey* exportProtobuf(ProtobufPool& pool) const {
			auto key{pool.allocate<ProtobufCKey>(1)};
			key__init(key);

			key->key.data = pool.allocate<uint8_t>(length);
			key->key.len = length;
			this->copyTo({
					uchar_to_byte(key->key.data),
					key->key.len
					});

			return key;
		}

		std::ostream& printHex(std::ostream& stream) const {
			static constexpr size_t width{30};

			if (this->empty) {
				return stream << "(empty)";
			}

			const size_t hex_length{this->size() * 2 + sizeof("")};
			auto hex{std::make_unique<char[]>(hex_length)};
			sodium_bin2hex({hex.get(), hex_length}, *this);

			for (size_t i{0}; i < hex_length; i++) {
				if ((width != 0) && ((i % width) == 0) && (i != 0)) {
					stream << '\n';
				} else if ((i % 2 == 0) && (i != 0)) {
					stream << ' ';
				}
				stream << hex[i];
			}

			return stream;
		}
	};

	class MessageKey : public Key<MESSAGE_KEY_SIZE,KeyType::MessageKey> {
	public:
		//inherit constructors
		using Key<MESSAGE_KEY_SIZE,KeyType::MessageKey>::Key;
	};

	class ChainKey : public Key<CHAIN_KEY_SIZE,KeyType::ChainKey> {
	public:
		//inherit constructors
		using Key<CHAIN_KEY_SIZE,KeyType::ChainKey>::Key;

		MessageKey deriveMessageKey() const {
			MessageKey message_key;
			this->deriveTo(message_key, 0);

			return message_key;
		}

		ChainKey deriveChainKey() const {
			ChainKey chain_key;
			this->deriveTo(chain_key, 1);

			return chain_key;
		}
	};

	class HeaderKey : public Key<HEADER_KEY_SIZE,KeyType::HeaderKey> {
	public:
		//inherit constructors
		using Key<HEADER_KEY_SIZE,KeyType::HeaderKey>::Key;
	};

	class RootKey : public Key<ROOT_KEY_SIZE,KeyType::RootKey> {
	public:
		//inherit constructors
		using Key<ROOT_KEY_SIZE,KeyType::RootKey>::Key;
	};

	class BackupKey : public Key<BACKUP_KEY_SIZE,KeyType::BackupKey> {
	public:
		//inherit constructors
		using Key<BACKUP_KEY_SIZE,KeyType::BackupKey>::Key;
	};

	class PublicKey : public Key<PUBLIC_KEY_SIZE,KeyType::PublicKey> {
	public:
		//inherit constructors
		using Key<PUBLIC_KEY_SIZE,KeyType::PublicKey>::Key;
	};
	class PrivateKey : public Key<PRIVATE_KEY_SIZE,KeyType::PrivateKey> {
	public:
		//inherit constructors
		using Key<PRIVATE_KEY_SIZE,KeyType::PrivateKey>::Key;
	};

	class PublicSigningKey : public Key<PUBLIC_MASTER_KEY_SIZE,KeyType::PublicSigningKey> {
	public:
		//inherit constructors
		using Key<PUBLIC_MASTER_KEY_SIZE,KeyType::PublicSigningKey>::Key;
	};
	class PrivateSigningKey : public Key<PRIVATE_MASTER_KEY_SIZE,KeyType::PrivateSigningKey> {
	public:
		//inherit constructors
		using Key<PRIVATE_MASTER_KEY_SIZE,KeyType::PrivateSigningKey>::Key;
	};
}

#endif /* LIB_KEY_HPP */
<commit_msg>Coverity: Key: Catch exception in destructor<commit_after>/*
 * Molch, an implementation of the axolotl ratchet based on libsodium
 *
 * ISC License
 *
 * Copyright (C) 2015-2016 Max Bruckner (FSMaxB) <max at maxbruckner dot de>
 *
 * 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.
 */

#ifndef LIB_KEY_HPP
#define LIB_KEY_HPP

#include <array>
#include <memory>
#include <ostream>
#include <sodium.h>
#include <iterator>
#include <algorithm>

#include "buffer.hpp"
#include "constants.h"
#include "molch-exception.hpp"
#include "endianness.hpp"
#include "gsl.hpp"
#include "protobuf-pool.hpp"
#include "protobuf.hpp"

namespace Molch {

	//type of key, this is used to distinguish key types
	//and for example avoid copying a private key to a public key
	enum class KeyType : uint8_t {
		Key,
		MessageKey,
		ChainKey,
		HeaderKey,
		RootKey,
		BackupKey,
		PublicKey,
		PrivateKey,
		PublicSigningKey,
		PrivateSigningKey
	};

	template <size_t length, KeyType keytype>
	class Key : public std::array<gsl::byte,length> {
	private:
		Key& copy(const Key& key) {
			this->empty = key.empty;

			std::copy(std::cbegin(key), std::cend(key), std::begin(*this));

			return *this;
		}

		Key& move(Key&& key) {
			this->empty = key.empty;

			if (key.empty) {
				key.clear();
				return *this;
			}

			for (size_t i{0}; i < length; ++i) {
				(*this)[i] = key[i];
			}

			return *this;
		}

	public:
		bool empty{true};

		Key() = default;
		Key(const Key& key) {
			this->copy(key);
		}

		Key(Key&& key) {
			this->move(std::move(key));
		}

		Key(const ProtobufCKey& key) {
			this->set({uchar_to_byte(key.key.data), key.key.len});
		}

		~Key() {
			try {
				this->clear();
			} catch (...) {
				std::terminate();
			}
		}

		Key& operator=(const Key& key) {
			return this->copy(key);
		}
		Key& operator=(Key&& key) {
			return this->move(std::move(key));
		}

		/*
		 * Constant time comparison of two keys.
		 *
		 * returns:
		 *  -1 if this is less than key
		 *   0 if this is equal to key
		 *  +1 if this is greater than key
		 *
		 *  throws an exception if either is empty.
		 */
		int compare(const Key& key) const {
			Expects(!this->empty && !key.empty);

			return sodium_compare(*this, key);
		}

		//comparison operators
		bool operator>(const Key& key) const {
			return (this->compare(key) == 1);
		}
		bool operator<(const Key& key) const {
			return (this->compare(key) == -1);
		}
		bool operator>=(const Key& key) const {
			return !(*this < key);
		}
		bool operator<=(const Key& key) const {
			return !(*this > key);
		}
		bool operator==(const Key& key) const {
			if (this->empty && key.empty) {
				return true;
			} else if (this->empty || key.empty) {
				return false;
			}

			return (this->compare(key) == 0);
		}
		bool operator!=(const Key& key) const {
			return !(*this == key);
		}

		KeyType type() {
			return type;
		}

		template <size_t derived_length,KeyType derived_type>
		void deriveTo(Key<derived_length,derived_type>& derived_key, const uint32_t subkey_counter) const {
			Expects(!this->empty);

			static_assert(derived_length <= crypto_generichash_blake2b_BYTES_MAX, "The derived length is greater than crypto_generichas_blake2b_BYTES_MAX");
			static_assert(derived_length >= crypto_generichash_blake2b_BYTES_MIN, "The derived length is smaller than crypto_generichash_blake2b_BYTES_MAX");
			static_assert(length <= crypto_generichash_blake2b_KEYBYTES_MAX, "The length to derive from is greater than crypto_generichash_blake2b_KEYBYTES_MAX");
			static_assert(length >= crypto_generichash_blake2b_KEYBYTES_MIN, "The length to derive from is smaller than crypto_generichash_blake2b_KEYBYTES_MIN");

			//create a salt that contains the number of the subkey
			Key<crypto_generichash_blake2b_SALTBYTES,KeyType::Key> salt;
			salt.clear(); //fill with zeroes
			salt.empty = false;

			//fill the salt with a big endian representation of the subkey counter
			to_big_endian(subkey_counter, {salt.data()+ salt.size() - sizeof(uint32_t), sizeof(uint32_t)});

			const unsigned char personal[]{"molch_cryptolib"};
			static_assert(sizeof(personal) == crypto_generichash_blake2b_PERSONALBYTES, "personal string is not crypto_generichash_blake2b_PERSONALBYTES long");

			//set length of output
			crypto_generichash_blake2b_salt_personal(
					derived_key,
					{nullptr}, //input
					*this,
					salt,
					{uchar_to_byte(personal), sizeof(personal)});

			derived_key.empty = false;
		}

		void fillRandom() {
			randombytes_buf(*this);
			this->empty = false;
		}

		//TODO get rid of this
		bool isNone() const {
			if (this->empty) {
				return true;
			}

			return sodium_is_zero(*this);
		}

		//copy from a raw byte array
		void set(const span<const gsl::byte> data) {
			Expects(data.size() == length);

			std::copy(std::cbegin(data), std::cend(data), this->data());
			this->empty = false;
		}

		//copy to a raw byte array
		void copyTo(span<gsl::byte> data) const {
			Expects(data.size() == length);

			std::copy(std::cbegin(*this), std::cend(*this), std::begin(data));
		}

		void clear() {
			sodium_memzero(*this);
			this->empty = true;
		}

		ProtobufCKey* exportProtobuf(ProtobufPool& pool) const {
			auto key{pool.allocate<ProtobufCKey>(1)};
			key__init(key);

			key->key.data = pool.allocate<uint8_t>(length);
			key->key.len = length;
			this->copyTo({
					uchar_to_byte(key->key.data),
					key->key.len
					});

			return key;
		}

		std::ostream& printHex(std::ostream& stream) const {
			static constexpr size_t width{30};

			if (this->empty) {
				return stream << "(empty)";
			}

			const size_t hex_length{this->size() * 2 + sizeof("")};
			auto hex{std::make_unique<char[]>(hex_length)};
			sodium_bin2hex({hex.get(), hex_length}, *this);

			for (size_t i{0}; i < hex_length; i++) {
				if ((width != 0) && ((i % width) == 0) && (i != 0)) {
					stream << '\n';
				} else if ((i % 2 == 0) && (i != 0)) {
					stream << ' ';
				}
				stream << hex[i];
			}

			return stream;
		}
	};

	class MessageKey : public Key<MESSAGE_KEY_SIZE,KeyType::MessageKey> {
	public:
		//inherit constructors
		using Key<MESSAGE_KEY_SIZE,KeyType::MessageKey>::Key;
	};

	class ChainKey : public Key<CHAIN_KEY_SIZE,KeyType::ChainKey> {
	public:
		//inherit constructors
		using Key<CHAIN_KEY_SIZE,KeyType::ChainKey>::Key;

		MessageKey deriveMessageKey() const {
			MessageKey message_key;
			this->deriveTo(message_key, 0);

			return message_key;
		}

		ChainKey deriveChainKey() const {
			ChainKey chain_key;
			this->deriveTo(chain_key, 1);

			return chain_key;
		}
	};

	class HeaderKey : public Key<HEADER_KEY_SIZE,KeyType::HeaderKey> {
	public:
		//inherit constructors
		using Key<HEADER_KEY_SIZE,KeyType::HeaderKey>::Key;
	};

	class RootKey : public Key<ROOT_KEY_SIZE,KeyType::RootKey> {
	public:
		//inherit constructors
		using Key<ROOT_KEY_SIZE,KeyType::RootKey>::Key;
	};

	class BackupKey : public Key<BACKUP_KEY_SIZE,KeyType::BackupKey> {
	public:
		//inherit constructors
		using Key<BACKUP_KEY_SIZE,KeyType::BackupKey>::Key;
	};

	class PublicKey : public Key<PUBLIC_KEY_SIZE,KeyType::PublicKey> {
	public:
		//inherit constructors
		using Key<PUBLIC_KEY_SIZE,KeyType::PublicKey>::Key;
	};
	class PrivateKey : public Key<PRIVATE_KEY_SIZE,KeyType::PrivateKey> {
	public:
		//inherit constructors
		using Key<PRIVATE_KEY_SIZE,KeyType::PrivateKey>::Key;
	};

	class PublicSigningKey : public Key<PUBLIC_MASTER_KEY_SIZE,KeyType::PublicSigningKey> {
	public:
		//inherit constructors
		using Key<PUBLIC_MASTER_KEY_SIZE,KeyType::PublicSigningKey>::Key;
	};
	class PrivateSigningKey : public Key<PRIVATE_MASTER_KEY_SIZE,KeyType::PrivateSigningKey> {
	public:
		//inherit constructors
		using Key<PRIVATE_MASTER_KEY_SIZE,KeyType::PrivateSigningKey>::Key;
	};
}

#endif /* LIB_KEY_HPP */
<|endoftext|>


<commit_before>#include "stdafx.h"

#include "d3d.h"
#include <SADXModLoader.h>

#include "Trampoline.h"
#include "globals.h"
#include "../include/lanternapi.h"

#include "Obj_SkyDeck.h"

DataPointer(int, SkyDeck_AltitudeMode, 0x03C80608);
DataPointer(float, SkyDeck_SkyAltitude, 0x03C80610);

static Trampoline* SkyDeck_SimulateAltitude_t = nullptr;
static Trampoline* SkyBox_SkyDeck_t = nullptr;
static Trampoline* Obj_SkyDeck_t = nullptr;
static size_t handle = 0;

static void __cdecl SkyDeck_SimulateAltitude_r(Uint16 act)
{
	TARGET_DYNAMIC(SkyDeck_SimulateAltitude)(act);

	// 0 = high altitide (bright), 1 = low altitude (dark)
	if (SkyDeck_AltitudeMode > 1)
	{
		set_blend_factor(0.0f);
	}
	else
	{
		float f = (max(180.0f, min(250.0f, SkyDeck_SkyAltitude)) - 180.0f) / 70.0f;
		set_blend_factor(f);
	}
}

static void __cdecl SkyBox_SkyDeck_Delete(ObjectMaster*)
{
	// Disable blending in the shader.
	set_shader_flags(ShaderFlags_Blend, false);
	// Reset blend indices.
	set_blend(-1, -1);
}

static void __cdecl SkyBox_SkyDeck_r(ObjectMaster* _this)
{
	if (_this->DeleteSub != SkyBox_SkyDeck_Delete)
	{
		_this->DeleteSub = SkyBox_SkyDeck_Delete;
	}

	TARGET_DYNAMIC(SkyBox_SkyDeck)(_this);
}

static void __cdecl Obj_SkyDeck_Delete(ObjectMaster* _this)
{
	globals::palettes.Remove(handle);
	set_shader_flags(ShaderFlags_Blend, false);
	globals::palettes.ForwardBlendAll(false);
	handle = 0;
}

static void __cdecl Obj_SkyDeck_r(ObjectMaster* _this)
{
	TARGET_DYNAMIC(Obj_SkyDeck)(_this);

	if (handle || d3d::effect == nullptr)
	{
		return;
	}

	_this->DeleteSub = Obj_SkyDeck_Delete;

	globals::palettes.LoadPalette(LevelIDs_SkyDeck, 0);
	globals::palettes.LoadSource(LevelIDs_SkyDeck, 0);

	LanternInstance lantern(&param::PaletteB);

	lantern.LoadPalette(LevelIDs_SkyDeck, 1);
	handle = globals::palettes.Add(lantern);
	globals::palettes.SetLastLevel(-1, -1);
	globals::palettes.ForwardBlendAll(true);
	set_shader_flags(ShaderFlags_Blend, true);
}

void SkyDeck_Init()
{
	Obj_SkyDeck_t              = new Trampoline(0x005F02E0, 0x005F02E5, Obj_SkyDeck_r);
	SkyDeck_SimulateAltitude_t = new Trampoline(0x005ECA80, 0x005ECA87, SkyDeck_SimulateAltitude_r);
	SkyBox_SkyDeck_t           = new Trampoline(0x005F0340, 0x005F0347, SkyBox_SkyDeck_r);
}
<commit_msg>More work on #45 (fixes perma-darkness after going to the past)<commit_after>#include "stdafx.h"

#include "d3d.h"
#include <SADXModLoader.h>

#include "Trampoline.h"
#include "globals.h"
#include "../include/lanternapi.h"

#include "Obj_SkyDeck.h"

DataPointer(int, SkyDeck_AltitudeMode, 0x03C80608);
DataPointer(float, SkyDeck_SkyAltitude, 0x03C80610);

static Trampoline* SkyDeck_SimulateAltitude_t = nullptr;
static Trampoline* SkyBox_SkyDeck_t = nullptr;
static Trampoline* Obj_SkyDeck_t = nullptr;
static size_t handle = 0;

static void __cdecl SkyDeck_SimulateAltitude_r(Uint16 act)
{
	TARGET_DYNAMIC(SkyDeck_SimulateAltitude)(act);

	// 0 = high altitide (bright), 1 = low altitude (dark)
	if (SkyDeck_AltitudeMode > 1)
	{
		set_blend_factor(0.0f);
	}
	else
	{
		float f = (max(180.0f, min(250.0f, SkyDeck_SkyAltitude)) - 180.0f) / 70.0f;
		set_blend_factor(f);
	}
}

static void __cdecl SkyBox_SkyDeck_Delete(ObjectMaster*)
{
	// Disable blending in the shader.
	set_shader_flags(ShaderFlags_Blend, false);
	// Reset blend indices.
	set_blend(-1, -1);
}

static void __cdecl SkyBox_SkyDeck_r(ObjectMaster* _this)
{
	if (_this->DeleteSub != SkyBox_SkyDeck_Delete)
	{
		_this->DeleteSub = SkyBox_SkyDeck_Delete;
	}

	TARGET_DYNAMIC(SkyBox_SkyDeck)(_this);
}

static void __cdecl Obj_SkyDeck_Delete(ObjectMaster* _this)
{
	globals::palettes.Remove(handle);
	set_shader_flags(ShaderFlags_Blend, false);
	globals::palettes.ForwardBlendAll(false);
	handle = 0;
}

static void __cdecl Obj_SkyDeck_r(ObjectMaster* _this)
{
	TARGET_DYNAMIC(Obj_SkyDeck)(_this);

	if (handle || d3d::effect == nullptr)
	{
		return;
	}

	_this->DeleteSub = Obj_SkyDeck_Delete;

	globals::palettes.LoadPalette(LevelIDs_SkyDeck, 0);
	globals::palettes.LoadSource(LevelIDs_SkyDeck, 0);

	param::PaletteB = nullptr;
	LanternInstance lantern(&param::PaletteB);

	lantern.LoadPalette(LevelIDs_SkyDeck, 1);
	handle = globals::palettes.Add(lantern);
	globals::palettes.SetLastLevel(-1, -1);
	globals::palettes.ForwardBlendAll(true);
	set_shader_flags(ShaderFlags_Blend, true);
}

void SkyDeck_Init()
{
	Obj_SkyDeck_t              = new Trampoline(0x005F02E0, 0x005F02E5, Obj_SkyDeck_r);
	SkyDeck_SimulateAltitude_t = new Trampoline(0x005ECA80, 0x005ECA87, SkyDeck_SimulateAltitude_r);
	SkyBox_SkyDeck_t           = new Trampoline(0x005F0340, 0x005F0347, SkyBox_SkyDeck_r);
}
<|endoftext|>


<commit_before>/* Copyright 2019 The TensorFlow Authors. All Rights Reserved.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
==============================================================================*/

#include "tensorflow/lite/kernels/internal/common.h"
#include "tensorflow/lite/kernels/internal/reference/integer_ops/conv.h"

namespace tflite {
namespace reference_integer_ops {

// Fixed-point per-channel-quantization convolution reference kernel.
void OneByOneConvPerChannel(
    const ConvParams& params, const int32_t* output_multiplier,
    const int32_t* output_shift, const RuntimeShape& input_shape,
    const int8_t* input_data, const RuntimeShape& filter_shape,
    const int8_t* filter_data, const RuntimeShape& bias_shape,
    const int32_t* bias_data, const RuntimeShape& output_shape,
    int8_t* output_data) {
  // Get parameters.
  const int32_t input_offset = params.input_offset;  // r = s(q - Z)
  const int stride_width = params.stride_width;
  const int stride_height = params.stride_height;
  const int dilation_width_factor = params.dilation_width_factor;
  const int dilation_height_factor = params.dilation_height_factor;
  const int pad_width = params.padding_values.width;
  const int pad_height = params.padding_values.height;
  const int32_t output_offset = params.output_offset;

  // Set min and max value of the output.
  const int32_t output_activation_min = params.quantized_activation_min;
  const int32_t output_activation_max = params.quantized_activation_max;

  // Consistency check.
  TFLITE_DCHECK_LE(output_activation_min, output_activation_max);
  TFLITE_DCHECK_EQ(input_shape.DimensionsCount(), 4);
  TFLITE_DCHECK_EQ(filter_shape.DimensionsCount(), 4);
  TFLITE_DCHECK_EQ(output_shape.DimensionsCount(), 4);
  const int batches = MatchingDim(input_shape, 0, output_shape, 0);
  const int input_depth = input_shape.Dims(3);
  const int output_depth = MatchingDim(filter_shape, 0, output_shape, 3);
  if (bias_data) {
    TFLITE_DCHECK_EQ(bias_shape.FlatSize(), output_depth);
  }

  // Check dimensions of the tensors.
  const int input_height = input_shape.Dims(1);
  const int input_width = input_shape.Dims(2);
  const int filter_height = filter_shape.Dims(1);
  const int filter_width = filter_shape.Dims(2);
  const int filter_input_depth = filter_shape.Dims(3);
  const int groups = input_depth / filter_input_depth;
  TFLITE_DCHECK_EQ(input_depth % filter_input_depth, 0);
  const int filters_per_group = output_depth / groups;
  const int output_height = output_shape.Dims(1);
  const int output_width = output_shape.Dims(2);
  for (int batch = 0; batch < batches; ++batch) {
    for (int out_y = 0; out_y < output_height; ++out_y) {
      const int in_y_origin = (out_y * stride_height) - pad_height;
      for (int out_x = 0; out_x < output_width; ++out_x) {
        const int in_x_origin = (out_x * stride_width) - pad_width;
        for (int out_channel = 0; out_channel < output_depth; ++out_channel) {
          auto group = out_channel / filters_per_group;
          int32_t acc = 0;
          for (int filter_y = 0; filter_y < filter_height; ++filter_y) {
            const int in_y = in_y_origin + dilation_height_factor * filter_y;
            for (int filter_x = 0; filter_x < filter_width; ++filter_x) {
              const int in_x = in_x_origin + dilation_width_factor * filter_x;

              // Zero padding by omitting the areas outside the image.
              const bool is_point_inside_image =
                  (in_x >= 0) && (in_x < input_width) && (in_y >= 0) &&
                  (in_y < input_height);

              if (!is_point_inside_image) {
                continue;
              }

              for (int in_channel = 0; in_channel < filter_input_depth;
                   ++in_channel) {
                int32_t input_val =
                    input_data[Offset(input_shape, batch, in_y, in_x,
                                      in_channel + group * filter_input_depth)];
                int32_t filter_val = filter_data[Offset(
                    filter_shape, out_channel, filter_y, filter_x, in_channel)];
                // Accumulate with 32 bits accumulator.
                // In the nudging process during model quantization, we force
                // real value of 0.0 be represented by a quantized value. This
                // guarantees that the input_offset is a int8_t, even though
                // it is represented using int32_t. int32_t += int8_t *
                // (int8_t - int8_t) so the highest value we can get from each
                // accumulation is [-127, 127] * ([-128, 127] -
                // [-128, 127]), which is [-32512, 32512]. log2(32512)
                // = 14.98, which means we can accumulate at least 2^16
                // multiplications without overflow. The accumulator is
                // applied to a filter so the accumulation logic will hold as
                // long as the filter size (filter_y * filter_x * in_channel)
                // does not exceed 2^16, which is the case in all the models
                // we have seen so far.
                // TODO(b/174275578): Add a check to make sure the
                // accumulator depth is smaller than 2^16.
                acc += filter_val * (input_val + input_offset);
              }
            }
          }

          if (bias_data) {
            acc += bias_data[out_channel];
          }
          acc = MultiplyByQuantizedMultiplier(
              acc, output_multiplier[out_channel], output_shift[out_channel]);
          acc += output_offset;
          acc = std::max(acc, output_activation_min);
          acc = std::min(acc, output_activation_max);
          output_data[Offset(output_shape, batch, out_y, out_x, out_channel)] =
              static_cast<int8_t>(acc);
        }
      }
    }
  }
}

}  // namespace reference_integer_ops
}  // namespace tflite
<commit_msg>fccm_tutorial/conv: accelerate<commit_after>/* Copyright 2019 The TensorFlow Authors. All Rights Reserved.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
==============================================================================*/

#include <cstdio>

#include "fccm_cfu.h"
#include "tensorflow/lite/kernels/internal/common.h"
#include "tensorflow/lite/kernels/internal/reference/integer_ops/conv.h"

namespace tflite {
namespace reference_integer_ops {

inline const uint32_t* as_word_ptr(const int8_t* byte_ptr) {
  return reinterpret_cast<const uint32_t*>(byte_ptr);
}

// Fixed-point per-channel-quantization convolution reference kernel.
void OneByOneConvPerChannel(
    const ConvParams& params, const int32_t* output_multiplier,
    const int32_t* output_shift, const RuntimeShape& input_shape,
    const int8_t* input_data, const RuntimeShape& filter_shape,
    const int8_t* filter_data, const RuntimeShape& bias_shape,
    const int32_t* bias_data, const RuntimeShape& output_shape,
    int8_t* output_data) {
  // Get parameters.
  const int stride_width = params.stride_width;
  const int stride_height = params.stride_height;
  const int dilation_width_factor = params.dilation_width_factor;
  const int dilation_height_factor = params.dilation_height_factor;
  const int pad_width = params.padding_values.width;
  const int pad_height = params.padding_values.height;
  const int32_t output_offset = params.output_offset;

  // Set min and max value of the output.
  const int32_t output_activation_min = params.quantized_activation_min;
  const int32_t output_activation_max = params.quantized_activation_max;

  // Consistency check.
  TFLITE_DCHECK_LE(output_activation_min, output_activation_max);
  TFLITE_DCHECK_EQ(input_shape.DimensionsCount(), 4);
  TFLITE_DCHECK_EQ(filter_shape.DimensionsCount(), 4);
  TFLITE_DCHECK_EQ(output_shape.DimensionsCount(), 4);
  const int batches = MatchingDim(input_shape, 0, output_shape, 0);
  const int input_depth = input_shape.Dims(3);
  const int output_depth = MatchingDim(filter_shape, 0, output_shape, 3);
  if (bias_data) {
    TFLITE_DCHECK_EQ(bias_shape.FlatSize(), output_depth);
  }

  // Check dimensions of the tensors.
  const int input_height = input_shape.Dims(1);
  const int input_width = input_shape.Dims(2);
  const int filter_height = filter_shape.Dims(1);
  const int filter_width = filter_shape.Dims(2);
  const int filter_input_depth = filter_shape.Dims(3);
  const int groups = input_depth / filter_input_depth;
  TFLITE_DCHECK_EQ(input_depth % filter_input_depth, 0);
  const int filters_per_group = output_depth / groups;
  const int output_height = output_shape.Dims(1);
  const int output_width = output_shape.Dims(2);
  for (int batch = 0; batch < batches; ++batch) {
    for (int out_y = 0; out_y < output_height; ++out_y) {
      const int in_y_origin = (out_y * stride_height) - pad_height;
      for (int out_x = 0; out_x < output_width; ++out_x) {
        const int in_x_origin = (out_x * stride_width) - pad_width;
        for (int out_channel = 0; out_channel < output_depth; ++out_channel) {
          auto group = out_channel / filters_per_group;
          cfu_reset();
          for (int filter_y = 0; filter_y < filter_height; ++filter_y) {
            const int in_y = in_y_origin + dilation_height_factor * filter_y;
            for (int filter_x = 0; filter_x < filter_width; ++filter_x) {
              const int in_x = in_x_origin + dilation_width_factor * filter_x;

              // Zero padding by omitting the areas outside the image.
              const bool is_point_inside_image =
                  (in_x >= 0) && (in_x < input_width) && (in_y >= 0) &&
                  (in_y < input_height);

              if (!is_point_inside_image) {
                continue;
              }

              // filter_input_depth is divisible by 4
              for (int in_channel = 0; in_channel < filter_input_depth;
                   in_channel += 4) {

                const uint32_t * input_ptr = as_word_ptr(input_data+Offset(input_shape, batch, in_y, in_x,
                                      in_channel + group * filter_input_depth));
                const uint32_t * filter_ptr = as_word_ptr(filter_data + Offset(
                    filter_shape, out_channel, filter_y, filter_x, in_channel));
                cfu_accumulate(*input_ptr, *filter_ptr);
              }
            }
          }

          int32_t acc = cfu_read();
          if (bias_data) {
            acc += bias_data[out_channel];
          }
          acc = MultiplyByQuantizedMultiplier(
              acc, output_multiplier[out_channel], output_shift[out_channel]);
          acc += output_offset;
          acc = std::max(acc, output_activation_min);
          acc = std::min(acc, output_activation_max);
          output_data[Offset(output_shape, batch, out_y, out_x, out_channel)] =
              static_cast<int8_t>(acc);
        }
      }
    }
  }
}

}  // namespace reference_integer_ops
}  // namespace tflite
<|endoftext|>


<commit_before>#include "dict.h"

#include <string>
#include <vector>
#include <boost/regex.hpp>
#include <boost/algorithm/string/regex.hpp>

void Dict::AsVector(const WordID& id, std::vector<std::string>* results) const {
  boost::algorithm::split_regex(*results, Convert(id), boost::regex(" \\|\\|\\| "));
}

<commit_msg>Fixed major bug with spaces in regex.<commit_after>#include "dict.h"

#include <string>
#include <vector>
#include <boost/regex.hpp>
#include <boost/algorithm/string/regex.hpp>

void Dict::AsVector(const WordID& id, std::vector<std::string>* results) const {
  boost::algorithm::split_regex(*results, Convert(id), boost::regex("\\s\\|\\|\\|\\s"));
}

<|endoftext|>