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(); } } }