static void handle_setconfiguration_req(struct unix_client *client,
struct bt_set_configuration_req *req)
{
int err = 0;
if (req->codec.seid != client->seid) {
error("Unable to set configuration: seid %d not opened",
req->codec.seid);
goto failed;
}
if (!client->dev)
goto failed;
if (req->codec.transport == BT_CAPABILITIES_TRANSPORT_SCO) {
err = handle_sco_transport(client, req);
if (err < 0) {
err = -err;
goto failed;
}
} else if (req->codec.transport == BT_CAPABILITIES_TRANSPORT_A2DP) {
err = handle_a2dp_transport(client, req);
if (err < 0) {
err = -err;
goto failed;
}
}
start_config(client->dev, client);
return;
failed:
unix_ipc_error(client, BT_SET_CONFIGURATION, err ? : EIO);
}
static nodes_data::ptr configure_test_setup(const std::string &path)
{
start_nodes_config start_config(results_reporter::get_stream(), std::vector<server_config>({
server_config::default_value().apply_options(config_data()
("group", 5)
("cache_size", "100K")
("cache_shards", 1)
)
}), path);
return start_nodes(start_config);
}
static void configure_nodes(const std::vector<std::string> &remotes, const std::string &path)
{
if (remotes.empty()) {
start_nodes_config start_config(results_reporter::get_stream(), std::vector<server_config>({
server_config::default_value().apply_options(config_data()
("group", 2)
)
}), path);
global_data = start_nodes(start_config);
} else {
global_data = start_nodes(results_reporter::get_stream(), remotes, path);
}
}
static void configure_nodes(const std::string &path)
{
std::vector<server_config> servers;
for (size_t i = 0; i < groups_count; ++i) {
for (size_t j = 0; j < nodes_count; ++j) {
const int group = i + 1;
server_config server = default_value(group);
servers.push_back(server);
}
}
start_nodes_config start_config(results_reporter::get_stream(), std::move(servers), path);
start_config.fork = true;
global_data = start_nodes(start_config);
}
static nodes_data::ptr configure_test_setup(const std::string &path)
{
std::vector<server_config> servers;
for (size_t i = 0; i < groups_count; ++i) {
for (size_t j = 0; j < nodes_count; ++j) {
server_config server = default_value(i);
server.backends[0]("enable", true);
server.backends[3]("enable", true);
servers.push_back(server);
}
}
servers.push_back(default_value(groups_count));
start_nodes_config start_config(results_reporter::get_stream(), std::move(servers), path);
start_config.fork = true;
return start_nodes(start_config);
}
void leap (config c, int size, int step)
{
int i;
start_config(c);
print_bool(c);
printf(" &\n");
for (i=0; i<step; i++) {
c = config_null(size);
/* printf("WHITE SLIDE\n"); */
c->white_frogs[0] = full;
W_frog_slide(c, i, i+1);
printf(" &\n");
/* printf("\nBLACK SLIDE\n"); */
c = config_null(size);
c->black_frogs[size*2] = full;
B_frog_slide(c, i, i+1);
printf(" &\n");
/* printf("\nWHITE JUMP\n"); */
c = config_null(size);
c->white_frogs[0] = full;
c->black_frogs[1] = full;
W_frog_jump(c, i, i+1);
printf(" &\n");
/* printf("\nBLACK JUMP\n"); */
c = config_null(size);
c->white_frogs[(size*2)-1] = full;
c->black_frogs[size*2] = full;
B_frog_jump(c, i, i+1);
/* if (i != step && i != 0) */
printf(" &\n");
/* else */
/* printf("\n"); */
}
win_config(c);
print_bool(c);
printf("\n");
}
bool NAO_PLANNER_CONTROL::compute_circular_manipulation_plan(rrt_planner_msgs::Compute_Circular_Manipulation_Plan::Request &req, rrt_planner_msgs::Compute_Circular_Manipulation_Plan::Response &resp)
{
std::cout <<"Circular Manipulation Plan requested"<< std::endl;
// //-----------Start Configuration (= current robot state in planning_scene)
// //Get the joint names of the planning group
// //const std::vector<std::string>& joint_names = p_s_->getRobotModel()->getJointModelGroup(PLANNING_GROUP)->getJointModelNames();
// //Get the current configuration (for the whole robot)
// //planning_scene_monitor::LockedPlanningSceneRO p_s(psm_);
// robot_state::RobotState state = p_s_->getCurrentState();
// //Get Joint Names
// std::vector<std::string> joint_title;
// joint_title = state.getVariableNames();
// //Get Joint Values
// const double *joint_vals;
// joint_vals = state.getVariablePositions();
// std::map< std::string, double > joint_values;
// for (int i = 0; i < joint_title.size(); i++)
// {
// joint_values.insert ( std::pair<std::string,double>(joint_title[i],joint_vals[i]));
// }
// //std::map< std::string, double > joint_values;
// //state.getStateValues(joint_values);
// //Erase configuration elements not used for planning
// joint_values.erase("HeadPitch");
// joint_values.erase("HeadYaw");
// joint_values.erase("world_joint/theta");
// joint_values.erase("world_joint/x");
// joint_values.erase("world_joint/y");
// //Plot remaining configuration
// // for(std::map<std::string,double>::const_iterator i = joint_values.begin(); i != joint_values.end(); ++i)
// // {
// // std::cout << i->first << ": " << i->second << std::endl;
// // }
// //Configuration for Planning (rearrange joint values in the order of the Kin. Model)
// std::vector<double> start_config(joint_names_.size());
// for(unsigned int i = 0 ; i < joint_names_.size(); i++)
// {
// start_config[i] = joint_values[joint_names_[i]];
// //std::cout << joint_names[i] << ": " << joint_values[joint_names_[i]] << std::endl;
// }
//-----------Start Configuration (from request) -------------------------
std::vector<double> start_config(joint_names_.size());
std::cout<<"Start Config."<<std::endl;
for(unsigned int i = 0 ; i < joint_names_.size(); i++)
{
start_config[i] = req.wb_start_config[i];
std::cout << joint_names_[i] << ": " << start_config[i] << std::endl;
}
//-----------Init Planning variables
//Weights for limb joints
double r_arm_weight = 0.0;
double l_arm_weight = 0.0;
double leg_weight = 0.0;
//Weight array
std::vector<double> joint_weights(joint_names_.size());
//Array storing the goal config for the approach motion plan
std::vector<double> goal_config_approach(joint_names_.size());
//Array storing the goal config for the manipulate motion plan
std::vector<double> goal_config_manipulate(joint_names_.size());
//Set trajectory mode (false will prepare a trajectory for simulation, true will prepare a trajectory for execution)
planner_->setTrajectoryMode(req.execute_trajectory);
//------------------ Motion Planning Approach
//Goal Configuration for approach (generated by SCG)
std::vector<double> desired_right_hand_pose(6);
//Set the desired position of the right hand w.r.t the right foot
desired_right_hand_pose[0] = req.right_hand_position.x; // X [m]
desired_right_hand_pose[1] = req.right_hand_position.y; // Y [m]
desired_right_hand_pose[2] = req.right_hand_position.z; // Z [m]
//Set the desired direction of the z-axis of the right hand w.r.t the right foot frame
desired_right_hand_pose[3] = req.right_hand_direction.x; // X component of hand z-axis
desired_right_hand_pose[4] = req.right_hand_direction.y; // Y component of hand z-axis
desired_right_hand_pose[5] = req.right_hand_direction.z; // Z component of hand z-axis
//Sample a Goal Configuration
scg_->setDesiredRightArmPose(desired_right_hand_pose);
int num_goal_configs_found = 0;
float time_for_first_config = 0.0;
num_goal_configs_found = scg_->sample_stable_configs(MAX_SAMPLES,MAX_IK_ITERATIONS,time_for_first_config, true, SSC_DS_GOAL_CONFIG_FIRST);
//Time required to find first config
resp.time_goal_config_approach = time_for_first_config;
//Proceed only when a goal config has been found
if (num_goal_configs_found > 0)
{
//Get the best goal config from the file
scg_->loadGoalConfig(goal_config_approach, SSC_DS_GOAL_CONFIG_FIRST);
//Assign best goal config to service response
resp.wb_goal_config_approach = goal_config_approach;
//Set joint weights
r_arm_weight = R_ARM_WEIGHT_APPROACH;
l_arm_weight = L_ARM_WEIGHT_APPROACH;
leg_weight = LEGS_WEIGHT_APPROACH;
joint_weights[0] = leg_weight; // RAnkleRoll
joint_weights[1] = leg_weight; // RAnklePitch
joint_weights[2] = leg_weight; // RKneePitch
joint_weights[3] = leg_weight; // RHipPitch
joint_weights[4] = leg_weight; // RHipRoll
joint_weights[5] = l_arm_weight; // LShoulderPitch
joint_weights[6] = l_arm_weight; // LShoulderRoll
joint_weights[7] = l_arm_weight; // LElbowYaw
joint_weights[8] = l_arm_weight; // LElbowRoll
joint_weights[9] = l_arm_weight; // LWristYaw
joint_weights[10] = r_arm_weight; // RShoulderPitch
joint_weights[11] = r_arm_weight; // RShoulderRoll
joint_weights[12] = r_arm_weight; // RElbowYaw
joint_weights[13] = r_arm_weight; // RElbowRoll
joint_weights[14] = r_arm_weight; // RWristYaw
joint_weights[15] = 0.0; // LHipYawPitch
joint_weights[16] = leg_weight; // LHipRoll
joint_weights[17] = leg_weight; // LHipPitch
joint_weights[18] = leg_weight; // LKneePitch
joint_weights[19] = leg_weight; // LAnklePitch
joint_weights[20] = leg_weight; // LAnkleRoll
planner_->setJointWeights(joint_weights);
if (planner_->setStartGoalConfigs(start_config,goal_config_approach))
{
//Solve the query
int num_nodes = 0;
float time_elapsed = 0.0;
resp.motion_plan_approach = planner_->solveQuery(MAX_EXPAND_ITERATIONS, ADVANCE_STEPS,SOLUTION_FILE_PATH_FIRST, num_nodes,time_elapsed);
resp.num_nodes_generated_approach = num_nodes;
resp.search_time_approach = time_elapsed;
}
if(resp.motion_plan_approach.trajectory.size() > 0 && resp.motion_plan_approach.trajectory[0].joint_trajectory.points.size() > 0)
resp.success_approach = true;
else
resp.success_approach = false;
}
else
{
resp.success_approach = false;
}
//------------------ Motion Planning Manipulation
if (resp.success_approach == true)
{
//Goal Configuration for manipulation (generated by SCG)
//From handle position to point on the door
double x_door = desired_right_hand_pose[0] + req.clearance_handle * cos(req.relative_angle*(M_PI/180));
double y_door = desired_right_hand_pose[1] - req.clearance_handle * sin(req.relative_angle*(M_PI/180));
double z_door = desired_right_hand_pose[2];
//From point on the door to hinge position
double x_hinge = x_door - req.rotation_radius * sin(req.relative_angle*(M_PI/180));
double y_hinge = y_door - req.rotation_radius * cos(req.relative_angle*(M_PI/180));
double z_hinge = z_door;
//Goal position for point on the door
double tmp_angle = (req.rotation_angle-req.relative_angle) *(M_PI/180);
x_door = x_hinge - req.rotation_radius * sin(tmp_angle); // X [m];
y_door = y_hinge + req.rotation_radius * cos(tmp_angle); // Y [m];
z_door = z_hinge; // Z [m];
//Goal position for door handle
tmp_angle = 1.5708 - tmp_angle; // 90 degree - tmp_angle
std::vector<double> desired_right_hand_pose_manipulate(6);
desired_right_hand_pose_manipulate[0] = x_door - req.clearance_handle * sin(tmp_angle);
desired_right_hand_pose_manipulate[1] = y_door - req.clearance_handle * cos(tmp_angle);
desired_right_hand_pose_manipulate[2] = z_door;
//Set the desired direction of the z-axis of the right hand w.r.t the right foot frame
desired_right_hand_pose_manipulate[3] = req.right_hand_direction.x; // X component of hand z-axis
desired_right_hand_pose_manipulate[4] = req.right_hand_direction.y; // Y component of hand z-axis
desired_right_hand_pose_manipulate[5] = req.right_hand_direction.z; // Z component of hand z-axis
//Sample a Goal Configuration
scg_->setDesiredRightArmPose(desired_right_hand_pose_manipulate);
time_for_first_config = 0.0;
num_goal_configs_found = scg_->sample_stable_configs(MAX_SAMPLES,MAX_IK_ITERATIONS,time_for_first_config, true, SSC_DS_GOAL_CONFIG_SECOND);
//Time required to find first config
resp.time_goal_config_manipulate = time_for_first_config;
if (num_goal_configs_found > 0)
{
//Get the best goal config from the file
scg_->loadGoalConfig(goal_config_manipulate, SSC_DS_GOAL_CONFIG_SECOND);
//Assign best goal config to service response
resp.wb_goal_config_manipulate = goal_config_manipulate;
//Parameters for the door (required)
std::vector<double> object_parameter(4);
object_parameter[0] = req.rotation_radius; //door width
object_parameter[1] = req.rotation_angle; //door opening angle
object_parameter[2] = req.relative_angle; //relative angle of door w.r.t right foot frame
object_parameter[3] = req.clearance_handle; // distance between door and handle
//Activate manipulation constraint
planner_->activate_door_constraint(desired_right_hand_pose,desired_right_hand_pose_manipulate,20,object_parameter);
//Set joint weights
r_arm_weight = R_ARM_WEIGHT_MANIPULATE;
l_arm_weight = L_ARM_WEIGHT_MANIPULATE;
leg_weight = LEGS_WEIGHT_MANIPULATE;
joint_weights[0] = leg_weight; // RAnkleRoll
joint_weights[1] = leg_weight; // RAnklePitch
joint_weights[2] = leg_weight; // RKneePitch
joint_weights[3] = leg_weight; // RHipPitch
joint_weights[4] = leg_weight; // RHipRoll
joint_weights[5] = l_arm_weight; // LShoulderPitch
joint_weights[6] = l_arm_weight; // LShoulderRoll
joint_weights[7] = l_arm_weight; // LElbowYaw
joint_weights[8] = l_arm_weight; // LElbowRoll
joint_weights[9] = l_arm_weight; // LWristYaw
joint_weights[10] = r_arm_weight; // RShoulderPitch
joint_weights[11] = r_arm_weight; // RShoulderRoll
joint_weights[12] = r_arm_weight; // RElbowYaw
joint_weights[13] = r_arm_weight; // RElbowRoll
joint_weights[14] = r_arm_weight; // RWristYaw
joint_weights[15] = 0.0; // LHipYawPitch
joint_weights[16] = leg_weight; // LHipRoll
joint_weights[17] = leg_weight; // LHipPitch
joint_weights[18] = leg_weight; // LKneePitch
joint_weights[19] = leg_weight; // LAnklePitch
joint_weights[20] = leg_weight; // LAnkleRoll
planner_->setJointWeights(joint_weights);
if (planner_->setStartGoalConfigs(goal_config_approach,goal_config_manipulate))
{
//Solve the query
int num_nodes = 0;
float time_elapsed = 0.0;
resp.motion_plan_manipulation = planner_->solveQuery(MAX_EXPAND_ITERATIONS, ADVANCE_STEPS,SOLUTION_FILE_PATH_SECOND, num_nodes,time_elapsed);
resp.num_nodes_generated_manipulate = num_nodes;
resp.search_time_manipulate = time_elapsed;
}
if(resp.motion_plan_manipulation.trajectory.size() > 0 && resp.motion_plan_manipulation.trajectory[0].joint_trajectory.points.size() > 0)
{
resp.success_manipulation = true;
}
else
resp.success_manipulation = false;
} //END if num_goal_configs_found > 0
else
{
resp.success_manipulation = false;
}
}//END if resp.success == true
return true;
}
bool NAO_PLANNER_CONTROL::compute_motion_plan(rrt_planner_msgs::Compute_Motion_Plan::Request &req, rrt_planner_msgs::Compute_Motion_Plan::Response &resp)
{
std::cout <<"Compute Manipulation Plan requested"<< std::endl;
// //-----------Start Configuration (= current robot state in planning_scene)
// //Get the joint names of the planning group
// //const std::vector<std::string>& joint_names = p_s_->getRobotModel()->getJointModelGroup(PLANNING_GROUP)->getJointModelNames();
// //Get the current configuration (for the whole robot)
// robot_state::RobotState state = p_s_->getCurrentState();
// //Get Joint Names
// std::vector<std::string> joint_title;
// joint_title = state.getVariableNames();
// //Get Joint Values
// const double *joint_vals;
// joint_vals = state.getVariablePositions();
// std::map< std::string, double > joint_values;
// for (int i = 0; i < joint_title.size(); i++)
// {
// joint_values.insert ( std::pair<std::string,double>(joint_title[i],joint_vals[i]));
// }
// //state.getStateValues(joint_values);
// //Erase configuration elements not used for planning
// joint_values.erase("HeadPitch");
// joint_values.erase("HeadYaw");
// joint_values.erase("world_joint/theta");
// joint_values.erase("world_joint/x");
// joint_values.erase("world_joint/y");
// // //Plot remaining configuration
// // for(std::map<std::string,double>::const_iterator i = joint_values.begin(); i != joint_values.end(); ++i)
// // {
// // std::cout << i->first << ": " << i->second << std::endl;
// // }
// //Configuration for Planning (rearrange joint values in the order of the Kin. Model)
// std::vector<double> start_config(joint_names_.size());
// std::cout<<"Start Config."<<std::endl;
// for(unsigned int i = 0 ; i < joint_names_.size(); i++)
// {
// start_config[i] = joint_values[joint_names_[i]];
// std::cout << joint_names_[i] << ": " << joint_values[joint_names_[i]] << std::endl;
// }
//-----------Start Configuration (from request) -------------------------
std::vector<double> start_config(joint_names_.size());
std::cout<<"Start Config."<<std::endl;
for(unsigned int i = 0 ; i < joint_names_.size(); i++)
{
start_config[i] = req.wb_start_config[i];
std::cout << joint_names_[i] << ": " << start_config[i] << std::endl;
}
//Set trajectory mode (false will prepare a trajectory for simulation, true will prepare a trajectory for execution)
planner_->setTrajectoryMode(req.execute_trajectory);
//-----------Goal Configuration (generated by SCG)
std::vector<double> desired_right_hand_pose(6);
//Set the desired position of the right hand w.r.t the right foot
desired_right_hand_pose[0] = req.right_hand_position.x; // X [m]
desired_right_hand_pose[1] = req.right_hand_position.y; // Y [m]
desired_right_hand_pose[2] = req.right_hand_position.z; // Z [m]
//Set the desired direction of the z-axis of the right hand w.r.t the right foot frame
desired_right_hand_pose[3] = req.right_hand_direction.x; // X component of hand z-axis
desired_right_hand_pose[4] = req.right_hand_direction.y; // Y component of hand z-axis
desired_right_hand_pose[5] = req.right_hand_direction.z; // Z component of hand z-axis
//Sample a goal configuration
scg_->setDesiredRightArmPose(desired_right_hand_pose);
int num_goal_configs_found = 0;
float time_for_first_config = 0.0;
num_goal_configs_found = scg_->sample_stable_configs(MAX_SAMPLES,MAX_IK_ITERATIONS,time_for_first_config, true, SSC_DS_GOAL_CONFIG_FIRST);
//Time required to find first config
resp.time_goal_config = time_for_first_config;
if (num_goal_configs_found > 0)
{
//Get the best goal config from the file
std::vector<double> goal_config(joint_names_.size());
scg_->loadGoalConfig(goal_config, SSC_DS_GOAL_CONFIG_FIRST);
//Assign best goal config to service response
resp.wb_goal_config = goal_config;
//-----------Planner
//Set joint weights
std::vector<double> joint_weights(joint_names_.size());
//Set joint weights
double r_arm_weight = R_ARM_WEIGHT_APPROACH;
double l_arm_weight = L_ARM_WEIGHT_APPROACH;
double leg_weight = LEGS_WEIGHT_APPROACH;
joint_weights[0] = leg_weight; // RAnkleRoll
joint_weights[1] = leg_weight; // RAnklePitch
joint_weights[2] = leg_weight; // RKneePitch
joint_weights[3] = leg_weight; // RHipPitch
joint_weights[4] = leg_weight; // RHipRoll
joint_weights[5] = l_arm_weight; // LShoulderPitch
joint_weights[6] = l_arm_weight; // LShoulderRoll
joint_weights[7] = l_arm_weight; // LElbowYaw
joint_weights[8] = l_arm_weight; // LElbowRoll
joint_weights[9] = l_arm_weight; // LWristYaw
joint_weights[10] = r_arm_weight; // RShoulderPitch
joint_weights[11] = r_arm_weight; // RShoulderRoll
joint_weights[12] = r_arm_weight; // RElbowYaw
joint_weights[13] = r_arm_weight; // RElbowRoll
joint_weights[14] = r_arm_weight; // RWristYaw
joint_weights[15] = 0.0; // LHipYawPitch
joint_weights[16] = leg_weight; // LHipRoll
joint_weights[17] = leg_weight; // LHipPitch
joint_weights[18] = leg_weight; // LKneePitch
joint_weights[19] = leg_weight; // LAnklePitch
joint_weights[20] = leg_weight; // LAnkleRoll
planner_->setJointWeights(joint_weights);
std::cout<<"Right before setStartGoalConfigs in nao_planner_control"<<std::endl;
if (planner_->setStartGoalConfigs(start_config,goal_config))
{
//Solve the query
int num_nodes = 0;
float time_elapsed = 0.0;
resp.motion_plan = planner_->solveQuery(MAX_EXPAND_ITERATIONS, ADVANCE_STEPS,SOLUTION_FILE_PATH_FIRST,num_nodes,time_elapsed);
resp.num_nodes_generated = num_nodes;
resp.search_time = time_elapsed;
}
if(resp.motion_plan.trajectory.size()>0 && resp.motion_plan.trajectory[0].joint_trajectory.points.size() > 0)
resp.success = true;
else
resp.success = false;
}
else
{
resp.success = false;
}
return true;
}
/* ioctl routine */
int
vinumioctl(dev_t dev,
u_long cmd,
caddr_t data,
int flag,
struct thread *td)
{
unsigned int objno;
int error = 0;
struct sd *sd;
struct plex *plex;
struct volume *vol;
unsigned int index; /* for transferring config info */
unsigned int sdno; /* for transferring config info */
int fe; /* free list element number */
struct _ioctl_reply *ioctl_reply = (struct _ioctl_reply *) data; /* struct to return */
/* First, decide what we're looking at */
switch (DEVTYPE(dev)) {
case VINUM_SUPERDEV_TYPE: /* ordinary super device */
ioctl_reply = (struct _ioctl_reply *) data; /* save the address to reply to */
switch (cmd) {
#ifdef VINUMDEBUG
case VINUM_DEBUG:
if (((struct debuginfo *) data)->changeit) /* change debug settings */
debug = (((struct debuginfo *) data)->param);
else {
if (debug & DEBUG_REMOTEGDB)
boothowto |= RB_GDB; /* serial debug line */
else
boothowto &= ~RB_GDB; /* local ddb */
Debugger("vinum debug");
}
ioctl_reply = (struct _ioctl_reply *) data; /* reinstate the address to reply to */
ioctl_reply->error = 0;
return 0;
#endif
case VINUM_CREATE: /* create a vinum object */
error = lock_config(); /* get the config for us alone */
if (error) /* can't do it, */
return error; /* give up */
error = setjmp(command_fail); /* come back here on error */
if (error == 0) /* first time, */
ioctl_reply->error = parse_user_config((char *) data, /* update the config */
&keyword_set);
else if (ioctl_reply->error == 0) { /* longjmp, but no error status */
ioctl_reply->error = EINVAL; /* note that something's up */
ioctl_reply->msg[0] = '\0'; /* no message? */
}
unlock_config();
return 0; /* must be 0 to return the real error info */
case VINUM_GETCONFIG: /* get the configuration information */
bcopy(&vinum_conf, data, sizeof(vinum_conf));
return 0;
/* start configuring the subsystem */
case VINUM_STARTCONFIG:
return start_config(*(int *) data); /* just lock it. Parameter is 'force' */
/*
* Move the individual parts of the config to user space.
*
* Specify the index of the object in the first word of data,
* and return the object there
*/
case VINUM_DRIVECONFIG:
index = *(int *) data; /* get the index */
if (index >= (unsigned) vinum_conf.drives_allocated) /* can't do it */
return ENXIO; /* bang */
bcopy(&DRIVE[index], data, sizeof(struct _drive)); /* copy the config item out */
return 0;
case VINUM_SDCONFIG:
index = *(int *) data; /* get the index */
if (index >= (unsigned) vinum_conf.subdisks_allocated) /* can't do it */
return ENXIO; /* bang */
bcopy(&SD[index], data, sizeof(struct _sd)); /* copy the config item out */
return 0;
case VINUM_PLEXCONFIG:
index = *(int *) data; /* get the index */
if (index >= (unsigned) vinum_conf.plexes_allocated) /* can't do it */
return ENXIO; /* bang */
bcopy(&PLEX[index], data, sizeof(struct _plex)); /* copy the config item out */
return 0;
case VINUM_VOLCONFIG:
index = *(int *) data; /* get the index */
if (index >= (unsigned) vinum_conf.volumes_allocated) /* can't do it */
return ENXIO; /* bang */
bcopy(&VOL[index], data, sizeof(struct _volume)); /* copy the config item out */
return 0;
case VINUM_PLEXSDCONFIG:
index = *(int *) data; /* get the plex index */
sdno = ((int *) data)[1]; /* and the sd index */
if ((index >= (unsigned) vinum_conf.plexes_allocated) /* plex doesn't exist */
||(sdno >= PLEX[index].subdisks)) /* or it doesn't have this many subdisks */
return ENXIO; /* bang */
bcopy(&SD[PLEX[index].sdnos[sdno]], /* copy the config item out */
data,
sizeof(struct _sd));
return 0;
/*
* We get called in two places: one from the
* userland config routines, which call us
* to complete the config and save it. This
* call supplies the value 0 as a parameter.
*
* The other place is from the user "saveconfig"
* routine, which can only work if we're *not*
* configuring. In this case, supply parameter 1.
*/
case VINUM_SAVECONFIG:
if (VFLAGS & VF_CONFIGURING) { /* must be us, the others are asleep */
if (*(int *) data == 0) /* finish config */
finish_config(1); /* finish the configuration and update it */
else
return EBUSY; /* can't do it now */
}
save_config(); /* save configuration to disk */
return 0;
case VINUM_RELEASECONFIG: /* release the config */
if (VFLAGS & VF_CONFIGURING) { /* must be us, the others are asleep */
finish_config(0); /* finish the configuration, don't change it */
save_config(); /* save configuration to disk */
} else
error = EINVAL; /* release what config? */
return error;
case VINUM_INIT:
ioctl_reply = (struct _ioctl_reply *) data; /* reinstate the address to reply to */
ioctl_reply->error = 0;
return 0;
case VINUM_RESETCONFIG:
if (vinum_inactive(0)) { /* if the volumes are not active */
/*
* Note the open count. We may be called from v, so we'll be open.
* Keep the count so we don't underflow
*/
free_vinum(1); /* clean up everything */
log(LOG_NOTICE, "vinum: CONFIGURATION OBLITERATED\n");
ioctl_reply = (struct _ioctl_reply *) data; /* reinstate the address to reply to */
ioctl_reply->error = 0;
return 0;
}
return EBUSY;
case VINUM_SETSTATE:
setstate((struct vinum_ioctl_msg *) data); /* set an object state */
return 0;
/*
* Set state by force, without changing
* anything else.
*/
case VINUM_SETSTATE_FORCE:
setstate_by_force((struct vinum_ioctl_msg *) data); /* set an object state */
return 0;
#ifdef VINUMDEBUG
case VINUM_MEMINFO:
vinum_meminfo(data);
return 0;
case VINUM_MALLOCINFO:
return vinum_mallocinfo(data);
case VINUM_RQINFO:
return vinum_rqinfo(data);
#endif
case VINUM_LABEL: /* label a volume */
ioctl_reply->error = write_volume_label(*(int *) data); /* index of the volume to label */
ioctl_reply->msg[0] = '\0'; /* no message */
return 0;
case VINUM_REMOVE:
remove((struct vinum_ioctl_msg *) data); /* remove an object */
return 0;
case VINUM_GETFREELIST: /* get a drive free list element */
index = *(int *) data; /* get the drive index */
fe = ((int *) data)[1]; /* and the free list element */
if ((index >= (unsigned) vinum_conf.drives_allocated) /* plex doesn't exist */
||(DRIVE[index].state == drive_unallocated))
return ENODEV;
if (fe >= DRIVE[index].freelist_entries) /* no such entry */
return ENOENT;
bcopy(&DRIVE[index].freelist[fe],
data,
sizeof(struct drive_freelist));
return 0;
case VINUM_RESETSTATS:
resetstats((struct vinum_ioctl_msg *) data); /* reset object stats */
return 0;
/* attach an object to a superordinate object */
case VINUM_ATTACH:
attachobject((struct vinum_ioctl_msg *) data);
return 0;
/* detach an object from a superordinate object */
case VINUM_DETACH:
detachobject((struct vinum_ioctl_msg *) data);
return 0;
/* rename an object */
case VINUM_RENAME:
renameobject((struct vinum_rename_msg *) data);
return 0;
/* replace an object */
case VINUM_REPLACE:
replaceobject((struct vinum_ioctl_msg *) data);
return 0;
case VINUM_DAEMON:
vinum_daemon(); /* perform the daemon */
return 0;
case VINUM_FINDDAEMON: /* check for presence of daemon */
return vinum_finddaemon();
return 0;
case VINUM_SETDAEMON: /* set daemon flags */
return vinum_setdaemonopts(*(int *) data);
case VINUM_GETDAEMON: /* get daemon flags */
*(int *) data = daemon_options;
return 0;
case VINUM_PARITYOP: /* check/rebuild RAID-4/5 parity */
parityops((struct vinum_ioctl_msg *) data);
return 0;
/* move an object */
case VINUM_MOVE:
moveobject((struct vinum_ioctl_msg *) data);
return 0;
default:
/* FALLTHROUGH */
break;
}
case VINUM_DRIVE_TYPE:
default:
log(LOG_WARNING,
"vinumioctl: invalid ioctl from process %d (%s): %lx\n",
curthread->td_proc->p_pid,
curthread->td_proc->p_comm,
cmd);
return EINVAL;
case VINUM_SD_TYPE:
case VINUM_RAWSD_TYPE:
objno = Sdno(dev);
sd = &SD[objno];
switch (cmd) {
case DIOCGDINFO: /* get disk label */
get_volume_label(sd->name, 1, sd->sectors, (struct disklabel *) data);
break;
/*
* We don't have this stuff on hardware,
* so just pretend to do it so that
* utilities don't get upset.
*/
case DIOCWDINFO: /* write partition info */
case DIOCSDINFO: /* set partition info */
return 0; /* not a titty */
default:
return ENOTTY; /* not my kind of ioctl */
}
return 0; /* pretend we did it */
case VINUM_RAWPLEX_TYPE:
case VINUM_PLEX_TYPE:
objno = Plexno(dev);
plex = &PLEX[objno];
switch (cmd) {
case DIOCGDINFO: /* get disk label */
get_volume_label(plex->name, 1, plex->length, (struct disklabel *) data);
break;
/*
* We don't have this stuff on hardware,
* so just pretend to do it so that
* utilities don't get upset.
*/
case DIOCWDINFO: /* write partition info */
case DIOCSDINFO: /* set partition info */
return 0; /* not a titty */
default:
return ENOTTY; /* not my kind of ioctl */
}
return 0; /* pretend we did it */
case VINUM_VOLUME_TYPE:
objno = Volno(dev);
if ((unsigned) objno >= (unsigned) vinum_conf.volumes_allocated) /* not a valid volume */
return ENXIO;
vol = &VOL[objno];
if (vol->state != volume_up) /* not up, */
return EIO; /* I/O error */
switch (cmd) {
case DIOCGMEDIASIZE:
*(off_t *)data = vol->size << DEV_BSHIFT;
break;
case DIOCGSECTORSIZE:
*(u_int *)data = DEV_BSIZE;
break;
/*
* We don't have this stuff on hardware,
* so just pretend to do it so that
* utilities don't get upset.
*/
case DIOCWDINFO: /* write partition info */
case DIOCSDINFO: /* set partition info */
return 0; /* not a titty */
case DIOCWLABEL: /* set or reset label writeable */
if ((flag & FWRITE) == 0) /* not writeable? */
return EACCES; /* no, die */
if (*(int *) data != 0) /* set it? */
vol->flags |= VF_WLABEL; /* yes */
else
vol->flags &= ~VF_WLABEL; /* no, reset */
break;
default:
return ENOTTY; /* not my kind of ioctl */
}
break;
}
return 0; /* XXX */
}
/* ioctl routine */
int
vinumioctl(struct dev_ioctl_args *ap)
{
cdev_t dev = ap->a_head.a_dev;
u_long cmd = ap->a_cmd;
caddr_t data = ap->a_data;
int error;
unsigned int index; /* for transferring config info */
unsigned int sdno; /* for transferring config info */
unsigned int objno;
struct volume *vol;
struct partinfo *dpart;
int fe; /* free list element number */
struct _ioctl_reply *ioctl_reply; /* struct to return */
error = 0;
/* First, decide what we're looking at */
switch (DEVTYPE(dev)) {
case VINUM_SUPERDEV_TYPE: /* ordinary super device */
ioctl_reply = (struct _ioctl_reply *) data; /* save the address to reply to */
switch (cmd) {
#ifdef VINUMDEBUG
case VINUM_DEBUG:
if (((struct debuginfo *) data)->changeit) /* change debug settings */
debug = (((struct debuginfo *) data)->param);
else {
if (debug & DEBUG_REMOTEGDB)
boothowto |= RB_GDB; /* serial debug line */
else
boothowto &= ~RB_GDB; /* local ddb */
Debugger("vinum debug");
}
ioctl_reply = (struct _ioctl_reply *) data; /* reinstate the address to reply to */
ioctl_reply->error = 0;
break;
#endif
case VINUM_CREATE: /* create a vinum object */
error = lock_config(); /* get the config for us alone */
if (error) /* can't do it, */
break;
error = setjmp(command_fail); /* come back here on error */
if (error == 0) /* first time, */
ioctl_reply->error = parse_user_config((char *) data, /* update the config */
&keyword_set);
else if (ioctl_reply->error == 0) { /* longjmp, but no error status */
error = 0;
ioctl_reply->error = EINVAL; /* note that something's up */
ioctl_reply->msg[0] = '\0'; /* no message? */
}
unlock_config();
break;
case VINUM_GETCONFIG: /* get the configuration information */
bcopy(&vinum_conf, data, sizeof(vinum_conf));
break;
/* start configuring the subsystem */
case VINUM_STARTCONFIG:
error = start_config(*(int *) data); /* just lock it. Parameter is 'force' */
break;
case VINUM_DRIVECONFIG:
/*
* Move the individual parts of the config to user space.
*
* Specify the index of the object in the first word of data,
* and return the object there
*/
index = *(int *) data;
if (index >= (unsigned)vinum_conf.drives_allocated) {
error = ENXIO;
} else {
bcopy(&DRIVE[index], data, sizeof(struct drive));
}
break;
case VINUM_SDCONFIG:
index = *(int *) data;
if (index >= (unsigned) vinum_conf.subdisks_allocated) {
error = ENXIO;
} else {
bcopy(&SD[index], data, sizeof(struct sd));
}
break;
case VINUM_PLEXCONFIG:
index = *(int *) data;
if (index >= (unsigned) vinum_conf.plexes_allocated) {
error = ENXIO;
} else {
bcopy(&PLEX[index], data, sizeof(struct plex));
}
break;
case VINUM_VOLCONFIG:
index = *(int *) data;
if (index >= (unsigned) vinum_conf.volumes_allocated) {
error = ENXIO;
} else {
bcopy(&VOL[index], data, sizeof(struct volume));
}
break;
case VINUM_PLEXSDCONFIG:
index = ((int *)data)[0]; /* get the plex index */
sdno = ((int *)data)[1]; /* and the sd index */
if ((index >= (unsigned) vinum_conf.plexes_allocated)
||(sdno >= PLEX[index].subdisks)) {
error = ENXIO;
} else {
bcopy(&SD[PLEX[index].sdnos[sdno]], data, sizeof(struct sd));
}
break;
case VINUM_SAVECONFIG:
/*
* We get called in two places: one from the
* userland config routines, which call us
* to complete the config and save it. This
* call supplies the value 0 as a parameter.
*
* The other place is from the user "saveconfig"
* routine, which can only work if we're *not*
* configuring. In this case, supply parameter 1.
*/
if (VFLAGS & VF_CONFIGURING) { /* must be us, the others are asleep */
if (*(int *) data == 0) /* finish config */
finish_config(1); /* finish the configuration and update it */
else
error = EBUSY;
}
if (error == 0)
save_config(); /* save configuration to disk */
break;
case VINUM_RELEASECONFIG: /* release the config */
if (VFLAGS & VF_CONFIGURING) { /* must be us, the others are asleep */
finish_config(0); /* finish the configuration, don't change it */
save_config(); /* save configuration to disk */
} else {
error = EINVAL; /* release what config? */
}
break;
case VINUM_INIT:
ioctl_reply = (struct _ioctl_reply *) data; /* reinstate the address to reply to */
ioctl_reply->error = 0;
break;
case VINUM_RESETCONFIG:
if (vinum_inactive(0)) { /* if the volumes are not active */
/*
* Note the open count. We may be called from v, so we'll be open.
* Keep the count so we don't underflow
*/
free_vinum(1); /* clean up everything */
log(LOG_NOTICE, "vinum: CONFIGURATION OBLITERATED\n");
ioctl_reply = (struct _ioctl_reply *) data; /* reinstate the address to reply to */
ioctl_reply->error = 0;
} else {
error = EBUSY;
}
case VINUM_SETSTATE:
setstate((struct vinum_ioctl_msg *) data); /* set an object state */
break;
/*
* Set state by force, without changing
* anything else.
*/
case VINUM_SETSTATE_FORCE:
setstate_by_force((struct vinum_ioctl_msg *) data); /* set an object state */
break;
#ifdef VINUMDEBUG
case VINUM_MEMINFO:
vinum_meminfo(data);
break;
case VINUM_MALLOCINFO:
error = vinum_mallocinfo(data);
break;
case VINUM_RQINFO:
error = vinum_rqinfo(data);
break;
#endif
case VINUM_REMOVE:
remove((struct vinum_ioctl_msg *) data); /* remove an object */
break;
case VINUM_GETFREELIST: /* get a drive free list element */
index = *(int *) data; /* get the drive index */
fe = ((int *) data)[1]; /* and the free list element */
if ((index >= (unsigned) vinum_conf.drives_allocated) /* plex doesn't exist */
||(DRIVE[index].state == drive_unallocated)) {
error = ENODEV;
} else if (fe >= DRIVE[index].freelist_entries) {
error = ENOENT;
} else {
bcopy(&DRIVE[index].freelist[fe], data,
sizeof(struct drive_freelist));
}
break;
case VINUM_RESETSTATS:
resetstats((struct vinum_ioctl_msg *) data); /* reset object stats */
break;
/* attach an object to a superordinate object */
case VINUM_ATTACH:
attachobject((struct vinum_ioctl_msg *) data);
break;
/* detach an object from a superordinate object */
case VINUM_DETACH:
detachobject((struct vinum_ioctl_msg *) data);
break;
/* rename an object */
case VINUM_RENAME:
renameobject((struct vinum_rename_msg *) data);
break;
/* replace an object */
case VINUM_REPLACE:
replaceobject((struct vinum_ioctl_msg *) data);
break;
case VINUM_DAEMON:
vinum_daemon(); /* perform the daemon */
break;
case VINUM_FINDDAEMON: /* check for presence of daemon */
error = vinum_finddaemon();
break;
case VINUM_SETDAEMON: /* set daemon flags */
error = vinum_setdaemonopts(*(int *) data);
break;
case VINUM_GETDAEMON: /* get daemon flags */
*(int *) data = daemon_options;
break;
case VINUM_PARITYOP: /* check/rebuild RAID-4/5 parity */
parityops((struct vinum_ioctl_msg *) data);
break;
/* move an object */
case VINUM_MOVE:
moveobject((struct vinum_ioctl_msg *) data);
break;
default:
error = EINVAL;
break;
}
break;
case VINUM_LABEL:
case VINUM_DRIVE_TYPE:
case VINUM_SD_TYPE:
case VINUM_RAWSD_TYPE:
case VINUM_RAWPLEX_TYPE:
case VINUM_PLEX_TYPE:
error = EINVAL;
break;
case VINUM_VOLUME_TYPE:
objno = Volno(dev);
if ((unsigned)objno >= (unsigned)vinum_conf.volumes_allocated) {
error = ENXIO;
break;
}
vol = &VOL[objno];
if (vol->state != volume_up) {
error = EIO;
break;
}
switch(cmd) {
case DIOCGPART:
dpart = (void *)data;
bzero(dpart, sizeof(*dpart));
dpart->media_offset = 0;
dpart->media_size = (u_int64_t)vol->size * DEV_BSIZE;
dpart->media_blocks = vol->size;
dpart->media_blksize = DEV_BSIZE;
dpart->fstype = FS_BSDFFS;
break;
default:
error = EINVAL;
}
break;
default:
error = EINVAL;
break;
}
if (error) {
log(LOG_WARNING,
"vinumioctl: invalid ioctl from process %d (%s): %lx\n",
curproc->p_pid,
curproc->p_comm,
cmd);
}
return error;
}
