/*
* This routine sets the configuration fields for an existing device's
* descriptor. It only works for the last device, but that's OK because that's
* how we use it.
*/
static void set_config(struct device *dev, unsigned len, const void *conf)
{
/* Check we haven't overflowed our single page. */
if (device_config(dev) + len > devices.descpage + getpagesize())
errx(1, "Too many devices");
/* Copy in the config information, and store the length. */
memcpy(device_config(dev), conf, len);
dev->desc->config_len = len;
/* Size must fit in config_len field (8 bits)! */
assert(dev->desc->config_len == len);
}
static void set_config(struct device *dev, unsigned len, const void *conf)
{
if (device_config(dev) + len > devices.descpage + getpagesize())
errx(1, "Too many devices");
memcpy(device_config(dev), conf, len);
dev->desc->config_len = len;
assert(dev->desc->config_len == len);
}
void TConfigParser::LoadDevice(PPortConfig port_config,
const Json::Value& device_data,
const std::string& default_id)
{
if (!device_data.isObject())
throw TConfigParserException("malformed config");
if (device_data.isMember("enabled") && !device_data["enabled"].asBool())
return;
PTemplate tmpl;
PDeviceConfig device_config(new TDeviceConfig);
device_config->Id = device_data.isMember("id") ? device_data["id"].asString() : default_id;
device_config->Name = device_data.isMember("name") ? device_data["name"].asString() : "";
device_config->SlaveId = GetInt(device_data, "slave_id");
if (device_data.isMember("max_reg_hole"))
device_config->MaxRegHole = GetInt(device_data, "max_reg_hole");
if (device_data.isMember("max_bit_hole"))
device_config->MaxRegHole = GetInt(device_data, "max_bit_hole");
if (device_data.isMember("device_type")){
device_config->DeviceType = device_data["device_type"].asString();
auto it = Templates->find(device_config->DeviceType);
if (it != Templates->end()) {
tmpl = it->second;
if (tmpl->DeviceData.isMember("name")) {
if (device_config->Name == "")
device_config->Name = tmpl->DeviceData["name"].asString() + " " +
std::to_string(device_config->SlaveId);
} else if (device_config->Name == "")
throw TConfigParserException(
"Property device_name is missing in " + device_config->DeviceType + " template");
if (tmpl->DeviceData.isMember("id")) {
if (device_config->Id == default_id)
device_config->Id = tmpl->DeviceData["id"].asString() + "_" +
std::to_string(device_config->SlaveId);
}
LoadDeviceTemplatableConfigPart(device_config, tmpl->DeviceData);
} else
throw TConfigParserException(
"Can't find the template for '" + device_config->DeviceType + "' device type.");
}
LoadDeviceTemplatableConfigPart(device_config, device_data);
MergeAndLoadChannels(device_config, device_data, tmpl);
if (device_config->DeviceChannels.empty())
throw TConfigParserException("the device has no channels: " + device_config->Name);
port_config->AddDeviceConfig(device_config);
for (auto channel: device_config->DeviceChannels) {
for (auto reg: channel->Registers)
if (reg->PollInterval.count() < 0)
reg->PollInterval = port_config->PollInterval;
}
}
Calibration::Calibration(
cml::Config *cml_config,
OpticalDevice::Config& proj_cfg,
OpticalDevice::Config& depth_cfg,
OpticalDevice::Config& rgb_cfg
)
{
string kcalib =
cml_config->kinect_calib_path;
string pcalib =
cml_config->projector_calib_path;
device_config(
pcalib,
"proj_intrinsics",
"proj_size",
"R", "T",
proj_cfg );
device_config(
kcalib,
"depth_intrinsics",
"depth_size",
"R", "T",
depth_cfg );
//override depth extrinsics
//to be the world origin
//RT in depth config is RT depth->rgb
depth_cfg.extrinsics(
ofVec3f( 1.,0.,0. ),
ofVec3f( 0.,1.,0. ),
ofVec3f( 0.,0.,1. ),
ofVec3f( 0.,0.,0. ) );
device_config(
kcalib,
"rgb_intrinsics",
"rgb_size",
"R", "T",
rgb_cfg );
};
/*
* Each device descriptor is followed by the description of its virtqueues. We
* specify how many descriptors the virtqueue is to have.
*/
static void add_virtqueue(struct device *dev, unsigned int num_descs,
void (*service)(struct virtqueue *))
{
unsigned int pages;
struct virtqueue **i, *vq = malloc(sizeof(*vq));
void *p;
/* First we need some memory for this virtqueue. */
pages = (vring_size(num_descs, LGUEST_VRING_ALIGN) + getpagesize() - 1)
/ getpagesize();
p = get_pages(pages);
/* Initialize the virtqueue */
vq->next = NULL;
vq->last_avail_idx = 0;
vq->dev = dev;
/*
* This is the routine the service thread will run, and its Process ID
* once it's running.
*/
vq->service = service;
vq->thread = (pid_t)-1;
/* Initialize the configuration. */
vq->config.num = num_descs;
vq->config.irq = devices.next_irq++;
vq->config.pfn = to_guest_phys(p) / getpagesize();
/* Initialize the vring. */
vring_init(&vq->vring, num_descs, p, LGUEST_VRING_ALIGN);
/*
* Append virtqueue to this device's descriptor. We use
* device_config() to get the end of the device's current virtqueues;
* we check that we haven't added any config or feature information
* yet, otherwise we'd be overwriting them.
*/
assert(dev->desc->config_len == 0 && dev->desc->feature_len == 0);
memcpy(device_config(dev), &vq->config, sizeof(vq->config));
dev->num_vq++;
dev->desc->num_vq++;
verbose("Virtqueue page %#lx\n", to_guest_phys(p));
/*
* Add to tail of list, so dev->vq is first vq, dev->vq->next is
* second.
*/
for (i = &dev->vq; *i; i = &(*i)->next);
*i = vq;
}
Calibration::Calibration( cml::Config *cml_config )
{
near = cml_config->near;
far = cml_config->far;
string kcalib = cml_config->kinect_calib_path;
string pcalib = cml_config->projector_calib_path;
device_config( &proj_cfg, pcalib,
"proj_intrinsics",
"proj_size",
"R", "T" );
device_config( &depth_cfg, kcalib,
"depth_intrinsics",
"depth_size",
"R", "T" );
device_config( &rgb_cfg, kcalib,
"rgb_intrinsics",
"rgb_size",
"R", "T" );
};
static struct lguest_device_desc *new_dev_desc(u16 type)
{
struct lguest_device_desc d = { .type = type };
void *p;
if (devices.lastdev)
p = device_config(devices.lastdev)
+ devices.lastdev->desc->config_len;
else
p = devices.descpage;
if (p + sizeof(d) > (void *)devices.descpage + getpagesize())
errx(1, "Too many devices");
return memcpy(p, &d, sizeof(d));
}
/*
* This routine allocates a new "struct lguest_device_desc" from descriptor
* table page just above the Guest's normal memory. It returns a pointer to
* that descriptor.
*/
static struct lguest_device_desc *new_dev_desc(u16 type)
{
struct lguest_device_desc d = { .type = type };
void *p;
/* Figure out where the next device config is, based on the last one. */
if (devices.lastdev)
p = device_config(devices.lastdev)
+ devices.lastdev->desc->config_len;
else
p = devices.descpage;
/* We only have one page for all the descriptors. */
if (p + sizeof(d) > (void *)devices.descpage + getpagesize())
errx(1, "Too many devices");
/* p might not be aligned, so we memcpy in. */
return memcpy(p, &d, sizeof(d));
}
static void add_virtqueue(struct device *dev, unsigned int num_descs,
void (*service)(struct virtqueue *))
{
unsigned int pages;
struct virtqueue **i, *vq = malloc(sizeof(*vq));
void *p;
pages = (vring_size(num_descs, LGUEST_VRING_ALIGN) + getpagesize() - 1)
/ getpagesize();
p = get_pages(pages);
vq->next = NULL;
vq->last_avail_idx = 0;
vq->dev = dev;
vq->service = service;
vq->thread = (pid_t)-1;
vq->config.num = num_descs;
vq->config.irq = devices.next_irq++;
vq->config.pfn = to_guest_phys(p) / getpagesize();
vring_init(&vq->vring, num_descs, p, LGUEST_VRING_ALIGN);
assert(dev->desc->config_len == 0 && dev->desc->feature_len == 0);
memcpy(device_config(dev), &vq->config, sizeof(vq->config));
dev->num_vq++;
dev->desc->num_vq++;
verbose("Virtqueue page %#lx\n", to_guest_phys(p));
for (i = &dev->vq; *i; i = &(*i)->next);
*i = vq;
}
void Calibration::device_config(
OpticalDevice::Config *dev_config,
string calib_path,
string int_id,
string size_id,
string R_id, string T_id )
{
CvMat* _int = (CvMat*)cvLoad( calib_path.c_str(),
NULL, int_id.c_str() );
CvMat* _size = (CvMat*)cvLoad( calib_path.c_str(),
NULL, size_id.c_str() );
CvMat* _R = (CvMat*)cvLoad( calib_path.c_str(),
NULL, R_id.c_str() );
CvMat* _T = (CvMat*)cvLoad( calib_path.c_str(),
NULL, T_id.c_str() );
if (ofGetLogLevel() == OF_LOG_VERBOSE)
{
ofLog(OF_LOG_VERBOSE,
"\n cml::Calibration "+calib_path);
ofLog( OF_LOG_VERBOSE, "intrinsics" );
printM( _int );
//ofLog( OF_LOG_VERBOSE, "R" );
//printM( _R );
//ofLog( OF_LOG_VERBOSE, "T" );
//printM( _T );
}
device_config( dev_config, _size, _int, _R, _T );
cvReleaseMat( &_int );
cvReleaseMat( &_size );
cvReleaseMat( &_R );
cvReleaseMat( &_T );
};
void
device_config_cb (GtkWidget * widget, gpointer data)
{
device_config ();
}
