#PROGRAM       /GFPACK8(15AM,22AM,DBM,EBM)
#PAGE

#INCLUDE       STACK(/DEFS)

#PROGRAM

#              Pack 8 bit bytes into a buffer

#              gf pack8 (int *from, int len, int *to, int pos)

#SET           ARGS=0

#DEFINE        ARGFROM=ARGS
#SET           ARGS=ARGS+1

#DEFINE        ARGLEN=ARGS
#SET           ARGS=ARGS+1

#DEFINE        ARGTO=ARGS
#SET           ARGS=ARGS+1

#DEFINE        ARGPOS=ARGS
#SET           ARGS=ARGS+1

#              Stack frame

#SET           STFRAME=SXFRAME

#              First the arguments:

#DEFINE        STFROM=SXPAR1  
#SET           STFRAME=STFRAME+1

#DEFINE        STLEN=SXPAR2
#SET           STFRAME=STFRAME+1

#DEFINE        STTO=SXPAR3
#SET           STFRAME=STFRAME+1

#DEFINE        STPOS=SXPAR4
#SET           STFRAME=STFRAME+1

#              Now local vars:

#              Pick up call by value parameters

      STACK 2
      OBEY     ARGLEN(1)
      LDX   0  0(3) 
      STO   0  STLEN(2)

      OBEY     ARGPOS(1) 
      LDX   0  0(3)
      STO   0  STPOS(2)

#              Pick up call by name parameters

      OBEY     ARGFROM(1)
      ARRAY 3                        [ arg is array elem or name
      STACK 2                        [ restore stack top
      STO   3  STFROM(2)

      OBEY     ARGTO(1)
      ARRAY 3                        [ arg is array elem or name
      STACK 2                        [ restore stack top
      STO   3  STTO(2)

      ADN   1  ARGS

#              Now Pascal style entry point

#CUE           GPPACK8

      BEGIN 2  3,1,STFRAME


#              X1 - source
#              X3 - dest
#              X5 - count left
#              X7 - pos in word (0, 1, 2)

      LDX   5  STLEN(2)               [ get bytes to move
      BZE   5  BYE                    [ all done?

      LDX   1  STFROM(2)              [ get source addr
      LDX   3  STTO(2)                [ get destination buf
      LDX   7  STPOS(2)               [ get position to write to

      BZE   7  WDS                    [ optimise frequent case

      LDN   0  3
      DVS   6  0                      [ X7 = pos/3. x6 = remainder

      ADX   1  7                      [ point to dest word

#               Jump if writing to start of word

      BZE   6   WDS                   [ go copy full words

      LDX   4   0(3)                  [ get dest word

#               X6 = 1, middle byte, = 2, rightmost byte

      SBNC  6   2
      BCC       RIGHT                 [ go write to rightmost byte

#               Write to middle byte

MID   LDX   0   0(1)                  [ get byte

      SLL   0   8                     [ move byte to middle

#               -4081 = 0xfff00f, reduce shifting needed
     
      NGN   6   4081                  [ get mask to X6
      SLC   6   4                     [ x6 = 0xff00ff
      ANDX  4   6                     [ carve hole for byte
      ORX   4   0                     [ or in byte

      SBN   5   1                     [ decrement bytes to do
      BNZ   5   *+3                   [ carry on if more

      STO   4   0(3)                  [ save word
      BRN       BYE                   [ and exit 

      ADN   1   1                     [ increment input pointer

#               Write to rightmost byte

RIGHT LDX   0   0(1)                  [ get byte

      NGN   6   256                   [ get mask
      ANDX  4   6                     [ mask off unwanted
      ORX   4   0                     [ or in byte

      STO   4   0(3)                  [ save word

      SBN   5   1                     [ more to do?
      BZE   5   BYE

      ADN   1   1                     [ increment input pointer
      ADN   3   1                     [ incr output pointer

#               Now move full words, don't need to worry
#               about previous contents

#               get here with X5 = bytes left to do
#               use X4 = byte count, X5 = word count 

WDS   LDN   0   3
      DVS   4   0                      [ x5 = count/3, x4 = remainder
      BZE   5   TRAIL                  [ go deal with trailing bytes

NXT   LDX   0   0(1)                   [ get leftmost byte
      SLL   0   8 
      ORX   0   1(1)                   [ get middle byte
      SLL   0   8
      ORX   0   2(1)                   [ get rightmost byte
      STO   0   0(3)                   [ save
      ADN   1   3                      [ bump pointers
      ADN   3   1
      BCT   5   NXT                    [ loop back for next word

#               Now move any trailing bytes
#               X4 = byte count, 0, 1 or 2

TRAIL BZE   4   BYE

      LDX   0   0(1)                  [ get byte
      SRC   0   8                     [ move to top

      NGN   5   256                   [ get mask
      SRC   5   8                     [ x5 = 0x00ffff

      ANDX  5   0(3)                  [ mask in word
      ORX   5   0                     [ or in byte

      SBNC  4   2                     [ done?
      BCS       DONE
    
      LDX   0   1(1)                  [ get next byte 

      SLL   0   8                     [ move to middle
      NGN   6   4081                  [ get mask to X6
      SLC   6   4                     [ x6 = 0xff00ff

      ANDX  5   6                     [ Carve hole for byte
      ORX   5   0                     [ or in byte

DONE  STO   5   0(3)                  [ and save

BYE   END   2  1,0

#END
#FINISH