1 /*
2 *************************************************************
3 ** P64 reference implementation by Benjamin 'BeRo' Rosseaux *
4 *************************************************************
5 **
6 ** Copyright (c) 2011-2012, Benjamin Rosseaux
7 **
8 ** This software is provided 'as-is', without any express or implied
9 ** warranty. In no event will the authors be held liable for any damages
10 ** arising from the use of this software.
11 **
12 ** Permission is granted to anyone to use this software for any purpose,
13 ** including commercial applications, and to alter it and redistribute it
14 ** freely, subject to the following restrictions:
15 **
16 **    1. The origin of this software must not be misrepresented; you must not
17 **    claim that you wrote the original software. If you use this software
18 **    in a product, an acknowledgment in the product documentation would be
19 **    appreciated but is not required.
20 **
21 **    2. Altered source versions must be plainly marked as such, and must not be
22 **    misrepresented as being the original software.
23 **
24 **    3. This notice may not be removed or altered from any source
25 **   distribution.
26 **
27 */
28 
29 #include "p64.h"
30 
P64CRC32(p64_uint8_t * Data,p64_uint32_t Len)31 static p64_uint32_t P64CRC32(p64_uint8_t* Data, p64_uint32_t Len) {
32 
33     const p64_uint32_t CRC32Table[16] = {
34         0x00000000UL, 0x1db71064UL, 0x3b6e20c8UL, 0x26d930acUL,
35         0x76dc4190UL, 0x6b6b51f4UL, 0x4db26158UL, 0x5005713cUL,
36         0xedb88320UL, 0xf00f9344UL, 0xd6d6a3e8UL, 0xcb61b38cUL,
37         0x9b64c2b0UL, 0x86d3d2d4UL, 0xa00ae278UL, 0xbdbdf21cUL
38     };
39 
40     p64_uint32_t value, pos;
41 
42     if(!Len) {
43         return 0;
44     }
45 
46     for(value = 0xffffffffUL, pos = 0; pos < Len; pos++) {
47         value ^= Data[pos];
48         value = CRC32Table[value & 0xfUL] ^(value >> 4);
49         value = CRC32Table[value & 0xfUL] ^(value >> 4);
50     }
51 
52     return value ^ 0xffffffffUL;
53 }
54 
55 typedef p64_uint32_t* PP64RangeCoderProbabilities;
56 
57 typedef struct {
58     p64_uint8_t* Buffer;
59     p64_uint32_t BufferSize;
60     p64_uint32_t BufferPosition;
61     p64_uint32_t RangeCode;
62     p64_uint32_t RangeLow;
63     p64_uint32_t RangeHigh;
64     p64_uint32_t RangeMiddle;
65 } TP64RangeCoder;
66 
67 typedef TP64RangeCoder* PP64RangeCoder;
68 
P64RangeCoderProbabilitiesAllocate(p64_uint32_t Count)69 static PP64RangeCoderProbabilities P64RangeCoderProbabilitiesAllocate(p64_uint32_t Count) {
70     return p64_malloc(Count * sizeof(p64_uint32_t));
71 }
72 
P64RangeCoderProbabilitiesFree(PP64RangeCoderProbabilities Probabilities)73 static void P64RangeCoderProbabilitiesFree(PP64RangeCoderProbabilities Probabilities) {
74     p64_free(Probabilities);
75 }
76 
P64RangeCoderProbabilitiesReset(PP64RangeCoderProbabilities Probabilities,p64_uint32_t Count)77 static void P64RangeCoderProbabilitiesReset(PP64RangeCoderProbabilities Probabilities, p64_uint32_t Count) {
78     p64_uint32_t Index;
79     for(Index = 0; Index < Count; Index++) {
80         Probabilities[Index] = 2048;
81     }
82 }
83 
P64RangeCoderRead(PP64RangeCoder Instance)84 static p64_uint8_t P64RangeCoderRead(PP64RangeCoder Instance) {
85     if(Instance->BufferPosition < Instance->BufferSize) {
86         return Instance->Buffer[Instance->BufferPosition++];
87     }
88     return 0;
89 }
90 
P64RangeCoderWrite(PP64RangeCoder Instance,p64_uint8_t Value)91 static void P64RangeCoderWrite(PP64RangeCoder Instance, p64_uint8_t Value) {
92     if(Instance->BufferPosition >= Instance->BufferSize) {
93         if(Instance->BufferSize < 16) {
94             Instance->BufferSize = 16;
95         }
96         while(Instance->BufferPosition >= Instance->BufferSize) {
97             Instance->BufferSize += Instance->BufferSize;
98         }
99         if(Instance->Buffer) {
100             Instance->Buffer = p64_realloc(Instance->Buffer, Instance->BufferSize);
101         } else {
102             Instance->Buffer = p64_malloc(Instance->BufferSize);
103         }
104     }
105     Instance->Buffer[Instance->BufferPosition++] = Value;
106 }
107 
P64RangeCoderInit(PP64RangeCoder Instance)108 static void P64RangeCoderInit(PP64RangeCoder Instance) {
109     Instance->RangeCode = 0;
110     Instance->RangeLow = 0;
111     Instance->RangeHigh = 0xffffffffUL;
112 }
113 
P64RangeCoderStart(PP64RangeCoder Instance)114 static void P64RangeCoderStart(PP64RangeCoder Instance) {
115     p64_uint32_t Counter;
116     for(Counter = 0; Counter < 4; Counter++) {
117         Instance->RangeCode = (Instance->RangeCode << 8) | P64RangeCoderRead(Instance);
118     }
119 }
120 
P64RangeCoderFlush(PP64RangeCoder Instance)121 static void P64RangeCoderFlush(PP64RangeCoder Instance) {
122     p64_uint32_t Counter;
123     for(Counter = 0; Counter < 4; Counter++) {
124         P64RangeCoderWrite(Instance, (p64_uint8_t)(Instance->RangeHigh >> 24));
125         Instance->RangeHigh <<= 8;
126     }
127 }
128 
P64RangeCoderEncodeNormalize(PP64RangeCoder Instance)129 static void P64RangeCoderEncodeNormalize(PP64RangeCoder Instance) {
130     while(!((Instance->RangeLow ^ Instance->RangeHigh) & 0xff000000UL)) {
131         P64RangeCoderWrite(Instance, (p64_uint8_t)(Instance->RangeHigh >> 24));
132         Instance->RangeLow <<= 8;
133         Instance->RangeHigh = (Instance->RangeHigh << 8) | 0xffUL;
134     }
135 }
136 
P64RangeCoderEncodeBit(PP64RangeCoder Instance,p64_uint32_t * Probability,p64_uint32_t Shift,p64_uint32_t BitValue)137 static p64_uint32_t P64RangeCoderEncodeBit(PP64RangeCoder Instance, p64_uint32_t* Probability, p64_uint32_t Shift, p64_uint32_t BitValue) {
138     Instance->RangeMiddle = Instance->RangeLow + ((p64_uint32_t)((p64_uint32_t)(Instance->RangeHigh - Instance->RangeLow) >> 12) * (*Probability));
139     if(BitValue) {
140         *Probability += (p64_uint32_t)((0xfffUL - *Probability) >> Shift);
141         Instance->RangeHigh = Instance->RangeMiddle;
142     } else {
143         *Probability -= *Probability >> Shift;
144         Instance->RangeLow = Instance->RangeMiddle + 1;
145     }
146     P64RangeCoderEncodeNormalize(Instance);
147     return BitValue;
148 }
149 
150 
151 /* unused right now (2017-04-17, compyx) */
152 #if 0
153 static p64_uint32_t P64RangeCoderEncodeBitWithoutProbability(PP64RangeCoder Instance, p64_uint32_t BitValue) {
154     Instance->RangeMiddle = Instance->RangeLow + ((Instance->RangeHigh - Instance->RangeLow) >> 1);
155     if(BitValue) {
156         Instance->RangeHigh = Instance->RangeMiddle;
157     } else {
158         Instance->RangeLow = Instance->RangeMiddle + 1;
159     }
160     P64RangeCoderEncodeNormalize(Instance);
161     return BitValue;
162 }
163 #endif
164 
P64RangeCoderDecodeNormalize(PP64RangeCoder Instance)165 static void P64RangeCoderDecodeNormalize(PP64RangeCoder Instance) {
166     while(!((Instance->RangeLow ^ Instance->RangeHigh) & 0xff000000UL)) {
167         Instance->RangeLow <<= 8;
168         Instance->RangeHigh = (Instance->RangeHigh << 8) | 0xffUL;
169         Instance->RangeCode = (Instance->RangeCode << 8) | P64RangeCoderRead(Instance);
170     }
171 }
172 
P64RangeCoderDecodeBit(PP64RangeCoder Instance,p64_uint32_t * Probability,p64_uint32_t Shift)173 static p64_uint32_t P64RangeCoderDecodeBit(PP64RangeCoder Instance, p64_uint32_t *Probability, p64_uint32_t Shift) {
174     p64_uint32_t bit;
175     Instance->RangeMiddle = Instance->RangeLow + ((p64_uint32_t)((p64_uint32_t)(Instance->RangeHigh - Instance->RangeLow) >> 12) * (*Probability));
176     if(Instance->RangeCode <= Instance->RangeMiddle) {
177         *Probability += (p64_uint32_t)((0xfffUL - *Probability) >> Shift);
178         Instance->RangeHigh = Instance->RangeMiddle;
179         bit = 1;
180     } else {
181         *Probability -= *Probability >> Shift;
182         Instance->RangeLow = Instance->RangeMiddle + 1;
183         bit = 0;
184     }
185     P64RangeCoderDecodeNormalize(Instance);
186     return bit;
187 }
188 
189 
190 /* unused right now (2017-04-17, compyx) */
191 #if 0
192 static p64_uint32_t P64RangeCoderDecodeBitWithoutProbability(PP64RangeCoder Instance) {
193     p64_uint32_t bit;
194     Instance->RangeMiddle = Instance->RangeLow + ((Instance->RangeHigh - Instance->RangeLow) >> 1);
195     if(Instance->RangeCode <= Instance->RangeMiddle) {
196         Instance->RangeHigh = Instance->RangeMiddle;
197         bit = 1;
198     } else {
199         Instance->RangeLow = Instance->RangeMiddle + 1;
200         bit = 0;
201     }
202     P64RangeCoderDecodeNormalize(Instance);
203     return bit;
204 }
205 #endif
206 
207 /* unused right now (2017-04-17, compyx) */
208 #if 0
209 static p64_uint32_t P64RangeCoderEncodeDirectBits(PP64RangeCoder Instance, p64_uint32_t Bits, p64_uint32_t Value) {
210     while(Bits--) {
211         P64RangeCoderEncodeBitWithoutProbability(Instance, (Value >> Bits) & 1);
212     }
213     return Value;
214 }
215 #endif
216 
217 /* unused right now (2017-04-17, compyx) */
218 #if 0
219 static p64_uint32_t P64RangeCoderDecodeDirectBits(PP64RangeCoder Instance, p64_uint32_t Bits) {
220     p64_uint32_t Value = 0;
221     while(Bits--) {
222         Value += Value + P64RangeCoderDecodeBitWithoutProbability(Instance);
223     }
224     return Value;
225 }
226 #endif
227 
P64MemoryStreamCreate(PP64MemoryStream Instance)228 void P64MemoryStreamCreate(PP64MemoryStream Instance) {
229     memset(Instance, 0, sizeof(TP64MemoryStream));
230 }
231 
P64MemoryStreamDestroy(PP64MemoryStream Instance)232 void P64MemoryStreamDestroy(PP64MemoryStream Instance) {
233     if(Instance->Data) {
234         p64_free(Instance->Data);
235     }
236     memset(Instance, 0, sizeof(TP64MemoryStream));
237 }
238 
P64MemoryStreamClear(PP64MemoryStream Instance)239 void P64MemoryStreamClear(PP64MemoryStream Instance) {
240     if(Instance->Data) {
241         p64_free(Instance->Data);
242     }
243     memset(Instance, 0, sizeof(TP64MemoryStream));
244 }
245 
P64MemoryStreamSeek(PP64MemoryStream Instance,p64_uint32_t Position)246 p64_uint32_t P64MemoryStreamSeek(PP64MemoryStream Instance, p64_uint32_t Position) {
247     if(Position < Instance->Size) {
248         Instance->Position = Position;
249     }
250     return Instance->Position;
251 }
252 
P64MemoryStreamRead(PP64MemoryStream Instance,p64_uint8_t * Data,p64_uint32_t Count)253 p64_uint32_t P64MemoryStreamRead(PP64MemoryStream Instance, p64_uint8_t* Data, p64_uint32_t Count) {
254     p64_uint32_t ToDo = 0;
255     if((Count > 0) && (Instance->Position < Instance->Size)) {
256         ToDo = Instance->Size - Instance->Position;
257         if(ToDo > Count) {
258             ToDo = Count;
259         }
260         memmove(Data, Instance->Data + Instance->Position, ToDo);
261         Instance->Position += ToDo;
262     }
263     return ToDo;
264 }
265 
P64MemoryStreamWrite(PP64MemoryStream Instance,p64_uint8_t * Data,p64_uint32_t Count)266 p64_uint32_t P64MemoryStreamWrite(PP64MemoryStream Instance, p64_uint8_t* Data, p64_uint32_t Count) {
267     if(Count) {
268         if((Instance->Position + Count) >= Instance->Allocated) {
269             if(Instance->Allocated < 16) {
270                 Instance->Allocated = 16;
271             }
272             while((Instance->Position + Count) >= Instance->Allocated) {
273                 Instance->Allocated += Instance->Allocated;
274             }
275             if(Instance->Data) {
276                 Instance->Data = p64_realloc(Instance->Data, Instance->Allocated);
277             } else {
278                 Instance->Data = p64_malloc(Instance->Allocated);
279             }
280         }
281         memmove(Instance->Data + Instance->Position, Data, Count);
282         Instance->Position += Count;
283         if(Instance->Size < Instance->Position) {
284             Instance->Size = Instance->Position;
285         }
286         return Count;
287     } else {
288         return 0;
289     }
290 }
291 
P64MemoryStreamReadByte(PP64MemoryStream Instance,p64_uint8_t * Data)292 p64_int32_t P64MemoryStreamReadByte(PP64MemoryStream Instance, p64_uint8_t* Data) {
293     return P64MemoryStreamRead(Instance, Data, sizeof(p64_uint8_t)) ? 1 : 0;
294 }
295 
P64MemoryStreamReadWord(PP64MemoryStream Instance,p64_uint16_t * Data)296 p64_int32_t P64MemoryStreamReadWord(PP64MemoryStream Instance, p64_uint16_t* Data) {
297     p64_uint8_t b[2];
298     if(!P64MemoryStreamReadByte(Instance, &b[0])) {
299         return 0;
300     }
301     if(!P64MemoryStreamReadByte(Instance, &b[1])) {
302         return 0;
303     }
304     *Data = ((p64_uint16_t)b[0]) | (((p64_uint16_t)b[1]) << 8);
305     return 1;
306 }
307 
P64MemoryStreamReadDWord(PP64MemoryStream Instance,p64_uint32_t * Data)308 p64_int32_t P64MemoryStreamReadDWord(PP64MemoryStream Instance, p64_uint32_t* Data) {
309     p64_uint16_t w[2];
310     if(!P64MemoryStreamReadWord(Instance, &w[0])) {
311         return 0;
312     }
313     if(!P64MemoryStreamReadWord(Instance, &w[1])) {
314         return 0;
315     }
316     *Data = ((p64_uint32_t)w[0]) | (((p64_uint32_t)w[1]) << 16);
317     return 1;
318 }
319 
P64MemoryStreamWriteByte(PP64MemoryStream Instance,p64_uint8_t * Data)320 p64_int32_t P64MemoryStreamWriteByte(PP64MemoryStream Instance, p64_uint8_t* Data) {
321     return P64MemoryStreamWrite(Instance, Data, sizeof(p64_uint8_t)) ? 1 : 0;
322 }
323 
P64MemoryStreamWriteWord(PP64MemoryStream Instance,p64_uint16_t * Data)324 p64_int32_t P64MemoryStreamWriteWord(PP64MemoryStream Instance, p64_uint16_t* Data) {
325     p64_uint8_t b[2];
326     b[0] = (p64_uint8_t)(*Data & 0xffUL);
327     b[1] = (p64_uint8_t)((*Data >> 8) & 0xffUL);
328     if(!P64MemoryStreamWriteByte(Instance, &b[0])) {
329         return 0;
330     }
331     if(!P64MemoryStreamWriteByte(Instance, &b[1])) {
332         return 0;
333     }
334     return 1;
335 }
336 
P64MemoryStreamWriteDWord(PP64MemoryStream Instance,p64_uint32_t * Data)337 p64_int32_t P64MemoryStreamWriteDWord(PP64MemoryStream Instance, p64_uint32_t* Data) {
338     p64_uint16_t w[2];
339     w[0] = (p64_uint16_t)(*Data & 0xffffUL);
340     w[1] = (p64_uint16_t)((*Data >> 16) & 0xffffUL);
341     if(!P64MemoryStreamWriteWord(Instance, &w[0])) {
342         return 0;
343     }
344     if(!P64MemoryStreamWriteWord(Instance, &w[1])) {
345         return 0;
346     }
347     return 1;
348 }
349 
P64MemoryStreamAssign(PP64MemoryStream Instance,PP64MemoryStream FromInstance)350 p64_uint32_t P64MemoryStreamAssign(PP64MemoryStream Instance, PP64MemoryStream FromInstance) {
351     if(Instance->Data) {
352         p64_free(Instance->Data);
353     }
354     memset(Instance, 0, sizeof(TP64MemoryStream));
355     Instance->Data = p64_malloc(FromInstance->Allocated);
356     Instance->Size = FromInstance->Size;
357     Instance->Allocated = FromInstance->Allocated;
358     Instance->Position = 0;
359     if(Instance->Size) {
360         memmove(Instance->Data, FromInstance->Data, Instance->Size);
361     }
362     return Instance->Size;
363 }
364 
P64MemoryStreamAppend(PP64MemoryStream Instance,PP64MemoryStream FromInstance)365 p64_uint32_t P64MemoryStreamAppend(PP64MemoryStream Instance, PP64MemoryStream FromInstance) {
366     if(FromInstance->Size) {
367         FromInstance->Position = FromInstance->Size;
368         return P64MemoryStreamWrite(Instance, FromInstance->Data, FromInstance->Size);
369     }
370     return 0;
371 }
372 
P64MemoryStreamAppendFrom(PP64MemoryStream Instance,PP64MemoryStream FromInstance)373 p64_uint32_t P64MemoryStreamAppendFrom(PP64MemoryStream Instance, PP64MemoryStream FromInstance) {
374     if((FromInstance->Size > 0) && (FromInstance->Position < FromInstance->Size)) {
375         if(P64MemoryStreamWrite(Instance, FromInstance->Data + FromInstance->Position, FromInstance->Size - FromInstance->Position)) {
376             FromInstance->Position = FromInstance->Size;
377             return 1;
378         }
379         FromInstance->Position = FromInstance->Size;
380     }
381     return 0;
382 }
383 
P64MemoryStreamAppendFromCount(PP64MemoryStream Instance,PP64MemoryStream FromInstance,p64_uint32_t Count)384 p64_uint32_t P64MemoryStreamAppendFromCount(PP64MemoryStream Instance, PP64MemoryStream FromInstance, p64_uint32_t Count) {
385     p64_uint32_t ToDo = 0;
386     if((Count > 0) && (FromInstance->Position < FromInstance->Size)) {
387         ToDo = FromInstance->Size - FromInstance->Position;
388         if(ToDo > Count) {
389             ToDo = Count;
390         }
391         if(ToDo > 0) {
392             ToDo = P64MemoryStreamWrite(Instance, FromInstance->Data + FromInstance->Position, ToDo);
393             FromInstance->Position += ToDo;
394         }
395     }
396     return ToDo;
397 }
398 
P64PulseStreamCreate(PP64PulseStream Instance)399 void P64PulseStreamCreate(PP64PulseStream Instance) {
400     memset(Instance, 0, sizeof(TP64PulseStream));
401     Instance->Pulses = 0;
402     Instance->PulsesAllocated = 0;
403     Instance->PulsesCount = 0;
404     Instance->UsedFirst = -1;
405     Instance->UsedLast = -1;
406     Instance->FreeList = -1;
407     Instance->CurrentIndex = -1;
408 }
409 
P64PulseStreamDestroy(PP64PulseStream Instance)410 void P64PulseStreamDestroy(PP64PulseStream Instance) {
411     P64PulseStreamClear(Instance);
412     memset(Instance, 0, sizeof(TP64PulseStream));
413 }
414 
P64PulseStreamClear(PP64PulseStream Instance)415 void P64PulseStreamClear(PP64PulseStream Instance) {
416     if(Instance->Pulses) {
417         p64_free(Instance->Pulses);
418     }
419     Instance->Pulses = 0;
420     Instance->PulsesAllocated = 0;
421     Instance->PulsesCount = 0;
422     Instance->UsedFirst = -1;
423     Instance->UsedLast = -1;
424     Instance->FreeList = -1;
425     Instance->CurrentIndex = -1;
426 }
427 
P64PulseStreamAllocatePulse(PP64PulseStream Instance)428 p64_int32_t P64PulseStreamAllocatePulse(PP64PulseStream Instance) {
429     p64_int32_t Index;
430     if(Instance->FreeList < 0) {
431         if(Instance->PulsesCount >= Instance->PulsesAllocated) {
432             if(Instance->PulsesAllocated < 16) {
433                 Instance->PulsesAllocated = 16;
434             }
435             while(Instance->PulsesCount >= Instance->PulsesAllocated) {
436                 Instance->PulsesAllocated += Instance->PulsesAllocated;
437             }
438             if(Instance->Pulses) {
439                 Instance->Pulses = p64_realloc(Instance->Pulses, Instance->PulsesAllocated * sizeof(TP64Pulse));
440             } else {
441                 Instance->Pulses = p64_malloc(Instance->PulsesAllocated * sizeof(TP64Pulse));
442             }
443         }
444         Index = (p64_int32_t)(Instance->PulsesCount++);
445     } else {
446         Index = Instance->FreeList;
447         Instance->FreeList = Instance->Pulses[Index].Next;
448     }
449     Instance->Pulses[Index].Previous = -1;
450     Instance->Pulses[Index].Next = -1;
451     Instance->Pulses[Index].Position = 0;
452     Instance->Pulses[Index].Strength = 0;
453     return Index;
454 }
455 
P64PulseStreamFreePulse(PP64PulseStream Instance,p64_int32_t Index)456 void P64PulseStreamFreePulse(PP64PulseStream Instance, p64_int32_t Index) {
457     if(Instance->CurrentIndex == Index) {
458         Instance->CurrentIndex = Instance->Pulses[Index].Next;
459     }
460     if(Instance->Pulses[Index].Previous < 0) {
461         Instance->UsedFirst = Instance->Pulses[Index].Next;
462     } else {
463         Instance->Pulses[Instance->Pulses[Index].Previous].Next = Instance->Pulses[Index].Next;
464     }
465     if(Instance->Pulses[Index].Next < 0) {
466         Instance->UsedLast = Instance->Pulses[Index].Previous;
467     } else {
468         Instance->Pulses[Instance->Pulses[Index].Next].Previous = Instance->Pulses[Index].Previous;
469     }
470     Instance->Pulses[Index].Previous = -1;
471     Instance->Pulses[Index].Next = Instance->FreeList;
472     Instance->FreeList = Index;
473 }
474 
P64PulseStreamAddPulse(PP64PulseStream Instance,p64_uint32_t Position,p64_uint32_t Strength)475 void P64PulseStreamAddPulse(PP64PulseStream Instance, p64_uint32_t Position, p64_uint32_t Strength) {
476     p64_int32_t Current, Index;
477     while(Position >= P64PulseSamplesPerRotation) {
478         Position -= P64PulseSamplesPerRotation;
479     }
480     Current = Instance->CurrentIndex;
481     if((Instance->UsedLast >= 0) && (Instance->Pulses[Instance->UsedLast].Position < Position)) {
482         Current = -1;
483     } else {
484         if((Current < 0) || ((Current != Instance->UsedFirst) && ((Instance->Pulses[Current].Previous >= 0) && (Instance->Pulses[Instance->Pulses[Current].Previous].Position >= Position)))) {
485             Current = Instance->UsedFirst;
486         }
487         while((Current >= 0) && (Instance->Pulses[Current].Position < Position)) {
488             Current = Instance->Pulses[Current].Next;
489         }
490     }
491     if(Current < 0) {
492         Index = P64PulseStreamAllocatePulse(Instance);
493         if(Instance->UsedLast < 0) {
494             Instance->UsedFirst = Index;
495         } else {
496             Instance->Pulses[Instance->UsedLast].Next = Index;
497             Instance->Pulses[Index].Previous = Instance->UsedLast;
498         }
499         Instance->UsedLast = Index;
500     } else {
501         if(Instance->Pulses[Current].Position == Position) {
502             Index = Current;
503         } else {
504             Index = P64PulseStreamAllocatePulse(Instance);
505             Instance->Pulses[Index].Previous = Instance->Pulses[Current].Previous;
506             Instance->Pulses[Index].Next = Current;
507             Instance->Pulses[Current].Previous = Index;
508             if(Instance->Pulses[Index].Previous < 0) {
509                 Instance->UsedFirst = Index;
510             } else {
511                 Instance->Pulses[Instance->Pulses[Index].Previous].Next = Index;
512             }
513         }
514     }
515     Instance->Pulses[Index].Position = Position;
516     Instance->Pulses[Index].Strength = Strength;
517     Instance->CurrentIndex = Index;
518 }
519 
P64PulseStreamRemovePulses(PP64PulseStream Instance,p64_uint32_t Position,p64_uint32_t Count)520 void P64PulseStreamRemovePulses(PP64PulseStream Instance, p64_uint32_t Position, p64_uint32_t Count) {
521     p64_uint32_t ToDo;
522     p64_int32_t Current, Next;
523     while(Position >= P64PulseSamplesPerRotation) {
524         Position -= P64PulseSamplesPerRotation;
525     }
526     while(Count) {
527         ToDo = ((Position + Count) > P64PulseSamplesPerRotation) ? (P64PulseSamplesPerRotation - Position) : Count;
528         Current = Instance->CurrentIndex;
529         if((Current < 0) || ((Current != Instance->UsedFirst) && ((Instance->Pulses[Current].Previous >= 0) && (Instance->Pulses[Instance->Pulses[Current].Previous].Position >= Position)))) {
530             Current = Instance->UsedFirst;
531         }
532         while((Current >= 0) && (Instance->Pulses[Current].Position < Position)) {
533             Current = Instance->Pulses[Current].Next;
534         }
535         while((Current >= 0) && ((Instance->Pulses[Current].Position >= Position) && (Instance->Pulses[Current].Position < (Position + ToDo)))) {
536             Next = Instance->Pulses[Current].Next;
537             P64PulseStreamFreePulse(Instance, Current);
538             Current = Next;
539         }
540         Position += ToDo;
541         Count -= ToDo;
542     }
543 }
544 
P64PulseStreamRemovePulse(PP64PulseStream Instance,p64_uint32_t Position)545 void P64PulseStreamRemovePulse(PP64PulseStream Instance, p64_uint32_t Position) {
546     p64_int32_t Current;
547     while(Position >= P64PulseSamplesPerRotation) {
548         Position -= P64PulseSamplesPerRotation;
549     }
550     Current = Instance->CurrentIndex;
551     if((Current < 0) || ((Current != Instance->UsedFirst) && ((Instance->Pulses[Current].Previous >= 0) && (Instance->Pulses[Instance->Pulses[Current].Previous].Position >= Position)))) {
552         Current = Instance->UsedFirst;
553     }
554     while((Current >= 0) && (Instance->Pulses[Current].Position < Position)) {
555         Current = Instance->Pulses[Current].Next;
556     }
557     if((Current >= 0) && (Instance->Pulses[Current].Position == Position)) {
558         P64PulseStreamFreePulse(Instance, Current);
559     }
560 }
561 
P64PulseStreamDeltaPositionToNextPulse(PP64PulseStream Instance,p64_uint32_t Position)562 p64_uint32_t P64PulseStreamDeltaPositionToNextPulse(PP64PulseStream Instance, p64_uint32_t Position) {
563     p64_int32_t Current;
564     while(Position >= P64PulseSamplesPerRotation) {
565         Position -= P64PulseSamplesPerRotation;
566     }
567     Current = Instance->CurrentIndex;
568     if((Current < 0) || ((Current != Instance->UsedFirst) && ((Instance->Pulses[Current].Previous >= 0) && (Instance->Pulses[Instance->Pulses[Current].Previous].Position >= Position)))) {
569         Current = Instance->UsedFirst;
570     }
571     while((Current >= 0) && (Instance->Pulses[Current].Position < Position)) {
572         Current = Instance->Pulses[Current].Next;
573     }
574     if(Current < 0) {
575         if(Instance->UsedFirst < 0) {
576             return P64PulseSamplesPerRotation - Position;
577         } else {
578             return (P64PulseSamplesPerRotation + Instance->Pulses[Instance->UsedFirst].Position) - Position;
579         }
580     } else {
581         Instance->CurrentIndex = Current;
582         return Instance->Pulses[Current].Position - Position;
583     }
584 }
585 
P64PulseStreamGetNextPulse(PP64PulseStream Instance,p64_uint32_t Position)586 p64_uint32_t P64PulseStreamGetNextPulse(PP64PulseStream Instance, p64_uint32_t Position) {
587     p64_int32_t Current;
588     while(Position >= P64PulseSamplesPerRotation) {
589         Position -= P64PulseSamplesPerRotation;
590     }
591     Current = Instance->CurrentIndex;
592     if((Current < 0) || ((Current != Instance->UsedFirst) && ((Instance->Pulses[Current].Previous >= 0) && (Instance->Pulses[Instance->Pulses[Current].Previous].Position >= Position)))) {
593         Current = Instance->UsedFirst;
594     }
595     while((Current >= 0) && (Instance->Pulses[Current].Position < Position)) {
596         Current = Instance->Pulses[Current].Next;
597     }
598     if(Current < 0) {
599         if(Instance->UsedFirst < 0) {
600             return 0;
601         } else {
602             return Instance->Pulses[Instance->UsedFirst].Strength;
603         }
604     } else {
605         Instance->CurrentIndex = Current;
606         return Instance->Pulses[Current].Strength;
607     }
608 }
609 
P64PulseStreamGetPulseCount(PP64PulseStream Instance)610 p64_uint32_t P64PulseStreamGetPulseCount(PP64PulseStream Instance) {
611     p64_int32_t Current, Count = 0;
612     Current = Instance->CurrentIndex;
613     while(Current >= 0) {
614         Count++;
615         Current = Instance->Pulses[Current].Next;
616     }
617     return (p64_uint32_t)Count;
618 }
619 
P64PulseStreamGetPulse(PP64PulseStream Instance,p64_uint32_t Position)620 p64_uint32_t P64PulseStreamGetPulse(PP64PulseStream Instance, p64_uint32_t Position) {
621     p64_int32_t Current;
622     while(Position >= P64PulseSamplesPerRotation) {
623         Position -= P64PulseSamplesPerRotation;
624     }
625     Current = Instance->CurrentIndex;
626     if((Current < 0) || ((Current != Instance->UsedFirst) && ((Instance->Pulses[Current].Previous >= 0) && (Instance->Pulses[Instance->Pulses[Current].Previous].Position >= Position)))) {
627         Current = Instance->UsedFirst;
628     }
629     while((Current >= 0) && (Instance->Pulses[Current].Position < Position)) {
630         Current = Instance->Pulses[Current].Next;
631     }
632     if((Current < 0) || (Instance->Pulses[Current].Position != Position)) {
633         return 0;
634     } else {
635         Instance->CurrentIndex = Current;
636         return Instance->Pulses[Current].Strength;
637     }
638 }
639 
P64PulseStreamSetPulse(PP64PulseStream Instance,p64_uint32_t Position,p64_uint32_t Strength)640 void P64PulseStreamSetPulse(PP64PulseStream Instance, p64_uint32_t Position, p64_uint32_t Strength) {
641     if(Strength) {
642         P64PulseStreamAddPulse(Instance, Position, Strength);
643     } else {
644         P64PulseStreamRemovePulse(Instance, Position);
645     }
646 }
647 
P64PulseStreamSeek(PP64PulseStream Instance,p64_uint32_t Position)648 void P64PulseStreamSeek(PP64PulseStream Instance, p64_uint32_t Position) {
649     p64_int32_t Current;
650     while(Position >= P64PulseSamplesPerRotation) {
651         Position -= P64PulseSamplesPerRotation;
652     }
653     Current = Instance->CurrentIndex;
654     if((Current < 0) || ((Current != Instance->UsedFirst) && ((Instance->Pulses[Current].Previous >= 0) && (Instance->Pulses[Instance->Pulses[Current].Previous].Position >= Position)))) {
655         Current = Instance->UsedFirst;
656     }
657     while((Current >= 0) && (Instance->Pulses[Current].Position < Position)) {
658         Current = Instance->Pulses[Current].Next;
659     }
660     Instance->CurrentIndex = Current;
661 }
662 
P64PulseStreamConvertFromGCR(PP64PulseStream Instance,p64_uint8_t * Bytes,p64_uint32_t Len)663 void P64PulseStreamConvertFromGCR(PP64PulseStream Instance, p64_uint8_t* Bytes, p64_uint32_t Len) {
664     p64_uint32_t PositionHi, PositionLo, IncrementHi, IncrementLo, BitStreamPosition;
665     P64PulseStreamClear(Instance);
666     if(Len) {
667         IncrementHi = P64PulseSamplesPerRotation / Len;
668         IncrementLo = P64PulseSamplesPerRotation % Len;
669         PositionHi = (P64PulseSamplesPerRotation >> 1) / Len;
670         PositionLo = (P64PulseSamplesPerRotation >> 1) % Len;
671         for(BitStreamPosition = 0; BitStreamPosition < Len; BitStreamPosition++) {
672             if(((p64_uint8_t)(Bytes[BitStreamPosition >> 3])) & (1 << ((~BitStreamPosition) & 7))) {
673                 P64PulseStreamAddPulse(Instance, PositionHi, 0xffffffffUL);
674             }
675             PositionHi += IncrementHi;
676             PositionLo += IncrementLo;
677             while(PositionLo >= Len) {
678                 PositionLo -= Len;
679                 PositionHi++;
680             }
681 
682         }
683     }
684 }
685 
P64PulseStreamConvertToGCR(PP64PulseStream Instance,p64_uint8_t * Bytes,p64_uint32_t Len)686 void P64PulseStreamConvertToGCR(PP64PulseStream Instance, p64_uint8_t* Bytes, p64_uint32_t Len) {
687     p64_uint32_t Range, PositionHi, PositionLo, IncrementHi, IncrementLo, BitStreamPosition;
688     p64_int32_t Current;
689     if(Len) {
690         memset(Bytes, 0, (Len + 7) >> 3);
691         Range = P64PulseSamplesPerRotation;
692         IncrementHi = Range / Len;
693         IncrementLo = Range % Len;
694         Current = Instance->UsedFirst;
695         PositionHi = (Current >= 0) ? Instance->Pulses[Current].Position - 1 : 0;
696         PositionLo = Len - 1;
697         for(BitStreamPosition = 0; BitStreamPosition < Len; BitStreamPosition++) {
698             PositionHi += IncrementHi;
699             PositionLo += IncrementLo;
700             while(PositionLo >= Len) {
701                 PositionLo -= Len;
702                 PositionHi++;
703             }
704             while(1) {
705                 if((Current >= 0) && (Instance->Pulses[Current].Position < PositionHi)) {
706                     PositionHi = (Instance->Pulses[Current].Position + IncrementHi) - 20; /* 1.25 microseconds headroom */
707                     PositionLo = IncrementLo;
708                     Current = Instance->Pulses[Current].Next;
709                     Bytes[BitStreamPosition >> 3] |= (p64_uint8_t)(1 << ((~BitStreamPosition) & 7));
710                 } else if(PositionHi >= Range) {
711                     PositionHi -= Range;
712                     Current = Instance->UsedFirst;
713                     continue;
714                 }
715                 break;
716             }
717         }
718 
719         /* optional: add here GCR byte-realigning-to-syncmark-borders code, if your GCR routines are working bytewise-only */
720 
721     }
722 }
723 
P64PulseStreamConvertToGCRWithLogic(PP64PulseStream Instance,p64_uint8_t * Bytes,p64_uint32_t Len,p64_uint32_t SpeedZone)724 p64_uint32_t P64PulseStreamConvertToGCRWithLogic(PP64PulseStream Instance, p64_uint8_t* Bytes, p64_uint32_t Len, p64_uint32_t SpeedZone) {
725     p64_uint32_t Position, LastPosition, Delta, DelayCounter, FlipFlop, LastFlipFlop, Clock, Counter, BitStreamPosition;
726     p64_int32_t Current;
727     if(Len) {
728         memset(Bytes, 0, (Len + 7) >> 3);
729         LastPosition = 0;
730         FlipFlop = 0;
731         LastFlipFlop = 0;
732         Clock = SpeedZone;
733         Counter = 0;
734         BitStreamPosition = 0;
735         Current = Instance->UsedFirst;
736         while((Current >= 0) && (BitStreamPosition < Len)) {
737             if(Instance->Pulses[Current].Strength >= 0x80000000UL) {
738                 Position = Instance->Pulses[Current].Position;
739                 Delta = Position - LastPosition;
740                 LastPosition = Position;
741                 DelayCounter = 0;
742                 FlipFlop ^= 1;
743                 do {
744                     if((DelayCounter == 40) && (LastFlipFlop != FlipFlop)) {
745                         LastFlipFlop = FlipFlop;
746                         Clock = SpeedZone;
747                         Counter = 0;
748                     }
749                     if(Clock == 16) {
750                         Clock = SpeedZone;
751                         Counter = (Counter + 1) & 0xfUL;
752                         if((Counter & 3) == 2) {
753                             Bytes[BitStreamPosition >> 3] |= (p64_uint8_t)((((Counter + 0x1c) >> 4) & 1) << ((~BitStreamPosition) & 7));
754                             BitStreamPosition++;
755                         }
756                     }
757                     Clock++;
758                 } while(++DelayCounter < Delta);
759             }
760             Current = Instance->Pulses[Current].Next;
761         }
762 
763         /* optional: add here GCR byte-realigning-to-syncmark-borders code, if your GCR routines are working bytewise-only */
764 
765         return BitStreamPosition;
766 
767     }
768 
769     return 0;
770 }
771 
772 #define ModelPosition 0
773 #define ModelStrength 4
774 #define ModelPositionFlag 8
775 #define ModelStrengthFlag 9
776 
777 #define ProbabilityModelCount 10
778 
779 const p64_uint32_t ProbabilityCounts[ProbabilityModelCount] = {65536, 65536, 65536, 65536, 65536, 65536, 65536, 65536, 4, 4};
780 
P64PulseStreamReadFromStream(PP64PulseStream Instance,PP64MemoryStream Stream)781 p64_uint32_t P64PulseStreamReadFromStream(PP64PulseStream Instance, PP64MemoryStream Stream) {
782     PP64RangeCoderProbabilities RangeCoderProbabilities;
783     p64_uint32_t RangeCoderProbabilityOffsets[ProbabilityModelCount];
784     p64_uint32_t RangeCoderProbabilityStates[ProbabilityModelCount];
785     TP64RangeCoder RangeCoderInstance;
786     p64_uint32_t ProbabilityCount, Index, Count, DeltaPosition, Position, Strength, result, CountPulses, Size;
787     p64_uint8_t *Buffer;
788 
789     if(P64MemoryStreamReadDWord(Stream, &CountPulses)) {
790 
791         if(P64MemoryStreamReadDWord(Stream, &Size)) {
792 
793             if(!Size) {
794                 return CountPulses ? 0 : 1;
795             }
796 
797             Buffer = p64_malloc(Size);
798 
799             if(P64MemoryStreamRead(Stream, Buffer, Size) == Size) {
800 
801                 ProbabilityCount = 0;
802                 for(Index = 0; Index < ProbabilityModelCount; Index++) {
803                     RangeCoderProbabilityOffsets[Index] = ProbabilityCount;
804                     ProbabilityCount += ProbabilityCounts[Index];
805                     RangeCoderProbabilityStates[Index] = 0;
806                 }
807                 RangeCoderProbabilities = P64RangeCoderProbabilitiesAllocate(ProbabilityCount);
808                 P64RangeCoderProbabilitiesReset(RangeCoderProbabilities, ProbabilityCount);
809 
810                 memset(&RangeCoderInstance, 0, sizeof(TP64RangeCoder));
811                 P64RangeCoderInit(&RangeCoderInstance);
812 
813                 RangeCoderInstance.Buffer = Buffer;
814                 RangeCoderInstance.BufferSize = Size;
815                 RangeCoderInstance.BufferPosition = 0;
816                 P64RangeCoderStart(&RangeCoderInstance);
817 
818                 Count = 0;
819 
820                 Position = 0;
821                 DeltaPosition = 0;
822 
823                 Strength = 0;
824 
825 #define ReadBit(Model) (RangeCoderProbabilityStates[Model] = P64RangeCoderDecodeBit(&RangeCoderInstance, RangeCoderProbabilities + (RangeCoderProbabilityOffsets[Model] + RangeCoderProbabilityStates[Model]), 4))
826 
827 #define ReadDWord(Model) \
828           { \
829             p64_uint32_t ByteValue, ByteIndex, Context; \
830             p64_int32_t Bit; \
831                       result = 0; \
832                       for (ByteIndex = 0; ByteIndex < 4; ByteIndex++) { \
833                           Context = 1; \
834                           for (Bit = 7; Bit >= 0; Bit--) { \
835                               Context = (Context << 1) | P64RangeCoderDecodeBit(&RangeCoderInstance, RangeCoderProbabilities + (RangeCoderProbabilityOffsets[Model + ByteIndex] + (((RangeCoderProbabilityStates[Model + ByteIndex] << 8) | Context) & 0xffffUL)), 4); \
836                           } \
837                           ByteValue = Context & 0xffUL; \
838                           RangeCoderProbabilityStates[Model + ByteIndex] = ByteValue; \
839                           result |= (p64_uint32_t)(((ByteValue & 0xffUL) << (ByteIndex << 3))); \
840             } \
841                     } \
842 
843                 while(Count < CountPulses) {
844 
845                     if(ReadBit(ModelPositionFlag)) {
846                         ReadDWord(ModelPosition);
847                         DeltaPosition = result;
848                         if(!DeltaPosition) {
849                             break;
850                         }
851                     }
852                     Position += DeltaPosition;
853 
854                     if(ReadBit(ModelStrengthFlag)) {
855                         ReadDWord(ModelStrength);
856                         Strength += result;
857                     }
858 
859                     P64PulseStreamAddPulse(Instance, Position, Strength);
860 
861                     Count++;
862                 }
863 
864                 P64RangeCoderProbabilitiesFree(RangeCoderProbabilities);
865 
866                 p64_free(Buffer);
867 
868                 return Count == CountPulses;
869 
870             }
871 
872             p64_free(Buffer);
873         }
874 
875     }
876 
877 #undef ReadBit
878 #undef ReadDWord
879 
880     return 0;
881 }
882 
P64PulseStreamWriteToStream(PP64PulseStream Instance,PP64MemoryStream Stream)883 p64_uint32_t P64PulseStreamWriteToStream(PP64PulseStream Instance, PP64MemoryStream Stream) {
884     PP64RangeCoderProbabilities RangeCoderProbabilities;
885     p64_uint32_t RangeCoderProbabilityOffsets[ProbabilityModelCount];
886     p64_uint32_t RangeCoderProbabilityStates[ProbabilityModelCount];
887     TP64RangeCoder RangeCoderInstance;
888     p64_int32_t Index, Current;
889     p64_uint32_t ProbabilityCount, LastPosition, PreviousDeltaPosition, DeltaPosition, LastStrength, CountPulses, Size;
890 
891     ProbabilityCount = 0;
892     for(Index = 0; Index < ProbabilityModelCount; Index++) {
893         RangeCoderProbabilityOffsets[Index] = ProbabilityCount;
894         ProbabilityCount += ProbabilityCounts[Index];
895         RangeCoderProbabilityStates[Index] = 0;
896     }
897     RangeCoderProbabilities = P64RangeCoderProbabilitiesAllocate(ProbabilityCount);
898     P64RangeCoderProbabilitiesReset(RangeCoderProbabilities, ProbabilityCount);
899 
900     memset(&RangeCoderInstance, 0, sizeof(TP64RangeCoder));
901     P64RangeCoderInit(&RangeCoderInstance);
902 
903     LastPosition = 0;
904     PreviousDeltaPosition = 0;
905 
906     LastStrength = 0;
907 
908 #define WriteBit(Model, BitValue) RangeCoderProbabilityStates[Model] = P64RangeCoderEncodeBit(&RangeCoderInstance, RangeCoderProbabilities + (RangeCoderProbabilityOffsets[Model] + RangeCoderProbabilityStates[Model]), 4, BitValue)
909 
910 #define WriteDWord(Model, DWordValue) \
911       { \
912         p64_uint32_t ByteValue, ByteIndex, Context, Value; \
913         p64_int32_t Bit; \
914         Value = DWordValue; \
915                for (ByteIndex = 0; ByteIndex < 4; ByteIndex++) { \
916                   ByteValue = (Value >> (ByteIndex << 3)) & 0xffUL; \
917                   Context = 1; \
918                   for (Bit = 7; Bit >= 0; Bit--) { \
919                       Context = (Context << 1) | P64RangeCoderEncodeBit(&RangeCoderInstance, RangeCoderProbabilities + (RangeCoderProbabilityOffsets[Model + ByteIndex] + (((RangeCoderProbabilityStates[Model+ByteIndex] << 8) | Context) & 0xffffUL)), 4, (ByteValue >> Bit) & 1); \
920                   } \
921                   RangeCoderProbabilityStates[Model + ByteIndex] = ByteValue; \
922               }    \
923             }
924 
925     CountPulses = 0;
926 
927     Current = Instance->UsedFirst;
928     while(Current >= 0) {
929         DeltaPosition = Instance->Pulses[Current].Position - LastPosition;
930         if(PreviousDeltaPosition != DeltaPosition) {
931             PreviousDeltaPosition = DeltaPosition;
932             WriteBit(ModelPositionFlag, 1);
933             WriteDWord(ModelPosition, DeltaPosition);
934         } else {
935             WriteBit(ModelPositionFlag, 0);
936         }
937         LastPosition = Instance->Pulses[Current].Position;
938 
939         if(LastStrength != Instance->Pulses[Current].Strength) {
940             WriteBit(ModelStrengthFlag, 1);
941             WriteDWord(ModelStrength, Instance->Pulses[Current].Strength - LastStrength);
942         } else {
943             WriteBit(ModelStrengthFlag, 0);
944         }
945         LastStrength = Instance->Pulses[Current].Strength;
946 
947         CountPulses++;
948 
949         Current = Instance->Pulses[Current].Next;
950     }
951 
952     WriteBit(ModelPositionFlag, 1);
953     WriteDWord(ModelPosition, 0);
954 
955     P64RangeCoderFlush(&RangeCoderInstance);
956 
957     P64RangeCoderProbabilitiesFree(RangeCoderProbabilities);
958 
959     if(RangeCoderInstance.Buffer) {
960         Size = RangeCoderInstance.BufferPosition;
961     } else {
962         Size = 0;
963     }
964 
965     if(P64MemoryStreamWriteDWord(Stream, &CountPulses)) {
966         if(P64MemoryStreamWriteDWord(Stream, &Size)) {
967             if(RangeCoderInstance.Buffer) {
968                 if(P64MemoryStreamWrite(Stream, RangeCoderInstance.Buffer, RangeCoderInstance.BufferPosition) == RangeCoderInstance.BufferPosition) {
969                     p64_free(RangeCoderInstance.Buffer);
970                     return 1;
971                 }
972                 p64_free(RangeCoderInstance.Buffer);
973                 return 0;
974             }
975             return 1;
976         }
977     }
978 
979 #undef WriteBit
980 #undef WriteDWord
981     return 0;
982 }
983 
P64ImageCreate(PP64Image Instance)984 void P64ImageCreate(PP64Image Instance) {
985     p64_int32_t HalfTrack, side;
986     memset(Instance, 0, sizeof(TP64Image));
987     Instance->noSides = 1;
988     for(side=0; side<2; side++) {
989         for(HalfTrack = 0; HalfTrack <= P64LastHalfTrack; HalfTrack++) {
990             P64PulseStreamCreate(&Instance->PulseStreams[side][HalfTrack]);
991         }
992     }
993     P64ImageClear(Instance);
994 }
995 
P64ImageDestroy(PP64Image Instance)996 void P64ImageDestroy(PP64Image Instance) {
997     p64_int32_t HalfTrack, side;
998     for(side=0; side<2; side++) {
999         for(HalfTrack = 0; HalfTrack <= P64LastHalfTrack; HalfTrack++) {
1000             P64PulseStreamDestroy(&Instance->PulseStreams[side][HalfTrack]);
1001         }
1002     }
1003     memset(Instance, 0, sizeof(TP64Image));
1004 }
1005 
P64ImageClear(PP64Image Instance)1006 void P64ImageClear(PP64Image Instance) {
1007     p64_int32_t HalfTrack, side;
1008     Instance->WriteProtected = 0;
1009     for(side=0; side<2; side++) {
1010         for(HalfTrack = 0; HalfTrack <= P64LastHalfTrack; HalfTrack++) {
1011             P64PulseStreamClear(&Instance->PulseStreams[side][HalfTrack]);
1012         }
1013     }
1014 }
1015 
P64ImageReadFromStream(PP64Image Instance,PP64MemoryStream Stream)1016 p64_uint32_t P64ImageReadFromStream(PP64Image Instance, PP64MemoryStream Stream) {
1017     TP64MemoryStream ChunksMemoryStream, ChunkMemoryStream;
1018     p64_uint32_t Version, Flags, Size, Checksum, HalfTrack, OK, side;
1019     TP64HeaderSignature HeaderSignature;
1020     TP64ChunkSignature ChunkSignature;
1021 
1022     OK = 0;
1023     P64ImageClear(Instance);
1024     if(P64MemoryStreamSeek(Stream, 0) == 0) {
1025         if(P64MemoryStreamRead(Stream, (void*)&HeaderSignature, sizeof(TP64HeaderSignature)) == sizeof(TP64HeaderSignature)) {
1026             if((HeaderSignature[0] == 'P') && (HeaderSignature[1] == '6') && (HeaderSignature[2] == '4') && (HeaderSignature[3] == '-') && (HeaderSignature[4] == '1') && (HeaderSignature[5] == '5') && (HeaderSignature[6] == '4') && (HeaderSignature[7] == '1')) {
1027                 if(P64MemoryStreamReadDWord(Stream, &Version)) {
1028                     if(Version == 0x00000000) {
1029                         if(P64MemoryStreamReadDWord(Stream, &Flags)) {
1030                             if(P64MemoryStreamReadDWord(Stream, &Size)) {
1031                                 if(P64MemoryStreamReadDWord(Stream, &Checksum)) {
1032                                     Instance->WriteProtected = (Flags & 1) != 0;
1033                                     Instance->noSides = 1+!!(Flags & 2);
1034                                     P64MemoryStreamCreate(&ChunksMemoryStream);
1035                                     if(P64MemoryStreamAppendFromCount(&ChunksMemoryStream, Stream, Size) == Size) {
1036                                         if(P64CRC32(ChunksMemoryStream.Data, Size) == Checksum) {
1037                                             if(P64MemoryStreamSeek(&ChunksMemoryStream, 0) == 0) {
1038                                                 OK = 1;
1039                                                 while(OK && (ChunksMemoryStream.Position < ChunksMemoryStream.Size)) {
1040                                                     if(P64MemoryStreamRead(&ChunksMemoryStream, (void*)&ChunkSignature, sizeof(TP64ChunkSignature)) == sizeof(TP64ChunkSignature)) {
1041                                                         if(P64MemoryStreamReadDWord(&ChunksMemoryStream, &Size)) {
1042                                                             if(P64MemoryStreamReadDWord(&ChunksMemoryStream, &Checksum)) {
1043                                                                 OK = 0;
1044                                                                 P64MemoryStreamCreate(&ChunkMemoryStream);
1045                                                                 if(Size == 0) {
1046                                                                     if(Checksum == 0) {
1047                                                                         if((ChunkSignature[0] == 'D') && (ChunkSignature[1] == 'O') && (ChunkSignature[2] == 'N') && (ChunkSignature[3] == 'E')) {
1048                                                                             OK = 1;
1049                                                                         } else {
1050                                                                             OK = 1;
1051                                                                         }
1052                                                                     }
1053                                                                 } else {
1054                                                                     if(P64MemoryStreamAppendFromCount(&ChunkMemoryStream, &ChunksMemoryStream, Size) == Size) {
1055                                                                         if(P64MemoryStreamSeek(&ChunkMemoryStream, 0) == 0) {
1056                                                                             if(P64CRC32(ChunkMemoryStream.Data, Size) == Checksum) {
1057                                                                                 if((ChunkSignature[0] == 'H') && (ChunkSignature[1] == 'T') && (ChunkSignature[2] == 'P') && (((ChunkSignature[3] & 127) >= P64FirstHalfTrack) && ((ChunkSignature[3] & 127) <= P64LastHalfTrack))) {
1058                                                                                     HalfTrack = ChunkSignature[3] & 127;
1059                                                                                     side = !!(ChunkSignature[3] & 128);
1060                                                                                     OK = P64PulseStreamReadFromStream(&Instance->PulseStreams[side][HalfTrack], &ChunkMemoryStream);
1061                                                                                 } else {
1062                                                                                     OK = 1;
1063                                                                                 }
1064                                                                             }
1065                                                                         }
1066                                                                     }
1067                                                                 }
1068                                                                 P64MemoryStreamDestroy(&ChunkMemoryStream);
1069                                                                 continue;
1070                                                             }
1071                                                         }
1072                                                     }
1073                                                     break;
1074                                                 }
1075 
1076                                             }
1077                                         }
1078                                     }
1079                                     P64MemoryStreamDestroy(&ChunksMemoryStream);
1080                                 }
1081                             }
1082                         }
1083                     }
1084                 }
1085             }
1086         }
1087     }
1088     return OK;
1089 }
1090 
P64ImageWriteToStream(PP64Image Instance,PP64MemoryStream Stream)1091 p64_uint32_t P64ImageWriteToStream(PP64Image Instance, PP64MemoryStream Stream) {
1092     TP64MemoryStream MemoryStream, ChunksMemoryStream, ChunkMemoryStream;
1093     p64_uint32_t Version, Flags, Size, Checksum, HalfTrack, result, WriteChunkResult, side;
1094 
1095     TP64HeaderSignature HeaderSignature;
1096     TP64ChunkSignature ChunkSignature;
1097 
1098 #define WriteChunk() \
1099 { \
1100     WriteChunkResult = 0; \
1101     Size = ChunkMemoryStream.Size; \
1102     Checksum = (ChunkMemoryStream.Size > 0) ? P64CRC32(ChunkMemoryStream.Data,ChunkMemoryStream.Size) : 0; \
1103     if (P64MemoryStreamWrite(&ChunksMemoryStream, (void*)&ChunkSignature, sizeof(TP64ChunkSignature)) == sizeof(TP64ChunkSignature)) { \
1104         if (P64MemoryStreamWriteDWord(&ChunksMemoryStream, &Size)) { \
1105             if (P64MemoryStreamWriteDWord(&ChunksMemoryStream, &Checksum)) { \
1106                 if (ChunkMemoryStream.Size == 0) { \
1107                     WriteChunkResult = 1; \
1108                 }else{ \
1109                     if (P64MemoryStreamSeek(&ChunkMemoryStream, 0) == 0) { \
1110                         if (P64MemoryStreamAppendFromCount(&ChunksMemoryStream, &ChunkMemoryStream, ChunkMemoryStream.Size) == ChunkMemoryStream.Size) { \
1111                             WriteChunkResult = 1; \
1112                         } \
1113                     } \
1114           } \
1115             } \
1116         } \
1117     } \
1118 }
1119 
1120     result = 0;
1121 
1122     HeaderSignature[0] = 'P';
1123     HeaderSignature[1] = '6';
1124     HeaderSignature[2] = '4';
1125     HeaderSignature[3] = '-';
1126     HeaderSignature[4] = '1';
1127     HeaderSignature[5] = '5';
1128     HeaderSignature[6] = '4';
1129     HeaderSignature[7] = '1';
1130     Version = 0x00000000;
1131 
1132     P64MemoryStreamCreate(&MemoryStream);
1133     P64MemoryStreamCreate(&ChunksMemoryStream);
1134 
1135     result = 1;
1136     for (side = 0; side < (p64_uint32_t)Instance->noSides; side++) {
1137         for(HalfTrack = P64FirstHalfTrack; HalfTrack <= P64LastHalfTrack; HalfTrack++) {
1138 
1139             P64MemoryStreamCreate(&ChunkMemoryStream);
1140             result = P64PulseStreamWriteToStream(&Instance->PulseStreams[side][HalfTrack], &ChunkMemoryStream);
1141             if(result) {
1142                 ChunkSignature[0] = 'H';
1143                 ChunkSignature[1] = 'T';
1144                 ChunkSignature[2] = 'P';
1145                 ChunkSignature[3] = (p64_uint8_t)(HalfTrack + 128*side);
1146                 WriteChunk();
1147                 result = WriteChunkResult;
1148             }
1149             P64MemoryStreamDestroy(&ChunkMemoryStream);
1150             if(!result) {
1151                 break;
1152             }
1153         }
1154     }
1155 
1156     if(result) {
1157 
1158         P64MemoryStreamCreate(&ChunkMemoryStream);
1159         ChunkSignature[0] = 'D';
1160         ChunkSignature[1] = 'O';
1161         ChunkSignature[2] = 'N';
1162         ChunkSignature[3] = 'E';
1163         WriteChunk();
1164         result = WriteChunkResult;
1165         P64MemoryStreamDestroy(&ChunkMemoryStream);
1166 
1167         if(result) {
1168             result = 0;
1169 
1170             Flags = 0;
1171             if(Instance->WriteProtected) {
1172                 Flags |= 1;
1173             }
1174             if(Instance->noSides == 2) {
1175                 Flags |= 2;
1176             }
1177 
1178             Size = ChunksMemoryStream.Size;
1179             Checksum = P64CRC32(ChunksMemoryStream.Data, Size);
1180 
1181             if(P64MemoryStreamWrite(&MemoryStream, (void*)&HeaderSignature, sizeof(TP64HeaderSignature)) == sizeof(TP64HeaderSignature)) {
1182                 if(P64MemoryStreamWriteDWord(&MemoryStream, &Version)) {
1183                     if(P64MemoryStreamWriteDWord(&MemoryStream, &Flags)) {
1184                         if(P64MemoryStreamWriteDWord(&MemoryStream, &Size)) {
1185                             if(P64MemoryStreamWriteDWord(&MemoryStream, &Checksum)) {
1186                                 if(P64MemoryStreamSeek(&ChunksMemoryStream, 0) == 0) {
1187                                     if(P64MemoryStreamAppendFromCount(&MemoryStream, &ChunksMemoryStream, ChunksMemoryStream.Size) == ChunksMemoryStream.Size) {
1188                                         if(P64MemoryStreamSeek(&MemoryStream, 0) == 0) {
1189                                             if(P64MemoryStreamAppendFromCount(Stream, &MemoryStream, MemoryStream.Size) == MemoryStream.Size) {
1190                                                 result = 1;
1191                                             }
1192                                         }
1193                                     }
1194                                 }
1195                             }
1196                         }
1197                     }
1198                 }
1199             }
1200 
1201         }
1202 
1203     }
1204 
1205     P64MemoryStreamDestroy(&ChunksMemoryStream);
1206     P64MemoryStreamDestroy(&MemoryStream);
1207 
1208 #undef WriteChunk
1209 
1210     return result;
1211 }
1212