%begin; ! Disk formatting program for Mini-Filestore

%include "i:util.inc"

{ disk characteristics }
%constant %integer restore step rate   =     3,       { 1.5ms }
                   seek step rate      =     0,       { 10us }
                   size                = 16_20,
                   sectors per block   = 2,
                   blocks per track    = 16,
                   tracks per cylinder = 4,
                   cylinders per drive = 256,
                   number of drives    =1  {Just format first drive}

{ disk controller commands }
%constant %integer restore      = 16_10 + restore step rate,
                   seek         = 16_70 + seek step rate,
                   read sector  = 16_20,               { + 8 for DMA }
                   write sector = 16_30,
                   format track = 16_50

{ status values }
%constant %integer busy          = 16_80,
                   ready         = 16_40,
                   write fault   = 16_20,
                   seek complete = 16_10,
                   data request  = 16_08,
                   error         = 16_01
 
{ disk controller task file }
%record %format mfsr (%short status,
                      %short size drive head,
                      %short cylinder high,
                      %short cylinder low,
                      %short sector number,
                      %short sector count,
                      %short error register,
                      %short data register)

%record %format mfsw (%short command,
                      %short size drive head,
                      %short cylinder high,
                      %short cylinder low,
                      %short sector number,
                      %short sector count,
                      %short precomp,
                      %short data register)

@ 16_7ffe0 %record (mfsr) read
@ 16_7ffe0 %record (mfsw) rite

%own %integer BL Display Error = 1

%routine wait(%string(20) cmd)

  %Integer i
  %routine report byte(%string(20) text, %integer byte)
    printstring(text)   
    phex2(byte)
    newline
  %end
 
  %cycle
  %repeat %until read_status & busy = 0
  %For i=1,1,6000{0} %cycle ; %Repeat
  report byte ("Stat : ",read_status)
  %if read_status & error # 0 %start
    %if BL Display Error # 0 %start
      newline
      printstring("Mini-file-store error on ")
      printstring(cmd)
      printstring(" command")
      newline
      report byte("Status:          ", read_status)
      report byte("Size/Drive/Head: ", read_size drive head)
      report byte("Cylinder high:   ", read_cylinder high)
      report byte("Cylinder low:    ", read_cylinder low)
      report byte("Sector number:   ", read_sector number)
      report byte("Sector count:    ", read_sector count)
      report byte("Error register:  ", read_error register)
      report byte("Data register:   ", read_data register)
    %finish
    %signal %event 9,3
  %finish
%end
 
%routine BL Reset
  %integer drive
  rite_precomp = 16_20             { precomp from track 128 onward }
  %for drive = 0,1,number of drives-1 %cycle
    rite_size drive head = drive << 3
    rite_command = restore
    wait("restore")
    rite_cylinder high = 0
    rite_cylinder low = 0
    rite_command = seek            { subsequent seeks at 10us step rate }
    wait("seek")
  %repeat
%end

%routine setup task file(%integer block number)
  %integer drive, cylinder, head, block, remainder

  block     = rem(block number, blocks per track)
  remainder = block number // blocks per track
  head      = rem(remainder, tracks per cylinder)
  remainder = remainder // tracks per cylinder
  cylinder  = rem(remainder, cylinders per drive)
  drive     = remainder // cylinders per drive
  %signal %event 9,3 %if drive >= number of drives
  rite_sector number   = block * sectors per block
  rite_size drive head = drive << 3 + head
  rite_cylinder high   = cylinder >> 8
  rite_cylinder low    = cylinder
%end

%routine BL Read Block(%integer block number, block)
!                                 %byte %array %name block(0:511))
  %integer byte, sector

  setup task file(block number)
  %for sector = 0,1,1 %cycle
    rite_command = read sector
    wait("read sector")
!   %for byte = sector * 256, 1, 255 + sector * 256 %cycle
!     block(byte) = read_data register
!   %repeat
    *movea.l block,a0
    *move.l  sector,d0
    *lsl     #8,d0
    *adda.l  d0,a0
    *move.l  #63,d0
    *lea     16_7ffef,a1
loop:
    *move.b  (a1),(a0)+
    *move.b  (a1),(a0)+
    *move.b  (a1),(a0)+
    *move.b  (a1),(a0)+
!    *move.b  16_7ffef,d1
!    *lsl     #8,d1
!    *move.b  16_7ffef,d1
!    *lsl     #8,d1
!    *move.b  16_7ffef,d1
!    *lsl     #8,d1
!    *move.b  16_7ffef,d1
!    *move.l  d1,(a0)+
!    *dbra    d0,#-34
    *dbra    d0,loop
    rite_sector number = read_sector number + 1 { assumes even      }
                                                         { sectors per track }
  %repeat
%end

%routine BL Write Block(%integer block number, block)
{                                  %byte %array %name block(0:511))
  %integer byte, sector

  setup task file(block number)
  %for sector = 0,1,1 %cycle
    rite_command = write sector
!   %for byte = sector * 256, 1, 255 + sector * 256 %cycle
!     rite_data register = block(byte)
!   %repeat
    *movea.l block,a0
    *move.l  sector,d0
    *lsl     #8,d0
    *adda.l  d0,a0
    *move.l  #63,d0
    *lea     16_7ffef,a1
loop:
    *move.b  (a0)+,(a1)
    *move.b  (a0)+,(a1)
    *move.b  (a0)+,(a1)
    *move.b  (a0)+,(a1)
!    *move.l  (a0)+,d1
!    *rol     #8,d1
!    *move.b  d1,16_7ffef
!    *rol     #8,d1
!    *move.b  d1,16_7ffef
!    *rol     #8,d1
!    *move.b  d1,16_7ffef
!    *rol     #8,d1
!    *move.b  d1,16_7ffef
!    *dbra    d0,#-36
    *dbra    d0,loop
    wait("write sector")
    rite_sector number = read_sector number + 1 { assumes even      }
                                                         { sectors per track }
  %repeat
%end

%routine BL Format Track(%integer drive,cylinder,track,data)
  rite_sector number   = 0
  rite_size drive head = drive << 3 + track
  rite_cylinder high   = cylinder >> 8
  rite_cylinder low    = cylinder
  rite_sector count    = sectors per block * blocks per track
  rite_command = format track
  *movea.l data,a0
  *move.l  #127,d0
  *lea     16_7ffef,a1
loop:
  *move.b  (a0)+,(a1)
  *move.b  (a0)+,(a1)
  *move.b  (a0)+,(a1)
  *move.b  (a0)+,(a1)
  *dbra    d0,loop
  wait("format track")
  rite_sector count = 1
%end

%routine format system
%constinteger sectors per track = sectors per block * blocks per track
%constinteger increment=3
%bytearray b(0:255)
%bytearray s(0:sectors per track-1)
%integer data,i,j,n
  %routine add(%integer delta,%integername n)
    n = (n+delta)&(sectors per track-1)
  %end
  data = addr(b(0))
  b(i) = 0 %for i = 0,1,255
  s(i) = 0 %for i = 0,1,sectors per track-1
  i = 0
  %for j = 1,1,sectors per track %cycle
    add(1,i) %while s(i)#0; s(i) = j
    b(j*2-1) = i
    add(increment,i)
  %repeat
{}%for j = 0,1,sectors per track-1 %cycle
{}  write(j,2); write(s(j),2); write(b(j*2+1),2); newline
{}%repeat
  %for n = 0,1,number of drives-1 %cycle
    %for i = 0,1,cylinders per drive-1 %cycle
{}write(n,2); write(i,3); printsymbol(':')
      %for j = 0,1,tracks per cylinder-1 %cycle
{}write(j,2)
        bl format track(n,i,j,data)
      %repeat
{}newline
    %repeat
  %repeat
%end

%routine do(%integer op,bytes,da,ub)
  %cycle
    %if op=0 %start
      bl read block(da>>9,ub)
    %else
      bl writeblock(da>>9,ub)
    %finish
    ub = ub+512; da = da+512; bytes = bytes-512
  %repeatuntil bytes<=0
%end

%constinteger dwrite=1,dread=2,dverify=4

%integerfn transfer(%integer func,bytes,disqaddr,%integername buf)
%integer lives=0
  %onevent 9 %start
    %result = -1
  %finish
  do(1, bytes,disqaddr,addr(buf)) %if func&dwrite#0
  do(0,  bytes,disqaddr,addr(buf)) %if func&dread#0
  %result = bytes
%end

bl reset
format system

%end
