AVR Freaks

void mb_rol(char n,void *p)
{
 asm("      movw  r26,r18   \n"      //  x=p
     "      clc             \n"
     "lp1:  ld    r24,x     \n"
     "      rol   r24       \n"
     "      st    x+,r24    \n"
     "      dec   r16       \n"
     "      brne  lp1       \n"); 
}

//
// Shift a into b; both are n bytes long
//
void mb_rol2(char n,void *a,void *b)
{
 asm("      movw  r26,r18   \n"    //  x=p
     "      clc             \n"
     "      mov   r17,r16   \n"     // save n

     "lp1a: ld    r24,x     \n"
     "      rol   r24       \n"
     "      st    x+,r24    \n"
     "      dec   r16       \n"
     "      brne  lp1a      \n"
     "                      \n"    
     "      ldd   r26,y+0   \n"     // x=b       
     "      ldd   r27,y+1   \n"
     "lp1b: ld    r24,x     \n"
     "      rol   r24       \n"
     "      st    x+,r24    \n"
     "      dec   r17       \n"
     "      brne  lp1b      \n"
);  
}


//
// Multibyte add, b<=a+b
//
void mb_add(char n,void *a,void *b)
{
 asm("      movw   r26,r18  \n"
     "      ldd    r30,y+0  \n"
     "      ldd    r31,y+1  \n"
     "      clc             \n"
     " lp2: ld     r24,x+   \n"
     "      ld     r25,z    \n"
     "      adc    r24,r25  \n"
     "      st     z+,r24   \n"
     "      dec    r16      \n"
     "      brne   lp2      \n");
}

//
// Subtract, a=a-b
//
void mb_sub(char n,void *a,void *b)
{
 asm("      movw    r26,r18 \n"
     "      ldd     r30,y+0 \n"
     "      ldd     r31,y+1 \n"
     "      clc             \n"
     " lp6: ld      r24,x   \n"
     "      ld      r25,z+  \n"
     "      sbc     r24,r25 \n"
     "      st      x+,r24  \n"
     "      dec     r16     \n"
     "      brne    lp6     \n");
}

//
// Compare two multiwords
// Returns 1 if b>=a
//
char mb_cmp(char n,void *a,void *b)
{
 asm("      movw    r26,r18 \n"
     "      ldd     r30,y+0 \n"
     "      ldd     r31,y+1 \n"
     "      sec             \n"
     " lp7: ld      r24,x+  \n"
     "      ld      r25,z+  \n"
     "      cpc     r24,r25 \n"
     "      dec     r16     \n"
     "      brne    lp7     \n"
     "      brcc    lp8     \n"     // if no carry, skip setting r16 to 1
     "      inc     r16     \n"
     " lp8:                 \n");
 return(n);
}

//
// Add one byte to a multibyte word, a=a+b
//
void mb_add8(char n,void *a,char b)
{
asm("       movw   r26,r18  \n" // load z  with a
     "      ldd    r25,y+0  \n" // get b
     "      ld     r24,x    \n" // get  lsb 
     "      add    r24,r25  \n" // add B
     "      st     x+,r24   \n" // store  back
     "      brcc   lp5      \n" // If no carry is generated return early
     "      clr    r25      \n"
     " lp4: dec    r16      \n" // correct n
     "      breq   lp5      \n"
     " lp3: ld     r24,x    \n" // loop to propagate carry
     "      adc    r24,r25  \n"
     "      st     x+,r24   \n"
     "      brcs   lp4      \n" // Early exit if no carry   
     " lp5: ret             \n");
}

// 
// 40x16 => 56 multiply
// Watch out, a and r are pointers, but b is not!
//
void mul40(uint40 *a,unsigned int b,uint56 *r)
{
 char i;
 uint56 ac;
 
 // Extend a to 56 bits
 ac.n[0]=a->n[0];
 ac.n[1]=a->n[1];
 ac.n[2]=a->n[2];
 ac.n[3]=a->n[3];
 ac.n[4]=a->n[4];
 ac.n[5]=0;
 ac.n[6]=0;

 // Set result to zero
 memset(r,0,7);
 
 for (i=16;i>0;i--)
  {
  if (b & 1) mb_add(7,&ac,r);
  b>>=1;
  mb_rol(7,&ac);
  }
}

//
// Divide a two 56 bit numbers 
// Stores the result in dividend
void div56(uint56 *dividend,uint56 *divisor)
{
 char i;
 uint56 rem;  

 for (i=0;i<7;i++)
  rem.n[i]=0;

 for (i=0;i<56;i++)
  {
  // shift dividend into remainder
  mb_rol2(7,dividend,&rem); 

  // Can divisor be subtracted from remainder?
  if (!mb_cmp(7,&rem,divisor))
   {
   mb_sub(7,&rem,divisor);
   dividend->n[0]|=1;
   }
  }
}