Added wskew.c, exercised in examples/wgt27.c - also added FPU support in boot.S

part12-wgt/boot/boot.S

@@ -42,6 +42,15 @@ _start:
     br      x4
     b       1b
 2:  // We're on the main core!
+    // First enable the FPU
+
+    mov     x0, #0x33ff
+    msr     cptr_el3, x0 	 // Disable coprocessor traps to EL2
+    mov     x0, #3 << 20
+    msr     cpacr_el1, x0	 // Enable FP/SIMD at EL1
+
+    // Now get ready to switch from EL3 down to EL1
+
     ldr     x0, =SCTLR_VALUE_MMU_DISABLED
     msr	    sctlr_el1, x0		
 
@@ -59,6 +68,7 @@ _start:
 
     eret			
 el1_entry:
+    // We're in EL1
     // Clean the BSS section
     ldr     x1, =__bss_start     // Start address
     ldr     w2, =__bss_size      // Size of the section
@@ -77,22 +87,22 @@ el1_entry:
 
 .ltorg
 
-.org 0xd8
+.org 0x100
 .globl spin_cpu0
 spin_cpu0:
         .quad 0
 
-.org 0xe0
+.org 0x108
 .globl spin_cpu1
 spin_cpu1:
         .quad 0
 
-.org 0xe8
+.org 0x110
 .globl spin_cpu2
 spin_cpu2:
         .quad 0
 
-.org 0xf0
+.org 0x118
 .globl spin_cpu3
 spin_cpu3:
         .quad 0

part12-wgt/examples/wgt27.c

@@ -0,0 +1,102 @@
+#include "wgt.h"
+#include "include/mem.h"
+
+// ######## REQUIRED FUNCTIONS ########
+
+unsigned long state0 = 1000;
+unsigned long state1 = 2000;
+
+unsigned long rand(void)
+{
+    unsigned long s1 = state0;
+    unsigned long s0 = state1;
+
+    state0 = s0;
+    s1 ^= s1 << 23;
+    s1 ^= s1 >> 17;
+    s1 ^= s0;
+    s1 ^= s0 >> 26;
+    state1 = s1;
+
+    return state0 + state1;
+}
+
+void wait_msec(unsigned int n)
+{
+    register unsigned long f, t, r;
+
+    // Get the current counter frequency
+    asm volatile ("mrs %0, cntfrq_el0" : "=r"(f));
+    // Read the current counter
+    asm volatile ("mrs %0, cntpct_el0" : "=r"(t));
+    // Calculate expire value for counter
+    t+=((f/1000)*n)/1000;
+    do{asm volatile ("mrs %0, cntpct_el0" : "=r"(r));}while(r<t);
+}
+
+// ######## STUB FUNCTIONS ########
+
+unsigned int kb = 0;
+
+unsigned int kbhit(void) {
+    kb++;
+    return kb / 500;
+}
+
+void getch(void) {
+    wait_msec(0x500000);
+    kb = 0;
+}
+
+// ######## WGT EXAMPLES ########
+
+void wgt27()
+{
+  block skewit;                   /* Pointer to our block */
+  color palette[256];             /* Our palette */
+  int i = 0;                      /* Loop counter */
+
+  set_clock_rate(get_max_clock());
+  mem_init();
+  vga256 ();
+
+  wreadpalette (0, 255, palette); /* Store our current palette */
+
+  wcls (vgapal[0]);               /* Clear screen with black */
+
+  for (i = 100; i > 0; i--)       /* Draw 100 filled circles */
+    {
+     wsetcolor (vgapal[i]);
+     wfill_circle (160, 100, i);
+    }
+
+  getch();
+
+  wsetcolor (vgapal[0]);                /* Use black as active color */
+  wbar (0, 0, 104, 199);                /* Draw two solid rectangles */
+  wbar (216, 0, 319, 199);
+  skewit=wnewblock (100, 40, 220, 160); /* Grab a block for skewing */
+
+  getch();
+
+  wcls (vgapal[0]);                     /* Clear screen with black */
+  do {
+    for (i = -100; i < 100; i += 2)     /* Skew image 2 pixels at a time */
+      {
+       wskew (100, 40, skewit, i);
+       delay(2);
+      }
+
+    for (i = 100; i > -100; i -= 2)     /* Skew image back to starting pos */
+      {
+       wskew (100, 40, skewit, i);
+       delay(2);
+      }
+   } while (1);
+}
+
+void main()
+{
+    wgt27();
+    while (1);
+}

part12-wgt/wgt.h

@@ -117,3 +117,4 @@ void wsetscreen (block image);
 void wnormscreen (void);
 void wvertres (short x, short y, short y2, block image);
 void wwipe (short x, short y, short x2, short y2, block image);
+void wskew (short x, short y, block image, short degrees);

part12-wgt/wskew.c

@@ -0,0 +1,53 @@
+#include "wgt.h"
+
+#define fastcopy memcpy
+
+void wskew (short x, short y, block image, short degrees)
+{
+  float newx, offs, slope;
+  long lnewx, lslope;
+  /* long values converted from floats (speed up) */
+  short ctr;                             /* y ctr */
+  short hgt, wid, display, shx, of2;
+  short onewx;
+
+  hgt = wgetblockheight (image);       /* Find height of block */
+  wid = wgetblockwidth (image);        /* Find width of block */
+  slope = (float)degrees / 45;         /* Calculate slope of new image */
+  offs = ((float)hgt / 2)*slope;       /* Find distance to shift horiz. */
+  newx = (float)x + offs;              /* Calculate new x position */
+  onewx = newx;                        /* Store it as integer for clipping */
+  lnewx = (float)newx * 2000;          /* Convert floating pt to long int */
+  lslope = (float)slope * 2000;        /* Convert floating pt to long int */
+
+  /* Draw each row of image */
+  for  (ctr = y; ctr < y + hgt - 1; ctr++)
+  {
+    if  (onewx + wid > bx) display = bx + 1 - onewx;
+    /* Clip width */
+    else display = wid;
+    if  (onewx < tx)                   /* See if image overlaps left clip */
+    {
+      of2 = tx - onewx;                /* Find offset into bitmap */
+      shx = tx;                        /* Set to draw starting here */
+      display = display - of2;         /* New width to draw */
+    }
+    else
+    {
+      shx = onewx;                     /* X value is fine */
+      of2 = 0;                         /* Use entire bitmap */
+    }
+    /* If width to display >0, and <= original width, and
+    we are within top and bottom Y clipping boundaries:
+    Copy row of bitmap to visual screen, using DISPLAY contiguous
+    bytes.
+    */
+    if  ((display > 0) & (display <= wid) & (ctr >= ty) & (ctr <= by))
+      fastcopy (&abuf[(ctr)*WGT_SYS.xres + shx], &image[(ctr - y)*wid + 2
+	+ of2], display);
+    lnewx = lnewx - lslope;
+    /* Find X position after shifting next row */
+    onewx = lnewx / 2000;
+    /* Calculate integer value from long int   */
+  }
+}

part12-wgt/wvertres.c

@@ -1,10 +1,12 @@
 #include "wgt.h"
 
+#define fastcopy memcpy
+
 void wvertres (short x, short y, short y2, block image)
 {
-  short width,height,base,yy,temp;
-  int fy,ctr2;
-  int same2,incr2;
+  short width, height, base, yy, temp;
+  long fy, ctr2;
+  long same2, incr2;
 
   width = wgetblockwidth (image);
   height = wgetblockheight (image);      /* store width and height */
@@ -16,8 +18,7 @@ void wvertres (short x, short y, short y2, block image)
     y2 = temp;
   }
 
-  incr2 = (y2 - y + 1) * (2000 / height);
- 
+  incr2 = (float)(y2 - y + 1) / height * 2000;
   /* find increment  */
   fy = y2;
   yy = y;
@@ -29,7 +30,8 @@ void wvertres (short x, short y, short y2, block image)
     if  (incr2 >= 2000)
       while  ((same2 - ctr2 > 999) & (yy < fy))
       {
-	memcpy (&abuf[(yy)*WGT_SYS.xres + x], &image[(base)*width + 2], width);
+	fastcopy (&abuf[(yy)*WGT_SYS.xres + x], &image[(base)*width + 2],
+		width);
 	yy ++;
 	ctr2 += 2000;
       }
@@ -40,11 +42,11 @@ void wvertres (short x, short y, short y2, block image)
 	base ++;
       }
     if  (yy < fy)
-      memcpy (&abuf[(yy)*WGT_SYS.xres + x], &image[(base)*width + 2], width);
+      fastcopy (&abuf[(yy)*WGT_SYS.xres + x], &image[(base)*width + 2],
+	      width);
     same2 += incr2;
     ctr2 += 2000;
     base ++;
     yy ++;
   }
-
 }