forked from paulocode/picostation
-
Notifications
You must be signed in to change notification settings - Fork 0
/
subq.c
157 lines (140 loc) · 5.51 KB
/
subq.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
#include <stdio.h>
#include "hardware/gpio.h"
#include "hardware/pio.h"
#include "pico/stdlib.h"
#include "main.pio.h"
#include "utils.h"
#include "values.h"
#include "subq.h"
extern volatile uint subq_offset;
extern volatile uint subq_offset;
extern volatile int track;
extern volatile int current_logical_track;
extern volatile int sector;
extern volatile int sector_for_track_update;
extern volatile int subq_start;
extern volatile int mode;
extern volatile bool hasData;
extern volatile int num_logical_tracks;
extern bool *is_data_track;
extern int *logical_track_to_sector;
uint8_t tracksubq[12];
void printf_subq(uint8_t *subqdata) {
for (int i=0; i<12; i++) {
printf("%02X ", subqdata[i]);
}
}
static inline void send_subq(uint8_t *subqdata) {
subq_program_init(pio1, SUBQ_SM, subq_offset, SQSO, SQCK);
pio_sm_set_enabled(pio1, SUBQ_SM, true);
uint sub1 = (reverseBits(subqdata[0],8) << 24) |
(reverseBits(subqdata[1],8) << 16) |
(reverseBits(subqdata[2],8) << 8) |
(reverseBits(subqdata[3],8));
uint sub2 = (reverseBits(subqdata[4],8) << 24) |
(reverseBits(subqdata[5],8) << 16) |
(reverseBits(subqdata[6],8) << 8) |
(reverseBits(subqdata[7],8));
uint sub3 = (reverseBits(subqdata[8],8) << 24) |
(reverseBits(subqdata[9],8) << 16) |
(reverseBits(subqdata[10],8) << 8) |
(reverseBits(subqdata[11],8));
pio_sm_put_blocking(pio1, SUBQ_SM, reverseBits(sub1,32));
pio_sm_put_blocking(pio1, SUBQ_SM, reverseBits(sub2,32));
pio_sm_put_blocking(pio1, SUBQ_SM, reverseBits(sub3,32));
pio_sm_put_blocking(pio1, SCOR_SM, 1);
}
void start_subq() {
if (sector < 4500) {
int subq_entry = sector%(3+num_logical_tracks);
if (subq_entry == 0) {
tracksubq[0] = hasData ? 0x61 : 0x21;
tracksubq[1] = 0x00;
tracksubq[2] = 0xA0;
tracksubq[7] = 0x01;
tracksubq[8] = 0x20;
tracksubq[9] = 0x00;
} else if (subq_entry == 1) {
tracksubq[0] = hasData ? 0x61 : 0x21;
tracksubq[1] = 0x00;
tracksubq[2] = 0xA1;
tracksubq[7] = tobcd(num_logical_tracks);
tracksubq[8] = 0x00;
tracksubq[9] = 0x00;
} else if (subq_entry == 2) {
int sector_lead_out = logical_track_to_sector[num_logical_tracks+1] - 4500;
tracksubq[0] = hasData ? 0x61 : 0x21;
tracksubq[1] = 0x00;
tracksubq[2] = 0xA2;
tracksubq[7] = tobcd(sector_lead_out/75/60);
tracksubq[8] = tobcd((sector_lead_out/75) % 60);
tracksubq[9] = tobcd(sector_lead_out % 75);
} else if (subq_entry > 2) {
int sector_track;
int logical_track = subq_entry-2;
if (logical_track == 1) {
sector_track = 150;
} else {
sector_track = logical_track_to_sector[logical_track] - 4500;
}
tracksubq[0] = is_data_track[logical_track] ? 0x41 : 0x01;
tracksubq[1] = 0x00;
tracksubq[2] = tobcd(logical_track);
tracksubq[7] = tobcd(sector_track/75/60);
tracksubq[8] = tobcd((sector_track/75) % 60);
tracksubq[9] = tobcd(sector_track % 75);
}
tracksubq[3] = tobcd(sector/75/60);
tracksubq[4] = tobcd((sector/75) % 60);
tracksubq[5] = tobcd(sector % 75);
tracksubq[6] = 0x00;
tracksubq[10] = 0x00;
tracksubq[11] = 0x00;
send_subq(tracksubq);
} else {
int logical_track = num_logical_tracks+1; // in case seek overshoots past end of disc
for (int i=0; i<num_logical_tracks+2; i++) { // + 2 for lead in & lead out
if (logical_track_to_sector[i+1] > sector) {
logical_track = i;
break;
}
}
int sector_track = sector - logical_track_to_sector[logical_track];
int sector_abs = (sector - 4500);
int sector_track_after_pause;
if (logical_track == 1) {
sector_track_after_pause = sector_track - 150;
} else {
sector_track_after_pause = sector_track;
}
current_logical_track = logical_track;
tracksubq[0] = is_data_track[logical_track] ? 0x41 : 0x01;
if (logical_track == num_logical_tracks+1) {
tracksubq[1] = 0xAA;
} else {
tracksubq[1] = tobcd(logical_track);
}
if (sector_track < 150 && logical_track == 1) { // 2 sec pause track
tracksubq[2] = 0x00;
tracksubq[3] = 0x00; // min
tracksubq[4] = tobcd(1-(sector_track/75)); // sec (count down)
tracksubq[5] = tobcd(74 - (sector_track % 75)); // frame (count down)
} else {
tracksubq[2] = 0x01;
tracksubq[3] = tobcd(sector_track_after_pause/75/60); // min
tracksubq[4] = tobcd((sector_track_after_pause/75) % 60); // sec
tracksubq[5] = tobcd(sector_track_after_pause % 75); // frame
}
tracksubq[6] = 0x00;
tracksubq[7] = tobcd(sector_abs/75/60); // amin
tracksubq[8] = tobcd((sector_abs/75) % 60); // asec
tracksubq[9] = tobcd(sector_abs % 75); // aframe
tracksubq[10] = 0x00;
tracksubq[11] = ((sector % 2) == 0) ? 0x00 : 0x80;
if (sector % (50+num_logical_tracks) == 0) {
printf_subq(tracksubq);
printf("%d\n",sector);
}
send_subq(tracksubq);
}
}