\ =========================================================================
\  File: fixpt-math-lib.fs for Mecrisp-Stellaris by Matthias Koch
\
\  This file contains these functions for s31.32 fixed point numbers:
\
\           sqrt, sin, cos, tan, asin, acos, atan
\           log2, log10, ln, pow2, pow10, exp
\
\ ------------- Comments on sqrt and trig functions -----------------------
\
\  All angles are in degrees.
\
\  Accuracy is good rounded to 7 significant digits, with some exceptions.
\  In particular, the asin and acos functions have reduced accuracy
\  near the endpoints of the range of their inputs (+/-1) due to their
\  very large slopes there.  See the tests in fixpt-mat-lib-tests.fs.
\
\  The sin function is based on Maclaurin series for sin and cos over
\  the interval [0, pi/4], evaluated as polynomials with the Horner
\  method.  This is extended to all angles using (anti)symmetry.  Cos is
\  calculated from sin with cos(x) = sin(x+90), and tan is calculated as
\  sin/cos.
\
\  Atan is based on the first 7 terms of its Euler series over the
\  interval [0, 1/8].  It is extended to [0, 1] using the identity
\
\   atan(x) = atan(c) + atan((x-c)/(1+x*c)), c = 1/8, 2/8, ..., 7/8, 1.
\
\  For x>1 we use atan(x) = 90 - atan(1/x).  Negative arguments are handled
\  by antisymmetry.  Asin and acos are calculated using the formulas
\
\    asin(x) = atan(x / sqrt(1 - x^2)),  x^2 <= 1/2
\
\    asin(x) = 90 - atan(sqrt(1 - x^2) / x),  x^2 > 1/2
\
\    acos(x) = 90 - asin(x)
\
\  The square root is calculated bitwise with a standard algorithm over
\  the interval [0, 1] and is extended to all positive x by division by 4
\  until the quotient is in [0, 1].
\
\ ------------- Comments on the log and power functions -------------------
\
\  The user can check for accuracy by running the test functions in the
\  file fixpt-math-lib-tests.fs, or by running tests tailored to their use.
\  Generally the functions are accurate when rounded to about 7 significant
\  digits.  However, the user should not expect good accuracy when dealing
\  with very small fractional values due to the limitations of fixed
\  point.  In particular, this affects the values of the power and
\  exponential functions for larger negative inputs, when the relative
\  accuracy decreases significantly.
\
\  If the argument to a log function is non-positive, the function returns
\  "minus infinity," the largest negative s31.32 value.  This is the only
\  signal that an invalid input has been used.  Large negative inputs
\  to the power and exponential functions will return zero.  Large positive
\  inputs will return "plus infinity," the largest positive s31.32 value.
\  The code shows the specific values used to determine "large" in each
\  case.
\
\  The algorithm for calculating the base 2 log is taken from pseudocode
\  in the Wikipedia article "Binary logarithm" which is based on:
\
\     Majithia, J. C.; Levan, D. (1973), "A note on base-2 logarithm
\     computations", Proceedings of the IEEE, 61 (10): 1519–1520,
\     doi:10.1109/PROC.1973.9318
\
\  The log10(x) and natural logarithm ln(x) make use of the identities
\
\     log10(x) = log10(2)*log2(x), ln(x) = ln(2)*log2(x)
\
\  where log10(2) and ln(2) are given constants.
\
\  The pow2(x) = 2^x function is calculated as 2^x = (2^z)*(2^n) where
\  x = z + n, n is an integer with n <= x < n+1, and 0 <= z < 1.  The
\  factor 2^z is calculated by the identity 2^z = exp(ln(2)*z) where
\  exp(y) is calculated using its Maclaurin series.  The other factor
\  is accounted for by shifting 2^z n times (shift left for n > 0, shift
\  right for n < 0).
\
\  The pow10(x) = 10^x function is calculated using the identity
\
\               10^x = 2^(x*ln(10)/ln(2))
\
\  except for positive integer values of x, where simple multiplication
\  is used.
\
\  The exp(y) function is calculated using the series above if y is
\  between -0.36 and +0.36.  Otherwise it is calculated from pow2 using the
\  identity exp(y) = pow2(y/ln(2)).
\
\ -------------------------------------------------------------------------
\  Note:  Some s31.32 constant values were rounded from theoretical values
\         and entered below as (comma-part) integers rather calculating
\         them using Forth conversions, which trucate.
\
\ -------------------------------------------------------------------------
\  Andrew Palm
\  2018.04.09
\ =========================================================================


\ -------------------------------------------------------------------------
\  Misc. helper words, constants, and variables
\ -------------------------------------------------------------------------
\ Most positive and negative s31.32 values possible
$FFFFFFFF $7FFFFFFF 2constant +inf  \ 2147483647,9999999999
$0 $80000000 2constant -inf         \ 2147483648,0

\ Return the floor of an s31.32 value df
: floor ( df -- df ) nip 0 swap 2-foldable ;

\ Convert an s31.32 angle df1 in degrees to an angle df2 in [0, 360)
\ such that df1 = df2 + n*360 where n is an integer
: deg0to360 ( df1 -- df2 )  360,0 d/mod 2drop 2dup d0< if 360,0 d+ then
  2-foldable
;

\ Convert an s31.32 angle df1 in degrees to an angle df2 in [-90, 90)
\ such that df1 = df2 + n*180 where n is an integer.  (For tan only.)
: deg-90to90 ( df1 -- df2 )
  180,0 d/mod 2drop
  2dup 90,0 d< not if
    180,0 d-
  else
    2dup -90,0 d< if
      180,0 d+
    then
  then
  2-foldable
;

\ From common directory of Mecrisp-Stellaris Forth 2.4.0
: numbertable <builds does> swap 2 lshift + @ ;

\ -------------------------------------------------------------------------
\  Square root functions
\ -------------------------------------------------------------------------
: 0to1sqrt ( x -- sqrtx )
  \ Take square root of s31.32 number x with x in interval [0, 1]
  \ Special cases x = 0 and x = 1
  2dup d0= if exit then
  2dup 1,0 d= if exit then

  swap    \ Put x in MSW of unsigned 64-bit integer u

  \ Find square root of u as 64-bit unsigned int
  0,0 2swap 1,0 30 lshift  \ Stack: ( res  u  bit )
  \ Start value of bit is highest power of 4 <= u
  begin 2over 2over du< while dshr dshr repeat

  \ Do while bit not zero
  begin 2dup 0,0 d<> while
    2rot 2over 2over d+ 7 pick 7 pick du> not if  \ u >= res+bit ?
      2rot 2over d- 2rot 2tuck d-    \ u = u - res - bit
      2swap 2rot dshr 2over d+            \ res = (res >> 1) + bit
    else
      dshr    \ res = res >> 1
    then
    2-rot       \ Return stack to ( res u bit )
    dshr dshr   \ bit = bit >> 2
  repeat

  \ Drop u and bit, res is s31.32 square root of x
  2drop 2drop
  2-foldable
;

: sqrt ( x -- sqrtx )
  \ Find square root of non-negative s31.32 number x
  \ If x in interval [0, 1], use 0to1sqrt
  2dup 1,0 d> not if
    0to1sqrt
  else
    \ Divide x by 4 until result is in interval [0, 1]
    0,0 2swap   \ Init count of divides ndiv (use double for convenience)
    begin 2dup 1,0 d> while
      2swap 1,0 d+ 2swap    \ Incr count
      dshr dshr             \ Divide by 4
    repeat
    0to1sqrt
    2swap nip 0 do    \ ndiv consumed
      dshl                  \ Multiply by 2 ndiv times
    loop
  then
  2-foldable
;

\ -------------------------------------------------------------------------
\  Helpers and constants for trig functions
\ -------------------------------------------------------------------------
: deg2rad ( deg -- rad )
  \ Convert s31.32 in degress to s31.32 in radians
  74961321 0 f*
  2-foldable
;

: rad2deg ( rad -- deg )
  \ Convert s31.32 in radians to s31.32 in degrees
  1270363336 57 f*
  2-foldable
;

\ pi/2 and pi/4 as s31.32 numbers (whole part first for retrieval with 2@)
2451551556 1 2constant pi/2
3373259426 0 2constant pi/4

\ s31.32 comma parts of coefficients in Horner expression of 7-term series
\ expansion of sine after an x is factored out.  The whole parts are 0 and
\ are supplied in code.
numbertable sin-coef
   20452225 ,   \  1/(14*15)
   27531842 ,   \  1/(12*13)
   39045157 ,   \  1/(10*11)
   59652324 ,   \  1/(8*9)
  102261126 ,   \  1/(6*7)
  214748365 ,   \  1/(4*5)
  715827883 ,   \  1/(2*3)

: half-q1-sin-rad  ( x -- sinx )
  \ Sin(x) for x in first half of first quadrant Q1 and its negative
  \ x is a s31.32 angle in radians between -pi/4 and pi/4
  2dup 2dup f*          \  x and x^2 on stack as dfs
  \ Calculate Horner terms
  -1,0   \ Starting Horner term is -1
  7 0 do
    \ Multiply last term by x^2 and coefficient, then add +1 or -1 to get
    \ new term
    2over f* i sin-coef 0 f* 0 1
    i 2 mod 0= if d+ else d- then
  loop
  \ Last term is multiplied by x
  2nip f*
  2-foldable
;

\ s31.32 comma parts of coefficients in Horner expression of 8-term series
\ expansion of cosine.  The whole parts are 0 and are supplied in code.
numbertable cos-coef
   17895697 ,   \  1/(15*16)
   23598721 ,   \  1/(13*14)
   32537631 ,   \  1/(11*12)
   47721859 ,   \  1/(9*10)
   76695845 ,   \  1/(7*8)
  143165577 ,   \  1/(5*6)
  357913941 ,   \  1/(3*4)
 2147483648 ,   \  1/2

: half-q1-cos-rad  ( x -- cosx )
  \ Cos(x) for x in first half of first quadrant Q1 and its negative
  \ x is a s31.32 angle in radians between -pi/4 and pi/4
  2dup f*          \  x^2 on stack
  \ Calculate Horner terms
  1,0   \ Starting Horner term is 1
  8 0 do
    \ Multiply last term by x^2 and coefficient, then add +1 or -1 to get
    \ new term
    2over f* i cos-coef 0 f* 0 1
    i 2 mod 0= if d- else d+ then
  loop
  2nip
  2-foldable
;

: q1-sin-rad ( x -- sinx )
  \ Sin(x) for x in first quadrant Q1 and its negative
  \ x is a s31.32 angle in radians between -pi/2 and pi/2
  2dup pi/4 d< if
    half-q1-sin-rad
  else
    pi/2 2swap d- half-q1-cos-rad
  then
  \ Apply max/min limits
  \ 2dup 1,0 d> if 2drop 1,0 exit then
  \ 2dup -1,0 d< if 2drop -1,0 exit then
  2-foldable
;

: q1toq4-sin ( x -- sinx )
  \ Sin(x) for x in quadrants Q1 through Q4
  \ x is a s31.32 angle in degrees between 0 and 360
  2dup 270,0 d> if
    360,0 d- deg2rad q1-sin-rad
  else 2dup 90,0 d> if
    180,0 d- deg2rad q1-sin-rad dnegate
  else
    deg2rad q1-sin-rad
  then then
  2-foldable
;

\ s31.32 comma parts of coefficients in Horner expression of 6-term Euler
\ expansion of atan after x/(x^2+1) is factored out.  The whole parts are
\ 0 and are supplied in code.  The series variable is y = (x^2)/(x^2+1).
numbertable atan-coef
  3964585196 ,    \   12/13
  3904515724 ,    \   10/11
  3817748708 ,    \   8/9
  3681400539 ,    \   6/7
  3435973837 ,    \   4/5
  2863311531 ,    \   2/3

: base-ivl-atan ( x -- atanx )
  \ Calc atan for s32.31 x in base interval 0 to 1/8.
  2dup 2dup f* 2dup 1,0 d+     \ Stack: ( x  x^2  x^2+1 )
  2rot 2swap f/                \ Stack: ( x^2  x/(x^2+1) )
  2swap 2dup 1,0 d+ f/         \ Stack: ( x/(x^2+1)  (x^2)/(x^2+1) )
  \ Calc Horner terms for powers of y = (x^2)/(x^2+1)
  1,0   \ Starting Horner term is 1
  6 0 do
    \ Multiply last term by y and coefficient, then add 1 to get new term
    2over f* i atan-coef 0 f* 1,0 d+
  loop
  \ Last term is multiplied by x/(x^2+1)
  2nip f*
  2-foldable
;

\ Table of atan(i/8), i = 0, 1, ..., 8, values in radians
\ Only comma parts given, all whole parts are 0.
numbertable atan-table
           0 ,
   534100635 ,
  1052175346 ,
  1540908296 ,
  1991351318 ,
  2399165791 ,
  2763816217 ,
  3087351340 ,
  3373259426 ,

: 0to1-atan ( x -- atanx )
  \ Calc atan for s31.32 x in interval [0, 1]
  2dup 1,0 d= if
    2drop
    8 atan-table 0
  else
    \ Find interval [i/8, (i+1)/8) containing x, then use formula
    \ atan(x) = atan(i/8) + atan((x - (i/8))/(1 + (x*i/8))) where
    \ the argument in the second term is in [0, 1/8].
    0 7 do
      0 i 8,0 f/ 2over 2over d< not if
        2over 2over d-
        2-rot f* 1,0 d+
        f/ base-ivl-atan
        i atan-table 0 d+
        leave
      else
        2drop
      then
    -1 +loop
  then
  2-foldable
;

\ -------------------------------------------------------------------------
\  Trig functions
\ -------------------------------------------------------------------------
: sin ( x -- sinx )
  \ x is any s31.32 angle in degrees
  2dup 2dup d0< if dabs then
  \ Stack is ( x |x| )
  360,0 ud/mod 2drop
  q1toq4-sin    \ sin|x|
  \ Negate if x is negative
  2swap d0< if dnegate then
  2-foldable
;

: cos ( x -- cosx )
  \ x is any s31.32 angle in degrees
  90,0 d+ sin
  2-foldable
;

: tan ( x -- tanx )
  \ x is any s31.32 angle in degrees
  \ Move x to equivalent value in [-90, 90)
  deg-90to90
  \ If |x| > 89,9 deg, use approximation sgn(x)(180/pi)/(90-|x|)
  2dup dabs 2dup 89,8 d> if
    90,0 2swap d- 608135817 3 f* 180,0 2swap f/
    2swap d0< if dnegate then
  else
    2drop 2dup sin 2swap cos f/
  then
  2-foldable
;

: atan ( x -- atanx )
  \ Calc atan for s31.32 x, return result in degrees
  2dup 2dup d0< if dabs then   \ Stack: ( x |x| )
  \ Find atan(|x|)
  2dup 1,0 d> if
    \ |x| > 1, use atan(|x|) = (pi/2) - atan(1/|x|) with 1/|x| in [0, 1]
    1,0 2swap f/ 0to1-atan pi/2 2swap d-
  else
    \ |x| <= 1
    0to1-atan
  then
  \ Negate if x is negative
  2swap d0< if dnegate then
  rad2deg
  2-foldable
;

: asin ( x -- asinx )
  \ Calc asin for s31.32 x in interval [-1, 1], return result in degrees
  2dup 2dup d0< if dabs then
  \ Stack is ( x |x| )
  2dup 1,0 d> if drop exit then     \ Exit if |x|>1 with x on stack
  2dup 2dup f* 1,0 2swap d- 0to1sqrt    \ Stack: ( x  |x|  sqrt(1-x^2) )
  2over 2dup f* 0,5 d> if           \ x^2 > (1/2) ?
    2swap f/ atan 90,0 2swap d-
  else
    f/ atan
  then
  \ Negate if x is negative
  2swap d0< if dnegate then
  2-foldable
;

: acos ( x -- acosx )
  \ Calc acos for s31.32 x in interval [-1, 1], return result in degrees
  90,0 2swap asin d-
  2-foldable
;

\ -------------------------------------------------------------------------
\  Helper for logarithmic functions
\ -------------------------------------------------------------------------
: log2-1to2 ( y -- log2y )
  \ Helper function that requires y is s31.32 value with 1 <= y < 2
  0 0 2swap 0
  ( retval y cum_m )
  \ while((cum_m < 33) && (y > 1))
  begin dup 2over
    ( retval y cum_m cum_m y )
    1,0 d> swap 33 < and while
    ( retval y cum_m )
    rot rot 0 -rot        \ m = 0, z = y
    ( retval cum_m m z)
    \ Do z = z*z, m = m+1 until 2 <= z.  We also get z < 4
    begin
      2dup f* rot 1 + -rot
      ( retval cum_m m z )
      2dup 2,0 d< not
    until
    \ At this point z = y^(2^m) so that log2(y) = (2^(-m))*log2(z)
    \ = (2^(-m))*(1 + log2(z/2)) and 1 <= z/2 < 2
    \ We will add m to cum_m and add 2*(-cum_m) to the returned value,
    \ then iterate with a new y = z/2
    ( retval cum_m m z )
    2swap + -rot dshr 2>r   \ cum_m = cum_m + m, y = z/2
    ( retval cum_m ) ( R: y=z/2 )
    \ retval = retval + 2^-cum_m
    1,0 2 pick 0 do dshr loop
    ( retval cum_m 2^-cum_m )
    rot >r d+
    ( retval ) ( R: y cum_m )
    r> 2r> rot
    ( retval y cum_m )
  repeat
  drop 2drop
  2-foldable
;

\ -------------------------------------------------------------------------
\  Logarithmic functions
\ -------------------------------------------------------------------------
: log2 ( x -- log2x )
  \ Calculates base 2 logarithm of positive s31.32 value x

  \ Treat error and special cases
  \ Check that x > 0.  If not, return "minus infinity"
  2dup 0,0 d> not if 2drop -inf exit then
  \ If x = 1, return 0
  2dup 1,0 d= if 2drop 0,0 exit then

  \ Find the n such that 1 <= (2^(-n))*x < 2
  \ This n is the integer part (characteristic) of log2(x)
  0 -rot
  ( n=0 y=x )
  2dup 1,0 d> if
    \ Do n = n+1, y = y/2 while (y >= 2)
    begin 2dup 2,0 d< not while
      ( n y )
      dshr rot 1 + -rot
    repeat
  else
    \ Do n = n-1, y = 2*y while (y < 1)
    begin 2dup 1,0 d< while
      ( n y )
      dshl rot 1 - -rot
    repeat
  then

  \ Now y = (2^(-n))*x so log2(x) = n + log2(y) and we use the
  \ helper function to get log2(y) since 1 <= y < 2
  log2-1to2 rot 0 swap d+
  ( log2x )
  2-foldable
;

1292913986 0 2constant log10of2

: log10 ( x -- log10x )
  \ Calculates base 10 logarithm of positive s31.32 value x

  \ Treat error and special cases
  \ Check that x > 0.  If not, return "minus infinity"
  2dup 0,0 d> not if 2drop -inf exit then
  \ If x = 1, return 0
  2dup 1,0 d= if 2drop 0,0 exit then

  \ Find the n such that 1 <= (10^(-n))*x < 10
  \ This n is the integer part (characteristic) of log2(x)
  0 -rot
  ( n=0 y=x )
  2dup 1,0 d> if
    \ Do n = n+1, y = y/10 while (y >= 10)
    begin 2dup 10,0 d< not while
      ( n y )
      10,0 f/ rot 1 + -rot
    repeat
  else
    \ Do n = n-1, y = 10*y while (y < 1)
    begin 2dup 1,0 d<  while
      ( n y )
      10,0 f* rot 1 - -rot
    repeat
  then

  \ Now y = (10^(-n))*x so log10(x) = n + log10(y) and we use the
  \ identity log10(y) = log10(2)*log2(y)
  log2 log10of2 f* rot 0 swap d+
  ( log10x )
  2-foldable
;

2977044472 0 2constant lnof2

: ln ( x -- lnx )
  \ Return the natural logarithm of a postive s31.32 value x

  \ Treat error and special cases
  \ Check that x > 0.  If not, return "minus infinity"
  2dup 0,0 d> not if 2drop -inf exit then
  \ If x = 1, return 0
  2dup 1,0 d= if 2drop 0,0 exit then

  log2 lnof2 f*
  2-foldable
;

\ -------------------------------------------------------------------------
\  Power functions
\ -------------------------------------------------------------------------
\ s31.32 comma parts of all but first coefficient in Horner expansion of
\ a partial sum of the series expansion of exp(x).  The whole parts are 0
\ and are supplied in code.
numbertable exp-coef
   390451572 ,   \  1/11
   429496730 ,   \  1/10
   477218588 ,   \  1/9
   536870912 ,   \  1/8
   615366757 ,   \  1/7
   715827883 ,   \  1/6
   858993459 ,   \  1/5
  1073741824 ,   \  1/4
  1431655765 ,   \  1/3
  2147483648 ,   \  1/2

: exp-1to1 ( x -- expx )
  \ Calculate exp(x) for x an s31.32 value.  Values are correct when
  \ when rounded to six decimal places when x is between +/-0.7.  Uses an
  \ 11-term partial sum evaluated using Horner's method.
  \ Calculate Horner terms
  1,0   \ Starting Horner term is 1
  10 0 do
    \ Multiply last term by x and coefficient, then add to get new term
    2over f* i exp-coef 0 f* 0 1 d+
  loop
  \ Last part of expansion
  2over f* 0 1 d+
  2nip
  2-foldable
;

: pow2 ( x -- 2^x )
  \ Return 2 raised to the power x where x is s31.32
  \ If x is 0, return 1
  2dup 0,0 d= if 2drop 1,0 exit then
  \ If x < -32, 0 is returned.  If x >= 31, returns s31.32 ceiling
  2dup -32,0 d< if 2drop 0,0 exit then
  2dup 31,0 d< not if 2drop +inf exit then
  \ Get largest integer n such that n <= x so x = z + n, 0 <= z < 1
  2dup floor 2swap 2over d-
  ( n z )
  \ Get exp(z*ln2) = 2^z, then shift n times to get 2^x = (2^n)*(2^z)
  lnof2 f* exp-1to1 2swap nip
  ( 2^z n )  \ n now a single
  dup 0= if
    drop
  else
    dup 0< if
      negate 0 do dshr loop
    else
      0 do dshl loop
    then
  then
  2-foldable
;

1901360723 1 2constant 1overlnof2

: exp ( x -- expx )
  \ Return the exponential e^x of the s31.32 value x
  \ If x is 0, return 1
  2dup 0,0 d= if 2drop 1,0 exit then
  \ Return s31.32 ceiling for large pos. exponents, 0 for large neg.
  2dup 21,5 d> if 2drop +inf exit then
  2dup -22,2 d< if 2drop 0,0 exit then
  \ If |x| < 0.36, use exponential series approximation
  \ Otherwise, use exp(x) = pow2(x/ln(2))
  2dup dabs 0,36 d< if
    exp-1to1
  else
    1overlnof2 f* pow2
  then
  2-foldable
;

1382670639 3 2constant ln10overln2

: pow10 ( x -- 10^x )
  \ Return 10 raised to the power x where x is s31.32
  \ If x is 0, return 1
  2dup 0,0 d= if 2drop 1,0 exit then
  \ Return s31.32 ceiling for large pos. exponents, 0 for large neg.
  2dup 9,35 d> if 2drop +inf exit then
  2dup -9,64 d< if 2drop 0,0 exit then
  \ If x is a positive integer generate powers of 10 with multiplications
  \ Otherwise use 10^x = 2^(x*ln(10)/ln(2))
  2dup 2dup floor d= if
    2dup 0,0 d> if
      1,0 2swap nip
      0 do 10,0 f* loop
    else
      ln10overln2 f* pow2
    then
  else
    ln10overln2 f* pow2
  then
  2-foldable
;

\ -------------------------------------------------------------------------

: s>f ( n -- f ) 0 swap  1-foldable ; \ Signed integer --> Fixpoint s31.32
: f>s ( f -- n ) nip     2-foldable ; \ Fixpoint s31.32 --> Signed integer

\ ARM Cortex M0 Disassembler, Copyright (C) 2013  Matthias Koch
\ This is free software under GNU General Public License v3.
\ Knows M0 machine instructions, resolves call entry points,
\ literal pools and handles inline strings.
\ Usage: Specify your target address in disasm-$ and give disasm-step some calls.

\ ------------------------
\  A quick list of words
\ ------------------------

: list ( -- )
  cr
  dictionarystart
  begin
    dup 6 + ctype space
    dictionarynext
  until
  drop
;

\ ---------------------------------------
\  Memory pointer and instruction fetch
\ ---------------------------------------

0 variable disasm-$   \ Current position for disassembling

: disasm-fetch        \ ( -- Data ) Fetches opcodes and operands, increments disasm-$
    disasm-$ @ h@     \             Holt Opcode oder Operand, incrementiert disasm-$
  2 disasm-$ +!   ;

\ --------------------------------------------------
\  Try to find address as code start in Dictionary
\ --------------------------------------------------

: disasm-string ( -- ) \ Takes care of an inline string
  disasm-$ @ dup ctype skipstring disasm-$ !
;

: name. ( Address -- ) \ If the address is Code-Start of a dictionary word, it gets named.
  1 bic \ Thumb has LSB of address set.

  >r
  dictionarystart
  begin
    dup   6 + dup skipstring r@ = if ."   --> " ctype else drop then
    dictionarynext
  until
  drop
  r>

  case \ Check for inline strings ! They are introduced by calls to ." or s" internals.
    ['] ." $1E + of ."   -->  .' " disasm-string ." '" endof \ It is ." runtime ?
    ['] s"  $4 + of ."   -->  s' " disasm-string ." '" endof \ It is .s runtime ?
    ['] c"  $4 + of ."   -->  c' " disasm-string ." '" endof \ It is .c runtime ?
  endcase
;

\ -------------------
\  Beautiful output
\ -------------------

: u.4  0 <# # # # # #> type ;
: u.8  0 <# # # # # # # # # #> type ;
: u.ns 0 <# #s #> type ;
: const. ."  #" u.ns ;
: addr. u.8 ;

: register. ( u -- )
  case
    13 of ."  sp" endof
    14 of ."  lr" endof
    15 of ."  pc" endof
    dup ."  r" decimal u.ns hex
  endcase ;

\ ----------------------------------------
\  Disassembler logic and opcode cutters
\ ----------------------------------------

: opcode? ( Opcode Bits Mask -- Opcode ? ) \ (Opcode and Mask) = Bits
  rot ( Bits Mask Opcode )
  tuck ( Bits Opcode Mask Opcode )
  and ( Bits Opcode Opcode* )
  rot ( Opcode Opcode* Bits )
  =
;

: reg.    ( Opcode Position -- Opcode ) over swap rshift  $7 and register. ;
: reg16.  ( Opcode Position -- Opcode ) over swap rshift  $F and register. ;
: reg16split. ( Opcode -- Opcode ) dup $0007 and over 4 rshift $0008 and or register. ;
: registerlist. ( Opcode -- Opcode ) 8 0 do dup 1 i lshift and if i register. space then loop ;

: imm3. ( Opcode Position -- Opcode ) over swap rshift  $7  and const. ;
: imm5. ( Opcode Position -- Opcode ) over swap rshift  $1F and const. ;
: imm8. ( Opcode Position -- Opcode ) over swap rshift  $FF and const. ;

: imm3<<1. ( Opcode Position -- Opcode ) over swap rshift  $7  and shl const. ;
: imm5<<1. ( Opcode Position -- Opcode ) over swap rshift  $1F and shl const. ;
: imm8<<1. ( Opcode Position -- Opcode ) over swap rshift  $FF and shl const. ;

: imm3<<2. ( Opcode Position -- Opcode ) over swap rshift  $7  and shl shl const. ;
: imm5<<2. ( Opcode Position -- Opcode ) over swap rshift  $1F and shl shl const. ;
: imm7<<2. ( Opcode Position -- Opcode ) over swap rshift  $7F and shl shl const. ;
: imm8<<2. ( Opcode Position -- Opcode ) over swap rshift  $FF and shl shl const. ;

\ --------------------------------------
\  Name resolving for blx r0 sequences
\ --------------------------------------

0 variable destination-r0

\ ----------------------------------
\  Single instruction disassembler
\ ----------------------------------

: disasm ( -- ) \ Disassembles one machine instruction and advances disasm-$

disasm-fetch \ Instruction opcode on stack the whole time.

$4140 $FFC0 opcode? if ." adcs"  0 reg. 3 reg. then          \ ADC
$1C00 $FE00 opcode? if ." adds" 0 reg. 3 reg. 6 imm3. then   \ ADD(1) small immediate two registers
$3000 $F800 opcode? if ." adds" 8 reg. 0 imm8. then          \ ADD(2) big immediate one register
$1800 $FE00 opcode? if ." adds" 0 reg. 3 reg. 6 reg. then    \ ADD(3) three registers
$4400 $FF00 opcode? if ." add"  reg16split. 3 reg16. then    \ ADD(4) two registers one or both high no flags
$A000 $F800 opcode? if ." add"  8 reg. ."  pc " 0 imm8<<2. then  \ ADD(5) rd = pc plus immediate
$A800 $F800 opcode? if ." add"  8 reg. ."  sp " 0 imm8<<2. then  \ ADD(6) rd = sp plus immediate
$B000 $FF80 opcode? if ." add sp" 0 imm7<<2. then            \ ADD(7) sp plus immediate

$4000 $FFC0 opcode? if ." ands" 0 reg. 3 reg. then           \ AND
$1000 $F800 opcode? if ." asrs" 0 reg. 3 reg. 6 imm5. then   \ ASR(1) two register immediate
$4100 $FFC0 opcode? if ." asrs" 0 reg. 3 reg. then           \ ASR(2) two register
$D000 $F000 opcode? not if else dup $0F00 and 8 rshift       \ B(1) conditional branch
                       case
                         $00 of ." beq" endof  \ Z set
                         $01 of ." bne" endof  \ Z clear
                         $02 of ." bcs" endof  \ C set
                         $03 of ." bcc" endof  \ C clear

                         $04 of ." bmi" endof  \ N set
                         $05 of ." bpl" endof  \ N clear
                         $06 of ." bvs" endof  \ V set
                         $07 of ." bvc" endof  \ V clear

                         $08 of ." bhi" endof  \ C set Z clear
                         $09 of ." bls" endof  \ C clear or Z set
                         $0A of ." bge" endof  \ N == V
                         $0B of ." blt" endof  \ N != V

                         $0C of ." bgt" endof  \ Z==0 and N == V
                         $0D of ." ble" endof  \ Z==1 or N != V
                         \ $0E: Undefined Instruction
                         $0F of ." swi"  0 imm8.   drop exit endof
                       endcase
                       space
                       dup $FF and dup $80 and if $FFFFFF00 or then
                       shl disasm-$ @ 1 bic + 2 + addr.
                    then

$E000 $F800 opcode? if ." b"                                 \ B(2) unconditional branch
                      dup $7FF and shl
                      dup $800 and if $FFFFF000 or then
                      disasm-$ @ + 2+
                      space addr.
                    then

$4380 $FFC0 opcode? if ." bics" 0 reg. 3 reg. then           \ BIC
$BE00 $FF00 opcode? if ." bkpt" 0 imm8. then                 \ BKPT

$F000 $F800 opcode? if disasm-fetch ( Opcode1 Opcode2 )      \ BL/BLX(1)
                     \  dup $1000 and
                     \  if ." bl  "
                     \  else ." blx not available in M0"
                     \  then

                      ." bl  " ( Opcode1 Opcode2 )
                      $7FF and ( Opcode1 DestinationL )
                      over ( Opcode1 DestinationL Opcode1 )
                      $7FF and ( Opcode1 DestinationL DestinationH )
                      dup $400 and if $FFFFF800 or then ( Opcode DestinationL DestinationHsigned )
                      11 lshift or ( Opcode1 Destination )
                      shl
                      disasm-$ @ +
                      dup addr. name. \ Try to resolve destination
                    then

$4780 $FF87 opcode? if ." blx"  3 reg16. then                \ BLX(2)
$4700 $FF87 opcode? if ." bx"   3 reg16. then                \ BX
$42C0 $FFC0 opcode? if ." cmns" 0 reg. 3 reg. then           \ CMN
$2800 $F800 opcode? if ." cmp"  8 reg. 0 imm8. then          \ CMP(1) compare immediate
$4280 $FFC0 opcode? if ." cmp"  0 reg. 3 reg. then           \ CMP(2) compare register
$4500 $FF00 opcode? if ." cmp"  reg16split. 3 reg16. then    \ CMP(3) compare high register
$B660 $FFE8 opcode? if ." cps"  0 imm5. then                 \ CPS
$4040 $FFC0 opcode? if ." eors" 0 reg. 3 reg. then           \ EOR

$C800 $F800 opcode? if ." ldmia" 8 reg. ."  {" registerlist. ." }" then     \ LDMIA

$6800 $F800 opcode? if ." ldr" 0 reg. ."  [" 3 reg. 6 imm5<<2. ."  ]" then  \ LDR(1) two register immediate
$5800 $FE00 opcode? if ." ldr" 0 reg. ."  [" 3 reg. 6 reg. ."  ]" then      \ LDR(2) three register
$4800 $F800 opcode? if ." ldr" 8 reg. ."  [ pc" 0 imm8<<2. ."  ]  Literal " \ LDR(3) literal pool
                       dup $FF and shl shl ( Opcode Offset ) \ Offset for PC
                       disasm-$ @ 2+ 3 bic + ( Opcode Address )
                       dup addr. ." : " @ addr. then

$9800 $F800 opcode? if ." ldr"  8 reg. ."  [ sp" 0 imm8<<2. ."  ]" then     \ LDR(4)

$7800 $F800 opcode? if ." ldrb" 0 reg. ."  [" 3 reg. 6 imm5. ."  ]" then    \ LDRB(1) two register immediate
$5C00 $FE00 opcode? if ." ldrb" 0 reg. ."  [" 3 reg. 6 reg.  ."  ]" then    \ LDRB(2) three register

$8800 $F800 opcode? if ." ldrh" 0 reg. ."  [" 3 reg. 6 imm5<<1. ."  ]" then \ LDRH(1) two register immediate
$5A00 $FE00 opcode? if ." ldrh" 0 reg. ."  [" 3 reg. 6 reg.  ."  ]" then    \ LDRH(2) three register

$5600 $FE00 opcode? if ." ldrsb" 0 reg. ."  [" 3 reg. 6 reg. ."  ]" then    \ LDRSB
$5E00 $FE00 opcode? if ." ldrsh" 0 reg. ."  [" 3 reg. 6 reg. ."  ]" then    \ LDRSH

$0000 $F800 opcode? if ." lsls" 0 reg. 3 reg. 6 imm5. then   \ LSL(1)
$4080 $FFC0 opcode? if ." lsls" 0 reg. 3 reg. then           \ LSL(2) two register
$0800 $F800 opcode? if ." lsrs" 0 reg. 3 reg. 6 imm5. then   \ LSR(1) two register immediate
$40C0 $FFC0 opcode? if ." lsrs" 0 reg. 3 reg. then           \ LSR(2) two register
$2000 $F800 opcode? if ." movs" 8 reg. 0 imm8. then          \ MOV(1) immediate
$4600 $FF00 opcode? if ." mov" reg16split. 3 reg16. then     \ MOV(3)

$4340 $FFC0 opcode? if ." muls" 0 reg. 3 reg. then           \ MUL
$43C0 $FFC0 opcode? if ." mvns" 0 reg. 3 reg. then           \ MVN
$4240 $FFC0 opcode? if ." negs" 0 reg. 3 reg. then           \ NEG
$4300 $FFC0 opcode? if ." orrs" 0 reg. 3 reg. then           \ ORR

$BC00 $FE00 opcode? if ." pop {"  registerlist. dup $0100 and if ."  pc " then ." }" then \ POP
$B400 $FE00 opcode? if ." push {" registerlist. dup $0100 and if ."  lr " then ." }" then \ PUSH

$BA00 $FFC0 opcode? if ." rev"   0 reg. 3 reg. then         \ REV
$BA40 $FFC0 opcode? if ." rev16" 0 reg. 3 reg. then         \ REV16
$BAC0 $FFC0 opcode? if ." revsh" 0 reg. 3 reg. then         \ REVSH
$41C0 $FFC0 opcode? if ." rors"  0 reg. 3 reg. then         \ ROR
$4180 $FFC0 opcode? if ." sbcs"  0 reg. 3 reg. then         \ SBC

$C000 $F800 opcode? if ." stmia" 8 reg. ."  {" registerlist. ." }" then     \ STMIA

$6000 $F800 opcode? if ." str" 0 reg. ."  [" 3 reg. 6 imm5<<2. ."  ]" then  \ STR(1) two register immediate
$5000 $FE00 opcode? if ." str" 0 reg. ."  [" 3 reg. 6 reg. ."  ]" then      \ STR(2) three register
$9000 $F800 opcode? if ." str" 8 reg. ."  [ sp + " 0 imm8<<2. ."  ]" then   \ STR(3)

$7000 $F800 opcode? if ." strb" 0 reg. ."  [" 3 reg. 6 imm5. ."  ]" then    \ STRB(1) two register immediate
$5400 $FE00 opcode? if ." strb" 0 reg. ."  [" 3 reg. 6 reg.  ."  ]" then    \ STRB(2) three register

$8000 $F800 opcode? if ." strh" 0 reg. ."  [" 3 reg. 6 imm5<<1. ."  ]" then \ STRH(1) two register immediate
$5200 $FE00 opcode? if ." strh" 0 reg. ."  [" 3 reg. 6 reg.  ."  ]" then    \ STRH(2) three register

$1E00 $FE00 opcode? if ." subs" 0 reg. 3 reg. 6 imm3. then   \ SUB(1)
$3800 $F800 opcode? if ." subs" 8 reg. 0 imm8. then          \ SUB(2)
$1A00 $FE00 opcode? if ." subs" 0 reg. 3 reg. 6 reg. then    \ SUB(3)
$B080 $FF80 opcode? if ." sub sp" 0 imm7<<2. then            \ SUB(4)

$B240 $FFC0 opcode? if ." sxtb" 0 reg. 3 reg. then           \ SXTB
$B200 $FFC0 opcode? if ." sxth" 0 reg. 3 reg. then           \ SXTH
$4200 $FFC0 opcode? if ." tst"  0 reg. 3 reg. then           \ TST
$B2C0 $FFC0 opcode? if ." uxtb" 0 reg. 3 reg. then           \ UXTB
$B280 $FFC0 opcode? if ." uxth" 0 reg. 3 reg. then           \ UXTH

\ If nothing of the above hits: Invalid Instruction... They are not checked for.

\ Try name resolving for blx r0 sequences:

$2000 $FF00 opcode? if dup $FF and destination-r0  ! then \ movs r0, #...
$3000 $FF00 opcode? if dup $FF and destination-r0 +! then \ adds r0, #...
$0000 $F83F opcode? if destination-r0 @                   \ lsls r0, r0, #...
                         over $07C0 and 6 rshift lshift
                       destination-r0 ! then
dup $4780 =         if destination-r0 @ name. then        \ blx r0

drop \ Forget opcode
; \ disasm

\ ------------------------------
\  Single instruction printing
\ ------------------------------

: memstamp \ ( Addr -- ) Shows a memory location nicely
    dup u.8 ." : " h@ u.4 ."   " ;

: disasm-step ( -- )
    disasm-$ @                 \ Note current position
    dup memstamp disasm cr     \ Disassemble one instruction

    begin \ Write out all disassembled memory locations
      2+ dup disasm-$ @ <>
    while
      dup memstamp cr
    repeat
    drop
;

\ ------------------------------
\  Disassembler for definitions
\ ------------------------------

: seec ( -- ) \ Continues to see
  base @ hex cr

  begin
    disasm-$ @ h@           $4770 =  \ Flag: Loop terminates with bx lr
    disasm-$ @ h@ $FF00 and $BD00 =  \ Flag: Loop terminates with pop { ... pc }
    or
    disasm-step
  until

  base !
;

: see ( -- ) \ Takes name of definition and shows its contents from beginning to first ret
  ' disasm-$ !
  seec
;

\ ARM Cortex M0 Assembler, Copyright (C) 2014  Matthias Koch
\ This is free software under GNU General Public License v3.
\ Resolves constants, symbols and variable names and
\ gives you a comfortable way to write machine instructions.

\ Still missing: ADR, CPS, SWI, BKPT, PC relative LDR, and special cases of ADD with SP

\ -----------------------------------------------------------------------------
\ A few helpers for strings - Ein paar Stringhelferlein
\ -----------------------------------------------------------------------------

: vorneabschneiden ( Adresse Länge -- Adresse Länge ) \ Remove first character
  1- swap 1+ swap
;

: ersteszeichen ( Adresse Länge -- Adresse Länge Zeichen ) \ Get first character
  over c@
;

\ -----------------------------------------------------------------------------
\ A few helpers for parsing
\ -----------------------------------------------------------------------------

\ Muss jetzt noch angegebene Konstanten, Variablen, Wörter und Werte abhandeln.
\ Das passiert dadurch, dass ich das Ding
\ - im Dictionary suche
\     - Falls Variable  --> Variablenadresse
\     - Falls Konstante --> Konstante holen
\ - Versuche in eine Zahl umzuwandeln

\ Konstanten im Flash und Variablen im RAM haben Flag $40, Feste Variablen im Flash haben Flag $81, selbstdefinierte Einfachvariablen $C1 oder Doppeltvariablen $C2.

: symbolwert ( Stringadresse Länge -- Symbolwert )
  ( token )

  2dup find ( Adresse Flags ) over 0<>
        if
          \ Probe, ob die Flags auf Variablen oder Konstanten hindeuten
          ( Adresse Flags )
          $8000 bic \ Remove visible Flag on some targets
          swap
          ( Flags Adresse )
          over $40 = if execute then \ Konstanten im RAM und Flash; Variablen im RAM
          over $81 = if execute then \ Selbstdefinierte Variablen        im Flash
          over $82 = if execute then \ Selbstdefinierte Doppeltvariablen im Flash
          nip ( Adresse oder geholte Konstante )

          nip nip exit \ Vergiss Stringadresse, Rücksprung
        else
          2drop
        then

  2dup number 1 =
        if \ ."  Zahl " dup u.
          nip nip exit \ Vergiss Stringadresse, Rücksprung
        then

  ." Invalid Symbol: "    \ ." Ungültiges Symbol: "
  type cr quit
;

: registerparser16 ( Stringadresse Länge -- Nummer )
  ersteszeichen [char] r = if base @ >r decimal
                             vorneabschneiden
                             number 1 <> if ." Invalid Register. " cr quit then
                             dup $F bic  if ." Invalid Register. " cr quit then
                             r> base !
                           exit
                           then

  2dup s" tos" compare if 2drop 6 exit then
  2dup s" psp" compare if 2drop 7 exit then
  2dup s" sp"  compare if 2drop 13 exit then
  2dup s" lr"  compare if 2drop 14 exit then
  2dup s" pc"  compare if 2drop 15 exit then

  ." Invalid Operand" type \ ." Falsche Operandenangabe"
  cr quit
;

: registerparser ( Stringadresse Länge -- Nummer )
  registerparser16 dup 8 u>= if ." Only low registers allowed." cr quit then
;

: operandenparser  \ Bearbeitet r14, #42
  ( Stringadresse Länge -- Daten Register? )

    ersteszeichen [char] # =
      if  \ ." Konstante "
        vorneabschneiden \ # weg
        symbolwert false
      exit
      then

    registerparser true
;

\ -----------------------------------------------------------------------------
\ Jumps and Labels
\ -----------------------------------------------------------------------------

0 variable label-
0 variable label--
0 variable label---

: l-:   label-- @ label--- !
        label-  @ label--  !
        here      label-   !
immediate ;

\ Forward labels
\ Idea: Remember place and jump-opcodes to be filled in when label is reached.

0. 2variable label-f1
0. 2variable label-f2
0. 2variable label-f3
0. 2variable label-f4
0. 2variable label-f5
0. 2variable label-f6
0. 2variable label-f7
0. 2variable label-f8

: check+label ( Sprungkonstruktadresse -- )
  dup @ $000F and ( Addr Labelzähler )
    dup if \ Ungleich 0 ( Addr Labelzähler )

          1 = if ( Adresse )
                dup 2@ ( Adresse Lückenadresse Labelopcode )
                1- \ Label 1 auf 0 setzen, damit Opcode stimmt !
                ( Adresse Lückenadresse Opcode-Bitmaske )
                here swap
                ( Adresse Lückenadresse Zieladresse Opcode-Bitmaske )
                dup $E000 = if drop jump, else cjump, then
                ( Adresse )
                >r 0. r> 2!  \ Sprunglabel aus Tabelle löschen

              else

                dup ( Adresse Adresse )
                @   ( Adresse Labelopcode )
                1-  ( Adresse Labelopcode* )
                swap ( Labelopcode* Adresse )
                !
              then

        else \ Null - nichts tun
          ( Addr Labelzähler )
          2drop
        then
;

: l+:
  \ Time to fill in opcodes for forward jumps.
  \ Crawl the notes to see what is pending.
  label-f1 check+label
  label-f2 check+label
  label-f3 check+label
  label-f4 check+label
  label-f5 check+label
  label-f6 check+label
  label-f7 check+label
  label-f8 check+label
immediate ;

: remember+jump ( Zieladresse Opcode )
  \ Find an empty location in the forward-jump-notes.
  \ If it contains zero, the fresh wish for a jump can be filled in.
  label-f1 @ 0= if label-f1 2!  exit then
  label-f2 @ 0= if label-f2 2!  exit then
  label-f3 @ 0= if label-f3 2!  exit then
  label-f4 @ 0= if label-f4 2!  exit then
  label-f5 @ 0= if label-f5 2!  exit then
  label-f6 @ 0= if label-f6 2!  exit then
  label-f7 @ 0= if label-f7 2!  exit then
  label-f8 @ 0= if label-f8 2!  exit then
  ." Too many forward references" cr quit
;

: jump-destination ( Opcode Stringaddress Length -- Opcode Flag )
    2dup s" +"   compare if 2drop 1 or false exit then  \ False: Has to be resolved later
    2dup s" ++"  compare if 2drop 2 or false exit then
    2dup s" +++" compare if 2drop 3 or false exit then

    2dup s" -"   compare if 2drop label-   @ true exit then \ True: Insert now
    2dup s" --"  compare if 2drop label--  @ true exit then
    2dup s" ---" compare if 2drop label--- @ true exit then

    symbolwert true
;

: jumps <builds h, $30 setflags
        does> @ token
              jump-destination  \ Maybe some checks for jump distance soon ?
              if   \ Backward Jump
                ( Opcode Destination )
                here 2 allot -rot ( Location Opcode Destination )
                swap ( Location Destination Opcode )
                dup $E000 = if drop jump, else cjump, then
              else \ Forward-Jump
                here swap remember+jump 2 allot
              then
;

\ Conditional jumps have this instruction format: %1011ccccdddddddd

$D000 jumps beq
$D100 jumps bne
$D200 jumps bcs
$D300 jumps bcc
$D400 jumps bmi
$D500 jumps bpl
$D600 jumps bvs
$D700 jumps bvc
$D800 jumps bhi
$D900 jumps bls
$DA00 jumps bge
$DB00 jumps blt
$DC00 jumps bgt
$DD00 jumps ble

\ Aliases:

$D200 jumps bhs
$D300 jumps blo

\ Unconditional jump:

$E000 jumps b

\ -----------------------------------------------------------------------------
\ Simple instructions without operands
\ -----------------------------------------------------------------------------

: zero-operand <builds h, $30 setflags
                 does> h@ h, ;

$BF30 zero-operand wfi
$BF40 zero-operand sev

\ -----------------------------------------------------------------------------
\ Instructions with its own special handling
\ -----------------------------------------------------------------------------

: bl ( -- ) \ If range is too far for bl opcode, it will generate a blx r0 sequence.
  token symbolwert call,
immediate inline ; \ This combination gives immediate-compileonly

: ldr= ( -- ) \ Generated opcodes may change flags !
  token registerparser
  token symbolwert
  swap
  registerliteral,
immediate inline ; \ This combination gives immediate-compileonly

: mov&add      <builds h, $30 setflags
                 does> h@ ( Opcode )
                       token registerparser16 dup 7 and swap 8 and 4 lshift or
                       or
                       token registerparser16 3 lshift or
                       h,
;

$4400 mov&add add
$4600 mov&add mov

\ -----------------------------------------------------------------------------
\ Instructions with one register-16 operand
\ -----------------------------------------------------------------------------

: single-operand <builds h, $30 setflags
                 does> h@ ( Opcode )
                       token registerparser16 3 lshift or
                       h,
;

$4700 single-operand bx
$4780 single-operand blx

\ -----------------------------------------------------------------------------
\ Instructions with two register operands
\ -----------------------------------------------------------------------------

: double-operand <builds h, $30 setflags
                 does> h@ ( Opcode )
                       token registerparser or
                       token registerparser 3 lshift or
                       h,
;

$4000 double-operand ands
$4040 double-operand eors
$4140 double-operand adcs
$4180 double-operand sbcs
$41C0 double-operand rors
$4200 double-operand tst
$4300 double-operand orrs
$4340 double-operand muls
$4380 double-operand bics
$43C0 double-operand mvns

$B240 double-operand sxtb
$B200 double-operand sxth
$B2C0 double-operand uxtb
$B280 double-operand uxth

$BA00 double-operand rev
$BA40 double-operand rev16
$BAC0 double-operand revsh

\ -----------------------------------------------------------------------------
\ Instructions with 8 bit immediate operands
\ -----------------------------------------------------------------------------

: movs&cmp      <builds h, ( Opcode immediate ) h, ( Opcode register ) $30 setflags
                 does> >r
                       token registerparser
                       token operandenparser
                       if \ Register
                         ( reg reg )
                         3 lshift
                         r> 2+ h@ or or
                       else \ Immediate
                         ( reg imm )
                         dup $FF u> if ." Immediate too big" cr quit then
                         r> h@ or
                         swap 8 lshift or
                       then
                       h,
;

\ Reg  Imm8
$0000 $2000 movs&cmp movs
$4280 $2800 movs&cmp cmp

: adds&subs      <builds h, ( Opcode short immediate ) h, ( Opcode immediate ) h, ( Opcode register ) $30 setflags
                 does> >r
                       token registerparser
                       token operandenparser
                       if \ Register
                         ( reg reg )

                         token operandenparser
                         if ( reg reg reg )
                           6 lshift
                           r> 4 + h@ or
                           swap 3 lshift or
                           or
                         else ( reg reg imm )
                           dup 7 u> if ." Immediate too big" cr quit then
                           6 lshift
                           r> h@ or
                           swap 3 lshift or
                           or
                         then

                       else \ Immediate
                         ( reg imm )
                         dup $FF u> if ." Immediate too big" cr quit then
                         r> 2+ h@ or
                         swap 8 lshift or
                       then
                       h,
;

\ Reg  Imm8  Imm3
$1800 $3000 $1C00 adds&subs adds
$1A00 $3800 $1E00 adds&subs subs

\ -----------------------------------------------------------------------------
\ Instructions for Load and Store
\ -----------------------------------------------------------------------------

: load&store    <builds h, ( Shift ) h, ( Opcode immediate ) h, ( Opcode register ) $30 setflags
                 does> >r
                       token registerparser
                       token registerparser 3 lshift
                       token operandenparser
                       if \ Register
                         ( reg reg reg )
                         6 lshift
                         r> 4 + h@
                       else \ Immediate
                         ( reg reg imm )
                         r@ h@ rshift
                         dup 32 u> if ." Immediate too big" cr quit then
                         6 lshift
                         ( reg reg imm* )
                         r> 2+ h@
                       then
                       or or or h,
;

$5000 $6000 2 load&store str
$5400 $7000 0 load&store strb
$5200 $8000 1 load&store strh

$5800 $6800 2 load&store ldr
$5C00 $7800 0 load&store ldrb
$5A00 $8800 1 load&store ldrh

: signedloads  <builds h, $30 setflags
                 does> h@
                       token registerparser or
                       token registerparser 3 lshift or
                       token registerparser 6 lshift or
                       h,
;

$5600 signedloads ldrsb
$5E00 signedloads ldrsh

\ -----------------------------------------------------------------------------
\ Shift instructions
\ -----------------------------------------------------------------------------

: shifts-imm   <builds h, $30 setflags
                 does> h@
                       token registerparser
                       token registerparser
                       token operandenparser
                       if \ Register
                         cr quit
                       else \ Immediate
                         ( opcode reg reg imm )
                         dup 32 u> if ." Immediate too big" cr quit then
                         6 lshift
                         swap 3 lshift or
                         or or h,
                       then
;

$0000 shifts-imm lsls
$0800 shifts-imm lsrs
$1000 shifts-imm asrs

\ Shifts by a register are like logic operations.
\ Forth cannot tell apart both variants just by their operands, so they have other mnemonics here.

$4080 double-operand lslsr
$40C0 double-operand lsrsr
$4100 double-operand asrsr

\ -----------------------------------------------------------------------------
\ Instructions with register lists
\ -----------------------------------------------------------------------------

: push&pop     <builds h, $30 setflags
                 does> h@
                       token s" {" compare not if cr quit then
                       begin
                         token 2dup s" }" compare not
                       while
                         registerparser16
                         dup 14 = over 15 = or \ LR or PC ?
                         if drop $0100 or
                         else
                           dup 7 u<=
                           if 1 swap lshift or else ." Only low registers allowed." cr quit then
                         then
                       repeat
                       2drop
                       h,
;

$B400 push&pop push
$BC00 push&pop pop

: stmia&ldmia  <builds h, $30 setflags
                 does> h@
                       token registerparser 8 lshift or

                       token s" {" compare not if cr quit then
                       begin
                         token 2dup s" }" compare not
                       while
                         registerparser 1 swap lshift or
                       repeat
                       2drop
                       h,
;

$C000 stmia&ldmia stmia
$C800 stmia&ldmia ldmia

\ -----------------------------------------------------------
\   Cooperative Multitasking
\ -----------------------------------------------------------

\ Configuration:

128 cells constant stackspace \ 128 stack elements for every task

\ Internal stucture of task memory:
\  0: Pointer to next task
\  4: Task currently active ?
\  8: Saved stack pointer
\ 12: Handler for Catch/Throw
\  Parameter stack space
\  Return    stack space

false 0 true flashvar-here 4 cells - 4 nvariable boot-task \ Boot task is active, without handler and has no extra stackspace.
boot-task boot-task ! \ For compilation into RAM only

boot-task variable up \ User Pointer
: next-task  ( -- task )    up @ inline ;
: task-state ( -- state )   up @ 1 cells + inline ;
: save-task  ( -- save )    up @ 2 cells + inline ;
: handler    ( -- handler ) up @ 3 cells + inline ;

: (pause) ( stacks fly around )
    [ $B430 h, ]        \ push { r4  r5 } to save I and I'
    rp@ sp@ save-task !  \ save return stack and stack pointer
    begin
      next-task @ up !     \ switch to next running task
    task-state @ until
    save-task @ sp! rp!  \ restore pointers
    unloop ;              \ pop { r4  r5 } to restore the loop registers

: wake ( task -- ) 1 cells +  true swap ! ; \ Wake a random task (IRQ save)
: idle ( task -- ) 1 cells + false swap ! ;  \ Idle a random task (IRQ save)

\ -------------------------------------------------------
\  Round-robin list task handling - do not use in IRQ !
\ -------------------------------------------------------

: stop ( -- ) false task-state ! pause ; \ Stop current task
: multitask  ( -- ) ['] (pause) hook-pause ! ;
: singletask ( -- ) [']  nop    hook-pause ! ;

: task-in-list? ( task -- ? ) \ Checks if a task is currently inside of round-robin list (do not use in IRQ)
  next-task
  begin
    ( Task-Address )
    2dup = if 2drop true exit then
    @ dup next-task = \ Stop when end of circular list is reached
  until
  2drop false
;

: previous ( task -- addr-of-task-before )
  \ Find the task that has the desired one in its next field
  >r next-task begin dup @ r@ <> while @ repeat rdrop
;

: insert ( task -- ) \ Insert a task into the round-robin list
  dup task-in-list?  \ Is the desired task currently linked into ?
  if drop else next-task @ over ! next-task ! then
;

: remove ( task -- ) \ Remove a task from the round-robin list
  dup task-in-list?  \ Is the desired task currently linked into ?
  if dup @ ( task next )
     swap previous ( next previous ) !
  else drop then
;

\ -----------------------------------------
\ Create a new task - do not use in IRQ !
\ -----------------------------------------

: task: ( "name" -- )  stackspace cell+ 2*  4 cells +  buffer: ;

: preparetask ( task continue -- )
  swap >r ( continue R: task )

    \ true  r@ 1 cells + ! \ Currently running
      false r@ 3 cells + ! \ No handler

    r@ 4 cells + stackspace + ( continue start-of-parameter-stack )
      dup   r@ 2 cells + ! \ Start of parameter stack

    dup stackspace + ( continue start-of-parameter-stack start-of-return-stack )
    tuck      ( continue start-of-return-stack start-of-parameter-stack start-of-return-stack )
    2 cells - ( continue start-of-return-stack start-of-parameter-stack start-of-return-stack* ) \ Adjust for saved loop index and limit
    swap  !   ( continue start-of-return-stack ) \ Store the adjusted return stack pointer into the parameter stack
    !         \ Store the desired entry address at top of the tasks return stack

  r> insert
;

: activate ( task --   R: continue -- )
  true over 1 cells + ! \ Currently running
  r> preparetask
;

: background ( task --   R: continue -- )
  false over 1 cells + ! \ Currently idling
  r> preparetask
;

\ --------------------------------------------------
\  Multitasking insight
\ --------------------------------------------------

: tasks ( -- ) \ Show tasks currently in round-robin list
  hook-pause @ singletask \ Stop multitasking as this list may be changed during printout.

  \ Start with current task.
  next-task cr

  begin
    ( Task-Address )
    dup             ." Task Address: " hex.
    dup           @ ." Next Task: " hex.
    dup 1 cells + @ ." State: " hex.
    dup 2 cells + @ ." Stack: " hex.
    dup 3 cells + @ ." Handler: " hex. cr

    @ dup next-task = \ Stop when end of circular list is reached
  until
  drop

  hook-pause ! \ Restore old state of multitasking
;

\ --------------------------------------------------
\  Exception handling
\ --------------------------------------------------

: catch ( x1 .. xn xt -- y1 .. yn throwcode / z1 .. zm 0 )
    [ $B430 h, ]  \ push { r4  r5 } to save I and I'
    sp@ >r handler @ >r rp@ handler !  execute
    r> handler !  rdrop  0 unloop ;

: throw ( throwcode -- )  dup IF
	handler @ 0= IF false task-state ! THEN \ unhandled error: stop task
	handler @ rp! r> handler ! r> swap >r sp! drop r>
	UNLOOP  EXIT
    ELSE  drop  THEN ;

\ A microsecond time counter

$40054000 constant TIMEHW
$40054004 constant TIMELW
$40054008 constant TIMEHR
$4005400C constant TIMELR
$40054024 constant TIMERAWH
$40054028 constant TIMERAWL

: cycles ( -- u ) TIMERAWL @ ;

: delay-cycles ( cycles -- )
  cycles ( cycles start )
  begin
    pause
    2dup ( cycles start cycles start )
    cycles ( cycles start cycles start current )
    swap - ( cycles start cycles elapsed )
    u<=
  until
  2drop
;

: us ( u -- )        delay-cycles ;
: ms ( u -- ) 1000 * delay-cycles ;

\ A convenient memory dump helper

: u.4 ( u -- ) 0 <# # # # # #> type ;
: u.2 ( u -- ) 0 <# # # #> type ;

: dump16 ( addr -- ) \ Print 16 bytes memory
  base @ >r hex
  $F bic
  dup hex. ." :  "

  dup 16 + over do
    i c@ u.2 space \ Print data with 2 digits
    i $F and 7 = if 2 spaces then
  loop

  ."  | "

  dup 16 + swap do
        i c@ 32 u>= i c@ 127 u< and if i c@ emit else [char] . emit then
        i $F and 7 = if 2 spaces then
      loop

  ."  |" cr
  r> base !
;

: dump ( addr len -- ) \ Print a memory region
  cr
  over 15 and if 16 + then \ One more line if not aligned on 16
  begin
    swap ( len addr )
    dup dump16
    16 + ( len addr+16 )
    swap 16 - ( addr+16 len-16 )
    dup 0<
  until
  2drop
;

\ -----------------------------------------------------------------------------
\  USB terminal driver for RP2040
\ -----------------------------------------------------------------------------

\ Inspired by C baremetal examples by Douglas H. Summerville:
\ https://github.com/dougsummerville/Bare-Metal-Raspberry-PI-Pico
\ https://github.com/dougsummerville/Bare-Metal-Raspberry-PI-Pico/blob/main/drivers/usbcdc.c

\ C code with polled operation and handshake:
\ https://codeberg.org/Mecrisp/Bare-Metal-Raspberry-Pi-Pico/src/branch/main/drivers/usbcdc.c

\ -----------------------------------------------------------------------------
\  Small tools by Jan Bramkamp
\ -----------------------------------------------------------------------------

: bit ( u -- x ) 1 swap lshift 1-foldable inline ;

: >r/w ( x -- x ) %11 12 lshift bic                  1-foldable inline ;
: >xor ( x -- x ) %10 12 lshift bic %01 12 lshift or 1-foldable inline ;
: >set ( x -- x ) %01 12 lshift bic %10 12 lshift or 1-foldable inline ;
: >clr ( x -- x )                   %11 12 lshift or 1-foldable inline ;

$4000c000 constant RESET_REQ
$4000c004 constant WDSEL
$4000c008 constant RESET_DONE

: reset-block   ( x -- ) RESET_REQ >set ! ;
: unreset-block ( x -- ) RESET_REQ >clr ! ;
: unreset-block-wait ( x -- ) dup unreset-block begin dup RESET_DONE bit@ until drop ;

\ -----------------------------------------------------------------------------
\  Descriptors
\ -----------------------------------------------------------------------------

create language_string_descriptor here
  $04 c, $03 c,
  $09 c,
  $04 c,
here align swap - constant language_string_descriptor.size

create manufacturer_string_descriptor here
  16 c, $03 c,
  [char] M c, 0 c,
  [char] e c, 0 c,
  [char] c c, 0 c,
  [char] r c, 0 c,
  [char] i c, 0 c,
  [char] s c, 0 c,
  [char] p c, 0 c,
here align swap - constant manufacturer_string_descriptor.size

create configuration_string_descriptor here
  18 c, $03 c,
  [char] T c, 0 c,
  [char] e c, 0 c,
  [char] r c, 0 c,
  [char] m c, 0 c,
  [char] i c, 0 c,
  [char] n c, 0 c,
  [char] a c, 0 c,
  [char] l c, 0 c,
here align swap - constant configuration_string_descriptor.size

64 constant MAX_PACKET_SIZE  \ 8,16,32,64

create device_descriptor here
   18 c, \ Length of this descriptor, in Bytes
  $01 c, \ Descriptor type = 1 indicating device descriptor
  $10 c, \ USB version (in BCD, lsb)
  $01 c, \ USB version (in BCD, msb)
  $02 c, \ Device class (02: communications
  $00 c, \ Device subclass
  $00 c, \ Device Protocol
  MAX_PACKET_SIZE c, \ Maximum packet size target can process
  $83 c, \ 0x28, \ Vendor ID (lsb)   ID 0d28:0204 NXP ARM mbed
  $04 c, \ 0x0d, \ Vendor ID (msb)   ID 0483:5740 STMicroelectronics Virtual COM Port
  $40 c, \ 0x04, \ Product ID (lsb)
  $57 c, \ 0x02, \ Product ID (msb)
  $01 c, \ Device Version (lsB)
  $00 c, \ Device Version (msB)
  $01 c, \ Index of Manufacturer string descriptor
  $00 c, \ Index of Device string descriptor
  $00 c, \ Index of Serial Number string descriptor
  $01 c, \ Number of configurations
here align swap - constant device_descriptor.size

create config_descriptor here
  $09 c,         \ bLength
  $02 c,         \ bDescriptorType (Configuration)
  $43 c,  $00 c, \ wTotalLength
  $02 c,         \ bNumInterfaces 2
  $01 c,         \ bConfigurationValue
  $02 c,         \ iConfiguration (String Index)
  $80 c,         \ bmAttributes
  $32 c,         \ bMaxPower 100mA
  \ Interface 1: Control
  $09 c,         \ bLength
  $04 c,         \ bDescriptorType (Interface)
  $00 c,         \ bInterfaceNumber 0
  $00 c,         \ bAlternateSetting
  $01 c,         \ bNumEndpoints 1
  $02 c,         \ bInterfaceClass (Communication)
  $02 c,         \ bInterfaceSubClass (ACM
  $01 c,         \ bInterfaceProtocol (AT Commands)
  $00 c,         \ iInterface (String Index)
  \ Header Functional Desc
  $05 c,  $24 c,  $00 c,  $10 c,  $01 c,  \ (CS Interface)
  $05 c,  $24 c,  $01 c,  $00 c,  $01 c,  \ Call Mgmt FD
  $04 c,  $24 c,  $02 c,  $02 c,          \ ACM FD
  $05 c,  $24 c,  $06 c,  $00 c,  $01 c,  \ Union FD
  \ EP
  $07 c,         \ bLength
  $05 c,         \ bDescriptorType (Endpoint)
  $81 c,         \ bEndpointAddress (IN/D2H)
  $03 c,         \ bmAttributes (Interrupt)
  $08 c,  $00 c, \ wMaxPacketSize 8
  $FF c,         \ bInterval 16 (unit depends on device speed)
  \ Interface 2: DATA
  $09 c,         \ bLength
  $04 c,         \ bDescriptorType (Interface)
  $01 c,         \ bInterfaceNumber 1
  $00 c,         \ bAlternateSetting
  $02 c,         \ bNumEndpoints 2
  $0A c,         \ bInterfaceClass (CDC)
  $02 c,         \ bInterfaceSubClass
  $00 c,         \ bInterfaceProtocol
  $00 c,         \ iInterface (String Index)
  \ EP 2
  $07 c,         \ bLength
  $05 c,         \ bDescriptorType (Endpoint)
  $82 c,         \ bEndpointAddress (IN)
  $02 c,         \ bmAttributes (Bulk)
  MAX_PACKET_SIZE c,  $00 c,   \ wMaxPacketSize 64
  $00 c,         \ bInterval 0 (unit depends on device speed)
  \ EP 3
  $07 c,         \ bLength
  $05 c,         \ bDescriptorType (Endpoint)
  $03 c,         \ bEndpointAddress (OUT)
  $02 c,         \ bmAttributes (Bulk)
  MAX_PACKET_SIZE c,  $00 c,   \ wMaxPacketSize 64
  $00 c,         \ bInterval 0 (unit depends on device speed)
here align swap - constant config_descriptor.size

create line_coding here
  $00 c,  $c2 c,  $01 c,  $00 c,  \ 115200
  $00 c,  \ number stop 1 1.5 2
  $00 c,  \ parity N O E M S
  $08 c,  \ data 5,6,7,8,16
here align swap - constant line_coding.size

\ -----------------------------------------------------------------------------
\  Registers
\ -----------------------------------------------------------------------------

$50110000 constant usbctrl.ADDR_ENDP
$50110004 constant usbctrl.ADDR_ENDP1
$50110008 constant usbctrl.ADDR_ENDP2
$5011000c constant usbctrl.ADDR_ENDP3
$50110010 constant usbctrl.ADDR_ENDP4
$50110014 constant usbctrl.ADDR_ENDP5
$50110018 constant usbctrl.ADDR_ENDP6
$5011001c constant usbctrl.ADDR_ENDP7
$50110020 constant usbctrl.ADDR_ENDP8
$50110024 constant usbctrl.ADDR_ENDP9
$50110028 constant usbctrl.ADDR_ENDP10
$5011002c constant usbctrl.ADDR_ENDP11
$50110030 constant usbctrl.ADDR_ENDP12
$50110034 constant usbctrl.ADDR_ENDP13
$50110038 constant usbctrl.ADDR_ENDP14
$5011003c constant usbctrl.ADDR_ENDP15
$50110040 constant usbctrl.MAIN_CTRL
$50110044 constant usbctrl.SOF_WR
$50110048 constant usbctrl.SOF_RD
$5011004c constant usbctrl.SIE_CTRL
$50110050 constant usbctrl.SIE_STATUS
$50110054 constant usbctrl.INT_EP_CTRL
$50110058 constant usbctrl.BUFF_STATUS
$5011005c constant usbctrl.BUFF_CPU_SHOULD_HANDLE
$50110060 constant usbctrl.EP_ABORT
$50110064 constant usbctrl.EP_ABORT_DONE
$50110068 constant usbctrl.EP_STALL_ARM
$5011006c constant usbctrl.NAK_POLL
$50110070 constant usbctrl.EP_STATUS_STALL_NAK
$50110074 constant usbctrl.USB_MUXING
$50110078 constant usbctrl.USB_PWR
$5011007c constant usbctrl.USBPHY_DIRECT
$50110080 constant usbctrl.USBPHY_DIRECT_OVERRIDE
$50110084 constant usbctrl.USBPHY_TRIM
$5011008c constant usbctrl.INTR
$50110090 constant usbctrl.INTE
$50110094 constant usbctrl.INTF
$50110098 constant usbctrl.INTS

\ -----------------------------------------------------------------------------
\  Structure of DPRAM
\ -----------------------------------------------------------------------------

$50100000 constant USB_DPRAM

\ EP Control Registers (EP0 space is used for setup packet

$50100000 constant usbram.ep_control_reg[0].in
$50100004 constant usbram.ep_control_reg[0].out
$50100008 constant usbram.ep_control_reg[1].in
$5010000C constant usbram.ep_control_reg[1].out
$50100010 constant usbram.ep_control_reg[2].in
$50100014 constant usbram.ep_control_reg[2].out
$50100018 constant usbram.ep_control_reg[3].in
$5010001C constant usbram.ep_control_reg[3].out

\ Buffer Control Registers (datasheet recommends 16b writes)

$50100080 constant usbram.ep_buffer_ctrl[0].in[0]
$50100082 constant usbram.ep_buffer_ctrl[0].in[1]
$50100084 constant usbram.ep_buffer_ctrl[0].out[0]
$50100086 constant usbram.ep_buffer_ctrl[0].out[1]
$50100088 constant usbram.ep_buffer_ctrl[1].in[0]
$5010008A constant usbram.ep_buffer_ctrl[1].in[1]
$5010008C constant usbram.ep_buffer_ctrl[1].out[0]
$5010008E constant usbram.ep_buffer_ctrl[1].out[1]
$50100090 constant usbram.ep_buffer_ctrl[2].in[0]
$50100092 constant usbram.ep_buffer_ctrl[2].in[1]
$50100094 constant usbram.ep_buffer_ctrl[2].out[0]
$50100096 constant usbram.ep_buffer_ctrl[2].out[1]
$50100098 constant usbram.ep_buffer_ctrl[3].in[0]
$5010009A constant usbram.ep_buffer_ctrl[3].in[1]
$5010009C constant usbram.ep_buffer_ctrl[3].out[0]
$5010009E constant usbram.ep_buffer_ctrl[3].out[1]

\ EP0 Buffer, hardwired

$50100100 constant usbram.ep0_buffer

\ Space for more data buffers, need to be aligned on 64 bytes

$50100140 constant usbram.ep1_buffer
$50100180 constant usbram.ep2_buffer
$501001C0 constant usbram.ep3_buffer

\ -----------------------------------------------------------------------------
\  More variables that are not used by the USB hardware directly
\  and therefore could be anywhere in RAM
\ -----------------------------------------------------------------------------

\ Endpoint states

0 variable ep_state[0].source_data
0 variable ep_state[0].bytes_remaining
0 variable ep_state[0].data01

0 variable ep_state[1].source_data
0 variable ep_state[1].bytes_remaining
0 variable ep_state[1].data01

0 variable ep_state[2].source_data
0 variable ep_state[2].bytes_remaining
0 variable ep_state[2].data01

0 variable ep_state[3].source_data
0 variable ep_state[3].bytes_remaining
0 variable ep_state[3].data01

\ Housekeeping

0 variable linestate
0 variable device_addr

: usb-connected? ( -- ? ) linestate @ 1 and 0<> ; \ Check DTR

\ -----------------------------------------------------------------------------
\  Ring buffers for terminal streams
\ -----------------------------------------------------------------------------

2048 constant rx-ring-size \ Buffer sizes must be powers of two.
2048 constant tx-ring-size

0 variable tx-head
0 variable tx-tail
tx-ring-size buffer: tx-ring

: tx-head-next   ( -- offset ) tx-head @ 1+ tx-ring-size 1- and ;
: tx-tail-next   ( -- offset ) tx-tail @ 1+ tx-ring-size 1- and ;
: tx-ring-empty? ( -- ? )      tx-head @    tx-tail @ = ;
: tx-ring-full?  ( -- ? )      tx-head-next tx-tail @ = ;
: tx-ring-len    ( -- u )      tx-head @ tx-tail @ - tx-ring-size 1- and ;
: >tx-ring       ( c -- )      tx-ring tx-head @ + c!  tx-head-next tx-head ! ;
:  tx-ring>      ( -- c )      tx-ring tx-tail @ + c@  tx-tail-next tx-tail ! ;

0 variable rx-head
0 variable rx-tail
rx-ring-size buffer: rx-ring

: rx-head-next   ( -- offset ) rx-head @ 1+ rx-ring-size 1- and ;
: rx-tail-next   ( -- offset ) rx-tail @ 1+ rx-ring-size 1- and ;
: rx-ring-empty? ( -- ? )      rx-head @    rx-tail @ = ;
: rx-ring-full?  ( -- ? )      rx-head-next rx-tail @ = ;
: rx-ring-len    ( -- u )      rx-head @ rx-tail @ - rx-ring-size 1- and ;
: >rx-ring       ( c -- )      rx-ring rx-head @ + c!  rx-head-next rx-head ! ;
:  rx-ring>      ( -- c )      rx-ring rx-tail @ + c@  rx-tail-next rx-tail ! ;

\ -----------------------------------------------------------------------------
\  Data structures for setup packets
\ -----------------------------------------------------------------------------

\ Create struct to represent setup packet and map it to EP_CTRL_REG[0]

$50100000 constant setup_packet.wRequest
$50100000 constant   setup_packet.bmRequestType
$50100001 constant   setup_packet.bRequest

$50100002 constant setup_packet.wValue
$50100002 constant   setup_packet.wValue_L
$50100003 constant   setup_packet.wValue_H

$50100004 constant setup_packet.wIndex
$50100006 constant setup_packet.wLength

\ -----------------------------------------------------------------------------
\  Small tools for handling packets
\ -----------------------------------------------------------------------------

: free_ep3_output_buf_and_toggle_data_sync ( -- )

  MAX_PACKET_SIZE
  ep_state[3].data01 @ if $2000 or else $0000 or then  \ data01 bit

  dup      usbram.ep_buffer_ctrl[3].out[0] h!
  ep_state[3].data01 @ not ep_state[3].data01 ! \ Satisfies delay before setting AVAIL
  $0400 or usbram.ep_buffer_ctrl[3].out[0] h! \ Set AVAIL several cycles after
;

: prepare_in_buffer_on_ep0 ( -- )

  ep_state[0].bytes_remaining @ MAX_PACKET_SIZE umin   ( desc_len )
  dup negate ep_state[0].bytes_remaining +!            ( desc_len )

  dup
  ep_state[0].source_data @ swap   ( desc_len source desc_len)
  usbram.ep0_buffer         swap   ( desc_len source destination desc_len )
  move

  dup ep_state[0].source_data +!

  ep_state[0].data01 @ if $A000 or else $8000 or then

  dup      usbram.ep_buffer_ctrl[0].in[0] h!
  ep_state[0].data01 @ not ep_state[0].data01 ! \ Satisfies delay before setting AVAIL
  $0400 or usbram.ep_buffer_ctrl[0].in[0] h! \ Set AVAIL several cycles after
;

\ -----------------------------------------------------------------------------
\ Setup packet handler
\ -----------------------------------------------------------------------------

: usb_setup_handling ( -- )

  \ Prepare empty response
  \ If we're receiving a setup and there is still data waiting to be sent, something went wrong. Throw away remaining data.

   0 ep_state[0].source_data !
   0 ep_state[0].bytes_remaining !
  -1 ep_state[0].data01 !

  \ There may be an out stage (e.g. set line coding)
  $2400 usbram.ep_buffer_ctrl[0].out[0] h! \ DATA1

  setup_packet.wRequest h@
  case

    $0680 of  \ Std Dev- Get Descriptor
      setup_packet.wValue_H c@
      case
        $01 of  \ Device
          device_descriptor      ep_state[0].source_data !
          device_descriptor.size ep_state[0].bytes_remaining !
        endof

        $02 of  \ Config
          config_descriptor      ep_state[0].source_data !
          config_descriptor.size ep_state[0].bytes_remaining !
        endof

        $03 of  \ String
          setup_packet.wValue_L c@
          case
            $00 of
              language_string_descriptor           ep_state[0].source_data !
              language_string_descriptor.size      ep_state[0].bytes_remaining !
            endof
            $01 of
              manufacturer_string_descriptor       ep_state[0].source_data !
              manufacturer_string_descriptor.size  ep_state[0].bytes_remaining !
            endof
            $02 of
              configuration_string_descriptor      ep_state[0].source_data !
              configuration_string_descriptor.size ep_state[0].bytes_remaining !
            endof
          endcase
        endof
      endcase
    endof

   $0500 of  \ Std Dev- Set Address
    setup_packet.wValue h@ $FF and device_addr !
    \ zero len status will be sent
   endof

\  $0102 of  \ CDC Set Communications Feature
\  endof

\  $2021 of  \ Class Interface Set line coding
\  endof

   $2221 of  \ Class Interface Set line state
     setup_packet.wValue h@ linestate !
   endof

\  $0900 of  \ Std Dev- Set Config
\    \ zero len status will be sent
\  endof

   $21A1 of  \ Class Interface Get Line Coding
     line_coding      ep_state[0].source_data !
     line_coding.size ep_state[0].bytes_remaining !
   endof

  endcase

  \ Simplify code by always preparing IN packet (no harm if not needed)
  \ If no dedicated response has been prepared, a zero-length status packet will be generated

  ep_state[0].bytes_remaining @ setup_packet.wLength h@ umin ep_state[0].bytes_remaining !
  prepare_in_buffer_on_ep0
;

\ -----------------------------------------------------------------------------
\  USB request handler
\ -----------------------------------------------------------------------------

: usb-poll ( -- )

  \ Setup packet received
  usbctrl.SIE_STATUS @ 17 bit and \ USBCTRL_SIE_STATUS_SETUP_REC
  if
    usb_setup_handling
    17 bit usbctrl.SIE_STATUS >clr !
  then

  \ IN of config data Pico --> Host completed, maybe send more
  usbram.ep_buffer_ctrl[0].in[0] h@ 10 bit and 0=
  if
    \ Addr is set only after SetAddress; if set, just finished status stage with old addr
    device_addr @
    if
      device_addr @ usbctrl.ADDR_ENDP !
      0 device_addr !
    else
      ep_state[0].bytes_remaining @ if prepare_in_buffer_on_ep0 then
    then
  then

  \ Received an USB reset
  usbctrl.SIE_STATUS @ 19 bit and \ USBCTRL_SIE_STATUS_BUS_RESET
  if
    0 linestate !

    \ Reset address
    0 usbctrl.ADDR_ENDP !
    0 device_addr !

    \ Clear state
    0 ep_state[0].data01 !
    0 ep_state[0].bytes_remaining !

    0 ep_state[1].data01 !
    0 ep_state[1].bytes_remaining !

    0 ep_state[2].data01 !
    0 ep_state[2].bytes_remaining !

    0 ep_state[3].data01 !
    0 ep_state[3].bytes_remaining !

    \ EP3 is Bulk in need to prime RX buf
    free_ep3_output_buf_and_toggle_data_sync

    19 bit usbctrl.SIE_STATUS >clr !

    \ Reset ring buffers
    0 tx-tail ! 0 tx-head !
    0 rx-tail ! 0 rx-head !
  then

  \ Get stream data received from host into the ring buffer, if possible.
  \ Maximum incoming packet size is limited to 64.

  \ Do not release the endpoint output buffer if the contents do not fit into the rx buf.
  \ NAK replies will be send until the buffer is consumed.

  \ Data received, as indicated by "buffer full" flag? --> Fetch data!
  usbram.ep_buffer_ctrl[3].out[0] h@ $8000 and
  if
    usbram.ep_buffer_ctrl[3].out[0] h@ $03FF and ( length-of-incoming-data )
    dup rx-ring-size rx-ring-len - u< \ Enough space to move the contents of the packet into the ring?
    if
      0 ?do usbram.ep3_buffer i + c@ >rx-ring loop
      free_ep3_output_buf_and_toggle_data_sync
    else
      drop
    then
  then

  \ Transmit stream data to the host only when connection is live
  \ Prepare a descriptor on EP2 from TX Ring Buf iff buffer is free

  usb-connected?
  if
    \ EP buffer not in use and something to transmit waits
    tx-ring-empty?                                       \ Nothing to transmit
    usbram.ep_buffer_ctrl[2].in[0] h@ 15 bit and 0<> or   \ Buffer currently in use
    usbctrl.ADDR_ENDP 0= or                                \ Not yet enumerated
    not                                                     \ --> Nothing to do.

    if
      tx-ring-len MAX_PACKET_SIZE umin ( desc_len )
      dup 0 ?do tx-ring> usbram.ep2_buffer i + c! loop

      ep_state[2].data01 @ if $A000 or else $8000 or then

      dup      usbram.ep_buffer_ctrl[2].in[0] h!
      ep_state[2].data01 @ not ep_state[2].data01 ! \ Satisfies delay before setting AVAIL
      $0400 or usbram.ep_buffer_ctrl[2].in[0] h! \ Set AVAIL several cycles after
    then
  then
;

\ -----------------------------------------------------------------------------
\ Hardware initialisation
\ -----------------------------------------------------------------------------

: usb-init ( -- )

  \ Bring USB peripheral into a known state
  24 bit ( RESET_USBCTRL ) dup reset-block unreset-block-wait

  \ Clear USB DPRAM
  USB_DPRAM 4096 0 fill

  \ Reset ring buffers
  0 tx-tail ! 0 tx-head !
  0 rx-tail ! 0 rx-head !

  \ No DTR before enumeration
  0 linestate !

  \ Connect USB to PHY (should be by default)
  0 bit    \ USBCTRL_USB_MUXING_TO_PHY
  3 bit or \ USBCTRL_USB_MUXING_SOFTCON
  usbctrl.usb_muxing !

  \ Bus power should be available.
  2 bit    \ USBCTRL_USB_PWR_VBUS_DETECT
  3 bit or \ USBCTRL_USB_PWR_VBUS_DETECT_OVERRIDE_EN
  usbctrl.usb_pwr !

  \ Enable controller in device mode
  1 usbctrl.main_ctrl !

  \ Set bit in BUFF_STATUS for every buffer completed on RW EP0
  29 bit usbctrl.sie_ctrl ! \ USBCTRL_SIE_CTRL_EP0_INT_1BUF

  \ -----------------------------------------------------------
  \ Set up endpoints
  \ -----------------------------------------------------------

  \ EP0 is hardwired and needs no further configuration

  \ EP1 IN is for control interface

  usbram.ep1_buffer $FFFF and
    1 31 lshift or  \ Endpoint Enable
    0 30 lshift or  \ Single Buffered
    0 29 lshift or  \ Disable IRQ every xfer
    0 28 lshift or  \ No Double Buffered IRQ
    0 26 lshift or  \ 2-bit Control Endpoint Type (CONTROL:0)
    0 18 lshift or  \ Reserved
    0 17 lshift or  \ IRQ on Stall
    0 26 lshift or  \ IRQ on NAK
  usbram.ep_control_reg[1].in !

  \ EP2 IN is for bulk IN

  usbram.ep2_buffer $FFFF and
    1 31 lshift or  \ Endpoint Enable
    0 30 lshift or  \ Single Buffered
    0 29 lshift or  \ Disable IRQ every xfer
    0 28 lshift or  \ No Double Buffered IRQ
    2 26 lshift or  \ 2-bit Control Endpoint Type (BULK:2)
    0 18 lshift or  \ Reserved
    0 17 lshift or  \ IRQ on Stall
    0 26 lshift or  \ IRQ on NAK
  usbram.ep_control_reg[2].in !

  \ EP3 OUT is for bulk OUT
  usbram.ep3_buffer $FFFF and
    1 31 lshift or  \ Endpoint Enable
    0 30 lshift or  \ Single Buffered
    0 29 lshift or  \ Disable IRQ every xfer
    0 28 lshift or  \ No Double Buffered IRQ
    2 26 lshift or  \ 2-bit Control Endpoint Type (BULK:2)
    0 18 lshift or  \ Reserved
    0 17 lshift or  \ IRQ on Stall
    0 26 lshift or  \ IRQ on NAK
  usbram.ep_control_reg[3].out !

  \ No DTR before enumeration
  0 linestate !

  \ Enable pull-ups to signal connect to host
  16 bit usbctrl.sie_ctrl >set ! \ USBCTRL_SIE_CTRL_PULLUP_EN
;

\ -----------------------------------------------------------------------------
\  Terminal using the ring buffers
\ -----------------------------------------------------------------------------

: usb-key?  ( -- ? ) pause usb-poll rx-ring-empty? 0= ;
: usb-emit? ( -- ? ) pause usb-poll tx-ring-full?  0= ;
: usb-key   ( -- c ) begin usb-key?  until  rx-ring>         ;
: usb-emit  ( c -- ) begin usb-emit? until >tx-ring usb-poll ;

\ -----------------------------------------------------------------------------

\ : try ( -- )
\   usb-init
\   begin
\     usb-poll
\     usb-key? if usb-key emit then
\   key? until
\ ;

: +usb ( -- )
    ['] usb-key?  hook-key?  !
    ['] usb-key   hook-key   !
    ['] usb-emit? hook-emit? !
    ['] usb-emit  hook-emit  !
;

: -usb ( -- )
    ['] serial-key?  hook-key?  !
    ['] serial-key   hook-key   !
    ['] serial-emit? hook-emit? !
    ['] serial-emit  hook-emit  !
;

: usb-terminal ( -- )
  usb-init
  ['] usb-poll hook-pause !
  +usb
;

\ -----------------------------------------------------------------------------
