GridMap GridMap::getSubmap(const Eigen::Vector2d& position, const Eigen::Array2d& length, Eigen::Array2i& indexInSubmap, bool& isSuccess)
{
// Submap the generate.
GridMap submap(types_);
submap.setClearTypes(clearTypes_);
submap.setTimestamp(timestamp_);
submap.setFrameId(frameId_);
// Get submap geometric information.
Array2i topLeftIndex;
Array2i submapBufferSize;
Eigen::Vector2d submapPosition;
Array2d submapLength;
if (!getSubmapInformation(topLeftIndex, submapBufferSize, submapPosition, submapLength, indexInSubmap, position,
length, length_, position_, resolution_, bufferSize_, bufferStartIndex_)) {
isSuccess = false;
return GridMap(types_);
}
submap.setGeometry(submapLength, resolution_, submapPosition);
submap.bufferStartIndex_.setZero(); // Because of the way we copy the data below.
// Copy data.
std::vector<Eigen::Array2i> submapIndeces;
std::vector<Eigen::Array2i> submapSizes;
if (!getBufferRegionsForSubmap(submapIndeces, submapSizes, topLeftIndex, submap.bufferSize_, bufferSize_, bufferStartIndex_)) {
cout << "Cannot access submap of this size." << endl;
isSuccess = false;
return GridMap(types_);
}
for (auto& data : data_) {
submap.data_[data.first].topLeftCorner (submapSizes[0](0), submapSizes[0](1)) = data.second.block(submapIndeces[0](0), submapIndeces[0](1), submapSizes[0](0), submapSizes[0](1));
submap.data_[data.first].topRightCorner (submapSizes[1](0), submapSizes[1](1)) = data.second.block(submapIndeces[1](0), submapIndeces[1](1), submapSizes[1](0), submapSizes[1](1));
submap.data_[data.first].bottomLeftCorner (submapSizes[2](0), submapSizes[2](1)) = data.second.block(submapIndeces[2](0), submapIndeces[2](1), submapSizes[2](0), submapSizes[2](1));
submap.data_[data.first].bottomRightCorner(submapSizes[3](0), submapSizes[3](1)) = data.second.block(submapIndeces[3](0), submapIndeces[3](1), submapSizes[3](0), submapSizes[3](1));
}
isSuccess = true;
return submap;
}
void address_map::uplift_submaps(running_machine &machine, device_t &device, device_t &owner, endianness_t endian)
{
address_map_entry *prev = 0;
address_map_entry *entry = m_entrylist.first();
while (entry)
{
if (entry->m_read.m_type == AMH_DEVICE_SUBMAP)
{
std::string tag = owner.subtag(entry->m_read.m_tag);
device_t *mapdevice = machine.device(tag.c_str());
if (mapdevice == NULL) {
throw emu_fatalerror("Attempted to submap a non-existent device '%s' in space %d of device '%s'\n", tag.c_str(), m_spacenum, device.basetag());
}
// Grab the submap
address_map submap(*mapdevice, entry);
// Recursively uplift it if needed
submap.uplift_submaps(machine, device, *mapdevice, endian);
// Compute the unit repartition characteristics
int entry_bits = entry->m_submap_bits;
if (!entry_bits)
entry_bits = m_databits;
if (submap.m_databits != entry_bits)
throw emu_fatalerror("AM_DEVICE wants a %d bits large address map and got a %d bits large one instead.\n", entry_bits, submap.m_databits);
int entry_bytes = entry_bits / 8;
int databytes = m_databits / 8;
offs_t mirror_address_mask = (databytes - 1) & ~(entry_bytes - 1);
UINT64 entry_mask = (2ULL << (entry_bits-1)) - 1;
int slot_offset[8];
int slot_count = 0;
int max_slot_count = m_databits / entry_bits;
int slot_xor_mask = endian == ENDIANNESS_LITTLE ? 0 : max_slot_count - 1;
UINT64 global_mask = entry->m_read.m_mask;
// zero means all
if (!global_mask)
global_mask = ~global_mask;
// mask consistency has already been checked in
// unitmask_is_appropriate, so one bit is enough
for (int slot=0; slot < max_slot_count; slot++)
if (global_mask & (1ULL << ((slot ^ slot_xor_mask) * entry_bits)))
slot_offset[slot_count++] = (slot ^ slot_xor_mask) * entry_bits;
// Merge in all the map contents in order
while (submap.m_entrylist.count())
{
address_map_entry *subentry = submap.m_entrylist.detach_head();
// Remap start and end
unsigned int start_offset = subentry->m_addrstart / entry_bytes;
unsigned int start_slot = start_offset % slot_count;
subentry->m_addrstart = entry->m_addrstart + (start_offset / slot_count) * databytes;
// Drop the entry if it ends up outside the range
if (subentry->m_addrstart > entry->m_addrend)
{
global_free(subentry);
continue;
}
unsigned int end_offset = subentry->m_addrend / entry_bytes;
unsigned int end_slot = end_offset % slot_count;
subentry->m_addrend = entry->m_addrstart + (end_offset / slot_count) * databytes + databytes - 1;
// Clip the entry to the end of the range
if (subentry->m_addrend > entry->m_addrend || subentry->m_addrend < entry->m_addrstart)
subentry->m_addrend = entry->m_addrend;
// Detect special unhandled case (range straddling
// slots, requiring splitting in multiple entries and
// unimplemented offset-add subunit handler)
if (subentry->m_addrstart + databytes - 1 != subentry->m_addrend &&
(start_slot != 0 || end_slot != slot_count - 1))
throw emu_fatalerror("uplift_submaps unhandled case: range straddling slots.\n");
if (entry->m_addrmask || subentry->m_addrmask)
throw emu_fatalerror("uplift_submaps unhandled case: address masks.\n");
if (subentry->m_addrmirror & mirror_address_mask)
throw emu_fatalerror("uplift_submaps unhandled case: address mirror bit within subentry.\n");
subentry->m_addrmirror |= entry->m_addrmirror;
// Twiddle the unitmask on the data accessors that need it
for (int data_entry = 0; data_entry < 3; data_entry++)
{
map_handler_data &mdata = (data_entry==0)? subentry->m_read : ((data_entry==1)? subentry->m_write : subentry->m_setoffsethd);
if (mdata.m_type == AMH_NONE)
continue;
if (mdata.m_type != AMH_DEVICE_DELEGATE && mdata.m_type != AMH_NOP)
throw emu_fatalerror("Only normal read/write methods are accepted in device submaps.\n");
if (mdata.m_bits == 0 && entry_bits != m_databits)
mdata.m_bits = entry_bits;
UINT64 mask = 0;
if (mdata.m_bits != m_databits)
{
UINT64 unitmask = mdata.m_mask ? mdata.m_mask : entry_mask;
for (int slot = start_slot; slot <= end_slot; slot++)
mask |= unitmask << slot_offset[slot];
}
mdata.m_mask = mask;
}
// Insert the entry in the map
m_entrylist.insert_after(*subentry, prev);
prev = subentry;
}
address_map_entry *to_delete = entry;
entry = entry->next();
m_entrylist.remove(*to_delete);
}
else
{
prev = entry;
entry = entry->next();
}
}
}
