MajenkoProjects/RTmFM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
d8ddc608aae0471048fc4fa03df81f1d661a21ef
RTmFM/firmware/Test/Test.ino

501 lines
12 KiB
C++
Raw Blame History

#include <CLI.h>
#include "datastream.h"
#define SECTOR_128 0
#define SECTOR_256 1
#define SECTOR_512 2
#define SECTOR_1024 3
static PIOProgram datastreamPgm(&datastream_program);
PIO pio;
int sm;
int offset;
#define POLY16 0x1021
#define POLY32 0xa00805
#define CQ16(V, C, P) C = crc16(V, C, P); queue(mfm_encode(V))
#define CQ32(V, C, P) C = crc32(V, C, P); queue(mfm_encode(V))
#define NUM_SECTORS 17
#define TRACK_SIZE (512 * NUM_SECTORS)
uint8_t *track_data;
#define OPT_HEADER_CRC16 0x00000001
#define OPT_HEADER_CRC32 0x00000002
#define OPT_HEADER_CRC_MASK 0xFFFFFFFC
#define OPT_DATA_CRC16 0x00000004
#define OPT_DATA_CRC32 0x00000008
#define OPT_DATA_CRC_MASK 0xFFFFFFF3
struct disk_format {
uint16_t cyls;
uint8_t heads;
uint8_t sectors;
uint8_t sector_size;
uint8_t track_pregap;
uint8_t track_postgap;
uint8_t header_postgap;
uint8_t data_postgap;
float data_rate;
uint32_t flags;
uint32_t header_poly;
uint32_t data_poly;
// Calculated data - not to be filled.
uint32_t tlen;
uint32_t slen;
uint32_t idx_ts;
uint32_t idx_period;
float clock_div;
};
struct disk_format RD54 = {
1225, // Cyl
15, // Heads
17, // Sectors,
SECTOR_512, // Bytes per sector
151, // Track pregap
151, // Track postgap
16, // Header postgap
50, // Data postgap
5000000, // Data Rate
OPT_HEADER_CRC16 | OPT_DATA_CRC32,
0x1021, // Header CRC polynomial
0xa00805, // Data CRC polynomial
};
struct disk_format *format;
void bindump(uint16_t v) {
for (int i = 0; i < 16; i++) {
v <<= 1;
}
}
uint16_t crc16(uint8_t val, uint16_t crc, uint16_t poly)
{
uint16_t xval = val;
int j;
crc = crc ^ (xval << 8);
for (j = 1; j <= 8; j++) {
if (crc & 0x8000) {
crc = (crc << 1) ^ poly;
} else {
crc = crc << 1;
}
}
return crc;
}
uint32_t crc32(uint8_t val, uint32_t crc, uint32_t poly)
{
int j;
crc = crc ^ (val << 24);
for (j = 1; j <= 8; j++) {
if (crc & 0x80000000) {
crc = (crc << 1) ^ poly;
} else {
crc = crc << 1;
}
}
return crc;
}
uint8_t last_bit = 0;
uint16_t mfm_encode_bit(uint8_t b) {
if (b == 0x80) {
last_bit = 1;
return 0b01;
}
if (last_bit == 0) {
return 0b10;
} else {
last_bit = 0;
return 0b00;
}
}
uint16_t mfm_encode(uint8_t b) {
uint16_t out = 0;
for (int i = 0; i < 8; i++) {
out <<= 2;
out |= mfm_encode_bit(b & 0x80);
b <<= 1;
}
return out;
}
static inline void queue(uint16_t val) {
while(pio_sm_is_tx_fifo_full(pio, sm));
pio->txf[sm] = val;
}
void sync() {
uint16_t sync = mfm_encode(0xA1);
sync &= 0b1111111111011111;
queue(sync);
}
void zero_pad() {
queue(mfm_encode(0x00));
}
void send_header(uint16_t cyl, uint8_t head, uint8_t sector, uint8_t size) {
zero_pad();
sync();
if (format->flags & OPT_HEADER_CRC16) {
uint16_t header_crc = 0xFFFF;
header_crc = crc16(0xA1, header_crc, format->header_poly);
CQ16(0xFE, header_crc, format->header_poly);
CQ16(cyl & 0xFF, header_crc, format->header_poly);
CQ16(((cyl >> 4) & 0xF0) | (head & 0x0F), header_crc, format->header_poly);
CQ16(sector, header_crc, format->header_poly);
CQ16(size, header_crc, format->header_poly);
queue(mfm_encode((header_crc >> 8) & 0xFF));
queue(mfm_encode(header_crc & 0xFF));
} else if (format->flags & OPT_HEADER_CRC32) {
uint32_t header_crc = 0xFFFFFFFF;
header_crc = crc32(0xA1, header_crc, format->header_poly);
CQ32(0xFE, header_crc, format->header_poly);
CQ32(cyl & 0xFF, header_crc, format->header_poly);
CQ32(((cyl >> 4) & 0xF0) | (head & 0x0F), header_crc, format->header_poly);
CQ32(sector, header_crc, format->header_poly);
CQ32(size, header_crc, format->header_poly);
queue(mfm_encode((header_crc >> 24) & 0xFF));
queue(mfm_encode((header_crc >> 16) & 0xFF));
queue(mfm_encode((header_crc >> 8) & 0xFF));
queue(mfm_encode(header_crc & 0xFF));
}
zero_pad();
}
void send_data(uint8_t *data, uint16_t len) {
zero_pad();
sync();
if (format->flags & OPT_DATA_CRC16) {
uint16_t data_crc = 0xFFFF;
data_crc = crc16(0xA1, data_crc, format->data_poly);
CQ16(0xFB, data_crc, format->data_poly);
for (int i = 0; i < len; i++) {
CQ16(data[i], data_crc, format->data_poly);
}
queue(mfm_encode((data_crc >> 8) & 0xFF));
queue(mfm_encode(data_crc & 0xFF));
} else if (format->flags & OPT_DATA_CRC32) {
uint32_t data_crc = 0xFFFFFFFF;
data_crc = crc32(0xA1, data_crc, format->data_poly);
CQ32(0xFB, data_crc, format->data_poly);
for (int i = 0; i < len; i++) {
CQ32(data[i], data_crc, format->data_poly);
}
queue(mfm_encode((data_crc >> 24) & 0xFF));
queue(mfm_encode((data_crc >> 16) & 0xFF));
queue(mfm_encode((data_crc >> 8) & 0xFF));
queue(mfm_encode(data_crc & 0xFF));
}
zero_pad();
}
void send_sector(uint16_t cyl, uint8_t head, uint8_t sector, uint8_t size, uint8_t *data, uint8_t pad) {
send_header(cyl, head, sector, size);
for (int i = 0; i < pad; i++) {
zero_pad();
}
uint16_t len = 0x80 << size;
send_data(data, len);
}
void second_cpu_thread() {
while (1) {
for (int i = 0; i < format->track_postgap; i++) {
zero_pad();
}
format->idx_period = micros() - format->idx_ts;
format->idx_ts = micros();
digitalWrite(13, LOW);
zero_pad();
digitalWrite(13, HIGH);
for (int i = 0; i < format->track_pregap; i++) {
zero_pad();
}
for (int sector = 0; sector < format->sectors; sector++) {
send_sector(0, 0, sector, format->sector_size, track_data + (sector * format->slen), format->header_postgap);
for (int i = 0; i < format->data_postgap; i++) {
zero_pad();
}
}
}
}
CLI_COMMAND(cli_status) {
uint32_t sector_bytes = 8 + format->slen + 6 + format->header_postgap + format->data_postgap + 4;
uint32_t total_clocks = ((sector_bytes * format->sectors) + format->track_pregap + format->track_postgap + 1) * 8;
if ((argc == 2) && (strcmp(argv[1], "ini") == 0)) {
dev->print("cyls="); dev->println(format->cyls);
dev->print("heads="); dev->println(format->heads);
dev->print("sectors="); dev->println(format->sectors);
dev->print("sector_size="); dev->println(0x80 << format->sector_size);
dev->print("track_pregap="); dev->println(format->track_pregap);
dev->print("track_postgap="); dev->println(format->track_postgap);
dev->print("header_postgap="); dev->println(format->header_postgap);
dev->print("data_postgap="); dev->println(format->data_postgap);
dev->print("data_rate="); dev->println(format->data_rate);
dev->print("header_crc="); dev->println((format->flags & OPT_HEADER_CRC16) ? "16" : (format->flags & OPT_HEADER_CRC32) ? "32" : "ERROR");
dev->print("data_crc="); dev->println((format->flags & OPT_DATA_CRC16) ? "16" : (format->flags & OPT_DATA_CRC32) ? "32" : "ERROR");
dev->print("header_poly="); dev->println(format->header_poly, HEX);
dev->print("data_poly="); dev->println(format->data_poly, HEX);
return 0;
}
float rpm = (format->data_rate / total_clocks) * 60.0;
dev->print("Cylinders: ");
dev->println(format->cyls);
dev->print("Heads: ");
dev->println(format->heads);
dev->print("Sectors: ");
dev->println(format->sectors);
dev->print("Sector Size: ");
dev->print(0x80 << format->sector_size);
dev->println(" bytes");
dev->println();
dev->print("Header CRC Bits: ");
dev->println((format->flags & OPT_HEADER_CRC16) ? "16" : (format->flags & OPT_HEADER_CRC32) ? "32" : "ERROR");
dev->print("Data CRC Bits: ");
dev->println((format->flags & OPT_DATA_CRC16) ? "16" : (format->flags & OPT_DATA_CRC32) ? "32" : "ERROR");
dev->print("Header CRC Polynomial: ");
dev->println(format->header_poly, HEX);
dev->print("Data CRC Polynomial: ");
dev->println(format->data_poly, HEX);
dev->println();
dev->print("Track Pregap: ");
dev->print(format->track_pregap);
dev->println(" bytes");
dev->print("Track Postgap: ");
dev->print(format->track_postgap);
dev->println(" bytes");
dev->print("Header Postgap: ");
dev->print(format->header_postgap);
dev->println(" bytes");
dev->print("Data Postgap: ");
dev->print(format->data_postgap);
dev->println(" bytes");
dev->println();
dev->print("Total clocks per track: ");
dev->println(total_clocks);
dev->print("Calculated RPM: ");
dev->println(rpm);
dev->print("Actual RPM: ");
float p = format->idx_period / 1000000.0;
float f = 1.0 / p;
float r = f * 60;
dev->println(r);
dev->print("Requested Data Rate: ");
dev->print(format->data_rate);
dev->println("MHz");
dev->print("Actual Data Rate: ");
dev->print(F_CPU / format->clock_div / 20.0);
dev->println("MHz");
dev->print("Clock divider: ");
dev->println(format->clock_div);
return 0;
}
CLI_COMMAND(cli_set) {
if (argc != 3) {
dev->println("Usage: set <item> <value>");
dev->println("Possible items:");
dev->println(" track_pregap");
dev->println(" track_posthap");
dev->println(" header_postgap");
dev->println(" data_postgap");
dev->println(" data_rate");
dev->println(" header_crc");
dev->println(" data_crc");
dev->println(" header_poly");
dev->println(" data_poly");
return 10;
}
if (strcmp(argv[1], "track_pregap") == 0) {
format->track_pregap = strtoul(argv[2], NULL, 10);
return 0;
}
if (strcmp(argv[1], "track_postgap") == 0) {
format->track_postgap = strtoul(argv[2], NULL, 10);
return 0;
}
if (strcmp(argv[1], "header_postgap") == 0) {
format->header_postgap = strtoul(argv[2], NULL, 10);
return 0;
}
if (strcmp(argv[1], "data_postgap") == 0) {
format->data_postgap = strtoul(argv[2], NULL, 10);
return 0;
}
if (strcmp(argv[1], "data_rate") == 0) {
float r = strtof(argv[2], NULL);
float cd = F_CPU / r / 20.0;
if (cd < 1) {
dev->println("Data rate too high for the CPU clock");
return 10;
}
format->data_rate = r;
format->clock_div = cd;
pio_sm_set_clkdiv(pio, sm, format->clock_div);
return 0;
}
if (strcmp(argv[1], "header_crc") == 0) {
int c = strtol(argv[2], NULL, 10);
if (c == 16) {
format->flags &= OPT_HEADER_CRC_MASK;
format->flags |= OPT_HEADER_CRC16;
return 0;
}
if (c == 32) {
format->flags &= OPT_HEADER_CRC_MASK;
format->flags |= OPT_HEADER_CRC32;
return 0;
}
dev->println("Error: header_crc must be 16 or 32");
return 10;
}
if (strcmp(argv[1], "data_crc") == 0) {
int c = strtol(argv[2], NULL, 10);
if (c == 16) {
format->flags &= OPT_DATA_CRC_MASK;
format->flags |= OPT_DATA_CRC16;
return 0;
}
if (c == 32) {
format->flags &= OPT_DATA_CRC_MASK;
format->flags |= OPT_DATA_CRC32;
return 0;
}
dev->println("Error: data_crc must be 16 or 32");
return 10;
}
if (strcmp(argv[1], "header_poly") == 0) {
format->header_poly = strtoul(argv[2], NULL, 16);
return 0;
}
if (strcmp(argv[1], "data_poly") == 0) {
format->data_poly = strtoul(argv[2], NULL, 16);
return 0;
}
dev->println("Possible items:");
dev->println(" track_pregap");
dev->println(" track_posthap");
dev->println(" header_postgap");
dev->println(" data_postgap");
return 10;
}
void setup() {
datastreamPgm.prepare(&pio, &sm, &offset);
datastream_program_init(pio, sm, offset);
pio_sm_set_enabled(pio, sm, true);
pio->txf[sm] = mfm_encode(0x00);
pinMode(13, OUTPUT);
digitalWrite(13, HIGH);
Serial.begin(115200);
format = &RD54;
format->slen = 0x80 << format->sector_size;
format->tlen = format->slen * format->sectors;
track_data = (uint8_t *)malloc(format->tlen);
for (int i = 0; i < format->tlen; i++) {
track_data[i] = rand();
}
format->clock_div = F_CPU / format->data_rate / 20.0;
pio_sm_set_clkdiv(pio, sm, format->clock_div);
multicore_launch_core1(second_cpu_thread);
Serial.begin(115200);
CLI.setDefaultPrompt("RTmFM> ");
CLI.addClient(Serial);
CLI.addCommand("status", cli_status);
CLI.addCommand("set", cli_set);
}
void loop() {
CLI.process();
}
Reference in New Issue View Git Blame Copy Permalink