diff --git a/Storage/Disk/DiskImage/Formats/AmigaADF.cpp b/Storage/Disk/DiskImage/Formats/AmigaADF.cpp index 82c91506c..66cc7dd7c 100644 --- a/Storage/Disk/DiskImage/Formats/AmigaADF.cpp +++ b/Storage/Disk/DiskImage/Formats/AmigaADF.cpp @@ -8,9 +8,85 @@ #include "AmigaADF.hpp" +#include "../../Encodings/MFM/Constants.hpp" +#include "../../Encodings/MFM/Encoder.hpp" +#include "../../Track/PCMTrack.hpp" using namespace Storage::Disk; +namespace { + +template void write_block(IteratorT begin, IteratorT end, std::unique_ptr &encoder) { + // Parse 1: write odd bits. + auto cursor = begin; + while(cursor != end) { + uint8_t source = + ((*cursor & 0x01) >> 0) | + ((*cursor & 0x04) >> 1) | + ((*cursor & 0x10) >> 2) | + ((*cursor & 0x40) >> 3); + ++cursor; + source |= + ((*cursor & 0x01) << 4) | + ((*cursor & 0x04) << 3) | + ((*cursor & 0x10) << 2) | + ((*cursor & 0x40) << 1); + ++cursor; + + encoder->add_byte(source); + } + + // Parse 2: write even bits. + cursor = begin; + while(cursor != end) { + uint8_t source = + ((*cursor & 0x02) >> 1) | + ((*cursor & 0x08) >> 2) | + ((*cursor & 0x20) >> 3) | + ((*cursor & 0x80) >> 4); + ++cursor; + source |= + ((*cursor & 0x02) << 3) | + ((*cursor & 0x08) << 2) | + ((*cursor & 0x20) << 1) | + ((*cursor & 0x80) << 0); + ++cursor; + + encoder->add_byte(source); + } +} + +template std::array checksum(IteratorT begin, IteratorT end) { + uint64_t sum = 0; + + while(begin != end) { + // Pull a big-endian 32-bit number. + uint32_t next = 0; + next |= uint64_t(*begin) << 24; ++begin; + next |= uint64_t(*begin) << 16; ++begin; + next |= uint64_t(*begin) << 8; ++begin; + next |= uint64_t(*begin) << 0; ++begin; + + // Add, and then add carry too. + sum += uint64_t(next); + sum += (sum >> 32); + sum &= 0xffff'ffff; + } + + // Invert. + sum = ~sum; + + // Pack big-endian. + return std::array{ + uint8_t((sum >> 24) & 0xff), + uint8_t((sum >> 16) & 0xff), + uint8_t((sum >> 8) & 0xff), + uint8_t((sum >> 0) & 0xff) + }; +} + +} + AmigaADF::AmigaADF(const std::string &file_name) : file_(file_name) { // Dumb validation only for now: a size check. @@ -26,11 +102,56 @@ int AmigaADF::get_head_count() { } std::shared_ptr AmigaADF::get_track_at_position(Track::Address address) { + using namespace Storage::Encodings; + + // Create an MFM encoder. + Storage::Disk::PCMSegment encoded_segment; + encoded_segment.data.reserve(102'400); // i.e. 0x1900 bytes. + auto encoder = MFM::GetMFMEncoder(encoded_segment.data); + + // Each track begins with two sync words. + encoder->output_short(MFM::MFMSync); + encoder->output_short(MFM::MFMSync); + + // Grab the unencoded track. file_.seek(get_file_offset_for_position(address), SEEK_SET); + const std::vector track_data = file_.read(512 * 11); - // TODO. + // Eleven sectors are then encoded. + for(size_t s = 0; s < 11; s++) { + // Two bytes of 0x00 act as an inter-sector gap. + encoder->add_byte(0); + encoder->add_byte(0); - return nullptr; + // Add additional sync. + encoder->output_short(MFM::MFMSync); + encoder->output_short(MFM::MFMSync); + + // Write the header. + const uint8_t header[4] = { + 0xff, // Amiga v1.0 format. + uint8_t(address.position.as_int()), // Track. + uint8_t(s), // Sector. + uint8_t(11 - s), // Sectors remaining. + }; + write_block(header, &header[4], encoder); + + // Write the sector label. + const std::array os_recovery{}; + write_block(os_recovery.begin(), os_recovery.end(), encoder); + + // Write checksums. + const auto header_checksum = checksum(&header[0], &header[4]); + write_block(header_checksum.begin(), header_checksum.end(), encoder); + + const auto data_checksum = checksum(&track_data[s * 512], &track_data[(s + 1) * 512]); + write_block(data_checksum.begin(), data_checksum.end(), encoder); + + // Write data. + write_block(&track_data[s * 512], &track_data[(s + 1) * 512], encoder); + } + + return std::make_shared(std::move(encoded_segment)); } long AmigaADF::get_file_offset_for_position(Track::Address address) {